Chapter 6. Manipulator dynamics.

Page 45

CHAPTER 6. MANIPULATOR DYNAMICS.

6.2 Determine the dynamic equations for the two-link manipulator in Section 6.7
when each link is modeled as a rectangular solid of homogeneous density. Each
link has dimensions li, wi, and hi, and total mass mi.

L1
1
PC 1 = X̂ 1
2
L
2
PC 2 = 2 X̂ 2
2
 m1 2 
 12 ( h1 + L1 )
2
0 0 
 m1 2 
C1
P1 =  0 ( w1 + h12 ) 0 
 12 
 m1 2 2 
 0 0 ( L1 + w1 )
12 
 m2 2 
 12 ( h2 + L2 )
2
0 0 
 m2 
C2
P2 =  0 ( w22 + h22 ) 0 
 12 
 m2 2 2 
 0 0 ( L2 + w2 )
12 
0 
1
w1 = θ
1 Ẑ 1 =  0 
1

θ
1 
0 
1
w
1 = θ

1 1 Ẑ 1 =  0 
θ

1 

 gs1 
1
v
1 =  gc1 
 0 
 L1
2 
 − 2 θ 1 + gs1 
 L 
1
v
C 1 =  1 θ

12 + gc1 
 2 
 0 
 

a. r./6.11.2003 1:39 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 6. Manipulator dynamics. Page 46

 L1
2 
− m1 2 θ 1 + m1 gs1 
 L 
1
F1 =  m1 1 θ

12 + m1 gc1 
 2 
 0 
 
 
 0 
1
N1 =  0 
 m1 2 
 ( L1 + w1 )θ

1 
2

 12 
 0 
2
w2 =  0 
θ
1 + θ
2 
 0 
2
w
2 =  0 
θ

1 + θ

2 
 L1θ

1 s 2 − L1θ
12 c 2 + gs12 
 
2
v
2 =  L1θ

1 c 2 + L1θ
12 s 2 + gc12 
 0 
 



2 L2

2
 L1θ 1 s 2 − L1θ 1 c 2 + gs12 − 2 ( θ 1 + θ 2 ) 
 L 
2
v
C 2 =  L1θ

1 c 2 + L1θ
12 s 2 + gc12 + 2 ( θ

1 + θ

2 ) 
 2 
 0 
 



2 L2

2
 m2 L1θ 1 s 2 − m2 L1θ 1 c 2 + m2 gs12 − m2 2 ( θ 1 + θ 2 ) 
 L 
2
F2 =  m2 L1θ

1 c 2 + m2 L1θ
12 s 2 + m2 gc12 + m2 2 ( θ

1 + θ

2 ) 
 2 
 0 
 
 
 0 
2
N2 =  0 
 m2 2 
 ( L2 + w2 )( θ

1 + θ

2
2
)
 12 

a. r./6.11.2003 1:39 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 6. Manipulator dynamics. Page 47

2
f 2 = 2 F2
 
 0 
2
n2 =  0 
m L L L L2 
 2 ( L22 + w22 )( θ

1 + θ

2 ) + m2 2 L1θ

1 c 2 + m2 2 L1θ
12 s 2 + m2 2 gc12 + m2 2 ( θ

1 + θ

2 )
 12 2 2 2 4 
m2 2 L L L L2
τ2 = ( L2 + w22 )( θ

1 + θ

2 ) + m2 2 L1θ

1 c 2 + m2 2 L1θ
12 s 2 + m2 2 gc12 + m2 2 ( θ

1 + θ

2 )
12 2 2 2 4

τ 1 = ...

6.5 Determine the dynamic equations for the two-link nonplanar manipulator
shown in Fig. 6.8. Assume that all the mass of the links can be considered as a
point mass located at the distal (outermost) end of the link. The mass values are
m1 and m2 and the link lengths are l1 and l2. This manipulator is like the first two
link of the arm in Exercise 3.3. Also assume the viscous friction is acting at each
joint with coefficients v1 and v2.

c 1 − s1 0 0  c1 − s1 0 L1 
s c1 0 0  0 0 − 1 0 
 1 
1T = 2T =
0 1
0 0 1 0 s2 c2 0 0
   
0 0 0 1 0 0 0 1
1
PC 1 = L1 X̂ 1
2
PC 2 = L2 X̂ 2
C1
I1 = 0
C2
I2 = 0
f 3 = 0 , n3 = 0 , w0 = 0 , w
0 = 0

0 
1
w1 = θ 1 Ẑ 1 =  0 

1

θ
1 
0 
1
w
1 = θ 1 Ẑ 1 =  0 


1

θ

1 

a. r./6.11.2003 1:39 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 6. Manipulator dynamics. Page 48

0 
1
v
1 =  0 
 g 

− L1θ
12 
 
1
v
C 1 =  L1θ

1 
 g 
 


 − m1 L1θ 12 
 
1
F1 =  m1 L1θ

1 
 m1 g 
 
0 
1
N1 = 0 
0 

 s 2θ
1 
 
2
w2 = c 2θ
1 
 θ
2 
 
 s 2θ

1 + c 2θ
1θ
2 
 
2
w
2 = c 2θ

1 − s 2θ
1θ
2 

 θ

2 

− c 2 L1θ
12 + s 2 g 
 
2
v
2 =  s 2 L1θ
12 + c 2 g 2 

 − L1θ

2 

− c 2θ
12 ( c 2 L2 + L1 ) − L2θ
22 + s 2 g 
 
2
v
C 2 =  s 2θ
12 ( c 2 L2 + L1 ) + L2θ

2 + c 2 g 
 2 L2 s 2θ
1θ
2 − ( c 2 L2 + L1 )θ

1 
 
 − m2 c 2θ
12 ( c 2 L2 + L1 ) − m2 L2θ
22 + m2 s 2 g 
 
2
F2 =  m2 s 2θ
12 ( c 2 L2 + L1 ) + m2 L2θ

2 + m2 c 2 g 

 2 m2 L2 s 2θ
1θ
2 − m2 ( c 2 L2 + L1 )θ

1 

0 
2
N 2 = 0 
0 
2
f 2 = 2 F2
 0 
2
n 2 =  − 2 m2 L2 s 2θ 1θ 2 + m2 L2 ( c 2 L2 + L1 )θ 1 
 2




 m2 L2 s 2θ
12 ( c 2 L2 + L1 ) + m2 L22θ

2 + m2 L2 c 2 g 

a. r./6.11.2003 1:39 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 6. Manipulator dynamics. Page 49

− m2θ
12 ( c 2 L2 + L1 ) − m2 L2 ( c 2θ
22 + s 2θ

2 ) − m1 L1θ
12 

 
1
f 1 =  − 2 m2 L2 s 2θ
1θ
2 + m2 ( c 2 L2 + L1 )θ

1 + m1 L1θ

1 

 m2 L2 ( s 2θ

2 − s 2θ
22 ) + m1 g + m2 g 

 ... 
1 
n1 =  ... 

 m2 L2 c 2 ( c 2 L2 + L1 )θ

1 − 2m2 L22 s 2 c 2θ
1θ
2 + m1 L21θ

1 + m2 L1 ( c 2 L2 + L1 )θ

1 − 2m2 L2 L1 s 2θ
1θ
2 

τ 1 = m2 ( c 2 L2 + L1 ) 2 θ

1 + m1 L21θ

1 − 2m2 L2 s 2 ( c 2 L2 + L1 )θ
1θ
2

τ 2 = m2 L2 s 2θ
12 ( c 2 L2 + L1 ) + m2 L22θ

2 + m2 L2 c 2 g

τ = M ( θ )θ

+ V ( θ ,θ
) + G( θ ) + F ( θ
)

m ( c L + L1 ) 2 + m1 L21 0 
M (θ ) =  2 2 2 
 0 m2 L22 
As expected, M ( θ )is symmetric and positive definite.
− 2m2 L2 s 2 ( c 2 L2 + L1 )θ
1θ
2 
V ( θ ,θ
) =  

2
 m2 L2 s 2θ 1 ( c 2 L2 + L1 ) 
 0 
G( θ ) =  
 m 2 L2 c 2 g 
 v θ

F ( θ
) =  1 1 


v 2θ 2 

6.19 Do Exercise 6.5 using Lagrange’s equations.

1 1 1
k1 = m1 vCT 1 vC 1 + 1 w1T C 1 I 1 1 w1 = m1 L21θ
12
2 2 2
1 1
k 2 = m2 vCT 2 vC 2 + 2 w2T C 2 I 2 2 w2 , vC 2 ?
2 2

Calculation of vC 2 :
 L2 c 2 c1 + L1 c1 
L c s + L s 
0
PC 2 = 02T 2PC 2 = 2 2 1 1 1

 L2 s 2 
 
 1 

a. r./6.11.2003 1:39 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 6. Manipulator dynamics. Page 50

1 1 1
k1 = m1 vCT 1 vC 1 + 1 w1T C 1 I 1 1 w1 = m1 L21θ
12
2 2 2
1 1
k 2 = m2 vCT 2 vC 2 + 2 w2T C 2 I 2 2 w2 , vC 2 ?
2 2

Calculation of vC 2 :
 − L2 c 2 s1 − L1 s1 − L2 s 2 c1 
( − L2 c 2 s1 − L1 s1 )θ
1 − L2 s 2 c1θ
2 
θ   
0
vC 2 =  L2 c 2 c1 + L1 c1 − L2 s 2 s1   1  =  ( L2 c 2 c1 + L1 c1 )θ
1 − L2 s 2 s1θ
2 
θ

 0 L2 c 2   2   L2 c 2θ
2 

2
2 2
2
vC 2 vC 2 = ( L1 + L2 c 2 ) θ 1 + L2θ 2
T

k2 =
1
2
[
m2 ( L1 + L2 c 2 ) 2 θ
12 + L22θ
22 ]

u1 = 0
u 2 = m2 gs 2 L2 + m2 gL2

d δk δ k δ u
τ= − +
dt δθ δθ δθ
δk m 2 ( c 2 L 2 + L1 ) 2 θ 1 + m1 L21θ 1 
= 
δθ  m 2 L22θ 2 
δk  0 
=
δθ − m 2 L2 s 2θ 1 ( c 2 L 2 + L1 )
2

δu  0 
=
δθ m 2 L 2 c 2 g 

d δk δk δu m 2 ( c 2 L 2 + L1 ) 2 θ 1 + m1 L21θ 1 − 2m 2 L2 s 2 ( c 2 L 2 + L1 )θ 1θ 2 
τ= − + = 
dt δθ δθ δθ  m 2 L2 s 2θ 12 ( c 2 L 2 + L1 ) + m 2 L22θ 2 + m 2 L2 c 2 g 

a. r./6.11.2003 1:39 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology