Lectures in Robotics Robotics, Geometry and Control - Rigid body motion and geometry Ravi Banavar1 1Systems and Control Engineering IIT Bombay HYCON-EECI Graduate School - Spring 2008 Lectures in Robotics Rigid body motion and geometry The material for these slides is largely taken from the text I A Mathematical Introduction to Robotic Manipulation - R. Murray, Z. Li and S. Sastry, CRC Press, 1994. Lectures in Robotics Rigid body motion and geometry Rigid body motion zb qb y B b za qa pab xb ya A gab xa Figure: Rigid body motion Lectures in Robotics Rigid body motion and geometry Rigid body motion Definition Rigid body motion is characterized by two properties I The distance between any two points remains invariant I The orientation of the body is preserved. (A right-handed coordinate system remains right-handed) Lectures in Robotics Rigid body motion and geometry SO(3) and SE(3) I Two groups which are of particular interest to us in robotics are SO(3) - the special orthogonal group that represents rotations - and SE(3) - the special Euclidean group that represents rigid body motions. I Elements of SO(3) are represented as 3 × 3 real matrices and satisfy RT R = I with det(R) = 1. 3 I An element of SE(3) is of the form (p; R) where p 2 R and R 2 SO(3). Lectures in Robotics Rigid body motion and geometry Frames of reference or coordinate frames I Rigid body motions are usually described using two frames of reference. One is called the body frame that remains fixed to the body and the other is the inertial frame that remains fixed in inertial space. I Position of a point p on the rigid body after the body undergoes a rotation of angle θ about the inertial z-axis. The transformation (or matrix) that maps the initial coordinates of p to the final coordiantes of p (both in the inertial frame) is the rotation matrix 0 1 cos(θ) − sin(θ) 0 B C Rzθ = @ sin(θ) cos(θ) 0 A 0 0 1 Lectures in Robotics Rigid body motion and geometry Rotations I Similarly for the y and x axes with angles α and β, we have 0 1 0 1 cos(α) 0 sin(α) 1 0 0 B C B C Ryα = @ 0 1 0 A Rxβ = @ 0 cos(β) − sin(β) A − sin(α) 0 cos(α) 0 sin(β) cos(β) Lectures in Robotics Rigid body motion and geometry Cross product and rotations T I The Cross Product between two vectors a = (a1; a2; a3) and T 3 b = (b1; b2; b3) in R is defined as 0 1 a2b3 − a3b2 B C a × b = @ a3b1 − a1b3 A a1b2 − a2b1 I The operation could be represented as the multiplication by a matrix as 0 1 0 −a3 a2 a ^ ^ B C b ! a × b = ab where a = @ a3 0 −a1 A −a2 a1 0 Notice that a^ is a skew-symmetric matrix. Lectures in Robotics Rigid body motion and geometry Angular velocity vector and skew-symmetric representation I The angular velocity vector of a rigid body can be expressed with respect to a certain basis as 0 1 0 1 !1 0 −!3 !2 B C B C ! = @ !2 A ≡ !^ = @ !3 0 −!1 A !3 −!2 !1 0 I Often it is expressed as a scalar (magnitude) multiplied to a unit vector (axis of rotation) Lectures in Robotics Rigid body motion and geometry Skew-symmetric matrices and rotations I Consider a rotation about the z-axis. In the standard basis, the z axis is (0; 0; 1) which in the skew-symmetric matrix form is 0 1 0 −1 0 B C !^ = @ 1 0 0 A 0 0 0 3×3 I The set of skew-symmetric matrices in R with the bracket operation defined as [X; Y ] = XY − YX forms a Lie algebra and is denoted as so(3). I In vector notation, the Lie bracket on so(3) between two 3 elements !1;!2 2 R is given by [!1;!2] = !1 × !2 OR [^!1; !^2] =! ^1!^2 − !^2!^1 Lectures in Robotics Rigid body motion and geometry A differential equation I Velocity of a point q on a rigid body undergoing pure rotation. Let the axis of rotation be defined by ! and we assume unit angular velocity. Then q_ (t) = 1! × q(t) =! ^q(t) I Time-invariant linear differential equation which may be integrated to give q(t) = e!^t q(0) I Rotation about the axis ! at unit velocity for θ units of time gives a net rotation of R(!; θ) = e!θ^ e(·): so(3) ! SO(3) Lectures in Robotics Rigid body motion and geometry Parametrizing rotational motion and a Lie algebra so(3) I Parametrize a rotation motion in time by a differentiable curve c(:): IR ⊃ I ! SO(3). The set 4 dc g = f j : c(t) 2 SO(3) and c(0) = Ig dt t=0 is the set of all tangent vectors at the identity of SO(3). It consists of all skew-symmetric matrices of dimension 3 called so(3). Lectures in Robotics Rigid body motion and geometry Some examples I Consider a rotation about the axis (0; 0; 1) in the standard basis parametrized as follows 0 1 cos(t) − sin(t) 0 B C Rz (:): t ! SO(3) Rz (t) = @ sin(t) cos(t) 0 A 0 0 1 I Let the axis of rotation be ! = (1; 0; 0), assume unit angular velocity and let the time of rotation be t. Then the rotation achieved is 0 1 0 1 0 1 0 0 0 0 0 0 0 0 0 1 e!^t = I+B 0 0 −t C+ B 0 0 −t C B 0 0 −t C+::: @ A 2! @ A @ A 0 t 0 0 t 0 0 t 0 Lectures in Robotics Rigid body motion and geometry The exponential map I Given the axis of rotation, the angular velocity and the time of rotation, the exponential map denoted by ”exp” gives the actual rotation. Mathematically, the exponential map is a transformation from so(3) to SO(3) given as 4 exp(^!) = I +! ^ +! ^2=2! + ::: 2 SO(3) I Remark: The axis of rotation ! is often normalized such that k!k = 1 and the angular velocity vector written as α! where α is the magnitude of the angular velocity. Lectures in Robotics Rigid body motion and geometry The exponential map Lemma The exponential map from the Lie algebra so(3) to the group SO(3) is a many-to-one map that is surjective. (A given rotation (2 SO(3))can be obtained in more than one (2 so(3)) way). Example: Consider the elements of so(3) 0 0 −α 0 1 0 0 −α − 2π 0 1 @ 0 0 α A @ 0 0 α + 2π A 0 0 0 0 0 0 Both yield the same value for exp(·) Lectures in Robotics Rigid body motion and geometry Axis of rotation and angular velocity I Consider the rotation of a rigid body where the rotation is parametrized in time by a curve R(t) 2 SO(3). Let the position (coordinates) of a point fixed on the rigid body, with respect to an inertial frame (A) and a body fixed frame (B) be qa and qb respectively. _ qa = Rabqb va = Rabqb since q_ b = 0 Lemma The matrices R_ (t)R−1(t) and R−1(t)R_ (t) both belong to the Lie algebra so(3). Lectures in Robotics Rigid body motion and geometry Skew-symmetric representations I Consider the identity R(t)R−1(t) = I On differentiation we have R_ (t)R−1(t) = −R(t)R−_ 1(t) = −(R_ (t)R−1(t))T Recall RT = R−1. −1 _ I Similarly R (t)R(t) is also skew-symmetric. I Now _ _ −1 _ −1 s va = Rabqb = RabRab Rabqb = RabRab qa =! ^abqa 4 s _ −1 where !^ab = RabRab 2 so(3) is termed the spatial angular velocity. Physically, the spatial angular velocity corresponds to the instantaneous angular velocity of the object as seen from the spatial coordinate frame. Lectures in Robotics Rigid body motion and geometry Body and spatial angular velocities I Similarly, we define the body angular veocity as 4 b −1 _ !^ab = Rab Rab 2 so(3) The body angular velocity describes the instantaneous angular velocity in the instantaneous body frame. I The velocity of the point in the body frame is 4 −1 −1 s b vb = Rab va = Rab !^abqa =! ^abqb Lectures in Robotics Rigid body motion and geometry Euclidean motions and groups I Suppose qa and qb are coordinates of a point q relative to frames A and B, respectively. qa = pab + Rabqb Here pab represents the position of the origin of the frame B with respect to frame A in frame A coordinates and Rab is the orientation of frame B with respect to frame A . I Appending a ”1” to the coordinates of a point ( to render the group operation as the usual matrix multiplication) ! ! ! qa Rab pab qb q¯a = = = g¯abq¯b 1 0 1 1 Lectures in Robotics Rigid body motion and geometry The Special Euclidean group SE(3) I Rigid body motion can be described in terms of members of SE(3) - the special Euclidean group expressing rigid body orientation/position. 3 I An element of SE(3) is of the form (p; R) where p 2 R describes the coordinates of the origin of a body fixed frame with respect to an inertial frame and R 2 SO(3) describes the orientation of the body-fixed frame with respect to the inertial frame.