Documents
Resources
Learning Center
Upload
Plans & pricing Sign in
Sign Out

Lecture Notes for A Mathematical Introduction to Robotic Manipulation

VIEWS: 26 PAGES: 71

									                                                                        Chapter 4 Robot Dynamics and Control

                 Summer School-Math. Methods in Robotics@TU-BS.DE                    -    July




Chapter
                                      Lecture Notes
Robot
Dynamics and
Control
                                            for
Lagrangian                     A Mathematical Introduction to
Equations

Inertial                           Robotic Manipulation
Properties of
Rigid Body

Dynamics of
an Open-chain                                          By
                                             Z.X. Li∗ and Y.Q. Wu
Manipulator

Newton-Euler
Equations
                           ∗
Coordinate-                    Dept. of ECE, Hong Kong University of Science & Technology
invariant
algorithms for
                                        School of ME, Shanghai Jiaotong University
robot
dynamics

Lagrange’s
                                                       July
Equations with
Constraints
                                                             Chapter 4 Robot Dynamics and Control

                 Summer School-Math. Methods in Robotics@TU-BS.DE         -    July




Chapter
Robot
Dynamics and
                  Chapter Robot Dynamics and Control
Control
                     Lagrangian Equations
Lagrangian
Equations

Inertial             Inertial Properties of Rigid Body
Properties of
Rigid Body

Dynamics of
                     Dynamics of an Open-chain Manipulator
an Open-chain
Manipulator

Newton-Euler
                     Newton-Euler Equations
Equations

Coordinate-          Coordinate-invariant algorithms for robot dynamics
invariant
algorithms for
robot
dynamics             Lagrange’s Equations with Constraints
Lagrange’s
Equations with
Constraints
                                                              Chapter 4 Robot Dynamics and Control

                 4.1 Lagrangian Equations
                                                                      y
                  ◻ A Simple Example:
                                                                                Fy     F
Chapter
                                                                            m          Fx
Robot
Dynamics and
Control
                                                                                mg
Lagrangian         Review:                                                                  x
Equations          Newton’s Equation:                     Lagrangian Equation:
Inertial
                                                                d ∂L ∂L
Properties of
                                                                      −    = Fx
Rigid Body                  x
                           m¨ = Fx                                 x
                                                               dt ∂˙ ∂x
Dynamics of
                            y
                           m¨ = Fy − mg
                                                      ⇔         d ∂L ∂L
an Open-chain
Manipulator                                                           −    = Fy
                                                                   y
                                                               dt ∂˙ ∂y
Newton-Euler                       x
                   Momentum: Px = m˙
Equations                                                 Lagrangian function:
                                        Py = m˙y
Coordinate-
                   d                d
                                                          L = T − V, Px = ∂L , Py =
                                                                           x
                                                                          ∂˙
                                                                                            ∂L
                                                                                             y
                                                                                            ∂˙
                   dt Px   =   Fx , dt Py = Fy − mg
invariant
algorithms for                                            Kinetic energy:
robot
dynamics                                                                x ˙
                                                              T = m(˙ + y )
Lagrange’s
Equations with
                                                          Potential energy:
Constraints                                                        V = mgy
                                                                Chapter 4 Robot Dynamics and Control

                 4.1 Lagrangian Equations

                  ◻ Generalization to multibody systems:
Chapter                                qi , i = , . . . , n: generalized coordinates
Robot
Dynamics and
                  y                    Kinetic energy:
Control
                           m
                                   q                            T = T(q, q)
                                                                          ˙
Lagrangian
Equations                              Potential energy:
Inertial                                                         V = V(q)
Properties of              m
Rigid Body                             Lagrangian:
Dynamics of                    q                       L(q, q) = T(q, q) − V(q)
                                                              ˙         ˙
an Open-chain
Manipulator
                       m
                                       τi , i = , . . . , n: external force on qi
Newton-Euler                           Lagrangian Equation:
Equations
                       q
Coordinate-                            x
invariant                                        d ∂L ∂L
algorithms for                                          −    = τi , i = , . . . , n
robot                                                q
                                                 dt ∂˙ i ∂qi
dynamics

Lagrange’s
Equations with
Constraints
                                                              Chapter 4 Robot Dynamics and Control

                  4.1 Lagrangian Equations

                       Example: Pendulum equation
                 Generalized coordinate:                  y
Chapter
Robot                            θ∈S
Dynamics and
Control          Kinematics:                                                 x
Lagrangian             x = l sin θ, y = −l cos θ
                  ⇒ x = l cos θ ⋅ θ, y = l sin θ ⋅ θ
Equations
                      ˙             ˙ ˙            ˙
Inertial
Properties of    Kinetic energy:                                θ
                 T(θ, θ) = m(˙ + y ) = ml θ
Rigid Body

Dynamics of
                      ˙           x ˙                ˙
an Open-chain
Manipulator      Potential energy:
Newton-Euler
Equations
                        V = mgl( − cos θ)                       mg
Coordinate-
                 Lagrangian function:
                   L = T − V = ml θ − mgl( − cos θ), ⇒           ˙ ∂L = −mgl sin θ
invariant
                                    ˙                  ∂L
algorithms for                                            = ml θ,
robot                                                   ˙
                                                       ∂θ          ∂θ
dynamics
                 Equation of motion:
                                      −   = τ ⇒ ml θ + mgl sin θ = τ
Lagrange’s
Equations with                   d ∂L ∂L             ¨
Constraints
                                dt ˙ ∂θ
                                                               Chapter 4 Robot Dynamics and Control

                  4.1 Lagrangian Equations

                      Example: Dynamics of a Spherical Pendulum
Chapter


                                             ⎤
Robot

                                             ⎥
Dynamics and
                           ⎡ l sin θ cos ϕ
                                             ⎥
Control
                           ⎢
                                             ⎥
                 r(θ, ϕ) = ⎢ l sin θ sin ϕ
                           ⎢ −l cos θ
                           ⎣                 ⎦
Lagrangian
Equations


                                    = ml (θ + ( − cos θ)ϕ )
Inertial
Properties of         T= m ˙
                           r              ˙             ˙
Rigid Body
                                                                                      θ
Dynamics of
an Open-chain
                      V = −mgl cos θ

                 L(q, q) = ml (θ + ( − cosθ )ϕ ) + mgl cos θ
Manipulator

Newton-Euler          ˙        ˙             ˙
Equations

Coordinate-
invariant                                                                            ϕ        mg
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                               Chapter 4 Robot Dynamics and Control

                 4.1 Lagrangian Equations



                 ⎧ d ∂L d
                 ⎪
                 ⎪
Chapter

                 ⎪ dt ˙ = dt (ml θ) = ml θ, ∂θ = ml sin θ cos θ ϕ − mgl sin θ
                 ⎪
Robot
                                            ¨ ∂L
                 ⎪ ∂θ
                                   ˙                            ˙
                 ⎪
Dynamics and

                 ⎨
Control

                 ⎪ d ∂L d
                 ⎪
                 ⎪
                 ⎪ dt ∂ ϕ = dt (ml sin θ ϕ) = ml sin θ ϕ + ml sin θ cos θ θ ϕ, ∂ϕ =
                 ⎪                                                        ˙ ˙ ∂L
Lagrangian
                                         ˙             ¨
                 ⎪
Equations

Inertial         ⎩      ˙
Properties of

                                               −ml sθ cθ ϕ
Rigid Body

                                           +                    +
                       ml              ¨
                                       θ                  ˙           mglsθ
Dynamics of
                                       ¨                 ˙ϕ                       =
an Open-chain
Manipulator
                             ml sθ     ϕ        ml sθ cθ θ ˙
Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                           Chapter 4 Robot Dynamics and Control

                 4.2 Intertial Properties of Rigid Body

                  ◻ Kinetic energy of a rigid body:
Chapter
Robot
Dynamics and                                        r
Control                                                     B
Lagrangian
Equations
                                         ra
Inertial
Properties of
Rigid Body                   A                gab
Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations
                        Volume occupied by the body:       V
Coordinate-
                                       Mass density:       ρ(r)
invariant
algorithms for                                 Mass:       m=     ∫
                                                                  V
                                                                      ρ(r)dV

                                                           r≜
robot
dynamics

Lagrange’s
                                            Mass center:
                                                              m       ∫
                                                                      V
                                                                          ρ(r)rdV
Equations with     Relative to frame at the mass center:   r=
Constraints
                                                                        Chapter 4 Robot Dynamics and Control

                 4.2 Intertial Properties of Rigid Body

                 In A-frame
Chapter


                               ρ(r) p + Rr dV =                            + pT Rr + Rr )dV
Robot
Dynamics and
Control          T=        ∫
                           V
                                    ˙ ˙                ∫  V
                                                              ρ(r)( p
                                                                    ˙        ˙ ˙     ˙

                                 + pT R        ρ(r)rdV +
Lagrangian
Equations

Inertial
                      = m p
                          ˙        ˙ ˙     ∫
                                           V
                                                              ∫ ρ(r) Rr
                                                                     ˙         dV

                                               =
Properties of
Rigid Body

Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations             ∫
                      V
                               ˙
                          ρ(r) Rr dV

                          ∫ ρ(r) R Rr dV = ∫ ρ(r) ωr dV = ∫ ρ(r) ˆω dV
Coordinate-                        T
invariant         =                ˙              ˆ              r
algorithms for


                          ∫ ρ(r)(−ω ˆ ω)dV = ω ∫ (−ρ(r)ˆ) dV ω ≜ ω I ω
robot
dynamics                               T              T                                         T
                  =                  r                 r
Lagrange’s
Equations with
Constraints
                                                                Chapter 4 Robot Dynamics and Control

                 4.2 Intertial Properties of Rigid Body


                                                                            ⎤
                 where
                                                                            ⎥
                                                       ⎡ Ixx Ixy Ixz
                                                                            ⎥
                                                       ⎢
Chapter
                                 I =−   ∫   ρ(r)ˆ dV ≜ ⎢ Ixy Iyy Iyz
                                                r                           ⎥
                                                                            ⎥
Robot                                                  ⎢
                                                       ⎣                    ⎦
Dynamics and                                           ⎢ Ixz Iyz Izz
Control
                 with
                                ∫ ρ(r)(y     + z )dxdydz, Ixy = −   ∫ ρ(r)xydxdydz
Lagrangian
Equations               Ixx =
Inertial
Properties of
Rigid Body

Dynamics of
an Open-chain
                        T= m p
                             ˙       + (ωb )T I b ωb = m RT p
                                                            ˙        + (ωb )T I b ωb
Manipulator

                         = (V b )T
                                        mI
Newton-Euler                                          Vb
Equations                                        Ib
Coordinate-
invariant                                   Mb
algorithms for

                 For V b = g − ⋅ g , with ωb = RT ⋅ R and vb = RT p, M b is the
robot
dynamics             ˆ           ˙        ˆ         ˙             ˙
Lagrange’s
Equations with
                 Generalized inertia matrix in B frame.
Constraints
                                                                                          Chapter 4 Robot Dynamics and Control

                 4.2 Intertial Properties of Rigid Body

                      Example: M b for a rectangular object
                      m                                                                                  z
Chapter
Robot            ρ=
                     lωh
                         ρ(y + z )dxdydz
Dynamics and
Control

Lagrangian
                 Ixx =  ∫   V                                                                                        y
Equations                   h           ω           l
                                                                                                  x
Inertial         =ρ   ∫ ∫ ∫                             (y + z )dxdydz                h                               l
Properties of
Rigid Body
                        −   h
                                    −   ω
                                                −   l



                                (lω h + lωh ) =                        (ω + h )
Dynamics of                                                        m                             w
an Open-chain    =ρ
Manipulator
                                                                   h      ω    l

                 Ixy = −        ∫   ρxydv = −ρ                   ∫ ∫ ∫
Newton-Euler
                                                                                   xydxdydz
                                                                  −h   −ω     −l
Equations
                                V
Coordinate-
invariant                                                   l
                                h           ω

                 = −ρ
algorithms for                                  y
robot
dynamics                ∫ ∫ −h          −ω
                                                        x
                                                            −l
                                                                 dydz =
Lagrange’s
Equations with
Constraints
                                                                      Chapter 4 Robot Dynamics and Control

                 4.2 Intertial Properties of Rigid Body

                            (w + h )                              ⎤
                                                                  ⎥
                    ⎡   m
                                                                  ⎥
                    ⎢
                                           (l + h )               ⎥,M =                  ×
                    ⎢                                                               mI
                                                                  ⎥
                                       m
                 I =⎢
                                                         (w + l ) ⎥
                    ⎢                                                                          I
                    ⎣                                             ⎦
Chapter             ⎢                                  m
Robot
Dynamics and
                                                                       g (t)
Control
                                                              g
Lagrangian
Equations
                  V = g− ⋅ g , M
                  ˆ        ˙                            A         B
Inertial
Properties of     T = VTM V
Rigid Body                                            g (t)             g
Dynamics of       V = Adg V
an Open-chain
Manipulator


                  T = (Adg V )T M (Adg V ) = V T AdT M Adg V ≜ V T M V
Newton-Euler
Equations
                                                   g
Coordinate-
invariant
algorithms for     ◻ M under change of frames:
robot
dynamics

Lagrange’s                                     M = AdT M Adg
                                                     g
Equations with
Constraints
                                                                     Chapter 4 Robot Dynamics and Control

                 4.2 Intertial Properties of Rigid Body


                     Example: Dynamics of a -dof planar robot
                                             ⎤
Chapter

                                             ⎥
Robot                 ⎡ Ixxi
                                             ⎥,i = ,
Dynamics and          ⎢
                                             ⎥
                 Ii = ⎢        Iyyi
                      ⎣                      ⎦
Control
                      ⎢               Izzi
Lagrangian
Equations

Inertial


                                       + ωT I ω
Properties of                                                                l2
Rigid Body            ˙
                 T(θ, θ) = m v
Dynamics of                                                                   r 2 θ2

                         + m v          + ωT I ω
an Open-chain
Manipulator
                                                                l1
Newton-Euler                                           y
Equations                                                  r1
                                                                     θ1
Coordinate-                                                 x
invariant
algorithms for
                 ω =             ω =
                                             θ +θ
robot
dynamics                 ˙
                         θ                   ˙ ˙
Lagrange’s
Equations with
Constraints
                                                               Chapter 4 Robot Dynamics and Control

                  4.2 Intertial Properties of Rigid Body

                        xi
                 Pi =   yi : Mass center γi : Distance from joint i to mass center

                           x = −r s θ
Chapter
Robot            x =rc     ˙         ˙
Dynamics and              ⇒
Control
                 y =rs     ˙
                           y =rcθ  ˙
Lagrangian
Equations        x =l c +r c     ˙ = −(l s + r s )θ − r s θ
                                 x                 ˙       ˙
                 y =l s +r s
                              ⇒
                                 y = (l c + r c )θ + r c θ
                                 ˙               ˙       ˙
Inertial
Properties of


                 T(θ, θ) = m (x + y ) + Iz θ + m (x + y ) + Iz (θ + θ )
Rigid Body

Dynamics of
                      ˙       ˙ ˙          ˙      ˙ ˙           ˙   ˙
an Open-chain

                                       α + βc    δ + βc
                         = [θ θ ]
Manipulator
                                                             ˙
                                                             θ
                                       δ + βc
                            ˙ ˙
Newton-Euler
Equations
                                                    δ        ˙
                                                             θ
Coordinate-
                                             M(θ)
                 α = Iz + Iz + m r + m (l + r ), β = m l r , δ = Iz + m r , L = T
invariant
algorithms for


                                            −βs θ −βx
robot

                          ⇒ M(θ) θ +
dynamics                            ¨             ˙             θ˙       τ
Lagrange’s                          ¨            ˙               ˙   = τ
Equations with                      θ        βs θ               θ
Constraints
                                                                            Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator

                  ◻ Dynamics of open-chain manipulator:
                  De nition:
                  Li : frame at mass center of link i, gsli (θ) = expξ θ ⋯ expξi θ i gsli (o)
Chapter                                                              ˆ        ˆ
Robot
Dynamics and
Control                                   θ1
Lagrangian                                              θ2                  θ3
Equations                                                         l1              l2
                                                             L2                  L3
Inertial
Properties of
Rigid Body                                    r1                       r2
                                 l0
Dynamics of                               L1
an Open-chain
Manipulator                                        r0
                                          S
Newton-Euler


                                                                          ⎡                 ⎤
Equations

                                                                          ⎢                 ⎥
                                                                                       ˙
                                                                                       θ
                                                                          ⎢                 ⎥
Coordinate-

                                                                          ⎢                 ⎥
invariant

                                                                          ⎢                 ⎥
algorithms for

                         Vsli = Jsli (θ)θ = [ξ † ξ † ⋯ ξi†             ⋯ ]⎢                 ⎥ = Ji (θ)θ
robot                                                                               ˙
                                                                                    θi
                                                                          ⎢                 ⎥
                          b      b      ˙                                                             ˙
                                                                          ⎢                 ⎥
dynamics
                                                                                   ˙i+
                                                                                   θ
                                                                          ⎢                 ⎥
                                                                          ⎢                 ⎥
Lagrange’s

                                                                          ⎣                 ⎦
Equations with
Constraints                                                                            ˙
                                                                                       θn
                                                                         Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator

                                  ξj† = Ad− (eξj+ θ j+ ⋯eξi θ i gsli ( ))ξj , j ≤ i
                                                 ˆ            ˆ



                 Ti (θ, θ) = (Vsli )T Mib Vsli = θ T JiT (θ)Mib Ji (θ)θ
                               b           b
Chapter
Robot
                        ˙                        ˙                    ˙
Dynamics and
                            n
                                Ti (θ, θ) = θ T M(θ)θ,
Control

Lagrangian        T(θ) =               ˙    ˙       ˙
Equations                  i=
                                                        n
                                JiT (θ)Mib Ji (θ) =           Mij (θ)θ i θ j
Inertial
Properties of
                  M(θ) =                                             ˙ ˙
Rigid Body
                            i                          i,j=
                 hi (θ): Height of Li ,    Vi (θ) = mi ghi (θ),                          mi ghi (θ)
Dynamics of
an Open-chain                                                           V(θ) =
Manipulator
                                                                                    i=
Newton-Euler
Equations
                 Lagrange’s Equation:

                                    −
Coordinate-                 d ∂L      ∂L
invariant                                 = Γi , i = , . . . , n,
algorithms for
                                 ˙
                            dt ∂ θ i ∂θ i
                                               d ⎛n     ˙⎞
robot
                                                                          n
                                                                               Mij θ j + Mij θ j
dynamics
                                      d ∂L                                         ¨     ˙ ˙
                                                     M θ =
                                           ˙i dt ⎝ j= ij j ⎠
Lagrange’s                                   =
Equations with                        dt ∂ θ                              j=
Constraints
                                                                                Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator

                                   n   ∂Mkj ˙ ˙ ∂V                                  ∂Mij ˙
                                             θkθj −
                     ∂L                                            ˙
                          =                                        Mij =                 θk
                     ∂θ i      j,k=     ∂θ i        ∂θ i                        k   ∂θ k
Chapter
Robot                         n                 n       ∂Mij ˙ ˙          ∂Mkj ˙ ˙
                                   Mij θ j +                 θj θk −            θkθj +
Dynamics and
                                       ¨                                               ∂V
Control                   ⇒                                                                 = Γi
                              j=               j,k=     ∂θ k               ∂θ i        ∂θ i
Lagrangian
Equations                     n                  n
                                   Mij θ j +          Γij θ k θ j +
                                       ¨               k˙ ˙         ∂V
Inertial                  ⇒                                              = Γi
Properties of
Rigid Body                    j=               j,k=                 ∂θ i
                                   ∂Mij ∂Mik ∂Mkj
                                        +      −
Dynamics of
                       k
an Open-chain         Γij =
Manipulator
                                   ∂θ k   ∂θ j   ∂θ i
Newton-Euler

                 θ i ⋅ θ j , i ≠ j : Coriolis force
Equations
                 ˙ ˙                                             ˙
                                                                 θ i : Centrifugal force
Coordinate-
                                           n                   n     ∂Mij ∂Mik ∂Mkj ˙
                 De ne: cij (θ, θ) =                                       +       −
invariant
algorithms for                  ˙                k˙
robot                                           Γij θ k =                                  θk
dynamics                                  k=                  k=      ∂θ k    ∂θ j    ∂θ i
Lagrange’s
Equations with             ⇒ M(θ)θ + C(θ, θ)θ + N(θ) = τ
                                 ¨        ˙ ˙
Constraints
                                                                 Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator


                  Property :
                      M(θ) = M T (θ), θ T M(θ)θ ≥ , θ T M(θ)θ = ⇔ θ =
Chapter
Robot                                 ˙       ˙     ˙       ˙
                      M − C ∈ Rn×n is skew symmetric
Dynamics and
Control               ˙
Lagrangian
Equations
                  Proof :
Inertial

                       (M − C)ij = Mij − cij (θ)
Properties of
Rigid Body              ˙           ˙
Dynamics of                         n ∂M         ∂Mij ˙               ∂Mkj ˙
                                            θk −      θk −       θk +
an Open-chain                            ij ˙              ∂Mik ˙
Manipulator                      =                                          θk
Newton-Euler                       k= ∂θ k       ∂θ k       ∂θ j       ∂θ i
                                      n    ∂Mkj ˙
                                                 θk −
Equations
                                                      ∂Mik ˙
Coordinate-                       =                         θk
invariant
                                      k=    ∂θ i       ∂θ j
algorithms for

                  Switching i and j shows (M − C)T = −(M − C)
robot
dynamics                                   ˙           ˙
Lagrange’s
Equations with
Constraints
                                                                    Chapter 4 Robot Dynamics and Control

                  4.3 Dynamics of Open-chain Manipulator


                      Example: Planar -DoF Robot (continued)
Chapter           m (θ) = α + β cos θ , m = δ
                  m (θ) = m (θ) = δ + β cos θ
Robot
Dynamics and
Control

Lagrangian
Equations
                 c (θ, θ) = −β sin θ ⋅ θ , c (θ, θ) = −β sin θ (θ + θ )
                       ˙                ˙         ˙             ˙ ˙
Inertial         c (θ, θ) = β sin θ ⋅ θ , c (θ, θ) =
                       ˙              ˙         ˙
Properties of
                 ⎧
                 ⎪Γ
                 ⎪    = (       +       −       )=
                          ∂M      ∂M      ∂M        ∂M
                 ⎪
                 ⎪
Rigid Body
                 ⎪
                                                          =
                 ⎪
                 ⎪
                 ⎪
                           ∂θ      ∂θ      ∂θ        ∂θ
                 ⎪
Dynamics of
                 ⎪
                 ⎪
                 ⎪Γ
an Open-chain
                 ⎪    = (       +       −       )=        = −β sin θ
                          ∂M      ∂M      ∂M        ∂M
                 ⎪
                 ⎪
Manipulator

                 ⎪
                 ⎨
Newton-Euler              ∂θ       ∂θ      ∂θ        ∂θ
                 ⎪
                 ⎪Γ
                 ⎪
Equations

                 ⎪    = (       +       −       )=        = −β sin θ
                          ∂M      ∂M      ∂M        ∂M
                 ⎪
                 ⎪
                 ⎪
                 ⎪
Coordinate-

                 ⎪
                           ∂θ      ∂θ      ∂θ        ∂θ
                 ⎪
                 ⎪
invariant

                 ⎪
                 ⎪Γ
                 ⎪
algorithms for

                      = (       +       −       )=        −            = −β sin θ
                          ∂M      ∂M      ∂M       ∂M         ∂M
                 ⎪
                 ⎪
robot

                 ⎩
dynamics
                           ∂θ      ∂θ      ∂θ       ∂θ         ∂θ
Lagrange’s
Equations with
Constraints
                                                               Chapter 4 Robot Dynamics and Control

                  4.3 Dynamics of Open-chain Manipulator

                 ⎧
                 ⎪Γ
                 ⎪    = (      +        −        )=        −
                         ∂M       ∂M       ∂M        ∂M        ∂M
                 ⎪
                 ⎪
                 ⎪
                                                                   = β sin θ
                 ⎪
                 ⎪
                 ⎪
                          ∂θ       ∂θ      ∂θ         ∂θ       ∂θ
                 ⎪
                 ⎪
                 ⎪
                 ⎪Γ
Chapter

                 ⎪  = (        +        −        )=
                         ∂M       ∂M       ∂M          ∂M
                 ⎪
Robot
Dynamics and
                 ⎪
                 ⎪
                                                             =
                 ⎨
Control                   ∂θ       ∂θ       ∂θ          ∂θ
                 ⎪
                 ⎪Γ
                 ⎪
                 ⎪  = (        +        −        )=
                         ∂M       ∂M       ∂M          ∂M
                 ⎪
Lagrangian

                 ⎪
                                                             =
                 ⎪
                 ⎪
Equations

                 ⎪
                          ∂θ       ∂θ       ∂θ          ∂θ
                 ⎪
                 ⎪
                 ⎪
                 ⎪Γ
Inertial

                 ⎪  = (        +         −        )=
                         ∂M       ∂M       ∂M          ∂M
                 ⎪
                 ⎪
Properties of

                 ⎩
Rigid Body                                                   =
                          ∂θ       ∂θ       ∂θ           ∂θ
                           − β sin θ ⋅ θ −β sin θ ⋅ θ
Dynamics of
                 M− C=
                                        ˙                ˙
an Open-chain    ˙
Manipulator
                            −β sin θ ⋅ θ
                                       ˙

                   − β sin θ ⋅ θ − β sin θ (θ + θ )
Newton-Euler

                 −
Equations                       ˙                 ˙ ˙
Coordinate-          β sin θ ⋅ θ
                               ˙
invariant

                                           β sin θ ( θ + θ )
                                                                ⇐ skew-symmetric
algorithms for                                        ˙ ˙
                   −β sin θ ( θ ˙ +θ )
robot
dynamics
                 =                  ˙
Lagrange’s
Equations with
Constraints
                                                                                            Chapter 4 Robot Dynamics and Control

                  4.3 Dynamics of Open-chain Manipulator

                       Example: Dynamics of a -dof robot
Chapter                                                             θ1
Robot
Dynamics and                                                                      θ2                    θ3

              ⎡         ⎤      ⎡      ⎤     ⎡       ⎤
                                                                                            l1                l2
              ⎢         ⎥      ⎢ −l   ⎥     ⎢ −l    ⎥
Control
              ⎢         ⎥      ⎢      ⎥     ⎢       ⎥
                                                                                       L2                    L3

              ⎢         ⎥      ⎢      ⎥     ⎢ l     ⎥
Equations ξ = ⎢         ⎥, ξ = ⎢ −    ⎥,ξ = ⎢ −     ⎥
Lagrangian
              ⎢         ⎥      ⎢      ⎥     ⎢       ⎥
                                                                        r1                         r2
              ⎢         ⎥      ⎢      ⎥     ⎢       ⎥
              ⎢         ⎥      ⎢      ⎥     ⎢       ⎥
                                                           l0
              ⎣         ⎦      ⎣      ⎦     ⎣       ⎦
Inertial                                                            L1
Properties of                                                                r0
Rigid Body                                                          S

Dynamics of
an Open-chain
                           ⎡            ⎤            ⎡                   ⎤            ⎡                               ⎤
                           ⎢            ⎥            ⎢                   ⎥            ⎢                      l +r     ⎥
                 gsl ( ) = ⎢ I          ⎥, gsl ( ) = ⎢ I                 ⎥, gsl ( ) = ⎢ I                             ⎥
Manipulator                                                     r
                           ⎢            ⎥            ⎢                   ⎥            ⎢                               ⎥
                           ⎢      r     ⎥            ⎢          l        ⎥            ⎢                        l      ⎥
                           ⎣            ⎦            ⎣                   ⎦            ⎣                               ⎦
Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                Chapter 4 Robot Dynamics and Control

                  4.3 Dynamics of Open-chain Manipulator

                       ⎡ mi                              ⎤
                       ⎢                                 ⎥
                       ⎢                                 ⎥
                       ⎢                                 ⎥
                             mi
                 Mi = ⎢                                  ⎥
                                  mi
                       ⎢                                 ⎥
Chapter

                       ⎢                                 ⎥
Robot                                     Ixi
                       ⎢                                 ⎥
                       ⎢                            Iz i ⎥
Dynamics and
                                              Iy i
                       ⎣                                 ⎦
Control

Lagrangian
Equations         mi : e mass of the object
Inertial          Ixi : e moment of inertia about the x axis
Properties of

                     = (Iy − Iz − m r )c s + (Iy − Iz )c   s − m (l c    + r c )(l s + r s )
Rigid Body
                 Γ
                     = (Iy − Iz )c s − m r s (l c + r c    )
Dynamics of
an Open-chain    Γ
                     = (Iy − Iz − m r )c s + (Iy − Iz )c    s − m (l c   + r c )(l s + r s )
Manipulator
                 Γ
                     = (Iy − Iz )c s − m r s (l c + r c    )
Newton-Euler
                 Γ
                     = (Iz − Iy + m r )c s + (Iz − Iy )c    s + m (l c   + r c )(l s + r s )
Equations
                 Γ
                     = −l m r s , Γ = −l m r s , Γ         = −l m r s
Coordinate-
invariant        Γ
                     = (Iz − Iy )c s + m r s (l c + r c    ) , Γ =l
algorithms for
robot            Γ                                                       mr s
dynamics

Lagrange’s
Equations with
Constraints
                                                                   Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator


                                , V(θ) = m gh (θ) + m gh (θ) + m gh (θ)
                        ˙    ∂V
                   N(θ, θ) =
                             ∂θ
Chapter

                     gsli (θ) = eξ θ ⋯eξi θ i gsli ( ) ⇒
Robot
Dynamics and
                                 ˆ       ˆ
Control

Lagrangian            h (θ) = r , h (θ) = l − r sin θ, h (θ)
                                             = l − l sin θ − r sin(θ + θ )gsli (θ)
Equations

Inertial

                                             = eξ θ ⋯eξi θ i gsli ( ) ⇒
Properties of
Rigid Body
                                                ˆ       ˆ


                      h (θ) = r , h (θ) = l − r sin θ, h (θ)
Dynamics of
an Open-chain


                                             = l − l sin θ − r sin(θ + θ )
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                        Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator


                                     ⎡          ⎤
                                     ⎢          ⎥
                                     ⎢          ⎥
                                     ⎢          ⎥
                       J = Jsl (θ) = ⎢          ⎥
                                     ⎢          ⎥
Chapter                     b

                                     ⎢          ⎥
Robot

                                     ⎢          ⎥
Dynamics and

                                     ⎣          ⎦
Control


                                     ⎡ −r c         ⎤
Lagrangian

                                     ⎢              ⎥
                                     ⎢              ⎥
Equations

                                     ⎢        −r    ⎥
                       J = Jsl (θ) = ⎢              ⎥
Inertial

                                     ⎢        −     ⎥
Properties of               b

                                     ⎢ −s           ⎥
Rigid Body

                                     ⎢ c            ⎥
                                     ⎣              ⎦
Dynamics of
an Open-chain

                                     ⎡ −l c − r c                     ⎤
                                     ⎢                                ⎥
Manipulator

                                     ⎢                                ⎥
                                     ⎢                             −r ⎥
Newton-Euler
                                                      ls
                       J = Jsl (θ) = ⎢              −r − l c
                                                                   − ⎥
Equations

                                                       −
                            b
                                     ⎢                                ⎥
                                     ⎢     −s                         ⎥
Coordinate-

                                     ⎢                                ⎥
invariant

                                     ⎣                                ⎦
algorithms for
robot                                       c
dynamics

Lagrange’s
Equations with
Constraints
                                                                   Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator


                              M    M     M
Chapter             M(θ) =    M   M      M      = JT M J + JT M J + JT M J
Robot                         M   M      M
                 M = Iy s + Iy s + Iz + Iz c + Iz c + m r c + m (l c + r c )
Dynamics and
Control

Lagrangian
Equations        M =M =M =M =
                 M = Ix + Ix + m l + M r + m r + m l r c
Inertial
Properties of

                 M = Ix + m r + m l r c
Rigid Body

Dynamics of

                 M = Ix + m r + m l r c
an Open-chain
Manipulator


                 M = Ix + m r
Newton-Euler
Equations

Coordinate-
invariant                              n                n    ∂Mij ∂Mik ∂Mkj ˙
                        Cij (θ, θ) =
                                ˙            k˙
                                                                  +      −
algorithms for
robot                                       Γij θ k =                           θk
dynamics
                                       k=               k=   ∂θ k   ∂θ j   ∂θ i
Lagrange’s
Equations with
Constraints
                                                                               Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator


                  ◻ Additional Properties of the dynamics in
Chapter
Robot            terms of POE:
Dynamics and
Control           De ne:
                                     ⎧Ad−
                                     ⎪
Lagrangian

                                     ⎪ e ξˆj+ θ j+ ⋯e ξˆi θ i i > j
                                     ⎪
                                     ⎪
Equations

                                     ⎪
                               Aij = ⎨I
Inertial

                                     ⎪
                                     ⎪
                                                              i=j
Properties of

                                     ⎪
                                     ⎪
Rigid Body

Dynamics of                          ⎪
                                     ⎩                        i<j
                           Ji (θ) = Adg − ( ) [Ai ξ ⋯Aii ξi ⋯ ]
an Open-chain
Manipulator

Newton-Euler                               sl i

                             Mi′                                       ( intertia of ith link in S)
Equations

Coordinate-
                                   =   AdT−
                                         gsl (        Mi Adg −
                                             i    )         sl i ( )
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                       Chapter 4 Robot Dynamics and Control

                 4.3 Dynamics of Open-chain Manipulator


                  Property :
                                                n
                               Mij (θ) =                ξiT AT Ml′ Alj ξj
Chapter
Robot
Dynamics and
                                                             li
Control                                    l=max(i,j)
                                               n    ∂Mij ∂Mik ∂Mkj ˙
                           Cij (θ, θ) =                  +      −
Lagrangian
Equations                          ˙                                   θk
Inertial                                      k=    ∂θ k   ∂θ j   ∂θ i
Properties of
Rigid Body        where
                                        n
                                               [Ak− ,i ξi , ξk ]T AT Ml′ Alj ξj
                               ∂Mij
Dynamics of                         =                              lk
an Open-chain
Manipulator
                               ∂θ k l=max(i,j)
Newton-Euler
Equations
                               +ξiT AT Ml′ Alk [Ak− ,j ξj , ξk ]
                                     li
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                             Chapter 4 Robot Dynamics and Control

                 4.4 Newton-Euler Equations

                  ◻ Newton-Euler equations in body frame:
                  Newtons Equation:
Chapter
Robot

                                                  (mp) = mp
Dynamics and                                   d
Control                                  f =        ˙     ¨
Lagrangian
                                               dt
Equations
                  Spatial angular momentum:
Inertial

                                 I′ ⋅ ωs = R(I ⋅ ωb ) = R ⋅ I ⋅ RT ⋅ωs
Properties of
Rigid Body

Dynamics of
an Open-chain                                               I′
Manipulator

Newton-Euler
Equations                                 r
Coordinate-
                                                     T
invariant
algorithms for
robot
dynamics
                                                                 f
Lagrange’s           S
                                  g ∶ (R, p)
Equations with
Constraints
                                                             Chapter 4 Robot Dynamics and Control

                 4.4 Newton-Euler Equations


                          (I ω ) = (RI b RT ωs ) = I s ωs + RI b RT ωs + RI b RT ωs
                       d s s       d
Chapter
                   τ=                                    ˙ ˙                      ˙
                      dt           dt
                    = I s ωs + RRT I s ωs − RI b RT ωs ωs = I s ωs + ωs × (I s ωs )
Robot
Dynamics and
Control                   ˙    ˙                    ˆ           ˙
Lagrangian                       ωs
                                 ˆ
Equations

Inertial          ◻ Transformation of all equations to
Properties of
Rigid Body       twist/wrench in body frame:
                       (mp) =       mRvb = mRvb + mR˙b , RT f s = mRT Rvb + m˙b
                    d          d               ˙                        ˙
Dynamics of              ˙                               v                   v
an Open-chain
                    dt         dt
                       ⇒ f b = mωb × vb + m˙b ,
Manipulator

Newton-Euler                          ˙      v
Equations

                         τb = RT τs = RT (RI b ωb ) = I b ωb + ωb × I b ωb
                                          d
Coordinate-                                                ˙
invariant                                 dt
                                        vb + ωb × mvb = f b = F b (∗)
algorithms for
robot                     mI            ˙
                                                 ωb × I b ωb
dynamics             ⇒            b
                                I       ωb
                                        ˙                         τb
Lagrange’s
Equations with
Constraints
                             M          Vb
                                                             Chapter 4 Robot Dynamics and Control

                 4.4 Newton-Euler Equations



Chapter          De ne: se( ) × se( ) ↦ se( ), ( ξ , ξ ) ↦ ξ ξ − ξ ξ ≜ ξ, if
                                                  ˆ ˆ      ˆˆ ˆ ˆ ˆ
                                      ξ i = ω i vi , i = ,
                                      ˆ         ˆ
Robot
Dynamics and
Control
                 then
                            ξ = (ω × ω ) ω v − ω v = ad ξi ⋅ ξ
Lagrangian                                    ∧   ˆ      ˆ
Equations                   ˆ
Inertial
Properties of    where
Rigid Body
                                          ad ξ = ω ωˆ v  ˆ
                                                         ˆ
Dynamics of
an Open-chain      us
                                  (∗) ⇔ Mb Vb − adV b Mb Vb = F b
Manipulator
                                                ˙
Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                               Chapter 4 Robot Dynamics and Control

                 4.4 Newton-Euler Equations

                  ◻ Coordinate invariance of Newton-Euler
                 equations:
Chapter
Robot
Dynamics and                                                                 r   g
Control
                                                                                          B
                  M V − adT M V = F
                     ˙                                         g
Lagrangian                 V
Equations
                  V = Adg V
                  AdT F = F ⇒
Inertial
Properties of       g
                  F = (AdT )− F = (Ad− )T F
Rigid Body
                                                   A               g
Dynamics of
                         g           g

                  M = Ad−T M Ad−
an Open-chain
Manipulator
                         g     g
                      −T     −                               −T
Newton-Euler
Equations         ⇒ Adg M Adg Adg V − adT g
                                  ˙
                                        (Ad            V ) Adg M
                                                       ˙           Ad− Adg V = Ad−T F
                                                                     g           g
Coordinate-

                                     = Adg adV Ad− , we have
invariant
algorithms for     Since adAdg   V               g
robot
dynamics

Lagrange’s
Equations with
                                           M V − adV M V = F
                                             ˙
Constraints
                                                                     Chapter 4 Robot Dynamics and Control

                   4.5 Coordinate-invariant algorithms for robot dynamics

                                                                          Cn       Cn+
                                                          C
                                           C

Chapter
Robot
Dynamics and
Control

Lagrangian                                        C
Equations
                    Ci :  Frame xed to link i, located along the ith axis
Inertial
Properties of       Fi :  Generalized force link i − exerting on link i, expressed in Ci
Rigid Body          τi :  Joint torque of link i
Dynamics of      gi− ,i : Transformation of Ci relative to Ci−
                                       gi− ,i (θ i ) = eξi θ i ⋅ gi− ,i ( ) = gi− ,i ( )eξi θ i
an Open-chain                                           ˆ′                               ˆ
Manipulator

Newton-Euler     ξi = Adg − ( ) ⋅ ξi′ : ith axis in Ci frame.
                               ⎧⎡      ⎤
                         i− ,i

                               ⎪⎢      ⎥
Equations

                               ⎪⎢
                               ⎪       ⎥∶
                               ⎪⎢
                               ⎪
                               ⎪⎢ zi ⎥
Coordinate-

                               ⎪       ⎥
                                                Revolute joint.
                               ⎪⎣
invariant


                        ξ = ⎨⎡         ⎦
algorithms for


                               ⎪⎢ zi ⎤
robot

                               ⎪⎢
                               ⎪       ⎥
                               ⎪⎢      ⎥∶
dynamics

                               ⎪
                               ⎪⎢      ⎥
                               ⎪
                               ⎪       ⎥
Lagrange’s                                      Prismatic joint.
                               ⎩⎣      ⎦
Equations with
Constraints
                                                                         Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics




                                                 −
Chapter
Robot
                                              ⇒ gi− ,i ⋅ gi− ,i = ξi ⋅ θ i
                                                         ˙        ˆ ˙
Dynamics and
Control
                  Mi : Moment of inertia in Ci
Lagrangian                 mI       −mi ˆ
                                        r     mi :             Mass of link i
Equations         Mi = m iˆ I − m iˆ
                               ri              Ii :
                             i     i     i ri                  Angular moment of inertia
Inertial
Properties of      gi = gi− gi− ,i
                                   − ˆ
                  Vi = gi− ⋅ gi = gi− ,i Vi− gi− ,i + ξi θ i
Rigid Body

Dynamics of
                  ˆ          ˙                        ˆ˙
                  Vi = Ad − Vi− + ξi θ i
an Open-chain
Manipulator                                ˙
                            gi−   ,i
Newton-Euler
Equations         ˙ ˙− ˆ
                  ˆ                           − ˆ
                  Vi = gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i + ξi θ i
                                                        ˙         − ˙  ˆ               ˆ¨
                          −            − ˆ                 − ˆ               −
                     = −gi− ,i gi− ,i gi− ,i Vi− gi− ,i + gi− ,i Vi− gi− ,i gi− ,i gi− ,i
Coordinate-
invariant
algorithms for
                               ˙                                                   ˙
                        − ˙
                     + gi− ,i Vi− gi− ,i + ξi θ i
robot
dynamics                      ˆ            ˆ¨
Lagrange’s
Equations with
Constraints
                                                                              Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics


                  = − ξi θ i (Adg − Vi− )∧ + (Adg − Vi− )∧ ξi θ i + (Adg − Vi− )∧ + ξi θ i
                      ˆ˙                                   ˆ˙                 ˙     ˆ¨
                  ⇒ Vi = ξi θ i + Ad − Vi− − ad ˙ (Ad − Vi− )
                                 i− ,i           i− ,i                  i− ,i
                     ˙         ¨          ˙
Chapter                              gi−   ,i             ξi θ i   gi−   ,i
Robot
Dynamics and       ◻ Forward Recursion:
                   i = ∶ V = ,V =
Control
                              ˙                   g
Lagrangian

                              ⎧
Equations

                              ⎪ gi− ,i = gi− ,i ( )eξi θ i
                              ⎪
                                                    ˆ
                              ⎪
                              ⎪
                              ⎪
Inertial

                              ⎪
Properties of

                              ⎨ Vi = Adgi− ,i Vi− + ξi θ i
Rigid Body
                                                             ˙
                              ⎪
                              ⎪
                                              −

                              ⎪ ˙
                              ⎪ V = ξ θ + Ad − V − ad (Ad − V )
Dynamics of

                              ⎪
                              ⎪
an Open-chain

                              ⎩
Manipulator                          i
                                            ¨
                                          i i              ˙
                                                    gi− ,i i−     ˙
                                                               ξi θ i gi− ,i i−
Newton-Euler
Equations

Coordinate-        ◻ Backward Recursion:
invariant
                   Fn+ : End-e ector wrench, gn,n+ : transform from tool frame to Cn
                                 Fi = AdT− ⋅ Fi+ + Mi Vi − adT i ⋅ Mi Vi
algorithms for
robot                                                   ˙     V
dynamics                                g          i,i+
Lagrange’s
Equations with                     τi =     ξiT   ⋅ Fi
Constraints
                                                                 Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics




Chapter
Robot             V = Adg −, ⋅ V + ξ θ
                                     ˙

                  Fn = AdT− ⋅ Fn+ + Mn Vn − adT n ⋅ (Mn Vn )
Dynamics and
Control                                       ˙       V
                           gn,n+
Lagrangian

                                              ⎡ θ ⎤
                  De ne:
                                              ⎢ ˙ ⎥
Equations


                                 ∈ R n× , θ = ⎢     ⎥ ∈ Rn , ξ =
                         V                                         ξ
                                              ⎢     ⎥
Inertial
                  V=                      ˙                                           ∈R    n×n
                                              ⎢ θn ⎥
Properties of                                                          ⋱
                         Vn                   ⎣     ⎦
Rigid Body
                                                  ˙                           ξn
                                                                 ⎡ Adg −    ⎤
                                                                 ⎢          ⎥
Dynamics of
                         F                      τ                ⎢          ⎥
an Open-chain

                                                    ∈R ,P =⎢                ⎥∈R
                                                                        ,

                                                                 ⎢          ⎥
Manipulator                          n×                 n                            n×
                  F=           ∈R       ,τ=
                        Fn                      τn               ⎢          ⎥
                                                                 ⎣          ⎦
Newton-Euler
Equations

Coordinate-
invariant
                  Pt = [ ⋯ Adg − ] ∈ R
                                   T          × n
                                  n,n+
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                   Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics

                                       ˙
                    V = Adg −, ⋅ V + ξ θ
                                      ˙
                      V − Adg − V = ξ θ
Chapter                         ,
Robot
Dynamics and
Control
                                      ˙
                  Vn − Adg − Vn− = ξn θ n
                     ⎡                                ⎤
Lagrangian
                          n− ,n

                     ⎢                                ⎥⎡ V ⎤
Equations
                           I           ⋯
                     ⎢ −Adg −, I                      ⎥⎢ V ⎥
                  ⇒⎢                                  ⎥⎢   ⎥
Inertial                               ⋱
                     ⎢                                ⎥⎢   ⎥
Properties of

                     ⎢          ⋱      ⋱              ⎥⎢ V ⎥
                     ⎢                              I ⎥⎣ n ⎦
Rigid Body

                     ⎣                                ⎦
Dynamics of                       −Adg −
                                          n− ,n
an Open-chain
Manipulator                                               V
                                    G−
                    ⎡ Adg −    ⎤    ⎡ ξ                  ⎤⎡       ⎤
                                                         ⎥⎢       ⎥
Newton-Euler

                    ⎢          ⎥    ⎢
                                                               ˙
                                                         ⎥⎢       ⎥
Equations
                                                               θ
                    ⎢          ⎥    ⎢
                   =⎢          ⎥V + ⎢                    ⎥⎢       ⎥
                           ,
                                                               ˙
                                                         ⎥⎢       ⎥
                                            ξ                  θ
                    ⎢          ⎥    ⎢
Coordinate-

                    ⎢          ⎥    ⎢                      ⎢      ⎥
                                                      ξn ⎥ ⎢
invariant
                                                    ⋱
                    ⎣          ⎦    ⎣                    ⎦⎣    ˙n ⎥
                                                               θ ⎦
algorithms for
robot
dynamics

Lagrange’s               P                      ξ              ˙
Equations with
                                                               θ
Constraints                          ˙
                    us V = GP V + Gξ θ
                                                                         Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics



                  where
                              ⎡ I                                             ⎤
                              ⎢ Ad −                                          ⎥
Chapter
                                                               ⋯
                              ⎢                                               ⎥
Robot

                              ⎢                                               ⎥
Dynamics and                                   I               ⋯
                          G = ⎢ Adg −,                                        ⎥∈R
Control                           g,
                              ⎢                                               ⎥
                                             Adg −         I   ⋱                         n× n
                              ⎢                                               ⎥
Lagrangian

                              ⎢                                               ⎥
                                                  ,
                                                               I
                              ⎢ Adg −                                       I ⎥
Equations
                                                           ⋱
Inertial
Properties of                 ⎣      ,n
                                             Adg −
                                                  ,n
                                                           ⋯ Adg −
                                                                 n− ,n        ⎦
Rigid Body

Dynamics of
an Open-chain                  V = ξ θ + Adg − V − ad ξ θ (Adg − V )
                               ˙     ¨         ˙        ˙
                                                       ,                       ,
Manipulator

Newton-Euler
                               ˙          ˙     ¨                        (Adg −, V )
Equations
                               V − Adg −, V = ξ θ − ad ξ             ˙
                                                                     θ
Coordinate-
invariant


                                            Vn− = ξn θ n − ad ξn θ n (Adg −            Vn− )
algorithms for
robot
dynamics
                           ˙
                           Vn − Adg −       ˙        ¨           ˙
                                    n− ,n                                      n− ,n
Lagrange’s
Equations with
Constraints
                                                                                               Chapter 4 Robot Dynamics and Control

                  4.5 Coordinate-invariant algorithms for robot dynamics



                 ⎡                    ⋯                  ⎤⎡            ⎤ ⎡ Adg −         ⎤    ⎡ ξ                                ⎤⎡           ⎤
                 ⎢ −Ad −                                 ⎥⎢                                                                      ⎥⎢           ⎥
                     I
                                       ⋱                               ⎥ ⎢               ⎥    ⎢
                                                                                                                                          ¨
                 ⎢                                       ⎥⎢                                                                      ⎥⎢           ⎥
                                                               ˙
                                                               V                                                                          θ
                 ⎢                                       ⎥⎢            ⎥ ⎢               ⎥˙ ⎢
                                                                                                                                 ⎥⎢           ⎥
                            I
                 ⎢                                       ⎥⎢            ⎥=⎢               ⎥ V +⎢
                                                                                                                                 ⎥⎢
                                                                                                                                          ¨
                                                                                                                                              ⎥
Chapter                g,                                      ˙                ,
                            ⋱          ⋱                                                                               ⋱
                                                               V                                              ξ                           θ
                 ⎢                                       ⎥             ⎥ ⎢               ⎥    ⎢
                                                       I ⎥⎢            ⎥ ⎢               ⎥    ⎢                                  ⎥⎢           ⎥
Robot
                 ⎢                  −Adg −                                                                                       ⎦⎢           ⎥
                 ⎣                                       ⎦⎣            ⎦ ⎣               ⎦    ⎣                                   ⎣           ⎦
Dynamics and                                                   ˙                                                            ξn        ¨
Control                                  n− ,n
                                                               Vn                                                                     θn
Lagrangian                                                                       P                                 ξ
                                G−
Equations

                                      ⎡ −ad ξ                               ⋯                 ⎤ ⎡ Ad −            ⎤
                                      ⎢                                                       ⎥⎢                  ⎥
Inertial
                                      ⎢                                                       ⎥⎢
                                                   ˙
                                                         −ad ξ              ⋱                                     ⎥
                                                   θ
                                     +⎢                                                       ⎥⎢
Properties of                                                                                       g ,
                                      ⎢                                                       ⎥⎢                  ⎥V
                                                           ⋱                ⋱                                     ⎥
                                                                   ˙
                                                                   θ
                                      ⎢                                                       ⎥⎢
Rigid Body
                                      ⎢                    ⋯                    −ad ξ n θ n   ⎥⎣                  ⎥
Dynamics of                           ⎣                                                 ˙     ⎦                   ⎦
an Open-chain
Manipulator                                                     ad ξ θ
                                                                     ˙
Newton-Euler
Equations

                      ⎡ −ad ξ                          ⋯                    ⎤⎡                          ⋯                  ⎤
                      ⎢                                                     ⎥ ⎢ Ad −                    ⋱                  ⎥⎡ V       ⎤
                      ⎢                                                     ⎥⎢                                             ⎥⎢ V       ⎥
Coordinate-                     ˙
                                       −ad ξ           ⋱
                                θ
                     +⎢                                                     ⎥⎢                                             ⎥⎢         ⎥
invariant
                      ⎢                                                     ⎥⎢                                             ⎥⎢         ⎥
                                                                                  g,
                                         ⋱             ⋱                                      ⋱         ⋱
                                               ˙
                                               θ
                      ⎢                                                     ⎥⎢                                             ⎥⎢ V       ⎥
algorithms for
                      ⎢                  ⋯                    −ad ξ n θ n   ⎥⎢                                             ⎥⎣ n       ⎦
                      ⎣                                                     ⎦⎣                                             ⎦
robot
dynamics                                                              ˙                               Adg −
                                                                                                         n− ,n

Lagrange’s                                                                                        Γ
Equations with
Constraints
                                                                  Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics




Chapter             us
Robot
Dynamics and
Control                   ˙         ¨         ˙
                          V = G ⋅ ξ θ + G ⋅ P V + G ⋅ ad ξ θ P V + G ⋅ ad ξ θ ΓV
                                                           ˙                ˙
Lagrangian
Equations
                  Finally the backward recursion:
Inertial


                                             Fn+ + Mn Vn − adT n ⋅ (Mn Vn )
Properties of
Rigid Body
                           Fn = AdT−
                                  g
                                                      ˙      V
Dynamics of                          n,n+

                         Fn− = AdT−          Fn + Mn− Vn− − adT n− ⋅ (Mn− Vn− )
an Open-chain
Manipulator
                                                      ˙       V
                                 g   n− ,n
Newton-Euler
Equations


                            F = AdT− F + M V − adT (M V )
                                           ˙
Coordinate-
invariant                         g  ,
                                                 V
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                    Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics


                   ⎡                                ⎤
                   ⎢   I    −AdT−                   ⎥                                       ⎡        ⎤
                   ⎢                                ⎥⎡       ⎤ ⎡                          ⎤⎢      ˙
                                                                                                  V  ⎥
                   ⎢                                ⎥⎢    F  ⎥ ⎢    M                     ⎥⎢         ⎥
                                g,
                   ⎢                                ⎥⎢       ⎥ ⎢                          ⎥⎢         ⎥
Chapter
                  ⇒⎢
                   ⎢
                              I      ⋱              ⎥⎢
                                                    ⎥⎢
                                                          F  ⎥=⎢
                                                             ⎥ ⎢
                                                                          M               ⎥⎢
                                                                                          ⎥⎢
                                                                                                  ˙
                                                                                                  V  ⎥
                                                                                                     ⎥
                   ⎢                 ⋱ −AdT−        ⎥⎢       ⎥ ⎢                  ⋱       ⎥⎢         ⎥
Robot
                   ⎢                                ⎥⎢    Fn ⎥ ⎢                       Mn ⎥ ⎢     ˙ ⎥
                              ⋱
                   ⎢                                ⎥⎣       ⎦ ⎣                          ⎦⎢      Vn ⎥
Dynamics and
                                           gn− ,n
                   ⎢                                ⎥                                       ⎣        ⎦
                   ⎣                                ⎦
Control
                              ⋯           I
                   ⎡            ⎤
Lagrangian                                                                    M

                   ⎢            ⎥       ⎡ −adT                      ⎤
                                        ⎢                           ⎥
Equations

                   ⎢            ⎥
                                ⎥ Fn+ + ⎢                           ⎥
                                                                         M                        V
                  +⎢
                                             V
                                        ⎢                           ⎥
Inertial

                   ⎢            ⎥       ⎢                           ⎥
Properties of                                       ⋱                             ⋱
                   ⎢ AdT−       ⎥       ⎣                  −adT n   ⎦
                                                                                      Mn          Vn
                   ⎣            ⎦
Rigid Body
                       gn,n+                                  V
Dynamics of
an Open-chain
Manipulator                 T
                           Pt                       adT
                                                      V
Newton-Euler
Equations

Coordinate-
invariant
                                                  T
                                 F = GT M V + GT Pt Fn+ +GT ⋅ adT MV
                                          ˙                     V
algorithms for
robot                                                     Ft
dynamics

Lagrange’s
Equations with
Constraints
                                                              Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics



                      τ = ξT ⋅ F
Chapter
Robot
                                               T
                      τ = ξ T GT M V + ξ T GT Pt Ft + ξ T GT ⋅ adT MV
                                   ˙                             V
Dynamics and
                            T T        ¨         ˙
Control                 = ξ G M(Gξ θ + GP V + G ⋅ ad ξ θ P V + G ⋅ ad ξ θ ΓV)
                                                              ˙         ˙
Lagrangian        ⇒
Equations
                                 T
                       + ξ T GT Pt Ft + ξ T GT ⋅ adT MV
                                                   V
Inertial                   T T        ¨ + ξ T GT MGP V + ξ T GT MG ⋅ ad ˙ ΓV
                                                      ˙
Properties of          = ξ G MGξ θ                                     ξθ
Rigid Body

Dynamics of
                                 T
                       + ξ T GT Pt Ft + ξ T GT ⋅ adT MV
                                                   V
an Open-chain
Manipulator       Finally we get:
                      M(θ)                           ˙
                                                 C(θ,θ)
Newton-Euler


                  ξ T GT MGξ θ + ξ T GT (MG ⋅ ad ξ θ Γ + adT M)Gξ θ
Equations
                             ¨                     ˙
                                                                  ˙
Coordinate-                                                V
invariant
algorithms for
                                           T
                  + ξ T GT MGP V + ξ T GT Pt Ft = τ
                                ˙
robot

                                       JtT (θ)
dynamics
                          ϕ(θ)
Lagrange’s
Equations with
Constraints
                                                              Chapter 4 Robot Dynamics and Control

                 4.5 Coordinate-invariant algorithms for robot dynamics




                               M(θ)θ + C(θ, θ) + ϕ(θ) + JtT (θ)Ft = τ
                                   ¨        ˙
Chapter
Robot
Dynamics and
Control

Lagrangian
Equations
                   M(θ) = ξ T GT MGξ
Inertial          C(θ, θ) = ξ T GT (MGad ξ θ Γ + adT M)Gξ θ
                       ˙                   ˙       V
                                                          ˙
Properties of

                    ϕ(θ) = ξ T GT MGP V
Rigid Body
                                      ˙
Dynamics of
an Open-chain
Manipulator
                       Jt = Pt Gξ
Newton-Euler      Property :
Equations
                      Γn = ,
                       G = (I − Γ)− = I + Γ + Γ + ⋯ + Γn−
Coordinate-
invariant
algorithms for
robot
dynamics          I + ΓG = G
Lagrange’s
Equations with
Constraints
                                                                Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                  De nition: Holonomic constraints
Chapter
                  Given generalized coordinates q = (q , . . . , qn ) ∈ E, a holonomic
Robot             constraint is a set of constraint equations:
                                           hi (q) = , i = , . . . , k
Dynamics and
Control

Lagrangian
Equations
                  Q = h ( ) is a manifold of dim n − k ≜ m if the constraints are
                        −

                  linearly independent.
                            Tq Q ∶ {V ∈ Tq E dhi ⋅ V = , ∀i = , . . . , k} ⊂ Tq E
Inertial
Properties of
Rigid Body
                  subspace of permissible velocities.
                     Tq Q⊥ ∶ {f ∈ Tq E ⟨f , v⟩ = , ∀V ∈ Tq Q} = span{dh , . . . , dhk }
                      ∗             ∗
Dynamics of
an Open-chain
Manipulator

Newton-Euler
                  subspace of constraint forces.
Equations

Coordinate-       De nition: Constraint forces
invariant
                                                    ∂h T
algorithms for                             Γ=       ∂q     ⋅λ
robot
                        k
dynamics
                  λ ∈ R is the vector of relative magnitudes of constraint forces.
Lagrange’s
Equations with
Constraints
                                                               Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                  De nition: Pfa an Constraints
Chapter
Robot             A Pfa an constraint has the form:
Dynamics and
Control
                                      A(q)˙ = , A(q) ∈ Rk×n
                                            q
                  Given a Pfa an constraint,
                                ∆q = {Vq ∈ Tq E A(q) ⋅ Vq = } ⊂ Tq E
Lagrangian
Equations

Inertial
Properties of     Distribution of permissible velocities.
Rigid Body

Dynamics of
an Open-chain
Manipulator       De nition:
Newton-Euler           q
                  A(q)˙ = is holonomic (or integrable) i ∆q is an involutive
Equations
                  distribution, or i
                           ∃hi ∶ E ↦ R, i = , . . . , k s.t. ∆q = Tq Q, Q = h− ( )
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                          Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                 § Constraint forces:
                                     Γ = AT (q) ⋅ λ, λ ∈ Rk
Chapter
Robot
Dynamics and
Control

Lagrangian
Equations
                 § Kinetic energy:
Inertial
Properties of
                                  T(q, q) = qT ⋅ M(q) ⋅ q
                                       ˙    ˙           ˙
Rigid Body

Dynamics of
an Open-chain    § Potential energy:
Manipulator

Newton-Euler                                 V(q)
Equations

Coordinate-
invariant        § Lagrangian:
                                 L(q, q) = T(q, q) − V(q)
algorithms for
robot
dynamics                              ˙         ˙
Lagrange’s
Equations with
Constraints
                                                       Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints



Chapter
Robot
                  ◻ Lagrange’s equations with constraints:
                            M(q)¨ + C(q, q) + N(q) + AT (q)λ = F
Dynamics and
Control                         q        ˙
Lagrangian
Equations

Inertial
Properties of
                  ◻ Explicit solution for constraint forces:
Rigid Body
                               q ˙ q
                           A(q)¨ + A(q)˙ =
                           (AM − AT )λ = AM − (F − C − N) + A˙
Dynamics of
an Open-chain                                               ˙q
Manipulator

Newton-Euler
Equations
                           λ = (AM − AT )− (AM − (F − C − N) + A˙ )
                                                               ˙q
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                            Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                   Example:
Chapter
Robot                                     x +y =l
                                          [x y] x
                                                ˙
Dynamics and
Control
                                                y
                                                ˙     =
Lagrangian
                     y
Equations                                  A(q)

                                   x      L(q, q) = m(˙ + y ) − mgy
Inertial
Properties of                                  ˙       x ˙
Rigid Body                   l
                                            m        x +
                                                     ¨                 x
                                                             mg + y λ =
                                 (x, y)
Dynamics of
                         θ                        m  y
                                                     ¨
                                          λ = (AM − AT )− (AM − (F − C − N) + A˙ )
an Open-chain
Manipulator                                                                   ˙q

                                            = − (gy + x + y )
Newton-Euler
Equations                                        m
                                                       ˙ ˙
Coordinate-                  mg                  l
invariant
                                          =     x λ = mg y + m (˙ + y )
                                                                 x ˙
algorithms for                                  y
robot                                                   l     l
dynamics

Lagrange’s
Equations with
Constraints
                                                          Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                  ◻ Lagrange-d’Alembert formulation:
                                                  q
                  Given the Pfa an constraint A(q)˙ =     and virtual displacement
                  δq ∈ Rk , we have:
Chapter
Robot
Dynamics and
Control
                    eorem (D’alembert Principle):
Lagrangian
Equations           Forces of constraints do no virtual work!
Inertial                 (AT (q)λ) ⋅ δq = for A(q)δq =
Properties of
Rigid Body

Dynamics of
an Open-chain
Manipulator
                                                                                  –
Newton-Euler
Equations
                    eorem (Lagrange-d’Alembert Equation):
Coordinate-
invariant                 d ∂L ∂L
algorithms for                 −   − τ ⋅ δq = , A(q)δq =
robot                        q
                         dt ∂˙ ∂q
dynamics

Lagrange’s
Equations with
Constraints
                                                                                   -
                                                           Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints



Chapter          Let A(q) = [A (q) A (q)], and A (q) ∈ Rk×k is invertible, then
                 δq ∈ Rn−k are free variables:
Robot
Dynamics and
Control

Lagrangian
Equations
                  δq = −A− (q)A (q)δq
Inertial              d ∂L ∂L
Properties of     ⇒         −  − τ ⋅ δq
Rigid Body                q
                      dt ∂˙ ∂q
Dynamics of
an Open-chain         d ∂L ∂L                    d ∂L    ∂L
Manipulator       =         −    −τ     ⋅ δq +         −    −τ         ⋅ δq
Newton-Euler
                          q
                      dt ∂˙   ∂q                     q
                                                 dt ∂˙   ∂q

                                                                       ⋅ (−A− A )δq
Equations
                      d ∂L ∂L                    d ∂L    ∂L
Coordinate-       =         −    −τ     ⋅ δq +         −    −τ
invariant                 q
                      dt ∂˙   ∂q                     q
                                                 dt ∂˙   ∂q
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                         Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints



Chapter
Robot            As δq ∈ Rn−k is free,
Dynamics and
Control


                                         − AT A−T
Lagrangian              d ∂L ∂L                     d ∂L    ∂L
Equations                     −    −τ                     −    −τ            =
Inertial
                            q
                        dt ∂˙   ∂q                      q
                                                    dt ∂˙   ∂q
Properties of
Rigid Body

Dynamics of                       Lagrange-d’Alembert equation
an Open-chain
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                          Chapter       Robot Dynamics and Control

                  4.6 Lagrange’s Equations with Constraints


                     Example: Dynamics of a rolling disk
Chapter


                     ⎧ x − ρ cos θ ϕ =
Robot
                     Pfa an constraint:
                     ⎪˙
                     ⎪
Dynamics and
                                 ˙˙
                     ⎨
Control                                                      ϕ


                     ⎪˙
                     ⎪ y − ρ sin θ ϕ =
Lagrangian

                     ⎩
Equations                          ˙                     (x, y)
                                                                    θ

Inertial
Properties of
Rigid Body                                            −ρ cos θ ˙
                                     q
                               ⇒ A(q)˙ =              −ρ sin θ q =
Dynamics of

                                 L(q, q) = m(˙ + y ) + I θ + I ϕ
                                                          ¨
an Open-chain
Manipulator                           ˙      x ˙                 ˙
                 Lagrange-d’Alembert equation:
                                 ⎡ m             ⎤    ⎡     ⎤
Newton-Euler

                               ⎛⎢                ⎥    ⎢     ⎥⎞
Equations

                                 ⎢      m
                                                   ¨ ⎢ τθ ⎥ ⋅ δq =
                                                 ⎥q − ⎢
                                 ⎢               ⎥          ⎥
Coordinate-

                               ⎝⎢              I ⎥    ⎢ τϕ ⎥⎠
                                           I
                                 ⎣               ⎦    ⎣     ⎦
invariant
algorithms for
robot
dynamics         where A(q) ⋅ δq = .
Lagrange’s
Equations with
Constraints
                                                                  Chapter   Robot Dynamics and Control

                  4.6 Lagrange’s Equations with Constraints

                 As
                                              δx = ρ cos θ ⋅ δϕ
Chapter
Robot                                         δy = ρ sin θ ⋅ δϕ
Dynamics and
Control          the equation of motion is:
                                        x +
                                        ¨        I           ¨
                                                             θ           τθ    δθ
Lagrangian
                       mρcθ mρsθ        y
                                        ¨            I       ¨     −    τϕ  ⋅ δϕ                =
Equations
                                                             ϕ
Inertial
                                                x
                                                ¨        I              ¨
                                                                        θ = τθ
Properties of            ⇒    mρcθ    mρsθ      y
                                                ¨    +       I          ¨    τϕ
Rigid Body                                                              ϕ
                                        ⎧ x = ρ cos θ ⋅ ϕ − ρ sin θ ⋅ θ ϕ
                 As
                                        ⎪¨
Dynamics of

                                        ⎪               ¨             ˙˙
an Open-chain

                                        ⎨
Manipulator

                                        ⎪ y = ρ sin θ ⋅ ϕ + ρ cos θ ⋅ θ ϕ
                                        ⎪¨
                                        ⎩
Newton-Euler                                            ¨             ˙˙
Equations

Coordinate-                               I                    ¨
                                                               θ = τθ
invariant                          ⇒            I + mρ         ¨         τϕ
                                                              ϕ
                 Solve for (θ(t), ϕ(t)), and then solve for (x(t), y(t)) from:
algorithms for


                             ⎧ x = ρ cos θ ⋅ ϕ
robot

                             ⎪˙
                             ⎪
dynamics
                                              ˙
                             ⎨
Lagrange’s

                             ⎪˙
                                                ⇐ st order di erential equation
                             ⎪ y = ρ sin θ ⋅ ϕ
Equations with

                             ⎩
Constraints                                  ˙
                                                             Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                  ◻ Nature of nonholonomic constraints:
                  Consider q = (r, s) ∈ R × R with Pfa an constraint,
Chapter


                                        ˙ + aT (r)˙ = , a(r) ∈ R
Robot
Dynamics and
Control                                 s          r
Lagrangian        Lagrangian
                                                L = L(r, ˙, ˙)
Equations
                                                          r s
Inertial
Properties of     and constrained Lagrangian
                                       Lc (r, ˙) = L(r, ˙, −aT (r)˙)
Rigid Body
                                              r          r         r
Dynamics of
an Open-chain     ⇒ Lagrange’s equation:
Manipulator
                                         d ∂Lc ∂Lc
Newton-Euler                                     −       = ,i = ,
Equations                               dt ∂˙ir     ∂ri
                                                        ⎛ ∂L ∂L      ∂aj ⎞
                                      − ai (r)
Coordinate-                   d ∂L              ∂L
                                                                        r
                                                        ⎝ ∂ri ∂˙ j ∂ri ⎠
invariant                ⇒                           −        −         ˙j =
algorithms for
robot
                             dt ∂˙i r            s
                                                ∂˙               s
dynamics

Lagrange’s
Equations with
Constraints
                                                                     Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                                                    ∂L ⎛                                  ∂aj ⎞
                                − ai (r)                ai (r) −                             ˙j (∗)
                     d ∂L ∂L             d ∂L ∂L
                                                                                             r
                                                    ∂˙ ⎝                                  ∂ri ⎠
Chapter          ⇒         −                   −  =      ˙
Robot                    r
                     dt ∂˙i ∂ri              s
                                         dt ∂˙ ∂s    s                                j
Dynamics and
Control

Lagrangian
                                                                                ≠
Equations        If the constraint is holonomic, i.e.
Inertial

                                    ai (r) =
Properties of                                  ∂h
Rigid Body                                         for some h ∶ E ↦ R
Dynamics of                                    ∂ri
an Open-chain
Manipulator
                 then RHS (right hand side) of (∗) equals
Newton-Euler


                                  ∂L ⎛                           ∂ h ⎞
Equations

Coordinate-                                   ∂ h
                                  ∂˙ ⎝
                                                     r
                                                                ∂rj ∂ri ⎠
invariant                                            ˙j −              r
                                                                       ˙j =
algorithms for                     s     j   ∂ri ∂rj        j
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                   Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                  ◻ Metric, duality and orthogonality on Tq E:
                                  xn
                       Tq Q
Chapter
Robot                                  ⋯
Dynamics and                      q
Control
                              x
                                                            K = qT M(q)˙
                                                                ˙      q
Lagrangian
Equations
                                           Q = h− ( )        =   ≪ q, q ≫M
                                                                   ˙ ˙
Inertial
Properties of
Rigid Body


                      Tq Q⊥ = {V ∈ Tq E ≪ V , V ≫M = V T MV = , ∀V ∈ Tq Q}
Dynamics of
an Open-chain

                      ∗            ∗
                     Tq Q⊥ = {f ∈ Tq E ⟨f , V⟩ = , ∀V ∈ Tq Q} ∶ constraint forces
Manipulator

Newton-Euler

                                              ∗      ∗      ∗
                       Tq E = Tq Q ⊕ Tq Q⊥ , Tq E = Tq Q ⊕ Tq Q⊥
Equations

Coordinate-
invariant
algorithms for    De nition:
                                    ∗
                      M b ∶ Tq E ↦ Tq E, ⟨M V , V ⟩ = V T MV =≪ V , V ≫M
robot
dynamics

Lagrange’s
                           ∗                            −
Equations with
Constraints
                      M ∶ Tq E ↦ Tq E, M = M
                                                                     Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints



Chapter
Robot
Dynamics and                                       Reciprocal
Control

Lagrangian
Equations

Inertial
Properties of
Rigid Body
                  Property :
                                                                                 ∗
Dynamics of
                                     ∂
an Open-chain
Manipulator
                  Under the basis   ∂qi   and dqi , i = , . . . , n of Tq E and Tq E
Newton-Euler      respectively, the matrix representation of M and M is M and
                  M − respectively.
Equations

Coordinate-
invariant
algorithms for    Property :
                                                   ∗
                                               M (Tq Q) = Tq Q
robot
dynamics

Lagrange’s
Equations with
                                                  ∗
                                              M (Tq Q⊥ ) = Tq Q⊥
Constraints
                                                               Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                 Given
Chapter                                       h ∶E ↦ Rk , m = n − k
Robot

                                       h∗ ≜ Tq h ∶Tq E ↦ Th(q) Rk
Dynamics and
Control

Lagrangian
Equations
                                             ∗     ∗          ∗
                                       h∗ ≜ Tq h ∶Th(q) Rk ↦ Tq E
Inertial
Properties of    we have
Rigid Body       Property :
Dynamics of                                                       ∗            ∗
                     ker h∗ = Tq Q, h∗ (Tq Q⊥ ) = Th(q) Rk , h∗ (Th(q) Rk ) = Tq Q⊥
                                     h∗
an Open-chain
Manipulator
                               ∗            ∗
Newton-Euler                 Tq E          Tq R k
Equations


                                                           M = h∗ ○ M ○ h∗
Coordinate-
invariant
algorithms for              M                    M
robot
dynamics

Lagrange’s                      Tq E        Th(q) Rk
Equations with
Constraints
                                       h∗
                                                             Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                  Lemma :       e map (I − Pω ) ∶ Tq E ↦ Tq Q⊥ given by
                                                   ∗      ∗

Chapter                             (I − Pω ) = h∗ ○ M ○ h∗ ○ M
Robot
Dynamics and      is a well-de ned projection map, with the property:
                                                            ∗
                                      (I − Pω )f = , ∀f ∈ Tq Q
Control


                                                            ∗
                                     (I − Pω )f = f , ∀f ∈ Tq Q⊥
Lagrangian
Equations

Inertial
Properties of     Proof :
                  Given f ∈ Tq Q, M (f ) ∈ Tq Q = ker h∗ , then (I − Pω )(f ) = . For
                              ∗
Rigid Body

                       ∗ ⊥
                  f ∈ Tq Q , ∃λ ∈ Rn−m s.t. f = h∗ λ, and
Dynamics of
an Open-chain

                                    (I − Pω )f = h∗ M h∗ M h∗ λ
Manipulator

Newton-Euler

                                                = h∗ λ = f
Equations

Coordinate-
                                                   ∗       ∗
invariant
algorithms for
                                         ⇒ P ω ∶ Tq E ↦ Tq Q
robot             is a well-de ned projection map. Similarly,
                                PT ∶ Tq E ↦ Tq Q, PT = I − M h∗ M b h∗
dynamics

Lagrange’s
Equations with
Constraints
                                                               Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                  and (I − PT ) ∶ Tq E ↦ Tq Q⊥ are projection maps.
Chapter
Robot             Lemma :
Dynamics and                                Pω M = MPT
                                           Pω h∗ = h∗ PT =
Control

Lagrangian
Equations

Inertial                                      PT = PT
                                                    ω
Properties of
Rigid Body

Dynamics of
an Open-chain
                 For nonholonomic constraints:
Manipulator                        h∗ ← A(q)
Newton-Euler
Equations                             h∗ ← A∗ (q)
Coordinate-                         Tq Q ← ∆ q
                                   ∗
                                  Tq Q⊥ ← span{ai (q), i = , . . . , k}
invariant
algorithms for
robot
dynamics
                 application in hybrid velocity/force control.
Lagrange’s
Equations with
Constraints
                                                            Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                  ◻ Lagrange’s equations of motion:
Chapter                     M(q)¨ + C(q, q) + N + AT (q)λ = τ
                                q        ˙
                                λ = (AM − AT )− (AM − (τ − C − N) + A˙ )
Robot
Dynamics and
Control
                                                                     ˙q
Lagrangian
Equations
                            M(q)¨ + AT (AM − AT )A˙ + Pω C + Pω N = Pω τ
                                q                ˙q
Inertial                        Pω = I − AT (AM − AT )− AM −
Properties of
Rigid Body

Dynamics of         ˜
                    C = Pω C
an Open-chain
Manipulator         ˜
                    N = Pω N
Newton-Euler
Equations            τ = Pω τ
                     ˜
                                                                                ∗
                   M θ + AT (AM − AT )− Aθ = Pω M θ ≜ M θ ∶ intertia forces in Tq Q
                     ¨                  ˙˙        ¨ ˜ ¨
Coordinate-
invariant

                                           ∗
algorithms for
robot
dynamics
                  De nition: Dynamics in Tq Q
Lagrange’s                               ˜ ¨ ˜ ˜ ˜
                                        Mθ + C + N = τ
Equations with
Constraints
                                                                Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                              (I − Pω )(M¨ + C + N) = (I − Pω )τ + AT τ
                                         q
                 Let
Chapter
Robot
                                   PT = I − M − AT (AM − AT )− A
Dynamics and
Control
                 and
                                          Pω M = MPT
                                        ∗   ⊥
Lagrangian
Equations
                 then the dynamics in T Q :
Inertial

                         M(I − PT )(¨ + M − C) = (I − Pω )(τ − N) + AT λ
Properties of
Rigid Body                          q
Dynamics of
an Open-chain     ◻ Geometric Interpretation:
                                            ∇↔M
Manipulator


                          M¨ + C + N = τ + AT λ ⇔ M∇q q = τ − N + AT λ
Newton-Euler
Equations
                           q                         ˙˙
Coordinate-
invariant                         ∇ ↔ induced metric on Tq Q
                                   ˜
                                                → N(Q) :   nd
algorithms for
robot                   S ∶TQ ⊗ TQ                              fundamental form
dynamics

Lagrange’s                ↑                         ↑
Equations with
Constraints               tangent vector eld normal vector eld
                                                             Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints



Chapter
Robot                                        ˜
                                      ∇X Y = ∇X Y + S(X, Y)
                                           −
                         M (I − PT )(¨ + M C) = (I − Pω )(τ − N) + AT λ
Dynamics and
Control
                                     q
Lagrangian
Equations                           S(˙ ,˙ )
                                      qq
Inertial
Properties of
                       MS(˙ , q): centrifugal force due to curvature of Q in E
                          q ˙
Rigid Body


                                                      ⎫
                                                      ⎪
Dynamics of

                                                      ⎪
                                                      ⎪
an Open-chain
                       ˜ ˙˙ ˜ ˜
                     M ∇q q = τ − N
                                                      ⎬ for hybrid control design
Manipulator

                                                   T ⎪
                   MS(˙ , q) = (I − Pω )(τ − N) + A λ ⎪
                                                      ⎪
Newton-Euler

                                                      ⎭
Equations             q ˙
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                         Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                   Example: Dynamics of a Spherical Pendulum

                  K = m(˙ + y + z )
Chapter
Robot
Dynamics and
                        x ˙ ˙
Control

Lagrangian           = qT M˙
                       ˙ q
Equations

Inertial
Properties of
                   q = (x, y, z)T , M = mI
Rigid Body

Dynamics of
                  h ∶qT q − r =
an Open-chain

                                   A = (x, y, z), M = AM AT = r m
Manipulator


                                          ⎡ y +z                       ⎤
Newton-Euler

                                          ⎢                            ⎥
Equations

                                  Pω = ⎢ −yz                           ⎥
                                                       −xy    −xz
                                          ⎢                            ⎥
Coordinate-

                                       r ⎢ −zx                         ⎥
                                                     x +z     −yz
                                          ⎣                            ⎦
invariant
algorithms for                                         −zy  x +y
                                          ⎡ x xy xz ⎤
robot

                                          ⎢              ⎥
dynamics

                              I − Pω = ⎢ yx y yz ⎥
                                          ⎢
                                       r ⎢ zx zy z ⎥
Lagrange’s

                                                         ⎥
                                          ⎣              ⎦
Equations with
Constraints
                                                                 Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                 PT = PT = Pω
                        ω
Chapter
                 (µ, ν): Spherical coordinates
                 q = (r cos µ cos ν, r cos µ sin ν, r sin µ)T
Robot
Dynamics and


                                ∇q q = PT (∇q q)
Control

Lagrangian
                                 ˜ ˙˙           ˙˙
Equations
                                            −r sin µ cos ν −r sin ν
Inertial
                                       = −r sin µ sin ν r cos ν          v
Properties of                                                            v
Rigid Body                                     r cos µ
Dynamics of
an Open-chain
                              S(˙ , q) = (I − PT )(∇q q)
                                q ˙                    ˙˙
Manipulator                                                   r cos µ cos ν
Newton-Euler                           = (− µ − cos µ ν ) r cos µ sin ν
                                            ˙             ˙
Equations                                                        r sin µ
Coordinate-      where
invariant
algorithms for                            v = µ + sin µ cos µ ν
                                                ¨              ˙
robot
dynamics                                 v = cos µ ν − sin µ µν
                                                   ¨         ˙˙
Lagrange’s
Equations with
Constraints
                                                           Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints



Chapter
                   ◻ Control Algorithm:
Robot
Dynamics and
Control
                       holonomic constraints:
                       ˜
                       q ∶ coordinates of Q
                       q = ψ(˜ ) ⇒ q = J ⋅ q
Lagrangian
Equations
                              q     ˙      ˙
                                           ˜
Inertial

                        τ = MJ(qd − Kv ˙ − Kp ˜) + C + N + AT (−λd + KI      ∫ (λ − λ ))
Properties of
Rigid Body                     ¨
                               ˜       ˜
                                       e      e                                          d
Dynamics of
an Open-chain          nonholonomic constraints:
                       Let J(q) ∈ Rn×m be s.t. AJ = . Write q = J ⋅ u for some u
Manipulator
                                                             ˙
                                                 ˙ + C + N + AT (−λd + KI (λ − λd ))
                  τ = MJ(˙ d − Kp (u − ud )) + M Ju
Newton-Euler
Equations

Coordinate-
                         u                                                     ∫
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                             Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                   Example: -DoF manipulator on a sphere with
                 frictionless point contact
Chapter
Robot
Dynamics and
Control
                   Contact constraint:

                  vz = ⇔ [               ]Adg − Vof =
Lagrangian
Equations
                                             fl f

                  ⇒ Holonomic constraint:
Inertial
Properties of

                  η = (αo , αfT , ψ): Parametrization of Q
                        T
Rigid Body


                  Pω = diag( , , , , , )
Dynamics of
an Open-chain
Manipulator

Newton-Euler       Newton-Euler Equations of motion:
                               M Vof − adT of MVof = Fm + G + AT λ
Equations
                                 ˙        V
                              ⎡ Rψ Mo                      ⎤
Coordinate-

                              ⎢                 −Mf        ⎥ ⎡ αo        ⎤
                              ⎢                            ⎥⎢ α          ⎥
invariant
                                                                ˙
                        Vof = ⎢ Rψ Ro Ko Mo −Ro Kf Mf      ⎥ ⎢ ˙f        ⎥ ≜ Jη
algorithms for

                              ⎢                            ⎥⎢ ψ          ⎥
robot                                                                         ˙
                              ⎢ −To Mo                     ⎥⎣ ˙          ⎦
dynamics

Lagrange’s                    ⎣                −Tf Mf      ⎦
                                MJ η + C = Fm + G + AT λ
Equations with
Constraints                         ¨                          ∗
                                                               Chapter   Robot Dynamics and Control

                  4.6 Lagrange’s Equations with Constraints

                 Pω (∗) ∶
                                      ˜¨ ˜
                                      M η + C = B Fm + B G
Chapter
Robot
Dynamics and                           −ϕ − λ = b Fm + b G
                            F = [f f f f f ]T = M(ηd − Kv ˙ − Kp ˜) + C − B G
Control

Lagrangian
                            ˆ                    ˜ ¨      ˜
                                                          e      e    ˜

                                                               ∫ (λ − λ ) − b G
Equations

Inertial                                    f = −ϕ − λd + KI                d
Properties of

                                             τ = JsT Fm
Rigid Body

Dynamics of
an Open-chain
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints

                   Example: -DoF manipulator rolling on a sphere
                                   ⎡                         ⎤
                                   ⎢                         ⎥
                                   ⎢                         ⎥ Vl l =
                                   ⎢                         ⎥ of
Chapter

                                   ⎢                         ⎥
                                   ⎣                         ⎦
Robot
Dynamics and
Control

Lagrangian
Equations
                                                A (q)
                                                ˙
                                         T
Inertial                           fc = A λ, λ ∈ R
Properties of
                              ωx
Rigid Body
                              ωy     = −R (Kf + Rψ Ko Rψ )Mf αf
                                                             ˙
Dynamics of
an Open-chain
Manipulator                    Vof = Adgflf ⋅ Vlo lf
                                                ⎡                            ⎤
                                                ⎢                            ⎥
Newton-Euler

                                                ⎢                            ⎥
                                                ⎢                            ⎥ αf
Equations

                                                ⎢                            ⎥˙
                                                ⎢ −Ro (Kf + Rψ Ko Rψ )Mf     ⎥
Coordinate-                          = Adgflf
                                                ⎢                            ⎥
invariant

                                                ⎣                            ⎦
algorithms for
robot                                                       o
                                    ≜ Jf αf
dynamics
                                         ˙
                   span{Jf }: Not involutive
Lagrange’s
Equations with
Constraints
                                                               Chapter   Robot Dynamics and Control

                 4.6 Lagrange’s Equations with Constraints


                        MJf αf + (M˙f αf − adTf αf MJf αf ) = Fm + G + AT λ
                            ¨       J ˙       J ˙      ˙
Chapter
Robot                Fm = MJf (αfd − Kp (αf − αfd )) + (M˙f αf − adT α MJf αf )
                               ¨         ˙     ˙            J ˙     J ˙   f
                                                                            ˙ f
Dynamics and

                        + AT (−λd +   ∫ (λ − λ )) − G
Control

Lagrangian
                                                d
Equations

Inertial
Properties of      Example: Redundant parallel manipulator
                                        θ = (θ , . . . , θ ) ∈ E
Rigid Body

Dynamics of

                                       θ a = (θ , θ , θ )
an Open-chain
Manipulator

Newton-Euler
                                      θ p = (θ , θ , θ )
                                ⎡                                        ⎤
Equations

                                ⎢   xa + lc + lc − xb − lc     − lc      ⎥
                                ⎢                                        ⎥
Coordinate-

                         H(θ) = ⎢                                        ⎥=
                                    ya + ls + ls − yb − ls     − ls
                                ⎢                                        ⎥
invariant

                                ⎢                                        ⎥
algorithms for
                                    xa + lc + lc − xc − lc     − lc
                                ⎣                                        ⎦
robot
dynamics                            ya + ls + ls − yc − ls     − ls
Lagrange’s
Equations with
Constraints
                                                                 Chapter   Robot Dynamics and Control

                  4.6 Lagrange’s Equations with Constraints


                 Mi (θ) ∈ R × : ith chain
Chapter          M(θ) = diag(M (θ), . . . , M (θ))
Robot
Dynamics and
                         ¨
                 M(θ)θ + C + N = τ + AT λ
Control
                 If all joints are actuated,
Lagrangian
Equations
                                         Position control of end-e ector
                                                        +
Inertial                                     internal grasping force
Properties of
                 As τ , τ , τ = ,
                                    θ ∈ R ∶ local parametrization of Q = H − ( )
Rigid Body

Dynamics of
                                    ˜
an Open-chain
Manipulator                             ˜
                                 θ = ψ(θ) ∶embedding of Q in E
Newton-Euler
Equations                         ˙    ˙
                                       ˜
                                  θ = Jθ
Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints
                                                                Chapter   Robot Dynamics and Control

                  4.6 Lagrange’s Equations with Constraints


                             ∗      ∗                     ∗
                 Given Pω ∶ Tθ E ↦ Tθ Q, the dynamics in Tθ Q is given by:
Chapter

                                  Pω MJ θ + Pω (C + N) = Pω τ
Robot
Dynamics and
                                         ¨
                                         ˜
                                 τ = (τ , τ , τ )
Control

Lagrangian                       ˜
                                Pω = (P , P , P )
Equations
                                ˜
Inertial
Properties of
Rigid Body                        ˆ ˆ ˜
                                  τ = Pω τ = Pω τ ∈ R
                                 τ = Pω MJ(θ d − Kv ˙ − Kp ˜) + Pω (C + N)
                                           ¨
Dynamics of
an Open-chain                    ˆ         ˜        ˜
                                                    e      e
Manipulator

Newton-Euler
Equations

Coordinate-
invariant
algorithms for
robot
dynamics

Lagrange’s
Equations with
Constraints

								
To top