Multi-joint kinematics and dynamics Emo Todorov Applied Mathematics Computer Science and Engineering University of Washington Kinematics in generalized vs. Cartesian coordinates 2 generalized q q ,q ,q ,q T q , q 1 2 3 4 1 2 A T Cartesian x Ax , Ay ,Bx ,By ,C x ,C y q3 C dim(q) equals the number of degrees of freedom (DOFs) dim(x)>dim(q), i.e. Cartesian coordinates are over-complete B q4 forward kinematics: always well-defined mapping from q to x = h(q) Ax q1 Ay q2 Bx q1 AB cosq3 By q2 AB sinq3 C x q1 AB cosq3 BC cosq3 q4 C y q2 AB sinq3 BC sinq3 q4 inverse kinematics: usually not well-defined, but one can resolve redundancy via optimization: of all q satisfying x = h(q) for given x, pick the one closest to a preferred q* ˆ q argminqˆ: xhqˆ q q * Kinematics in 3D 3 Attach a spatial frame to each body - usually at the e center of mass, or at the center of the joint connecting 2 it to its parent body. Rotations can be expressed as x e e3 1 | | | frame B A T 1 B R e1 e2 e3 R R , detR 1 o | | | A A A B frame A Transformation between frames: x BoB R x Addition and multiplication can be combined using homogeneous coordinates: A R Ao x A Rˆ B B xˆ : Axˆ ARˆ Bxˆ A Rˆ ARˆ BRˆ B B C B C 0 1 1 The spatia l relat ions between bo dy frames depen d on the jo int parameters q. Let q(n) be the parameters specifying the joint between body n and its parent. Then the corresponding transformation is parameterized as parentn ˆ n Rqn Computing the forward kinematics involves a forward recursion: world ˆ world ˆ parentn ˆ n R parentn R nRqn Numerically, forward kinematics is more accurate using quaternions instead of 3x3 matrices. Rigid-body dynamics 4 (Newton-Euler) ~r v r v see Featherstone’s slides on Spatial Vector Algebra Newtonian mechanics with implicit constraints 5 Newton’s second law for a scalar point mass is m x f m I x f For a set of n point masses in 3D we have 1 3 1,1 1,1 which in vector notation is D x f mn I3 xn,3 fn,3 Now consider a set of m positional equality constraints defined implicitly as x 0 They could specify that some masses belong to the same rigid body, or that some rigid bodies are constrained by joints, etc. The constraints eliminate m DOFs and create a 3n-m dimensional configuration manifold parameterized by q. null space The constraint forces can only act within the null space, which is spanned by the rows of the Jacobian matrix J . q T x Thusftot f J for some m-dimensional vector λ, fdbkfound by taking into account t hdffhe differentiate d constraints: J x J x, J x J x 0, where J x i i xi The constrained dynamics D x f tot are The constrained dynamics are the solution to the linear in x , equation 1 Dx f J T JD1 J T Jx JD1 f D J T x f J 0 Jx Constrained inertia and the Gauss principle 6 When the system is stationary, the constrained dynamics simplify to x A f where A is the inverse of the constrained inertia matrix: 1 A D 1 D1 J T JD1 J T JD1 There is no acceleration in the null space:J x JA f 0 , which follows from 1 JA JD1 JD1 J T JD1 J T JD1 JD1 JD1 0 A is singular, with rank(A) = dim(q). Using the matrix inversion lemma, we can represent A as T 1 A lim D J J Thus the constrained inertia is"D J T J" , and is infinite in the null space. The same results can be obtained from the more general Gauss principle: the constrained acceleration x is the solution to the minimization problem T x argmina a x0 D a x0 s.t. Ja b x0 is the unconstrained acceleration; J, b can encode general constraints. Explicit constraints 7 T 1 T 1 1 The implicitly-constrained dynamics Dx f J J D J J x J D f are expressed in over-complete Cartesian coordinates (x), which is often undesirable. Instead it is better to express the dynamics in generalized (q) coordinates. This is done through explicit constraints given by the forward kinematics function x = h(q) Differentiating the constraints twice yields x Jq q Jq q The dynamics areD x f fc where fc are the constraint forces. T Since the columns of J span the tangent space to the manifold, Jq fc 0 Assembling these equations, we obtain The constrained dynamics are a system which is linear in x,q, fc Mq q cq,q D I 0 x f I 0 J f Jq c where M J T D J T 0 J 0 q 0 T c J D J q J T f Coordinate transformations 8 Consider any set of coordinates x, related to q as x = h(q) Velocities in the two coordinate systems relate as x Jq q Let f and τ denote the same force expressed in x and q coordinates respectively. Power is coordinate-independent: T T T T q x f q J f Since this holds for any velocity, forces in the two coordinate systems relate as Jq T f Let D and M denote the same inertia ex pressed in x and q coordinates respectivel y. Kinetic energy is coordinate-independent: T T T T q Mq x D x q J DJq Since this holds for any velocity, inertias in the two coordinate systems relate as Mq Jq T DxJq Equality constraints in generalized coordinates 9 Equality constraints are handled as in the case of point-mass dynamics: we solve the linear in q , equation T Mq q cq,q Jq Jq q Jq q 0 Here the constraints areq 0 and the Jacobian is Jq q Equality constraints are often used to create kinematic loops (e.g. holding hands) In simulations, the constraints can be violated numerically due to integration errors. Thus it is necessary to introduce constraint stabilization (resembling PD control). Example: 2-link arm 10 m1 (0,0) (x , x ) q1 3 4 m1 m D l2 2 m l1 2 m1 q m2 2 (x1, x2) Implicit constraints: x 2 x 2 l 2 x x 0 0 0 x 1 2 1 1 2 2 2 2 J x 2 x3 x1 x4 x2 l2 x1 x3 x2 x4 x3 x1 x4 x2 Explicit constraints: l cosq l sinq 0 1 1 1 1 l sinq l1 cosq1 0 x hq 1 1 J x l cosq l cos q q h l sinq l sin q q l sin q q 1 1 2 1 2 1 1 2 1 2 2 1 2 l1 sinq1 l2 sin q1 q2 l1 cosq1 l2 cos q1 q2 l2 cos q1 q2 Fast recursive computation of M and c 11 T Computing M J T D J and c J D J q directly is inefficient. Instead one can use faster algorithms exploiting the structure of kinematic trees. Let si be the 6D motion vector of the (1-dof) joint connecting body i to its parent. Composite Rigid Body algorithm for computing the inertia matrix M(q) (1) backward recursion: (2) set: T comp s j Di si if i descendantsj comp comp Di Di D j T comp jchildren(i) Mij s j D j si if j descendantsi 0 otherwise Recursive Newton-Euler algorithm for computing the inverse dynamics q,q,q (1) forward recursion: (2) backward recursion: (3) set: x x s q f D x x D x f sT f i parenti i i i i i i i i jchildren(i) j i i i xi xparenti si qi si qi running this algorithm with q 0 yields cq,q 1 Once M and c are computed, we can compute q M c and integrate. Dynamics in generalized coordinates 12 Mq q c q,q g q M inertia matrix c Coriolis and centrifugal forces where c q,q q q q k ij ij,k i j g gravitational forces 1 M q M jk q Mij q applied/control forces q ik ij,k Christoffel symbols 2 q j qi qk This can be derived from the Euler-Lagrange equation: d Lq,q Lq,q dt q q where the Lagrangian is the kinetic energy minus the potential energy: Lq,q Kq,q Pq 1 T Kq,q q Mq q 2 Pq Pq 9.81m h q , g q n n n q If M does not depend on q, then c=0 and we have Newton’s second law: M q g Hamiltonian formulation 13 The same dynamics can be obtained from the equivalent Hamiltonian formulation, based on the Hamiltonian H = K + P instead of the Lagrangian L = K – P. Now the state is represented in terms of q and the generalized momentum p Mq q T H and L are related by the Legendre transformation H q p L 1 T 1 1 T Kinetic energy in the new coordinates is Kq, p p Mq p q Mq q K q,q 2 2 Hq, p Hamilton’s equations are: p q Hq, p q p The rate of change of the Hamiltonian (i.e.