3D Geometry Basics (For Robotics) Lecture Notes

3D geometry basics (for robotics) lecture notes

Marc Toussaint Machine Learning & Robotics lab, FU Berlin Arnimallee 7, 14195 Berlin, Germany

October 30, 2011

This document introduces to some basic geometry, fo- Concatenation is non-trivial in this representation cussing on 3D transformations, and introduces proper and we don’t discuss it here. In practice, a rota- conventions for notation. There exist one-to-one imple- tion vector is first converted to a rotation matrix or mentations of the concepts and equations in libORS. quaternion. Convertion to a matrix: For every vector w ∈ R3 we 1 Rotations define its skew symmetric matrix as  0 −w w   3 2    There are many ways to represent rotations in SO(3). We   w =  w 0 −w  . (2) b  3 1    restrict ourselves to three basic ones: rotation matrix, ro- −w2 w1 0 tation vector, and quaternion. The rotation vector is also the most natural representation for a “rotation velocity” Note that such skew-symmetric matrices are related (angular velocities). Euler angles or raw-pitch-roll are an to the cross product: w × v = wb v, where the cross alternative, but they have singularities and I don’t recom- product is rewritten as a matrix product. The rota- mend using them in practice. tion matrix R(w) that corresponds to a given rotation vector w is: A rotation matrix is a matrix R ∈ R3×3 which is or- thonormal (columns and rows are orthogonal unit R(w) = exp(wb) (3) vectors, implying determinant 1). While a 3 × 3 ma- > 2 = cos θ I + sin θ w/θb + (1 − cos θ) ww /θ trix has 9 degrees of freedom (DoFs), the constraint (4) of orthogonality and determinant 1 constraints this: The set of rotation matrices has only 3 DoFs (∼ the The exp function is called exponential map (gener- local Lie algebra is 3-dim). ating a group element (=rotation matrix) via an element of the Lie algebra (=skew matrix)). The other The application of R on a vector x is simply the formular is called Rodrigues’ formular: the first term matrix-vector product Rx. is a diagonal matrix (I is the 3D identity matrix), the R R Concatenation of two rotations 1 and 2 is the nor- second terms the skew symmetric part, the last term R R mal matrix-matrix product 1 2. the symmetric part (ww> is also called outper prod- Inversion is the transpose, R-1 = R>. uct). 3 A rotation vector is an unconstraint vector w ∈ R . The Angular velocity & derivative of a rotation matrix: We w = w 3 vector’s direction |w| determines the rotation represent angular velocities by a vector w ∈ R , the axis, the vector’s length |w| = θ determins the rota- direction w determines the rotation axis, the length tion angle (in radians, using the right thumb conven- |w| is the rotation velocity (in radians per second). tion). When a body’s orientation at time t is described by a The application of a rotation described by w ∈ R3 on rotation matrix R(t) and the body’s angular velocity a vector x ∈ R3 is given as (Rodrigues’ formula) is w, then > ˙ w · x = cos θ x + sin θ (w × x) + (1 − cos θ) w(w x) R(t) = wb R(t) . (5) (1) (That’s intuitive to see for a rotation about the x-axis where θ = |w| is the rotation angle and w = w/θ the with velocity 1.) Some insights from this relation: unit length rotation axis. Since R(t) must always be a rotation matrix (fulfill The inverse rotation is described by the negative of orthogonality and determinant 1), its derivative R˙ (t) the rotation vector. must also fulfill certain constraints; in particular it

1 3D geometry basics (for robotics) lecture notes, Marc Toussaint—October 30, 2011 2

can only live in a 3-dimensional sub-space. It turns a small time interval δ, w generates a rotation vector out that the derivative R˙ of a rotation matrix R must δw, which converts to a quaterion always be a skew symmetric matrix w times R – any- b ∆r = (cos(δ/2), sin(δ/2)w) . (13) thing else would be inconsistent with the contraints of orthogonality and determinant 1. That rotation is concatenated LHS to the original Note also that, assuming R(0) = I, the solution to quaternion, ˙ the differential equation R(t) = wb R(t) can be writ- r(t + δ) = ∆r ◦ r(t) . (14) ten as R(t) = exp(twb), where here the exponential function notation is used to denote a more general Now, if we take the derivative w.r.t. δ and evaluate so-called exponential map, as used in the context of it at δ = 0, all the cos(δ/2) terms become − sin(δ/2) Lie groups. It also follows that R(w) from (3) is the and evaluate to zero, all the sin(δ/2) terms become rotation matrix you get when you rotate for 1 second cos(δ/2) and evaluate to one, and we have with angular velocity described by w. 1 1 r˙(t) = (−w>r,¯ r w +r ¯ × w) = (0, w) ◦ r(t) 2 0 2 Quaternion (I’m not describing the general definition, (15) only the “quaternion to represent rotation” defini- 4 tion.) A quaternion is a unit length 4D vector r ∈ R4; Here (0, w) ∈ R is a four-vector; for |w| = 1 it is a the first entry r0 is related to the rotation angle θ normalized quaternion. However, due to the linear- via r0 = cos(θ/2), the last three entries r¯ ≡ r1:3 ity the equation holds for any w. w are related to the unit length rotation axis via Quaternion velocity → angular velocity The following r¯ = sin(θ/2) w . is relevant when taking the derivative w.r.t. the pa- The inverse of a quaternion is given by negating r¯, rameters of a quaternion, e.g., for a ball joint repre- -1 r = (r0, −r¯) (or, alternatively, negating r0). sented as quaternion. Given r˙, we have The concatenation of two rotations r, r0 is given as 1 1 r˙ ◦ r-1 = (0, w) ◦ r ◦ r-1 = (0, w) (16) the quaternion product 2 2 which allows us to read off the angular velocity in- r ◦ r0 = (r r0 − r¯>r¯0, r r¯0 + r0 r¯ +r ¯0 × r¯) (6) 0 0 0 0 duced by a change of quaternion. However, the RHS zero will hold true only iff r˙ is orthogonal to r (where The application of a rotation quaternion r on a vector > > > r˙ r =r ˙0r0 +¯˙r r¯ = 0, see (6)). In case r˙ r 6= 0, the x can be expressed by converting the vector first to change in length of the quaterion does not represent (0, x) the quaternion , then computing any angular velocity; in typical kinematics engines

-1 a non-unit length is ignored. Therefore one first or- r · x = (r ◦ (0, x) ◦ r )1:3 , (7) thogonalizes r˙ ← r˙ − r(r ˙>r). I think a bit more efficient is to first convert the ro- As a special case of application, consider computing tation quaternion r to the equivalent rotation matrix the partial derivative w.r.t. quaternion coordinates, R, as given by where r˙ is the unit vectors e0, .., e3. In this case, the orthogonalization becomes simply ei ← ei − rri and 1 − r − r r − r r + r   22 33 12 03 13 02      -1 -1 R =  r + r 1 − r − r r − r  (ei − rir) ◦ r = ei ◦ r − ri(1, 0, 0, 0) (17)  12 03 11 33 23 01    r13 − r02 r23 + r01 1 − r11 − r22 -1 wi = 2[ei ◦ r ]1:3 , (18) rij := 2rirj . (8) where wi is the rotation vector implied by r˙ = ei. (Note: In comparison to (3) this does not require to In case the original quaternion r wasn’t normal- compute a sin or cos.) Inversely, the quaterion r for a ized (which could be, if a standard optimization given matrix R is algorithm searches in the quaternion configuration space), then r actually represents the normalized √ 1 quaternion r¯ = √1 r, and (due to linearity of the r = 1 + trR (9) r2 0 2 above), the rotation vector implied by r˙ = ei is r3 = (R21 − R12)/(4r0) (10) 2 -1 r = (R − R )/(4r ) wi = √ [ei ◦ r ]1:3 . (19) 2 13 31 0 (11) r2 r1 = (R32 − R23)/(4r0) . (12) 2 Transformations Angular velocity → quaternion velocity Given an angular velocity w ∈ R3 and a current quaterion r(t) ∈ We consider two types of transformations here: ei- R, what is the time derivative r˙(t) (in analogy to ther static (translation+rotation), or dynamic (transla- Eq. (5))? For simplicity, let’s first assume |w| = 1. For tion+velocity+rotation+angular velocity). The first maps 3D geometry basics (for robotics) lecture notes, Marc Toussaint—October 30, 2011 3 between two static reference frames, the latter between v = v1 + w1 × (r1 · t2) + r1 · v2 (26) moving reference frames, e.g. between reference frames r = r1 ◦ r2 (27) attached to moving rigid bodies. w = w1 + r1 · w2 (28)

2.1 Static transformations For completeness, the footnote1 also describes how accelerations transform, including the case when the trans- Concerning the static transformations, again there are dif- form itself is accelerating. The inverse (t0, r0, v0, w0) = ferent representations: (t, r, v, w)-1 of a dynamic transform is given as

A homogeneous matrix is a 4 × 4-matrix of the form 0 1 t = −r- · t (35)   0 -1 R t  r = r (36) T =   (20)  0 1 v0 = r-1 · (w × t − v) (37) where R is a 3 × 3-matrix (rotation in our case) and t w0 = −r-1 · w (38) a 3-vector (translation). Sequences of transformations by TA→B we denote the In homogeneous coordinates, vectors x ∈ R3 are ex-   transformation from frame A to frame B. The frames x 4 panded to 4D vectors   ∈ R by appending a 1. A and B can be thought of coordinate frames (tuples  1  of an offset (in an afﬁne space) and three local or- 3 Application of a transform T on a vector x ∈ R is thonormal basis vectors) attached to two bodies A then given as the normal matrix-vector product and B. It holds         0 x R t  x Rx + t x = T · x = T   =     =   . TA→C = TA→B ◦ TB→C (39)  1   0 1  1   1  (21) where ◦ is the concatenation described above. Let p be a point (rigorously, in the afﬁne space). We write Concatenation is given by the ordinary 4-dim pA for the coordinate vector of point p relative to matrix-matrix product. frame A; and pB for the coordinate vector of point The inverse transform is p relative to frame B. It holds

 -1  -1 -1  A B -1 R t  R −R t p = TA→B p . (40) T =   =   (22)  0 1  0 1  2.3 A note on afﬁne coordinate frames Translation and quaternion: A transformation can efﬁ-

ciently be stored as a pair (t, r) of a translation vec- Instead of the notation TA→B, other text books often use t r A tor and a rotation quaternion . Analogous to notations such as TAB or TB . A common question regard- the above, the application of (t, r) on a vector x is ing notation TA→B is the following: x0 = t + r · x; the inverse is (t, r)-1 = (−r-1 · t, r-1); the The notation TA→B is confusing, since it transforms concatenation is (t1, r1)◦(t2, r2) = (t1 +r1 ·t2, r1 ◦r2). coordinates from frame B to frame A. Why not the other way around? 2.2 Dynamic transformations I think the notation TA→B is intuitive for the following Just as static transformations map between (static) co- reasons. The core is to understand that a transformation ordinate frames, dynamic transformations map between can be thought of in two ways: as a transformation of the moving (inertial) frames which are, e.g., attached to mov- coordinate frame itself, and as transformation of the coor- ing bodies. A dynamic transformation is described by dinates relative to a coodrinate frame. I’ll first give a non- a tuple (t, r, v, w) with translation t, rotation r, velocity formal explanation and later more formal definitions of v and angular velocity w. Under a dynamic transform affine frames and their transformation. (t, r, v, w) a position and velocity (x, x˙) maps to a new po- 1Transformation of accelerations: sition and velocity (x0, x˙ 0) given as v˙ =v ˙1 +w ˙ 1 × (r1 · t2) + w1 × (w1 × (r1 · t2)) 0 x = t + r · x (23) + 2 w1 × (r1 · v2) + r1 · v˙2 (29) x˙ 0 = v + w × (r · x) + r · x˙ (24) w˙ =w ˙ 1 + w1 × (r1 · w2) + r1 · w˙ 2 (30) Used identities: for any vectors a, b, c and rotation r: (the second term is the additional linear velocity of x˙ 0 r · (a × b) = (r · a) × (r · b) (31) arising from the angular velocity w of the dynamic trans- a × (b × c) = b(ac) − c(ab) (32) form). The concatenation (t, r, v, w) = (t1, r1, v1, w1) ◦ ∂t(r · a) = w × (r · a) + r · a˙ (33) (t2, r2, v2, w2) of two dynamic transforms is given as ∂t(w × a) =w ˙ × t + w × a˙ (34)

t = t1 + r1 · t2 (25) 3D geometry basics (for robotics) lecture notes, Marc Toussaint—October 30, 2011 4

Think of TW →B as translating and rotating a real rigid 3 Kinematic chains body: First, the body is located at the world origin; then the body is moved by a translation t; then the body is ro- In this section we only consider static transformations tated (around its own center) as described by R. In that compost of translation and rotation. But given the   R t  general concatenation rules above, everything said here sense, TW →B =   describes the “forward” transfor-  0 1 generealizes directly to the dynamic case. mation of the body. Consider that a coordinate frame B is attached to the rigid body and a frame W to the world 3.1 Rigid and actuated transforms origin. Given a point p in the world, we can express its coordinates relative to the world, pW , or relative to the A actuated kinematic chain with n joints is a series of body pB. You can convince yourself with simple exam- transformations of the form W B ples that p = TW →B p , that is, TW →B also describes the T ◦ Q ◦ T ◦ Q ◦ T ◦ Q ◦ · · · (51) “backward” transformation of body-relative-coordinates W →1 1 1→2 2 2→3 3 to world-relative-coordinates. Each Ti-1→i describes so-called “links” (or bones) of the Formally: Let (A, V ) be an affine space. A coordinate kinematic chain: the rigid (non-actuated) transformation frame is a tuple (o, e1, .., en) of an origin o ∈ A and basis from the i-1th joint to the ith joint. The first TW →1 is the vectors ei ∈ V . Given a point p ∈ A, its coordinates p1:n transformation from world coordinates to the first joint. w.r.t. a coordinate frame (o, e1, .., en) are given implicitly Each Qi is the actuated transformation of the joint – via usually simply a rotation around the joint’s x-axis with a X specific angle, the so-called joint angle. These joint angles p = o + piei . (41) i (and therefore each Qi) are actuated and may change over time. A transformation TW →B is a (“forward”) transformation When we control the robot we essentially tell it to actu- of the coordinate frame itself: ate its joint so as to change the joint angles. There are two fundamental computations necessary for control: B B B W W W (o , e1 , .., en ) = (o + t, Re1 , .., Ren ) (42) 1. For a given n-dimensional vector q ∈ Rn of joint an- where t ∈ V is the affine translation in A and R the ro- gles, compute the absolute frames TW →i (world to B W link transformation) of each link i. tation in V . Note that the coordinates (ei )1:n of a basis B vector e relative to frame W are the columns of R: n i 2. For a given n-dimensional vector q˙ ∈ R of joint an- X X gle velocities, compute absolute (world-relative) ve- eB = (eB)W eW = R eW (43) i i j j ji j locity and angular velocity of the ith link. j j The first problem is solved by “forward chaining” the Given this transformation of the coordinate frame itself, transformations: we can compute the absolute trans- the coordinates transform as follows: forms TW →i (i.e., the transformation from world to the ith link) for each link, namely: W X W W p = o + pi ei (44) i TW →i = TW →i-1 ◦ Qi ◦ Ti-1→i . (52) X p = oB + pB eB (45) i i i = 2, .., n i Iterating this for we get positions and orienta- X tions TW →i of all links in world coordinates. = oW + t + pB (ReW ) (46) i i The second problem is addressed in the next section. i W X W W X B W = o + ti ei + pj (Rej ) (47) i j 3.2 Jacobian & Hessian W X W W X B X W = o + ti ei + pj ( Rij ei ) (48) Assume we have computed the absolute position and ori- i j i entation of each link in an actuated kinematic chain. Then h i W X W X B W pW i = o + ti + Rij pj ei (49) we want to know how a point i attached to the th i j frame (and coordinates expressed w.r.t. the world frame W W X B W j ⇒ p = t + R p . (50) ) changes when rotating the th joint. Equally we want i i ij j W j to know how some arbitrary vector ai attached to the ith frame (coordinates relative to the world frame W ) rotates W Another way to express this formally: TW →B maps co- when rotating the jth joint. Let aj be the unit length variant vectors (including “basis vectors”) forward, but rotation axis of the jth joint (which, by convention, is contra-variant vectors (including “coordinate vectors”) rW →j · (1, 0, 0) if rW →j is the rotation in TW →j). In the backward; see Wikipedia “Covariance and contravari- following we drop the superscript W because all coor- ance of vectors”. dinates are expressed in the world frame. Let qj be the 3D geometry basics (for robotics) lecture notes, Marc Toussaint—October 30, 2011 5

ai Why did we write this as a transposed vector? By definition, a derivative is something that, when you multiply it pi with an infinitesimal displacement δx ∈ Rn, it returns the aj infinitesimal change δf(x) ∈ R: a p k ∂f(x) j δf(x) = δx . (58) ∂x pk Since in our case δx ∈ Rn is a vector, for this equation to ∂f(x) make sense, ∂x must be a transposed vector:  > ∂f(x)     δx  ∂x1   1       ∂f(x)       δx   ∂x   2  δf(x) =  2    (59)        ...  Figure 1: Illustration for the Jacobian and Hessian.  ...         ∂f(x)  δxn ∂xn j X ∂f(x) joint angle of the th joint. For the purpose of computing = δx . (60) ∂x i a partial deriviative of something attached to link i w.r.t. i i the actuation at joint j we can think of everything inbe- tween as rigid. Hence, the point and vector Jacobian and Expressed more formally: The partial derivative corre- Hessian are simply: sponds to a 1-form (also called dual vector). The param- eters of a 1-form actually form a co-variant vector, not dij := pi − pj a contra-variant vector; which in normal notation corre- ∂pi sponds to a transposed vector, not a normal vector. See = aj × dij (53) ∂qj Wikipedia “Covariance and contravariance of vectors”. d ∂ai Consider a vector-valued mapping φ(x) ∈ R . What = αj × ai (54) ∂ ∂qj exactly is the partial derivative ∂x φ(x)? We can write it ∂2p ∂a ∂d as follows: i = j × d + a × ij ij j   ∂qj ∂qk ∂qk ∂qk ∂φ1(x) ∂φ1(x) ∂φ1(x)  ...   ∂x1 ∂x2 ∂xn     ∂φ2(x) ∂φ2(x) ∂φ2(x)  = (ak × aj) × dij + aj × [ak × (pi − pk) − ak × (pj − pk)]   ∂  ...   ∂x1 ∂x2 ∂xn  φ(x) =     (61) = (a × a ) × d + a × (a × d )  . .  k j ij j k ij ∂x  . .     . .  h i   using a × (b × c) + b × (c × a) + c × (a × b) = 0  ∂φd(x) ∂φd(x) ... ∂φd(x)  ∂x1 ∂x2 ∂xn

= ak × (aj × dij) (55) = d × n (62) ∂2a i = (a × α ) × a + α × (a × a ) Why is this a d × n matrix and not the transposed n × d ∂q ∂q k j i j k i j k matrix? Let’s check the infinitesimal variations again: We = a × (α × a ) n d k j i (56) have δx ∈ R and want δφ ∈ R : Efficient computation: Assume we have an articulated ∂φ(x) δφ(x) = δx (63) kinematic tree of links and joints. We write j < i if joint ∂x   j is inward from link (or joint) i. Then, for each body i ∂φ1(x) ∂φ1(x) ∂φ1(x)  ...     ∂x1 ∂x2 ∂xn    δx1 consider all inward edges j < i and further inward edges  ∂φ2(x) ∂φ2(x) ∂φ2(x)         ...     ∂x1 ∂x2 ∂xn   δx2  k ≤ j H = ∂ ∂ p H =     and compute ijk θj θk i. Note that ijk is     (64)  . .     . .   ...      symmetric in j and k – so computing for k ≤ j and copy-  . .      δx  ∂φd(x) ∂φd(x) ... ∂φd(x)  n ing the rest is sufficient. ∂x1 ∂x2 ∂xn = d × n n (65)   P ∂φ1(x) 4 A note on partial derivatives w.r.t. a  δxi   i ∂xi     ∂φ2(x)  P   δxi   i ∂xi  vector =     (66)  .   .     .  n   Let x ∈ . Consider a scalar function f(x) ∈ . What P ∂φd(x)  R R δxi ∂ i ∂xi exactly is the partial derivative ∂x f(x)? We can write it as follows: Loosly speaking: taking a partial derivative always ap-

 ∂f(x) > pends a (co-variant) index to a tensor. The reason is that    ∂x1    the resulting tensor needs to multiply to a (contra-variant  ∂f(x)  ∂    ∂x  f(x) =  2  (57) inﬁnitesimal variation) vector from the right.   ∂x    ...     ∂f(x)  ∂xn