robotics, geometry and control - rigid body motion and ... · pdf filelectures in robotics...

Lectures in Robotics

Robotics, Geometry and Control - Rigid bodymotion and geometry

Ravi Banavar1

1Systems and Control EngineeringIIT Bombay

HYCON-EECI Graduate School - Spring 2008


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.



Rigid body motion

y

x

z

A

B

gab

z

x

y

pab

qb

qa

a

a

a

b

b

b

Figure: Rigid body motion



Rigid body motion

DefinitionRigid 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-handedcoordinate system remains right-handed)



SO(3) and SE(3)

I Two groups which are of particular interest to us in robotics areSO(3) - the special orthogonal group that represents rotations -and SE(3) - the special Euclidean group that represents rigidbody motions.

I Elements of SO(3) are represented as 3× 3 real matrices andsatisfy

RT R = I

with det(R) = 1.

I An element of SE(3) is of the form (p,R) where p ∈ R3 andR ∈ SO(3).



Frames of reference or coordinate frames

I Rigid body motions are usually described using two frames ofreference. One is called the body frame that remains fixed to thebody and the other is the inertial frame that remains fixed ininertial space.

I Position of a point p on the rigid body after the body undergoes arotation of angle θ about the inertial z-axis. The transformation(or matrix) that maps the initial coordinates of p to the finalcoordiantes of p (both in the inertial frame) is the rotation matrix

Rzθ =

cos(θ) − sin(θ) 0sin(θ) cos(θ) 0

0 0 1



Rotations

I Similarly for the y and x axes with angles α and β, we have

Ryα =

cos(α) 0 sin(α)

0 1 0− sin(α) 0 cos(α)

Rxβ =

1 0 00 cos(β) − sin(β)

0 sin(β) cos(β)



Cross product and rotationsI The Cross Product between two vectors a = (a1,a2,a3)T and

b = (b1,b2,b3)T in R3 is defined as

a× b =

a2b3 − a3b2

a3b1 − a1b3

a1b2 − a2b1

I The operation could be represented as the multiplication by a

matrix as

b a→ a× b = ab where a =

0 −a3 a2

a3 0 −a1

−a2 a1 0

Notice that a is a skew-symmetric matrix.



Angular velocity vector and skew-symmetricrepresentation

I The angular velocity vector of a rigid body can be expressed withrespect to a certain basis as

ω =

ω1

ω2

ω3

≡ ω =

0 −ω3 ω2

ω3 0 −ω1

−ω2 ω1 0

I Often it is expressed as a scalar (magnitude) multiplied to a unit

vector (axis of rotation)



Skew-symmetric matrices and rotationsI 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 01 0 00 0 0

I The set of skew-symmetric matrices in R3×3 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

elements ω1, ω2 ∈ R3 is given by

[ω1, ω2] = ω1 × ω2 OR [ω1, ω2] = ω1ω2 − ω2ω1



A differential equation

I Velocity of a point q on a rigid body undergoing pure rotation. Letthe axis of rotation be defined by ω and we assume unit angularvelocity. Then

q(t) = 1ω × q(t) = ωq(t)

I Time-invariant linear differential equation which may beintegrated to give

q(t) = eωtq(0)

I Rotation about the axis ω at unit velocity for θ units of time givesa net rotation of

R(ω, θ) = eωθ e(·) : so(3)→ SO(3)



Parametrizing rotational motion and a Lie algebraso(3)

I Parametrize a rotation motion in time by a differentiable curvec(.) : IR ⊃ I → SO(3). The set

g 4= {dcdt|t=0 : c(t) ∈ SO(3) and c(0) = I}

is the set of all tangent vectors at the identity of SO(3). It consistsof all skew-symmetric matrices of dimension 3 called so(3).



Some examples

I Consider a rotation about the axis (0,0,1) in the standard basisparametrized as follows

Rz(.) : t → SO(3) Rz(t) =

cos(t) − sin(t) 0sin(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 rotationachieved is

eωt = I+

0 0 00 0 −t0 t 0

+12!

0 0 00 0 −t0 t 0

0 0 0

0 0 −t0 t 0

+. . .



The exponential map

I Given the axis of rotation, the angular velocity and the time ofrotation, the exponential map denoted by ”exp” gives the actualrotation. Mathematically, the exponential map is a transformationfrom so(3) to SO(3) given as

exp(ω)4= I + ω + ω2/2! + . . . ∈ SO(3)

I Remark: The axis of rotation ω is often normalized such that‖ω‖ = 1 and the angular velocity vector written as αω where α isthe magnitude of the angular velocity.



The exponential map

LemmaThe 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 0 α0 0 0

0 −α− 2π 00 0 α + 2π0 0 0

Both yield the same value for exp(·)



Axis of rotation and angular velocity

I Consider the rotation of a rigid body where the rotation isparametrized in time by a curve R(t) ∈ SO(3). Let the position(coordinates) of a point fixed on the rigid body, with respect to aninertial frame (A) and a body fixed frame (B) be qa and qb

respectively.

qa = Rabqb va = Rabqb since qb = 0

LemmaThe matrices R(t)R−1(t) and R−1(t)R(t) both belong to the Liealgebra so(3).



Skew-symmetric representationsI 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.I Similarly R−1(t)R(t) is also skew-symmetric.I Now

va = Rabqb = RabR−1ab Rabqb = RabR−1

ab qa = ωsabqa

where ωsab4= RabR−1

ab ∈ so(3) is termed the spatial angularvelocity. Physically, the spatial angular velocity corresponds tothe instantaneous angular velocity of the object as seen from thespatial coordinate frame.



Body and spatial angular velocities

I Similarly, we define the body angular veocity as

ωbab4= R−1

ab Rab ∈ so(3)

The body angular velocity describes the instantaneous angularvelocity in the instantaneous body frame.

I The velocity of the point in the body frame is

vb4= R−1

ab va = R−1ab ω

sabqa = ωb

abqb



Euclidean motions and groups

I Suppose qa and qb are coordinates of a point q relative to framesA and B, respectively.

qa = pab + Rabqb

Here pab represents the position of the origin of the frame B withrespect to frame A in frame A coordinates and Rab is theorientation of frame B with respect to frame A .

I Appending a ”1” to the coordinates of a point ( to render thegroup operation as the usual matrix multiplication)

qa =

(qa

1

)=

(Rab pab

0 1

)(qb

1

)= gabqb



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 bodyorientation/position.

I An element of SE(3) is of the form (p,R) where p ∈ R3

describes the coordinates of the origin of a body fixed frame withrespect to an inertial frame and R ∈ SO(3) describes theorientation of the body-fixed frame with respect to the inertialframe.

I To make the group operation a matrix multiplication in the usualsense, (p,R)(∈ SE(3)) is in the form of a 4× 4 matrix(

R p0 1

)Question : Show that the representation of SE(3) as above satisfiesthe conditions of a group under the operation of matrix multiplication



Rigid body velocities

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

)

where M ∈ so(3) (recall axis of rotation) and v ∈ R3.



Twist representationsI The velocity of a point p on the link shown in the figure can be

given asp(t) = ω × r(t)

where r(t) is the vector from the origin of the body-frame to thepoint 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 define

ξ4=

(ω v0 0

)

with v 4= (−ω × q)



A differential equation and the exponential mapI Then (

p0

)=

(ω v0 0

)(p1

)or ˙p = ξp

I We once again have a set of first order differential equationswhose solution can be expressed as

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

LemmaThe exponential map from the Lie algebra se(3) to the group SE(3) isa many-to-one map that is surjective.

RemarkNotice once again, like in the case of rotations, more than oneelement in se(3) may generate the same rigid body motion (∈ SE(3))



Twist representationsI The matrix ξ is called a twist and belongs to se(3) - the Lie

algebra associated with the Lie group SE(3). The twistcoordinates are given as

ξ =

[vω

]v ∈ R3, ω ∈ R3

I The Lie bracket on se(3) between two elements ξ1, ξ2 ∈ R6 isgiven by

[ξ1, ξ2] = ξ1ξ2 − ξ2ξ1

or in twist coordinates

[ξ1, ξ2] =

[ω1 × v2 − ω2 × v1

ω1 × ω2

]



Motion in a planeI 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).The Lie bracket on se(2) between two elements ξ1, ξ2 ∈ R3 isgiven by

[ξ1, ξ2] = ξ1ξ2 − ξ2ξ1

or in twist coordinates

[ξ1, ξ2] =

[ω1 × v2 − ω2 × v1

0

]Here

ξi =

[vi

ωi

]vi ∈ R2, ωi ∈ R

ξ4= (v , ω)



Screw motionsI An important concept in robotics which helps express the motion

of revolute and prismatic joints is the notion of a screw. As thename implies, it is the type of motion exhibited by a screw. If h isthe 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 figure and elementary physics, we can express the netmotion 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θω0 1

)(p1

)



Screw motions

I We equate this to a motion achieved by a twist (v , ω)(eωθ (I − eωθ)(ω × v) + ωωT vθ0 1

)(p1

)



Twist representations - Body and Spatial velocities

LemmaThe matrice g(t)g−1(t) and g−1(t)g(t) both belong to the Lie algebrase(3).Consider

g =

(R p0 1

)g−1 =

(RT −RT p0 1

)and

g−1g =

(RT R RT p

0 0

)gg−1 =

(RRT −RRT p + p

0 0

)



Spatial velocity - rotation and translation

Now

qa =

(qa1

)=

(Rab pab0 1

)(qb1

)= gabqb

I The spatial velocity is

va = gabqb = gabg−1ab gabqb = gabg−1

ab qa

I The matrix gabg−1ab4= V s

ab ∈ se(3) has the form of a twist given by(RabRT

ab −RabRTabpab + pab

0 0

)=

(ωs

ab vsab

0 0

)



Body velocity

I Similar to the case of rotations, we can describe a body framevelocity (the angular and translational velocity relative to theinstantaneous body frame) as

V bab4= g−1

ab gab =

(RT

abRab RTabpab

0 0

)=

(ωb

ab vbab

0 0

)∈ se(3)



Relating the body and spatial representations

I These two velocities (spatial and body) are related by a similaritytransformation as

V bab = g−1

ab V sabgab

I The 6× 6 matrix which transforms twist coordinates (∈ R6) inone reference frame to another is called the adjointtransformation (Adgab : G → G) associated with gab.

V sab =

(vs

ab

ωsab

)=

(Rab pabRab

0 Rab

)(vb

ab

ωbab

)

where

Adgab

4=

(Rab pabRab

0 Rab

)



Body velocity

LemmaIf ξ ∈ se(3) is a twist with twist coordinates ξ ∈ R6, then for anyg ∈ SE(3), gξg−1 is a twist with twist coordinates Adgξ ∈ R6

LemmaThe exponential map from se(3) to SE(3) is a surjective map. Henceevery rigid transformation g can be written as the exponential ofsome twist.

RemarkNotice once again, like in the case of rotations, more than oneelement in se(3) may generate the same rigid body motion (∈ SE(3))

robotics, geometry and control - rigid body motion and ... · pdf filelectures in robotics...

Documents