Analytical Mechanics: Link Mechanisms - Beyond …hirai/edu/2017/analyticalmechanics/handout/... ·...

Analytical Mechanics: Link Mechanisms

Shinichi Hirai

Dept. Robotics, Ritsumeikan Univ.

Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 1 / 28

Agenda

Agenda

1 Open Link MechanismKinematics of Open Link MechanismDynamics of Open Link Mechanism

2 Closed Link MechanismKinematics of Closed Link MechanismDynamics of Closed Link Mechanism


Open Link Mechanism Kinematics of Open Link Mechanism

Kinematics of two link open mechanism

θ1

θ2l1

l2

link 1

link 2

joint 1

joint 2

lc2

lc1

x

y

two link open link mechanismli length of link ilci distance btw. joint i and

the center of mass of link imi mass of link iJi inertia of moment of link i

around its center of massθ1 rotation angle of joint 1θ2 rotation angle of joint 2


Open Link Mechanism Kinematics of Open Link Mechanism

Kinematics of two link open mechanism

position of the center of mass of link 1:

xc1△=

[xc1yc1

]= lc1

[C1

S1

]position of the center of mass of link 2:

xc2△=

[xc2yc2

]= l1

[C1

S1

]+ lc2

[C1+2

S1+2

]orientation angle of link 1:

θ1

orientation angle of link 2:θ1 + θ2


Open Link Mechanism Dynamics of Open Link Mechanism

Kinetic energy

velocity of the center of mass of link 1:

xc1 = lc1θ1

[−S1

C1

]angular velocity of link 1:

θ1

kinetic energy of link 1:

T1 =1

2m1xT

c1xc1 +1

2J1θ

21

=1

2(m1l

2c1 + J1)θ

21



Kinetic energy

velocity of the center of mass of link 2:

xc2 = l1θ1

[−S1

C1

]+ lc2(θ1 + θ2)

[−S1+2

C1+2

]angular velocity of link 2:

θ1 + θ2

kinetic energy of link 2:

T2 =1

2m2xT

c2xc2 +1

2J2(θ1 + θ2)

2

=1

2m2{l21 θ21 + l2c2(θ1 + θ2)

2 + 2l1lc2C2θ1(θ1 + θ2)}+1

2J2(θ1 + θ2)

2



Kinetic energy

total kinetic energy

T = T1 + T2 =1

2

[θ1 θ2

] [ H11 H12

H21 H22

] [θ1θ2

]where

H11 = J1 +m1l2c1 + J2 +m2(l

21 + l2c2 + 2l1lc2C2)

H22 = J2 +m2l2c2

H12 = H21 = J2 +m2(l2c2 + l1lc2C2)

inertia matrix

H△=

[H11 H12

H21 H22

]Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 7 / 28


Partial derivatives

H11 and H12 = H21 depend on θ2:

∂H11

∂θ2= −2h12,

∂H12

∂θ2=

∂H21

∂θ2= −h12 (h12

△= m2l1lc2S2)

H11 = −2h12θ2, H12 = H21 = −h12θ2∂T

∂θ1= H11θ1 + H12θ2,

∂T

∂θ2= H21θ1 + H22θ2

− d

dt

∂T

∂θ1= −H11θ1 − H11θ1 − H12θ2 − H12θ2

= 2h12θ1θ2 + h12θ22 − H11θ1 − H12θ2

− d

dt

∂T

∂θ2= −H21θ1 − H21θ1 − H22θ2 − H22θ2

= h12θ1θ2 − H21θ1 − H22θ2



Partial derivatives

H11, H22, and H12 = H21 are independent of θ1

∂T

∂θ1=

1

2

[θ1 θ2

] [ 0 00 0

] [θ1θ2

]= 0

H11 and H12 = H21 depend on θ2

∂T

∂θ2=

1

2

[θ1 θ2

] [ −2h12 −h12−h12 0

] [θ1θ2

]= −h12θ

21 − h12θ1θ2

contribution of kinetic energy:

∂T

∂θ1− d

dt

∂T

∂θ1= 2h12θ1θ2 + h12θ

22 − H11θ1 − H12θ2

∂T

∂θ2− d

dt

∂T

∂θ2= −h12θ

21 − H21θ1 − H22θ2



Gravitational potential energy

potential energy of link 1:

U1 = m1g yc1 = m1g lc1S1

potential energy of link 2:

U2 = m2g yc2 = m2g (l1S1 + lc2S1+2)

potential energy:U = U1 + U2

partial derivatives w.r.t. joint angles:

∂U

∂θ1= G1 + G12,

∂U

∂θ2= G12

whereG1 = (m1lc1 +m2l1) gC1, G12 = m2lc2 gC1+2



Work done by actuator torques

work done by τ1 applied to rotational joint 1:

τ1θ1

work done by τ2 applied to rotational joint 2:

τ2θ2

work done by the two actuator torques:

W = τ1θ1 + τ2θ2

partial derivatives w.r.t. joint angles:

∂W

∂θ1= τ1,

∂W

∂θ2= τ2



Lagrange equations of motion

Lagrangian:L = T − U +W


∂L

∂θ1− d

dt

∂L

∂θ1= 0

∂L

∂θ2− d

dt

∂L

∂θ2= 0

let ω1△= θ1 and ω2

△= θ2:

− H11ω1 − H12ω2 + h12ω22 + 2h12ω1ω2 − G1 − G12 + τ1 = 0

− H22ω2 − H12ω1 − h12ω21 − G12 + τ2 = 0




canonical form of ordinary differential equations:[θ1θ2

]=

[ω1

ω2

][H11 H12

H21 H22

] [ω1

ω2

]=

[h12ω

22 + 2h12ω1ω2 − G1 − G12 + τ1

−h12ω21 − G12 + τ2

]state variables: joint angles θ1, θ2 and angular velocities ω1, ω2

the inertia matrix is regular −→ 2nd eq. is solvable−→ we can compute ω1 and ω2

θ1, θ2, ω1, ω2 are functions of θ1, θ2, ω1, ω2

⇓we can sketch θ1, θ2, ω1, ω2 using any ODE solver(e.g. Runge-Kutta method)



PD control

τ1 = KP1(θd1 − θ1)− KD1θ1

τ2 = KP2(θd2 − θ2)− KD2θ2

⇓[θ1θ2

]=

[ω1

ω2

][H11 H12

H21 H22

] [ω1

ω2

]=

[· · ·+ KP1(θ

d1 − θ1)− KD1ω1

· · ·+ KP2(θd2 − θ2)− KD2ω2

]

current values of θ1, θ2, ω1, ω2

⇓their time derivatives θ1, θ2, ω1, ω2



PI control

τ1 = KP1(θd1 − θ1) + KI1

∫ t

0

{(θd1 − θ1(τ)} dτ

τ2 = KP2(θd2 − θ2) + KI2

∫ t

0

{(θd2 − θ2(τ)} dτ

Introduce additional variables:

ξ1△=

∫ t

0

{(θd1 − θ1(τ)} dτ

ξ2△=

∫ t

0

{(θd2 − θ2(τ)} dτ

ξ1 = θd1 − θ1, τ1 = KP1(θd1 − θ1) + KI1ξ1

ξ2 = θd2 − θ2, τ2 = KP2(θd2 − θ2) + KI2ξ2



PI control

⇓[θ1θ2

]=

[ω1

ω2

][H11 H12

H21 H22

] [ω1

ω2

]=

[· · ·+ KP1(θ

d1 − θ1) + KI1ξ1

· · ·+ KP2(θd2 − θ2) + KI2ξ2

]ξ1 = θd1 − θ1

ξ2 = θd2 − θ2

current values of θ1, θ2, ω1, ω2, ξ1, ξ2⇓

their time derivatives θ1, θ2, ω1, ω2, ξ1, ξ2Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 16 / 28

Closed Link Mechanism Kinematics of Closed Link Mechanism

Kinematics of four-link closed mechanism

x

y

O

link 1

link 2

link 3

link 4

θ1

θ3

θ2θ4

tip point

l1

l2

l3

l4

(x1,y1) (x3,y3)

joint 1

joint 2

joint 3

joint 4

joint 5

joint 1, 3: activejoint 2, 4, 5: passive

θ1, θ2, θ3, θ4:rotation angles

τ1, τ3:actuator torques


Closed Link Mechanism Kinematics of Closed Link Mechanism

Kinematics of four-link closed mechanism

decomposition of closed link mechanism into open link mechanisms:left arm link 1 and 2right arm link 3 and 4

end point of the left arm:[x1y1

]+ l1

[C1

S1

]+ l2

[C1+2

S1+2

]end point of the right arm:[

x3y3

]+ l3

[C3

C3

]+ l4

[C3+4

S3+4

]two constraints:

X△= l1C1 + l2C1+2 − l3C3 − l4C3+4 + x1 − x3 = 0

Y△= l1S1 + l2S1+2 − l3S3 − l4S3+4 + y1 − y3 = 0


Closed Link Mechanism Dynamics of Closed Link Mechanism

Lagrangian

Lagrangian of the closed link mechanism:

L = Lleft + Lright + λxX + λyY ,

Lleft, Lright Lagrangians of the left and right armsλx , λy Lagrange multipliers

Lagrange equations of motion:

∂L

∂θ1− d

dt

∂L

∂ω1= 0

∂L

∂θ2− d

dt

∂L

∂ω2= 0

∂L

∂θ3− d

dt

∂L

∂ω3= 0

∂L

∂θ4− d

dt

∂L

∂ω4= 0



Contributions of Lleft

contributions of Lagrangian Lleft to the four Lagrange eqs:

− H11ω1 − H12ω2 + h12ω22 + 2h12ω1ω2 − G1 − G12 + τ1,

− H22ω2 − H21ω1 − h12ω21 − G12,

0,

0

where

H11 = J1 +m1l2c1 + J2 +m2(l

21 + l2c2 + 2l1lc2C2),

H22 = J2 +m2l2c2,

H12 = H21 = J2 +m2(l2c2 + l1lc2C2),

h12 = m2l1lc2S2,

G1 = (m1lc1 +m2l1) gC1, G12 = m2lc2 gC1+2



Contributions of Lright

contributions of Lagrangian Lright to the four Lagrange eqs:

0,

0,

− H33ω3 − H34ω4 + h34ω24 + 2h34ω3ω4 − G3 − G34 + τ3,

− H44ω4 − H43ω3 − h34ω23 − G34

where

H33 = J3 +m3l2c3 + J4 +m4(l

23 + l2c4 + 2l3lc4C4),

H44 = J4 +m4l2c4,

H34 = H43 = J4 +m4(l2c4 + l3lc4C4),

h34 = m4l3lc4S4,

G3 = (m3lc3 +m4l3) gC3, G34 = m4lc4 gC3+4



Contributions of λxX + λyY

contributions of λxX + λyY to the four Lagrange eqs:

λx(−l1S1 − l2S1+2) + λy (l1C1 + l2C1+2)△= λxJx1 + λyJy1,

λx(−l2S1+2) + λy l2C1+2△= λxJx2 + λyJy2,

λx(l3S3 + l4S3+4) + λy (−l3C3 − l4C3+4)△= −λxJx3 − λyJy3,

λx l4S3+4 + λy (−l4C3+4)△= −λxJx4 − λyJy4,

where

J12 =

[Jx1 Jx2Jy1 Jy2

], J34 =

[Jx3 Jx4Jy3 Jy4

]are Jacobian matrices of the left and right arms



Contributions of λxX + λyY

constraint force λ = [λx , λy ]T

equivalent torques around rotational joints 1 and 2:

JT12λ =

[λx(−l1S1 − l2S1+2) + λy (l1C1 + l2C1+2)

λx(−l2S1+2) + λy l2C1+2

]

reaction force −λequivalent torques around rotational joint 3 and 4:

JT34(−λ) =

[λx(l3S3 + l4S3+4) + λy (−l3C3 − l4C3+4)

λx l4S3+4 + λy (−l4C3+4)

]




H11 H12 0 0 −Jx1 −Jy1H12 H22 0 0 −Jx2 −Jy20 0 H33 H34 Jx3 Jy30 0 H34 H44 Jx4 Jy4

ω1

ω2

ω3

ω4

λx

λy

=

f1f2f3f4

where

f1 = h12ω22 + 2h12ω1ω2 − G1 − G12 + τ1, f2 = −h12ω

21 − G12

f3 = h34ω24 + 2h34ω3ω4 − G3 − G34 + τ3, f4 = −h34ω

23 − G34



Equations stabilizing constraints

X + 2νX + ν2X = 0, Y + 2νY + ν2Y = 0

⇓[−Jx1 −Jx2 Jx3 Jx4−Jy1 −Jy2 Jy3 Jy4

]ω1

ω2

ω3

ω4

=

[gxgy

]where

gx = −(Jy1ω

21 + Jy2ω

22 + 2Jy2ω1ω2

)+(Jy3ω

23 + Jy4ω

24 + 2Jy4ω3ω4

)+2ν(Jx1ω1 + Jx2ω2 − Jx3ω3 − Jx4ω4) + ν2X

gy =(Jx1ω

21 + Jx2ω

22 + 2Jx2ω1ω2

)−

(Jx3ω

23 + Jx4ω

24 + 2Jx4ω3ω4

)+2ν(Jy1ω1 + Jy2ω2 − Jy3ω3 − Jy4ω4) + ν2Y



Dynamic equations for closed link mechanism

H11 H12 0 0 −Jx1 −Jy1H12 H22 0 0 −Jx2 −Jy20 0 H33 H34 Jx3 Jy30 0 H34 H44 Jx4 Jy4

−Jx1 −Jx2 Jx3 Jx4 0 0−Jy1 −Jy2 Jy3 Jy4 0 0

ω1

ω2

ω3

ω4

λx

λy

=

f1f2f3f4gxgy

the coefficient matrix is regular −→ we can compute ω1 through ω4



Dynamic Simulation of a Link Mechanism

Report #4 due date : Jan. 8 (Mon.)

Simulate the control of a four-link closed manipulator. Apply PIcontrol to active joints 1 and 3. Use appropriate values ofgeometrical and physical parameters of the manipulator. Show howposition of joint 5 changes according to time.

x

y

O

link 1

link 2

link 3

link 4

θ1

θ3

θ2θ4

tip point

l1

l2

l3

l4

(x1,y1) (x3,y3)

joint 1

joint 2

joint 3

joint 4

joint 5


Summary

Summary

Open link mechanism

inertia matrix depends on joint angles

Lagrange equations of motion of open link mechanism

Closed link mechanism

two open link mechanisms with geometric constraints

synthesized from Lagrange equations of open link mechanisms


Analytical Mechanics: Link Mechanisms - Beyond …hirai/edu/2017/analyticalmechanics/handout/... ·...

Documents

Transcript of Analytical Mechanics: Link Mechanisms - Beyond …hirai/edu/2017/analyticalmechanics/handout/... ·...