Chapter 4 Manipulator Dynamics




                                                                                       ...
Chapter 4 Manipulator Dynamics




                                                                                      2...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.1 Introduction
                                                        ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange Equations
                                                  ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange Equations
                                                  ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.2 Lagrange’s Equations
                                                ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.3 Dynamics of Open-chain Manipulators
                                 ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
Chapter 4 Manipulator Dynamics

                 4.4 Coordinate Invariant Algorithms
                                     ...
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
[Download] Rev-Chapter-4
Upcoming SlideShare
Loading in …5
×

[Download] Rev-Chapter-4

823 views
786 views

Published on

2nd International Summer School on Geometric Methods in Robotics, Mechanism Design and Manufacturing Research-Lecture 04 Robot Dynamics and Control

Published in: Education, Technology, Sports
0 Comments
0 Likes
Statistics
Notes
  • Be the first to comment

  • Be the first to like this

No Downloads
Views
Total views
823
On SlideShare
0
From Embeds
0
Number of Embeds
12
Actions
Shares
0
Downloads
47
Comments
0
Likes
0
Embeds 0
No embeds

No notes for slide

[Download] Rev-Chapter-4

  1. 1. Chapter 4 Manipulator Dynamics 1 Chapter Lecture Notes for Manipulator Dynamics A Geometrical Introduction to Introduction Lagrange’s Robotics and Manipulation Equations Dynamics of Open-chain Richard Murray and Zexiang Li and Shankar S. Sastry Manipulators CRC Press Coordinate Invariant Algorithms Lagrange’s Zexiang Li and Yuanqing Wu Equations with Constraints ECE, Hong Kong University of Science & Technology June ,
  2. 2. Chapter 4 Manipulator Dynamics 2 Chapter Manipulator Dynamics Chapter Manipulator Dynamics Introduction Introduction Lagrange’s Equations Lagrange’s Equations Dynamics of Open-chain Manipulators Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms Lagrange’s Coordinate Invariant Algorithms Equations with Constraints Lagrange’s Equations with Constraints
  3. 3. Chapter 4 Manipulator Dynamics 4.1 Introduction 3 Definition: Dynamics Physical laws governing the motions of bodies and aggregates of Chapter Manipulator bodies. Dynamics Introduction Lagrange’s Equations Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  4. 4. Chapter 4 Manipulator Dynamics 4.1 Introduction 3 Definition: Dynamics Physical laws governing the motions of bodies and aggregates of Chapter Manipulator bodies. Dynamics Introduction ◻ A short history: Lagrange’s Equations “Everything happens for a reason.” Dynamics of Open-chain Aristotle (384 BC - 322 BC) Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  5. 5. Chapter 4 Manipulator Dynamics 4.1 Introduction 3 Definition: Dynamics Physical laws governing the motions of bodies and aggregates of Chapter Manipulator bodies. Dynamics Introduction ◻ A short history: Lagrange’s Equations “Everything happens for a reason.” Dynamics of Open-chain Aristotle (384 BC - 322 BC) Manipulators Coordinate Invariant Algorithms Experiments with cannon balls from the tower of Pisa. Lagrange’s G. Galilei (1564 - 1642) Equations with Constraints
  6. 6. Chapter 4 Manipulator Dynamics 4.1 Introduction 3 Definition: Dynamics Physical laws governing the motions of bodies and aggregates of Chapter Manipulator bodies. Dynamics Introduction ◻ A short history: Lagrange’s Equations “Everything happens for a reason.” Dynamics of Open-chain Aristotle (384 BC - 322 BC) Manipulators Coordinate Invariant Algorithms Experiments with cannon balls from the tower of Pisa. Lagrange’s G. Galilei (1564 - 1642) Equations with Constraints Laws of motion. I. Newton (1642 - 1726) (Continues next slide)
  7. 7. Chapter 4 Manipulator Dynamics 4.1 Introduction 4 Chapter Manipulator Laws of motion from particles to rigid bodies. Dynamics L. Euler (1707 - 1783) Introduction Lagrange’s Equations Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  8. 8. Chapter 4 Manipulator Dynamics 4.1 Introduction 4 Chapter Manipulator Laws of motion from particles to rigid bodies. Dynamics L. Euler (1707 - 1783) Introduction Lagrange’s Equations Calculus of Variation and the Principles of least action. Dynamics of Open-chain J. Lagrange (1736 - 1813) Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  9. 9. Chapter 4 Manipulator Dynamics 4.1 Introduction 4 Chapter Manipulator Laws of motion from particles to rigid bodies. Dynamics L. Euler (1707 - 1783) Introduction Lagrange’s Equations Calculus of Variation and the Principles of least action. Dynamics of Open-chain J. Lagrange (1736 - 1813) Manipulators Coordinate Invariant Algorithms Quaternions and Hamilton’s Principle. Lagrange’s W. Hamilton (1805 - 1865) Equations with Constraints † End of Section †
  10. 10. Chapter 4 Manipulator Dynamics 4.2 Lagrange Equations 5 ◻ A simple example: Newton’s Equation: Lagrangian Equation: m¨ = Fx Chapter Manipulator x m¨ = Fy − mg Dynamics Introduction y Momentum: Px = m˙ Lagrange’s Equations x Dynamics of Py = m˙y = Fx , dt Py = Fy − mg Open-chain d d Manipulators P dt x Coordinate Invariant y Algorithms Lagrange’s Fy F Equations with Constraints m Fx mg x Figure 4.1
  11. 11. Chapter 4 Manipulator Dynamics 4.2 Lagrange Equations 5 ◻ A simple example: Newton’s Equation: Lagrangian Equation: m¨ = Fx Chapter − = Fx Manipulator x d ∂L ∂L m¨ = Fy − mg Dynamics y dt ∂˙ ∂x x Introduction − = Fy d ∂L ∂L Momentum: Px = m˙ Lagrange’s Equations x dt ∂˙ ∂y y Dynamics of Py = m˙y Lagrangian function: = Fx , dt Py = Fy − mg Open-chain d d Manipulators P dt x Coordinate Invariant y ⇔ L = T − V, Px = ∂L , Py = ∂L Algorithms ∂˙ x ∂˙ y Lagrange’s Fy F Kinetic energy: Equations with m Fx T = m(˙ + y ) Constraints x ˙ mg x Potential energy: Figure 4.1 V = mgy
  12. 12. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 6 ◻ Generalization to multibody systems: qi , i = , . . . , n: generalized coordinates Chapter Kinetic energy: Manipulator Dynamics y T = T(q, q) ˙ Introduction m q Lagrange’s Equations Potential energy: V = V(q) Dynamics of Open-chain m Manipulators q Lagrangian: Coordinate Invariant L(q, q) = T(q, q) − V(q) Algorithms m ˙ ˙ Lagrange’s q τ i , i = , . . . , n: external force on qi Equations with Constraints x Lagrangian Equation: Figure 4.2 d ∂L ∂L − = τi , i = , . . . , n dt ∂˙ i ∂qi q
  13. 13. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 7 Example: Pendulum equation Generalized coordinate: θ∈S Chapter y Manipulator Dynamics Kinematics: x = l sin θ, y = −l cos θ Introduction x Lagrange’s l x = l cos θ ⋅ θ, y = l sin θ ⋅ θ Equations Dynamics of ˙ ˙ ˙ ˙ θ Open-chain Manipulators Kinetic energy: Coordinate T(θ, θ) = m(˙ + y ) = ml θ ˙ x ˙ ˙ Invariant mg Algorithms Potential energy: Figure 4.3 Lagrange’s Equations with V = mgl( − cos θ) Constraints Lagrangian function: L = T − V = ml θ − mgl( − cos θ), ⇒ ˙ ∂L ˙ ∂θ = ml θ, ˙ ∂L ∂θ = −mgl sin θ Equation of motion: d ∂L ˙ dt ∂ θ − ∂L ∂θ = τ ⇒ ml θ + mgl sin θ = τ ¨
  14. 14. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 8 Example: A spherical pendulum Generalized coordinate: ⎡ l sin θ cos ϕ ⎤ Chapter Manipulator ⎢ ⎥ Dynamics r(θ, ϕ) = ⎢ l sin θ sin ϕ ⎢ −l cos θ ⎥ ⎥ ⎣ ⎦ Introduction Lagrange’s Kinetic energy: Equations Dynamics of Open-chain T= m ˙ r = ml (θ + ( − cos θ)ϕ ) ˙ ˙ Manipulators θ Coordinate Invariant Potential energy: Algorithms Lagrange’s V = −mgl cos θ Equations with Constraints Lagrangian function: ϕ mg L(q, q) = ˙ ml (θ + ( − cos θ)ϕ ) + mgl cos θ ˙ ˙ Figure 4.4 (continues next slide)
  15. 15. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 9 ⎧d ⎪ ∂L d ⎪ ⎪ ⎪ dt = (ml θ) = ml θ, ˙ ¨ ⎪ ˙ ⎨ ∂θ dt ⎪ Chapter ⎪ ⎪ ∂L Manipulator ⎪ ⎪ = ml sin θ cos θ ϕ − mgl sin θ ˙ Dynamics ⎩ ∂θ ⎧ ⎪d ∂L d ⎪ Introduction ⎪ ⎪ dt = (ml sin θ ϕ) = ml sin θ ϕ + ml sin θ cos θ θ ϕ, ˙ ¨ ˙˙ ⎪ ⎪ ˙ ∂ϕ dt ⎨ Lagrange’s Equations ⎪ ⎪ ⎪ ∂L ⎪ ⎪ = Dynamics of ⎪ ⎩ ∂ϕ Open-chain Manipulators ml ¨ −ml sθ cθ ϕ˙ Coordinate ml sθ θ ¨ + ˙˙ + mglsθ = Invariant ϕ ml sθ cθ θ ϕ Algorithms Lagrange’s Equations with Constraints
  16. 16. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 10 ◻ Kinetic energy of a rigid body: Chapter Manipulator r Dynamics B Introduction Lagrange’s ra Equations A gab Dynamics of Open-chain Manipulators Coordinate Figure 4.5 Invariant Algorithms Volume occupied by the body: V Lagrange’s Equations with Mass density: ρ(r) m= ∫ Constraints Mass: ρ(r)dV V Mass center: r≜ m ∫ V ρ(r)rdV Relative to frame at the mass center: r= (Continues next slide)
  17. 17. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 11 Kinetic energy (In A-frame): T= ρ(r) p + Rr dV = + pT Rr + Rr )dV Chapter Manipulator Dynamics ∫V ˙ ˙ ∫ V ˙ ρ(r)( p ˙ ˙ ˙ = m p + pT R ∫ ρ(r)rdV + ∫ Introduction ˙ ˙ ˙ ˙ ρ(r) Rr dV Lagrange’s V V Equations = Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  18. 18. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 11 Kinetic energy (In A-frame): T= ρ(r) p + Rr dV = + pT Rr + Rr )dV Chapter Manipulator Dynamics ∫V ˙ ˙ ∫ V ˙ ρ(r)( p ˙ ˙ ˙ = m p + pT R ∫ ρ(r)rdV + ∫ Introduction ˙ ˙ ˙ ˙ ρ(r) Rr dV Lagrange’s V V Equations = Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms ∫ V ˙ ρ(r) Rr dV = ∫ ρ(r) R Rr dV = ∫ ρ(r) ωr dV = ∫ ρ(r) ˆω dV Lagrange’s ˙T Equations with ˆ r Constraints = ∫ ρ(r)(−ω ˆ ω)dV = ω − ∫ ρ(r)ˆ dV ω ≜ ω Iω r T T r T (Continues next slide)
  19. 19. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 12 ⎡ Ixx Ixy Ixz ⎤ where ⎢ ⎥ I = − ρ(r)ˆ dV ≜ ⎢ Ixy Iyy Iyz ⎥ Chapter ∫ ⎢ ⎥ Manipulator r ⎢ Ixz Iyz Izz ⎥ Dynamics Introduction ⎣ ⎦ Lagrange’s is the Inertia tensor with Equations Dynamics of Open-chain Ixx = ∫ ρ(r)(y + z )dxdydz, Ixy = − ∫ ρ(r)xydxdydz Manipulators Coordinate T= m p ˙ + (ωb )T Iωb = m RT p ˙ + (ωb )T Iωb Invariant Algorithms Lagrange’s = (V b )T mI I Vb Equations with Constraints Mb M b : Generalized inertia matrix in B-frame.
  20. 20. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 13 Example: Mb for a rectangular object ρ= Chapter m z Manipulator Dynamics lωh Introduction Ixx = ∫ ρ(y + z )dxdydz y V Lagrange’s h ω l x h =ρ (y + z )dxdydz Equations Dynamics of ∫ ∫ ∫ − h − ω − l l Open-chain w =ρ (lω h + lωh ) = (ω + h ) Manipulators m Coordinate Figure 4.6 Invariant h ω l Ixy = − ρxydV = −ρ Algorithms Lagrange’s Equations with ∫ V ∫ ∫ ∫ −h −ω −l xydxdydz Constraints l h ω = −ρ dydz = y ∫ ∫ −h −ω x −l (Continues next slide)
  21. 21. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 14 ⎡ (w + h ) ⎤ ⎢ ⎥ m ⎢ ⎥ b I=⎢ (l + h ) ⎥,M = mI I ⎢ ⎥ m × ⎢ (w + l ) ⎥ Chapter ⎣ ⎦ Manipulator m Dynamics Introduction Lagrange’s Equations Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  22. 22. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 14 ⎡ (w + h ) ⎤ ⎢ ⎥ m ⎢ ⎥ b I=⎢ (l + h ) ⎥,M = mI I ⎢ ⎥ m × ⎢ (w + l ) ⎥ Chapter ⎣ ⎦ Manipulator m Dynamics ◻ M b under change of frames: Introduction Lagrange’s Equations Dynamics of V = g− ⋅ g , T = V T Mb V ˆ ˙ g g (t) Open-chain V = Adg V Manipulators Coordinate T = (Adg V )T M b (Adg V ) Invariant Algorithms g (t) g Lagrange’s = V T AdT M b Adg V ≜ V T M b V Equations with Constraints g Figure 4.7 M b = AdT M b Adg g
  23. 23. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 15 Example: Dynamics of a -dof planar robot ⎡ Ixxi ⎤ Chapter ⎢ ⎥ Manipulator Ii = ⎢ Iyyi ⎥,i = , ⎢ ⎥ Dynamics Introduction ⎣ Izzi ⎦ l2 r 2 θ2 Lagrange’s Equations T(θ, θ) = m v + ωT I ω Dynamics of l1 Open-chain Manipulators ˙ y r1 θ1 + m v + ω I ω Coordinate Invariant T x Algorithms Figure 4.8 Lagrange’s Equations with Constraints ω = ω = ˙ θ θ +θ ˙ ˙ (Continues next slide)
  24. 24. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 16 xi Chapter Pi = yi ∶ Mass center γi ∶ Distance from joint i to mass center Manipulator Dynamics Introduction Lagrange’s Equations Change of Coordinates: ⎧ x = −r s θ ⎪˙ x =rc ⎪ Dynamics of ˙ ⇒⎨ Open-chain y =rs ⎪y =r c θ Manipulators Coordinate ⎪˙ ⎩ ˙ Invariant ⎧ x = −(l s + r s )θ − r s θ Algorithms x =l c +r c ⎪˙ ⎪ ˙ ˙ ⇒⎨ Lagrange’s y =l s +r s ⎪ y = (l c + r c )θ + r c θ ⎪˙ Equations with ⎩ Constraints ˙ ˙ (Continues next slide)
  25. 25. Chapter 4 Manipulator Dynamics 4.2 Lagrange’s Equations 17 Kinetic energy: Chapter Manipulator T(θ, θ) = m (x + y ) + Iz θ + m (x + y ) + Iz (θ + θ ) ˙ ˙ ˙ ˙ ˙ ˙ ˙ ˙ α + βc δ + βc Dynamics = [θ θ ] ˙ θ δ + βc Introduction ˙ ˙ Lagrange’s δ ˙ θ Equations M(θ) Dynamics of Open-chain α = Iz + Iz + m r + m (l + r ), Manipulators β = m l r , δ = Iz + m r , L = T Coordinate Invariant Algorithms Lagrange’s Equations with Equation of motion: Constraints −βs θ −βs (θ + θ ) + = ¨ θ ˙ ˙ ˙ ˙ θ τ M(θ) ¨ ˙ ˙ τ θ βs θ θ † End of Section †
  26. 26. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 18 ◻ Dynamics of an open-chain manipulator: Chapter Definition: Li : frame at mass center of link i, gsli (θ) = e ξ ⋯e ξ i θ i gsli (o) Manipulator ˆ θ ˆ Dynamics θ1 Introduction θ2 θ3 l1 l2 Lagrange’s L2 L3 Equations Dynamics of r1 r2 Open-chain l0 L1 Manipulators r0 Coordinate S Invariant Algorithms Figure 4.9 Lagrange’s ⎡ θ ⎤ Equations with ⎢ ⎥ Constraints ˙ ⎢ ⎥ ⎢ ˙ ⎥ ⎢ ⎥ Vsli = Jsli (θ)θ = [ξ † ξ † ⋯ ξ † b b ⋯ ] ⎢ ˙θ i ⎥ = Ji (θ)θ ⎢ θ i+ ⎥ ˙ ˙ ⎢ ⎥ i ⎢ ⎥ ⎢ ˙ ⎥ ⎣ θn ⎦ (Continues next slide)
  27. 27. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 19 ξ † = Ad− (e ξ j+ ⋯e ξ i θ i gsli ( ))ξ j , j ≤ i ˆ θ j+ ˆ j Ti (θ, θ) = (Vsli )T Mib Vsli = θ T JiT (θ)Mib Ji (θ)θ Chapter ˙ b b ˙ ˙ Manipulator Dynamics n Introduction T(θ) = Ti (θ, θ) = ˙ θ T M(θ)θ, ˙ ˙ i= Lagrange’s n JiT (θ)Mib Ji (θ) = Mij (θ)θ i θ j Equations M(θ) = ˙ ˙ Dynamics of i i,j= Open-chain Manipulators hi (θ): Height of Li , Vi (θ) = mi ghi (θ), V(θ) = mi ghi (θ) Coordinate i= Invariant Algorithms Lagrange’s Equation: Lagrange’s d ∂L ∂L − = τ i , i = , . . . , n, Equations with ˙ dt ∂θ i ∂θ i Constraints d ∂L d ⎛n ˙⎞ n ¨ ˙ ˙ Mij θ j = Mij θ j + Mij θ j dt ⎝ j= ⎠ = ˙ dt ∂θ i j= (Continues next slide)
  28. 28. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 20 ∂L n ∂Mkj ∂V ∂Mij ˙ = ˙ ˙ θk θj − , ˙ Mij = θk ∂θ i j,k= ∂θ i ∂θ i k ∂θ k Chapter Manipulator n n ∂Mij ˙ ˙ ∂Mkj ⇒ ¨ ˙ ˙ ∂V Dynamics Mij θ j + θj θk − θk θj + = τi j= j,k= ∂θ k ∂θ i ∂θ i Introduction n n ⇒ Lagrange’s ¨ ˙ ˙ ∂V Mij θ j + Γijk θ k θ j + = τi Equations j= j,k= ∂θ i Dynamics of Open-chain ∂Mij ∂Mik ∂Mkj Γijk ≜ + − Manipulators ∂θ k ∂θ j ∂θ i Coordinate ˙ ˙ θ i ⋅ θ j , i ≠ j ∶ Coriolis term, ˙ θ i : Centrifugal term Invariant Algorithms Lagrange’s Equations with Constraints
  29. 29. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 20 ∂L n ∂Mkj ∂V ∂Mij ˙ = ˙ ˙ θk θj − , ˙ Mij = θk ∂θ i j,k= ∂θ i ∂θ i k ∂θ k Chapter Manipulator n n ∂Mij ˙ ˙ ∂Mkj ⇒ ¨ ˙ ˙ ∂V Dynamics Mij θ j + θj θk − θk θj + = τi j= j,k= ∂θ k ∂θ i ∂θ i Introduction n n ⇒ Lagrange’s ¨ ˙ ˙ ∂V Mij θ j + Γijk θ k θ j + = τi Equations j= j,k= ∂θ i Dynamics of Open-chain ∂Mij ∂Mik ∂Mkj Γijk ≜ + − Manipulators ∂θ k ∂θ j ∂θ i Coordinate ˙ ˙ θ i ⋅ θ j , i ≠ j ∶ Coriolis term, ˙ θ i : Centrifugal term Invariant Algorithms n n ∂Mij ∂Mik ∂Mkj ˙ Lagrange’s Equations with Define: Cij (θ, θ) = ˙ ˙ Γijk θ k = + − θk k= k= ∂θ k ∂θ j ∂θ i Constraints ⇒ M(θ)θ + C(θ, θ)θ + N(θ) = τ ¨ ˙ ˙
  30. 30. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 21 Chapter Manipulator Property 1: M(θ) = M T (θ), θ T M(θ)θ ≥ , θ T M(θ)θ = ⇔θ= Dynamics ˙ ˙ ˙ ˙ ˙ Introduction ˙ − C ∈ ℝn×n is skew symmetric M Lagrange’s Equations Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  31. 31. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 21 Chapter Manipulator Property 1: M(θ) = M T (θ), θ T M(θ)θ ≥ , θ T M(θ)θ = ⇔θ= Dynamics ˙ ˙ ˙ ˙ ˙ Introduction ˙ − C ∈ ℝn×n is skew symmetric M Lagrange’s Equations Proof : (M − C)ij = Mij − Cij (θ) Dynamics of Open-chain ˙ ˙ Manipulators n ∂Mij ˙ ∂Mij ˙ ∂Mik ˙ ∂Mkj Coordinate = θk − θk − θk + ˙ θk Invariant k= ∂θ k ∂θ k ∂θ j ∂θ i Algorithms n ∂Mkj ∂Mik ˙ Lagrange’s = ˙ θk − θk Equations with k= ∂θ i ∂θ j Constraints Switching i and j shows (M − C)T = −(M − C) ˙ ˙
  32. 32. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 22 Example: Planar 2-DoF Robot (continued) Chapter Manipulator m (θ) = α + β cos θ , m =δ m (θ) = m (θ) = δ + β cos θ Dynamics Introduction c (θ, θ) = −β sin θ ⋅ θ , c (θ, θ) = −β sin θ (θ + θ ) ˙ ˙ ˙ ˙ ˙ Lagrange’s Equations c (θ, θ) = β sin θ ⋅ θ , c (θ, θ) = ˙ ˙ ˙ Dynamics of Open-chain ( )= Manipulators ∂M ∂M ∂M ∂M Γ = + − = Coordinate ∂θ ∂θ ∂θ ∂θ Invariant ( )= Algorithms ∂M ∂M ∂M ∂M Γ = + − = −β sin θ Lagrange’s ∂θ ∂θ ∂θ ∂θ ( )= Equations with ∂M ∂M ∂M ∂M Constraints Γ = + − = −β sin θ ∂θ ∂θ ∂θ ∂θ ( )= ∂M ∂M ∂M ∂M ∂M Γ = + − − = −β sin θ ∂θ ∂θ ∂θ ∂θ ∂θ (Continues next slide)
  33. 33. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 23 ( )= ∂M ∂M ∂M ∂M ∂M Chapter Γ = + − − = β sin θ Manipulator ∂θ ∂θ ∂θ ∂θ ∂θ Dynamics ( )= ∂M ∂M ∂M ∂M Γ = + − = Introduction ∂θ ∂θ ∂θ ∂θ ( )= Lagrange’s ∂M ∂M ∂M ∂M Equations Γ = + − = ∂θ ∂θ ∂θ ∂θ Dynamics of ( )= Open-chain ∂M ∂M ∂M ∂M Γ = + − = Manipulators ∂θ ∂θ ∂θ ∂θ Coordinate − β sin θ ⋅ θ˙ ˙ −β sin θ ⋅ θ Invariant ˙ M− C= −β sin θ ⋅ θ˙ Algorithms Lagrange’s − ˙ − β sin θ ⋅ θ − β sin θ (θ + θ ) ˙ ˙ Equations with ˙ β sin θ ⋅ θ Constraints β sin θ ( θ + θ ) ⇐ skew-symmetric ˙ ˙ −β sin θ ( θ + θ ) = ˙ ˙
  34. 34. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 24 Example: Dynamics of a -dof robot ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ −l ⎥ θ1 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ l ⎥ −l θ2 θ3 ξ =⎢ ⎥,ξ = ⎢ ⎥,ξ = ⎢ − ⎥ Chapter l1 l2 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ Manipulator ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ − L2 L3 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ Dynamics ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ r1 r2 ⎡ ⎤ Introduction ⎢ I ⎥ l0 gsl ( ) = ⎢ ⎥, L1 ⎢ ⎥ Lagrange’s ⎢ r ⎥ Equations r0 ⎣ ⎦ S Dynamics of ⎡ ⎤ ⎢ ⎥ gsl ( ) = ⎢ I ⎥, Open-chain r ⎢ ⎥ Figure 4.9 ⎢ ⎥ Manipulators l ⎣ ⎦ ⎡ ⎤ Coordinate ⎢ ⎥ Invariant Algorithms gsl ( ) = ⎢ I ⎢ l +r ⎥ ⎥ ⎢ l ⎥ Lagrange’s ⎣ ⎦ ⎡ mi ⎤ Equations with ⎢ ⎥ ⎢ ⎥ Constraints mi ⎢ ⎥ Mi = ⎢ ⎥ mi ⎢ Ix i ⎥ ⎢ Iy i ⎥ ⎢ ⎥ ⎢ Iz i ⎥ ⎣ ⎦ (Continues next slide)
  35. 35. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 25 mi : The mass of the object Chapter Ix i : The moment of inertia about the x axis Manipulator Dynamics Introduction Γ = (Iy − Iz − m r )c s + (Iy − Iz )c s − t(l s + r s ) Lagrange’s Γ = (Iy − Iz )c s − tr s Equations Dynamics of Γ = (Iy − Iz − m r )c s + (Iy − Iz )c s − t(l s + r s ) Open-chain Γ = (Iy − Iz )c s − tr s Manipulators Coordinate Γ = (Iz − Iy + m r )c s + (Iz − Iy )c s + t(l s + r s ) Invariant Γ = −l m r s Algorithms Γ = −l m r s Lagrange’s Equations with Γ = −l m r s = (Iz − Iy )c s Constraints Γ + tr s Γ =lm r s where t = m (l c + r c ). (Continues next slide)
  36. 36. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 26 , V(θ) = m gh (θ) + m gh (θ) + m gh (θ) ˙ ∂V N(θ, θ) = ∂θ gsli (θ) = e ξ ⋯e ξ i θ i gsli ( ) ⇒ Chapter ˆ θ ˆ Manipulator Dynamics h (θ) = r , h (θ) = l − r sin θ, h (θ) Introduction = l − l sin θ − r sin(θ + θ ) ⎡ ⎤ ⎢ ⎥ Lagrange’s ⎢ ⎥ ⎢ ⎥ Equations J = Jsl (θ) = ⎢ b ⎥ ⎢ ⎥ ⎢ ⎥ Dynamics of ⎢ ⎥ Open-chain Manipulators ⎣ ⎦ ⎡ −r c ⎤ ⎢ ⎥ ⎢ ⎥ Coordinate Invariant ⎢ ⎥ J = Jsl (θ) = ⎢ b −r ⎥ ⎢ ⎥ Algorithms ⎢ −s ⎥ − ⎢ c ⎥ ⎣ ⎦ Lagrange’s Equations with ⎡ −l c − r c ⎤ ⎢ ⎥ Constraints ⎢ ⎥ ⎢ ⎥ ls J = Jsl (θ) = ⎢ b −r − l c ⎥ ⎢ ⎥ −r ⎢ ⎥ − − ⎢ ⎥ ⎣ ⎦ −s c (Continues next slide)
  37. 37. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 27 M M M Chapter M(θ) = M M M = JT M J + JT M J + JT M J Manipulator M M M + m r c + m (l c + r c ) Dynamics M = Iy s + Iy s + Iz + Iz c + Iz c Introduction M =M =M =M = Lagrange’s Equations M = Ix + Ix + m l + M r + m r + m l r c Dynamics of M = Ix + m r + m l r c Open-chain Manipulators M = Ix + m r + m l r c Coordinate M = Ix + m r Invariant Algorithms n n ∂Mij ∂Mik ∂Mkj Lagrange’s Cij (θ, θ) = ˙ ˙ Γijk θ k = + − ˙ θk Equations with k= k= ∂θ k ∂θ j ∂θ i Constraints
  38. 38. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 28 ◻ Additional Properties of the dynamics in terms of POE: ⎧Ad− Define: ⎪ ⎪ ⎪ e ξ j+ θ j+ ⋯e ξ i θ i i > j Chapter ⎪ ⎪ Manipulator ˆ ˆ Aij = ⎨I ⎪ Dynamics ⎪ i=j ⎪ ⎪ ⎪ ⎩ Introduction i<j Lagrange’s Equations Ji (θ) = Adg − ( ) [Ai ξ ⋯Aii ξ i ⋯ ] sl i Dynamics of Open-chain Mi′ = AdT− g Mi Adg − ( intertia of ith link in S) sl i ( ) sl i ( ) Manipulators Coordinate Invariant Algorithms Lagrange’s Equations with Constraints
  39. 39. Chapter 4 Manipulator Dynamics 4.3 Dynamics of Open-chain Manipulators 28 ◻ Additional Properties of the dynamics in terms of POE: ⎧Ad− Define: ⎪ ⎪ ⎪ e ξ j+ θ j+ ⋯e ξ i θ i i > j Chapter ⎪ ⎪ Manipulator ˆ ˆ Aij = ⎨I ⎪ Dynamics ⎪ i=j ⎪ ⎪ ⎪ ⎩ Introduction i<j Lagrange’s Equations Ji (θ) = Adg − ( ) [Ai ξ ⋯Aii ξ i ⋯ ] sl i Dynamics of Open-chain Mi′ = AdT− g Mi Adg − ( intertia of ith link in S) sl i ( ) sl i ( ) Manipulators Coordinate Invariant Property 2: Algorithms n n ∂Mij ∂Mik ∂Mkj Lagrange’s Mij (θ) = ξ iT AT Ml′ Alj ξ j , Cij (θ, θ) = li ˙ + − ˙ θk Equations with l=max(i,j) k= ∂θ k ∂θ j ∂θ i Constraints where ∂Mij n = [Ak− ,i ξ i , ξ k ]T AT Ml′ Alj ξ j + ξ iT AT Ml′ Alk [Ak− ,j ξ j , ξ k ] lk li ∂θ k l=max(i,j) † End of Section †
  40. 40. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 29 ◻ Newton-Euler equations in spatial frame: Newton’s Equation: f s = (mp) = mp Chapter d Manipulator ˙ ¨ Dynamics dt Introduction Spatial angular momentum: Lagrange’s Equations I s ⋅ ω s = R(I ⋅ ω b ) = R ⋅ I ⋅ RT ⋅ω s Is Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms r T Lagrange’s Equations with Constraints f S g ∶ (R, p) Figure 4.10 (Continues next slide)
  41. 41. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 30 τs = (I ω ) = (RIRT ω s ) = I s ω s + RIRT ω s + RI RT ω s d s s d ˙ ˙ ˙ dt dt = I s ω s + RRT I s ω s − RIRT ω s ω s = I s ω s + ω s × (I s ω s ) Chapter Manipulator ˙ ˙ ˆ ˙ Dynamics Introduction ωs ˆ Lagrange’s Equations ◻ Transform equations to body frame: (mp) = mRvb = mRvb + mR˙ b , RT f s = mRT Rvb + m˙ b Dynamics of d d ˙ ˙ Open-chain ˙ v v Manipulators dt dt Coordinate ⇒ f b = mω b × vb + m˙ b , v Invariant τ b = RT τ s = RT (RIω b ) = I ω b + ω b × Iω b Algorithms d ˙ Lagrange’s dt Equations with Constraints ⇒ mI vb ˙ + ω b × mvb = fb = Fb (∗) I ωb ˙ ω b × Iω b τb Mb Vb (Continues next slide)
  42. 42. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 31 Define: [ , ] ∶ se( ) × se( ) ↦ se( ), [ ξ , ξ ] ≜ ξ ξ − ξ ξ ˆ ˆ ˆˆ ˆ ˆ Chapter Manipulator if [ξ , ξ ] = ωi ˆ vi ,i = , Dynamics ˆ ˆ Introduction then ξ= (ω × ω )∧ ω v −ω v = ad ξ i ⋅ ξ Lagrange’s Equations ˆ ˆ ˆ Dynamics of Open-chain where ad ξ = ω ˆ v ˆ Manipulators Coordinate ωˆ Invariant Algorithms It is straightforward computation to see that: Lagrange’s (∗) ⇔ M b V b − adT b M b V b = F b ˙ V Equations with Constraints Property 3: Adg [ ξ , ξ ] = [Adg ξ , Adg ξ ] ⇒ Adg ad ξ = adAd g ξ Adg ˆ ˆ ˆ ˆ ˆ ˆ
  43. 43. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 32 ◻ Coordinate invariance of Newton-Euler equations: Chapter M V − adT M V = F Manipulator Dynamics ˙ V ⎧V = Adg V , r g ⎪ ⎪ Introduction g B ⎨ ⇒ ⎪ F = AdT− F Lagrange’s ⎪ ⎩ Equations g F = (AdT )− F = (Ad− )T F Dynamics of Open-chain g Manipulators g g A Coordinate M = Ad−T M Ad− g g Figure 4.11 Invariant ⇒ Adg M Ad− −T Adg V − ad(Ad g T −T Ad− Adg V = Ad−T F Algorithms ˙ g V ) Adg M ˙ g g Lagrange’s Equations with = Adg adV Ad− by Property 3, we have Constraints Since adAd g V g M V − adT M V = F ˙ V
  44. 44. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 33 Chapter Manipulator C2 C3 Dynamics C1 Introduction l0 Lagrange’s l1 l2 Equations C0 Dynamics of Open-chain Figure 4.12 Manipulators Ci : Frame fixed to link i, located along the ith axis Coordinate Invariant Fi : Generalized force link i − exerting on link i, expressed in Ci Algorithms τi : Joint torque of link i Lagrange’s gi− ,i : Transformation of Ci relative to Ci− gi− ,i (θ i ) = e ξ i θ i ⋅ gi− ,i ( ) = gi− ,i ( )e ξ i θ i ˆ′ ˆ Equations with Constraints ′ th ξ i = Adg − ( ) ⋅ ξ i : i axis in Ci frame. ⎧⎡ ⎤ i− ,i ⎪⎢ ⎪⎢ ⎪ ⎥ ⎪ z ⎥∶ ⎪⎢ ⎪ ⎥ ⎪ Revolute joint. ξ i = ⎨⎣ i ⎦ ⎡ z ⎤ ⎪⎢ i ⎥ ⎪ ⎪⎢ ⎪ ⎪⎢ ⎥∶ ⎪ ⎪ ⎥ Prismatic joint. ⎩⎣ ⎦
  45. 45. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 34 Chapter ⇒ gi− ,i ⋅ gi− ,i = ξ i ⋅ θ i − ˙ ˆ ˙ Manipulator Dynamics Mi : Moment of inertia in Ci −mi ˆi Mi = Introduction mi I r mi : Mass of link i Lagrange’s mi ˆi Ii − mi ˆi r r Ii : inertia tensor gi = gi− gi− ,i Equations Dynamics of Vi = gi− ⋅ gi = gi− ,i Vi− gi− ,i + ξ i θ i Open-chain Manipulators ˆ ˙ − ˆ ˆ˙ Vi = Ad − Vi− + ξ i θ i ˙ Coordinate Invariant gi− ,i Algorithms Lagrange’s Vi = gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i + ξ i θ i ˙ ˙− ˆ ˆ − ˆ ˙ − ˙ ˆ ˆ¨ = −˙i− ,i gi− ,i gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i gi− ,i gi− ,i Equations with Constraints g− − ˆ − ˆ − ˙ + gi− ,i Vi− gi− ,i + ξ i θ i − ˙ ˆ ˆ¨ (Continues next slide)
  46. 46. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 35 = − ξ i θ i (Adg − Vi− )∧ + (Adg − Vi− )∧ ξ i θ i + (Adg − Vi− )∧ + ξ i θ i ˆ˙ ˆ˙ ˙ ˆ¨ i− ,i i− ,i i− ,i Chapter ⇒ Vi = ξ i θ i + Adg − Vi− − ad ξ ˙ ¨ ˙ ˙ (Adg − Vi− ) Manipulator i− ,i i θi i− ,i Dynamics Introduction ◻ Forward Recursion (kinematics): ⎧ init. ∶ V = , V = g (gravity vector) ⎪ ⎪ Lagrange’s ⎪ ˙ ⎪ ⎪ ⎪ Equations ⎪ ⎪g ⎪ i− ,i = gi− ,i ( )e ξ i θ i ⎪ ⎪ ˆ Dynamics of ⎨ ⎪ Vi = Ad − Vi− + ξ i θ i Open-chain Manipulators ⎪ ⎪ ⎪ ˙ ⎪ ⎪ ⎪ g i− ,i ⎪ ˙ ⎪ V = ξ θ + Ad − V − ad (Ad − V ) ⎪ ⎪ Coordinate ⎩ Invariant i ¨ i i ˙ g i− ,i i− ˙ ξi θ i g i− ,i i− Algorithms Lagrange’s Equations with Constraints ◻ Backward Recursion (inverse dynamics): Fn+ : End-effector wrench, gn,n+ : transform from tool frame to Cn Fi = AdT− g ⋅ Fi+ + Mi Vi − adT i ⋅ Mi Vi ˙ V i,i+ τ i = ξ iT ⋅ Fi
  47. 47. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 36 Chapter Manipulator V = Adg −, ⋅ V + ξ θ ˙ Fn = AdTn,n+ ⋅ Fn+ + Mn Vn − adT n ⋅ (Mn Vn ) Dynamics g− ˙ V Introduction Lagrange’s Equations Define: ⎡ θ ⎤ ⎢ ˙ ⎥ ˙=⎢ ⎥ ∈ ℝn , ξ = Dynamics of V ξ V= ∈ℝ ,θ ⎢ ⎥ ⋱ ∈ℝ Open-chain n× n×n ⎢ θn ⎥ Manipulators Vn ⎣ ⎦ ˙ ξn ⎡ Adg − ⎤ Coordinate ⎢ ⎥ Invariant F τ ⎢ ⎥ F= ∈ℝ ,τ = ∈ ℝ ,P = ⎢ ⎥∈ℝ Algorithms , ⎢ ⎥ n× n n× ⎢ ⎥ Lagrange’s Fn τn ⎣ ⎦ Equations with Constraints Pt = [ ⋯ Adgn,n+ ] ∈ ℝ − × n
  48. 48. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 37 ˙ V = Adg − ⋅ V + ξ θ , Chapter Manipulator ˙ V − Adg − V = ξ θ Dynamics , Introduction Vn − Adg − ˙ Vn− = ξ n θ n Lagrange’s n− ,n Equations ⎡ ⎤ ⎢ −Ad − ⎥⎡ V ⎤ I ⋯ ⎢ ⎥⎢ V ⎥ Dynamics of ⇒⎢ ⎥⎢ ⎥ I ⋱ ⎢ ⎥⎢ ⎥ Open-chain g, ⎢ ⋱ ⋱ ⎥⎢ ⎥ ⎢ I ⎥ ⎣ Vn ⎦ Manipulators ⎣ ⎦ −Adg − Coordinate n− ,n Invariant V Algorithms G− ⎡ Adg − ⎤ ⎡ ξ ⎤⎡ ⎤ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ˙ ⎥⎢ ⎥ θ ⎢ ⎥ ⎢ Lagrange’s =⎢ ⎥V +⎢ ⎥⎢ ⎥ ⎥⎢ ⎥ , ξ ˙ θ ⎢ ⎥ ⎢ Equations with ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⋱ ⎦⎢ ⎥ Constraints ⎣ ⎦ ⎣ ξn ⎣ ˙ θn ⎦ P ξ ˙ θ ˙ Thus V = GP V + Gξ θ
  49. 49. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 38 where ⎡ I ⋯ ⎤ Chapter ⎢ Ad − ⋯ ⎥ ⎢ ⎥ Manipulator ⎢ ⎥ Dynamics I G = ⎢ Adg −, ⎥∈ℝ g, Introduction ⎢ Adg −, I ⋱ ⎥ n× n ⎢ ⋱ ⎥ ⎢ I ⎥ ⎢ Adg − ⋯ Adgn− ,n I ⎥ Lagrange’s ⎣ Adg −,n ⎦ Equations − ,n Dynamics of Open-chain Manipulators Coordinate Invariant Algorithms V = ξ θ + Adg −, V − ad ξ θ (Adg −, V ) ˙ ¨ ˙ ˙ Lagrange’s Equations with V − Adg −, V = ξ θ − ad ξ ˙ ˙ ¨ ˙ θ (Adg −, V ) Vn − Adgn− ,n Vn− = ξ n θ n − ad ξ n θ n (Adgn− ,n Vn− ) Constraints ˙ − ˙ ¨ ˙ − (Continues next slide)
  50. 50. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 39 ⎡ I ⋯ ⎤ ⎡ V ⎤ ⎡Ad − ⎤ ⎡ξ ⎤ ⎡θ ⎤ ⎢ ⎥⎢ ˙ ⎥ ⎢ g , ⎥ ⎢ ⎥⎢¨ ⎥ ¨ ⎢−Adg −, I ⋱ ⎥ ⎢V ⎥ ⎢ ⎥˙ ⎢ ξ ⎥ ⎢θ ⎥ ⎢ ⎥⎢ ˙ ⎥ = ⎢ ⎥V +⎢ ⎢ ⎥+ ⎢ ⋱ ⋱ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⋱ ⎥⎢ ⎥ ⎥⎢ ⎥ Chapter ⎢ −Adgn− ,n I ⎥ ⎢Vn ⎥ ⎢ ⎥ ⎢ ⎢ ⎥⎣˙ ⎦ ⎣ ξ n ⎥ ⎢θ n ⎥ Manipulator ⎣ ⎦ ⎦ ⎣ ⎦⎣¨ ⎦ Dynamics − Introduction G− P ξ ⎡−ad ξ θ ⋯ ⎤ ⎡Ad − Lagrange’s ⎢ ⎥⎢ g , ⎤ ⎥ Equations ⎢ −ad ξ ⋱ ⎥⎢ ⎥ ˙ ⎢ ⎥⎢ ⎥V + ⎢ ⎥⎢ Dynamics of ⋱ ⋱ ⎥ ˙ θ ⎢ ⎥ −ad ξ n θ n ⎥ ⎢ ⎥ Open-chain ⎢ ⋯ ˙ ⎦⎣ ⎦ Manipulators Coordinate ⎣ Invariant Algorithms ad ξ θ ˙ ⎡−ad ξ θ ⋯ ⎤⎡ ⋯ ⎤ ⎡V ⎤ ⎢ ⎥⎢ ⎥⎢ ⎥ Lagrange’s ⎢ −ad ξ ⋱ ⎥ ⎢Adg −, ⋱ ⎥ ⎢V ⎥ ˙ ⎢ ⎥⎢ Equations with ⎢ ⎥⎢ ⋱ ⋱ ⎥⎢ ⎥ ⋱ ⋱ ⎥⎢ ⎥ Constraints ˙ θ ⎢ ⎥ ⎢ ⎣ ⋯ −ad ξ n θ n ⎥ ⎢ ˙ ⎦⎣ Adgn− ,n − ⎥ ⎣Vn ⎦ ⎦ Γ (Continues next slide)
  51. 51. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 40 Chapter Thus V = G ⋅ ξ θ + G ⋅ P V + G ⋅ ad ξ θ P V + G ⋅ ad ξ θ ΓV Manipulator Dynamics ˙ ¨ ˙ ˙ ˙ Introduction Lagrange’s Finally the backward recursion: Fn = AdT− Fn+ + Mn Vn − adT n ⋅ (Mn Vn ) Equations Dynamics of g ˙ V n,n+ Fn− = Fn + Mn− Vn− − adT n− ⋅ (Mn− Vn− ) Open-chain Manipulators AdT− gn− ˙ V ,n Coordinate Invariant Algorithms Lagrange’s F = AdT− F + M V − adT (M V ) g ˙ V , Equations with Constraints (Continues next slide)
  52. 52. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 41 ⎡ I−AdT− ⎤ ⎢ ⎥ ⎡ F ⎤ ⎡M ⎤ ⎡V ⎤ ⎡ ⎤ ⎢ ⎥ ⎢F ⎥ ⎢ M ⎥ ⎢V ⎥ ⎢ ⎥ ˙ I ⋱ ⎥⎢ ˙ ⎥ +⎢ ⎥ g, ⎢ ⇒⎢ ⎥⎢ ⎥ = ⎢ ⎢ ⎥⎢ ⎥ ⎢ ⋱ ⋱−Adg − ⎥ ⎢ ⎥ ⎢ ⋱ ⎥ ⎢ ⎥ ⎢ T ⎥ Fn+ + ⎢ ⎥ ⎢ ⎥ Mn ⎥ ⎢V ⎥ ⎢Adgn,n+ ⎥ Chapter T ⎢ n− ,n ⎥ ⎣F n ⎦ ⎣ ⎦ ⎣ ˙ n⎦ ⎣ Manipulator Dynamics ⎣ ⋯ I ⎦ − ⎦ Introduction M PT ⎡−adT ⎤ Lagrange’s t ⎢ ⎥ M ⎢ ⎥ Equations V ⎢ ⋱ ⋱ V ⎢ T ⎥ −adVn ⎥ Dynamics of ⎣ ⎦ Open-chain Mn Vn Manipulators Coordinate ad T Invariant V Algorithms Lagrange’s Equations with Constraints
  53. 53. Chapter 4 Manipulator Dynamics 4.4 Coordinate Invariant Algorithms 41 ⎡ I−AdT− ⎤ ⎢ ⎥ ⎡ F ⎤ ⎡M ⎤ ⎡V ⎤ ⎡ ⎤ ⎢ ⎥ ⎢F ⎥ ⎢ M ⎥ ⎢V ⎥ ⎢ ⎥ ˙ I ⋱ ⎥⎢ ˙ ⎥ +⎢ ⎥ g, ⎢ ⇒⎢ ⎥⎢ ⎥ = ⎢  

×