Forward and Inverse Kinematics CSE 3541 Matt Boggus.
-
Upload
megan-atkins -
Category
Documents
-
view
230 -
download
5
Transcript of Forward and Inverse Kinematics CSE 3541 Matt Boggus.
Forward and Inverse Kinematics
CSE 3541Matt Boggus
Hierarchical Modeling
Relative motion
Constrains motion Reduces dimensionality
Parent-child relationshipSimplifies motion specification
Modeling & animating hierarchies
3 aspects1. Linkages & Joints – the relationships2. Data structure – how to represent such a hierarchy3. Converting local coordinate frames into global space
Terms
Joint – allowed relative motion & parameters
Joint Limits – limit on valid joint angle values
Link – object involved in relative motion
Linkage – entire joint-link hierarchy
End effector – most distant link in linkage
Pose – configuration of linkage using given set of joint angles
Forward vs. Inverse Kinematics
Forward KinematicsInput: joint anglesOutput: link positions and orientations end effector position
Inverse KinematicsInput: end effectorOutput: joint angles
Joints – relative movement
Every joint allowing motion in one dimension is said to have one degree of freedom (DOF)
Example: flying (Six DOF)• x, y, and z positions (prismatic or translation)• roll, pitch, and yaw (revolute or rotation)
Complex Joints
Hierarchical structure
Forward Kinematics: A Simple Example• Forward kinematics map as a coordinate transformation
– The body local coordinate system of the end-effector was initially coincide with the global coordinate system
– Forward kinematics map transforms the position and orientation of the end-effector according to joint angles
2
11l
X Y
1
0
0
1
Ty
x
e
e2l
X
Y
Example from Jehee Lee
A Chain of Transformations
1
0
0
1
Ty
x
e
e
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
2
11l
X Y
2l
X
Y
Thinking of Transformations• In a view of body-attached coordinate system
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
X
Y
X Y
X
Y
Thinking of Transformations• In a view of body-attached coordinate system
1X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
Thinking of Transformations• In a view of body-attached coordinate system
11L
X Y
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
2
11L
X Y
Thinking of Transformations• In a view of body-attached coordinate system
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
2
11L
X Y
2L
Thinking of Transformations• In a view of body-attached coordinate system
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
Thinking of Transformations• In a view of global coordinate system
X
Y
X Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
Thinking of Transformations• In a view of global coordinate system
X
Y
2L
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
2
X Y
2L
Thinking of Transformations• In a view of global coordinate system
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
Thinking of Transformations• In a view of global coordinate system
21L
X Y
2L
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
Thinking of Transformations• In a view of global coordinate system
2
11L
X Y
2L
X
Y
100
010
01
100
0cossin
0sincos
100
010
01
100
0cossin
0sincos 2
22
221
11
11
2211
ll
translrottranslrotT
Controlling forward kinematics
• QWOP http://www.foddy.net/Athletics.html
IK solution uniqueness
Images from http://freespace.virgin.net/hugo.elias/models/m_ik.htm
Zero One
Two Many
Inverse kinematics
Given goal position for end effectorCompute internal joint angles
Simple linkages analytic solutionOtherwise numeric iterative solution
Inverse kinematics – spaces
Configuration spaceReachable workspace
Analytic Inverse Kinematics
• Given target position (X,Y)– Compute angle with
respect to origin of linkage– Solve for angle of rotation
for each joint
• See reference text for solution and derivation
IK - numeric
Remember: a pose is the position and orientation of all links for given a set of joint angles
- Compute the desired change from this poseVector to the goal, orMinimal distance between end effector and goal
- Compute set of changes to joint angles to make that change
Solve iteratively – numerically solve for step toward goal
IK math notation
),,,,,(
),,,,,(
),,,,,(
),,,,,(
),,,,,(
),,,,,(
65432166
65432155
65432144
65432133
65432122
65432111
xxxxxxfy
xxxxxxfy
xxxxxxfy
xxxxxxfy
xxxxxxfy
xxxxxxfy
XFY
Given the values for each xi, we can compute each yi
IK – chain rule
dt
x
x
f
dt
x
x
f
dt
x
x
f
dt
x
x
f
dt
x
x
f
dt
dx
x
f
dt
dy iiiiiii 6
6
5
5
4
4
3
3
2
2
1
1
XX
FY
The change in output variables (y) relative to the change in input variables (x)
Inverse Kinematics - Jacobian
JV Desired motion of end effector
Unknown change in articulation variables
The Jacobian is the matrix relating the two: describing how each coordinate changes with respect to each joint angle in our system
XX
FY
Inverse Kinematics - Jacobian
JV zyxzyx vvvV ,,,,, 654321 ,,,,,
61
1
621
zz
y
xxx
p
ppp
J
Change in position
Change in orientation
Jacobian
Change in articulation variables
Solving
VJ
JJJJVJJJ
JJVJ
JV
TTTT
TT
11
If J is not square, use of pseudo-inverse of JacobianIf J square, compute inverse, J-1
VJ
JV1
IK – compute positional change vectors induced by changes in joint angles
Instantaneous positional change vectors
Desired change vector
One approach to IK computes linear combination of change vectors that equal desired vector
IK - singularity
Some singular configurations are not so easily recognizable
Inverse Kinematics - Numeric
Given • Current configuration• Goal position
Determine • Goal vector• Positions & local coordinate systems of interior joints (in global coordinates)
Solve for change in joint angles & take small step – or clamp acceleration or clamp velocity
Repeat until:• Within epsilon of goal• Stuck in some configuration• Taking too long
IK – cyclic coordinate descent
Consider one joint at a time, from outside inAt each joint, choose update that best gets end effector to goal position
In 2D – pretty simple
Heuristic solution
EndEffector
Goal
Ji
axisi
Cyclic-Coordinate Descent- Starting with the root of our effector, R, to our current endpoint, E.- Next, we draw a vector from R to our desired endpoint, D- The inverse cosine of the dot product gives us the angle between the vectors: cos(a) = RD ● RE
Cyclic-Coordinate DescentRotate our link so that RE falls on RD
Cyclic-Coordinate DescentMove one link up the chain, and repeat the process
Cyclic-Coordinate DescentThe process is basically repeated until the root joint is reached. Then the process begins all over again starting with the end effector, and will continue until we are close enough to D for an acceptable solution.
Cyclic-Coordinate Descent
Cyclic-Coordinate DescentWe’ve reached the root. Repeat the process
Cyclic-Coordinate Descent
Cyclic-Coordinate Descent
Cyclic-Coordinate Descent
Cyclic-Coordinate Descent
Cyclic-Coordinate Descent
Cyclic-Coordinate DescentWe’ve reached the root again. Repeat the process until solution reached.
IK – cyclic coordinate descent
Other orderings of processing joints are possible
Because of its procedural nature• Lends itself to enforcing joint limits• Easy to clamp angular velocity
IK – 3D cyclic coordinate descent
First – goal has to be projected onto plane defined by axis (normal to plane) and EF
EndEffector
Goal
Ji
axisi
Projected goal
Second– determine angle at joint