Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs...
Transcript of Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs...
![Page 1: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/1.jpg)
Chapter 2 Math Fundamentals
Part 4 2.7 Transform Graphs and Pose Networks
Mobile Robotics - Prof Alonzo Kelly, CMU RI 1
![Page 2: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/2.jpg)
Outline • 2.7.1 Transforms as Relationships • 2.7.2 Solving Pose Networks • 2.7.3 Overconstrained Networks • 2.7.4 Differential Kin of Frames in General Position • Summary
Mobile Robotics - Prof Alonzo Kelly, CMU RI 2
![Page 3: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/3.jpg)
Outline • 2.7.1 Transforms as Relationships • 2.7.2 Solving Pose Networks • 2.7.3 Overconstrained Networks • 2.7.4 Differential Kin of Frames in General Position • Summary
Mobile Robotics - Prof Alonzo Kelly, CMU RI 3
![Page 4: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/4.jpg)
Spatial Relationships • Orthogonal
Transforms represent a spatial relationship.
• The property of “being positioned 3 units to the right, 7 units above, and rotated 60 degrees with respect to something else.”
T
cπ3--- sπ
3---– 3
sπ3--- cπ
3--- 7
0 0 1
=
Rot π3--- Trans 3 7,( )
Mobile Robotics - Prof Alonzo Kelly, CMU RI 4
![Page 5: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/5.jpg)
Abstract Relationships • Relationships are
abstract. • We may visualize this
best by drawing two arbitrary frames.
xa
ya
xb yb
7
3
Mobile Robotics - Prof Alonzo Kelly, CMU RI 5
![Page 6: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/6.jpg)
Instantiated Relationships • We may also “instantiate”
the relationship. – Indicate two objects which
have the relationship.
Tboxhand
Box
Hand
3 units 7 units
Mobile Robotics - Prof Alonzo Kelly, CMU RI 6
![Page 7: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/7.jpg)
Graphical Representation • We have 3 elements
– 2 objects – a relationship
• The relationship is directional.
• So, draw it with an arrow like so….
• We also know how to compute the inverse relationship from box to hand.
“Pose of box is known wrt hand”.
Box Hand
Mobile Robotics - Prof Alonzo Kelly, CMU RI 7
![Page 8: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/8.jpg)
Compounded Relationships • Suppose the hand
belongs to a robot and we have a fwd kinematic solution.
• Call this figure a: – Transform Graph, or – Pose Network
Hand
Base
Box
Box
Hand
Base
Mobile Robotics - Prof Alonzo Kelly, CMU RI 8
![Page 9: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/9.jpg)
Rigidity and Transitivity • Suppose (for now), the
edges represent rigid spatial relationships.
• Connectedness is transitive. – Two nodes in a graph are
connected if there is a path between them.
• Therefore: – Anything is fixed (known) wrt
anything else if there is a path between them.
Box
Hand
Base
Mobile Robotics - Prof Alonzo Kelly, CMU RI 9
![Page 10: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/10.jpg)
Derived Relationships • How to compute their
relationship?
• This relationship is computable even though it is not in the graph.
Tboxbase Thand
base Tboxhand=
Box
Hand
Base
Mobile Robotics - Prof Alonzo Kelly, CMU RI 10
![Page 11: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/11.jpg)
2.7.1.2 Trees of Relationships • Often pose networks
are treelike in structure.
• For tracking … can we compute?
• Useful to imagine the robot “stamping” the floor with axes.
TcornerRoboti 1+
𝑇𝑇𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑐𝑅𝑅𝑐𝑐𝑅𝑅𝑐𝑐𝑅𝑅 𝑖𝑖+1
Robot(i)
World
Robot(i+1) Corner
Mobile Robotics - Prof Alonzo Kelly, CMU RI 11
![Page 12: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/12.jpg)
General Derived Relationships • Is this transform computable?
A
B
Mobile Robotics - Prof Alonzo Kelly, CMU RI 12
![Page 13: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/13.jpg)
General Derived Relationships • Is there a path
from A to B? – Hint: Look at the
colors of the letters.
A
B
Mobile Robotics - Prof Alonzo Kelly, CMU RI 13
![Page 14: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/14.jpg)
General Derived Relationships • Is there a path from
A to B? – Hint: Look at the
colors of the letters.
• How could we fix it?
A
B
Mobile Robotics - Prof Alonzo Kelly, CMU RI 14
![Page 15: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/15.jpg)
General Derived Relationships • Is there a path from
A to B? – Hint: Look at the
colors of the letters.
• How could we fix it? – Like so…
A
B
Mobile Robotics - Prof Alonzo Kelly, CMU RI 15
![Page 16: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/16.jpg)
2.7.1.3 Uniqueness • For a given
relationship: – The HT matrix is
uniquely defined. – The corresponding
pose is not (in 3D). • Euler angles are not
unique: – Two solutions for a
given HT. • Also Euler angles are
ambiguous. – Several conventions
in use (zyx, zxy etc.)
ρba
u v w φ θ ψ=
Tba
cψcθ cψsθ sφ sψcφ–( ) cψsθcφ sψsφ+( ) usψcθ sψsθsφ cψcφ+( ) sψsθ cφ cψsφ–( ) v
s θ– cθ sφ cθcφ w0 0 0 1
=
Mobile Robotics - Prof Alonzo Kelly, CMU RI 16
![Page 17: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/17.jpg)
Coding Poses • In OO code, it
makes sense for these to be different manifestations of the same class.
Class pose: public HT{ … Double poseData[6]; … }
Class Pose extends AffineTransform { }
Java
C++
Mobile Robotics - Prof Alonzo Kelly, CMU RI 17
![Page 18: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/18.jpg)
Vector Space? • 3D angles cannot be
added like vectors. • The closest
facsimile is: • Which is kinda like: • In reality, pose
composition is done like so:
• Write this stylistically like so:
Tca Tb
aTcb=
ρca ρ T ρb
a( )T ρcb( )[ ]=
ρca ρb
a ρcb+= “ ”
Mobile Robotics - Prof Alonzo Kelly, CMU RI 18
![Page 19: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/19.jpg)
Outline • 2.7.1 Transforms as Relationships • 2.7.2 Solving Pose Networks • 2.7.3 Overconstrained Networks • 2.7.4 Differential Kin of Frames in General Position • Summary
Mobile Robotics - Prof Alonzo Kelly, CMU RI 19
![Page 20: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/20.jpg)
Graph To Math • Our fun with
subscripts and superscripts is equivalent to finding a path in a network.
• Common letters in adjacent T’s means edges are adjacent.
• Again, if a path exists, you are in business.
Tda Tb
aTcbTd
c=
• Means pose of b wrt a is known.
• T is known. b a
c d b a
b a
Mobile Robotics - Prof Alonzo Kelly, CMU RI 20
![Page 21: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/21.jpg)
Solving for a Pose in a Graph 1. Write down what you know. Draw the
frames involved in a roughly correct spatial relationship. Draw all known edges.
2. Find the (or a) path from the start (superscript) to the end (subscript).
3. Write “O”perator matrices in left to right order as the path is traversed. – Invert any transforms whose arrow is followed
in the reverse direction. 4. Substitute all known constraints.
21 Mobile Robotics - Prof Alonzo Kelly, CMU RI
![Page 22: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/22.jpg)
2.7.2.2 Example: Robot Fork Truck • Vision system sees pallet.
• Find:
– desired new pose of robot …
– … relative to present pose.
• Fork tips must line up with pallet holes.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 23
R1
B1
T1
Measure This
Compute This
![Page 23: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/23.jpg)
2.7.2.2 Example: Robot Fork Truck • Designate all relevant
frames. – Robot – Base – Tip – Pallet
Mobile Robotics - Prof Alonzo Kelly, CMU RI 24
R
B
T
![Page 24: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/24.jpg)
2.7.2.2 Example: Robot Fork Truck • A robot “here” … • … and a robot “there”.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 25
R1
B1
T1
![Page 25: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/25.jpg)
2.7.2.2 Example: Robot Fork Truck • Draw known transforms: • Read path from R1 to
R2:
• The solution requires:
TR2R1 TB1
R1 TPB1TT2
P TB2T2 TR2
B2=
TT2P I=
from camera
Mobile Robotics - Prof Alonzo Kelly, CMU RI 26
R1
B1
T1
T1
B1
R1
![Page 26: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/26.jpg)
2.7.2.2 Example: Robot Fork Truck • Transforms relating
frames fixed to robot are all constant:
• Substituting:
TB1R1 TB
R=
TR2B2 TB
R( )1–
=
TR2R1 TB
RTPB TT
B( )1–
TBR( )
1–=
Very common “Sandwich”
TR2R1 TB1
R1 TPB1TT2
P TB2T2 TR2
B2=
I
Measurement
TB2T2 TT
B( )1–
=
Mobile Robotics - Prof Alonzo Kelly, CMU RI 27
R1
B1
T1
![Page 27: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/27.jpg)
Trajectory Generation • Generating a feasible motion
to connect the two is not trivial.
• See later in course.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 28
R1
B1
T1
![Page 28: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/28.jpg)
Outline • 2.7.1 Transforms as Relationships • 2.7.2 Solving Pose Networks • 2.7.3 Overconstrained Networks • 2.7.4 Differential Kin of Frames in General Position • Summary
Mobile Robotics - Prof Alonzo Kelly, CMU RI 29
![Page 29: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/29.jpg)
2.7.3 Cyclic Networks • Any edges beyond the
number required to connect everything create cycles in the network.
• Cycles create the possibility of inconsistent information.
• Cycles are not always bad. – They contain rare,
powerful information.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 30
You are 7 units away. Ball is 5 units away.
Ball is 3 units away.
Hmmm…
7 − 5 ≠ 3 !
R2 B R1
CYCLE !
![Page 30: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/30.jpg)
2.7.3.1 Spanning Tree • A “tree” is fully connected but
acyclic – Just the right number of edges.
• Can form a spanning tree from a cyclic network. – Pick a root. – Traverse any way you like. – Enter nodes only once. – Stop when all nodes connected.
• Motion planning algorithms are based heavily on this notion.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 31
1
2
0
4
6
5
3
0
1 4
2 3 5
6
Loop 1 : [ 2 1 0 4 3 ] Loop 2 : [ 6 2 1 0 4 5 ]
![Page 31: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/31.jpg)
Paths in Spanning Trees • Each node has exactly
one parent. • Exactly one acyclic path
between any two nodes. – Through most recent
common ancestor
• Each omitted edge closes an independent cycle in the cycle basis of the graph.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 32
1
2
0
4
6
5
3
0
1 4
2 3 5
6
Loop 1 : [ 2 1 0 4 3 ] Loop 2 : [ 6 2 1 0 4 5 ]
![Page 32: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/32.jpg)
2.7.3.2 Cycle Basis • N nodes • N-1 edges in Spanning
Tree • If there were E edges in
original network: • Then there were:
– L=E-(N-1) … – … independent loops in
original network..
Why?
Mobile Robotics - Prof Alonzo Kelly, CMU RI 33
1
2
0
4
6
5
3
0
1 4
2 3 5
6
Loop 1 : [ 2 1 0 4 3 ] Loop 2 : [ 6 2 1 0 4 5 ]
![Page 33: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/33.jpg)
Outline • 2.7.1 Transforms as Relationships • 2.7.2 Solving Pose Networks • 2.7.3 Overconstrained Networks • 2.7.4 Differential Kin of Frames in General Position • Summary
Mobile Robotics - Prof Alonzo Kelly, CMU RI 34
![Page 34: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/34.jpg)
2.7.4.1 Pose Composition
• What are , and ?
yi
xi
xj
yj
yk xk
aj i bj
i
θj i
ak j
θk j
bk j
ak i bk
i θk i
ρki ρj
i*ρkj=
Mobile Robotics - Prof Alonzo Kelly, CMU RI 35
![Page 35: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/35.jpg)
Mobile Robotics - Prof Alonzo Kelly, CMU RI 36
![Page 36: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/36.jpg)
Pose Composition • The pose of frame k with respect to frame i can
be extracted from the compound transform:
• Read result (more or less) directly:
Tki
cθji sθj
i– aji
sθji cθj
i bji
0 0 1
cθkj sθk
j– a kj
sθkj cθk
j bkj
0 0 1
cθki sθk
i– cθji ak
j sθjibk
j– aji+
sθki cθk
i sθji ak
j cθjibk
j bji+ +
0 0 1
= =
aki cθj
iakj sθj
ibkj– aj
i+=
bki sθj
iakj cθj
ibkj bj
i+ +=
θki θj
i θkj+=
Eqn A
Mobile Robotics - Prof Alonzo Kelly, CMU RI 37
![Page 37: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/37.jpg)
2.7.4.2 Compound-Left Pose Jacobian • See figure
yi
xi
input
output
xj
yj yk
xk
fixed
Mobile Robotics - Prof Alonzo Kelly, CMU RI 38
Jiki = geekey
![Page 38: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/38.jpg)
2.7.4.2 Compound-Left Pose Jacobian
J ijik
ρji∂
∂ρ ki 1 0 s θj
ia kj cθj
i bkj+( )–
0 1 cθji ak
j sθji bk
j–( )
0 0 1
1 0 bki bj
i–( )–
0 1 a ki a j
i–( )
0 0 1
= = =
yi
xi
xj
yj
yk xk
aj i bj
i
θj i
ak j
θk j
bk j
Differentiated Eqn A
aki cθj
iakj sθj
ibkj– aj
i+=
bki sθj
iakj cθj
ibkj bj
i+ +=
θki θj
i θkj+=
Eqn A
Mobile Robotics - Prof Alonzo Kelly, CMU RI 39
![Page 39: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/39.jpg)
2.7.4.3 Compound-Right Pose Jacobian • See figure
input
output
fixed
Mobile Robotics - Prof Alonzo Kelly, CMU RI 40
kjki = kudgekey
![Page 40: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/40.jpg)
2.7.4.3 Compound-Right Pose Jacobian
yi
xi
xj
yj
yk xk
aj i bj
i
θj i
ak j
θk j
bk j
J jkik
ρkj∂
∂ρki cθj
i sθji– 0
sθji cθj
i 0
0 0 1
= = Differentiated Eqn A
aki cθj
iakj sθj
ibkj– aj
i+=
bki sθj
iakj cθj
ibkj bj
i+ +=
θki θj
i θkj+=
Eqn A
Mobile Robotics - Prof Alonzo Kelly, CMU RI 41
![Page 41: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/41.jpg)
2.7.4.4 Right-Left Pose Jacobian • See figure
output
fixed
input
Mobile Robotics - Prof Alonzo Kelly, CMU RI 42
Jikj = geekudge
![Page 42: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/42.jpg)
2.7.4.4 Right-Left Pose Jacobian ak
j
bkj
cθji sθj
i
sθji– cθj
i
aki aj
i–
bki bj
i–=
cθji sθj
i
sθji– cθj
i
ai jk
bi jk
=
Differentiated Eqn B (not easy. see text)
Eqn B
Jijjk
ρji∂
∂ρkj cθj
i sθji bk
j–
sθji– cθj
i akj
0 0 1
–= =
θkj θk
i θji–=
Mobile Robotics - Prof Alonzo Kelly, CMU RI 43
![Page 43: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/43.jpg)
2.7.4.5 Compound Inner Pose Jacobian
input
output
C-Left * C-Right ! J jkil
ρkj∂
∂ρ li 1 0 bl
i bki–( )–
0 1 ali ak
i–( )
0 0 1
cθji sθj
i– 0
sθji cθj
i 00 0 1
= =
J jkil
ρkj∂
∂ρli cθj
i sθji– bi k
l–
sθji cθj
i ai kl
0 0 1
cθji sθj
i– bli bk
i–( )–
sθji cθj
i ali ak
i–( )
0 0 1
= = =
fixed fixed
Use frame k as intermediate
Mobile Robotics - Prof Alonzo Kelly, CMU RI 44
Jkli = kili * kjki Jucklee = keylee*kudgekey
![Page 44: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/44.jpg)
2.7.4.6 Inverse Pose Jacobian • Intuition:
– Need it to solve problems like this…
yi
xi
xj yj
aj i
bj i
θj i
ai j
bi j
input
output
ρij∂
∂ρda
ρji∂
∂ρda
ρij∂
∂ρji
=For two frames a & d
Mobile Robotics - Prof Alonzo Kelly, CMU RI 45
![Page 45: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/45.jpg)
Inverse of a Pose
Tij( )
1–Tj
icθi
j sθij cθi
j aij– sθi
jbij–
sθij– cθi
j sθijai
j cθij bi
j–0 0 1
= =
aji cθi
j aij– sθi
jbij–=
bji sθi
jaij cθi
jbij–=
θji θi
j–=
Eqn C
Differentiated Eqn C ρi
j∂
∂ρji cθi
j– sθij– sθi
jaij cθi
jbij–
sθij cθi
j– cθijai
j sθijbi
j+
0 0 1–
cθij sθi
j bji–
sθij– cθi
j aji
0 0 1
–= =
Mobile Robotics - Prof Alonzo Kelly, CMU RI 46
![Page 46: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/46.jpg)
Outline • 2.7.1 Transforms as Relationships • 2.7.2 Solving Pose Networks • 2.7.3 Overconstrained Networks • 2.7.4 Differential Kin of Frames in General Position • Summary
Mobile Robotics - Prof Alonzo Kelly, CMU RI 47
![Page 47: Chapter 2 Math Fundamentalsalonzo/books/math4.pdf · Math Fundamentals Part 4 2.7 Transform Graphs and Pose Networks . 1 Mobile Robotics - Prof Alonzo Kelly, CMU RI . Outline •](https://reader034.fdocuments.us/reader034/viewer/2022050108/5f466d9c4531ec30be5fa7cf/html5/thumbnails/47.jpg)
Summary • HTs encode a spatial relationship (between two
frames). • A “pose” and a HT are two expressions of exactly the
same underlying thing. • It is trivial to write the compound HT relating any two
frames in a pose network of arbitrary complexity. – But it only exists if they are connected.
• Overconstrained networks can contain inconsistencies and that is valuable information. – Spanning Trees and Cycle Bases are fundamental concepts
in such networks. • Derivatives of pose compositions can be computed in
closed form.
Mobile Robotics - Prof Alonzo Kelly, CMU RI 48