Cartesian Coordinates, Points, and TransformationsCIS - 600.445Russell TaylorAcknowledgment: I would like to thank Ms. Sarah Graham for providing some of the material in this presentationxxxCT imagePlanned holePinsFemurTool pathCTFCOMMON NOTATION: Use the notation Fobjto represent a coordinate system or the position and orientation of an object (relative to some unspecified coordinate system). Use Fx,yto mean position and orientation of y relative to x.xxxCT imagePlanned holePinsFemurTool pathCT imagePin 1Pin 2Pin 3PlannedholeTool pathFemurAssume equalxxxxxxxxxBase of robotCT imagePin 1Pin 2Pin 3PlannedholeTool holderTool tipTool pathFemurAssume equalCan calibrate (assume known for now)Can controlWant these to be equalBase of robotTool holderTool tipWristFWTFTip Wrist WT=FFFiTargetTargetFWristTip TargetQuestion: What value of will make ?=FFFI1Wrist Target WT1Target WTAnswer:−−==FFIFFFiiiCT imagePin 1Pin 2Pin 3PlannedholeTool pathFemurAssume equalHPFHoleFCP Hole HP=FFFi1b"2b"3b"CT imagePin 1Pin 2Pin 3Tool pathCP Hole HP=FFFiBase of robotTool holderTool tipWristFWTFCTFWristQuestion: What value of will make these equal?F1b"2b"3b"CT imagePin 1Pin 2Pin 3Tool pathCP Hole HP=FFFiBase of robotTool holderTool tipWristFWTFCTF1Wrist CT CP WT1CT CP WT−−==iiiiiFFFIFFFFBut: We must find FCT… Let’s review some math1b"2b"3b"x0y0z0x1y1z1],[ pRF =FCoordinate Frame TransformationbF = [R,p]bF = [R,p]bF = [ I,0]bF = [R,0]R•v = R b""bF = [R,p]R•v = R b""p"R+=•+v = v pRbp"""""Coordinate Frames"""""v = F b[R,p] bRbp•=•=•+bF = [R,p]Forward and Inverse Frame Transformations],[ pRF =pbRbpRbFv+•=•=•=],[pRvRpvRbbvF1111•−•=−•==−−−−)(],[ pRRF111•−=−−−ForwardInverseComposition1112 2212 11Assume [ , ], [ , ]Then()()[,]( )[, ]So==••=• •=• •+=••+=••+•+=• •+••= •=• +22211 2 212 1211212112 11 2212121FRp FRpFFbF FbFRbpRp R bpRRbRppRRRpp bFF [R,p][R,p][R R ,R p p ]"""""""""""""""""""Vectors[]zyxrowzyxcolvvvvvvvv==vxyz222 :lengthzyxvvvv ++=wv⋅=a :productdot wvu ×=:product crossθcoswv=()zzyyxxwvwvwv ++=yzzyzxxzxyyx−=−−vw vwvw vwvw vwθsin, wvu =wv•wu = vxwVectors as Displacementsvzwv+wxy+++=+zzyyxxwvwvwvwv−−−=−zzyyxxwvwvwvwvvwv-wxywRotations: Some Notation( , ) Rotation by angle about axis ( ) Rotation by angle about axis () (, )(, ,) (,) (, ) (,)( , , ) (, ) (, ) (, )RotRotαααααβγ α β γαβγ α β γ••••axyzzyzaaRaRa a aRRxRyRzRRzRyRz"""#"#"""#"""#"""#Rotations: A few useful facts1( , ) and ( , )ˆˆ(, ) (, ) where ˆˆ ˆ(, ) (, ) (, )ˆˆ(, ) (, )( ,0) i.e., ( ,0) the identity rotationˆ(,RotRot s RotRot RotRot Rot RotRot RotRot RotRotαααααβ αβααα−•= • ===•=+=−•= = =aaa abbaaa aaaa aaaabb a Ia""""" """"""""() ()()ˆˆ ˆ ˆˆ)(,)ˆˆ ˆˆˆ(,)(,) (,)((,),)RotRot Rot Rot Rot Rotααβ β βα•= • + • − ••=• −•baba a babaab b ba"" ""Rotations: more factsIf [ , , ] then a rotation may be described in ˆterms of the effects of on orthogonal unit vectors, [1,0,0] , ˆˆ[0,1,0] , [0,0,1] whereˆˆTxyzTTTxx yy zzxyzvvvvvv=•===•= + +=•=•=vRvRxyzRv r r rrRxrRyrR"""""""""()()ˆNote that rotation doesn't affect inner products•••=zRb Rc bc""""iiRotations in the plane[, ]Txy=v"cos sinsin coscos sinsin cosxx yyx yxyθθθθθθθθ−  •=  +  −=•R•Rv"θRotations in the plane[][]cos sin 1 0ˆˆsin cos 0 1ˆˆθθθθ−•= •=• •RxyRxRyθ3D Rotation Matrices[][]ˆˆˆ ˆ ˆ ˆˆˆˆyz•=•••=xRxyzRxRyRzrrrˆˆˆˆˆˆˆˆ ˆˆ ˆˆ100ˆˆˆˆ ˆˆ010ˆˆ ˆˆ ˆˆ001TTTyyzzTTTyzTTTyyyyzTTTzzyzz•= •==xxxx x xxxrRRr rrrrrr rr rrrrrr rrrr rr rriiiiiiiiiInverse of a Rotation Matrix equals its transpose:R-1 = RTRT R=R RT = IThe Determinant of a Rotation matrix is equal to +1:det(R)= +1Any Rotation can be described by consecutive rotations about the three primary axes, x, y, and z:R = Rz,θθθθRy,φφφφRx,ψψψψProperties of Rotation MatricesCanonical 3D Rotation MatricesNote: Right-Handed Coordinate System10 0() (,) 0 cos ) sin )0sin) cos)cos ) 0 sin )() (,) 0 1 0sin ) 0 cos )cos ) sin ) 0() (,) sin ) cos ) 0001Rot (θ (θ(θ (θ(θ (θRot(θ (θ(θ (θRot (θ (θθθθθθθ== −==−−==xyzRxRyRz""""""Homogeneous Coordinates• Widely used in graphics, geometric calculations• Represent 3D vector as 4D quantity///xsyszss≅v"1xyz=• For our current purposes, we will keep the “scale” s = 1Representing Frame Transformations as Matrices100010001000 1 1+→ =•xxyyzzpvpvvpPvpv11•→R0vRv00[,]  •→ • = = =    Ip R RpPR Rp F01 01 01()11 1+ →=  iiRpv Rv pFv0xxxxxxCT imagePin 1Pin 2Pin 31b"2b"3b"Base of robotTool holderTool tipWrist,1FWTFCTFWrist,1 WT CT 1=1vF p Fb"""#i iCT imagePin 1Pin 2Pin 31b"2b"3b"Base of robotTool holderTool tipWrist,2FWTFCTFWrist,1 WT CT 12Wrist,2WTCT2==1vF p FbvF p Fb"""#i i"""#i iCT imagePin 1Pin 2Pin 31b"2b"3b"Base of robotTool holderTool tipWrist,3FWTFCTFWrist,1 WT CT 12Wrist,2WTCT23Wrist,3WTCT3===1vF p FbvF p FbvF p Fb"""#i i"""#i i"""#i iFrame transformation from 3 point pairsxxx1v"3v"2v"robFxxx1b"2b"3b"CTFFrame transformation from 3 point pairsxxx1v"3v"2v"robF1b"2b"3b"CTF1ror TC Cb−= FF F1 rob k CT kkrobCTkkrCk−•= •=•=•FvFbvFFbvFb""""""Frame transformation from 3 point pairs3311Define1133 krCk rCkrCmkmkkkmkkm== +===− =−∑∑vFbRbpvvbbuvvabb""""""""""""""rC k rC k rC=+Fa Ra p"""()rC k rC rC k m rC+= −+Ra p R b b p"""" "rC k rC k rC rC m rC=+−−Ra Rb p Rb p"""" "rC k k m k=−=Ra v v u""" "xxxxxxxxxxxxxx1a"2b"3b"mb"1b"2a"3a"1v"3v"2v"mv"2u"1u"3u"rC m rC m=−puRb"""Solve These!!Rotation from multiple vector pairs1, , .kkkn==Ra u R""$Given a system for the problem is to estimate This will require at least three