Position and orientation of rigid bodies

Transcription

1 Robotics 1 Position and orientation of rigid bodies Prof. Alessandro De Luca Robotics 1 1

2 Position and orientation right-handed orthogonal Reference Frames RF A A z A p AB B RF B z B x B y A rigid body y B position: A p AB (vector I R 3 ), expressed in RF A (use of coordinates other than Cartesian is possible, e.g., cylindrical or spherical) orientation: orthonormal 3 3 matrix (R T = R -1 A R B B R A = I), with det = +1 x A A R B = [ A x B A y B A z B ] x A y A z A (x B y B z B ) are unit vectors (with unitary norm) of frame RF A (RF B ) components in A R B are the direction cosines of the axes of RF B with respect to (w.r.t.) RF A Robotics 1 2

3 A R B = orthonormal, with det = +1 orientation of RF i w.r.t. RF k Rotation matrix x A T x B x A T y B x A T z B y A T x B y A T y B y A T z B z A T x B z A T y B z A T z B chain rule property k R i i R j = k R j orientation of RF j w.r.t. RF i direction cosine of z B w.r.t. x A algebraic structure of a group SO(3) (neutral element = I; inverse element = R T ) orientation of RF j w.r.t. RF k NOTE: in general, the product of rotation matrices does not commute! Robotics 1 3

4 Change of coordinates z p x z 1 P p = p y p z = 1 p x x p y y p z z 1 y 1 = x 1 y 1 z 1 1 p x 1 p y RF 1 1 p z RF y = R 1 1 p x x 1 the rotation matrix R 1 (i.e., the orientation of RF 1 w.r.t. RF ) represents also the change of coordinates of a vector from RF 1 to RF Robotics 1 4

5 Orientation of frames in a plane (elementary rotation around z-axis) v y C O θ P θ u x B OP RF C RF x y z = x = OB xb = u cos θ - v sin θ y = OC + Cy = u sin θ + v cos θ z = w or x C y C z C cos θ sin θ sin θ cos θ 1 u v w = R z (θ) C OP u v w similarly: R x (θ) = 1 cos θ sin θ sin θ cos θ R z (-θ) = R zt (θ) R y (θ) = cos θ sin θ 1 sin θ cos θ Robotics 1 5

6 Ex: Rotation of a vector around z y y θ v v x = v cos α y = v sin α x = v cos (α + θ) = v (cos α cos θ - sin α sin θ) x = x cos θ - y sin θ O α x x y = v sin (α + θ) = v (sin α cos θ + cos α sin θ) x = x sin θ + y cos θ or z = z x y z = cos θ sin θ sin θ cos θ 1 x y z = R z (θ) x y z as before! Robotics 1 6

7 Equivalent interpretations of a rotation matrix the same rotation matrix, e.g., R z (θ), may represent: v RF C P RF C θ v θ θ RF RF the orientation of a rigid body with respect to a reference frame RF ex: [ x c y c z c ] = R z (θ) the change of coordinates from RF C to RF ex: P = R z (θ) C P the vector rotation operator ex: v = R z (θ) v the rotation matrix R C is an operator superposing frame RF to frame RF C Robotics 1 7

8 Composition of rotations 1 R 2 brings RF on RF 1 2 brings RF 1 on RF 2 R 3 brings RF 2 on RF 3 R 1 p 23 = 3 p p 1 = p 12 = RF 1 RF a comment on computational complexity RF 2 RF 3 p = ( R 1 1 R 2 2 R 3 ) 3 p = R 3 3 p 63 products 42 summations p = R 1 ( 1 R 2 ( 2 R 33 p)) 27 products 18 summations 2 p Robotics 1 1 p 8

9 Axis/angle representation z 1 RF 1 r z z P v θ v r y 1 DATA unit vector r ( r = 1) θ (positive if counterclockwise, as seen from an observer oriented like r with the head placed on the arrow) DIRECT PROBLEM x r x x 1 RF r y y RF 1 is the result of rotating RF by an angle θ around the unit vector r find R(θ,r) = [ x 1 y 1 z 1 ] such that P= R(θ,r) 1 P v = R(θ,r) v Robotics 1 9

10 Axis/angle: Direct problem z 1 3 z 1 C C -1 = C T 2 R z (θ) r y 1 R(θ,r) = C R z (θ) C T sequence of three rotations C = n s r RF 1 RF y after the first rotation the z-axis coincides with r x x 1 sequence of 3 rotations that bring frame RF to superpose with frame RF 1 n and s are orthogonal unit vectors such that n s = r, or n y s z - s y n z = r x n z s x - s z n x = r y n x s y - s x n y = r z Robotics 1 1

11 Axis/angle: Direct problem solution R(θ,r) = C R z (θ) C T R(θ,r) = n s r cθ - sθ sθ cθ 1 n T s T r T hint: use - outer product of two vectors - dyadic form of a matrix - matrix product as product of dyads = r r T + (n n T + s s T ) cθ + (s n T - n s T ) sθ taking into account that C C T = n n T + s s T + r r T = I, and that depends only on r and θ!! -r z r y s n T - n s T = -r x = S(r) R(θ,r) = r r T + (I - r r T ) cθ + S(r) sθ skew-symmetric(r): r v = S(r)v = - S(v)r = R T (-θ,r) = R(-θ,-r) Robotics 1 11

12 developing computations R(θ,r) = Final expression of R(θ,r) r x2 (1- cosθ)+cosθ r x r y (1- cosθ)-r z sinθ r x r z (1- cos θ)+r y sinθ r x r y (1- cosθ)+r z sinθ r y2 (1- cosθ)+cosθ r y r z (1- cos θ)-r x sinθ r x r z (1- cosθ)-r y sinθ r y r z (1- cos θ)+r x sinθ r z2 (1- cosθ)+cosθ Robotics 1 12

13 Axis/angle: a simple example R(θ,r) = r r T + (I - r r T ) cθ + S(r) sθ r = = z 1 R(θ,r) = = cθ + 1 sθ 1 cθ -sθ sθ cθ = R (θ) z 1 Robotics 1 13

14 Axis/angle: proof of Rodriguez formula v = R(θ,r) v v = v cos θ + (r v) sin θ + (1 - cos θ)(r T v) r proof: R(θ,r) v = (r r T + (I - r r T ) cos θ + S(r) sin θ)v = r r T v (1 - cos θ) + v cos θ + (r v) sin θ q.e.d. Robotics 1 14

15 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) 5. tr(r) = tr(r r T ) + tr(i - r r T )cθ = cθ = Σ λ i 1. λ 1 = 1 Properties of R(θ,r) 4. & 5. λ 2 + λ 3 = 2 cθ λ 2-2 cθ λ + 1 = λ 2,3 = cθ ± c 2 θ 1 = cθ ± i sθ = e±i θ identities in brown hold for any matrix! all eigenvalues λ have unitary module ( R orthonormal) Robotics 1 15

16 Axis/angle: Inverse problem GIVEN a rotation matrix R, FIND a unit vector r and an angle θ such that R = r r T + (I - r r T ) cos θ + S(r) sin θ = R(θ,r) Note first that tr(r) = R 11 + R 22 + R 33 = cos θ ; so, one could solve θ = arcos R 11 + R 22 + R but: provides only values in [,π] (thus, never negative angles θ ) loss of numerical accuracy for θ Robotics 1 16

17 Axis/angle: Inverse problem solution from R 12 -R 21 R 13 -R 31 -r z r y (**) R - R T = R 23 -R 32 = 2 sin θ -r x it follows 1 r = 1 sin θ = ± (R 12 - R 21 ) 2 + (R 13 - R 31 ) 2 + (R 23 - R 32 ) 2 2 θ = ATAN2 {± (R 12 - R 21 ) 2 + (R 13 - R 31 ) 2 + (R 23 - R 32 ) 2, R 11 + R 22 + R 33-1} see next slide r x r = r y = r z 1 2 sin θ R 32 - R 23 R 13 - R 31 R 21 - R 12 can be used only if sin θ (test made in advance on the expression (*) of sin θ in terms of the R ij s) Robotics 1 17 (*)

18 ATAN2 function arctangent with output values in the four quadrants two input arguments takes values in [-π,+π ] undefined only for (,) 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

19 Singular cases (use when sin θ = ) if θ = from (**), there is no given solution for r (rotation axis is undefined) if θ = ±π from (**), then set sin θ =, cos θ = -1 r x r y r = = r z R = 2r r T - I ± (R )/2 ± (R )/2 ± (R )/2 with r x r y = R 12 /2 r x r z = R 13 /2 r y r z = R 23 /2 exercise: determine the two solutions (r, θ) for R = resolving multiple signs ambiguities (always two solutions, of opposite sign) Robotics

20 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, } 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 2