Click here to load reader
Upload
brice-taylor
View
279
Download
21
Embed Size (px)
Citation preview
Chapter 2Rigid Motions and
Coordinate TransformationsDr. Amit Goradia
Robot Kinematics
• Mathematical tools needed:– Vector and matrix algebra
• Rigid body transformations– Translation– Rotation
• Homogenous Transformations used to represent rigid body transformations
Forward Kinematics
• Given the various joint and link parameters, compute the end-effector (tool) location in task space.
• Given joint variables
– End-effector position and orientation, -
Formula?
),,,,,,( 654321 nqqqqqqqq
),,,,,( TAOzyxY x
y
z
Joint Frame
World Frame
Forward
Inverse
Inverse Kinematics
• Given a position in task space, compute the joint angles required to attain it.
• End effector position and orientation
• Formula for joint variables
x
y
z
),,,,,,( 654321 nqqqqqqqq
),,,,,( TAOzyx
Joint Frame
World Frame
Forward
Inverse
Example 1
0x
0y
)/(tan
kinematics Inverse
sin
cos
kinematics Forward
001
0
0
xy
ly
lx
l
Example 2: Three Link Planer Robot
• Joint Coordinates
• Task Coordinates
• Forward Kinematics– Calculate Task
Coordinates in terms of Joint Coordinates
2l
1l
3l
1
321 ,,
)cos(
)cos()cos(
3213
21211
l
llx
)sin(
)sin()sin(
3213
21211
l
lly
)( 321
,, yx
2 3
Linear Algebra Concepts
• The set of Real numbers is represented by:
• The set of positive real numbers is formally defined as:
• is an Eucledian space of real dimension
• Geometric Objects– Points– Vectors– Transformations– Coordinate frames– Lines – Rays – Planes – Normals– etc.
R
,0: xRxR
nR
n
Points v/s Vectors
• Points in represented as 3 numbers
• Vectors in represented as 3 numbers
• They are not the same thing!!!!
• They have different operations and different behaviours
Tcbap
Tcbav
3R
3R
1D Example: Time
• Point in time: class meeting time – class meets at 9:00 am
• Vector in time: class duration– class lasts 2 hours
• Operations: class at 2PM + class at 3PM ≠ class at 5PM !! 2 hour class + 3 hour class = 5 hours of classes class ends at 11AM – starts at 9AM = 3 hour class class starts at 9AM + lasts 2 hours = ends at 11AM
2 classes at 3PM ≠ one class at 6PM !! 2 classes last 3 hours = 6 hours of classes Class from 2PM to 10PM, half done at
1
22PM + 1
26PM = 4PM
("affine combination")
“Coordinate Systems” for Time
Knowing just the hour number doesn’t tell you everything…
• AM vs. PM (or use 8h00 vs 20h00)• Time zones: same point, many representations
– 10 (India) == 4:30 (UK)– to remove ambiguity, often use GMT (World frame)
• If always staying in local time zone, not important– if scheduling globally, must be careful.– convert from one time zone to another.
• Also, hours only good within one day– need to specify date & time– UNIX time: seconds since 01/01/1970, 00h00 GMT
• Notice: time durations are unaffected by all this!
Points and Vectors in Space
• Point describes a location in space– can subtract points to get a vector– can take weighted average of points to get a point
• Vector describes a displacement in space– has a magnitude and direction– can add, subtract, scale vectors– can add vector to a point to get a point– Has a dot product and cross product defined
• To represent them as three numbers, you must specify which coordinate system
Vector Interpretation
• Think of a vector as a line in 2D or 3D• Think of a matrix as a transformation on a line
or set of lines
'
'
y
x
dc
ba
y
xV
V’
Vectors: Dot Product
• Interpretation: the dot product measures to what degree two vectors are aligned
A
B
A
B
C
A+B = C(use the head-to-tail method
to combine vectors)
Vectors: Dot Product
332211
3
2
1
321 bababa
b
b
b
aaababa T
2332211
2aaaaaaaaa T
)cos(baba
Think of the dot product as a matrix multiplication
The magnitude is the dot product of a vector with itself
The dot product is also related to the angle between the two vectors
Vectors: Cross Product
• The cross product of vectors A and B is a vector C which is perpendicular to A and B
• The magnitude of C is proportional to the sine of the angle between A and B
• The direction of C follows the right hand rule – this why we call it a “right-handed coordinate system”
)sin(baba
Arithmetic for Points and Vectors
w
u
v vector+vector vector
v
w
u vector-vector vector
us
v scale a vector
qp v point+vector point
v q p point-point vector
r 1
4p
3
4q weighted average of points point
Coordinate Frames in 3D
• Origin point, and 3 orthonormal vectors for x,y,z axes (right-handed)
f
g
h
a
b
x
y
z
O
P
Q
o,x,y,z
p,a,b,c
q,f,g,h
c
Coordinates
• If you have coordinate triples such as:
• Then, with frame such as you can construct a point or vector:
• Same coordinates, different frame different point or vector– Coordinates have no real meaning without a frame– vectors don’t depend on the origin of the frame, only the axes
3
6
1
or
9
2
5
p o 3x 6
y
z
v 9
x 2
y 5
z
o,x,y,z
Coordinates of a Frame
• Suppose you have a frame • In world coordinates, might have
• But in itself always have coordinates:
p,a,b,c
p 10
3
3
a
0
1
1
b
0.707
0
0.707
c
0.707
0
0.707
p,a,b,c
p 0
0
0
a
1
0
0
b
0
1
0
c
0
0
1
Coordinate equivalences
• Given a frame: – Can have a point with some coords:
– Can have a vector with same coords:
– Formally, we have:
– Informally, p and look and act about the same• People often sloppy, don’t distinguish point from vector• Can only get away with it if you stay in same frame!• And even then need to be careful…
Ox
y
z
v
p
b
ca
Tcbap
Tcbav
zyxo
opv
vop
v
Coordinate Transformation
• Rotation only– Point p can be represented in
both coordinate frames
wvu kji wvuuvw pppP
x
y
zP
zyx kji zyxxyz pppP
uvwxyz RPP u
vw
How to relate the coordinate in these two frames?
Rotations• Basic Rotation
– , , and represent the projections of onto OX, OY, OZ axes, respectively
– Since
xp Pyp zp
wvux pppPp wxvxuxx kijiiii
wvuy pppPp wyvyuyy kjjjijj
wvuz pppPp wzvzuzz kkjkikk
wvu kji wvu pppP
Rotations• Basic Rotation Matrix
– Rotation about x-axis by angle
w
v
u
z
y
x
p
p
p
p
p
p
wzvzuz
wyvyuy
wxvxux
kkjkik
kjjjij
kijiii
x
z
y
v
wP
u
cossin0
sincos0
001
),(xRot
Rotations• Is it True?
– Rotation about x axis by angle
cossin
sincos
cossin0
sincos0
001
wvz
wvy
ux
w
v
u
z
y
x
ppp
ppp
pp
p
p
p
p
p
p
x
z
y
v
wP
u
Basic Rotation Matrices– Rotation about x-axis by angle
– Rotation about y-axis by angle
– Rotation about z-axis by angle
uvwxyz RPP
CS
SCxRot
0
0
001
),(
0
010
0
),(
CS
SC
yRot
100
0
0
),(
CS
SC
zRot
Properties of Rotation Matrices• Basic Rotation Matrix
– Obtain the coordinate of from the coordinate of
– R is an orthogonal matrix (inverse equal to transpose)
uvwxyz RPP
wzvzuz
wyvyuy
wxvxux
kkjkik
kjjjij
kijiii
R
xyzuvw QPP
TRRQ 1
31 IRRRRQR T
uvwP xyzP
<== 3X3 identity matrix
z
y
x
w
v
u
p
p
p
p
p
p
zwywxw
zvyvxv
zuyuxu
kkjkik
kjjjij
kijiii
Dot products are commutative!
Properties of Rotation Matrices
• Columns of R are unit vectors and are mutually orthogonal
• Columns of R represent a vector in the UVW frame w.r.t. XYZ frame
• Rows of r are also unit vectors and represent a basis in XYZ frame wrt UVW
• • Space of all possible rotation matrices is
denoted as (special orthogonal group of order 3)
1)det( R
)3(SO
Rotation Matrix as a Direction Cosine Matrix
• Elements of the rotation matrix are the cosines of the unsigned angles between the moving axes and the fixed axes.
• is the unsigned angle between x’ and y
yx ,'
Example•
•
•
•
yyxx ,','
2,' yx
2,' xy
0,' zz
Example 3• A point is attached to a rotating frame,
the frame rotates 60 degree about the OZ axis of the reference frame. Find the coordinates of the point relative to the reference frame after the rotation.
)2,3,4(uvwa
2
964.4
598.0
2
3
4
100
05.0866.0
0866.05.0
)60,( uvwxyz azRota
Example 4• A point is the coordinate w.r.t. the
reference coordinate system, find the corresponding point w.r.t. the rotated OU-V-W coordinate system if it has been rotated 60 degree about OZ axis.
)2,3,4(xyza
uvwa
2
964.1
598.4
2
3
4
100
05.0866.0
0866.05.0
)60,( xyzT
uvw azRota
Composition of Rotations
• Consider a point p represented in 3 coordinate frames.
• Substituting:
• Which implies:
22
1122001
100 ,, pRppRppRp
22
1100 pRRp
21
10
20 RRR
Composite Rotation Matrix• A sequence of finite rotations
– matrix multiplications do not commute– rules:
• if rotating coordinate O-U-V-W is rotating about principal axis of OXYZ frame, then Pre-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrix
• if rotating coordinate OUVW is rotating about its own principal axes, then post-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrix
Example 5• Find the rotation matrix for the following
operations:
Post-multiply if rotate about the OUVW axes
Pre-multiply if rotate about the OXYZ axes
...
axis OUabout Rotation
axisOW about Rotation
axis OYabout Rotation
Answer
SSSCCSCCSSCS
SCCCS
CSSSCCSCSSCC
CS
SCCS
SC
uRotwRotIyRotR
0
0
001
100
0
0
C0S-
010
S0C
),(),(),( 3
Composite Rotations (Intermediate Reference Frame)
• Successive Rotations about Intermediate axes:• Post-Multiply successive rotation matrices
Composite Rotations (Fixed Reference Frame)
• Successive Rotations about fixed reference frames:
• Pre-multiply successive rotation matrices
R is a rotation about the fixed Z axis
Orientation Representation Revisited
• Rotation matrix representation needs 9 elements to completely describe the orientation of a rotating rigid body.
• There are only 3 unique quantities in a rotation matrix • There are 6 redundant elements!!!!!!• Compact method to represent orientation?
Euler Angles Representation
wzvzuz
wyvyuy
wxvxux
kkjkik
kjjjij
kijiii
R
Orientation Representation
• Problems:– 6 extra elements need to be specified– Long series of computations with inherent
numerical errors causes R to loose orthogonal property. Need to orthogonalize from time to time
– Can calculate a rotation difference but canot smoothly interpolate over a difference.
• Cannot smoothly rotate from one orientation to another.
Orientation Representation• Euler Angles Representation ( , , )
– Many different types– Description of Euler angle representations
Euler Angle I Euler Angle II Roll-Pitch-Yaw
Sequence about OZ axis about OZ axis about OX axis
of about OU axis about OV axis about OY axis
Rotations about OW axis about OW axis about OZ axis
X convention Y convention Cardanian
x
y
z
u'
v'
v "
w"
w'=
=u"
v'"
u'"
w'"=
Euler Angle I, Animated
Orientation Representation• Euler Angle I
100
0cossin
0sincos
,
cossin0
sincos0
001
,
100
0cossin
0sincos
''
'
w
uz
R
RR
Euler Angle IResultant eulerian rotation matrix:
CCSSS
CSCCCSSCCSSC
SSCSCCSCSSCC
RRRR wuz '''
Euler Angle II, Animated
x
y
z
u'
v'
=v"
w"
w'=
u"
v"'
u"'
w"'=
Note the opposite (clockwise) sense of the third rotation, .
Euler Angles → Rotation Matrix• Rotation Matrix with Euler Angle II
CSSSC
SSCCSCCCCSSC
SCCCSCSCCCSS
RRRR wvz '''
Rotation Matrix → Euler Angles
)2313,(atan2 rr )3231,(atan2 rr
CSSSC
SSCCSCCCCSSC
SCCCSCSCCCSS
RRRR wvz '''
33acos r
What if θ = nπ, n Є Z??
Singularity - Gimbal Lock
• Two out of three axes are aligned
• Loss of one degree of freedom
• Apollo 11 and gimbal lock
Orientation Representation• Description of Roll Pitch Yaw
X
Y
Z`
– Description of Yaw,
Pitch, Roll• A rotation of about the
OX axis ( ) -- yaw• A rotation of about the
OY axis ( ) -- pitch• A rotation of about the
OZ axis ( ) -- roll
,xR
,yR
,zR
Roll Pitch Yaw
• Pre-multiply using the basic rotation matrices
Valid Rotation Sequences
• 6 sets of Euler Angles– ZXZ, ZYZ, XZX, XYX, YZY, YXY
• 6 sets of Cardanian angles– XYZ, YZX, ZXY, XZY, ZYX, YXZ
• Euler Rotation Theorem:– In three-dimensional space, any displacement
of a rigid body such that a point on the rigid body remains fixed, is equivalent to a rotation about a fixed axis through that point.
Universality of Euler Angles
• Consider Euler 1 notation (X Convention)
• Remember that:– Rotation matrix represents moving frame
basis wrt fixed frame basis– Columns represent moving axes wrt fixed
axes– Rows represent fixed axes wrt moving axes
CCSSS
CSCCCSSCCSSC
SSCSCCSCSSCC
RRRR wuz '''
Universality of Euler Angles
• Show that:– Parameterization of R can represent any
proper orthonormal matrix– Columns of R can be transformed into any
arbitrary set of orthonormal vectors
• Proof:– Column 3 : – Representation of unit vector with spherical
angle coordinates– Can be made to coincide with any arbitrary
vector by choosing proper angles
TCCSSSe 3
Universality of Euler Angles• Column 1:
• Where,
• Now, u1 , u2 are unit vectors and
• So u1 and u2 span the plane perpendicular to e3 and we can choose phi to generate any angle in that plane
211 u sinu cos
SS
CCSSC
CSSCC
e
S
CC
CS
S
C
21 u ,
0
u
0uuuu 213231 ee
Universality of Euler Angles
• With choice of and , we can locate the Z axis of the new coordinate system arbitrarily
• With the choice of , we can locate the X axis perpendicular to Z and at any desired angle in that plane
• Choose Y to complete the triad
• Proof complete
Angle Axis Representationa
b
( , )
cos sin ( ) 1 cos
Rot
c a b
b a b b a b
(Rodriguez formula)
Rotation of a vector by angle about axis b a
Angle Axis Representation
• Define rotation axis r and an angle of rotation about r
• Results in a compact but non-minimal representation
• Procedure– Rotate by about axis Z till r is
coplaner with axis X– Rotate by about axis Y to align
r with Z– Rotate about Z by– Return r to original position by
rotating about Y by and about z by
• Result
r
XY
Z
Angle Axis Representation
• Consider following simplification:
• Now:
Angle Axis Representation
• Total 4 parameters: – 1 vector (3 parameters)– A rotation angle about the vector
• Selected vector is normalized:
• If , then inverse is not defined.– Singularities located at
• Not unique:
0 ,0
Rotation Representation using Quaternions
• Quaternions were first introduced for complex number analysis
• Generalization of complex number representation
• 4 dimensional quantity– One scalar– A three tuple representing a vector.
• Avoid drawbacks of angle axis representation by– Distinguish between rotations of different signs– Avoid singularity at
3210 kqjqiqqQ
qqQ ,0
,0
QuaternionsInvented by Hamilton in 1843:
Along the royal canal in Dublin…
Popularized for use in rigid motion representation incomputer graphics by Shoemake in 80’s
Increases computational efficiency as it requires less number of additions and multiplications for computing a given pose of a rigid body
Quaternions: Basic Properties
1 ijkkkjjii
kjiij
ikjjk
jikki
Properties of complex dimensions
Consider 2 quaternions: qqQ
,0 ppP
,0
32103210 kpjpippkqjqiqqQP
qppqqpqpqpQP 0000 ,
Multiplication
qqqqQ ,, 0*
0*
Complex Conjugate
Quaternions
123
22
21
20
2 qqqqQLet’s consider the set of
unit quaternions:
This is a four-dimensional hypersphere, i.e. the 3-sphere3S
Therefore, the inverse of a unit quaternion is:1* QQ
)0,1(,, 00* qqqqQQ
The identity quaternion is: 0,1Q
Consider:
Quaternions
You can rotate from frame a to b:Pa *ba
aba
b PQQP
Given a unit axis, , and an angle, :
Associate a rotation with a unit quaternion as follows:
k̂
2sinˆ,
2cos
,ˆ
kQkThe associated quaternion is:
Therefore, represents the same rotation asQ Q
Let be the quaternion associated with the vector pP ii ,0 pi
bacbca QQQ Composition:1 bacacb QQQInversion:
Example 6
Rotate by
0
0
,2
12
1Q
0
0
1
,0Pa
0
0
,
0
0
1
,0
0
0
,2
12
12
12
1*PQQP ab
21
21
21
21 0,0
0
0
,
1
0
0
,000,0
21
21
21
21
What rotation is this?
Example 7
0
02
1
kFind the difference between these two axis
angle rotations:
0
0
22k
0
0
,2
12
1caQ
0
0,2
1
21
baQ 2
144 cossin
0
0,
0
0
,2
1
21
21
211
bacacb QQQ
qppqqpqpqpQP 0000 ,
21
21
21
21
212
12
1
21
21 ,,
120cos2 211
cb
332
332
332
cbk
Quaternions: Interpolations
Suppose you’re given two rotations, and
How do you calculate intermediate rotations?
1R 2R
21 1 RRRi
Do quaternions help?
21
21
1
1
QQQi
Suprisingly, this actually works
• Finds a geodesic (shortest path between points in space)
Affine Transformations
•
• In Euclidean space, an affine transformation preserves following properties– Collinearity of points– Distance ratios
• Affine transformation composed of linear transformations and a translation.
bAxx
Rigid Motions• position vector of P in {B} is transformed to position vector of P in {A}• description of {B} as seen from an observer in {A}•Rigid body motions are Affine transformations
Rotation of {B} with respect to {A}
Translation of the origin of {B} with respect to origin of {A}
Rigid Motions• Two Special Cases
1. Translation only– Axes of {B} and {A} are
parallel
2. Rotation only– Origins of {B} and {A}
are coincident
33IRBA
'oAPBB
APA rrRr
0' oAr
Homogeneous Representation• Coordinate transformation from {B} to {A}
• Homogeneous transformation matrix
'oAPBB
APA rrRr
1101 31
' PBoAB
APA rrRr
10101333
31
' PRrRT
oAB
A
BA
Position vector
Rotation matrix
Scaling
Homogeneous Transformation• Special cases
1. Translation
2. Rotation
10
0
31
13BA
BA RT
10 31
'33
oA
BA rIT
Homogeneous Coordinates• Add an extra component. 1 for a point, 0 for a vector:
• Transform a point:– Top 3 rows are the affine transform– Bottom row stays 1
• Transform a vector– Top 3 rows are the linear transform– Bottom row stays 0
Tzyx pppp 1 Tzyx vvvv 0
rRpp
p
p
rR
p
p
p
z
y
x
z
y
x
1
10
1
'
'
'
vRv
v
v
rR
v
v
v
z
y
x
z
y
x
0
10
0
'
'
'
Homogeneous Arithmetic
• Legal operations always end in 0 or 1!
vector+vector: 0
0
0
vector-vector: 0
0
0
scalar*vector: s0
0
point+vector: 1
0
1
point-point: 1
1
0
point+point: 1
1
2
scalar*point: s1
s
weighted average
affine combination
of points: 1
3
1
2
3
1
1
Example 8• Translation along Z-axis by h:
1000
100
0010
0001
),(h
hzTrans
111000
100
0010
0001
1
hp
p
p
p
p
p
hz
y
x
w
v
u
w
v
u
x
y
z P
u
vw
O, O’hx
y
z
P
u
vw
O, O’
Example 9• Rotation about the X-axis by
1000
00
00
0001
),(
CS
SCxRot
x
z
y
v
wP
u
11000
00
00
0001
1w
v
u
p
p
p
CS
SC
z
y
x
Composite Homogeneous Transformation
• Rules:– Transformation (rotation/translation) w.r.t
(X,Y,Z) (OLD FRAME), using pre-multiplication
– Transformation (rotation/translation) w.r.t (U,V,W) (NEW FRAME), using post-multiplication
Example 10• Find the homogeneous transformation matrix
(T) for the following operations:
:
axis OZabout ofRotation
axis OZ along d ofn Translatio
axis OX along a ofn Translatio
axis OXabout Rotation
Answer
44,,,, ITTTTT xaxdzz
1000
00
00
0001
1000
0100
0010
001
1000
100
0010
0001
1000
0100
00
00
CS
SC
a
d
CS
SC
Homogeneous Representation• A frame in space (Geometric
Interpretation)
x
y
z),,( zyx pppP
1000zzzz
yyyy
xxxx
pasn
pasn
pasn
F
n
sa
101333 PR
F
Principal axis n w.r.t. the reference coordinate system
(X’)
(y’)(z’)
Homogeneous Transformation
• Translation
y
z
n
sa n
sa
1000
10001000
100
010
001
zzzzz
yyyyy
xxxxx
zzzz
yyyy
xxxx
z
y
x
new
dpasn
dpasn
dpasn
pasn
pasn
pasn
d
d
d
F
oldzyxnew FdddTransF ),,(
Homogeneous Transformation
21
10
20 AAA
Composite Homogeneous Transformation Matrix
0x
0z
0y
10 A
21A
1x
1z
1y 2x
2z2y
?i
i A1 Transformation matrix for adjacent coordinate frames
Chain product of successive coordinate transformation matrices
Example 11• For the figure shown below, find the 4x4 homogeneous transformation
matrices and for i=1, 2, 3, 4, 5
1000zzzz
yyyy
xxxx
pasn
pasn
pasn
F
ii A1
iA0
0x 0y
0z
a
b
c
d
e
1x
1y
1z
2z2x
2y
3y3x
3z
4z
4y4x
5x5y
5z
1000
010
100
0001
10
da
ceA
1000
0100
001
010
20 ce
b
A
1000
0001
100
010
21 da
b
A
Can you find the answer by observation based on the geometric interpretation of homogeneous transformation matrix?
Inverse Homogenous Transformation
101 PRR
TTT
441
10
0
1010
I
RRPRPRRTT
TTT
Inverse of the rotation submatrix is equivalent to its transpose