Robotics 1
Position and orientation
of rigid bodies
Prof. Alessandro De Luca
Robotics 1 1
Position and orientation
right-handed orthogonal RFB rigid body
Reference Frames zB
• position: ApAB (vector ∈ IR3),
zA B expressed in RFA (use of coordinates
RFA other than Cartesian is possible, e.g.,
pAB yB cylindrical or spherical)
xB • orientation:
orthonormal 3×3 matrix
A yA (RT = R-1 ⇒ ARB BRA = I), with det = +1
xA AR
B = [x
A
B
Ay
B
Az
B ]
• xA yA zA (xB yB zB) are unit vectors (with unitary norm) of frame RFA (RFB)
• components in ARB are the direction cosines of the axes of RFB with respect
to (w.r.t.) RFA
Robotics 1 2
Rotation matrix
direction cosine of
xAT xB xAT yB xAT zB zB w.r.t. xA
AR = yAT xB yAT yB yAT zB
B
orthonormal, zAT xB zAT yB zAT zB
with det = +1 algebraic structure
chain rule property of a group SO(3)
(neutral element = I;
kR inverse element = RT)
i · iRj = kRj
orientation of RFi orientation of RFj
w.r.t. RFk w.r.t. RFk
orientation of RFj
w.r.t. RFi
NOTE: in general, the product of rotation matrices does not commute!
Robotics 1 3
Change of coordinates
z0
0p
x
0p 1p 0x
z1 • P 0p = y = x 1 + 1py 0y1 + 1pz 0z1
0p
z
1p
x
y1 1p
= 0x 0y 0z
1 1 1 y
1p
RF1 z
y0
RF0 = 0R1 1p
x0
the rotation matrix 0R1 (i.e., the orientation of RF1
w.r.t. RF0) represents also the change of
x1 coordinates of a vector from RF1 to RF0
Robotics 1 4
Orientation of frames in a plane
(elementary rotation around z-axis)
x = OB – xB = u cos θ - v sin θ
y •P y = OC + Cy = u sin θ + v cos θ
θ RFC z=w
v
C or…
u
COP
0x 0y 0z
θ RF0 C C C
x B
O
x cos θ
-sin θ
0 u u
0OP y = sin θ
cos θ 0 v = Rz(θ) v
z 0 0 1 w w
Rz(-θ) = RzT(θ)
similarly:
1 0 0 cos θ 0 sin θ
Rx(θ) = 0 cos θ - sin θ Ry(θ) = 0 1 0
0 sin θ cos θ
- sin θ 0 cos θ
Robotics 1 5
Ex: Rotation of a vector around z
y’
v’ x = |v| cos α
y = |v| sin α
y v x’ = |v| cos (α + θ) = |v| (cos α cos θ - sin α sin θ)
θ x’ = x cos θ - y sin θ
α
y’ = |v| sin (α + θ) = |v| (sin α cos θ + cos α sin θ)
x’ x x’ = x sin θ + y cos θ
O
z’ = z
or…
x’ cos θ - sin θ 0 x x …as
y’ = sin θ cos θ 0 y = Rz(θ) y before!
z’ 0 0 1 z z
Robotics 1 6
Equivalent interpretations
of a rotation matrix
the same rotation matrix, e.g., Rz(θ), may represent:
v’
RFC
v
• P RFC θ
θ θ
RF0 RF0
the orientation of a rigid the change of coordinates the vector
body with respect to a from RFC to RF0 rotation operator
reference frame RF0 ex: 0P = Rz(θ) CP ex: v’ = Rz(θ) v
ex: [0xc 0yc 0zc] = Rz(θ)
the rotation matrix 0RC is an operator
superposing frame RF0 to frame RFC
Robotics 1 7
Composition of rotations
1R brings RF1 on RF2 2R brings RF2 on RF3
brings RF0 on RF1 2 3
0R
1
p23 = 0 • 3p
p01 = 0 p12 = 0 RF2
RF3
RF1
RF0
a comment on computational complexity
0p 63 products
= (0R1 1R2 2R3) 3p = 0R3 3p
42 summations
0p = 0R1 (1R2 (2R33p)) 27 products
2p
18 summations
1p
Robotics 1 8
Axis/angle representation
z0
DATA
rz
• unit vector r (║r║ = 1)
•P
r • θ (positive if counterclockwise, as
v seen from an “observer” oriented like r
z1 y1 with the head placed on the arrow)
θ
RF1 v’ ry DIRECT PROBLEM
RF0 y0
find
rx
R(θ,r) = [0x1 0y1 0z1]
x0 RF1 is the result of rotating
RF0 by an angle θ around
the unit vector r
such that
0P= R(θ,r) 1P 0v’ = R(θ,r) 0v
x1
Robotics 1 9
Axis/angle: Direct problem
z0 R(θ,r) = C Rz(θ) CT
1
C sequence of three rotations
C-1 = CT r
3 C= n s r
z1 2 y1
Rz(θ)
RF1 after the first rotation
RF0
y0 the z-axis coincides with r
sequence of 3 rotations that n and s are orthogonal
x0 bring frame RF0 to superpose unit vectors such that
with frame RF1 n × s = r, or
nysz - synz = rx
x1
nzsx - sznx = ry
nxsy - sxny = rz
Robotics 1 10
Axis/angle: Direct problem
solution
R(θ,r) = C Rz(θ) CT
cθ - sθ 0 nT hint: use
- outer product of two vectors
R(θ,r) = n s r sθ cθ 0 sT - dyadic form of a matrix
0 0 1 rT - matrix product as product of dyads
= r rT + (n nT + s sT) cθ + (s nT - n sT) sθ
taking into account that
C CT = n nT + s sT + r rT = I , and that
0 -rz ry
skew-symmetric(r):
s nT - n sT = 0 -rx = S(r)
r × v = S(r)v = - S(v)r
0
depends only
R(θ,r) = r rT + (I - r rT) cθ + S(r) sθ = RT(-θ,r) = R(-θ,-r)
on r and θ !!
Robotics 1 11
Final expression of R(θ,r)
developing computations…
R(θ,r) =
rx2(1- cosθ)+cosθ rxry(1- cosθ)-rzsinθ rxrz(1- cos θ)+rysinθ
rxry(1- cosθ)+rzsinθ ry2(1- cosθ)+cosθ ryrz(1- cos θ)-rxsinθ
rxrz(1- cosθ)-rysinθ ryrz(1- cos θ)+rxsinθ rz2(1- cosθ)+cosθ
Robotics 1 12
Axis/angle: a simple example
R(θ,r) = r rT + (I - r rT) cθ + S(r) sθ
0
r= 0 = z0
1
⎡0 0 0 ⎤ ⎡1 0 0 ⎤ ⎡0 −1 0⎤
R(θ,r) = ⎢0 0 0 ⎥ + ⎢0 1 0 ⎥ cθ + ⎢1 0 0⎥ sθ
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣0 0 1 ⎦ ⎣0 0 0 ⎦ ⎣0 0 0 ⎦
⎡cθ -sθ 0⎤
= ⎢sθ cθ 0⎥ = R z (θ)
⎢ ⎥
€ ⎣ 0 0 1 ⎦
Robotics 1 13
Axis/angle: proof of Rodriguez formula
v’ = R(θ,r) v
v’ = v cos θ + (r × v) sin θ + (1 - cos θ)(rTv) r
proof:
R(θ,r) v = (r rT + (I - r rT) cos θ + S(r) sin θ)v
= r rT v (1 - cos θ) + v cos θ + (r × v) sin θ
q.e.d.
Robotics 1 14
Properties of R(θ,r)
1. R(θ,r)r = r (r is the invariant axis in this rotation)
2. when r is one of the coordinate axes, R boils down to one
of the known elementary rotation matrices
3. (θ,r) → R is not an injective map: R(θ,r) = R(-θ,-r)
4. det R = +1 = Π λi (eigenvalues) identities in
brown hold
5. tr(R) = tr(r r ) + tr(I - r r )cθ = 1 + 2 cθ = Σ λi for any matrix!
T T
1. ⇒ λ1 = 1
4. & 5. ⇒ λ2 + λ3 = 2 cθ ⇒ λ2 - 2 cθ λ + 1 = 0
⇒ λ2,3 = cθ ± √c2θ - 1 = cθ ± i sθ = e±i θ
all eigenvalues λ have unitary module (⇐ R orthonormal)
Robotics 1 15
Axis/angle: Inverse problem
GIVEN a rotation matrix R,
FIND a unit vector r and an angle θ such that
R = r rT + (I - r rT) cos θ + S(r) sin θ = R(θ,r)
Note first that tr(R) = R11 + R22 + R33 = 1 + 2 cos θ
; so, one could solve
R11 + R22 + R33 - 1
θ = arcos
2
but:
• provides only values in [0,π] (thus, never negative angles θ …)
• loss of numerical accuracy for θ → 0
Robotics 1 16
Axis/angle: Inverse problem
solution
from 0 R12-R21 R13-R31 0 -rz ry
R - RT = 0 R23-R32 = 2 sin θ 0 -rx
0 0
it follows
1
║r║ = 1 ⇒ sin θ = ± √ (R12 - R21)2 + (R13 - R31)2 + (R23 - R32)2 (*)
2
(**)
θ = ATAN2 {±√ (R12 - R21)2 + (R13 - R31)2 + (R23 - R32)2, R11 + R22 + R33 - 1}
see next slide can be used only if
rx R32 - R23
1
r= ry = R13 - R31 sin θ ≠ 0
2 sin θ
rz R21 - R12 (test made in advance
on the expression (*) of sin θ
in terms of the Rij’s)
Robotics 1 17
ATAN2 function
arctangent with output values “in the four quadrants”
two input arguments
takes values in [-π ,+π ]
undefined only for (0,0)
uses the sign of both arguments to define the output quadrant
based on arctan function with output values in [-π /2,+π /2]
available in main languages (C++, Matlab, …)
Robotics 1 18
Singular cases
(use when sin θ = 0)
if θ = 0 from (**), there is no given solution for r
(rotation axis is undefined)
if θ = ±π from (**), then set sin θ = 0, cos θ = -1
⇒ R = 2r rT - I
resolving
multiple signs
rx ±√(R11 + 1)/2
rx ry = R12/2 ambiguities
r= ry = ±√(R22 + 1)/2 with rx rz = R13/2 (always two
ry rz = R23/2 solutions,
rz ±√(R33 + 1)/2 of opposite
sign)
⎛−1 0 0 ⎞
⎜ 1 1
⎟
exercise: determine the two solutions (r, θ) for R = ⎜ 0 − 2 − 2 ⎟
⎜ 0 − 1 1 ⎟
⎝ 2 2 ⎠
Robotics 1 19
€
Unit quaternion
to eliminate undetermined and singular cases arising in
the axis/angle representation, one can use the unit
quaternion representation
Q = {η, ε} = {cos(θ/2), sin(θ/2) r}
a scalar 3-dim vector
η2 + ║ε║2 = 1 (thus, “unit ...”)
(θ, r) and (-θ, -r) gives the same quaternion Q
the absence of rotation is associated to Q = {1, 0}
unit quaternions can be composed with special rules (in
a similar way as in a product of rotation matrices)
Q 1*Q 2 = {η1η2 - ε1Tε2, η1ε2 + η2ε1 + ε1×ε2}
Robotics 1 20
