Rigid Body Motion and Geometry

Home , Motion (geometry), Rigid transformation

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

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

Deﬁnition 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 ∈ R and R ∈ 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 ﬁxed to the body and the other is the inertial frame that remains ﬁxed 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 ﬁnal coordiantes of p (both in the inertial frame) is the rotation matrix   cos(θ) − sin(θ) 0   Rzθ =  sin(θ) cos(θ) 0  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     cos(α) 0 sin(α) 1 0 0     Ryα =  0 1 0  Rxβ =  0 cos(β) − sin(β)  − 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 deﬁned as   a2b3 − a3b2   a × b =  a3b1 − a1b3  a1b2 − a2b1

I The operation could be represented as the multiplication by a matrix as   0 −a3 a2 a ˆ ˆ   b → a × b = ab where a =  a3 0 −a1  −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     ω1 0 −ω3 ω2     ω =  ω2  ≡ ωˆ =  ω3 0 −ω1  ω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 0  0 0 0

3×3 I The set of skew-symmetric matrices in R with the bracket operation deﬁned 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 ∈ 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 deﬁned 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 = { | : c(t) ∈ SO(3) and c(0) = I} 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   cos(t) − sin(t) 0   Rz (.): t → SO(3) Rz (t) =  sin(t) cos(t) 0  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 0 0 0 0 0 0 0 0 1 eωˆt = I+ 0 0 −t +  0 0 −t   0 0 −t +...   2!     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! + ... ∈ 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 (∈ SO(3))can be obtained in more than one (∈ so(3)) way). Example: Consider the elements of so(3)

 0 −α 0   0 −α − 2π 0   0 0 α   0 0 α + 2π  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) ∈ SO(3). Let the position (coordinates) of a point ﬁxed on the rigid body, with respect to an

inertial frame (A) and a body ﬁxed 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 s 4 ˙ −1 where ωˆab = RabRab ∈ 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 deﬁne the body angular veocity as

b 4 −1 ˙ ωˆab = Rab Rab ∈ 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 ∈ R describes the coordinates of the origin of a body ﬁxed frame with respect to an inertial frame and R ∈ SO(3) describes the orientation of the body-ﬁxed frame with respect to the inertial frame.

I To make the group operation a matrix multiplication in the usual sense, (p, R)(∈ SE(3)) is in the form of a 4 × 4 matrix ! R p 0 1 Question : Show that the representation of SE(3) as above satisﬁes the conditions of a group under the operation of matrix multiplication Lectures in Robotics Rigid body motion and geometry

Rigid body velocities

I Rigid body velocities are described by elements of se(3) as ! M v 0 0

where M ∈ so(3) (recall axis of rotation) and v ∈ R3. Lectures in Robotics Rigid body motion and geometry

Twist representations

I The velocity of a point p on the link shown in the ﬁgure can be given as p˙ (t) = ω × r(t) where r(t) is the vector from the origin of the body-frame to the point p. This can be rewritten as

p˙ (t) = ω × (p(t) − q(t))

Note that p(t) and q(t) here are expressed in the inertial frame.

I We rewrite the above equation in homogeneous coordinates. First we deﬁne ! 4 ωˆ v ξˆ = 0 0

4 with v = (−ω × q) Lectures in Robotics Rigid body motion and geometry

A differential equation and the exponential map Then I ! ! ! p˙ ωˆ v p = or p¯˙ = ξˆp¯ 0 0 0 1

I We once again have a set of ﬁrst order differential equations whose solution can be expressed as

ˆ p¯(t) = eξt p¯(0)

Lemma The exponential map from the Lie algebra se(3) to the group SE(3) is a many-to-one map that is surjective. Remark Notice once again, like in the case of rotations, more than one element in se(3) may generate the same rigid body motion (∈ SE(3)) Lectures in Robotics Rigid body motion and geometry

Twist representations

I The matrix ξˆ is called a twist and belongs to se(3) - the Lie algebra associated with the Lie group SE(3). The twist coordinates are given as " # v ξ = v ∈ R3, ω ∈ R3 ω

6 I The Lie bracket on se(3) between two elements ξ1, ξ2 ∈ R is given by ˆ ˆ ˆ ˆ ˆ ˆ [ξ1, ξ2] = ξ1ξ2 − ξ2ξ1 or in twist coordinates " # ω1 × v2 − ω2 × v1 [ξ1, ξ2] = ω1 × ω2 Lectures in Robotics Rigid body motion and geometry

Motion in a plane

I If the rigid body motion is restricted to a plane (two-dimensional) then the group reduces to SE(2) and the Lie algebra to se(2). 3 The Lie bracket on se(2) between two elements ξ1, ξ2 ∈ R is given by ˆ ˆ ˆ ˆ ˆ ˆ [ξ1, ξ2] = ξ1ξ2 − ξ2ξ1 or in twist coordinates " # ω1 × v2 − ω2 × v1 [ξ1, ξ2] = 0

Here " # vi 2 ξi = vi ∈ R , ωi ∈ R ωi

4 ξ = (v, ω) Lectures in Robotics Rigid body motion and geometry

Screw motions

I An important concept in robotics which helps express the motion of revolute and prismatic joints is the notion of a screw. As the name implies, it is the type of motion exhibited by a screw. If h is the pitch of the screw, a rotation of θ radians along an axis ω results in a linear translation of hθ along the axis of the screw.

I We ask: What is the twist that achieves a certain screw motion ? From the ﬁgure and elementary physics, we can express the net motion of the screw in an inertial frame as q (the position vector to q) + eωˆ θ(p − q) (the vector from q to s) +hθω (the vector from s to f ) which can be expressed as ! ! eωθˆ (I − eωθˆ )q + hθω p 0 1 1 Lectures in Robotics Rigid body motion and geometry

Screw motions

I We equate this to a motion achieved by a twist (v, ω) ! ! eωθˆ (I − eωθˆ )(ω × v) + ωωT vθ p 0 1 1 Lectures in Robotics Rigid body motion and geometry

Twist representations - Body and Spatial velocities

Lemma The matrice g˙ (t)g−1(t) and g−1(t)g˙ (t) both belong to the Lie algebra se(3). Consider R p RT −RT p g = g−1 = 0 1 0 1

and RT RR˙ T p˙ g−1g˙ = 0 0

RR˙ T −RR˙ T p + p˙ gg˙ −1 = 0 0 Lectures in Robotics Rigid body motion and geometry

Spatial velocity - rotation and translation

Now q R p q q¯ = a = ab ab b = g¯ q¯ a 1 0 1 1 ab b

I The spatial velocity is

˙ ˙ −1 ˙ −1 va = gabqb = gabgab gabqb = gabgab qa

˙ −1 4 ˆ s I The matrix gabgab = Vab ∈ se(3) has the form of a twist given by ! ! R˙ RT −R˙ RT p + p˙ ωˆs v s ab ab ab ab ab ab = ab ab 0 0 0 0 Lectures in Robotics Rigid body motion and geometry

Body velocity

I Similar to the case of rotations, we can describe a body frame velocity (the angular and translational velocity relative to the instantaneous body frame) as ! ! T ˙ T ˙ b b ˆ b 4 −1 RabRab Rabpab ωˆab vab V = g g˙ ab = = ∈ se(3) ab ab 0 0 0 0 Lectures in Robotics Rigid body motion and geometry

Relating the body and spatial representations

I These two velocities (spatial and body) are related by a similarity transformation as ˆ b −1 ˆ s Vab = gab Vabgab 6 I The 6 × 6 matrix which transforms twist coordinates (∈ R ) in one reference frame to another is called the adjoint

transformation (Adgab : G → G) associated with gab. ! ! ! v s R pˆ R v b s = ab = ab ab ab ab Vab s b ωab 0 Rab ωab

where ! 4 Rab pˆabRab Adgab = 0 Rab Lectures in Robotics Rigid body motion and geometry

Body velocity

Lemma If ξˆ ∈ se(3) is a twist with twist coordinates ξ ∈ R6, then for any ˆ −1 6 g ∈ SE(3), gξg is a twist with twist coordinates Adgξ ∈ R Lemma The exponential map from se(3) to SE(3) is a surjective map. Hence every rigid transformation g can be written as the exponential of some twist. Remark Notice once again, like in the case of rotations, more than one element in se(3) may generate the same rigid body motion (∈ SE(3))