Cartesian Coordinates, Points, and Transformations

Transcription

1 Cartesian Coordinates, Points, and Transformations CIS Russell Taylor Acknowledgment: I would like to thank Ms. Sarah Graham for providing some of the material in this presentation

2 Femur Planned hole Pins F CT CT image Tool path COMMON NOTATION: Use the notation F obj to represent a coordinate system or the position and orientation of an object (relative to some unspecified coordinate system). Use F,y to mean position and orientation of y relative to.

3 Femur Planned hole Pins CT image Tool path

4 Tool path Pin Pin 2 Pin 3 Planned hole Femur Assume equal CT image

5

6

7 Want these to be equal Can control Tool holder Tool tip Can calibrate (assume known for now) Tool path Base of robot Pin Pin 2 Pin 3 Planned hole Femur Assume equal CT image

8 Tool holder F WT Tool tip I F = F if Tip Wrist WT Target F Wrist F Target Base of robot Question: What value of will make F = F? Answer: Tip Target Wrist = Target ii WT F F I F F Wrist = F Target if WT

9 Tool path F = F if CP Hole HP F HP Femur Pin Pin 2 b b 2 b 3 Assume equal Pin 3 CT image Planned hole F Hole

10 Question: What value of F Wrist will make these equal? Tool tip Tool holder F WT Tool path F Wrist Base of robot F CT Pin Pin 2 b b 2 b 3 Pin 3 CT image F = F if CP Hole HP

11 But: We must find F CT Let s review some math Tool holder F Wrist F WT Tool tip F = F if iiif Wrist CT CP WT = F if if CT CP WT Tool path Base of robot F CT Pin Pin 2 b b 2 b 3 Pin 3 CT image F = F if CP Hole HP

12 Coordinate Frame Transformation F = [ R, p] 0 F y z y 0 z 0

13

14 b F = [R,p]

15 b F = [R,p]

16 b F = [ I,0]

17 b v R = R b F = [R,0]

18 v = R b R p F = [R,p] b + v = v R p = R b+ p

19 Coordinate Frames v = F b = [R,p] b = R b+ p b F = [R,p]

20 Forward and Inverse Frame Transformations ], [ p R F = p b R b p R b F v + = = = ], [ p R v R p v R b b v F = = = ) ( ], [ p R R F = Forward Inverse

21 Composition Assume F = [ R, p ], F = [ R, p ] Then F F2 b= F ( F2 b) = F ( R2 b+ p2) = [ R, p] ( R2 b+ p2) = R R2 b+ R p2 + p = [ R R, R p + p ] b So 2 2 F F 2 = [R,p ] [R 2,p 2] = [R R,R p + p ] 2 2

22 v v col row v = v v = length : y z [ v v v ] y 2 z 2 y v = v + v + Vectors v 2 z u = vw z v w v w y dot product : a = v w = ( v w + v w + v w ) y y z z = v w cosθ cross product : u = v w vw vw = vw vw vw vw y z z y z z y y, u = v w sinθ

23 Vectors as Displacements v z w v+w y = + z z y y w v w v w v w v = z z y y w v w v w v w v v w v-w y w

24 Rotations: Some Notation Rot( a, α) Rotation by angle α about ais R a( α) Rotation by angle α about ais Ra ( ) Rot( a, a) a a Ryz ( α, β, γ) R(, α) R( y, β) R( z, γ) R ( α, β, γ) R(, z α) R(, y β) R(, z γ) zyz

25 Rotations: A few useful facts Rot( sa, α) a= a and Rot( a, α) b = b a Rot(, a α) = Rot(, aˆ α) where aˆ = a Rot(, aˆ α) Rot(, aˆ β) = Rot(, aˆ α + β) Rot(, aˆ α) = Rot(, aˆ α) Rot( a,0) b= b i.e., Rot( a,0) = I = the identity rotation Rot Rot(, aˆ α) b= aˆ b aˆ+ Rot( aˆ, α) b aˆ b aˆ ( ) ( ) ( ) Rot( aˆ, α) Rot( bˆ, β) = Rot( bˆ, β) Rot( Rot( bˆ, β) aˆ, α)

26 Rotations: more facts T If v = [ v, v, v ] then a rotation R v may be described in T terms of the effects of R on orthogonal unit vectors, ˆ = [,0,0], yˆ y z T T = [0,,0], zˆ = [0,0,] R v= v r + v r + v r where r ˆ = R r ˆ y = R y r = R zˆ z ( ) i( ) y y z z Note that rotation doesn't affect inner products R b R c = bic

27 Rotations in the plane R v R cosθ ysinθ y = sinθ + ycosθ cosθ sinθ = sinθ cosθ y θ v = [, y] T

28 Rotations in the plane cosθ sinθ 0 R [ ˆ yˆ] = sinθ cosθ 0 [ R ˆ R yˆ] = θ

29 3D Rotation Matrices [ ˆ ˆ ˆ] [ ˆ ˆ ˆ] R y z = R R y R z = rˆ ˆ ˆ ry rz T rˆ T T R R = rˆ ˆ ˆ ˆ y r ry rz ˆ rz T T T rˆ ˆ ˆ ˆ ˆ ˆ ir r iry r irz 0 0 T T T = ˆ ˆ ˆ ˆ ˆ ˆ y y y y z = 0 0 r ir r ir r ir T T T ˆ ˆ ˆ ˆ ˆ ˆ rz ir rz iry rz irz 0 0

30 Properties of Rotation Matrices Inverse of a Rotation Matri equals its transpose: R - = R T R T R=R R T = I The Determinant of a Rotation matri is equal to +: det(r)= + Any Rotation can be described by consecutive rotations about the three primary aes,, y, and z: R = R z,θ R y,φ R,ψ

31 Canonical 3D Rotation Matrices Note: Right-Handed Coordinate System R 0 0 ( θ) = Rot (, θ) = 0 cos (θ) sin (θ) 0 sin (θ) cos (θ) R y cos (θ) 0 sin (θ) ( θ) = Rot( y, θ) = 0 0 sin (θ) 0 cos (θ) R z cos (θ) sin (θ) 0 ( θ) = Rot ( z, θ) = sin (θ) cos (θ) 0 0 0

32 Homogeneous Coordinates Widely used in graphics, geometric calculations Represent 3D vector as 4D quantity For our current purposes, we will keep the scale s = v / s y/ s y = z / s z s

33 Representing Frame Transformations as Matrices 0 0 p v 0 0 p y v y v+ p = P v 0 0 pzvz R 0 v R v 0 I p R 0 R p P R = = [ R, p] = F Fv i R p v ( Riv) + p = 0

34

35 Tool tip v F p = F b i i Wrist, WT CT Tool holder F WT F Wrist, Base of robot F CT Pin Pin 2 b b 2 b 3 Pin 3 CT image

36 Tool tip v F i p = F i b v F i p = F i b Wrist, WT CT 2 Wrist,2 WT CT 2 Tool holder F WT F Wrist,2 Base of robot F CT Pin Pin 2 b b 2 b 3 Pin 3 CT image

37 Tool holder F WT Tool tip v F i p = F i b v F i p = F i b v F i p = F i b Wrist, WT CT 2 Wrist,2 WT CT 2 3 Wrist,3 WT CT 3 F Wrist,3 Base of robot F CT Pin Pin 2 b b 2 b 3 Pin 3 CT image

38 Frame transformation from 3 point pairs b 3 F CT b b 2 v 3 v v 2 F rob

39 Frame transformation from 3 point pairs F CT b b 2 F = F F rc rob CT b 3 v 3 Frob vk = FCT bk v F F b v = F b k = rob CT k k rc k v F rob v 2

40 Frame transformation from 3 point pairs v = F b = R b + p k rc k rc k rc Define 3 3 v = v b = b u = v v a = b b m k m k 3 3 k k m k k m FrCa k = RrCa k + p rc ( ) RrCak + prc = RrC bk bm + prc R a = R b + p R b p RrCak = vk vm = uk p = u R b rc k rc k rc rc m rc Solve These!! rc m rc m a b m b a 2 v 3 a b 3 3 v m u 3 u 2 b 2 u v 2 v

41 Rotation from multiple vector pairs Given a system Rak = uk for k =,, n the problem is to estimate R. This will require at least three such point pairs. Later in the course we will cover some good ways to solve this system. Here is a not-so-good way that will produce roughly correct answers: Step : Form matrices U= u u and A a a [ ] = [ ] Step 2: Solve the system RA = U for R. E.g., by R = UA T Step 3: Renormalize R to guarantee R R = I. n n

42 Renormalizing Rotation Matri T Given "rotation" matri R = r ry rz, modify it so R R = I. Step : Step 2: Step 3: a= ry r b= r a R z normalized z z = a b rz a b r

43 Calibrating a pointer But what is b tip?? F ptr b tip vtip = Fptr btip F lab

44 Calibrating a pointer b = F b = Rb + p post k tip k tip k F ptr F k b tip b post F lab

45 Calibrating a pointer For each measurement k, we have b = R b + p I. e., post k tip k Rb b = p k tip post k Set up a least squares problem b tip Rk I b post p k F ptr b tip b tip b tip btip F ptr F ptr Fptr

46 Kinematic Links F k F k- L k θ k F = F F k k k, k [ Rk, pk] = [ Rk, pk ] Rk, k, pk, k = [ R, p ] [ Rot( r, θ ), L Rot( r, θ ) ] k k k k k k k

47 Kinematic Links End of link k- Fk, k End of link k F k F k Base of robot F = F F k k k, k [ Rk, pk] = [ Rk, pk ] Rk, k, pk, k = [ R, p ] [ Rot( r, θ ), L Rot( r, θ ) ] k k k k k k k

48 Kinematic Chains F0 = [, I 0] R3 = R0,R,2R2,3 = Rot( r, θ) Rot( r2, θ2) Rot( r3, θ3) p ( ) 3 = p0,+ R0, p,2 + R,2p2,3 = LRot ( r, θ) + L2Rot( r, θ) Rot( r2, θ2) + L Rot( r, θ ) Rot( r, θ ) Rot( r, θ ) F 2 L 2 θ 2 θ 3 L 3 F 3 F F 0 L θ

49 Kinematic Chains If r = r2 = r3 = z, R3 = Rot(, z θ) Rot(, z θ2) Rot(, z θ3) = Rot(, z θ+ θ2 + θ3) p ( ) 3 = p0,+ R0, p,2 + R,2p2,3 = LRot (, z θ) + L2Rot(, z θ) Rot(, z θ2) + LRot 3 (, z θ) Rot(, z θ2) Rot(, z θ3) = LRot (, z θ) + LRot 2 ( z, θ+ θ2) + LRot(, z θ + θ + θ ) F 0 L θ L 2 θ 2 θ 3 F 3 L 3

50 Kinematic Chains If r = r = r = z, R p 2 3 cos( θ+ θ2 + θ3) sin( θ+ θ2 + θ3) 0 = sin( θ + θ + θ ) cos( θ + θ + θ ) Lcos( θ) + L2cos( θ+ θ2) + L3cos( θ+ θ2 + θ3) = L sin( θ ) + L sin( θ + θ ) + L sin( θ + θ + θ )

51 Small Transformations A great deal of CIS is concerned with computing and using geometric information based on imprecise knowledge Similarly, one is often concerned with the effects of relatively small rotations and displacements Essentially, we will be using fairly straightforward linearizations to model these situations, but a specialized notation is often useful

52 Small Frame Transformations Represent a "small" pose shift consisting of a small rotation followed by a small displacement p as F= [ R, p] Then F v= R v+ p R

53 Small Rotations R a small rotation R a( α) a rotation by a small angle α about ais a Rot( a, a ) b a b+ b for a sufficiently small Ra ( ) a rotation that is small enough so that any error introduced by this approimation is negligible R( λ a) R( µ b) R( λ a+ µ b) (Linearity for small rotations) Eercise: Work out the linearity proposition by substitution

54 Approimations to Small Frames Fa (, p) [ Ra (), p] Fa (, p) v= Ra () v+ p v+ a v+ p a v= skew( a) v 0 az ay v az 0 a v y ay a 0 vz Ra ( ) I+ skew( a) skew( a) a= 0

55 actual nominal Errors & sensitivity Often, we do not have an accurate value for a transformation, so we need to model the error. We model this as a composition of a "nominal" frame and a small displacement F = F F * Often, we will use the notation F for F and will just use F for F nominal * F = F F actual. Thus we may write something like * * or (less often) F = F F. We also use v = v+ v, etc. Thus, if we use the former form (error on the right), and have nominal relationship v = F b, we get * * * v = F b = F F ( b+ b)

56 b v F = [R,p]

57 F * = F F v v v * = + b b b * = +

58 Errors & Sensitivity Suppose that we know nominal values for F, b, and and that T T [-ε,-ε,-ε] v [ ε, ε, ε] What does this tell us about F= [ R, p]? F * = F F v v v * = + b b b * = + v ; Copyright 999, 2000 rht+sg

59 Errors & Sensitivity so * * * v = F b = F F ( b+ b) ( = () ( + ) + ) R Ra b b p + p ( ) R b b a b a b p + p = + + ( R b p R b+ a b+ a b+ p) v+ R ( b+ a b+ p) if a b a b is negli gible (it usually is) v v v R b+ a b+ p = R b+ R a b+ R p ( ) * =

60 Digression: rotation triple product Epressions like R a b are linear in a, but are not always convenient to work with. Often we would prefer something like MRb (, ) a. R a b= R b a = R skew( b) a = skew( ) T R b a

61 Digression: rotation triple product Here are a few more useful facts: ( ) R a b = ( R a) ( R b) a R b = R R a b ( ) ( ) ( ) Consequently skew a R = R R a R skew( a) R = skew( R a) ( ) skew( )

62 Some more stuff Consider R RR 2 R2 = RR 2 R3 R( I + sk( α)) R2( I + sk( α2)) RR 2( I + sk( α3)) R2 ( I + sk( α)) R2( I + sk( α2)) I + sk( α3) I + R2 sk( α) R2 + sk( α2) + R2 sk( α) R2sk( α2) I + sk( α3) sk( α3) R2 sk( α) R2 + sk( α2) since we pretend sk( α) R2sk( α2) = 0 sk( α ) sk( R α ) + sk( α ) = sk( R α + α ) so α R α + α 3 2 2

63 Errors & Sensitivity Previous epression was v R b + a b+ p ( ) Substituting triple product and rearranging gives b v skew( ) R R R b p a So ε ε ε skew( ) b R R R b p ε ε a ε

64 Errors & Sensitivity Now, suppose we know that b β, this will give us a system of linear constraints ε ε ε ε b ε R R R skew( b) ε β p I 0 0 β β a β β β

65 Error Propagation in Chains F k F k- L k θ k F = F F * * * k k k, k F F = F F F F k k k k k, k k, k ( ) F = F F F F F k k k k k, k k, k ( ) = F F F F k, k k k, k k, k

66 Eercise Suppose that you have Rk, k = R( ak) I+ skew( ak) p = e k, k k θ 3 F 3 Work out approimate formulas for [ R 3, p in terms of L, r θ a and e. You should k k k k, k, k k come up with a formula that is linear in L, a, and e. F k F 2 3 ] L 2 θ 2 L 3 F 0 L θ

67 Eercise Suppose we want to know error in F = F F 0,3 0 3 F = F F if if if 0, ,,2 2,3 F = F if if * * * * 0,3 0,,2 2,3 F i F = F if if * * * 0,3 0,3 0,,2 2,3 F = F if i F if i F if i F 3 0,3 0, 0,,2,2 2,3 2,3 = F if if if i F if i F if i F 2,3,2 0, 0, 0,,2,2 2,3 2,3 = F2,3 if,2 i F0, if,2i F,2 if2,3 i F2,3 F F 2 L 2 θ 2 θ 3 L 3 F 3 F 0 L θ

68 Parametric Sensitivity Suppose you have an eplicit formula like Lcos( θ) + L2cos( θ+ θ2) + L3cos( θ+ θ2 + θ3) p3 = Lsin( θ) + L2sin( θ+ θ2) + L3sin( θ+ θ2 + θ3) 0 and know that the only variation is in parameters like Lk and θ. Then you can estimate the variation in p as a function k of variation in and θ by remembering your calculus. L k k 3 p p p L θ L θ 3 3 3

69 Grinding this out gives: Parametric Sensitivity p3 p3 L p3 L θ θ where T L = [ L, L2, L3] T θ = [ θ, θ, θ ] 2 3 cos( θ ) cos( θ + θ ) cos( θ + θ + θ ) p3 = sin( θ) sin( θ+ θ2) sin( θ+ θ2 + θ3) L Lsin( θ) L2sin( θ+ θ2) L3sin( θ+ θ2 + θ3) L2sin( θ+ θ2) L3sin( θ+ θ2 + θ3) L3sin( θ+ θ2 + θ3) p3 = Lcos( θ) L2cos( θ θ2) L3cos( θ θ2 θ3) L2cos( θ θ2) L3cos( θ θ2 θ3) L3cos( θ θ2 θ3) θ