ME 328: Medical Robotics Winter 2019 Lecture 5: Robot dynamics and simulation Allison Okamura Stanford University Robot dynamics equations of motion describe the relationship between forces/torques and motion (in joint space or workspace variables) two possible goals: ˙ ¨ 1. Given motion variables (e.g. ✓~ , ✓~ , ✓~ or ~x, ~ x,˙ ~x¨ ), what joint torques ( ~⌧ ) or end-effector forces ( f~ ) would have been the cause? (this is inverse dynamics) ~ 2. Given joint torques ( ~⌧ ) or end-effector forces ( f ), ˙ ¨ what motions (e.g. ✓~ , ✓~ , ✓~ or ~x, ~x, ˙ ~x¨ ) would result? (this is forward dynamics) developing equations of motion using Lagrange’s equation The Lagrangian is L = T V − where T is the kinetic energy of the system and V is the potential energy of the system d @L @L Lagrange’s equation is = Qj dt @q˙ − @q ✓ j ◆ j @q where j =1 , 2 ,...,n , and q˙ = j is the generalized j @t velocity and Q j is the nonconservative generalized force corresponding to the generalized coordinate qj what are generalized coordinates? • equations of motion can be formalized in a number of different coordinate systems • n independent coordinates are necessary to describe the motion of a system having n degrees of freedom • any set of n independent coordinates is called generalized coordinates: q1,q2,...qn • these coordinates may be lengths, angles, etc. generalized forces • When external forces act on the system, the configuration changes: generalized coordinates q j change by δ q j , j =1, 2,...,n • If U j is the work done in changing q j by δ q j , the Uj corresponding generalized force is Q j = , where j =1, 2,...,n δqj • Q j is a force/moment and q j is a linear/angular displacement. This can include dissipation (damping). d @L @L ( ) = Qj,wherej =1, 2,...,n dt @q˙j − @qj where@q does Lagrange’s j whereq ˙j = @t is the generalized velocity and Qj is the nonconservative generalized forceequation corresponding to come the generalized from? coordinate qj. Where does it come from? Hamilton’s principle of least action: a system moves from q(t1)toq(t2) in such a way that the following integral takes on the least possible value. S = t2 L(q, q,˙ t)dt t1 R The calculus of variations is used to obtain Lagrange’s equations of mo- tion. We’re concerned with minimizing t2 f (y(t), y˙(t); t) dt t1 R The minimization leads to the equation @f d @f =0 @y − dt @y˙ If there is more than one set of variables in the functional f (e.g. yi andy ˙i) then you get one equation for each set. To use for coupled systems, we need potential and kinetic energy expressions in matrix form. Define: xi, displacement of mass mi Fi, force applied in the direction of xi at mass mi 3 This can be rewritten in matrix form: 1 T T = 2 ˙x M ˙x where: x˙ 1(t) x˙ 2(t) ˙x = 2 . 3 . 6 7 6 x˙ (t) 7 6 n 7 M =4diag(m15,m2,...,mn) In general, the mass matrix M might not be diagonal, for example if we use a di↵erent set of generalized coordinates, such as the relative displace- ments of the masses, rather than the absolute (ground-referenced) displace- ment. We can also use generalized coordinates: 1 T V = 2 q Kq K generalized sti↵ness matrix ! 1 T T = 2 ˙q M ˙q M generalized mass matrix ! Note: In general,usingM needLagrange’s not be diagonal equation to Using Lagrange’sderive Equation equations to derive of Equations motion of Motion d @L @L ( ) = Qj,wherej =1, 2,...,n dt @q˙j − @qj L = T V − Substitute: @L = @T @V @q˙j @q˙j − @q˙j Last term is zero because P.E. is not dependent on velocities @L = @T ! @q˙j @q˙j @L = @T @V @qj @qj − @qj d @T @T @V ( ) + = Qj,wherej =1, 2,...,n dt @q˙j − @qj @qj Qj is a nonconservative generalized force corresponding to coordinate qj, e.g. damping For a conservative system, d ( @T ) @T + @V = 0, where j =1, 2,...,n dt @q˙j − @qj @qj 5 Example: Torsional System Let q1 = ✓1, q2 = ✓2, q3 = ✓3 Kinetic energy: 1 ˙2 1 ˙2 1 ˙2 T = 2 J1✓1 + 2 J2✓2 + 2 J3✓3 Potential energy: V = 1 k ✓2 + 1 k (✓ ✓ )2 + 1 k (✓ ✓ )2 2 1 1 2 2 2 − 1 2 3 3 − 2 External moments are M1, M2, M3 Generalized force: th If Fxk,Fyk,Fzk are the external forces acting on the k mass of the system in the x, y, z directions: Q = (F @xk + F @yk + F @zk ) j k xk @qj yk @qj zk @qj th WherePxk, yk, zk are the displacements of the k mass in the x, y, z direc- tions. For a torsional system F is replaced by M, moment. Q = 3 M @✓k = 3 M @✓k j k=1 k @qj k=1 k @✓j P P Q = M @✓1 + M @✓2 + M @✓3 = M 1 1 @✓1 2 @✓1 3 @✓1 1 Q = M @✓1 + M @✓2 + M @✓3 = M 2 1 @✓2 2 @✓2 3 @✓2 2 6 adding dissipation since the left side of Lagrange’s equation only includes terms for potential and kinetic energy, any dissipative terms (e.g., damping) must be added on the right hand side (and Q j are now only the input forces/torques) d @L @L @R = Q dt @q˙ − @q j − @q˙ ✓ j ◆ j j 1 R = b q˙2 2 j j j X is a simplified form of Rayleigh’s dissipation function example: double pendulum (review on your own) Double Pendulum Example Double Pendulum Example Velocity of m1: v1 = l1✓˙1 2 2 1 Velocity of m2: v2 =(v2x + v2y)Double2 Pendulum Example v2x = l1✓˙1 cos(✓1)+l2✓˙2 cos(✓2) v2y = l1✓˙1 sin(✓1)+l2✓˙2 sin(✓2) Kinetic energy: 1 ˙ 2 1 ˙ ˙ 2 ˙ ˙ 2 T = 2 m1(l1✓1) + 2 m2((l1✓1 cos(✓1)+l2✓2 cos(✓2)) +(l1✓1 sin(✓1)+l2✓2 sin(✓2)) ) Velocity of m1: v1 = l1✓˙1 2 2 1 Velocity of m2: v2 =(v2x + v2y) 2 v2x = l1✓˙1 cos(✓1)+l2✓˙2 cos(✓2) v2y = l1✓˙1 sin(example:✓1)+l2✓˙2 sin(✓2 ) double pendulum Velocity of m1: v1 = l1✓˙1 2 2 1 Kinetic energy: Velocity of m2: v2 =(v2x + v2y) 2 1 ˙ 2 1 ˙ v2x = l1✓˙1 cos(˙ ✓1)+l2✓˙2 2cos(✓2) ˙ ˙ 2 T = 2 m1(l1✓1) + 2 m2((l1✓1 cos(✓1)+l2✓2 cos(✓2)) +(l1✓1 sin(✓1)+l2✓2 sin(✓2)) ) v2y = l1✓˙1 sin(✓1)+l2✓˙2 sin(✓2) Potential energy Kinetic energy: T = 1 m (l ✓˙ )2+ 1 m ((l ✓˙ cos(✓ )+l ✓˙ cos(✓ ))2+(l ✓˙ sin(✓ )+l ✓˙ sin(✓ ))2) V = m gl (1 cos(✓ )) + m g(l (1 2 cos(1 1 ✓1 ))2 + l2 (11 1 cos(1✓ )))2 2 2 1 1 1 2 2 2 1 1 − 1 2 1 − 1 2 − 2 3 The Lagrangian is: L = T V 1 − 2 ˙2 1 2 ˙2 Potential˙ energy˙ PotentialL = 2 (m1+ energym2)l1✓1+ 2 m2l2✓2+m2l1l2✓1✓2 cos(✓1 ✓2)+(m1+m2)gl1 cos(✓1)+ V = m1gl1(1 cos(✓1))− + m2g(l1(1 cos(✓1)) + l2(1 cos(✓2))) Vm2=glm2 cos(gl✓(12) cos(✓ )) + m g(l (1 cos(− ✓ )) + l (1 cos(− ✓ ))) − 1 1 − 1 2 1 − 1 2 − 2 For ✓1: 3 @L 2 ˙ 2 ˙ ˙ ˙ = m1l1✓1 + m2l1✓1 + m2l1l2✓2 cos(✓1 ✓2) @✓1 3 − d @L 2 ¨ ¨ ˙ ˙ ˙ dt ˙ =(m1+m2)l1✓1+m2l1l2✓2 cos(✓1 ✓2) m2l1l2✓2 sin(✓1 ✓2)(✓1 ✓2) @✓1 − − − − ⇣ ⌘ @L = l1g(m1 + m2)sin(✓1) m2l1l2✓˙1✓˙2 sin(✓1 ✓2) @✓1 − − − Thus, the di↵erential equation for ✓1 becomes: (m + m )l2✓¨ + m l l ✓¨ cos(✓ ✓ )+m l l ✓˙2 sin(✓ ✓ )+l g(m + 1 2 1 1 2 1 2 2 1 − 2 2 1 2 2 1 − 2 1 1 m2)sin(✓1)=0 Divide through by l1 and this simplifies to: (m +m )l ✓¨ +m l ✓¨ cos(✓ ✓ )+m l ✓˙2 sin(✓ ✓ )+g(m +m )sin(✓ )= 1 2 1 1 2 2 2 1− 2 2 2 2 1− 2 1 2 1 0 Similarly, for ✓2: @L 2 ˙ ˙ ˙ = m2l2✓2 + m2l1l2✓1 cos(✓1 ✓2) @✓2 − d @L ¨ ¨ ˙ ˙ ˙ dt ˙ = m2l2✓2 + m2l1l2✓1 cos(✓1 ✓2) m2l1l2✓1 sin(✓1 ✓2)(✓1 ✓2) @✓2 − − − − ⇣ ⌘ @L = m2l1l2✓˙1✓˙2 sin(✓1 ✓2) l2m2g sin ✓2 @✓2 − − Thus, the di↵erential equation for ✓2 becomes: m l2✓¨ + m l l ✓¨ cos(✓ ✓ ) m l l ✓˙2 sin(✓ ✓ )+l m g sin ✓ =0 2 2 2 2 1 2 1 1 − 2 − 2 1 2 1 1 − 2 2 2 2 Divide through by l2 and this simplifies to: m l ✓¨ + m l ✓¨ cos(✓ ✓ ) m l ✓˙2 sin(✓ ✓ )+m g sin ✓ =0 2 2 2 2 1 1 1 − 2 − 2 1 1 1 − 2 2 2 So, we have developed a very complicated set of coupled equations of motion from a very simple system! ⇡ For the initial conditions ✓1 = 2 and ✓2 = ⇡, you get the response be- low. If you perturbed these initial conditions slightly, you would get a very di↵erent response. This is evidence of a chaotic system. If you linearized the equations or added damping, you would get a much more predictable 4 The Lagrangian is: L = T V − L = 1 (m +m )l2✓˙2+ 1 m l2✓˙2+m l l ✓˙ ✓˙ cos(✓ ✓ )+(m +m )gl cos(✓ )+ 2 1 example:2 1 1 2 2 2 2 double2 1 2 1 2 pendulum1− 2 1 2 1 1 m2gl2 cos(✓2) For ✓1: @L 2 ˙ 2 ˙ ˙ ˙ = m1l1✓1 + m2l1✓1 + m2l1l2✓2 cos(✓1 ✓2) @✓1 − d @L 2 ¨ ¨ ˙ ˙ ˙ dt ˙ =(m1+m2)l1✓1+m2l1l2✓2 cos(✓1 ✓2) m2l1l2✓2 sin(✓1 ✓2)(✓1 ✓2) @✓1 − − − − ⇣ ⌘ @L = l1g(m1 + m2)sin(✓1) m2l1l2✓˙1✓˙2 sin(✓1 ✓2) @✓1 − − − Thus, the di↵erential equation for ✓1 becomes: (m + m )l2✓¨ + m l l ✓¨ cos(✓ ✓ )+m l l ✓˙2 sin(✓ ✓ )+l g(m + 1 2 1 1 2 1 2 2 1 − 2 2 1 2 2 1 − 2 1 1 m2)sin(✓1)=0 Divide through by l1 and this simplifies to: (m +m )l ✓¨ +m l ✓¨ cos(✓ ✓ )+m l ✓˙2 sin(✓ ✓ )+g(m +m )sin(✓ )= 1 2 1 1 2 2 2 1− 2 2 2 2 1− 2 1 2 1 0 Similarly, for ✓2: @L 2 ˙ ˙ ˙ = m2l2✓2 + m2l1l2✓1 cos(✓1 ✓2) @✓2 − d @L ¨ ¨ ˙ ˙ ˙ dt ˙ = m2l2✓2 + m2l1l2✓1 cos(✓1 ✓2) m2l1l2✓1 sin(✓1 ✓2)(✓1 ✓2) @✓2 − − − − ⇣ ⌘ @L = m2l1l2✓˙1✓˙2 sin(✓1 ✓2) l2m2g sin ✓2 @✓2 − − Thus, the di↵erential equation for ✓2 becomes: m l2✓¨ + m l l ✓¨ cos(✓ ✓ ) m l l ✓˙2 sin(✓ ✓ )+l m g sin ✓ =0 2 2 2 2 1 2 1 1 − 2 − 2 1 2 1 1 − 2 2 2 2 Divide through by l2 and this simplifies to: m l ✓¨ + m l ✓¨ cos(✓ ✓ ) m l ✓˙2 sin(✓ ✓ )+m g sin ✓ =0 2 2 2 2 1 1 1 − 2 − 2 1 1 1 − 2 2 2 So, we have developed a very complicated set of coupled equations of motion from a very simple system! ⇡ For the initial conditions ✓1 = 2 and ✓2 = ⇡, you get the response be- low.