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