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 cosq3

By  q2  AB sinq3

C x  q1  AB cosq3  BC cosq3  q4 

C y  q2  AB sinq3  BC sinq3  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ˆ: xhqˆ 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 , detR  1 o    | | |  A A A B frame A Transformation between frames: x  BoB 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 spati al re lat ions between bo dy frames depen d on t he 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 parentn ˆ n Rqn  Computing the forward kinematics involves a forward recursion: world ˆ world ˆ parentn ˆ n R  parentn R nRqn  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   xn,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 JD1 J T Jx  JD1 f  D  J T  x   f                J 0    Jx 

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  D1 J T JD1 J T  JD1 There is no acceleration in the null space:J x  JA f  0 , which follows from 1 JA  JD1  JD1 J T JD1 J T  JD1  JD1  JD1  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  x0  D a  x0  s.t. Ja  b

x0 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  Jq q Jq 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, Jq fc  0

Assembling these equations, we obtain The constrained dynamics are a system which is linear in x,q, fc Mq q cq,q    D  I 0  x   f       I 0  J f  Jq   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  Jq 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   Jq 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 Mq  Jq T DxJq 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 Mq q cq,q   Jq  Jq q Jq q  0  Here the constraints areq  0 and the Jacobian is Jq  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 cosq    l sinq  0   1 1   1 1   l sinq   l1 cosq1 0  x  hq  1 1 J x  l cosq  l cos q  q  h  l sinq  l sin q  q  l sin q  q   1 1 2 1 2   1 1 2 1 2 2 1 2       l1 sinq1  l2 sin q1  q2   l1 cosq1  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  descendantsj comp comp  Di  Di  D j T comp  jchildren(i) Mij  s j D j si if j  descendantsi  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 parenti i i i i i  i i i  jchildren(i) j i i i

xi  xparenti  si qi  si qi running this algorithm with q   0 yields cq,q

1 Once M and c are computed, we can compute q   M    c  and integrate.

Dynamics in generalized coordinates 12

Mq 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 Lq,q Lq,q     dt q q

where the Lagrangian is the kinetic energy minus the potential energy:

Lq,q  Kq,q Pq

1 T Kq,q  q Mq q 2 Pq Pq  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  Mq 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 Kq, p  p Mq p  q Mq q  K q,q  2 2

Hq, p Hamilton’s equations are: p    q Hq, p q  p

The rate of change of the Hamiltonian (i.e. the total energy) equals power:

d H H H H H H H T Hq, p  q  p      q  dt qT pT qT p pT q pT

In the absence of external forces, the Hamiltonian is conserved.

Manifolds and metrics 14

Q is a differentiable manifold and TqQ the tangent space at point q. T Q T* Q denotes the co-tangent (or dual) space. q q u q A metric defines a dot-product on the tangent space: u,v  uT M q v  M uiv j  M ui v j (Einstein) q  ij ij ij Q The manifold is Riemannian if M(q) is s.p.d. for all q.

The dot-product on the co-tangent space is defined by the inverse of M: u*,v *  u *T M q 1 v*  M ij u v where M ij  M 1 , u  ui , u*  u q  i j    ij    i The metric provides the mapping between the two spaces: 1 j i ij u*  Mu, u  M u*; in coordinates, ui  Mij u , u  M u j

T i T Tangent and co-tangent vectors are multiplied directly: u v*  u vi  u M v

Application to multi-joint dynamics: The configuration space of a multi-joint system is a Riemannian manifold with metric given by the joint-space inertia matrix M(q). The tangent vectors are velocities q . The co-tangent vectors are forces f and momentap  Mq q . T T p q is kinetic energy; f q  is power. Covariant derivatives and geodesics 15

The tangent basis vectors are associated with partial derivatives: ei   qi

The co-tangent basis vectors are associated with differential forms:  i  dqi i If f(q) is scalar andv  v ei is a tangent vector, then vf is the directional derivative:

i i f T vf  v ei f  v  v gradf qi A connection specifies how nearby coordinate frames “connect”, i.e. how the basis vectors change over the manifold. The usual vector directional derivative is replaced k with the covariant derivative, defined in coordinates by the Christoffel symbols ij q  e  k e ei j ij k  uk  For general vectorsu  uie , v  vie the covariant derivative is  i k i j  i i vu  v  ij u v ek  qi  k A connection is flat whenij  0 . In that case we recover the regular derivative.

For a Riemannian manifold with metric M(q), there exists a unique metric-preserving torsion-free connection (the Levi-Civita connection) with Christoffel symbols: 1  M M M  k  M ks    is  js  ij  ij ij,s ij,s   2  q j qi qs  A geodesic is a curve γ(t) such that , i.e. k k i j for all k.   0   ij    0 This is called the geodesic equation.

Unforced motions as geodesics 16

The unforced motions of a multi-joint system satisfy Mq q cq,q  0

1 which we can rewrite (using the fact that M is s.p.d.) as q Mq cq,q  0

Recalling the expression for c, this can be written in component form as

q  M 1c  q  kq q  0 k   k k ij ij i  j

1  M M M  where k  M 1 and    is  js  ij  ij s ks ij,s ij,s   2  q j qi qs 

Thus we have recovered the geodesic equation q q  0

The Levi-Civita connection for the Riemannian metric defined by the inertia matrix is called the mechanical connection. Its geodesics are the unforced motions.

With external forces and gravity, the dynamics become Mqq q   g

This is equivalent to Newton’s s second law, d with the covariant derivative in place of the regular derivative: q  q q dt 