Robot Design:

Robot for welding task can be designed by multiple configurations of links and joints. There are two
types of joints:

1-Revolute joint

2-Prismatic joint

Following is figure of revolute joint.

Prismatic joint

Revolute
joint

Welding robot should be able to do all required movements which are necessary for welding tasks.

1
Related Work
 Robot Forward Kinematics(using homogeneous transformation and DH convention)
 Robot inverse kinematics
 Robot velocity kinematics
 Robot Dynamics
 Position Control of robot

Robot Links Creation:

Here is the robot links (four rotational links and one wrist) creation in matlab
L11_update_vertices =[-0.05 -0.05 0; 0.05 -0.05 0; 0.05 0.05 0 ;
-0.05 0.05 0; -0.05 -0.05 0.85; 0.05 -0.05 0.85; 0.05 0.05 0.85 ;
-0.05 0.05 0.85 ];

L21_update_vertices =[-0.05 -0.05 0.75; 0.45 -0.05 0.75; 0.45

0.05 0.75 ;-0.05 0.05 0.75; -0.05 -0.05 0.85 ;0.45 -0.05 0.85;
0.45 0.05 0.85 ;-0.05 0.05 0.85];

L31_update_vertices =[0.35 -0.05 0.75; 0.85 -0.05 0.75; 0.85 0.05

0.75 ;0.35 0.05 0.75; 0.35 -0.05 0.85 ;0.85 -0.05 0.85; 0.85 0.05
0.85 ;0.35 0.05 0.85];

L41_update_vertices = [0.75 -0.05 0.75; 1.25 -0.05 0.75; 1.25

0.05 0.75 ;0.75 0.05 0.75; 0.75 -0.05 0.85 ;1.25 -0.05 0.85; 1.25
0.05 0.85 ;0.75 0.05 0.85];

L51_update_vertices=[1.45 -0.025 0.775 ; 1.15 -0.025 0.775 ;

1.15 -0.025 0.825; 1.45 -0.025 0.825; 1.45 0.025 0.775 ;
1.15 0.025 0.775 ; 1.15 0.025 0.825; 1.45 0.025 0.825]
updated_facs = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];

2
 Link Link Link
2 3 4 wrist

Link
1

All links are created by patch command in Matlab. Dimentions of links are

Link1 length=80cm

Link2 length=40cm

Link3 length=40cm

Link4 length=40cm

Wrist length=20 cm

All links are designed by patch command matlab.

Forward Kinematics
Forward Kinematics is implemented in Matlab using 2 methods
1-Homogeneous Transformation
2-DH convention

1-Homogeneous Transformation
We know that
R (-θ) = R’
So, I rotated all links by –θ’s by homogeneous transformation and by DH convention
Matlab frame has x, y and z axis .All links rotate in this frame .Homogeneous transformation
rotation matrices are following

3
Here are the rotation matrices

We know that ,Rx (-θ) = Rx’ , Ry(-θ) = Ry’ , Rz(-θ) = Rz’

Link 1 rotation matrix using Homogeneous transformation
%Rotation of frame 1 about fix z axis by angle - θ 1
R1=[cos(theta1) sin(theta1) 0 ; -sin(theta1) cos(theta1) 0; 0 0 1];

Link 2 rotation matrix using Homogeneous transformation

%Rotation of frame 2 about current y axis by angle - θ 2
R2=[cos(theta2) 0 -sin(theta2);0 1 0; sin(theta2) 0 cos(theta2) ];

Link 3 rotation matrix using Homogeneous transformation

%Rotation of frame 3 about current y axis by angle - θ 3
R3=[cos(theta3) 0 -sin(theta3);0 1 0; sin(theta3) 0 cos(theta3) ];

Link 4 rotation matrix using Homogeneous transformation

%Rotation of frame 4 about current y axis by angle - θ 4
R4=[cos(theta4) 0 -sin(theta4);0 1 0; sin(theta4) 0 cos(theta4) ];

Wrist rotation matrices using Homogeneous transformation

% Rotation of Link 5 of wrist about current y axis
R5=[cos(theta5) 0 -sin(theta5); 0 1 0; sin(theta5) 0 cos(theta5)];%y

% Rotation of Link 6 of wrist about current z axis

R6=[cos(theta6) sin(theta6) 0;-sin(theta6) cos(theta6) 0;0 0 1]; %z

4
% Rotation of Link 7 of wrist about current x axis
R7=[1 0 0;0 cos(theta7) sin(theta7);0 -sin(theta7) cos(theta7)]; % x

Schematic Wrist Diagram

θ6 z6

θ7 x7

y5 θ5

Link 5 of wrist rotate about current y axis.

Link 6 of wrist rotate about current z axis.
Link 7 of wrist rotate about current x axis.
New points of each link are obtained by multiplication of old points and rotation matrices. Each
link has eight vertices points.

x' x

[ ] []
y'
z'
=Rx y
z

e.g, Link 2 is rotated 30 degree about y axis. Each new point of link 2 is obtained by multiplying
old points with rotation matrices.
All links are rotated as same procedure as above.
Rotation matrices for forward kinematics using homogeneous transformation
Rotation upto Link 1 = R1
Rotation upto Link 2 = R1*R2;
Rotation upto Link 3 = R1*R2*R3 ;
Rotation upto Link 4 = R1*R2*R3*R4;
Rotation upto Link 5 = R1*R2*R3*R4*R5;
5
Rotation upto Link 6 = R1*R2*R3*R4*R5*R6;
Rotation upto Link 17= R1*R2*R3*R4*R5*R6*R7;

2-Forward kinematics Using DH convention

I did the forward kinematics using DH convention and compare its result with forward
kinematics with homogeneous transformation .It’s result was equal to forward kinematics by
homogeneous transformation. In simulation robot model was rotating by homogeneous
transformation .I did homogeneous transformation by DH to compare the results only.

DH table for seven links of robot

Link no. a ∝ d θ

1 0 -90 d1 θ1

2 a2 0 0 θ2

3 a3 0 0 θ3

4 a4 0 0 θ4

5 0 90 0 θ5

6 0 -90 0 θ6

7 0 0 d7 θ7

Robot Forward Kinematic Model:

A1 =

[ cos(theta1), 0, -sin(theta1), 0]
[ sin(theta1), 0, cos(theta1), 0]
[ 0, -1, 0, d1]
[ 0, 0, 0, 1]
A2 =

[ 1, 0, 0, a2*cos(theta2)]
[ 0, 1, 0, a2*sin(theta2)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
A3 =

6
[ 1, 0, 0, a3*cos(theta3)]
[ 0, 1, 0, a3*sin(theta3)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

A4 =

[ 1, 0, 0, a4*cos(theta3 + theta4)]
[ 0, 1, 0, a4*sin(theta3 + theta4)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

A5 =

[ cos(theta5), 0, sin(theta5), 0]
[ sin(theta5), 0, -cos(theta5), 0]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]

A6 =

[ cos(theta6), 0, -sin(theta6), 0]
[ sin(theta6), 0, cos(theta6), 0]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]

A7 =

[ cos(theta7), -sin(theta7), 0, 0]
[ sin(theta7), cos(theta7), 0, 0]
[ 0, 0, 1, d7]
[ 0, 0, 0, 1]

A=A1*A2*A3*A4*A5*A6*A7;
1- Inverse Kinematics
Inverse kinematics is done using geometric report. First we will do the inverse kinematics of 4
links.
Below is the x-y view of robot .link 1 is rotated by some angle theta. All other links angle
theta2,theta3 and theta4 are 0
If a desire point is given to robot .And we have to do inverse kinematics for finding all angles of
robot joints. We have desire points in x ,y and z directions for robot which are xdes, ydes, zdes.
Theta1=atan2 (ydes,xdes)
Now we have find theta1 .And we have to find theta2, theta3 and theta4.
When we rotate link about z axis

7
Step 1: Find rotated axis x’,y’ ,z’
New axis x’,y’ and z’ are equal to old axis multiply rotation matrix

x' x

[ ] []
y' = R x y
z' z

x' cos ⁡(Ѳ) −sin ⁡(Ѳ) 0 x

[ ][
y ' = sin ⁡( Ѳ) cos ⁡(Ѳ) 0
z' 0 0 1 ][ ]
y
z

Frame x, y, z converted to frame x’, y’, z’

Shift frame x’,y’,z’ to tip of link 1
Link 1 has d1 length So, Frame transformation is

X’=X cos ⁡(Ѳ) -Y sin ⁡(Ѳ)

Y’=X sin ⁡(Ѳ) +Y cos ⁡(Ѳ)
Z’=Z-d1

Now forget the Link 1 .We have to find inverse kinematics of three link manipulator in x’,y’ and
z’ frame.
Desire point

Z’
X
Y’

X’

There are total 6 possible solutions of 3 link planar manipulator inverse kinematics.

8
 Soln. 1 Soln. 2

Z1 Z
2 Soln. 4

X2
Ѳ2

X1

Soln. 5 Soln. 6

I first chose the solution 3 to find 3 link manipulator angles.

theta21,theta31,theta41,theta5,theta6,theta7

Z’
9
By above figure we can see that
theta2=atan2(z’,x’)
Now we can solve theta3 and theta4 by 2 link manipulator inverse kinematic solution but first
we will find new rotated frame and do frame transformation
we can see in below figure that rotation occur about y’ axis
so,we know that rotation matrix about y axis

x' ' cos ⁡( Ѳ) 0 sin ⁡(Ѳ) x'

[ ][
y' '
z''
= 0 1 0
−sin ⁡( Ѳ) 0 cos ⁡(Ѳ) ][ ]
y'
z'

D=¿+Y ' ' 2-a 32-a 42)/(2*a3*a4)

xx=sqrt(1-D^2)

Theta41=atan2(xx ,D)
theta31=atan2(z’’, x’’) - atan2(a4*sin(theta4),a3+a4*cos(theta4))

Now just solve two link inverse kinematics in x’’ and z’’ plane
Now consider wrist
Link5=rotate about y axis theta5 (zero link length, brown link below)
Link 6=rotate about z axis theta6 (zero link length, green link below)
Link7=rotate about x axis theta7 (d7 link length, blue link below)
10
3rd transformation of frame
X’’’=X’’-a2*cos(theta2)-a3*cos(theta2+theta3)
Z’’’=Z’’=a2*sin(theta2)-a3*sin(theta2+theta3)
Theta y=atan2(z’’’,x’’’)

4th transformation of frame

x4 cos ⁡( Ѳ) 0 sin ⁡(Ѳ) x'''

[ ][
y4
z4
= 0 1 0
−sin ⁡( Ѳ) 0 cos ⁡(Ѳ) ][ ]
y' ''
z' ' '

X4=x’’’*cos ⁡(Ѳ) +z’’’ *sin ⁡(Ѳ)

Y4=y’’’
So,
thetaz=atan2(y4,x4) % rot about z axis
Theta7 effects nothing on wrist position..it can be rotated in any direction.
Thetax=Theta5/2 ;
There may be another solution of inverse kinematics.
theta22,theta32,theta42

Theta22=theta21

D=¿+Y ' ' 2-a 32-a 42)/(2*a3*a4)

11
 xx= - sqrt(1-D^2) % elbow down solution

theta42=atan2(xx ,D) =-theta41

theta32=atan2(z’’, x’’) - atan2(a4*sin(theta42),a3+a4*cos(theta42))

theta52=theta51; theta62=theta61; theta72=theta71

Forward Kinematics after Inverse kinematics

After doing inverse kinematics ,you find angles of all Links. But we know that in Homogeneous
transformation
Rotation upto Link 1 = R1
Rotation upto Link 2 = R1*R2;
Rotation upto Link 3 = R1*R2*R3 ;
Rotation upto Link 4 = R1*R2*R3*R4;
Above equations will not valid in forward kinematics after doing inverse kinematics
e.g, in solution 1
Rotation upto Link 1 = R1
Rotation upto Link 2 = R1*R2;
Rotation upto Link 3 = R1*R3 ;
Rotation upto Link 4 = R1*R3*R4;

Z’

X’
Link 3 and 4 will not rotate by theta2 angle as in homogeneous transformation,because in
inverse kinematics we estimate angle without including theta2 in link 3 and link 4.But
remember ,Link3 and 4 will translate due to link 2 but they will not rotate due to link 2

12
Similar thing is for wrist .Wrist will not rotate due to Link2,3 and 4 but only translate due to
these links.Its rotation matrix will be

R7_0=R1*Ry*Rz*Rx

Thetay, thetaz and theta6 are find in above method 1 of inverse kinematics. Ry is rotation about
y axis by theta y, . Rz is rotation about z axis by thetaz, Rx is rotation about x axis by theta x.
So, forward kinematics by DH convention will be
theta6=theta6-90*pi/180; % -90 degree offset to 2nd link of
wrist

d1=3.1;
a2=1.5; a4=1.5 ; a3=1.5; d7=0.5;

A1=[cos(theta1) 0 -sin(theta1) 0; sin(theta1) 0 cos(theta1) 0;

0 -1 0 d1 ; 0 0 0 1];

theta4=theta4+theta3; %in inverse kin link 4 rotate by angle

theta4+theta3

A2=[1 0 0 a2*cos(theta2); 0 1 0 a2*sin(theta2); 0 0 1 0; 0 0 0

1];
A3=[1 0 0 a3*cos(theta3);0 1 0 a3*sin(theta3); 0 0 1 0 ; 0 0 0
1];
A4=[1 0 0 a4*cos(theta4); 0 1 0 a4*sin(theta4);0 0 1 0 ; 0 0 0
1];

A5=[cos(theta5) 0 sin(theta5) 0;sin(theta5) 0 -cos(theta5) 0;0 1

0 0;0 0 0 1]; %y
A6=[cos(theta6) 0 -sin(theta6) 0; sin(theta6) 0 cos(theta6) 0;0
-1 0 0; 0 0 0 1];%z
A7=[cos(theta7) -sin(theta7) 0 0;sin(theta7) cos(theta7) 0 0; 0 0
1 d7;0 0 0 1];%x

A7_0=A1*A2*A3*A4*A5*A6*A7;

Notice above A matrices….

In A7_0 we can see by formulas of A2,A3,A4 have no rotation effect. They have just translation
effect. So there 4th column has some displacement and rotation matrices are identity matrices.

2.5 Velocity Kinematic Model

13
Below function takes as input all seven angles of links and compute jacobian of robot at every
point .
Function jacobian(theta1,theta2,theta3,theta4,theta5,theta6,theta7)

o0=[0 ;0; 0];

z0=[0; 0; 1];

d1=3.1;
a2=1.5; a4=1.5 ; a3=1.5;
d7=0.5;

A1=[cos(theta1) 0 -sin(theta1) 0; sin(theta1) 0 cos(theta1) 0; 0

-1 0 d1 ; 0 0 0 1];

theta4=theta4+theta3;

A2=[1 0 0 a2*cos(theta2); 0 1 0 a2*sin(theta2); 0 0 1 0; 0 0 0 1];

A3=[1 0 0 a3*cos(theta3);0 1 0 a3*sin(theta3); 0 0 1 0 ; 0 0 0 1];

A4=[1 0 0 a4*cos(theta4); 0 1 0 a4*sin(theta4); 0 0 1 0 ; 0 0 0 1];

A5=[cos(theta5) 0 sin(theta5) 0;sin(theta5) 0 -cos(theta5) 0;0 1 0 0;0

0 0 1]; %y

A6=[cos(theta6) 0 -sin(theta6) 0; sin(theta6) 0 cos(theta6) 0;0 -1 0

0; 0 0 0 1];%z

A7=[cos(theta7) -sin(theta7) 0 0;sin(theta7) cos(theta7) 0 0; 0 0 1

d7;0 0 0 1];%x

A2_0=A1*A2;
A3_0=A1*A2*A3;
A4_0=A1*A2*A3*A4;
A5_0=A1*A2*A3*A4*A5;
A6_0=A1*A2*A3*A4*A5*A6;
A7_0=A1*A2*A3*A4*A5*A6*A7;

o0=[0 ;0; 0];

z0=[0; 0; 1];

o1=[A1(1,4);A1(2,4) ;A1(3,4) ];
z1=[A1(1,3);A1(2,3) ;A1(3,3) ];

o2=[A2_0(1,4); A2_0(2,4) ;A2_0(3,4) ];

14
 z2=[A2_0(1,3); A2_0(2,3) ;A2_0(3,3) ];

o3=[A3_0(1,4); A3_0(2,4) ;A3_0(3,4) ];

z3=[A3_0(1,3); A3_0(2,3) ;A3_0(3,3) ];

o4=[A4_0(1,4); A4_0(2,4) ;A4_0(3,4) ];

z4=[A4_0(1,3); A4_0(2,3) ;A4_0(3,3) ];

o5=[A5_0(1,4); A5_0(2,4) ;A5_0(3,4) ];

z5=[A5_0(1,3); A5_0(2,3) ;A5_0(3,3) ];

o6=[A6_0(1,4); A6_0(2,4) ;A6_0(3,4) ];

z6=[A6_0(1,3); A6_0(2,3) ;A6_0(3,3) ];

o7=[A7_0(1,4); A7_0(2,4) ;A7_0(3,4) ];

J11=cross(z0,o7-o0 );
J12=cross(z1,o7-o1 );
J13=cross(z2,o7-o2 );
J14=cross(z3,o7-o3 );

J15=cross(z4,o7-o4 );
J16=cross(z5,o7-o5 );
J17=cross(z6,o7-o6 );

J=[J11 J12 J13 J14 J15 J16 J17 ;z0 z1 z2 z3 z4 z5 z6]

J is too large to write here as a symbolically

Singular postures for a manipulator is not desirable because at a singular posture, the inverse
kinematics of a manipulator does not have a meaningful solution. In practice, this means that
close to a singular posture, generating a velocity component in certain directions at the end-
effector of a manipulator requires very high joint rates, which are not physically possible for the
joints to afford. A redundant manipulator can avoid singular postures by exploiting its extra
DOFs than that required for a given main task. Here, the use of this feature of redundant
manipulators is discussed.
Singularity is avoided in simulation by restricting motions of all joints such that they not collide
themselves .
As it is redundant robot so we find pseudo inverse of jacobian.
In Matlab
Pinv(J) This command compute pseudo inverse of J.

15
2.5.1 Singular Value decomposition of jacobian :
SVD is based on a theorem from linear algebra which says that a rectangular matrix A can be
broken down into the product of three matrices-an orthogonal matrix U, a diagonal matrix S,
and the transpose of an orthogonal matrix V.
Amn =U mm Amn V Tnn

Where UU T =I, VV T =I

The columns of U are orthonormal eigenvectors of A AT ,the columns of V are orthonormal

eigenvectors of A AT ,and S is a diagonal matrix containing the square roots of eigenvalues
from U and V in descending order.
In matlab we can find singular values by following svd command
[U,S,V] = svd(J)

2.5.2 Null space of jacobian :

It can be used to avoid obstacles or joint angle limits.In null space motion of joint does
not effect the end effector pose.
Below equation is null space motion equation
q̇= J +¿(q) ẋ¿ +N N +¿ q̇ ns ¿

Where ẋis the end effector linear and angular velocities. It is 6 by 1 matrix. q̇ is joint velocities
NN +¿¿ can be used to avoid obstacle .N is null space motion of J(q).N=[n1,n2………nm] such that
J(q) ni=0
N is the set of matrix that is pre multiplied with jacobian matrix to result of zero.If robot has
100 joints than null space will be 100-6=94 .Than it will be 6*94 matrix.
My robot have 7 joints. Its null space will be 7*1 dimension.
N=null(J)
Above is matlab command to find null matrix from jacobian matrix.

0
(a4*cos(theta3 + theta4)*sin(theta3) - a4*sin(theta3 +
theta4)*cos(theta3))/(a2*(cos(theta2)*sin(theta3) - cos(theta3)*sin(theta2)))

16
 -(a2*a4*cos(theta3 + theta4)*sin(theta2) - a2*a4*sin(theta3 + theta4)*cos(theta2) +
a3*a4*cos(theta3 + theta4)*sin(theta3) - a3*a4*sin(theta3 +
theta4)*cos(theta3))/(a2*(a3*cos(theta2)*sin(theta3) - a3*cos(theta3)*sin(theta2)))

(a4*cos(theta3 + theta4)*sin(theta2) - a4*sin(theta3 + theta4)*cos(theta2) -

a3*cos(theta2)*sin(theta3) + a3*cos(theta3)*sin(theta2))/(a3*cos(theta2)*sin(theta3) -
a3*cos(theta3)*sin(theta2))

0
Above matrix is null matrix of my robot. its dimentions is 7*1.

Path planning
Different paths were given to robot. Every path contain some desire points .In every loop of
simulation we pick some desire point ,do inverse kinematics upto that desire point and then we

17
do forward kinematics and then we plot the robot. This loop continues until robot reaches all
desire points present in path.

Path Desire points Inverse

kinematics

Robot Forward
movement kinematics

Position Control
First we did the position control on robot. Controller of robot was designed to control the robot
position. disturbances were added in the system through guassian random function in matlab.
A Kalman filter was used to estimate the state of robot and then PID controller were
implemented. There were total 7 states of robot so 7 kalman filter were implemented and 7 PID
controller functions were designed.
18
 Desire
value

+ Inverse
error Theta
Kinematics Theta+disturbance
-

Robot Forward Kalman theta

output Dynamics Kinematics PID theta

PID control function is

function PID_theta1
global A
persistent uik1;
persistent errork1;

19
 % initialize persistent variables at beginning of simulation
if A.init==0
uik1 = 0;
errork1 = 0;
end

% error equation
error = A.theta1des-A.theta1_Kalman
% error = A.theta1_des

% proportional term
up = A.theta1_KP * error;

% integral term
if uik1>0.7
uik1=0;
end
ui = uik1 + A.theta1_KI * A.Ts/2 * (error + errork1);

error_derv=(error-A.error1_prev)/A.Ts
% angle1_der=(A.theta1_prev-A.theta1_pprev)/A.Ts
% derivative term
ud = A.theta1_KD*error_derv;

% implement PID control

A.theta1 = up + ui + ud;

A.error1_prev=error;
% update stored variables
uik1 = ui;
errork1 = error;

Kalman filtering function is

function Kalman_theta1
global A
persistent Pp Q R xp A1 H I
if isempty(Pp)
Pp=1; Q=5; R=100;
A1=0.5; H=1; I=eye(1); A.theta1=0;

20
end
% Compute priori % theta state measure by mathematical
mode/dynamics
% xp=A.q*A.Ts + xp; % dynamics//will be updated soon...
xp=A.theta1
Pp=A1*Pp*A1'+Q;

%////////////////////////////////////////////////////////////////
///////////

% Measurement update
K = Pp*H'*inv(H*Pp*H'+R);
A.theta1_kalman = xp+K*(A.theta1_meas-H*xp) % theta_meas
state is measured by realsensor model
A.theta1_kalman_plot(A.counter) = A.theta1_kalman;
% Update covariance
Pp=(I-K*H)*Pp;
xp = A.theta1_kalman;
% End or function

Pid tuning
Robot has total seven states . So,all seven thetas were tuned. Each have three constant kp, ki
and kd.Pid constants of seven thetas are following
A.theta1_KP = 1; % KP value in theta1 control
A.theta1_KI = 0; % KI value in theta1control
A.theta1_KD = 0.0; % KD value in theta1control

A.theta2_KP = 1; % KP value in theta2 control

A.theta2_KI =0.1; % KI value in theta2 control
A.theta2_KD = 0.0; % KD value in theta2 control

A.theta3_KP = 1; % KP value in theta3 control

A.theta3_KI = 0.2; % KI value in theta3 control
A.theta3_KD = 0.0; % KD value in theta3 control

A.theta4_KP = 1; % KP value in theta4 control

A.theta4_KI = 0.2; % KI value in theta4 control
A.theta4_KD = 0.0; % KD value in theta4 control

A.theta5_KP = 1; % KP value in theta5 control

A.theta5_KI = 0.2; % KI value in theta5 control
A.theta5_KD = 0.0; % KD value in theta5 control

A.theta6_KP = 1; % KP value in theta6 control

A.theta6_KI = 0.2; % KI value in theta6 control

21
A.theta6_KD = 0; % KD value in theta6 control

A.theta7_KP = 1; % KP value in theta7 control

A.theta7_KI = 0.1; % KI value in theta7 control
A.theta7_KD = 0.0; % KD value in theta7 control

Dynamics
Dynamic models of the robot manipulator describe the relation between the motion of the
robot, and the forces that cause the motion. A dynamic model is useful for, e.g., simulation,
control analysis, mechanical design, and real-time control. Some control algorithms require that
the inverse dynamics problem is solved. This means that the required actuator torque is
computed from the desired movement, including time derivatives. For simulation of the
manipulator movement, the direct dynamic problem must be solved. This means that the
dynamic model differential equations are solved with the actuator torques as input. Depending
on the intended use of the dynamic model, the manipulator can be modeled as rigid or elastic
There are several methods for obtaining a rigid dynamic model. The two most common
approaches are the Lagrange formulation and the Newton-Euler formulation. In the following,
only the Lagrange formulation will be described in some detail.

Euler Lagrangian Approach

The Euler Lagrangian Approach deals with the:

 Kinetic and Potential Energy of the manipulator, as it is moving, thus dealing with the
manipulator “As a Whole “in building force/torque equati ons.
 DH structural analysis to define “Generalized Coordinates” for the structural models of
the machine.
 It provides good insight into controller design related to State Space.
 It provides a closed form interpretation of the various components in the dynamic
model:
o D u e t o i n e r ti a
o D u e   t o G r a v i t a ti o n a l   E ff e c t s
o D u e t o F r i c ti o n ( j o i n t / l i n k / d r i v e r )
o D u e t o C o r i o l i s F o r c e s r e l a ti n g m o ti o n o f o n e l i n k   t o coupling eff ects
of other links’ moti ons
o Due to Centrifugal Forces that cause the link to have a tendency to fl y away
due to coupling to neighboring links and its own motion.
   
The dynamic model of a rigid manipulator can be expressed as

D(q)q¨+ C(q, q˙)q˙ + g(q) = τ

22
 where ,τ is the actuator torque vector, D(q) is the inertia matrix, c(q, q˙) is a vector of Coriolis
and centripetal torques, g(q) is the gravity torque vector.
The rigid body inertial parameters for each link are the mass, the center of mass, and the
inertia. The actuator inertia and mass are added to the corresponding link parameters.

KINETIC ENERGY

The kinetic energy of a manipulator can be expressed as:

For finding out the kinetic energy we need to find out the linear and angular jacobian and linear
and angular velocities of each and every link, inertia matrix of every link, Inertia Tensor matrix
of every link, potential energies and cristoffel symbols.

Linear and Angular Jacobians:

The Jacobians are already find out in the section5.We can extract the angular and linear
jacobian matrices from the jacobian matrix to find out the linear and angular velocities
respectively.

From Section 5:

J=[J11 J12 J13 J14 J15 J16 J17 ;z0 z1 z2 z3 z4 z5 z6]

First we extract the linear and angular matrices from the Jacobian matrix J.
%%%%........LINEAR JACOBIAN
Jv1x=J11(1,1);Jv1y=J11(2,1);Jv1z=J11(3,1);
Jv2x=J12(1,1);Jv2y=J12(2,1);Jv2z=J12(3,1);
Jv3x=J13(1,1);Jv3y=J13(2,1);Jv3z=J13(3,1);
Jv4x=J14(1,1);Jv4y=J14(2,1);Jv4z=J14(3,1);
%WRIST
Jv5x=J15(1,1);Jv5y=J15(2,1);Jv5z=J15(3,1);
Jv6x=J16(1,1);Jv6y=J16(2,1);Jv6z=J16(3,1);
Jv7x=J17(1,1);Jv7y=J17(2,1);Jv7z=J17(3,1);

%%%%%%%%.......ANGULAR JACOBIAN
Jw1x=z0(1,1);Jw1y=z0(2,1);Jw1z=z0(3,1);

23
Jw2x=z1(1,1);Jw2y=z1(2,1);Jw2z=z1(3,1);
Jw3x=z2(1,1);Jw3y=z2(2,1);Jw3z=z2(3,1);
Jw4x=z3(1,1);Jw4y=z3(2,1);Jw4z=z3(3,1);
%WRIST
Jw5x=z4(1,1);Jw5y=z4(2,1);Jw5z=z4(3,1);
Jw6x=z5(1,1);Jw6y=z5(2,1);Jw6z=z5(3,1);
Jw7x=z6(1,1);Jw7y=z6(2,1);Jw7z=z6(3,1);

Then compute the linear and angular Jacobian of each link.

Jv1,Jv2,……………….,Jv7 are the linear Jacobian matrices.

JV 1 x 0 0 0 0 0 0

[
J V 1= J V 1 y 0 0 0 0 0 0
J V 1z 0 0 0 0 0 0 ]
JV 1 x J V 2 x 0 0 0 0 0

[
J V 2= J V 1 y J V 2 y 0 0 0 0 0
J V 1z J V 2z 0 0 0 0 0 ]
JV 1 x J V 2 x JV 3 x 0 0 0 0

[
JV 3 = J V 1 y JV 2 y J V 3 y 0 0 0 0
J V 1 z J V 2 z JV 3 z 0 0 0 0 ]
J V 1 x JV 2 x J V 3 x J V 4 x 0 0 0

[
J V 4= J V 1 y J V 2 y J V 3 y J V 4 y 0 0 0
JV 1 z JV 2 z J V 3 z JV 4 z 0 0 0 ]
JV 1 x J V 2 x JV 3 x J V 4 x J V 5 x 0 0

[
JV 5 = J V 1 y JV 2 y J V 3 y JV 4 y JV 5 y 0 0
J V 1 z J V 2 z JV 3 z J V 4 z J V 5 z 0 0 ]
JV 1 x J V 2 x JV 3 x J V 4 x J V 5 x JV 6 x 0

[
JV 6 = JV 1 y JV 2 y J V 3 y JV 4 y JV 5 y J V 6 y 0
J V 1 z JV 2 z JV 3 z J V 4 z J V 5 z JV 6 z 0 ]
JV 1 x J V 2 x JV 3 x J V 4 x J V 5 x JV 6 x J V 7 x

[
JV 7 = JV 1 y JV 2 y J V 3 y JV 4 y JV 5 y J V 6 y JV 7 y
J V 1 z JV 2 z JV 3 z J V 4 z J V 5 z JV 6 z J V 7 z ]
Jw1,Jw2,………………,Jw7 are the angular Jacobian matrices.

24
 J w 1x 0 0 0 0 0 0

[
J w 1= J w 1 y 0 0 0 0 0 0
J w1 z 0 0 0 0 0 0 ]
J w 1 x Jw 2 x 0 0 0 0 0

[
J w 2= J w 1 y J w 2 y 0 0 0 0 0
J w1 z J w2 z 0 0 0 0 0 ]
J w 1 x J w 2 x J w3 x 0 0 0 0

[
J w 3= J w 1 y J w 2 y J w 3 y 0 0 0 0
J w1 z J w 2 z J w 3 z 0 0 0 0 ]
J w 1 x J w 2 x J w 3 x Jw 4 x 0 0 0

[
J w 4 = J w 1 y J w2 y J w3 y J w 4 y 0 0 0
J w 1 z J w 2 z Jw 3 z J w 4 z 0 0 0 ]
J w 1 x J w 2 x J w3 x J w 4 x J w5 x 0 0

[
J w 5= J w 1 y J w 2 y J w 3 y J w 4 y J w 5 y 0 0
J w1 z J w 2 z J w 3 z J w 4 z J w 5 z 0 0 ]
J w 1 x J w2 x J w3 x J w 4 x J w5 x J w 6 x 0

[
J w 6= J w 1 y J w 2 y J w 3 y J w 4 y J w 5 y J w6 y 0
J w 1 z J w 2 z J w 3 z Jw 4 z J w 5 z J w 6 z 0 ]
J w 1 x J w2 x J w3 x J w 4 x J w5 x J w 6 x J w 7 x

[
J w 7= J w 1 y J w 2 y J w 3 y J w 4 y J w 5 y J w6 y J w 7 y
J w1 z J w 2 z J w 3 z J w 4 z J w 5 z J w 6 z J w7 z ]
Linear and Angular Velocities:
Then find out the linear and angular velocities according to the formulas which is

Linear velocities can be find out by multiplying the Linear

jacobian with the derivative of theta.We can use the ‘q’ instead
of theta in the code.
v1 =J v 1 × θ̇1
v 2=J v 2 × θ̇2
v3 =J v 3 × θ̇3

25
v 4=J v 4 × θ̇4
v5 =J v 5 × θ̇5
v 6=J v 6 × θ̇6
v7 =J v 7 × θ̇7

Similarly, angular velocities can be find out by multiplying the

Angular jacobian with the derivative of theta.
w 1=J w 1 × θ̇1
w 2=J w 2 × θ̇2
w 3=J w 3 × θ̇3
w 4=J w 4 × θ̇ 4
w 5=J w 5 × θ̇5
w 6=J w 6 × θ̇ 6
w 7=J w 7 × θ̇7

Take the transpose of linear and angular jacobian matrices by using the command of transpose
in MATLAB to find out the transpose of linear and angular velocities.

v1 '=transpose(J ¿ ¿ v 1)× θ̇1 ¿

v 2 '=transpose(J v 2) × θ̇2
v3 '=transpose( J v 3)× θ̇3
v 4 '=transpose ( J ¿¿ v 4 )× θ̇4 ¿
v5 '=transpose( J ¿¿ v 5)× θ̇5 ¿
v 6 '=transpose (J ¿¿ v 6)× θ̇6 ¿
v7 '=transpose( J v 7) × θ̇7

w 1 ' =transpose (J ¿¿ w 1)× θ̇ 1 ¿

w 2 ' =transpose (J w 2)× θ̇2
w 3 ' =transpose (J w 3)× θ̇ 3
w 4 '=transpose(J w 4) × θ̇ 4
w 5 ' =transpose (J ¿¿ w 5)× θ̇5 ¿
w 6 ' =transpose (J w 6 )× θ̇6
w 7 ' =transpose (J ¿¿ w 7) × θ̇7 ¿
‘

Inertia Tensor Matrix:

26
Now comes to the inertia tensor matrix. If we denote as I the inertia tensor expressed instead in
the body attached frame, then the two matrices are related via a similarity transformation

according to

Moment of inertial matrices of the links are given below:

For link 1
ρ× aI 1 × bI 1× cI 1
I 1 xx = ×b I 12 ×cI 12
12
ρ× aI 1 ×bI 1× cI 1
I 1 yy = × a I 12 ×cI 12
12
ρ× aI 1 ×bI 1× cI 1
I 1 zz = ×b I 12 ×aI 12
12

I 1 xx 0 0
I 1= 0
0[ I 1 yy 0
0 I 1 zz ]
For link 2 3 4
ρ × aI 2 ×bI 2 × cI 2
I xx = × b I 22 × cI 22
12
ρ ×aI 2× bI 2 ×cI 2
I yy = × a I 22 × cI 22
12
ρ ×aI 2× bI 2 × cI 2
I zz = × b I 22 × aI 22
12

I xx 0 0
I= 0
0 [ I yy 0
0 I zz ]
For link 5 6
For link 5 and 6,this matrix is zero as it has no length width and height. It is just a point.

For link 7
ρ ×aI 7 ×bI 7 ×cI 7
I 7 xx = × b I 72 ×cI 72
12
ρ ×aI 7 ×bI 7 × cI 7
I 7 yy= × a I 72 × cI 7 2
12
ρ ×aI 7 ×bI 7× cI 7
I 7 zz = × b I 72 ×aI 72
12
27
 I 7xx 0 0

[
I 7= 0
0
I 7 yy 0
0 I 7 zz ]
Inertia Tensor Matrix can be easily be evaluated by multiplying the rotation matrices, Mass
Inertia matric and the transpose of rotation matrices. The Rotation matrices R1, R2,R3,
R4,R5,R6.R7 are obtained from the homogenous transformation of the links which is computed
in the Section of forward kinematics and take the transpose of these rotation matrices to get the
transposed matrices.

InTensor 1=R1 × I 1 × R1 '

InTensor 2=R1 × R2 × I × R 1 ' × R 2 '
InTensor 3=R1 × R3 × I × R1 ' × R 3 '
InTensor 4=R1 × R3 × R4 × I × R1 ' × R3 ' × R 4 '
InTensor 5=R1 × R5 × I 5 × R 1 ' × R 5 '
InTensor 6=R1 × R5 × R6 × I 6 × R1 ' × R5 ' × R6 '
InTensor 7=R1 × R5 × R6 × R 7 × I 7 × R1 ' × R5 ' × R6 ' × R7 '

Now we can easily find out the kinetic energy by putting the values of linear and angular
velocities and their transpose along with the inertia tensor matrices.

1 1
K 1= ×m 1 ×v '1 × v 1+ × w1 ' × InTensor 1× w1
2 2

1 1
K 2= × m2 × v'2 × v 2+ × w 2 ' × InTensor 2× w 2
2 2

1 1
K 3= × m3 × v '3 × v 3 + × w3 ' × InTensor 3 × w3
2 2

1 1
K 4= × m 4 × v '4 × v 4 + × w4 ' × InTensor 4 ×w 4
2 2

1 1
K 5= × m5 × v '5 × v 5 + × w5 ' × InTensor 5 × w5
2 2

1 1
K 6= × m 6 × v '6 × v 6+ × w6 ' × InTensor 6 × w6
2 2

1 1
K 7= × m 7 × v '7 ×v 7 + × w7 ' × InTensor 7 ×w 7
2 2

The total kinetic energy is obtained by summing up all the kinetic energies.

28
K= K 1+ K 2+ K 3+ K 4 + K 5+ K 6 + K 7

Inertia Matrix which is represented by D(q) can be computed as follows:

%%%%%%%%%Inertia Matrix
D 1=m 1 × Jv '1 × Jv 1+ J w 1 ' × InTens∨1 × Jw1
D 2=m2 × Jv '2 × Jv 2+ J w2 ' × InTensor 2 × Jw2
D 3=m3 × Jv '3 × Jv 3 +J w3 ' × InTensor 3 ×Jw 3
D 4=m4 × Jv '4 × Jv 4 + J w 4 ' × InTensor 4 × Jw 4
D 5=m5 × Jv '5 × Jv 5 +J w5 ' × InTensor 5× Jw 5
D 6=m6 × Jv '6 ×Jv 6+ J w6 ' × InTensor 6 × Jw 6
D 7=m7 × Jv '7 × Jv 7 + J w 7 ' × InTensor 7 × Jw7

D=D 1+ D 2+ D3+ D 4+ D 5+ D 6+ D 7

First we get the Inertia matrix of first four links which is of the order of 4×4 matrix and that matrix is symmetric
positive definite thatis verified from the MATLAB.

d 11 d 12 d 13 d 14
d

[
D= 21
d 31
d 41
d 22
d 32
d 42
d 23
d 33
d 43
d 24
d 34
d 44
]
Potential energy is defined as :
P=mgh
Actually the height of the links is along the z-axis. So we can obtained the height of every link
from the homogenous matrices .The 4th column of that matrix gives the translational
components along x,y and z axis. From 3rd element of the 4th column ,we can obtain the z-axis
which is ultimately the height of link and vice versa.
The potential energy of the first 4 links are given below:
d1
P 1=m1 × g ×
2
l2
P 2=m2 × g×( d 1+ ×sin q 2)
2
l3
P 3=m3 × g ×(d 1+ l2 sin q 2+ ×sin q 3)
2
P 4=m 3 × g ׿

29
P=P 1+ P 2+ P 3+ P 4
Now take the derivative of potential energy w.r.t each theta one by one to get the phi’s and
then make a matrix of it for further use in finding out the torque.
dP
∅ 1=
d q1

dP
∅ 2=
d q2

dP
∅ 3=
d q3

dP
∅4=
d q4

So, the final gravity matrix is here,

∅1
∅
∅= 2
∅3
∅4
[]
CHRISTOFFEL SYMBOLS

The general equation of finding the christoffel symbol is given below:

1 ∂ d kj ∂ d ki ∂ d ij
c ijk = { +
2 ∂ qi ∂ j
−
∂k }

Now simply use the formula of dynamics model to get the torques of the first four link as we
have the D(q) ,C(q) and g(q) of the first four links.

τ =D ( q ) q̈+C ¿

30
 d 11 d 12 d 13 d14 c 111 c122 c133 c 144 ∅1

[
d
τ = 21
d31
d 41
d 22
d 32
d 42
d 23
d 33
d 43
d24
d34
][
d 44
q̈ +
c 211
c 311
c 411
c 222
c322
c 422
c233
c333
c 433
][]
c 244
c 344
c 444
q̇ +
∅2
∅3
∅4

Welding
Final GUI
reference points

Robot Path

Figure. Robot path completed

Plots:
All plots are at frequency 166.6667 Hz

31
32
PID controller give different results at different frequency ,as we increase frequency, PID
controller give better results.
As we can see below controller result at 5k Hz frequency are

33
Selection of motors for robot links
According to design requirements we can use servo motors for robot links.
We can use SM series for servo motors ,which is chosen because of following specifications
 It is designed to meet the highest requirement for dynamism and accuracy
 Compact size
 High power density
 Low rotor inertia
 High resistance against winding damage
From SM series , we can use SM-100 servo motor.

34
General technical data

Appendix:

35
Inertia of rectangle rod:
This is an important observation because the inertia matrix expressed in the body attached frame
is a constant matrix independent of the motion of the object and easily computed. We next show
how to compute this matrix explicitly.

The above calculation can be used to find inertia of robot links.

References:
1. Gnanavelbabu, A., P. Arunagiri, et al. "Implementation of Robotic Welding for the Improvement
of Production Systems." International Journal of Engineering Trends and Technology (IJETT)–
Volume 49.

2. Kah, P., M. Shrestha, et al. (2015). "Robotic arc welding sensors and programming in industrial
applications." International Journal of Mechanical and Materials Engineering 10(1): 13.

3. Kuss, A., T. Dietz, et al. (2017). "Manufacturing task description for robotic welding and
automatic feature recognition on product CAD models." Procedia CIRP 60(1): 122-127.

36
37