Musculoskeletal Dynamics Grant Schaffner ATA Engineering, Inc. Part II Musculoskeletal Dynamics Free Body Diagram of Link i 0 x 0 y 0 z 0 O 1 ? i O i O Joint i Joint i +1 g i m Link i i i , 1 ? r ci i , r ci i , 1 ? r i i , 1 ? N 1 , + ? i i N i i , 1 ? f 1 , + ? i i f ci v i ω i I Lagrangian Formulation of Equations of Motion ? D escr ibes the behav ior of a dynamic system in terms of work and energy stored in the system ? C onstraint forces are automatically elim inated (an advantage and a disadvantage), called the “closed form” dynamic equations ? E quations are deriv ed systematically (eas ier to use) i i i i i i i n q Q n i Q q L q L dt d q q L q q coord d generalize to ing correspond force d generalize : from derived then are motion of Equations : Lagrangian Define energy potential total energy kinetic total system dynamic a of s coordinate d generalize , , 1 ) , ( , , 1 = = = ? ? ? ? ? ? = = = = m D D m U T U T (2-1) Lagrangian Formulation ? C ompute the veloc i ty and angular velocity of an indiv i dual link i (think of th e link as an end effector with coord sys a t th e link c.m.) q J J J ω q J J J v D D l D D D l D ) ( ) ( 1 ) ( 1 ) ( ) ( 1 ) ( 1 i A i i A i i A ci i L i i Li i L ci q q q q = + + = = + + = wh ere j -th column vectors of the 3xn Jacobian matrices link i , i.e., ) ( i Lj J ) ( i Aj J ) ( i L J ) ( i A J ] [ ] [ ) ( ) ( 1 ) ( ) ( ) ( 1 ) ( 0 0 J J J 0 0 J J J h h h h i A i i A i A i Li i L i L = = Note: tion of link i depends on only joints 1 through i , the column vectors are set to zero for j ≥ (2-2) (2-3) are the and for the linear and angular velocities of and Since the mo i Lagrangian Formulation ? E ach column vector is given by: ? ? ? = ? ? ? × = ? ? ? ? 0 b J b r b J 1 ) ( 1 , 1 1 ) ( j i Aj j ci j j i Lj (revolute jt) (prismatic jt) (revolute jt) (prismatic jt) = ? ci j , 1 r Pos i tion vector of centroid of link i wrt inboard link coordinate frame = ? 1 j b 3x1 unit vector along joint axis j -1 (2-4) ( ∑ = + = n i i i T i ci T ci i m 1 2 1 2 1 ω I ω v v T (2-5) wh ere frame coord base wrt centroid, the at tensor inertia link of mass 3 3 × = = i i i m I Note: wrt the base coord f rame i I ) varies with the orientation of the link Lagrangian F ormulation Note: tensor defined relativ e to the coord frame fixed to the link, using ( ( ) ( 1 ) ( ) ( ) ( ) ( 2 1 1 ) ( ) ( ) ( ) ( 2 1 definite positive symmetric is n) (n tensor inertia system where H J I J J J H q H q q J I J q q J J q T × = + = = + = ∑ ∑ = = n i i A i T i A i L T i L i T n i i A i T i A T i L T i L T i m m ? ? ? ? ? ? i I i I T i i i i 0 0 R I R I = (2-6) (2-7) (2-8) , the inertia can be obtained from ) ) Lagrangian F ormulation Potential Energy ∑ = = n i ci T i m 1 , 0 r g U (2-9) Generalized Forces ext T F J τ Q + = torques joint = τ moments and forces external = ext T F J Lagrange’s Equations of Motion (see Asada & Slotine fo r d e r ivation) ( n i Q G q q h q H i i k n j j n k ijk j n j ij , , 1 1 1 l C C C C = = + + ∑∑ ∑ = = i jk k ij ijk q H q H h ? ? ? ? ? = 2 1 (2-11) (2-10) Inertia torque s Cent rifugal/ Coriolis t r qs Gr a v ity torque G e ner a liz e d Force s ) ( 1 j Li n j T j i m G J g ∑ = = ) 1 = Example: 2 dof planar arm ? V e l oc ities of centroids c 1 and c 2 0 x 0 y 1 l 2 l 1 1 , τ θ 2 2 , τ θ 1 c l 2 c l 1 1 , m I 2 2 , m I q v ? ?? ? ?? ? ? = 0 cos 0 sin 1 1 1 1 1 θ θ c c c l l q v ? ?? ? ?? ? + + + + ? + ? ? = ) cos( ) cos( cos ) sin( ) sin( sin 2 1 2 2 1 2 1 1 2 1 2 2 1 2 1 1 2 θ θ θ θ θ θ θ θ θ θ c c c c c l l l l l l ? T hese 2x2 matrices are the ? a ssociated with the angular velocities are 1x2 row vectors in this planar case ) ( i L J ) ( i A J q q ? ? ? ? ? ] 1 1 [ ] 0 1 [ 2 1 2 1 1 = + = = = θ θ ω θ ω Example: 2 dof planar arm ? S ubstituting the linear and angular Jacobians into eqn (2-7) gives q H C ? ? ? ? ? ? + + + + + + + + + + = 2 2 2 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2 1 2 2 2 2 1 2 2 2 1 2 1 2 1 1 cos cos ) cos 2 ( I l m I l m l l m I l m l l m I l l l l m I l m c c c c c c c c θ θ θ 0 , 0 , sin sin 2 , sin , 0 221 212 222 2 2 1 2 211 2 2 1 2 121 112 2 2 1 2 122 111 = + = = ? = + ? = = h h h l l m h l l m h h l l m h h c c c θ θ θ ? C entrifugal / Coriolis term coefficients ? G ravity ter m s ? S ubstituting the above into (2-11) gives ] [ ] [ ) 2 ( 2 2 ) 1 ( 2 1 2 ) 2 ( 1 2 ) 1 ( 1 1 1 L L T L L T m m G m m G J J g J J g + = + = 2 2 2 1 211 1 12 2 22 1 1 2 1 121 112 2 2 122 2 12 1 11 ) ( τ θ θ θ τ θ θ θ θ θ = + + + = + + + + + G h H H G h h h H H C C C C C C C C C C C C