Robot Dynamics

1
 Dynamic model of a robot
Forward dynamics
Given forces/torques applied at the joints, find the
velocity and acceleration of the various links
Analysis problem
Inverse dynamics
Given required acceleration and velocity of the
various links, find the required force/torque at the
joints
Design problem
2
 Dynamic Model
Two important approaches
• Euler-Lagrangian approach

• Newton-Euler approach

The EL method gives closed form solution which is

useful for model based controller design
The NE method is iterative method which is useful for
computation

3
 The Differential Relationships
Definition of Differential Motion : A small movements of
mechanism that can be used to derive velocity relationships
between different parts of the mechanism.
The velocity of point B :
. ^ . ^ . . ^
V B  V A  V B / A  l1 1 sin 1 i  l1 1 cos1 j  l2 (1   2 ) sin(1   2 ) i
. . ^
 l2 (1   2 ) cos(1   2 ) j
V Bx   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 ) 1 
    
V B y   l1 cos 1  l 2 cos( 1   2 ) l 2 cos( 1   2 )  2 

4
Velocity relationship of point B :

 xB  l2 cos1  l2 cos(1   2 )


 y B  l1 sin 1  l2 sin(1   2 )

dxB  l2 sin 1d1  l2 sin(1   2 )(d1  d 2 )


 dyB  l1 cos1d1  l2 cos(1   2 )(d1  d 2 )

dxB   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 )  d1 

dy    l cos  l cos(   ) l cos(   )  d 
 B  1 1 2 1 2 2 1 2  2

Differential Jacobian Differential

Motion of B Motion of Joint

5
 Jacobian
Definition : Jacobian is a representation of the geometry of the
elements of a mechanism in time.
Formation : Jacobian is formed from the elements of the
position equations that were differentiated with respect to 1 and
2.
Assumption : A set of equations Yi in terms of a set of variables xj:
Yi  fi ( x1 , x2 , x3 ,, x j )
 f1 f1 f1
Y1  x1 x1  x2 x2    x j x j

Y2  f 2 x1  f 2 x2    f 2 x j
 x1 x2 x j
 
 f i f i f i
Yi  x1   x2    x j
 x1 x2 x j
6
 Jacobian Cont’d

 f1 f1 f1 

Y1   x1  x1 
  x2 x j   
 
   f 2   
Y2       x2 
 
  x1   Matrix Representation
     

Yi    fi x j 

        
     
 
 Yi 
 f i 
f i  
  xi 
    x j 
 x1 x j   

dx  d1 
dy  d 
   2
dx   Robot  d 3  Differentiating the position
   d  equations of a robot

  
x Jacobian 4  D  J D 
y  d 5 
   

 
z 
 6 
d
7
 Differential Motions of A Frame
The differential motion of a hand frame of the robot are caused by
the differential motions in each of the joints of the robot.
The differential motion of a frame:
 Differential translations,
 Differential rotations,
 Differential transformations(translations and rotations).

Definition : A translation of a frame at differential values.

Representation : Trans(dx, dy, dz)
 The frame has moved a differential amount along the 3 axes. 8
 Differential Motions of A Robot And
Its Hand Frame
Relation between the differential motions of the joint of the robot
and the differential motions of the hand frame and dT.
It is a function of the robot’s configuration and design and its
instantaneous location and orientation.

dx  d1 
dy  d 
   2
dx   Robot  d 3  OR D  J D 
   d 

  
x Jacobian 4 
y  d 5 
   
z  d 6 

9
 Calculation of The Jacobian
Key point : Each element in the jacobian is the derivative of a
corresponding kinematic equation with respect to one of the
variables.
 p x
 1  J11   S1 (C234 a4  C23a3  C2 a2 )

 p x  C1 (C234 a4  C23a3  C2 a2 )  p x  J12  C1 ( S 234 a4  S 23a3  S 2 a2 )
  2
 p   S (C a  C a  C a )  
 x    1 234 4 23 3 2 2   p x  J13  C1 ( S 234 a4  S 23a3 )
 p x   S 234 a4  S 23a3  S 2 a2    3

    Taking the derivative of px  p x  J14  C1 ( S 234 a4 )
  
1 1    4
 p x
  J15  0
  5
The last column of the forward  p x
  J16  0
kinematic equation of the robot   6
The first row of the Jacobian
10
 Calculation of The Jacobian
The velocity equation relative to the last frame

[T6 D] [T6 J ][ D ]

The differential motion relationship of Equation

 T6 d x   T6 J11 T6 J12  J16  d1 
T6

 T6   T6 
 J 26  d 2 
T6
 d y   J 21 T6 J 22
 T6 d z   T6 J 31  T6 J 36  d 3 
    
 T6 x   T6 J 41   J 46  d 4 
T6

 T6   T6 
  x   J 51   T6 J 56  d 5 
 
 T6   T6  T6 J 66  d 6 
  x   J 61  
11
 Inverse Jacobian
Inverse Jacobian used to calculate the differential motions needed
at the joints of the robot for a desired hand differential motion.
Inverse Jacobian calculates how fast each joint must move so that
the robot’s hand will yield a desired differential motion or
velocity.
To make sure the robot follows a desired path, the joint velocities
must be calculated continuously in order to ensure that the robot’s
hand maintains a desired velocity.
[ D]  [ J ][D ]
[ J 1 ][D]  [ J 1 ][ J ][D ]  [ D ]  [ J 1 ][D]
[T6 J 1 ][T6 D] [T6 J 1 ][T6 J ][D ]  D [T6 J 1 ][T6 D]
12
 Review
Steps to derive kinematics model:
 Assign D-H coordinates frames
 Find link parameters

 Transformation matrices of adjacent joints

 Calculate kinematics matrix

 When necessary, Euler angle representation

13
 Review Cont’d
D-H transformation matrix for adjacent coordinate
frames, i and i-1.
The position and orientation of the i-th frame coordinate can
be expressed in the (i-1)th frame by the following 4
successive elementary transformations:
Ti i 1  T ( zi 1 , d i ) R( zi 1 ,  i )T ( xi , ai ) R( xi ,  i )
C i  C i S i S i S i ai C i 
 S C i C i  S i C i ai S i 
 i
 0 S i C i di 
 
 0 0 0 1 
14
 Review Cont’d
Kinematics Equations
chain product of successive coordinate transformation
i
matrices of Ti 1
n
T0 specifies the location of the n-th coordinate frame w.r.t.
the base coordinate system
T0n  T01T12 Tnn1
Orientation
 R0n P0n  n s a P0n 
matrix
  
0 1  0 0 0 1 
Position
vector
15
 Jacobian Matrix
1  Forward x  1 
  y  x 
 
 2 
 y 
 2   Jacobian  
 3  z  3   z 
  Kinematics    
Matrix  
 4     4   x 
 y 
 5       
     5  z 
 6  Inverse   6 
Joint Space Task Space Jaconian Matrix: Relationship between joint
space velocity with task space velocity

16
 Forward kinematics
x   q1   h1 (q1 , q2 ,  , q6 ) 
y q  h (q , q ,  , q )
    2  2 1 2 6 
z   q3   h3 (q1 , q2 ,  , q6 )  Y61  h(qn1 )
   h (   61 
)  

  q
 4 h (
 4 1 2q , q ,  , q )
6 
   q5   h5 (q1 , q2 ,  , q6 ) 
     
   6 
q  6 1 2
h ( q , q ,  , q 6 
)  61

 d dh(q) dq dh(q)
Y61  h(qn1 )   q
dt dq dt dq
 x 
 y   q1 
  q 
 z   dh(q)   2 
   dq     Y61  J 6n q n1
 x    6n
 y    J
dh(q)
 
 z 
q n  n1 dq
17
 Forward kinematics Cont’d
Forward Kinematics
 x   h1 (q) 
p   y   h2 (q)
n s a p  z   h3 (q) 
T 
6

1  44
0
0 0 0  (q)  h4 (q)
 h1 (q )  {n, s, a}   (q)   h5 (q) 
h (q )  (q) h6 (q)
Y61  h(q)   2 
    x 
   y  Linear velocity Angular velocity
 
h6 (q)   z  V   

Y   
 x 
 
 x   V   y     
Y61  J 6n q n1  y 
 
   z   
 z 
18
 Forward kinematics Cont’d
 x 
 y 
   q1 
 z   dh(q)  q 2 
     Jacobian is a function of q, it

 x   dq  6n   
is not a constant!
 y   
 q n  n1
 
 z   h1 h1 h1 
 q 
q2 qn 
 1 
 dh(q )   h2 h2

h2 
J      q1 q2 qn 
 dq  6n      
 h h6 h6 
 6  
 q1 q2 qn  6n
19
 Forward kinematics Cont’d
Physical Interpretation

 q1 
 J11 J12  J16  q 
J J 22  J 26 
 2
 q3 
Y  Jq   21  
      q 4 
 q5 
   
 J 61 J 62  J 66  q6 

 x   J11q1  J12 q 2    J16 q6 

 y   J q  J q    J q 
   21 1 22 2 26 6 

 z   J 31q1  J 32 q 2    J 36 q6  How each individual joint space

     velocity contribute to task space

   41 1 J q  J q
42 2
    J q
46 6 
velocity.
   J 51q1  J 52 q 2    J 56 q6 
   
   J 61q1  J 62 q 2    J 66 q6 
20
 Example
2-DOF planar robot arm given l1, l2 ,
(x , y)
Find: Jacobian

2
 x  l1 cos1  l2 cos(1   2 )  h1 (1 , 2 ) 
 y    l sin   l sin(   )   h ( , ) l2
  1 1 2 1 2   2 1 2 

  x  1 
Y     J  1 l1

 y   2 

 h1 h1 
   2   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 )
J  1 
 h2 h2   l1 cos1  l2 cos(1   2 ) l2 cos(1   2 ) 
 1  2 
21
 Singularity
Find the singularity configuration of the 2-DOF planar
robot arm
 x 
Y     J  1 

 y   2 
 l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 ) V
J   (x , y)
 1
l cos  1  l 2 cos( 1   2 ) l 2 cos(1   2 
)

determinant(J)=0 Not full rank l2

Y

2  0 2 =
0
Det(J)=0 1 l1
x
22
 Continued…
Inverse Jacobian
 q1  q5
 J11 J12  J16  q 
J J 22  J 26 
 2
 q3 
Y  Jq   21  
      q 4 
 q5 
   
 J 61 J 62  J 66  q6 
q1
q  J Y1

Singularity
rank(J)<min{6,n},
Jacobian Matrix is less than full rank
Jacobian is non-invertable
Occurs when two or more of the axes of the robot form a
straight line, i.e., collinear
Avoid it
23
 Inverse of Jacobian Matrix
Pseudoinverse
Let A be an mxn matrix, and let A be the pseudoinverse of A. If
A is of full rank, thenA can be computed as:
 AT [ AAT ]1 mn
  1
A  A mn
[ AT A]1 AT mn

Example:

1 0 2 3 x  Ab  1 / 9[5,13,16]T

1  1 0 x   2
   
1 1  1 1 4 
 T 1   5 1 1 
A  A [ AA ]  0  1 
T
   1  5 
2 0   
1 2 9
4  2
24
 Equation of Motion

Mathematical equations describing the dynamic

behavior of the manipulator
For computer simulation
Design of suitable controller
Evaluation of robot structure
Joint torques Robot motion, i.e. acceleration,
velocity, position

25
 Equation of Motion Cont’d
Lagrange-Euler Formulation
d L L
( )  i
dt qi qi
Lagrange function is defined
L K P
K: Total kinetic energy of robot
P: Total potential energy of robot
qi: Joint variable of i-th joint qi
q i: first time derivative of
 i : Generalized force (torque) at i-th joint

26
 Equation of Motion
Kinetic energy
1 2
Single particle: k
mv
2
Rigid body in 3-D space with linear velocity (V) , and
angular velocity ( ) about the center of mass
1 1 T
k  mV V  I 
T

2 2
 ( y 2  z 2 )dm   xzdm 

  xydm

I     xydm  ( x  z )dm   yzdm 
2 2


   xzdm   yzdm  
2 2
( x y )dm

I : Inertia Tensor: I xx   ( y 2  z 2 )dm
Diagonal terms: moments of inertia
Off-diagonal terms: products of inertia I xy   ( xy )dm
27
 Velocity of a link
 xi 
 y  A point fixed in link i and expressed w.r.t. the
ri 
i  i  i-th frame
yi
 zi  Same point w.r.t the base frame zi
  i
 1  r0  T0i rii  (T01T12 Ti i 1 )rii i xi
i z r
r
Velocity of point i expressed w.r.t. i-th frame
i 0
is zero r i  0
i r0i y
0
Velocity of point ri iexpressed w.r.t. base frame is:
d i d 1 2 x0
Vi  V0i  r0  (T0 T1 Ti i 1 )rii
dt dt
 T01T12 Ti i 1rii  T01T12 Ti i 1rii 
T i i
  T T T r  T r  (
1 2 i
i 1 i
i 0
q j )rii
i i

j 1 q j
0 1 0 i

28
 Rotary joints
Rotary joints, qi  i
C i  C i S i S i S i ai C i 
 S C  i C  i  S i C  i ai S i 
Ti 1 
i  i

 0 S i C i di  0  1 0 0
 
 0 0 0 1  1 0 0 0
Qi  
 S i  C i C i S i C i  ai S i  0 0 0 0
 ai C i   
Ti i 1  C i  C i S i S i S i
 0 0 0 0
qi  0 0 0 0 
 
 0 0 0 1 

0  1 0 0 C i  C i S i S i S i ai C i 
1 0 0  S i C i C i  S i C i ai S i 
Ti i 1 0
 QiTi 1  
i

qi 0 0 0 0  0 S i C i di 
  
0 0 0 0  0 0 0 1 
29
 Prismatic joint
Prismatic joint, qi  d i
C i  C i S i S i S i ai C i 
 S C i C i  S i C  i ai S i 
Ti 1   i
i
 0 S i C i di 
 
 0 0 0 1 
0 0 0 0
Ti i 1 0 0 0 0
qi
 QiTi i 1
Qi  
0 0 0 1
0 0 0 0  
0 0 0 0
0 0
Ti 1 
i
0 0

qi 0 0 0 1
 
0 0 0 0
30
 Velocity of a link

The effect of the motion of joint j on all the points on link i

T0i T01T12 T j j21Q jT j j1 Ti i 1 for j  i


q j  0 for j  i

T0i T0 j 1Q jT ji1 for j  i

U ij  
q j  0 for j  i

d d i
T i i
Vi  V0i  r0i  (T01T12 Ti i 1 )rii  ( 0 q j )rii  (U ij q j )rii
dt dt j 1 q j j 1

31
 Kinetic Energy of link i
Kinetic energy of a particle with differential mass dm in link i
1 2 1
dK i  ( xi  y i  zi )dm  trace(ViVi )dm
2 2 T

2 2
1  i i 
 Tr  U ip q p ri ( U ir q r ri )  dm
i i T

2  p 1 r 1  n

1  i i  Tr ( A)   aii
 Tr  U ip ri ri U ir q p q r  dm
i iT T
i 1
2  p 1 r 1 
1  i i 
 Tr  U ip (ri dmri )U ir q p q r 
i iT T

2  p 1 r 1 

32
 Kinetic Cont’d
1  i i 
K i   dK i  Tr   U ip (  ri ri dm)U ir q p q r 
i iT T

2  p 1 r 1 
 xi 2 dm
  xi yi dm  xi zi dm  xi dm   xi 
  y 
I i   rii rii dm        ri   i
2
T  x i y i dm y i dm y i z i dm y i dm 
 i i  ii  i dm  zi dm  i  zi 
2
x z dm y z dm z
 x dm
  y dm  z dm  dm   
 i i i  1
  I xx  I yy  I zz  1

 2
I xy I xz m i i
x

xi  
mi
xi dm
 I xx  I yy  I zz  Center of mass
 I I m y
xy
2
yz i i

 I xx  I yy  I zz 
 I xz I yz mi zi 
Pseudo-inertia
 2 
 mi xi mi yi mi zi mi  matrix of link i
33
 Total Kinetic Energy
Total kinetic energy of a robot arm
n
1 n  i i 
K   K i   Tr   U ip (  ri ri dm)U ir q p q r 
i iT T

i 1 2 i 1  p 1 r 1 
1 n i i

  Tr (U ip I iU ir )q p q r
2 i 1 p 1 r 1
T


Scalar quantity, function of qi and q i , i  1,2,n

I i : Pseudo-inertia matrix of link i, dependent on the mass distribution

of link i and are expressed w.r.t. the i-th frame,
Need to be computed once for evaluating the kinetic energy
34
 Potential Energy of link i
Potential energy of link i
r0i : Center of mass
w.r.t. base frame
Pi   mi gr   mi g (T r )
0
i i i
0 i ri i : Center of mass
w.r.t. i-th frame

g  ( g x , g y , g z ,0) g : gravity row vector

expressed in base frame
g  9.8m / sec 2

Potential energy of a robot arm

n n
P   Pi   [mi g (T0i ri i )] Function of qi
i 1 i 1

35
 Lagrangian Equation
The Lagrangian function becomes

1 n i i
 
n
L  K  P   Tr (U ij I iU ik )q j q k   mi g (T0i ri i )
T

2 i 1 j 1 k 1 i 1

d L L
i  ( ) 
dt qi qi
n j n j j
U jk
  Tr (U jk I jU ji )qk   Tr ( I jU ji )q k q m
T T

j i k 1 j i k 1 m 1 qm
n
  m j gU ji rj j
j i

36
 Example
Using the aforementioned equations, derive the equations of
motion for the two-degree of freedom robot arm. The two links
are assumed to be of equal length.
Follow the same steps as before…….
 Write the A matrices for the two links;
 Develop the Dij , Dijk and Di for the
robot.

The final equations of motion without the actuator inertia terms are the same as below.
1 4  1 1 
T1   m1l 2  m2l 2  m2l 2C2 1   m2l 2  m2l 2C2 2
3 3  3 2 
1 
  1 1
  m2l 2 S 2 22  m2l 2 S 2 12  m1 glC1  m2 glC12  m2 glC1  I1( act )1
2  2 2
1 1  1  1  1
T2   m2l 2  m2l 2C2 1   m2l 2 2   m2l 2 S 2   m2 glC12  I 2( act )1
3 2  3  2  2 37
 Manipulator Dynamics
The effect of the motion of joint j on all the points on link i

T  j 1
i i
T0 Q jT j 1 for j  i
U ij   0
q j  0 for j  i
The interaction effects of the motion of joint j and joint k on all the points on
link i

 T0 j 1Q jT jk11Qk Tki1 ik j

U ij  k 1 j 1
 U ijk   T0 Qk Tk 1 Q jT j 1 i
i jk
qk 0
 i j or i  k
38
 Continued…
Dynamics Model
n n n
 i   Dik qk   hikmq k q m  Ci
k 1 k 1 m 1
n
Dik   Tr (U
T
jk I jU ji )
j  max(i , k )
n
hikm   Tr (U
T
jkm jI U ji )
j  max(i , k , m )

n
Ci   m j gU ji rj j

j i

39
 Continued…
Dynamics Model of n-link Arm

  D(q)q  h(q, q )  C(q)

 D11  D1n 
D     The Acceleration-related Inertia matrix
 term, Symmetric
 Dn1  Dnn 

 h1 
h(q, q )     The Coriolis and Centrifugal terms
hn 

 C1   1 
C (q)          Driving torque applied
The Gravity terms
Cn  on each link
 n 

40
 Example
Example: One joint arm with point mass (m) concentrated at the
end of the arm, link length is l , find the dynamic model of the
robot using L-E method.

L
Set up coordinate frame as in the figure
Y0 m

l 
g  [0,9.8,0,0]
Y1
X1

0 
1

r11   
X0

0 
  C1  S1 0 0
1   S C1 0 0 1
r0  T0 r1 
1 1 1  1
r1
 0 0 1 0
 
 0 0 0 1
41
 Solution
C1  S1 0 0
 S C1 0 0 1
r01  T01r11   1
L
r1
 0 0 1 0 Y0 m

 
 0 0 0 1 Y1
X1
1

X0

 l  S1 
 l  C 
d 1 1
V1  r0  T0 r1  Q1T01r11  
1 1  

dt  0 
 
 0 
42
 Solution Cont’d
1
dK  Tr (V1V1 )dm
T
Kinetic energy
2
 l  S1 
 l  C 
1
K   Tr (  1 
 l  S1 l  C1 0 0)12 dm
2  0 
 
 0 
1 2  1 2 2
 [l ( S1 )  l (C1 ) ]m  l m1
2 2 2 2

2 2
 l 2  ( S1 ) 2  l 2  S1  C1 0 0
 2 
1   l  S1  C1 l 2  (C1 ) 2 0 0  2
 Tr ( )1 m
2  0 0 0 0
 
 0 0 0 0
43
 Solution Cont’d
Potential energy C1  S1 0 0  l 
 S C1 0 0 0
P  mg (T01r1 )  m0  9.8 0 0 1
 0 0 1 0  0 
  
 9.8m  l  S1  0 0 0 1 1
Lagrange function
1 2 2
L  K  P  l m1  9.8m  l  S1
2
Equation of Motion
d L L
1  (  ) 
dt 1 1
d 2 
 (l m1 )  9.8m  l  C1  l 2 m1  9.8m  l  C1
dt
44
 Example: Puma 560
Derive dynamic equations for the first 4 links of PUMA 560
robot

Set up D-H Coordinate frame

Get robot link parameters
Get transformation matrices Ti i 1 45
 Solution
Get D, H, C terms
n
Dik   Tr (U n  3; i  1,2,3
T
jk I jU ji )
j  max(i , k )

D11  Tr (U11 I1U11 )  Tr (U 21I 2U 21 )  Tr (U 31 I 3U 31 )

T T T

D12  D21  Tr (U 22 I 2U 21 )  Tr (U 32 I 3U 31 )
T T

D13  D31  Tr (U 33 I 3U 31 )
T

D22  Tr (U 22 I 2U 22 )  Tr (U 32 I 3U 32 )
T T

D23  D32  Tr (U 33 I 3U 32 )
T

D33  Tr (U 33 I 3U 33 )
T

46
 Solution Cont’d
Get D, H, C terms n n

 h1  hi   hikmq k q m
h(q, q )    
k 1 m 1
n
hikm   Tr (U jkmI jU ji )
T
hn  j  max(i , k , m )

h111  Tr (U111I1U11 )  Tr (U 211I 2U 21 )  Tr (U 311I 3U 31 )

T T T

h1  h111q1  h112 q1q 2  h113q1q3  h121q1q 2  h122 q 2

2 2

 h123q 2 q3  h131q3q1  h132 q3q 2  h133q3

2

h112  Tr (U 212 I 2U 21 )  Tr (U 312 I 3U 31 )

T T

h113  Tr (U 313 I 3U
T
31 )
h121  Tr (U 221I 2U )  Tr (U 321I 3U
T T
21 31 )
h123  Tr (U 323 I 3U 31 )
T

h122  Tr (U 222 I 2U 21 )  Tr (U 322 I 3U 31 )

T T

47
 Solution Cont’d
Get D, H, C terms
 T0 j 1Q jT jk11Qk Tki1 ik j
U ij  k 1
 U ijk   T0 Qk T jk11Q jT ji1 i jk
qk 0
 i j or i  k

U 111  (Q1 ) 2 T01 U 211  (Q1 ) T

2 2 U 311  (Q1 ) 2 T03
0

U 212  U 221  Q T Q T 1 2 U 312  U 321  Q1T01Q2T13

1 0 2 1

U 313  Q T Q T
1 0
2
3 2
3 U 222  T01 (Q2 ) 2 T12 U 322  T01 (Q2 ) 2 T13

U 323  U 332  T01Q2T12Q3T23 U 331  Q1T02Q3T23 U 333  T02Q3Q2T23

48
 Solution Cont’d
Get D, H, C terms
n
Ci   m j gU ji rj j

j i

C1   m1 gU11r11  m2 gU 21r22  m3 gU 31r33

C2   m2 gU 22 r22  m3 gU 32 r33

C3  m3 gU 33r33

49