Analytical Mechanics: Link Mechanismshirai/edu/2020/analyticalmechanics/...Shinichi Hirai (Dept....
Transcript of Analytical Mechanics: Link Mechanismshirai/edu/2020/analyticalmechanics/...Shinichi Hirai (Dept....
Analytical Mechanics: Link Mechanisms
Shinichi Hirai
Dept. Robotics, Ritsumeikan Univ.
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 1 / 39
Agenda
1 Open Link MechanismKinematics of Open Link MechanismDynamics of 2DOF open link mechanism
2 Closed Link MechanismKinematics of Closed Link MechanismDynamics of 2DOF closed link mechanism
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 2 / 39
Kinematics of 2DOF open link 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 3 / 39
Kinematics of 2DOF open link 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 4 / 39
Kinetic energyvelocity 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 5 / 39
Kinetic energyvelocity 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 6 / 39
Kinetic energytotal 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 / 39
Partial derivativesH11 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 8 / 39
Partial derivativesH11, 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 9 / 39
Gravitational potential energypotential 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 10 / 39
Work done by actuator torqueswork 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 11 / 39
Lagrange equations of motionLagrangian:
L = T − U +W
Lagrange equations of motion
∂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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 12 / 39
Lagrange equations of motioncanonical 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 an ODE solver.
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 13 / 39
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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 14 / 39
PD control of 2DOF open link mechanismSample Programs
PD control of 2DOF open link mechanism
equation of motion of 2DOF open link mechanism under PDcontrol
definition of 2DOF open link mechanism
tip point coordinates with gradient vectors and Hessian matrices
calculating inertia matrix and generated torques
drawing 2DOF open link mechanism
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 15 / 39
PD control of 2DOF open link mechanism
Result
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 16 / 39
PD control of 2DOF open link mechanismResult
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 17 / 39
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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 18 / 39
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, ξ2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 19 / 39
Report
Report #2 due date : Nov. 16 (Mon.)
Simulate the motion of a 2DOF open link mechanism under PIDcontrol. PID control is applied to active joints 1 and 2. Useappropriate values of geometrical and physical parameters of themanipulator.
θ1
θ2l1
l2
link 1
link 2
joint 1
joint 2
lc2
lc1
x
y
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 20 / 39
Kinematics of 2DOF closed link 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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 21 / 39
Kinematics of 2DOF closed link mechanismdecomposition 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:[x1,2y1,2
]=
[x1y1
]+ l1
[C1
S1
]+ l2
[C1+2
S1+2
]end point of the right arm:[
x3,4y3,4
]=
[x3y3
]+ l3
[C3
C3
]+ l4
[C3+4
S3+4
]two constraints:
X△= x1,2 − x3,4 = l1C1 + l2C1+2 − l3C3 − l4C3+4 + x1 − x3 = 0
Y△= y1,2 − y3,4 = l1S1 + l2S1+2 − l3S3 − l4S3+4 + y1 − y3 = 0
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 22 / 39
LagrangianLagrangian of the closed link mechanism:
L = L1,2 + L3,4 + λxX + λyY
L1,2, L3,4 Lagrangians of the left and right armsλx , λy Lagrange multipliers
Lagrange equations of motion:
∂L∂θ1,2
− d
dt
∂L∂ω1,2
= 0
∂L∂θ3,4
− d
dt
∂L∂ω3,4
= 0
where
θ1,2 =
[θ1θ2
], ω1,2 =
[ω1
ω2
], θ3,4 =
[θ3θ4
], ω3,4 =
[ω3
ω4
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 23 / 39
Contributions of L1,2contributions of Lagrangian L1,2 to the Lagrange eqs:
− H1,2 ω1,2 + τ1,2
0
where
H1,2 =
[*** J2 +m2(l
2c2 + l1lc2C2)
J2 +m2(l2c2 + l1lc2C2) J2 +m2l
2c2
]τ1,2 =
[+h12ω
22 + 2h12ω1ω2 − G1 − G12 + τ1
−h12ω21 − G12
]*** = J1 +m1l
2c1 + J2 +m2(l
21 + l2c2 + 2l1lc2C2),
h12 = m2l1lc2S2,
G1 = (m1lc1 +m2l1) gC1, G12 = m2lc2 gC1+2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 24 / 39
Contributions of L3,4contributions of Lagrangian L3,4 to the Lagrange eqs:
0
− H3,4 ω3,4 + τ3,4
where
H3,4 =
[*** J4 +m4(l
2c4 + l3lc4C4)
J4 +m4(l2c4 + l3lc4C4) J4 +m4l
2c4
]τ3,4 =
[+h34ω
24 + 2h34ω3ω4 − G3 − G34 + τ3
−h34ω23 − G34
]*** = J3 +m3l
2c3 + J4 +m4(l
23 + l2c4 + 2l3lc4C4),
h34 = m4l3lc4S4,
G3 = (m3lc3 +m4l3) gC3, G34 = m4lc4 gC3+4
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 25 / 39
Contributions of λxXcontributions of λxX to the Lagrange eqs:
λxgx ;1,2
−λxgx ;3,4
where
gx ;1,2 =∂x1,2∂θ1,2
=
[−l1S1 − l2S1+2
−l2S1+2
]gx ;3,4 =
∂x3,4∂θ3,4
=
[−l3S3 − l4S3+4
−l4S3+4
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 26 / 39
Contributions of λyYcontributions of λyY to the Lagrange eqs:
λygy ;1,2
−λygy ;3,4
where
gy ;1,2 =∂y1,2∂θ1,2
=
[l1C1 + l2C1+2
l2C1+2
]gy ;3,4 =
∂y3,4∂θ3,4
=
[l3C3 + l4C3+4
l4C3+4
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 27 / 39
Lagrange equations of motion
−H1,2 ω1,2 + τ1,2 + λxgx ;1,2 + λygy ;1,2 = 0
−H3,4 ω3,4 + τ3,4 − λxgx ;3,4 − λygy ;3,4 = 0
⇓[H1,2 O2×2 −JT
1,2
O2×2 H3,4 JT3,4
] ω1,2
ω3,4
λ
=
[τ1,2τ3,4
]where
JT1,2 =
[gx ;1,2 gy ;1,2
]JT3,4 =
[gx ;3,4 gy ;3,4
]λ =
[λx
λy
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 28 / 39
Equation stabilizing constraint XRecall that X = x1,2 − x3,4
X =
(∂x1,2∂θ1,2
)T
θ1,2 −(∂x3,4∂θ3,4
)T
θ3,4 = gTx ;1,2 ω1,2 − gT
x ;3,4 ω3,4
X = gTx ;1,2 ω1,2 + gT
x ;1,2 ω1,2 − gTx ;3,4 ω3,4 − gT
x ;3,4 ω3,4
Since
gx ;1,2 =∂gx ;1,2
∂θT1,2
θ1,2 =∂gx ;1,2
∂θT1,2
ω1,2
we have gTx ;1,2 ω1,2 = ωT
1,2 Qx ;1,2 ω1,2, where
Qx ;1,2 =∂gT
x ;1,2
∂θ1,2=
[∂2x1,2/∂θ
21 ∂2x1,2/∂θ1∂θ2
∂2x1,2/∂θ2∂θ1 ∂2x1,2/∂θ22
]Hessian matrix
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 29 / 39
Equation stabilizing constraint X
X + 2αX + α2X = 0
⇓
−gTx ;1,2 ω1,2 + gT
x ;3,4 ω3,4 = ωT1,2 Qx ;1,2 ω1,2 − ωT
3,4 Qx ;3,4 ω3,4
+ 2α(gTx ;1,2 ω1,2 − gT
x ;3,4 ω3,4) + α2X
where
gx ;1,2 =
[−l1S1 − l2S1+2
−l2S1+2
], Qx ;1,2 =
[−l1C1 − l2C1+2 −l2C1+2
−l2C1+2 −l2C1+2
]gx ;3,4 =
[−l3S3 − l4S3+4
−l4S3+4
], Qx ;3,4 =
[−l3C3 − l4C3+4 −l4C3+4
−l4C3+4 −l4C3+4
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 30 / 39
Equation stabilizing constraint Y
Y + 2αY + α2Y = 0
⇓
−gTy ;1,2 ω1,2 + gT
y ;3,4 ω3,4 = ωT1,2 Qy ;1,2 ω1,2 − ωT
3,4 Qy ;3,4 ω3,4
+ 2α(gTy ;1,2 ω1,2 − gT
y ;3,4 ω3,4) + α2Y
where
gy ;1,2 =
[l1C1 + l2C1+2
l2C1+2
], Qy ;1,2 =
[−l1S1 − l2S1+2 −l2S1+2
−l2S1+2 −l2S1+2
]gy ;3,4 =
[l3C3 + l4C3+4
l4C3+4
], Qy ;3,4 =
[−l3S3 − l4S3+4 −l4S3+4
−l4S3+4 −l4S3+4
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 31 / 39
Equations stabilizing constraint X and Y
−gTx ;1,2 ω1,2 + gT
x ;3,4 ω3,4 = Cx(θ1,2,θ3,4,ω1,2,ω3,4)
−gTy ;1,2 ω1,2 + gT
y ;3,4 ω3,4 = Cy (θ1,2,θ3,4,ω1,2,ω3,4)
where
Cx = ωT1,2 Qx ;1,2 ω1,2 − ωT
3,4 Qx ;3,4 ω3,4 + 2α(gTx ;1,2 ω1,2 − gT
x ;3,4 ω3,4) + α2X
Cy = ωT1,2 Qy ;1,2 ω1,2 − ωT
3,4 Qy ;3,4 ω3,4 + 2α(gTy ;1,2 ω1,2 − gT
y ;3,4 ω3,4) + α2Y
⇓[−J1,2 J3,4 O2×2
] ω1,2
ω3,4
λ
= C
where C = [Cx , Cy ]T
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 32 / 39
Dynamic equations for closed link mechanism
H1,2 O2×2 −JT1,2
O2×2 H3,4 JT3,4
−J1,2 J3,4 O2×2
ω1,2
ω3,4
λ
=
τ1,2τ3,4C
the coefficient matrix is regular −→ we can compute ω1 through ω4
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 33 / 39
Physical Interpretation
J1,2 and J3,4 : Jacobian matrices of the left and right armsλ = [λx , λy ]
T : constraint forceequivalent torques around rotational joints 1 and 2:
JT1,2λ =
[λx(−l1S1 − l2S1+2) + λy (l1C1 + l2C1+2)
λx(−l2S1+2) + λy l2C1+2
]
reaction force −λequivalent torques around rotational joint 3 and 4:
JT3,4(−λ) =
[λx(l3S3 + l4S3+4) + λy (−l3C3 − l4C3+4)
λx l4S3+4 + λy (−l4C3+4)
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 34 / 39
PD control of 2DOF closed link mechanism
Sample Programs
PD control of 2DOF closed link mechanism
equation of motion of 2DOF closed link mechanism under PDcontrol
definition of 2DOF closed link mechanism
drawing 2DOF closed link mechanism
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 35 / 39
PD control of 2DOF closed link mechanism
Result
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 36 / 39
PD control of 2DOF closed link mechanismResult
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 37 / 39
Report
Report #3 due date : Nov. 30 (Mon.)
Simulate the motion of a 2DOF closed link mechanism under PIDcontrol. PID control is applied to active joints 1 and 3. Useappropriate values of geometrical and physical parameters of themanipulator.
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
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 38 / 39
Summary
Open link mechanisminertia matrix depends on joint angles
Lagrange equations of motion of open link mechanism
Closed link mechanismtwo open link mechanisms with geometric constraints
synthesized from Lagrange equations of open link mechanisms
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 39 / 39