Kinetic energy. Analytical Mechanics: Link Mechanisms. Kinetic energy. Agenda. Kinematics of two link open mechanism.

Size: px

Start display at page:

Download "Kinetic energy. Analytical Mechanics: Link Mechanisms. Kinetic energy. Agenda. Kinematics of two link open mechanism."

Antonia Austin
5 years ago
Views:

1 Analtical Mechanics: Link Mechanisms Shinichi Hirai Dept. Robotics, Ritsumeikan Univ. Kinetic energ velocit of the center of mass of link : angular velocit of link : kinetic energ of link : ẋ c l c θ S C θ Dnamics of T m ẋ T cẋc + J θ (m l c + J ) θ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Aga Aga Kinematics of Dnamics of Kinematics of Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 5 / 5 Kinetic energ velocit of the center of mass of : ẋ c l θ S C angular velocit of : kinetic energ of : Dnamics of + l c ( θ + θ ) θ + θ T m ẋ T cẋc + J ( θ + θ ) S+ C + m {l θ + l c( θ + θ ) + l l c C θ ( θ + θ )}+ J ( θ + θ ) Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Kinematics of Kinematics of two link open mechanism joint lc l θ lc link two link open link mechanism l i length of link i l ci distance btw. joint i and the center of mass of link i m i mass of link i J i inertia of moment of link i around its center of mass θ rotation angle of joint θ rotation angle of Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 6 / 5 Kinetic energ total kinetic energ inertia matri T T + T Dnamics of H H θ θ H H θ H J + m l c + J + m (l + l c + l l c C ) H J + m l c H H J + m (l c + l l c C ) H H H H H Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 3 / 5 Kinematics of Kinematics of two link open mechanism position of the center of mass of link : c c c l c C S position of the center of mass of : c C C+ c l + l c S c S + orientation angle of link : orientation angle of : θ θ + θ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 4 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 7 / 5 Partial derivatives Dnamics of H and H H dep on θ : H H h, H h (h m l l c S ) θ θ θ H h, H H h θ H θ + H, θ H θ + H d dt θ H θ H θ H H h θ + h H θ H d H dt θ θ H θ H H h θ θ H θ H θ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 8 / 5

2 Partial derivatives Dnamics of H, H, and H H are indepent of θ θ θ H and H H dep on θ h θ θ h θ h contribution of kinetic energ: θ θ θ θ h θ h θ θ d θ dt θ h θ + h H θ H d θ dt θ h H θ H Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 9 / 5 Gravitational potential energ potential energ of link : potential energ of : potential energ: Dnamics of U m g c m g l c S U m g c m g (l S + l c S + ) U U + U partial derivatives w.r.t. joint angles: U θ G + G, U θ G G (m l c + m l ) gc, G m l c gc + Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Work done b actuator torques work done b τ applied to rotational joint : τ θ work done b τ applied to rotational : τ θ work done b the two actuator torques: W τ θ + τ θ partial derivatives w.r.t. joint angles: W θ τ, Dnamics of W θ τ Dnamics of canonical form of ordinar differential equations: θ ω θ ω H H ω h ω + h ω ω G G + τ H H ω h ω G + τ state variables: joint angles θ, θ and angular velocities ω, ω the inertia matri is regular nd eq. is solvable we can compute ω and ω θ, θ, ω, ω are functions of θ, θ, ω, ω we can sketch θ, θ, ω, ω using an ODE solver. Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 3 / 5 open DOF definition.m Dnamics of function param open_link_dof_definition () % clindrical solid link densit ; len.; len_grav.; radius.5; m densit * len * (pi*(radius)^); Jc (/) * m * (3*radius^ + len^); J Jc + m * (len/ - len_grav)^; param struct( length, len,... length_grav, len_grav,... mass, m, inertia, J, base, ;); param struct( length, len,... length_grav, len_grav,... mass, m, inertia, J, base, ); param param; param; Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 4 / 5 open DOF dnamics.m Dnamics of function inertia_matri, torques open_link_dof_dnamics... (theta, omega, param, gravit) theta theta(); theta theta(); omega omega(); omega omega(); l param().length; lc param().length_grav; m param().mass; J param().inertia; param().length; lc param().length_grav; m param().mass; J param().inertia; inertia_matri(,) J + m*lc^ +... J + m*(l^ + lc^ + *l*lc*cos(theta)); inertia_matri(,) J + m*lc*; inertia_matri(,) J + m*(lc^ + l*lc*cos(theta)); inertia_matri(,) inertia_matri(,); Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Lagrangian: let ω θ and ω : L T U + W d θ dt θ d θ dt θ Dnamics of H ω H ω + h ω + h ω ω G G + τ H ω H ω h ω G + τ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 5 / 5 open DOF dnamics.m Dnamics of h m*l*lc*sin(theta); G (m*lc + m*l)*gravit*cos(theta); G m*lc*gravit*cos(theta+theta); torques h*omega^ + *h*omega*omega - G - G; -h*omega^ - G ; Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 6 / 5

3 Dnamics of Dnamics of PD control Result τ K P (θ d θ ) K D θ τ K P (θ d θ ) K D θ θ ω θ ω H H ω + KP (θ d θ ) K D ω H H ω + K P ( d θ ) K D ω current values of θ, θ, ω, ω their time derivatives θ, θ, ω, ω Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 7 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Dnamics of open DOF PD ode params.m open DOF draw.m Dnamics of function dotq open_link_dof_pd_ode_params... (t, q, param, gravit, thetad, Kp, Kd) theta q(:); omega q(3:4); inertia_matri, joint_torques... open_link_dof_dnamics(theta, omega, param, gravit); dottheta omega; tau -Kp.*(theta - thetad) - Kd.*omega; dotomega inertia_matri \ (joint_torques + tau); dotq dottheta; dotomega; function open_link_dof_draw (theta, param) theta theta(); theta theta(); l param().length; param().length; O param().base; P O + l*cos(theta); sin(theta); P P + *cos(theta+theta); sin(theta+theta); O(); P(); P(); O(); P(); P(); plot(,, k- ); Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 8 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Dnamics of Dnamics of open DOF PD.m Drawing param open_link_dof_definition (); gravit 9.8; thetad pi/3; pi/6 ; Kp 3; 3 ; Kd.5;.5 ; open_link_dof_pd_ode_params... (t, q, param, gravit, thetad, Kp, Kd); interval, ; thetainit ; ; omegainit ; ; qinit thetainit; omegainit; time, q ode45(open_link_dof_pd_ode, interval, qinit); clf; hold on for k::sz() open_link_dof_draw(theta(k,:), param); lim(-5,5); lim(,5); pbaspect( ); pause(.); hold off Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 9 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 3 / 5 Dnamics of Dnamics of open DOF PD.m Drawing Result thetad thetad()*ones(size(time)); thetad thetad()*ones(size(time)); plot(time, q(:,), r-, time, q(:,), b-,... time, thetad, r--, time, thetad, b-- ); Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 4 / 5

4 PI control Dnamics of τ K P (θ d θ ) + K I {(θ d θ (τ)} dτ τ K P ( d θ ) + K I {( d θ (τ)} dτ Introduce additional variables: ξ {(θ d θ (τ)} dτ ξ {( d θ (τ)} dτ ξ θ d θ, τ K P (θ d θ ) + K I ξ ξ θ d θ, τ K P (θ d θ ) + K I ξ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 5 / 5 PI control Dnamics of θ ω θ ω H H ω + KP (θ d θ ) + K I ξ H H ω + K P ( d θ ) + K I ξ ξ θ d θ ξ θ d θ current values of θ, θ, ω, ω, ξ, ξ their time derivatives θ, θ, ω, ω, ξ, ξ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 6 / 5 Dnamics of Dnamic Simulation of a Link Mechanism Report # due date : Nov. 6 (Mon.) Simulate the control of a two-link open manipulator. Appl PID control to active joints and. Use appropriate values of geometrical and phsical parameters of the manipulator. Show how joint angles change according to time. joint lc l θ lc link Kinematics of Kinematics of four-link closed mechanism decomposition of closed link mechanism into open link mechanisms: left arm link and right arm link 3 and 4 point of the left arm:,, point of the right arm: 3,4 two constraints: 3,4 3 3 C C+ + l + l S S + C3 C3+4 + l 3 + l C 4 3 S 3+4 X, 3,4 l C + l C + l 3 C 3 l 4 C Y, 3,4 l S + l S + l 3 S 3 l 4 S Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 9 / 5 Lagrangian Lagrangian of the closed link mechanism: L L, + L 3,4 + λ X + λ Y L,, L 3,4 Lagrangians of the left and right arms λ, λ Lagrange multipliers : d θ, dt ω, d θ 3,4 dt ω 3,4 θ, θ θ ω θ3 ω3, ω,, θ ω 3,4, ω θ 3,4 4 ω 4 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 3 / 5 Contributions of L, contributions of Lagrangian L, to the Lagrange eqs: H, ω, + τ, *** J H, + m (lc + l l c C ) J + m (lc + l l c C ) J + m lc +h ω τ, + h ω ω G G + τ h ω G *** J + m l c + J + m (l + l c + l l c C ), h m l l c S, G (m l c + m l ) gc, G m l c gc + Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 7 / 5 Kinematics of Kinematics of four-link closed mechanism O link joint tip point l θ (,) link 4 link 3 joint 3 joint 5 l4 joint 4 (3,3) θ4 θ3 l3 joint, 3: active, 4, 5: passive θ, θ, θ 3, θ 4 : rotation angles τ, τ 3 : actuator torques Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 3 / 5 Contributions of L 3,4 contributions of Lagrangian L 3,4 to the Lagrange eqs: H 3,4 ω 3,4 + τ 3,4 *** J H 3,4 4 + m 4 (lc4 + l 3 l c4 C 4 ) J 4 + m 4 (lc4 + l 3 l c4 C 4 ) J 4 + m 4 lc4 +h34 ω4 τ 3,4 + h 34 ω 3 ω 4 G 3 G 34 + τ 3 h 34 ω3 G 34 *** J 3 + m 3 l c3 + J 4 + m 4 (l 3 + l c4 + l 3 l c4 C 4 ), h 34 m 4 l 3 l c4 S 4, G 3 (m 3 l c3 + m 4 l 3 ) gc 3, G 34 m 4 l c4 gc 3+4 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 8 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 3 / 5

5 Contributions of λ X contributions of λ X to the Lagrange eqs: λ g ;, λ g ;3,4 g ;,, l S l S + θ, l S + g ;3,4 3,4 l3 S 3 l 4 S 3+4 θ 3,4 l 4 S 3+4 Equation stabilizing constraint X Ẍ + αẋ + α X g T ;, ω, + g T ;3,4 ω 3,4 ω T, Q ;, ω, ω T 3,4 Q ;3,4 ω 3,4 l S g ;, l S + l S + l3 S g ;3,4 3 l 4 S 3+4 l 4 S α(g T ;, ω, g T ;3,4 ω 3,4 ) + α X l C, Q ;, l C + l C + l C + l C + l3 C 3 l 4 C 3+4 l 4 C 3+4, Q ;3,4 l 4 C 3+4 l 4 C 3+4 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 33 / 5 Contributions of λ Y contributions of λ Y to the Lagrange eqs: λ g ;, λ g ;3,4 g ;,, l C + l C + θ, l C + g ;3,4 3,4 l3 C 3 + l 4 C 3+4 θ 3,4 l 4 C 3+4 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 37 / 5 Equation stabilizing constraint Y Ÿ + α Y + α Y g T ;, ω, + g T ;3,4 ω 3,4 ω T, Q ;, ω, ω T 3,4 Q ;3,4 ω 3,4 l C g ;, + l C + l C + l3 C g ;3,4 3 + l 4 C 3+4 l 4 C α(g T ;, ω, g T ;3,4 ω 3,4 ) + α Y l S, Q ;, l S + l S + l S + l S + l3 S 3 l 4 S 3+4 l 4 S 3+4, Q ;3,4 l 4 S 3+4 l 4 S 3+4 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 34 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 38 / 5 Equations stabilizing constraint X and Y H, ω, + τ, + λ g ;, + λ g ;, H 3,4 ω 3,4 + τ 3,4 λ g ;3,4 λ g ;3,4 H, O J, T O H 3,4 J T 3,4 J T, g ;, g ;, J T 3,4 g ;3,4 g ;3,4 λ λ λ ω, ω 3,4 τ, τ λ 3,4 g T ;, ω, + g T ;3,4 ω 3,4 C (θ,, θ 3,4, ω,, ω 3,4 ) g T ;, ω, + g T ;3,4 ω 3,4 C (θ,, θ 3,4, ω,, ω 3,4 ) C ω T, Q ;, ω, ω T 3,4 Q ;3,4 ω 3,4 + α(g T ;, ω, g T ;3,4 ω 3,4 ) + α X C ω T, Q ;, ω, ω T 3,4 Q ;3,4 ω 3,4 + α(g T ;, ω, g T ;3,4 ω 3,4 ) + α Y C C, C T J, J 3,4 O ω, ω 3,4 C λ Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 35 / 5 Equation stabilizing constraint X Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 39 / 5 Dnamic equations for closed link mechanism Recall that X, 3,4 Ẋ (, θ, ) T θ, ( 3,4 θ 3,4 ) T θ3,4 g T ;, ω, g T ;3,4 ω 3,4 Ẍ ġ T ;, ω, + g T ;, ω, ġ T ;3,4 ω 3,4 g T ;3,4 ω 3,4 Since ġ ;, g ;, θ θ, T, g ;, ω θ, T, we have ġ;, T ω, ω, T Q ;, ω,, Q ;, g T ;, θ,, / θ, / θ θ, / θ θ, / Hessian matri H, O J T, O H 3,4 J T 3,4 J, J 3,4 O ω, ω 3,4 λ τ, τ 3,4 C the coefficient matri is regular we can compute ω through ω 4 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 36 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 4 / 5

6 Phsical Interpretation closed DOF PD ode params.m J, and J 3,4 : Jacobian matrices of the left and right arms λ λ, λ T : constraint force equivalent torques around rotational joints and : J,λ T λ ( l S l S + ) + λ (l C + l C + ) λ ( l S + ) + λ l C + reaction force λ equivalent torques around rotational joint 3 and 4: J3,4( λ) T λ (l 3 S 3 + l 4 S 3+4 ) + λ ( l 3 C 3 l 4 C 3+4 ) λ l 4 S λ ( l 4 C 3+4 ) b joint_torques_ + tau(:); joint_torques_34 + tau(3:4); C ; C ; p A \ b; dotomega p(:4); dotq dottheta; dotomega; Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 4 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 45 / 5 open DOF geom.m closed DOF PD.m function tip, tip, g, g, Q, Q... open_link_dof_geom (theta, param) theta theta(); theta theta(); l param().length; param().length; base param().base; C cos(theta); S sin(theta); C cos(theta+theta); S sin(theta+theta); tip base() + l*c + *C; tip base() + l*s + *S; g -l*s-*s; -*S ; g l*c+*c; *C ; Q -l*c-*c, -*C; -*C, -*C ; Q -l*s-*s, -*S; -*S, -*S ; param closed_link_dof_definition (); gravit 9.8; thetad pi/3; pi/6 ; Kp 3; 3 ; Kd.5;.5 ; alpha ; closed_link_dof_pd_ode_params... (t, q, param, gravit, thetad, Kp, Kd, alpha); interval, ; thetainit pi/; -pi/6; pi/; pi/6 ; omegainit ; ; ; ; qinit thetainit; omegainit; time, q ode45(closed_link_dof_pd_ode, interval, qinit); Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 4 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 46 / 5 closed DOF PD ode params.m closed DOF PD.m function dotq closed_link_dof_pd_ode_params... (t, q, param, gravit, thetad, Kp, Kd, alpha) theta q(:4); omega q(5:8); theta theta(:); omega omega(:); inertia_matri_, joint_torques_... open_link_dof_dnamics(theta, omega, param(:), gravit); theta34 theta(3:4); omega34 omega(3:4); inertia_matri_34, joint_torques_34... open_link_dof_dnamics(theta34, omega34, param(3:4), gravit); dottheta omega; tau -Kp()*(theta()-thetad())-Kd()*omega(); ; -Kp()*(theta(3)-thetad())-Kd()*omega(3); ; Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 43 / 5 thetad thetad()*ones(size(time)); thetad3 thetad()*ones(size(time)); plot(time, q(:,), r-, time, q(:,3), b-,... time, thetad, r--, time, thetad3, b-- ); pause; clf; for k::sz() closed_link_dof_draw(theta(k,:), param); lim(-4,4); lim(,4); pbaspect( ); pause(.); Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 47 / 5 closed DOF PD ode params.m Result,, g, g, Q, Q... open_link_dof_geom (theta, param(:)); 34, 34, g34, g34, Q34, Q34... open_link_dof_geom (theta34, param(3:4)); X - 34; Y - 34; C omega *Q*omega + omega34 *(-Q34)*omega *alpha*(g *omega + (-g34) *omega34) + alpha^*x; C omega *Q*omega + omega34 *(-Q34)*omega *alpha*(g *omega + (-g34) *omega34) + alpha^*y; A inertia_matri_, zeros(,), -g, -g; zeros(,), inertia_matri_34, g34, g34; -g, g34,, ; -g, g34,, ; Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 44 / 5 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 48 / 5

7 closed DOF draw.m function closed_link_dof_draw (theta, param) hold on; open_link_dof_draw(theta(:), param(:)); open_link_dof_draw(theta(3:4), param(3:4)); hold off; Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 49 / 5 Drawing Result Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 5 / 5 Dnamic Simulation of a Link Mechanism Report #3 due date : Dec. (Mon.) Simulate the control of a four-link closed manipulator. Appl PID control to active joints and 3. Use appropriate values of geometrical and phsical parameters of the manipulator. Show how position of joint 5 changes according to time. tip point joint 5 l4 link 4 joint 4 θ4 O l link θ joint (,) link 3 joint 3 (3,3) θ3 l3 Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 5 / 5 Summar Summar Open link mechanism inertia matri deps on joint angles of open link mechanism Closed link mechanism two open link mechanisms with geometric constraints snthesized from Lagrange equations of open link mechanisms Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analtical Mechanics: Link Mechanisms 5 / 5

Analytical Mechanics: Variational Principles

Analytical Mechanics: Variational Principles Shinichi Hirai Dept. Robotics, Ritsumeikan Univ. Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.) Analytical Mechanics: Variational Principles 1 / 71 Agenda