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 2 R3 we 1 Rotations define its skew symmetric matrix as 0 0 −w w 1 B 3 2 C B C There are many ways to represent rotations in SO(3). We B C w = B w 0 −w C : (2) b B 3 1 C @ A 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 2 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 el- ement 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 2 R . The Angular velocity & derivative of a rotation matrix: We w = w 3 vector’s direction jwj determines the rotation represent angular velocities by a vector w 2 R , the axis, the vector’s length jwj = θ determins the rota- direction w determines the rotation axis, the length tion angle (in radians, using the right thumb conven- jwj 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 2 R3 on rotation matrix R(t) and the body’s angular velocity a vector x 2 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 θ = jwj 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 2 R4; Here (0; w) 2 R is a four-vector; for jwj = 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 01 − r − r r − r r + r 1 B 22 33 12 03 13 02 C B C B C -1 -1 R = B r + r 1 − r − r r − r C (ei − rir) ◦ r = ei ◦ r − ri(1; 0; 0; 0) (17) B 12 03 11 33 23 01 C @ A 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 p 1 quaternion r¯ = p1 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 = p [ei ◦ r ]1:3 : (19) 2 13 31 0 (11) r2 r1 = (R32 − R23)=(4r0) : (12) 2 Transformations Angular velocity ! quaternion velocity Given an an- gular velocity w 2 R3 and a current quaterion r(t) 2 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 jwj = 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 ac- celerations 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 0 -1 BR t C r = r (36) T = B C (20) @ 0 1A 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).