Introduction to Robotics (Fag 3480) · Ch. 2: Rigid Body Motions and Homogeneous Transforms. Fag 3480 - Introduction to Robotics side 8 Rotation matrices as projections. Projecting

Introduction to Robotics (Fag 3480)Vår 2011

Robert Wood (Harward Engineeering and Applied Sciences-Basis)Ole Jakob Elle, PhD (Modified for IFI/UIO)

Førsteamanuensis II, Institutt for InformatikkUniversitetet i Oslo

Seksjonsleder Teknologi, Intervensjonssenteret, Oslo Universitetssykehus (Rikshospitalet)

Fag 3480 - Introduction to Roboticsside 2

Ch. 3: Forward and Inverse Kinematics


Industrial robotsHigh precision and repetitive tasks

Pick and place, painting, etc

Hazardous environments


Common configurations: elbow manipulatorAnthropomorphic arm: ABB IRB1400 or KUKA

Very similar to the lab arm NACHI (RRR)


Simple example: control of a 2DOF planar manipulator

Move from ‘home’ position and follow the path AB with a constant contact force F all using visual feedback


Coordinate frames & forward kinematicsThree coordinate frames:

Positions:

Orientation of the tool frame:0 1

2

( )( )

=

11

11

1

1

sincos

θθ

aa

yx

( ) ( )( ) ( ) ty

xaaaa

yx

≡

++++

=

21211

21211

2

2

sinsincoscos

θθθθθθ

0 1 2

+++−+

=

⋅⋅⋅⋅

=)cos()sin()sin()cos(

ˆˆˆˆˆˆˆˆ

2121

2121

0202

020202 θθθθ

θθθθyyyxxyxx

R

=

=

10ˆ

01ˆ 00 yx ,

++−

=

++

=)cos()sin(ˆ

)sin()cos(ˆ

21

212

21

212 θθ

θθθθθθ

yx ,


Ch. 2: Rigid Body Motions and Homogeneous Transforms


Rotation matrices as projections

Projecting the axes of from o1 onto the axes of frame o0

Alternate approach

⋅⋅

=

⋅⋅

=01

0101

01

0101 ˆˆ

ˆˆˆˆˆˆ

yyxy

yyxxx

x ,

−=

−

+

=

⋅⋅⋅⋅

=

θθθθ

θθπ

πθθ

cossinsincos

cosˆˆ2

cosˆˆ

2cosˆˆcosˆˆ

ˆˆˆˆˆˆˆˆ

0101

0101

0101

010101

yyyx

xyxx

yyyxxyxx

R


Properties of rotation matricesSummary:

Columns (rows) of R are mutually orthogonal

Each column (row) of R is a unit vector

The set of all n x n matrices that have these properties are called the Special Orthogonal group of order n

( ) 1det

1

== −

RRRT

( )nSOR∈


3D rotations

( )3ˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆ

010101

010101

01010101 SO

zzzyzxyzyyyxxzxyxx

R ∈

⋅⋅⋅⋅⋅⋅⋅⋅⋅

=

General 3D rotation:

Special cases

Basic rotation matrices

−=

−=

θθ

θθ

θθθθ

θ

θ

cos0sin010

sin0cos

cossin0sincos0001

,

,

y

x

R

R

−=

1000cossin0sincos

, θθθθ

θzR


Properties of rotation matrices (cont’d)

SO(3) is a group under multiplication

Closure:

Identity:

Inverse:

Associativity:

In general, members of SO(3) do not commute

( ) ( )33 2121 SORRSORR ∈⇒∈ , if

( )3100010001

SOI ∈

=

1−= RRT

( ) ( )321321 RRRRRR = Allows us to combine rotations:bcabac RRR =

1221 RRRR ≠


Rotating a vectorAnother interpretation of a rotation matrix:

Rotating a vector about an axis in a fixed frame

Ex: rotate v0 about y0 by π/2

=

110

0v

=

−=

−=

=

=

011

110

001010100

110

cos0sin010

sin0cos

2/

02/,

1

πθ

π

θθ

θθ

vRv y


Rotation matrix summaryThree interpretations for the role of rotation matrix:

Representing the coordinates of a point in two different frames

Orientation of a transformed coordinate frame with respect to a fixed frame

Rotating vectors in the same coordinate frame


Compositions of rotationsw/ respect to the current frame

Ex: three frames o0, o1, o2

This defines the composition law for successive rotations about the currentreference frame: post-multiplication

202

0

212

1

101

0

pRp

pRp

pRp

=

=

=21

201

0 pRRp = 12

01

02 RRR =


Compositions of rotationsEx: R represents rotation about the current y-axis by φ followed by θ about

the current z-axis

−

−=

−

−=

=

φθφθφθθ

φθφθφθθθθ

φφ

φφθφ

cossinsincossin0cossin

sinsincoscoscos

1000cossin0sincos

cos0sin010

sin0cos,, zy RRR


Compositions of rotationsw/ respect to a fixed reference frame (o0)

Let the rotation between two frames o0 and o1 be defined by R10

Let R be a desired rotation w/ respect to the fixed frame o0

Using the definition of a similarity transform, we have:

This defines the composition law for successive rotations about a fixed reference frame: pre-multiplication

( )[ ] 01

01

101

01

02 RRRRRRR ==

−


Compositions of rotationsEx: we want a rotation matrix R that is a composition of φ about y0 (Ry,φ) and then θ about z0

(Rz,θ)

the second rotation needs to be projected back to the initial fixed frame

Now the combination of the two rotations is:

( )θθθ

θθθ

,,,

,,1

,02

yzy

yzy

RRRRRRR

−

−

=

=

[ ] φθφθφφ ,,,,,, yzyzyy RRRRRRR == −


Compositions of rotationsSummary:

Consecutive rotations w/ respect to the current reference frame:

Post-multiplying by successive rotation matrices

w/ respect to a fixed reference frame (o0)

Pre-multiplying by successive rotation matrices

We can also have hybrid compositions of rotations with respect to the current and a fixed frame using these same rules


Parameterizing rotationsThere are three parameters that need to be specified to create

arbitrary rigid body rotations

We will describe three such parameterizations:

Euler angles

Roll, Pitch, Yaw angles

Axis/Angle


Parameterizing rotationsEuler angles

Rotation by φ about the z-axis, followed by θ about the current y-axis, then ψ about the current z-axis

−+−+−−−

=

−

−

−==

θψθψθ

θφψφψθφψφψθφ

θφψφψθφψφψθφ

ψψ

ψψ

θθ

θθ

φφ

φφ

ψθφ

csscsssccscsscccssccssccssccc

cssc

cs

sccssc

RRRR zyzZYZ

10000

0010

0

10000

,,,


Parameterizing rotationsRoll, Pitch, Yaw angles

Three consecutive rotations about the fixed principal axes:

Yaw (x0) ψ, pitch (y0) θ, roll (z0) φ

−+−+++−

=

−

−

−=

=

ψθψθθ

ψθφψφψθφψφθφ

ψθφψφψθφψφθφ

ψψ

ψψ

θθ

θθ

φφ

φφ

ψθφ

ccscscssscssscccs

cscssssccscc

cssc

cs

sccssc

RRRR xyzXYZ

00

001

0010

0

10000

,,,


Parameterizing rotationsAxis/Angle representation

Any rotation matrix in SO(3) can be represented as a single rotation about a suitable axis through a set angle

For example, assume that we have a unit vector:

Given θ, we want to derive Rk,θ:

Intermediate step: project the z-axis onto k:

Where the rotation R is given by:

=

z

y

x

kkk

k̂

1,,

−= RRRR zk θθ

αβθβαθ

βα

−−=⇒

=

,,,,,,

,,

zyzyzk

yz

RRRRRRRRR


Parameterizing rotationsAxis/Angle representation

This is given by:

Inverse problem:

Given arbitrary R, find k and θ

++−−+++−+

=

θθθθθθ

θθθθθθ

θθθθθθ

θ

cvkskvkkskvkkskvkkcvkskvkkskvkkskvkkcvk

R

zxzyyzx

xzyyzyx

yzxzyxx

k2

2

2

,

( )

−−−

=

−

= −

1221

3113

2332

1

sin21ˆ

21cos

rrrrrr

k

RTr

θ

θ


Rigid motionsRigid motion is a combination of rotation and translation

Defined by a rotation matrix (R) and a displacement vector (d)

the group of all rigid motions (d,R) is known as the Special Euclidean group, SE(3)

Consider three frames, o0, o1, and o2 and corresponding rotation matrices R21, and

R10

Let d21 be the vector from the origin o1 to o2, d1

0 from o0 to o1

For a point p2 attached to o2, we can represent this vector in frames o0 and o1:

( )3

3R∈

∈

dSOR

( ) ( )33 3 SOSE ×= R

( )01

12

01

212

01

01

12

212

01

01

101

0

12

212

1

ddRpRR

ddpRR

dpRp

dpRp

++=

++=

+=

+=


Homogeneous transformsWe can represent rigid motions (rotations and translations) as matrix

multiplication

Define:

Now the point p2 can be represented in frame o0:

Where the P0 and P2 are:

=

=

10

1012

121

2

01

010

1

dRH

dRH

212

01

0 PHHP =

=

=

1 ,

1

22

00 p

Pp

P


Homogeneous transformsThe matrix multiplication H is known as a homogeneous

transform and we note that

Inverse transforms:

( )3SEH ∈

−=−

101 dRRH

TT


Homogeneous transformsBasic transforms:

Three pure translation, three pure rotation

=

=

=

1000100

00100001

10000100

0100001

100001000010

001

,

,

,

c

b

a

cz

by

ax

Trans

Trans

Trans

−

=

−=

−

=

100001000000

100000001000

100000000001

,

,

,

γγ

γγ

γ

ββ

ββ

β

αα

ααα

cssc

cs

sc

cssc

z

y

x

Rot

Rot

Rot


Ch. 3: Forward and Inverse Kinematics


Recap: rigid motionsRigid motion is a combination of rotation and translation

Defined by a rotation matrix (R) and a displacement vector (d)

the group of all rigid motions (d,R) is known as the Special Euclidean group, SE(3)

We can represent rigid motions (rotations and translations) as matrix multiplication

The matrix multiplication H is known as a homogeneous transform and we note that

Inverse transforms:

−=−

101 dRRH

TT

=

10dR

H


Recap: homogeneous transforms

Basic transforms:

Three pure translation, three pure rotation

=

=

=

1000100

00100001

10000100

0100001

100001000010

001

,

,

,

c

b

a

cz

by

ax

Trans

Trans

Trans

−

=

−=

−

=

100001000000

100000001000

100000000001

,

,

,

γγ

γγ

γ

ββ

ββ

β

αα

ααα

cssc

cs

sc

cssc

z

y

x

Rot

Rot

Rot


ExampleEuler angles: we have only discussed ZYZ Euler

angles. What is the set of all possible Euler angles that can be used to represent any rotation matrix?


Answer - Euler

XYZ, YZX, ZXY, XYX, YZY, ZXZ, XZY, YXZ, ZYX, XZX, YXY, ZYZ

ZZY cannot be used to describe any arbitrary rotation matrix since two consecutive rotations about the Z axis can be composed into one rotation


ExampleCompute the homogeneous transformation

representing a translation of 3 units along the x-axis followed by a rotation of π/2 about the current z-axis followed by a translation of 1 unit along the fixed y-axis


Answer – Homogeneous Transforms

−

=

−

=

=

1000010010013010

1000010000010010

1000010000103001

1000010010100001

2/,3,1, πzxy TTTT


Forward kinematics introductionChallenge: given all the joint parameters of a manipulator, determine the position

and orientation of the tool frame

Tool frame: coordinate frame attached to the most distal link of the manipulator

Inertial (base) frame: fixed (immobile) coordinate system fixed to the most proximal link of a manipulator

Therefore, we want a mapping between the tool frame and the inertial frame

This will be a function of all joint parameters and the physical geometry of the manipulator

Purely geometric: we do not worry about joint torques or dynamics

(yet!)


Convention

A n-DOF manipulator will have n joints (either revolute or prismatic) and n+1 links (since each joint connects two links)

We assume that each joint only has one DOF. Although this may seem like it does not include things like spherical or universal joints, we can think of multi-DOF joints as a combination of 1DOF joints with zero length between them

The o0 frame is the inertial frame (or base frame)

on is the tool frame

Joint i connects links i-1 and i

The oi is connected to link i

Joint variables, qi

=prismatic is joint ifrevolute is joint if

idi

qi

ii

θ


Convention

We said that a homogeneous transformation allowed us to express the position and orientation of oj with respect to oi

what we want is the position and orientation of the tool frame with respect to the inertial frame

An intermediate step is to determine the transformation matrix that gives position and orientation of oi with respect to oi-1: Ai

Now we can define the transformation oj to oi as:

( ) ijjiji

TI

AAAAT

ji

jijiiij

>=<

=

−

−++

if if if

1

21 ...


Convention

Finally, the position and orientation of the tool frame with respect to the inertial frame is given by one homogeneous transformation matrix:

For a n-DOF manipulator

Thus, to fully define the forward kinematics for any serial manipulator, all we need to do is create the Ai transformations and perform matrix multiplication

But there are shortcuts…

( ) ( ) ( )nnnnn qAqAqAToRH ⋅⋅⋅==

= 2211

000

10


The Denavit-Hartenberg (DH) Convention

Representing each individual homogeneous transformation as the product of four basic transformations:

−

−

=

−

−

=

=

10000

100000000001

100001000010

001

1000100

00100001

100001000000

,,,,

i

i

i

i

i

xaxdzzi

dcssascccscasscsc

cssc

a

dcssc

A

ii

iiiiii

iiiiii

ii

iiii

ii

iiii

αα

θαθαθθ

θαθαθθ

αα

ααθθ

θθ

αθ RotTransTransRot


The Denavit-Hartenberg (DH) Convention

Four DH parameters:

ai: link length

αi: link twist

di: link offset

θi: joint angle

Since each Ai is a function of only one variable, three of these will be constant for each link

di will be variable for prismatic joints and θi will be variable for revolute joints

But we said any rigid body needs 6 parameters to describe its position and orientation

Three angles (Euler angles, for example) and a 3x1 position vector

So how can there be just 4 DH parameters?...


Existence and uniquenessWhen can we represent a homogeneous transformation using the 4 DH parameters?

For example, consider two coordinate frames o0 and o1

There is a unique homogeneous transformation between these two frames

Now assume that the following holds:

DH1: perpendicular ->

DH2: intersects ->

If these hold, we claim that there

exists a unique transformation A:

01 ˆˆ zx ⊥

01 ˆˆ zx ∩

=

=

10

01

01

,,,,

oR

A xaxdzz αθ RotTransTransRot


Existence and uniquenessProof:

We assume that R10 has the form:

Use DH1 to verify the form of R10

Since the rows and columns of R10 must be unit vectors:

The remainder of R10 follows from the properties of rotation matrices

Therefore our assumption that there exists a unique θ and α that will give us R10

is correct given DH1

0100

0ˆˆ

31

31

21

11

00

0101

==

⇒

=⋅⇒⊥

rrrr

zxzxT

αθ ,,01 xz RRR =

=

3332

232221

13121101

0 rrrrrrrr

R

1

12

332

32

221

211

=+

=+

rr

rr


Existence and uniquenessProof:

Use DH2 to determine the form of o10

Since the two axes intersect, we can represent the line between the two frames as a linear combination of the two axes (within the plane formed by x1 and z0)

=

+

=⇒

+=⇒∩

dasac

sc

ado

axdzozx

θ

θ

θ

θ

0100

ˆˆ

01

01

00

0101


Physical basis for DH parameters

ai: link length, distance between the z0 and z1 (along x1)

αi: link twist, angle between z0 and z1 (measured around x1)

di: link offset, distance between o0 and intersection of z0 and x1 (along z0)

θi: joint angle, angle between x0 and x1 (measured around z0)

positive convention:


Assigning coordinate framesFor any n-link manipulator, we can always choose coordinate frames such that DH1 and DH2

are satisfied

The choice is not unique, but the end result will always be the same

Choose zi as axis of rotation for joint i+1

z0 is axis of rotation for joint 1, z1 is axis of rotation for joint 2, etc

If joint i+1 is revolute, zi is the axis of rotation of joint i+1

If joint i+1 is prismatic, zi is the axis of translation for joint i+1


Assigning coordinate framesAssign base frame

Can be any point along z0

Chose x0, y0 to follow the right-handed convention

Now start an iterative process to define frame i with respect to frame i-1

Consider three cases for the relationship of zi-1 and zi:

zi-1 and zi are non-coplanar

zi-1 and zi intersect

zi-1 and zi are parallelzi-1 and zi are coplanar


Assigning coordinate frameszi-1 and zi are non-coplanar

There is a unique shortest distance between the two axes

Choose this line segment to be xi

oi is at the intersection of zi and xi

Choose yi by right-handed convention


Assigning coordinate frames

zi-1 and zi intersect

Choose xi to be normal to the plane defined by ziand zi-1

oi is at the intersection of zi and xi



Assigning coordinate frameszi-1 and zi are parallel

Infinitely many normals of equal length between ziand zi-1

Free to choose oi anywhere along zi, however if we choose xi to be along the normal that intersects at oi-1, the resulting di will be zero



Assigning tool frameThe previous assignments are valid up to frame n-1

The tool frame assignment is most often defined by the axes n, s, a:

a is the approach direction

s is the ‘sliding’ direction (direction along which the grippers open/close)

n is the normal direction to a and s


Example 1: two-link planar manipulator

2DOF: need to assign three coordinate frames

Choose z0 axis (axis of rotation for joint 1, base frame)

Choose z1 axis (axis of rotation for joint 2)

Choose z2 axis (tool frame)

This is arbitrary for this case since we have described no wrist/gripper

Instead, define z2 as parallel to z1 and z0 (for consistency)

Choose xi axes

All zi’s are parallel

Therefore choose xi to intersect oi-1


Example 1: two-link planar manipulator

Now define DH parameters

First, define the constant parameters ai, αi

Second, define the variable parameters θi, di

The αi terms are 0 because all zi are parallel

Therefore only θi are variable

link ai αi di θi

1 a1 0 0 θ1

2 a2 0 0 θ2

−

=

−

=

10000100

00

10000100

00

2222

2222

21111

1111

1sacscasc

Asacscasc

A ,

++−

==

=

10000100

00

122111212

122111212

210

2

10

1

sasacscacasc

AAT

AT


Example 2: three-link cylindrical robot

3DOF: need to assign four coordinate frames

Choose z0 axis (axis of rotation for joint 1, base frame)

Choose z1 axis (axis of translation for joint 2)

Choose z2 axis (axis of translation for joint 3)

Choose z3 axis (tool frame)

This is again arbitrary for this case since we have described no wrist/gripper

Instead, define z3 as parallel to z2


Example 2: three-link cylindrical robot




link ai αi di θi

1 0 0 d1 θ1

2 0 -90 d2 0

3 0 0 d3 0

=

−=

−

=

1000100

00100001

1000010

01000001

1000100

0000

33

22

1

11

11

1 dA

dA

dcssc

A , ,

+−

−−

==

1000010

00

21

3111

3111

3210

3 dddccsdssc

AAAT


Example 3: spherical wrist3DOF: need to assign four coordinate frames

yaw, pitch, roll (θ4, θ5, θ6) all intersecting at one point o (wrist center)


Example 3: spherical wristlink ai αi di θi

4 0 -90 0 θ4

5 0 90 0 θ5

6 0 0 d6 θ6




−

=

−

−

=

−

−

=

1000100

0000

100000100000

100000100000

6

66

66

355

55

244

44

1 dcssc

Acssc

Acssc

A , ,

−+−+−−−

==

10006556565

654546465464654

654546465464654

6543

6 dcccscsdssssccscsscccsdscsccssccssccc

AAAT


Next class…More examples for common configurations

Link to movie that explains how to set-up the Denavit-Hartenberg parameters :

http://en.wikipedia.org/wiki/File:Denavit-Hartenberg_Tutorial_Video.ogv#file

http://en.wikipedia.org/wiki/File:Denavit-Hartenberg_Tutorial_Video.ogv

http://en.wikipedia.org/wiki/File:Denavit-Hartenberg_Tutorial_Video.ogv

Documents

Introduction to Robotics (Fag 3480) · Ch. 2: Rigid Body Motions and Homogeneous Transforms. Fag 3480 - Introduction to Robotics side 8 Rotation matrices as projections. Projecting