Question? Leave a message!




Lecture Notes for A Mathematical Introduction to Robotic Manipulation

Lecture Notes for A Mathematical Introduction to Robotic Manipulation 28
Chapter 4 Robot Dynamics and Control SummerSchoolMath. MethodsinRoboticsTUBS.DEÔççÔJulyòýýÀ Ô LectureNotes Chapter ¥ Robot Dynamics and for Control AMathematicalIntroductionto Lagrangian Equations RoboticManipulation Inertial Properties of Rigid Body Dynamics of an Openchain By Manipulator ∗ j Z.X.Li andY.Q.Wu NewtonEuler Equations ∗ Coordinate Dept. ofECE,HongKongUniversityofScienceTechnology invariant j SchoolofME,Shanghai JiaotongUniversity algorithms for robot dynamics ò¥JulyòýýÀ Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control SummerSchoolMath. MethodsinRoboticsTUBS.DEÔççÔJulyòýýÀ ò Chapter ¥ Robot Chapter¥RobotDynamicsandControl Dynamics and Control Ô LagrangianEquations Lagrangian Equations ò InertialPropertiesofRigidBody Inertial Properties of Rigid Body ç DynamicsofanOpenchainManipulator Dynamics of an Openchain Manipulator ¥ NewtonEulerEquations NewtonEuler Equations Coordinate   Coordinateinvariantalgorithmsforrobotdynamics invariant algorithms for robot â Lagrange’sEquationswithConstraints dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.1 Lagrangian Equations ç y ◻ASimpleExample: F y F m F x Chapter ¥ Robot Dynamics and Control mg x ♢Review: Lagrangian Equations Newton’sEquation: LagrangianEquation: Inertial d ∂L ∂L Properties of − =F x Rigid Body ¨ mx=F x ˙ dt ∂x ∂x ⇔ Dynamics of d ∂L ∂L an Openchain my¨=F −mg y − =F Manipulator y ˙ dt ∂y ∂y Momentum:P =mx˙ NewtonEuler x Equations Lagrangianfunction: P =my˙ y ∂L ∂L Coordinate L=T−V,P = ,P = x y ∂x˙ ∂y˙ d d invariant P =F , P =F −mg x x y y algorithms for dt dt Kineticenergy: robot Ô ò ò ò dynamics ˙ ˙ T= m(x +y ) ò Lagrange’s Potentialenergy: Equations with Constraints V=mgyChapter 4 Robot Dynamics and Control 4.1 Lagrangian Equations ¥ ◻Generalizationtomultibodysystems: q ,i=Ô,...,n: generalizedcoordinates Chapter ¥ i Robot y Kineticenergy: Dynamics and Control m ç T=T(q,q˙) q ç Lagrangian Potentialenergy: Equations V=V(q) Inertial Properties of m ò Lagrangian: Rigid Body q ò Dynamics of L(q,q˙)=T(q,q˙)−V(q) an Openchain Manipulator τ ,i=Ô,...,n: externalforceonq i i m Ô NewtonEuler LagrangianEquation: Equations q Ô Coordinate x invariant d ∂L ∂L algorithms for − = τ ,i=Ô,...,n i robot dt ∂q˙ ∂q i i dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.1 Lagrangian Equations   ◇Example:Pendulumequation y Generalizedcoordinate: Chapter ¥ Ô θ∈S Robot Dynamics and Kinematics: Control x Lagrangian x=lsinθ,y=−lcosθ Equations ˙ ˙ ⇒ x˙=lcosθ⋅θ,y˙=lsinθ⋅θ Inertial Properties of Kineticenergy: θ Rigid Body Ô Ô ò ò ò ò ˙ ˙ ˙ ˙ Dynamics of T(θ,θ)= m(x +y )= ml θ an Openchain ò ò Manipulator Potentialenergy: NewtonEuler mg V =mgl(Ô−cosθ) Equations Lagrangianfunction: Coordinate invariant Ô ∂L ∂L ò ò ˙ ˙ algorithms for L=T −V = ml θ −mgl(Ô−cosθ),⇒ =ml θ, =−mglsinθ robot ˙ ò ∂θ ∂θ dynamics Equationofmotion: Lagrange’s d ∂L ∂L Equations with ò ¨ − = τ⇒ml θ +mglsinθ= τ Constraints ˙ dt ∂θ ∂θChapter 4 Robot Dynamics and Control 4.1 Lagrangian Equations â ◇Example:DynamicsofaSphericalPendulum Chapter ¥ Robot Dynamics and ⎡ ⎤ lsinθcosϕ Control ⎢ ⎥ r(θ,ϕ)=⎢ lsinθsinϕ ⎥ Lagrangian ⎢ ⎥ Equations ⎣ −lcosθ ⎦ Inertial Ô Ô ò ò ò ò ò ˙ ˙ Properties of T= mYr˙Y = ml (θ +(Ô−cos θ)ϕ ) Rigid Body ò ò θ Dynamics of V =−mglcosθ an Openchain Manipulator Ô ò θ ò ˙ ˙ L(q,q˙)= ml (θ +(Ô−cos )ϕ )+mglcosθ NewtonEuler ò Equations Coordinate mg invariant ϕ algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.1 Lagrangian Equations Þ Chapter ¥ ⎧ Robot d ∂L d ∂L ⎪ ò ò ò ò ⎪ ˙ ¨ ˙ Dynamics and ⎪ = (ml θ)=ml θ, =ml sinθcosθϕ −mglsinθ ⎪ ⎪ Control ˙ dt dt ∂θ ⎪ ∂θ ⎨ Lagrangian d ∂L d ∂L ⎪ ò ò ò ò ò ⎪ Equations ˙ ˙ ¨ ˙ ⎪ = (ml sin θϕ)=ml sin θϕ+òml sinθcosθθϕ, =ý ⎪ ⎪ ˙ ⎪ dt dt ∂ϕ ∂ϕ Inertial ⎩ Properties of Rigid Body ò ò ò ˙ ¨ ml ý −ml s c ϕ θ mgls ý θ θ Dynamics of θ   + + =  ò ò ò an Openchain ¨ ˙˙ ý ý ml s ý ϕ òml s c θϕ θ θ θ Manipulator NewtonEuler Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body — ◻Kineticenergyofarigidbody: Chapter ¥ Robot Dynamics and r Control B Lagrangian Equations r a Inertial Properties of g Rigid Body ab A Dynamics of an Openchain Manipulator NewtonEuler Volumeoccupiedbythebody: V Equations Massdensity: ρ(r) Coordinate invariant Mass: m= ρ(r)dV algorithms for ∫ V robot Ô dynamics Masscenter: r ≜ ρ(r)rdV ∫ Lagrange’s m V Equations with Relativetoframeatthemasscenter: r=ý ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body À InAframe Chapter ¥ Robot Ô Ô ò ò T ò Dynamics and ˙ ˙ ˙ ˙ ˙ ˙ T= ρ(r)Yp+RrY dV= ρ(r)(YpY +òp Rr+YRrY )dV Control ∫ ∫ ò V ò V Lagrangian Ô Ô ò T ò Equations ˙ ˙ ˙ ˙ = mYpY +p R ρ(r)rdV+ ρ(r)YRrY dV ∫ ∫ ò V ò Inertial Properties of ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Rigid Body =ý Dynamics of an Openchain Manipulator Ô ò ˙ NewtonEuler ρ(r)YRrY dV ∫ Equations ò V Coordinate Ô Ô Ô T ò ò ò ˙ invariant = ρ(r)YR RrY dV = ρ(r)YωˆrY dV = ρ(r)YˆrωY dV ∫ ∫ ∫ algorithms for ò ò ò robot Ô Ô Ô dynamics T ò T ò T = ρ(r)(−ω ˆr ω)dV = ω ‹ (−ρ(r)ˆr) dV�ω ≜ ω Iω ∫ ∫ Lagrange’s ò ò ò Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body Ôý where ⎡ ⎤ I I I xx xy xz ⎢ ⎥ Chapter ¥ ò ⎢ ⎥ I I I I =− ρ(r)ˆr dV ≜ xy yy yz Robot ⎢ ⎥ ∫ ⎢ ⎥ Dynamics and I I I xz yz zz ⎣ ⎦ Control with Lagrangian ò ò Equations I = ρ(r)(y +z )dxdydz,I =− ρ(r)xydxdydz xx xy ∫ ∫ Inertial Properties of Rigid Body Ô Ô Ô Ô ò b T b b T ò b T b b Dynamics of ˙ ˙ T= mYpY + (ω ) I ω = mYR pY + (ω ) I ω an Openchain ò ò ò ò Manipulator Ô mI ý b T b NewtonEuler = (V )  V b Equations ý I ò ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Coordinate b invariant M algorithms for robot b −Ô b T b T b dynamics ˆ ˙ ˙ ForV =g ⋅g˙,with ωˆ =R ⋅Randv =R p,M isthe Lagrange’s GeneralizedinertiamatrixinBframe. Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body ÔÔ b ◇Example:M forarectangularobject z m Chapter ¥ ρ= Robot lωh Dynamics and Control ò ò I = ρ(y +z )dxdydz xx ∫ y Lagrangian V Equations h ω l x ò ò ò ò ò Inertial = ρ (y +z )dxdydz h ∫ ∫ ∫ Properties of h ω l l − − − ò ò ò Rigid Body Ô m Dynamics of ç ç ò ò w = ρ‹ (lω h+lωh )�= (ω +h ) an Openchain Ôò Ôò Manipulator h ω l ò ò ò NewtonEuler I =− ρxydv=−ρ xydxdydz Equations xy ∫ ∫ ∫ ∫ h ω l V − − − ò ò ò Coordinate l invariant h ω ò algorithms for ò ò y ò robot = −ρ x W dydz=ý ∫ ∫ dynamics h ω − − ò l ò ò − ò Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body Ôò ⎡ m ò ò ⎤ (w +h ) ý ý ⎢ ⎥ Ôò ⎢ ⎥ m ò ò mI ý ç×ç I =⎢ ý (l +h ) ý ⎥,M=  Ôò ý I ⎢ ⎥ m ò ò ⎢ ⎥ Chapter ¥ ý ý (w +l ) ⎣ Ôò ⎦ Robot Dynamics and Control g ý g (t) ò −Ô Lagrangian ˆ V =g ⋅g˙ , M A Ô Ô Ô Ô B Equations Ô T Inertial T= V M V Ô Ô Properties of Ô ò Rigid Body g (t) Ô g ý V =Ad V Dynamics of Ô g ò ý an Openchain Manipulator NewtonEuler Ô Ô Ô T T T T Equations T= (Ad V ) M (Ad V )= V Ad M Ad V ≜ V M V g ò Ô g ò Ô g ò ò ò ý ý ò g ý ò ý ò ò ò Coordinate invariant ◻Munderchangeofframes: algorithms for robot dynamics T Lagrange’s M =Ad M Ad ò Ô g g ý ý Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body Ôç ◇Example:Dynamicsofaòdofplanarrobot Chapter ¥ Robot ⎡ ⎤ I ý ý xx i Dynamics and ⎢ ⎥ ý I ý Control I =⎢ ⎥,i=Ô,ò yy i i ⎢ ⎥ ý ý I ⎣ ⎦ Lagrangian zz i Equations Inertial Properties of l 2 Ô Ô Rigid Body ò T ˙ T(θ,θ)= m Yv Y + ω I ω Ô Ô Ô Ô Ô θ 2 Dynamics of ò ò r 2 an Openchain Ô Ô Manipulator ò T + m Yv Y + ω I ω l ò ò ò ò 1 ò y ò ò NewtonEuler Equations r1 θ 1 x Coordinate invariant ý ý algorithms for ý ý robot ω = ω = Ô ò dynamics ˙ ˙ ˙ θ θ +θ Ô Ô ò Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.2 Intertial Properties of Rigid Body Ô¥ x i y P = :Masscenter γ:Distancefromjointitomasscenter i i i ý Chapter ¥ ˙ ˙ Robot x =−r s θ x =r c Ô Ô Ô Ô Ô Ô Ô Dynamics and ⇒ Control ˙ ˙ y =r s Ô Ô y =r c θ Ô Ô Ô Ô Ô Lagrangian ˙ ˙ ˙ Equations x =l c +r c x =−(l s +r s )θ −r s θ ò Ô Ô ò Ôò Ô ò Ôò ò ò Ô Ô ò Ôò ⇒ Inertial ˙ ˙ ˙ y =l s +r s Ô Ô ò Ôò y =(l c +r c )θ +r c θ Properties of ò Ô Ô ò Ôò Ô ò Ôò ò Ô Rigid Body Ô Ô Ô Ô ò ò ò ò ò ò ò ˙ ˙ ˙ ˙ ˙ ˙ ˙ ˙ T(θ,θ)= m (x +y )+ I θ + m (x +y )+ I (θ +θ ) Dynamics of Ô z ò z Ô Ô Ô ò ò Ô ò Ô ò an Openchain ò ò ò ò Manipulator Ô ˙ α+òβc δ+βc θ ò ò ˙ ˙ Ô = θ θ    NewtonEuler Ô ò ˙ δ+βc δ ò θ Equations ò ò ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Coordinate M(θ) invariant ò ò ò ò algorithms for α=I +I +m r +m (l +r ),β=m l r ,δ=I +m r ,L=T z z Ô ò ò Ô ò z ò Ô ò Ô Ô ò ò ò robot dynamics ˙ ¨ ˙ −βs θ −βx θ θ τ Ô ò ò Ô Ô ⇒ M(θ) +  =  Lagrange’s τ ¨ ˙ ˙ ò θ βs θ ý θ Equations with ò ò Ô ò ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator Ô  ◻Dynamicsofopenchainmanipulator: De�nition: Chapter ¥ ˆ ˆ ξ θ ξ θ Ô Ô i i Robot L :frameatmasscenteroflinki,g (θ)= exp ⋯exp g (o) i sl sl i i Dynamics and Control θ 1 Lagrangian θ2 θ3 Equations l l 1 2 L L 2 3 Inertial Properties of Rigid Body r r 1 2 l 0 Dynamics of L 1 an Openchain Manipulator r 0 S NewtonEuler Equations ˙ ⎡ ⎤ Coordinate θ Ô ⎢ ⎥ invariant ⎢ ⎥ ⋮ algorithms for ⎢ ⎥ ˙ robot ⎢ ⎥ b b † † † θ ˙ i ˙ ⎢ ⎥ dynamics V =J (θ)θ=ξ ξ ⋯ ξ ý⋯ý =J(θ)θ i sl sl Ô ò i i i ˙ ⎢ ⎥ θ i+Ô ⎢ ⎥ Lagrange’s ⋮ ⎢ ⎥ Equations with ⎢ ⎥ ˙ Constraints ⎣ θ ⎦ nChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator Ôâ ˆ ˆ † −Ô ξ θ ξ θ j+Ô j+Ô i i ξ = Ad (e ⋯e g (ý))ξ , j≤i sl j j i Ô Ô Chapter ¥ b T b b T T b ˙ ˙ ˙ T(θ,θ)= (V ) M V = θ J (θ)M J(θ)θ Robot i i sl i sl i i i i Dynamics and ò ò n Control Ô T ˙ ˙ ˙ T(θ)= T(θ,θ)= θ M(θ)θ, Q Lagrangian i ò Equations i=Ô n Inertial Ô T b Properties of ˙ ˙ M(θ)= J (θ)M J(θ)= M (θ)θ θ Q Q i ij i j i i Rigid Body ò i i,j=Ô Dynamics of an Openchain h(θ):HeightofL, V(θ)=mgh(θ), V(θ)= mgh(θ) Q i i i i i i i Manipulator i=Ô NewtonEuler Lagrange’sEquation: Equations d ∂L ∂L Coordinate − = Γ, i= Ô,...,n, i invariant ˙ dt ∂θ ∂θ algorithms for i i robot n n dynamics ⎛ ⎞ d ∂L d ˙ ¨ ˙ ˙ = M θ = M θ +M θ Q Q Lagrange’s ij j ij j ij j ˙ dt dt⎝ ⎠ Equations with ∂θ i j=Ô j=Ô ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator ÔÞ n ∂M ∂M ∂L Ô ∂V kj ij ˙ ˙ ˙ ˙ = θ θ − M = θ Q Q k j ij k ∂θ ò ∂θ ∂θ ∂θ i i i k j,k=Ô k Chapter ¥ Robot n n ∂M ∂M ij Ô kj ∂V Dynamics and ¨ ˙ ˙ ˙ ˙ ⇒ M θ + Œ θ θ − θ θ‘+ = Γ Control Q Q ij j j k k j i ∂θ ò ∂θ ∂θ i i k j=Ô j,k=Ô Lagrangian Equations n n ∂V k ¨ ˙ ˙ Inertial ⇒ M θ + Γ θ θ + = Γ Q Q ij j k j i ij Properties of ∂θ i j=Ô j,k=Ô Rigid Body Dynamics of ∂M ∂M Ô ∂M ij kj ik k an Openchain Γ = Œ + − ‘ ij Manipulator ò ∂θ ∂θ ∂θ k j i NewtonEuler Equations ò ˙ ˙ ˙ , i≠j θ ⋅θ :Coriolis force θ :Centrifugalforce i j i Coordinate invariant n n ∂M ∂M Ô ∂M ij kj k ik algorithms for ˙ ˙ ˙ De�ne: c (θ,θ)= Γ θ = Œ + − ‘θ Q Q ij k k robot ij ò ∂θ ∂θ ∂θ dynamics k j i k=Ô k=Ô Lagrange’s ¨ ˙ ˙ ⇒ M(θ)θ +C(θ,θ)θ +N(θ)= τ Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator Ô— PropertyÔ: Chapter ¥ T T T ˙ ˙ ˙ ˙ Robot Ô M(θ)=M (θ),θ M(θ)θ≥ ý,θ M(θ)θ= ý⇔ θ= ý Dynamics and n×n Control ˙ ò M−òC∈R isskewsymmetric Lagrangian Equations Proof: Inertial Properties of ˙ ˙ Rigid Body (M−òC) =M −òc (θ) ij ij ij Dynamics of n ∂M ∂M ∂M ∂M an Openchain ij ij kj ik ˙ ˙ ˙ ˙ Manipulator =Q θ − θ − θ + θ k k k k ∂θ ∂θ ∂θ ∂θ k k j i k=Ô NewtonEuler Equations n ∂M ∂M kj ik ˙ ˙ = θ − θ Coordinate Q k k invariant ∂θ ∂θ i j k=Ô algorithms for robot T ˙ ˙ dynamics Switchingiandjshows(M−òC) =−(M−òC) Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator ÔÀ ◇Example:PlanaròDoFRobot(continued) Chapter ¥ m (θ)= α+òβcosθ ,m = δ ÔÔ ò òò Robot Dynamics and m (θ)=m (θ)= δ+βcosθ Ôò òÔ ò Control ˙ ˙ ˙ ˙ ˙ Lagrangian c (θ,θ)=−βsinθ ⋅θ ,c (θ,θ)=−βsinθ (θ +θ ) ÔÔ ò ò Ôò ò Ô ò Equations ˙ ˙ ˙ Inertial c (θ,θ)= βsinθ ⋅θ ,c (θ,θ)= ý òÔ ò Ô òò Properties of ⎧ Ô ∂M ∂M ∂M Ô ∂M Rigid Body ÔÔ ÔÔ ÔÔ ÔÔ ⎪ Ô ⎪ Γ = ( + − )= =ý ⎪ ÔÔ ⎪ ⎪ Dynamics of ò ∂θ ∂θ ∂θ ò ∂θ ⎪ Ô Ô Ô Ô ⎪ an Openchain ⎪ ⎪ ⎪ Ô ∂M ∂M ∂M Ô ∂M Manipulator ⎪ ÔÔ Ôò òÔ ÔÔ ò ⎪ ⎪ Γ = ( + − )= =−βsinθ ò ⎪ ÔÔ ⎪ NewtonEuler ⎪ ò ∂θ ∂θ ∂θ ò ∂θ ò Ô Ô ò ⎨ Equations ⎪ Ô ∂M ∂M ∂M Ô ∂M Ôò ÔÔ Ôò ÔÔ ⎪ Ô ⎪ Γ = ( + − )= =−βsinθ Coordinate ⎪ ò ⎪ Ôò ⎪ ò ∂θ ∂θ ∂θ ò ∂θ invariant ⎪ Ô ò Ô ò ⎪ ⎪ algorithms for ⎪ ⎪ Ô ∂M ∂M ∂M ∂M Ô ∂M ⎪ robot Ôò Ôò òò Ôò òò ⎪ ò ⎪ Γ = ( + − )= − =−βsinθ ⎪ ò dynamics Ôò ⎪ ⎩ ò ∂θ ∂θ ∂θ ∂θ ò ∂θ ò ò Ô ò Ô Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator òý ⎧ Ô ∂M ∂M ∂M ∂M Ô ∂M òÔ òÔ ÔÔ òÔ ÔÔ ⎪ Ô ⎪ Γ = ( + − )= − = βsinθ ⎪ ò òÔ ⎪ ⎪ ò ∂θ ∂θ ∂θ ∂θ ò ∂θ ⎪ Ô Ô ò Ô ò ⎪ Chapter ¥ ⎪ ⎪ ⎪ Robot Ô ∂M ∂M ∂M Ô ∂M ⎪ òÔ òò òÔ òò ò ⎪ ⎪Γ = ( + − )= =ý Dynamics and ⎪ òÔ ⎪ ⎪ Control ò ∂θ ∂θ ∂θ ò ∂θ ò Ô ò Ô ⎨ Lagrangian ⎪ Ô ∂M ∂M ∂M Ô ∂M ⎪ òò òÔ Ôò òò Ô ⎪ Equations ⎪Γ = ( + − )= =ý òò ⎪ ⎪ ò ∂θ ∂θ ∂θ ò ∂θ ⎪ Ô ò ò Ô ⎪ Inertial ⎪ ⎪ ⎪ Properties of ⎪ Ô ∂M ∂M ∂M Ô ∂M òò òò òò òò ò ⎪ ⎪ Γ = ( + − )= =ý Rigid Body ⎪ òò ⎪ ò ∂θ ∂θ ∂θ ò ∂θ ⎩ ò ò ò ò Dynamics of ˙ ˙ an Openchain −òβsinθ ⋅θ −βsinθ ⋅θ ò ò ò ò ˙ M−òC= Manipulator ˙ −βsinθ ⋅θ ý ò ò NewtonEuler Equations ˙ ˙ ˙ −òβsinθ ⋅θ −òβsinθ (θ +θ ) ò ò ò Ô ò − ˙ Coordinate òβsinθ ⋅θ ý ò Ô invariant algorithms for ˙ ˙ ý βsinθ (òθ +θ ) ò Ô ò robot = ⇐ skewsymmetric dynamics ˙ ˙ −βsinθ (òθ +θ ) ý ò Ô ò Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator òÔ ◇Example:Dynamicsofaçdofrobot Chapter ¥ θ1 Robot θ θ 2 3 Dynamics and l l 1 2 Control ý ⎡ ý ⎤ ⎡ ý ⎤ ⎡ ⎤ L L 2 3 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ý −l −l ⎢ ⎥ ⎢ ý ⎥ ⎢ ý ⎥ Lagrangian ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ý ý l Ô ξ =⎢ ⎥, ξ =⎢ ⎥,ξ =⎢ ⎥ r r Equations Ô ò ç 1 2 ý ⎢ ⎥ ⎢ −Ô ⎥ ⎢ −Ô ⎥ l0 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ý ý ý L 1 ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ Inertial Ô ⎣ ⎦ ⎣ Ô ⎦ ⎣ Ô ⎦ Properties of r0 Rigid Body S Dynamics of an Openchain ⎡ ⎤ ⎡ ⎤ ⎡ ý ⎤ ý ý Manipulator ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ r I ‹ � I ‹ Ô � I Œ l +r ‘ ý Ô ò ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ g (ý)= ,g (ý)= ,g (ý)= sl sl sl Ô ⎢ r ⎥ ò ⎢ l ⎥ ç ⎢ ⎥ ý ý l ý NewtonEuler ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ý Ô ý Ô ⎣ ⎦ ⎣ ⎦ ⎣ ý Ô ⎦ Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator òò ⎡ ⎤ m ý ý i ⎢ ⎥ ý m ý ý i ⎢ ⎥ ⎢ ⎥ ý ý m i Chapter ¥ ⎢ ⎥ M = Robot i ⎢ I ý ý ⎥ x i Dynamics and ⎢ ⎥ ý I ý ⎢ ý ⎥ y Control i ⎢ ⎥ ý ý I ⎣ z ⎦ i Lagrangian Equations m :emassoftheobject i I :emomentofinertiaaboutthexaxis Inertial x i Properties of Rigid Body ò ò Γ =(I −I −m r )c s +(I −I )c s −m (l c +r c )(l s +r s ) Dynamics of y z ò ò ò y z òç òç ç Ô ò ò òç Ô ò ò òç ÔÔ ò ò Ô ò ç ç an Openchain Γ =(I −I )c s −m r s (l c +r c ) y z òç òç ç ò òç Ô ò ò òç ÔÔ ç ç Manipulator Ô ò Γ =(I −I −m r )c s +(I −I )c s −m (l c +r c )(l s +r s ) y z ò ò ò y z òç òç ç Ô ò ò òç Ô ò ò òç Ôò ò ò Ô ç ç NewtonEuler Ô Γ =(I −I )c s −m r s (l c +r c ) Equations y z òç òç ç ò òç Ô ò ò òç Ôç ç ç Ô ò Γ =(I −I +m r )c s +(I −I )c s +m (l c +r c )(l s +r s ) z y ò ò ò z y òç òç ç Ô ò ò òç Ô ò ò òç Coordinate òÔ ò ò Ô ç ç ç ò ç invariant Γ =−l m r s , Γ =−l m r s , Γ =−l m r s Ô ç ò ç Ô ç ò ç òò òç Ô ç ò ç òç algorithms for Ô ò Γ =(I −I )c s +m r s (l c +r c ) , Γ =l m r s robot çÔ zç yç òç òç ç ò òç Ô ò ò òç Ô ç ò ç çò dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator òç ∂V ˙ N(θ,θ)= , V(θ)=m gh(θ)+m gh (θ)+m gh (θ) Ô Ô ò ò ò ç ∂θ Chapter ¥ Robot ˆ ˆ ξ θ ξ θ Ô Ô i i Dynamics and g (θ)=e ⋯e g (ý)⇒ sl sl i i Control Lagrangian h(θ)=r , h (θ)=l −r sinθ,h (θ) Ô ý ò ý Ô ç Equations =l −l sinθ −r sin(θ +θ )g (θ) ý Ô ò ò ò ç sl Inertial i Properties of ˆ ˆ ξ θ ξ θ Rigid Body Ô Ô i i =e ⋯e g (ý)⇒ sl i Dynamics of an Openchain h(θ)=r , h (θ)=l −r sinθ,h (θ) Ô ý ò ý Ô ç Manipulator =l −l sinθ −r sin(θ +θ ) NewtonEuler ý Ô ò ò ò ç Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator ò¥ ⎡ ý ý ý ⎤ ⎢ ⎥ ý ý ý ⎢ ⎥ Chapter ¥ ⎢ ⎥ b ý ý ý J =J (θ)=⎢ ⎥ Robot Ô sl Ô ý ý ý ⎢ ⎥ Dynamics and ⎢ ⎥ ý ý ý Control ⎢ ⎥ Ô ý ý ⎣ ⎦ Lagrangian Equations ⎡ ⎤ −r c ý ý Ô ò ⎢ ⎥ ý ý ý ⎢ ⎥ Inertial ⎢ ⎥ b ý −r ý Properties of Ô ⎢ ⎥ J =J (θ)= ò Rigid Body sl ò ý −Ô ý ⎢ ⎥ ⎢ ⎥ −s ý ý ò Dynamics of ⎢ ⎥ c ý ý an Openchain ⎣ ⎦ ò Manipulator ⎡ −l c −r c ý ý ⎤ ò ò ò òç ⎢ ⎥ NewtonEuler ý l s ý ⎢ Ô ç ⎥ Equations ⎢ ⎥ b ý −r −l c −r ò Ô ç ò J =J (θ)=⎢ ⎥ ç sl Coordinate ç ⎢ ý −Ô −Ô ⎥ invariant ⎢ ⎥ −s ý ý òç ⎢ ⎥ algorithms for ⎣ c ý ý ⎦ robot òç dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator ò  M M M ÔÔ Ôò Ôç T T T Chapter ¥ M(θ)= M M M =J M J +J M J +J M J òÔ òò òç Ô Ô ò ò ç ç Ô ò ç Robot M M M çÔ çò çç Dynamics and ò ò ò ò ò ò ò Control M =I s +I s +I +I c +I c +m r c +m (l c +r c ) ÔÔ yò yç zÔ zò zç ò ç Ô ò ò òç ò òç ò òç Ô ò Lagrangian Equations M =M =M =M = ý Ôò Ôç òÔ çÔ Inertial ò ò ò M =I +I +m l +M r +m r +òm l r c Properties of òò xò xç ò ò ç ç Ô ò ç Ô Ô ò Rigid Body ò M =I +m r +m l r c Dynamics of òç xç ç ç Ô ò ç ò an Openchain ò Manipulator M =I +m r +m l r c çò xç ç ç Ô ò ç ò NewtonEuler ò Equations M =I +m r çç x ç ç ò Coordinate invariant n n ∂M ∂M Ô ∂M ij kj algorithms for ik k ˙ ˙ ˙ C (θ,θ)= Γ θ = Œ + − ‘θ Q Q robot ij k k ij ò ∂θ ∂θ ∂θ dynamics k j i k=Ô k=Ô Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator òâ ◻AdditionalPropertiesofthedynamicsin Chapter ¥ termsofPOE: Robot Dynamics and Control De�ne: Lagrangian Equations ⎧ −Ô ⎪ Ad ij ˆ ⎪ ξ θ ˆ ξ θ ⎪ j+Ô j+Ô i i ⎪ e ⋯e Inertial ⎪ Properties of A =⎨ ij I i=j Rigid Body ⎪ ⎪ ⎪ ⎪ Dynamics of ⎪ý ij ⎩ an Openchain Manipulator J(θ)= Ad −Ô A ξ ⋯A ξ ý⋯ý i iÔ Ô ii i g (ý) sl NewtonEuler i Equations ′ T th M = Ad −Ô M Ad −Ô (intertiaofi linkinS) i i g g Coordinate sl (ý) sl (ý) i i invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.3 Dynamics of Openchain Manipulator òÞ Propertyò: n Chapter ¥ T T ′ Robot M (θ)= ξ A M A ξ Q ij lj j i li l Dynamics and l=max(i,j) Control n Lagrangian ∂M Ô ∂M ∂M ij kj ik Equations ˙ ˙ C (θ,θ)= Œ + − ‘θ ij Q k ò ∂θ ∂θ ∂θ k j i Inertial k=Ô Properties of where Rigid Body n ∂M ij T T ′ Dynamics of = ‰A ξ ,ξ A M A ξ Q k−Ô,i i k lj j lk l an Openchain ∂θ k Manipulator l=max(i,j) T T ′ NewtonEuler +ξ A M A A ξ ,ξ Ž j i lk k−Ô,j k Equations li l Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.4 NewtonEuler Equations ò— ◻NewtonEulerequationsinbodyframe: NewtonsEquation: Chapter ¥ Robot d Dynamics and f = (mp˙)=mp¨ Control dt Lagrangian Equations Spatialangularmomentum: Inertial Properties of ′ s b T s Rigid Body I ⋅ω =R(I⋅ω )=R⋅I⋅R ⋅ω Dynamics of ´¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¶ ′ an Openchain I Manipulator NewtonEuler Equations r T Coordinate invariant algorithms for robot dynamics f Lagrange’s S Equations with Constraints g∶(R,p)Chapter 4 Robot Dynamics and Control 4.4 NewtonEuler Equations òÀ d d s s b T s s s b T s b T s ˙ ˙ ˙ τ= (I ω)= (RI R ω )=I ω +RI R ω +RI R ω Chapter ¥ dt dt Robot Dynamics and s s T s s b T s s s s s s s ˙ ˙ ˆ ˙ =I ω + RR I ω −RI R ω ω =I ω +ω ×(I ω) Control ± s Lagrangian ωˆ Equations ◻Transformationofallequationsto Inertial Properties of twist/wrenchinbodyframe: Rigid Body d d b b b T s T b b Dynamics of ˙ ˙ ˙ ˙ ˙ (mp)= ‰mRv Ž=mRv +mRv ,R f =mR Rv +mv an Openchain dt dt Manipulator b b b b ⇒ f =mω ×v˙ +mv˙ , NewtonEuler Equations d b T s T b b b b b b b Coordinate τ =R τ =R (RI ω )=I ω˙ +ω ×I ω invariant dt algorithms for b b b b robot mI ý v˙ ω ×mv f b ⇒  + = =F (∗) dynamics b b b b b b ý I ω˙ ω ×I ω τ Lagrange’s ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶´¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¶ Equations with M b V ConstraintsChapter 4 Robot Dynamics and Control 4.4 NewtonEuler Equations çý ˆ ˆ ˆ ˆ ˆ ˆ ˆ De�ne:se(ç)×se(ç)↦se(ç),(ξ ,ξ )↦ ξ ξ − ξ ξ ≜ ξ,if Ô ò Ô ò ò Ô Chapter ¥ Robot ˆ ω v ˆ i i ξ = ,i=Ô,ò Dynamics and i ý ý Control then Lagrangian ∧ (ω ×ω ) ωˆ v −ωˆ v ˆ Ô ò Ô ò ò Ô Equations ξ= = ad ⋅ ξ ξ ò i ý ý Inertial where Properties of ˆ ˆ Rigid Body ω v Ô Ô ad =  ξ Ô ý ωˆ Ô Dynamics of an Openchain us Manipulator b ˙ (∗)⇔M V −ad bM V =F b b V b b NewtonEuler Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.4 NewtonEuler Equations çÔ ◻CoordinateinvarianceofNewtonEuler equations: Chapter ¥ Robot g Dynamics and r ý Control B g T ò ˙ Lagrangian M V −ad M V =F Ô Ô Ô Ô Ô V Ô Equations V = Ad V Ô g ò ý Inertial T Ad F =F ⇒ Properties of Ô ò g ý g Rigid Body Ô T −Ô −Ô T A F =(Ad ) F =(Ad ) F Ô ò ò g g ý ý Dynamics of an Openchain −T −Ô Manipulator M = Ad M Ad Ô ò g g ý ý NewtonEuler −T −Ô T −T −Ô −T ˙ ⇒ Ad M Ad Ad V −ad Ad M Ad Ad V = Ad F ò g ò ò g Ô ò Equations g g ý ˙ g g ý g ý ý (Ad V ) ý ý ý g ò ý Coordinate invariant −Ô algorithms for Sincead = Ad ad Ad ,wehave Ad V g V g ý g ý ý robot dynamics Lagrange’s ˙ M V −ad M V =F ò ò V ò ò ò ò Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics çò C C n n+Ô C ò C Ô Chapter ¥ Robot Dynamics and Control C ý Lagrangian Equations th C: Frame�xedtolinki,locatedalongthei axis i Inertial F: Generalizedforcelinki−Ôexertingonlinki,expressedinC Properties of i i Rigid Body τ : Jointtorqueoflinki i Dynamics of g : Transformation ofC relativetoC i−Ô,i i i−Ô an Openchain ˆ′ ˆ ξ θ ξ θ i i i i Manipulator g (θ )=e ⋅g (ý)=g (ý)e i−Ô,i i i−Ô,i i−Ô,i ′ th NewtonEuler ξ = Ad −Ô ⋅ ξ : i axisinC frame. i i g (ý) i Equations i−Ô,i ⎡ ⎤ ⎧ ⎪⎢ ⎥ Coordinate ý ⎪ ⎪ ⎢ ⎥ invariant ⎪ ∶ Revolute joint. ⎪⎢ ⎥ ⎪ algorithms for ⎪ ⎢ z ⎥ ⎪ i robot ⎣ ⎦ ξ=⎨ dynamics ⎡ ⎤ ⎪ ⎢ ⎥ ⎪ z i ⎪ ⎢ ⎥ Lagrange’s ⎪ ∶ Prismaticjoint. ⎪ ⎪⎢ ⎥ Equations with ⎪ ⎪⎢ ý ⎥ Constraints ⎩ ⎣ ⎦Chapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics çç −Ô Chapter ¥ ˆ ˙ ˙ ⇒g ⋅g = ξ ⋅θ i−Ô,i i i i−Ô,i Robot Dynamics and Control M : MomentofinertiainC i i Lagrangian mI −m ˆr m : Massoflinki i i i i Equations M =  i ò I : Angularmomentofinertia mˆr I −mrˆ i i i i i i Inertial Properties of g =g g i i−Ô i−Ô,i Rigid Body −Ô −Ô ˆ ˆ ˆ ˙ ˙ Dynamics of V =g ⋅g =g V g + ξ θ i i i−Ô i−Ô,i i i i i−Ô,i an Openchain Manipulator ˙ V = Ad −Ô V + ξ θ i i−Ô i i g i−Ô,i NewtonEuler Equations ˙ −Ô −Ô −Ô ˙ ˆ ¨ ˆ ˆ ˆ ˆ V =g˙ V g +g V g˙ +g V g + ξ θ i i−Ô,i i−Ô i−Ô,i i−Ô,i i−Ô i−Ô,i i−Ô,i i−Ô i−Ô,i i i Coordinate −Ô −Ô −Ô −Ô invariant ˆ ˆ ˙ ˙ =−g g g V g +g V g g g i−Ô,i i−Ô i−Ô,i i−Ô i−Ô,i i−Ô,i i−Ô,i i−Ô,i i−Ô,i i−Ô,i algorithms for robot −Ô ˙ ˆ ¨ dynamics ˆ +g V g + ξ θ i−Ô i−Ô,i i i i−Ô,i Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics ç¥ ˆ ∧ ∧ˆ ∧ ˆ ˙ ˙ ¨ ˙ =−ξ θ(Ad −Ô V ) +(Ad −Ô V ) ξ θ +(Ad −Ô V ) + ξ θ i i i−Ô i−Ô i i i−Ô i i g g g i−Ô,i i−Ô,i i−Ô,i ¨ ˙ ˙ ⇒V = ξ θ +Ad −Ô V −ad (Ad −Ô V ) i i i i−Ô ˙ i−Ô g g ξ θ i i Chapter ¥ i−Ô,i i−Ô,i Robot ◻ForwardRecursion: Dynamics and Control g ˙ i= ý∶ V = ý,V =  ý ý ý Lagrangian Equations ˆ ⎧ ξ θ i i Inertial ⎪ g =g (ý)e ⎪ i−Ô,i i−Ô,i ⎪ Properties of ⎪ ⎪ Rigid Body ⎪ ˙ ⎨ V = Ad −Ô V +ξ θ i i−Ô i i g Dynamics of i−Ô,i ⎪ ⎪ an Openchain ⎪ ⎪ ¨ ⎪ ˙ ˙ Manipulator V = ξ θ +Ad −Ô V −ad (Ad −Ô V ) ⎪ ˙ i i i i−Ô i−Ô g g ξ θ ⎩ i i i−Ô,i i−Ô,i NewtonEuler Equations ◻BackwardRecursion: Coordinate invariant F : Endešectorwrench,g : transformfromtoolframetoC n+Ô n,n+Ô n algorithms for T T robot ˙ F = Ad ⋅F +MV −ad ⋅MV i −Ô i+Ô i i i i V dynamics g i i,i+Ô Lagrange’s T τ = ξ ⋅F Equations with i i i ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics ç  Chapter ¥ ˙ V = Ad −Ô ⋅V +ξ θ Ô ý Ô Ô Robot g ý,Ô Dynamics and T T Control ˙ F = Ad ⋅F +M V −ad ⋅(M V ) −Ô n n+Ô n n n n V g n n,n+Ô Lagrangian Equations De�ne: ⎡ ⎤ ˙ V θ ξ ý ý Inertial ⎢ ⎥ Ô Ô Ô ân×Ô n ân×n ˙ ⎢ ⎥ Properties of V= ⋮ ∈R ,θ= ⋮ ∈R , ξ= ý ⋱ ý ∈R ⎢ ⎥ Rigid Body V ⎢ ˙ ⎥ ý ý ξ n θ n n ⎣ ⎦ Dynamics of ⎡ ⎤ Ad −Ô an Openchain g ⎢ ⎥ ý,Ô F τ Ô Ô Manipulator ⎢ ⎥ ân×Ô n ân×â ý F= ⋮ ∈R ,τ= ⋮ ∈R ,P =⎢ ⎥∈R ý NewtonEuler ⎢ ⎥ τ ⋮ F n n ⎢ ⎥ Equations ý ⎣ ⎦ T â×ân Coordinate P =ý⋯ý Ad ∈R t −Ô g invariant n,n+Ô algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics çâ ˙ V = Ad −Ô ⋅V +ξ θ Ô ý Ô Ô g ý,Ô ˙ V −Ad −ÔV = ξ θ ò Ô ò ò g Ô,ò Chapter ¥ Robot ⋮ Dynamics and Control ˙ V −Ad −Ô V = ξ θ Lagrangian n n−Ô n n g n−Ô,n Equations ⎡ ⎤ I ý ⋯ ý ⎢ ⎥⎡ ⎤ V Ô Inertial ⎢ −Ad −Ô I ⋱ ⋮ ⎥⎢ ⎥ g V Properties of ⎢ Ô,ò ⎥⎢ ò ⎥ ⇒ Rigid Body ⎢ ⎥⎢ ⎥ ý ⋱ ⋱ ý ⋮ ⎢ ⎥⎢ ⎥ V ⎢ ý ý −Ad −Ô I ⎥⎣ ⎦ Dynamics of n g n−Ô,n ⎣ ⎦ an Openchain ´¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¶ ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Manipulator V −Ô G NewtonEuler ⎡ ⎤ Equations ˙ ⎡ ⎤ ⎡ ⎤ Ad −Ô θ ξ ⎢ ⎥ Ô g Ô ⎢ ⎥ ⎢ ⎥ ý,Ô ⎢ ⎥ ˙ Coordinate ⎢ ⎥ ⎢ ⎥ ξ ⎢ θ ⎥ ý ò ò =⎢ ⎥V +⎢ ⎥ invariant ý ⎢ ⎥ ⋱ ⎢ ⎥ ⎢ ⎥ ⋮ ⋮ algorithms for ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ξ ⎢ ˙ ⎥ robot ý ⎣ ⎦ ⎣ n ⎦ θ ⎣ n ⎦ dynamics ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ´¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¶ P ξ Lagrange’s ý ˙ θ Equations with ˙ Constraints usV =GP V +Gξθ ý ýChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics çÞ where Chapter ¥ ⎡ ⎤ Robot I ý ý ⋯ ý ⎢ ⎥ Dynamics and Ad −Ô I ý ⋯ ý ⎢ ⎥ g Control Ô,ò ⎢ ⎥ ân×ân ⎢ ⎥ Ad −Ô Ad −Ô I ⋱ ⋮ G= ∈R Lagrangian g g ⎢ ⎥ Ô,ç ò,ç Equations ⎢ ⎥ ⋮ ⋮ ⋱ I ý ⎢ ⎥ ⎢ ⎥ Inertial Ad −Ô Ad −Ô ⋯ Ad −Ô I g g g ⎣ ⎦ Ô,n ò,n n−Ô,n Properties of Rigid Body Dynamics of ¨ ˙ ˙ V = ξ θ +Ad −ÔV −ad (Ad −ÔV ) an Openchain ˙ Ô Ô Ô ý ý g ξ θ g ý,Ô Ô Ô ý,Ô Manipulator NewtonEuler Equations ¨ ˙ ˙ V −Ad −ÔV = ξ θ −ad (Ad −ÔV) ò Ô ò ò ˙ Ô g g ξ θ ò ò Ô,ò Ô,ò Coordinate invariant algorithms for robot ¨ ˙ ˙ V −Ad −Ô V = ξ θ −ad (Ad −Ô V ) ˙ dynamics n n−Ô n n n−Ô g g ξ θ n−Ô,n n n n−Ô,n Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics ç— I ý ⋯ ý ⎡ ⎤ ⎡ ⎤ ¨ ⎡ ˙ ⎤ ⎡ Ad ⎤ ⎡ ⎤ V −Ô θ ⎢ ⎥ Ô ξ ⎢ Ô ⎥ g Ô ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ −Ad −Ô I ⋱ ⋮ ⎢ ⎥ ý,Ô ⎢ ⎥ Chapter ¥ g ⎢ ˙ ⎥ ⎢ ⎥ ⎢ ⎥ ¨ ⎢ Ô,ò ⎥ V ˙ ξ ⎢ θ ⎥ ò ý ò ò ⎢ ⎥=⎢ ⎥V +⎢ ⎥ ý Robot ⎢ ⎥ ⎢ ⎥ ý ⋱ ⋱ ý ⋱ ⎢ ⋮ ⎥ ⎢ ⎥ ⎢ ⎥ ⋮ ⋮ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ Dynamics and ⎢ ý ý −Ad I ⎥ ⎢ ⎥ −Ô ˙ ξ ¨ ⎣ V ⎦ ⎣ ý ⎦ ⎣ n ⎦ g n θ ⎣ ⎦ ⎣ n ⎦ n−Ô,n Control ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ξ P −Ô ý Lagrangian G Equations Inertial ⎡ ⎤ −ad ý ⋯ ý ˙ ⎡ ⎤ Ad ⎢ ξ θ ⎥ −Ô Ô Ô ⎢ g ⎥ Properties of ⎢ ⎥ ý,Ô ý −ad ⋱ ⋮ ˙ ⎢ ⎥ ⎢ ξ θ ⎥ Rigid Body ò ò ý + ⎢ ⎥V ý ⎢ ⎥ ⋮ ⋱ ⋱ ý ⎢ ⎥ ⋮ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ Dynamics of ý ⋯ ý −ad ˙ ⎣ ý ⎦ ⎣ ξ θ ⎦ n n an Openchain ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Manipulator ad ˙ ξθ NewtonEuler Equations ⎡ ⎤⎡ ý ý ⋯ ý ⎤ Coordinate −ad ý ⋯ ý ˙ ξ θ ⎢ Ô Ô ⎥⎢ ⎥⎡ ⎤ V Ô Ad −Ô ý ⋱ ⋮ invariant ⎢ ⎥⎢ ⎥⎢ ⎥ g ý −ad ⋱ ⋮ ˙ V ⎢ ⎥⎢ Ô,ò ⎥⎢ ò ⎥ ξ θ + ò ò algorithms for ⎢ ⎥⎢ ⎥⎢ ⎥ ý ⋱ ⋱ ý ⋮ ⋮ ⋱ ⋱ ý ⎢ ⎥⎢ ⎥⎢ ⎥ robot V ⎢ ⎥⎢ ý ý Ad −Ô ý ⎥⎣ n ⎦ ý ⋯ ý −ad ˙ g dynamics ⎣ ξ θ ⎦⎣ ⎦ n n n−Ô,n ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Lagrange’s Γ Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics çÀ us Chapter ¥ Robot Dynamics and ¨ ˙ ˙ Control V=G⋅ ξθ+G⋅P V +G⋅ad P V +G⋅ad ΓV ˙ ˙ ý ý ý ý ξθ ξθ Lagrangian Equations Finallythebackwardrecursion: Inertial Properties of Rigid Body T T ˙ F = Ad F +M V −ad ⋅(M V ) −Ô n n+Ô n n n n V g n n,n+Ô Dynamics of an Openchain T T ˙ Manipulator F = Ad F +M V −ad ⋅(M V ) n−Ô −Ô n n−Ô n−Ô n−Ô n−Ô V g n−Ô n−Ô,n NewtonEuler Equations ⋮ Coordinate T T ˙ F = Ad F +M V −ad (M V) −Ô invariant Ô ò Ô Ô Ô Ô V g Ô Ô,ò algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics ¥ý ⎡ T ⎤ ⎢ I −Ad ý ý ⎥ ⎡ ⎤ −Ô ˙ ⎢ g ⎥⎡ ⎤ ⎡ ⎤⎢ ⎥ V F M Ô Ô,ò Ô ⎢ ⎥⎢ ⎥ ⎢ Ô ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ Chapter ¥ ˙ ý I ⋱ ý F M ò V ⎢ ⎥⎢ ⎥ ⎢ ò ⎥⎢ ⎥ ò ⇒ = Robot ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ T ⋱ ⋮ ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⋮ ⎥ ⋮ ⋱ ⋱ −Ad Dynamics and −Ô ⎢ ⎥⎢ ⎥ ⎢ ⎥⎢ ⎥ g M ⎢ n−Ô,n ⎥⎣ F ⎦ ⎣ ⎦⎢ ⎥ n ˙ Control n V ⎢ ⎥ ⎣ ⎦ n ⎣ ý ⋯ ý I ⎦ ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Lagrangian M Equations ý ⎡ ⎤ T ⎡ ⎤ ⎢ ⎥ −ad ⋮ ⎢ ⎥ M V Inertial V Ô Ô Ô ⎢ ⎥ ⎢ ⎥ Properties of ⋱ ⋱ +⎢ ý ⎥F +   ⋮ n+Ô ⎢ ⎥ Rigid Body ⎢ ⎥ T T M ⎢ ⎥ V n n ⎢ ⎥ −ad Ad −Ô ⎣ V ⎦ n g ⎣ ⎦ Dynamics of n,n+Ô ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ an Openchain ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ T Manipulator ad T V P t NewtonEuler Equations T T T T T ˙ Coordinate F=G MV+G P F +G ⋅ad MV n+Ô t V invariant ± algorithms for F t robot dynamics Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics ¥Ô T τ= ξ ⋅F T T T T T T T T Chapter ¥ ˙ τ= ξ G MV+ξ G P F +ξ G ⋅ad MV t t V Robot Dynamics and T T ¨ ˙ Control = ξ G M(Gξθ+GP V +G⋅ad P V +G⋅ad ΓV) ý ý ˙ ý ý ˙ ξθ ξθ ⇒ Lagrangian T T T T T T Equations + ξ G P F + ξ G ⋅ad MV t t V Inertial T T T T T T ¨ ˙ = ξ G MGξθ+ξ G MGP V + ξ G MG⋅ad ΓV Properties of ý ý ˙ ξθ Rigid Body T T T T T T + ξ G P F + ξ G ⋅ad MV Dynamics of t t V an Openchain Finallyweget: Manipulator ˙ M(θ) C(θ,θ) NewtonEuler ³¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹·¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹µ ³¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹·¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹µ Equations T T T T T ¨ ˙ ξ G MGξθ+ ξ G (MG⋅ad Γ+ad M)Gξθ Coordinate ˙ V ξθ invariant T T T T T ˙ algorithms for +ξ G MGP V +ξ G P F = τ ý ý t t robot ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ´¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¶ dynamics T ϕ(θ) J (θ) t Lagrange’s Equations with ConstraintsChapter 4 Robot Dynamics and Control 4.5 Coordinateinvariant algorithms for robot dynamics ¥ò Chapter ¥ T ¨ ˙ M(θ)θ+C(θ,θ)+ϕ(θ)+J (θ)F = τ Robot t t Dynamics and Control T T Lagrangian M(θ)= ξ G MGξ Equations T T T ˙ ˙ Inertial C(θ,θ)= ξ G (MGad Γ+ad M)Gξθ ˙ V ξθ Properties of Rigid Body T T ˙ ϕ(θ)= ξ G MGP V ý ý Dynamics of an Openchain J =P Gξ t t Manipulator Propertyç: NewtonEuler n Equations Γ = ý, Coordinate −Ô ò n−Ô invariant G=(I−Γ) =I+Γ+Γ +⋯+Γ algorithms for robot I+ΓG=G dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥ç De�nition:Holonomicconstraints Given generalizedcoordinatesq=(q ,...,q )∈E,aholonomic Ô n Chapter ¥ Robot constraint isasetofconstraint equations: Dynamics and Control h(q)= ý,i= Ô,...,k i −Ô Lagrangian Q=h (ý)isamanifoldofdimn−k≜miftheconstraintsare Equations linearlyindependent. Inertial Properties of T Q∶V ∈T ESdh ⋅V = ý,∀i= Ô,...,k⊂T E q q i q Rigid Body subspaceofpermissiblevelocities. Dynamics of ∗ ⊥ ∗ an Openchain T Q ∶f ∈T ES⟨f,v⟩= ý,∀V∈T Q= spandh ,...,dh q Ô k q q Manipulator subspaceofconstraintforces. NewtonEuler Equations Coordinate De�nition:Constraintforces invariant T ∂h algorithms for Γ= ⋅λ ∂q robot k dynamics λ∈R isthevectorofrelativemagnitudesofconstraintforces. Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥¥ De�nition:Pfa›anConstraints Chapter ¥ APfa›anconstrainthastheform: Robot k×n Dynamics and ˙ A(q)q= ý,A(q)∈R Control Given aPfa›anconstraint, Lagrangian Equations Δ =V ∈T ESA(q)⋅V = ý⊂T E q q q q q Inertial Distributionofpermissiblevelocities. Properties of Rigid Body Dynamics of an Openchain De�nition: Manipulator A(q)q˙= ýisholonomic(orintegrable)iš Δ isaninvolutive NewtonEuler q Equations distribution,oriš Coordinate −Ô ∃h ∶ E↦R,i= Ô,...,ks.t. Δ =T Q,Q=h (ý) invariant i q q algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥  §Constraintforces: Chapter ¥ T k Robot Γ=A (q)⋅λ,λ∈R Dynamics and Control Lagrangian §Kineticenergy: Equations Ô T Inertial T(q,q˙)= q˙ ⋅M(q)⋅q˙ Properties of ò Rigid Body Dynamics of an Openchain §Potentialenergy: Manipulator NewtonEuler V(q) Equations Coordinate §Lagrangian: invariant algorithms for robot L(q,q˙)=T(q,q˙)−V(q) dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥â Chapter ¥ ◻Lagrange’sequationswithconstraints: Robot Dynamics and T M(q)q¨+C(q,q˙)+N(q)+A (q)λ=F Control Lagrangian Equations Inertial ◻Explicitsolutionforconstraintforces: Properties of Rigid Body ˙ A(q)q¨+A(q)q˙= ý Dynamics of −Ô T −Ô an Openchain ˙ (AM A )λ=AM (F−C−N)+Aq˙ Manipulator −Ô T −Ô −Ô NewtonEuler ˙ λ=(AM A ) (AM (F−C−N)+Aq˙) Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥Þ ◇Example: Chapter ¥ ò ò ò x +y =l Robot Dynamics and x˙ Control x y  = ý y˙ y ² Lagrangian Equations A(q) Ô Inertial ò ò x ˙ ˙ ˙ L(q,q)= m(x +y )−mgy Properties of l ò Rigid Body x¨ x m ý ý Dynamics of   + + λ= ý mg y ¨ θ ý m y an Openchain (x,y) Manipulator −Ô T −Ô −Ô ˙ λ=(AM A ) (AM (F−C−N)+Aq˙) NewtonEuler m Equations ò ò =− (gy+x˙ +y˙ ) ò Coordinate l mg mg m invariant x ò ò =Z λZ= y+ (x˙ +y˙ ) algorithms for y robot l l dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥— ◻Lagranged’Alembertformulation: Given the Pfa›an constraint A(q)q˙ = ý and virtual displacement Chapter ¥ k δq∈R ,wehave: Robot Dynamics and Control eoremÔ(D’alembertPrinciple): Lagrangian Equations Forcesofconstraintsdonovirtualwork T Inertial (A (q)λ)⋅δq= ýforA(q)δq=ý Properties of Rigid Body Dynamics of an Openchain Manipulator ÔÞÔÞ–ÔÞ—ç NewtonEuler Equations eoremò(Lagranged’AlembertEquation): Coordinate d ∂L ∂L invariant Œ − −τ‘⋅δq= ý,A(q)δq= ý algorithms for dt ∂q˙ ∂q robot dynamics Lagrange’s Equations with Constraints ÔÞçâÔ—ÔçChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ¥À k×k LetA(q)=A(q) A (q),andA (q)∈R isinvertible,then Chapter ¥ Ô ò ò Robot n−k δq ∈R arefreevariables: Dynamics and Ô Control −Ô Lagrangian δq =−A (q)A(q)δq ò Ô Ô Equations ò Inertial d ∂L ∂L Properties of ⇒Œ − −τ‘⋅δq Rigid Body dt ∂q˙ ∂q Dynamics of an Openchain d ∂L ∂L d ∂L ∂L Manipulator =Œ − −τ ‘⋅δq +Œ − −τ ‘⋅δq Ô Ô ò ò dt ∂q˙ ∂q dt ∂q˙ ∂q Ô Ô ò ò NewtonEuler Equations d ∂L ∂L d ∂L ∂L −Ô =Œ − −τ ‘⋅δq +Œ − −τ ‘⋅(−A A)δq Coordinate Ô Ô ò Ô Ô ò invariant dt ∂q˙ ∂q dt ∂q˙ ∂q Ô Ô ò ò algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  ý Chapter ¥ n−k Robot Asδq ∈R isfree, Ô Dynamics and Control Lagrangian d ∂L ∂L d ∂L ∂L T −T Equations Œ − −τ‘−A A Œ − −τ ‘= ý Ô ò Ô ò dt ∂q˙ ∂q dt ∂q˙ ∂q Ô Ô ò ò Inertial Properties of Rigid Body Lagranged’Alembertequation Dynamics of an Openchain Manipulator NewtonEuler Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  Ô ◇Example:Dynamicsofarollingdisk Chapter ¥ Robot Pfa›anconstraint: Dynamics and ⎧ Control ˙˙ ϕ ⎪ x˙−ρcosθϕ= ý ⎪ Lagrangian ⎨ θ ⎪ ˙ Equations ˙ ⎪ y−ρsinθϕ= ý (x,y) ⎩ Inertial Properties of Rigid Body Ô ý ý −ρcosθ ⇒A(q)q˙= q˙= ý ý Ô ý −ρsinθ Dynamics of an Openchain Ô ò ò Ô ò Ô ò ¨ ˙ Manipulator L(q,q˙)= m(x˙ +y˙ )+ I θ + I ϕ Ô ò ò ò ò NewtonEuler Lagranged’Alembertequation: Equations ⎡ m ⎤ ⎡ ⎤ ý ⎛⎢ ⎥ ⎢ ⎥⎞ m Coordinate ý ⎢ ⎥ ⎢ ⎥ q¨− ⋅δq= ý invariant ⎢ I ⎥ ⎢ ⎥ Ô τ θ ⎝⎢ ⎥ ⎢ ⎥⎠ algorithms for τ I ϕ ò ⎣ ⎦ ⎣ ⎦ robot dynamics whereA(q)⋅δq= ý. Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  ò As δx= ρcosθ⋅δϕ œ Chapter ¥ δy= ρsinθ⋅δϕ Robot Dynamics and theequation ofmotion is: Control ¨ τ x¨ I ý θ δθ Lagrangian ý ý θ Ô ‹  +  − �⋅ = ý τ Equations mρc mρs y¨ ¨ ϕ θ θ ý I δϕ ò ϕ Inertial ¨ x¨ τ ý ý I ý θ Ô θ Properties of ⇒  +  =  τ mρc mρs ¨ ϕ Rigid Body θ θ y ý I ¨ ò ϕ Dynamics of As an Openchain ⎧ ¨ ˙˙ ⎪ x¨= ρcosθ⋅ϕ−ρsinθ⋅θϕ Manipulator ⎪ ⎨ NewtonEuler ¨ ˙˙ ⎪ ⎪y¨= ρsinθ⋅ϕ+ρcosθ⋅θϕ Equations ⎩ ¨ I ý τ Coordinate Ô θ θ ⇒  =  ò τ invariant ϕ ¨ ý I +mρ ò ϕ algorithms for robot Solve for(θ(t),ϕ(t)),andthensolvefor(x(t),y(t))from: dynamics ⎧ ˙ ⎪˙ x= ρcosθ⋅ϕ Lagrange’s ⎪ ⎨ ⇐ Ôstorderdišerentialequation Equations with ⎪ ˙ Constraints y˙= ρsinθ⋅ϕ ⎪ ⎩Chapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  ç ◻Natureofnonholonomicconstraints: Chapter ¥ ò Considerq=(r,s)∈R ×RwithPfa›anconstraint, Robot Dynamics and T ò ˙s+a (r)˙r= ý,a(r)∈R Control Lagrangian Lagrangian Equations L=L(r,˙r,˙s) Inertial Properties of andconstrainedLagrangian Rigid Body T L (r,˙r)=L(r,˙r,−a (r)r˙) c Dynamics of an Openchain ⇒Lagrange’sequation: Manipulator d ∂L ∂L c c NewtonEuler − = ý,i= Ô,ò Equations dt ∂r˙ ∂r i i Coordinate ⎛ ∂a ⎞ d ∂L ∂L ∂L ∂L j invariant ⇒ ‹ −a(r) �− − ˙r = ý Q i j algorithms for dt ∂r˙ ∂˙s ⎝∂r ∂˙s ∂r ⎠ i i i j robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  ¥ ⎛ ∂a ⎞ d ∂L ∂L d ∂L ∂L ∂L j ⇒‹ − �−a(r)‹ − �= a˙ (r)− r˙ (∗) Chapter ¥ Q i i j Robot dt ∂r˙ ∂r dt ∂˙s ∂s ∂˙s ⎝ ∂r ⎠ i i i j Dynamics and Control ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ ≠ ý Lagrangian Equations Iftheconstraintisholonomic,i.e. Inertial Properties of ∂h Rigid Body a(r)= forsomeh∶ E↦R i ∂r i Dynamics of an Openchain Manipulator thenRHS(righthandside)of(∗)equals NewtonEuler Equations ò ò ⎛ ⎞ ∂L ∂ h ∂ h Coordinate ˙r − r˙ = ý invariant Q Q j j ˙ algorithms for ∂s ⎝ ∂r ∂r ∂r ∂r ⎠ i j j i j j robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints    ◻Metric,dualityandorthogonalityonT E: q n x T Q q Chapter ¥ Robot ⋯ Dynamics and Ô Ô q x T Control K= q˙ M(q)q˙ ò Lagrangian Equations Ô −Ô Q= h (ý) = ≪q˙,q˙≫ M Inertial ò Properties of Rigid Body Dynamics of ⊥ T an Openchain T Q =V ∈T ES≪V ,V ≫ =V MV = ý,∀V ∈T Q q Ô q Ô ò M ò ò q Ô Manipulator ∗ ⊥ ∗ T Q =f ∈T ES⟨f,V⟩= ý,∀V ∈T Q∶ constraint forces NewtonEuler q q q Equations ⊥ ∗ ∗ ∗ ⊥ T E=T Q⊕T Q ,T E=T Q⊕T Q q q q Coordinate q q q invariant algorithms for De�nition: robot b ∗ ♭ T dynamics M ∶T E↦T E,⟨M V ,V ⟩=V MV =≪V ,V ≫ q Ô ò ò Ô ò M q Ô Lagrange’s −Ô ♯ ∗ ♯ ♭ Equations with M ∶T E↦T E,M =M q q ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  â Chapter ¥ Robot Reciprocal Dynamics and Control Lagrangian Equations Inertial Properties of Rigid Body Property¥: Dynamics of ∂ ∗ an Openchain Underthebasis anddq ,i= Ô,...,nofT EandT E i q q Manipulator ∂q i ♭ ♯ respectively,thematrixrepresentationofM andM isM and NewtonEuler Equations −Ô M respectively. Coordinate invariant Property : algorithms for robot ♯ ∗ M (T Q)=T Q dynamics q q Lagrange’s ♯ ∗ ⊥ ⊥ M (T Q )=T Q Equations with q q ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  Þ Given k Chapter ¥ h∶E↦R ,m=n−k Robot Dynamics and k h ≜T h∶T E↦T R Control ∗ q q h(q) Lagrangian ∗ ∗ ∗ k ∗ h ≜T h∶T R ↦T E Equations q h(q) q Inertial wehave Properties of Rigid Body Propertyâ: ⊥ k ∗ ∗ k ∗ ⊥ Dynamics of kerh =T Q,h (T Q )=T R ,h (T R )=T Q ∗ q ∗ q h(q) h(q) q an Openchain Manipulator ∗ h ∗ ∗ k T E NewtonEuler T R q q Equations Coordinate ♯ ♯ ∗ ♯ invariant ♯ M M =h ○M ○h ∗ M ò ò algorithms for robot dynamics k T E T R q Lagrange’s h(q) Equations with h ∗ ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  — ∗ ∗ ⊥ LemmaÔ: emap(I−P )∶T E↦T Q givenby ω q q ∗ ♭ ♯ Chapter ¥ (I−P )=h ○M ○h ○M ω ∗ ò Robot Dynamics and isawellde�nedprojectionmap,withtheproperty: Control ∗ (I−P )f = ý,∀f ∈T Q ω Ô Ô q Lagrangian Equations ∗ ⊥ (I−P )f =f ,∀f ∈T Q ω ò ò ò q Inertial Properties of Proof: Rigid Body ∗ ♯ Given f ∈ T Q,M (f )∈ T Q= kerh , then(I−P )(f )= ý. For Ô Ô q ∗ ω Ô q Dynamics of ∗ ⊥ n−m ∗ an Openchain f ∈T Q ,∃λ∈R s.t. f =h λ,and ò ò q Manipulator ∗ ♭ ♯ ∗ (I−P )f =h M h M h λ NewtonEuler ω ò ∗ ò Equations ∗ =h λ=f ò Coordinate ∗ ∗ invariant ⇒ P ∶T E↦T Q ω q q algorithms for robot isawellde�nedprojectionmap. Similarly, dynamics ♯ ∗ b P ∶ T E↦T Q,P =I−M h M h T q q T ∗ ò Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints  À ⊥ and(I−P )∶T E↦T Q areprojection maps. T q q Chapter ¥ Lemmaò: Robot Dynamics and P M=MP ω T Control ∗ P h =h P = ý Lagrangian ω ∗ T Equations T P =P T Inertial ω Properties of Rigid Body Dynamics of Fornonholonomicconstraints: an Openchain Manipulator h ← A(q) ∗ NewtonEuler ∗ ∗ h ← A (q) Equations Coordinate T Q← Δ q q invariant ∗ ⊥ algorithms for T Q ← spana (q),i= Ô,...,k i q robot dynamics application inhybridvelocity/forcecontrol. Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints âý ◻Lagrange’sequationsofmotion: T ¨ ˙ Chapter ¥ M(q)q+C(q,q)+N+A (q)λ= τ Robot −Ô T −Ô −Ô Dynamics and ˙ ˙ λ=(AM A ) (AM (τ−C−N)+Aq) Control T −Ô T Lagrangian ˙ M(q)q¨+A (AM A )Aq˙+P C+P N=P τ ω ω ω Equations T −Ô T −Ô −Ô Inertial P =I−A (AM A ) AM ω Properties of Rigid Body ˜ Dynamics of C=P C ω an Openchain Manipulator ˜ N=P N ω NewtonEuler ˜ Equations τ=P τ ω Coordinate T −Ô T −Ô ∗ ¨ ˙ ¨ ¨ ˙ ˜ Mθ+A (AM A ) Aθ=P Mθ ≜Mθ ∶ intertiaforcesinT Q invariant ω q algorithms for ∗ robot De�nition:DynamicsinT Q q dynamics ˜ ¨ ˜ ˜ ˜ Lagrange’s Mθ+C+N= τ Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints âÔ T (I−P )(Mq¨+C+N)=(I−P )τ+A τ ω ω Let −Ô T −Ô T −Ô Chapter ¥ P =I−M A (AM A ) A T Robot Dynamics and and Control P M=MP ω T Lagrangian ∗ ⊥ Equations thenthedynamicsinT Q : Inertial Properties of −Ô T M(I−P )(q¨+M C)=(I−P )(τ−N)+A λ Rigid Body T ω Dynamics of an Openchain ◻GeometricInterpretation: Manipulator ∇↔M NewtonEuler Equations T T ¨ ˙ Mq+C+N= τ+A λ⇔M∇ q= τ−N+A λ q˙ Coordinate ˜ ∇↔inducedmetriconT Q q invariant algorithms for nd S∶TQ⊗TQÐÐÐÐÐÐ→N(Q):ò fundamentalform robot dynamics Lagrange’s ↑ ↑ Equations with Constraints tangentvector�eld normalvector �eldChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints âò Chapter ¥ Robot ˜ ∇ Y =∇ Y +S(X,Y) X X Dynamics and −Ô T Control ¨ M(I−P )(q+M C)=(I−P )(τ−N)+A λ T ω Lagrangian ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Equations S(q˙,q˙) Inertial MS(q˙,q˙): centrifugalforceduetocurvatureofQinE Properties of Rigid Body Dynamics of an Openchain ⎫ ˜ ⎪ ˜ Manipulator ˜ ⎪ M∇ q˙= τ−N q˙ ⎪ ⎬ forhybridcontroldesign NewtonEuler T ⎪ MS(q˙,q˙)=(I−P )(τ−N)+A λ ⎪ Equations ω ⎪ ⎭ Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints âç ◇Example:DynamicsofaSphericalPendulum Chapter ¥ Ô ò ò ò Robot K= m(x˙ +y˙ +z˙ ) Dynamics and ò Control Ô T Lagrangian = q˙ Mq˙ Equations ò T Inertial q=(x,y,z) ,M=mI Properties of Rigid Body T ò h∶q q−r = ý Dynamics of an Openchain Manipulator ♯ ♯ T ò A=(x,y,z),M =AM A =r m ò NewtonEuler Equations ò ò ⎡ ⎤ y +z −xy −xz ⎢ ⎥ Ô Coordinate ò ò ⎢ ⎥ P = −yz x +z −yz ω invariant ⎢ ⎥ ò r ò ò ⎢ ⎥ algorithms for −zx −zy x +y ⎣ ⎦ robot dynamics ò ⎡ ⎤ x xy xz ⎢ ⎥ Ô Lagrange’s ò ⎢ ⎥ I−P = yx y yz ω Equations with ⎢ ⎥ ò r ò ⎢ ⎥ Constraints zx zy z ⎣ ⎦Chapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ⥠T P =P =P T ω ω Chapter ¥ (μ,ν): Sphericalcoordinates Robot Dynamics and T q=(rcosμcosν,rcosμsinν,rsinμ) Control ˜ ∇ q˙=P (∇ q˙) Lagrangian q˙ q˙ T Equations −rsinμcosν −rsinν v Inertial Ô −rsinμsinν rcosν =   v Properties of ò rcosμ ý Rigid Body Dynamics of S(q˙,q˙)=(I−P )(∇ q˙) T q˙ an Openchain Manipulator rcosμcosν ò ò ò NewtonEuler rcosμsinν =(−μ˙ −cos μν˙ ) Equations rsinμ Coordinate where invariant ò ¨ ˙ v = μ+sinμcosμν Ô algorithms for robot dynamics v = cosμν¨−òsinμμ˙ν˙ ò Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints â  ◻ControlAlgorithm: Chapter ¥ Robot Dynamics and Ô holonomicconstraints: Control q˜ ∶ coordinatesofQ Lagrangian Equations ˙ q=ψ(q˜)⇒q˙=J⋅q˜ Inertial Properties of T ¨ ˙ τ=MJ(q˜ −K ˜e−K ˜e)+C +N+A (−λ +K (λ−λ )) Rigid Body d v p Ô d I d ∫ Dynamics of ò nonholonomicconstraints: an Openchain Manipulator n×m LetJ(q)∈R bes.t. AJ= ý. Writeq˙=J⋅uforsomeu NewtonEuler T Equations ˙ τ=MJ(u˙ −K (u−u ))+MJu+C+N+A (−λ +K (λ−λ )) d p d d I d ∫ Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ââ ◇Example:âDoFmanipulatoronaspherewith frictionlesspointcontact Chapter ¥ Robot Dynamics and Contactconstraint: Control Lagrangian Equations v = ý⇔ýýÔýýýAd −ÔV = ý z g of fl f Inertial ⇒Holonomicconstraint: Properties of Rigid Body T T η=(α ,α ,ψ): ParametrizationofQ o f Dynamics of an Openchain P = diag(Ô,Ô,ý,Ô,Ô,Ô) ω Manipulator NewtonEuler NewtonEulerEquations ofmotion: Equations T T ˙ MV −ad MV =F +G+A λ of of m V of Coordinate invariant ⎡ R M −M ý ⎤ ψ o f ⎢ ⎥⎡ ˙ ⎤ α algorithms for o ⎢ ý ý ý ⎥⎢ ⎥ robot α˙ ˙ V =⎢ ⎥⎢ f ⎥≜Jη of R R K M −R K M ý dynamics ⎢ ψ o o o o f f ⎥⎢ ⎥ ˙ ψ ⎢ ⎥⎣ ⎦ −T M −T M Ô Lagrange’s o o f f ⎣ ⎦ Equations with T MJη¨+C =F +G+A λ ∗ Constraints Ô mChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints âÞ P (∗)∶ ω ˜ ˜ Mη¨+C =B F +B G Ô Ô m Ô Chapter ¥ Robot −ϕ −λ=b F +b G Dynamics and ç ò m ò Control T ˆ ˜ ˙ ˜ ¨ ˜ ˜ F=f f f f f =M(η −K e−K e)+C −B G Ô ò ¥   â v p Ô Ô Lagrangian d Equations f =−ϕ −λ +K (λ−λ )−b G Inertial ç ç I ò d d ∫ Properties of Rigid Body T τ=J F m s Dynamics of an Openchain Manipulator NewtonEuler Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints â— ◇Example:âDoFmanipulatorrollingonasphere ⎡ ⎤ Ô ý ý ý ý ý ⎢ ⎥ Chapter ¥ ý Ô ý ý ý ý ⎢ ⎥ V = ý Robot ⎢ ⎥ l l o f ý ý Ô ý ý ý ⎢ ⎥ Dynamics and ý ý ý ý ý Ô ⎣ ⎦ Control ´¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¸¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¹¶ Lagrangian ˙ A (q) Ô Equations T ¥ f =A λ,λ∈R Inertial c Ô Properties of ω Rigid Body x  =−R (K +R K R )M α˙ ω ý f ψ o ψ f f y Dynamics of an Openchain V = Ad ⋅V g Manipulator of l l fl o f f NewtonEuler ⎡ ⎤ ý Equations ⎢ ⎥ ý ⎢ ⎥ ⎢ ⎥ Coordinate ý = Ad α˙ g f ⎢ ⎥ fl invariant f ⎢ −R (K +R K R )M ⎥ o f ψ o ψ f algorithms for ⎢ ⎥ o robot ⎣ ⎦ dynamics ≜J α˙ f f Lagrange’s Equations with spanJ :Notinvolutive f ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints âÀ T T ˙ MJ α¨ +(MJ α˙ −ad MJ α˙ )=F +G+A λ f f f f f f m J α˙ f f Chapter ¥ T ˙ F =MJ (α¨ −K (α˙ −α˙ ))+(MJ α˙ −ad MJ α˙ ) Robot m f fd p f fd f f ˙ f f J α f f Dynamics and Control T +A (−λ + (λ−λ ))−G d d ∫ Lagrangian Equations Inertial Properties of ◇Example:Redundantparallelmanipulator Rigid Body θ=(θ ,...,θ )∈E Dynamics of Ô â an Openchain Manipulator θ =(θ ,θ ,θ ) a Ô ç   NewtonEuler θ =(θ ,θ ,θ ) p ò ¥ â Equations ⎡ ⎤ x +lc +lc −x −lc −lc a Ô Ôò b ç ç¥ Coordinate ⎢ ⎥ invariant ⎢ y +ls +ls −y −ls −ls ⎥ a Ô Ôò b ç ç¥ H(θ)=⎢ ⎥=ý algorithms for ⎢ x +lc +lc −x −lc −lc ⎥ a Ô Ôò c    â robot ⎢ ⎥ y +ls +ls −y −ls −ls dynamics a Ô Ôò c    â ⎣ ⎦ Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints Þý ò×ò th M(θ)∈R : i chain i M(θ)= diag(M(θ),...,M (θ)) Chapter ¥ Ô ç Robot T ¨ M(θ)θ+C+N= τ+A λ Dynamics and Control Ifalljointsareactuated, Lagrangian Position controlofendešector Equations + Inertial internalgraspingforce Properties of Asτ ,τ ,τ = ý, ò ¥ â Rigid Body ò −Ô ˜ θ∈R ∶localparametrizationofQ=H (ý) Dynamics of an Openchain ˜ Manipulator θ=ψ(θ)∶embeddingofQinE NewtonEuler ˙ Equations ˙ ˜ θ=Jθ Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with ConstraintsChapter ¥ RobotDynamicsand Control 4.6 Lagrange’s Equations with Constraints ÞÔ ∗ ∗ ∗ GivenP ∶T E↦T Q,thedynamicsinT Qisgivenby: ω θ θ θ Chapter ¥ Robot ¨ ˜ Dynamics and P MJθ+P (C +N)=P τ ω ω Ô ω Control τ˜=(τ ,τ ,τ ) Lagrangian Ô ç   Equations ˜ P =(P ,P ,P ) ω Ô ç   Inertial Properties of â ˆ ˆ ˜ Rigid Body τ=P τ=P τ∈R ω ω Dynamics of ¨ ˜ ˙ an Openchain τˆ=P MJ(θ −K ˜e−K ˜e)+P (C +N) ω d v p ω Ô Manipulator NewtonEuler Equations Coordinate invariant algorithms for robot dynamics Lagrange’s Equations with Constraints