Chapter 2 Math Fundamentals

Transcription

1 Chapter 2 Math Fundamentals Part Transform Graphs and Pose Networks 1 Moble Robotcs - Prof Alonzo Kelly, CMU RI

2 Outlne Transforms as Relatonshps Solvng Pose Networks Overconstraned Networks Dfferental Kn of Frames n General Poston Summary 2 Moble Robotcs - Prof Alonzo Kelly, CMU RI

3 Outlne Transforms as Relatonshps Solvng Pose Networks Overconstraned Networks Dfferental Kn of Frames n General Poston Summary 3 Moble Robotcs - Prof Alonzo Kelly, CMU RI

4 Spatal Relatonshps Orthogonal Transforms represent a spatal relatonshp. The property of beng postoned 3 unts to the rght, 7 unts above, and rotated 60 degrees wth respect to somethng else. Rot π 3 -- Trans( 3, 7) T c π 3 -- s π s π 3 -- c π Moble Robotcs - Prof Alonzo Kelly, CMU RI

5 Relatonshps are abstract. We may vsualze ths best by drawng two arbtrary frames. Abstract Relatonshps y b 3 x b 7 y a x a 5 Moble Robotcs - Prof Alonzo Kelly, CMU RI

6 Instantated Relatonshps We may also nstantate the relatonshp. Indcate two obects whch have the relatonshp. Box 3 unts 7 unts Hand hand T box 6 Moble Robotcs - Prof Alonzo Kelly, CMU RI

7 Graphcal Representaton We have 3 elements 2 obects a relatonshp The relatonshp s drectonal. So, draw t wth an arrow lke so. We also know how to compute the nverse relatonshp from box to hand. Hand Box Pose of box s known wrt hand. 7 Moble Robotcs - Prof Alonzo Kelly, CMU RI

8 Compounded Relatonshps Suppose the hand belongs to a robot and we have a fwd knematc soluton. Call ths fgure a: Transform Graph, or Pose Network Box Hand Base Base Box Hand 8 Moble Robotcs - Prof Alonzo Kelly, CMU RI

9 Rgdty and Transtvty Suppose (for now), the edges represent rgd spatal relatonshps. Connectedness s transtve. Two nodes n a graph are connected f there s a path between them. Therefore: Anythng s fxed (known) wrt anythng else f there s a path between them. Box Hand Base 9 Moble Robotcs - Prof Alonzo Kelly, CMU RI

10 How to compute ther relatonshp? Derved Relatonshps Box base T box base hand T handtbox Hand Ths relatonshp s computable even though t s not n the graph. Base 10 Moble Robotcs - Prof Alonzo Kelly, CMU RI

11 Trees of Relatonshps Often pose networks are treelke n structure. For trackng can we compute? World Robot(+1) RRRRRRRRRR +1 TT cccccccccccc Corner Robot T + 1 corner Robot() Useful to magne the robot stampng the floor wth axes. 11 Moble Robotcs - Prof Alonzo Kelly, CMU RI

12 General Derved Relatonshps Is ths transform computable? B 12 Moble Robotcs - Prof Alonzo Kelly, CMU RI A

13 General Derved Relatonshps Is there a path from A to B? Hnt: Look at the colors of the letters. B A 13 Moble Robotcs - Prof Alonzo Kelly, CMU RI

14 General Derved Relatonshps Is there a path from A to B? Hnt: Look at the colors of the letters. How could we fx t? B A 14 Moble Robotcs - Prof Alonzo Kelly, CMU RI

15 General Derved Relatonshps Is there a path from A to B? Hnt: Look at the colors of the letters. How could we fx t? Lke so B A 15 Moble Robotcs - Prof Alonzo Kelly, CMU RI

16 For a gven relatonshp: The HT matrx s unquely defned. The correspondng pose s not (n 3D). Euler angles are not unque: Two solutons for a gven HT. Also Euler angles are ambguous. Several conventons n use (zyx, zxy etc.) Unqueness T b a cψcθ ( cψsθ sφ sψcφ ) ( cψsθcφ + sψsφ ) u sψcθ ( sψsθsφ + cψcφ) ( sψsθ cφ cψsφ) v s θ cθ sφ cθcφ w ρ b a uvwφθψ 16 Moble Robotcs - Prof Alonzo Kelly, CMU RI

17 In OO code, t makes sense for these to be dfferent manfestatons of the same class. Codng Poses C++ Class pose: publc HT{ Double posedata[6]; } Java Class Pose extends AffneTransform { } 17 Moble Robotcs - Prof Alonzo Kelly, CMU RI

18 Vector Space? 3D angles cannot be added lke vectors. The closest facsmle s: Whch s knda lke: In realty, pose composton s done lke so: Wrte ths stylstcally lke so: T c a ρ c a ρ c a T b a Tc b a ρ b + ρ c b a b ρ[ T( ρ b )T( ρ c )] 18 Moble Robotcs - Prof Alonzo Kelly, CMU RI

19 Outlne Transforms as Relatonshps Solvng Pose Networks Overconstraned Networks Dfferental Kn of Frames n General Poston Summary 19 Moble Robotcs - Prof Alonzo Kelly, CMU RI

20 Our fun wth subscrpts and superscrpts s equvalent to fndng a path n a network. Common letters n adacent T s means edges are adacent. Agan, f a path exsts, you are n busness. Graph To Math T d a a T b a Tc b Td c Means pose of b wrt a s known. a T s known. b b a c b d 20 Moble Robotcs - Prof Alonzo Kelly, CMU RI

21 Solvng for a Pose n a Graph 1. Wrte down what you know. Draw the frames nvolved n a roughly correct spatal relatonshp. Draw all known edges. 2. Fnd the (or a) path from the start (superscrpt) to the end (subscrpt). 3. Wrte O perator matrces n left to rght order as the path s traversed. Invert any transforms whose arrow s followed n the reverse drecton. 4. Substtute all known constrants. 21 Moble Robotcs - Prof Alonzo Kelly, CMU RI

22 Example: Robot Fork Truck Vson system sees pallet. Fnd: desred new pose of robot relatve to present pose. Fork tps must lne up wth pallet holes. T1 B1 R1 Measure Ths Compute Ths 23 Moble Robotcs - Prof Alonzo Kelly, CMU RI

23 Example: Robot Fork Truck Desgnate all relevant frames. Robot Base Tp Pallet T B R 24 Moble Robotcs - Prof Alonzo Kelly, CMU RI

24 Example: Robot Fork Truck A robot here and a robot there. T1 B1 R1 25 Moble Robotcs - Prof Alonzo Kelly, CMU RI

25 Example: Robot Fork Truck Draw known transforms: Read path from R1 to R2: from camera T1 R1 T R2 R1 B1 P T2 B2 T B1TP TT2TB2TR2 The soluton requres: B1 P T T2 I R1 T1 B1 R1 26 Moble Robotcs - Prof Alonzo Kelly, CMU RI

26 Example: Robot Fork Truck Transforms relatng frames fxed to robot are all constant: R1 T B1 B2 T R2 T2 T B2 T B R R ( T B ) 1 B ( T T ) 1 T1 B1 Substtutng: R1 T R2 R1 B1 P T2 B2 T B1TP TT2TB2TR2 I R1 Measurement R1 T R2 T B R TP B TT B ( ) 1 R ( T B ) 1 Very common Sandwch 27 Moble Robotcs - Prof Alonzo Kelly, CMU RI

27 Traectory Generaton Generatng a feasble moton to connect the two s not trval. See later n course. T1 B1 R1 28 Moble Robotcs - Prof Alonzo Kelly, CMU RI

28 Outlne Transforms as Relatonshps Solvng Pose Networks Overconstraned Networks Dfferental Kn of Frames n General Poston Summary 29 Moble Robotcs - Prof Alonzo Kelly, CMU RI

29 Any edges beyond the number requred to connect everythng create cycles n the network. Cycles create the possblty of nconsstent nformaton. Cycles are not always bad. They contan rare, powerful nformaton Cyclc Networks R1 You are 7 unts away. Ball s 5 unts away. Ball s 3 unts away. Hmmm 7 5 3! B CYCLE! R2 30 Moble Robotcs - Prof Alonzo Kelly, CMU RI

30 Spannng Tree A tree s fully connected but acyclc Just the rght number of edges. Can form a spannng tree from a cyclc network. Pck a root. Traverse any way you lke. Enter nodes only once. Stop when all nodes connected. Moton plannng algorthms are based heavly on ths noton Loop 1 : [ ] Loop 2 : [ ] Moble Robotcs - Prof Alonzo Kelly, CMU RI

31 Paths n Spannng Trees Each node has exactly one parent. Exactly one acyclc path between any two nodes. Through most recent common ancestor Each omtted edge closes an ndependent cycle n the cycle bass of the graph Loop 1 : [ ] Loop 2 : [ ] Moble Robotcs - Prof Alonzo Kelly, CMU RI

32 Cycle Bass N nodes N-1 edges n Spannng Tree If there were E edges n orgnal network: Then there were: LE-(N-1) ndependent loops n orgnal network.. Why? Loop 1 : [ ] Loop 2 : [ ] Moble Robotcs - Prof Alonzo Kelly, CMU RI

33 Outlne Transforms as Relatonshps Solvng Pose Networks Overconstraned Networks Dfferental Kn of Frames n General Poston Summary 34 Moble Robotcs - Prof Alonzo Kelly, CMU RI

34 Pose Composton y k x k θ k y a k b k x θ y x a b ρ k ρ *ρk a k b k θ k What are, and? 35 Moble Robotcs - Prof Alonzo Kelly, CMU RI

35 36 Moble Robotcs - Prof Alonzo Kelly, CMU RI

36 Pose Composton The pose of frame k wth respect to frame can be extracted from the compound transform: T k cθ sθ sθ a cθ b cθ k sθ k a k sθ k cθ k b k cθ k sθ k sθ k cθ ak cθ k sθ ak sθ bk + a cθ bk + + b Read result (more or less) drectly: a k b k θ k cθ ak sθ ak θ + θ k sθ bk + a + cθ bk + b Eqn A 37 Moble Robotcs - Prof Alonzo Kelly, CMU RI

37 Compound-Left Pose Jacoban See fgure y y fxed y k x k x x nput output Jk geekey 38 Moble Robotcs - Prof Alonzo Kelly, CMU RI

38 a k b k θ k Compound-Left Pose Jacoban Eqn A cθ ak sθ ak θ + θ k y sθ bk + a + cθ bk + b x a y x a k b θ y k x k b k θ k Dfferentated Eqn A k J ρ 10 ( sθ a k + cθ bk ) k 01 ( cθ ak sθ bk ) ρ ( b k b ) 01 ( a k a ) Moble Robotcs - Prof Alonzo Kelly, CMU RI

39 Compound-Rght Pose Jacoban See fgure fxed nput output kk kudgekey 40 Moble Robotcs - Prof Alonzo Kelly, CMU RI

40 a k b k Compound-Rght Pose Jacoban θ k Eqn A cθ ak sθ ak θ + θ k sθ bk + a + cθ bk + b y x a k θ y k x k b k θ k y a b k J k x ρ k ρ k cθ sθ 0 Dfferentated Eqn A sθ cθ Moble Robotcs - Prof Alonzo Kelly, CMU RI

41 See fgure Rght-Left Pose Jacoban fxed nput output Jk geekudge 42 Moble Robotcs - Prof Alonzo Kelly, CMU RI

42 Rght-Left Pose Jacoban a k b k cθ sθ sθ cθ a k b k a b cθ sθ sθ cθ ak bk Eqn B θ k θ k θ k J ρ k ρ cθ sθ sθ cθ b k a k Dfferentated Eqn B (not easy. see text) 43 Moble Robotcs - Prof Alonzo Kelly, CMU RI

43 Compound Inner Pose Jacoban fxed fxed nput output Use frame k as ntermedate Jkl kl * kk Jucklee keylee*kudgekey l J k ρ 10 ( b l b k ) l 01 ( a l a k ) ρ k 00 1 cθ sθ sθ 0 cθ C-Left * C-Rght! l J k ρ b l k ρ sθ cθ al k cθ sθ k l cθ sθ sθ b l cθ ( ) a l b k ( ) a k 44 Moble Robotcs - Prof Alonzo Kelly, CMU RI

44 Intuton: Inverse Pose Jacoban Need t to solve problems lke ths output a y x b θ b y a x nput ρ d a ρ ρ d a ρ ρ ρ For two frames a & d 45 Moble Robotcs - Prof Alonzo Kelly, CMU RI

45 Inverse of a Pose ( T ) 1 a b θ T cθ sθ sθ cθ cθ a sθ b sθ a cθ a sθ b sθ a θ cθ b cθ b Eqn C ρ ρ cθ sθ sθ a cθ b sθ cθ cθ a + sθ b cθ sθ sθ cθ b a Dfferentated Eqn C 46 Moble Robotcs - Prof Alonzo Kelly, CMU RI

46 Outlne Transforms as Relatonshps Solvng Pose Networks Overconstraned Networks Dfferental Kn of Frames n General Poston Summary 47 Moble Robotcs - Prof Alonzo Kelly, CMU RI

47 Summary HTs encode a spatal relatonshp (between two frames). A pose and a HT are two expressons of exactly the same underlyng thng. It s trval to wrte the compound HT relatng any two frames n a pose network of arbtrary complexty. But t only exsts f they are connected. Overconstraned networks can contan nconsstences and that s valuable nformaton. Spannng Trees and Cycle Bases are fundamental concepts n such networks. Dervatves of pose compostons can be computed n closed form. 48 Moble Robotcs - Prof Alonzo Kelly, CMU RI