# Forward and Inverse Kinematics

Μηχανική

14 Νοε 2013 (πριν από 4 χρόνια και 7 μήνες)

95 εμφανίσεις

Forward and Inverse Kinematics

CSE 3541

Matt
Boggus

Hierarchical Modeling

Relative motion

Constrains motion

Reduces dimensionality

Parent
-
child relationship

Simplifies motion specification

Modeling & animating hierarchies

3 aspects

1.

the relationships

2.

Data structure

how to represent such a hierarchy

3.

Converting local coordinate frames into global space

Terms

Joint

allowed relative motion &
parameters

Joint Limits

limit on valid joint angle
values

object involved in relative
motion

entire joint
-
hierarchy

End
effector

Pose

configuration of linkage using given set of
joint angles

Forward vs. Inverse Kinematics

Forward Kinematics

Input: joint angles

Output:

end effector position

Inverse Kinematics

Input: end effector

Output: 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

1

1
l
X

Y

1
0
0
1
T
y
x
e
e
2
l
X
Y
Example from
Jehee

Lee

A Chain of Transformations

1
0
0
1
T
y
x
e
e

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

2

1

1
l
X

Y

2
l
X
Y
Thinking of Transformations

In a view of body
-
attached coordinate system

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

X
Y
X

Y

X
Y
Thinking of Transformations

In a view of body
-
attached coordinate system

1

X

Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

Thinking of Transformations

In a view of body
-
attached coordinate system

1

1
L
X

Y

X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

2

1

1
L
X

Y

Thinking of Transformations

In a view of body
-
attached coordinate system

X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

2

1

1
L
X

Y

2
L
Thinking of Transformations

In a view of body
-
attached coordinate system

X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

Thinking of Transformations

In a view of global coordinate system

X
Y
X

Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

Thinking of Transformations

In a view of global coordinate system

X

Y

2
L
X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

2

X

Y

2
L
Thinking of Transformations

In a view of global coordinate system

X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

Thinking of Transformations

In a view of global coordinate system

2

1
L
X

Y

2
L
X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

Thinking of Transformations

In a view of global coordinate system

2

1

1
L
X

Y

2
L
X
Y

1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
1
0
0
0
1
0
0
1
1
0
0
0
cos
sin
0
sin
cos
2
2
2
2
2
1
1
1
1
1
2
2
1
1
l
l
transl
rot
transl
rot
T

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 effector

Compute internal joint angles

Inverse kinematics

spaces

Configuration
space

Reachable
workspace

Analytic Inverse Kinematics

Given target position (X,Y)

Compute angle with

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
given a set of joint
angles

-

Compute the desired
change from this
pose

Vector to the goal, or

Minimal 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

)
,
,
,
,
,
(
)
,
,
,
,
,
(
)
,
,
,
,
,
(
)
,
,
,
,
,
(
)
,
,
,
,
,
(
)
,
,
,
,
,
(
6
5
4
3
2
1
6
6
6
5
4
3
2
1
5
5
6
5
4
3
2
1
4
4
6
5
4
3
2
1
3
3
6
5
4
3
2
1
2
2
6
5
4
3
2
1
1
1
x
x
x
x
x
x
f
y
x
x
x
x
x
x
f
y
x
x
x
x
x
x
f
y
x
x
x
x
x
x
f
y
x
x
x
x
x
x
f
y
x
x
x
x
x
x
f
y

X
F
Y

Given the values for each x
i
, we can compute each
y
i

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
i
i
i
i
i
i
i
6
6
5
5
4
4
3
3
2
2
1
1

X
X
F
Y

The change in output variables (y) relative to the change in input variables (x)

Inverse Kinematics
-

Jacobian

J
V

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

X
X
F
Y

Inverse Kinematics
-

Jacobian

J
V

z
y
x
z
y
x
v
v
v
V

,
,
,
,
,

6
5
4
3
2
1
,
,
,
,
,

6
1
1
6
2
1

z
z
y
x
x
x
p
p
p
p
J

Change in
position

Change in
orientation

Jacobian

Change in
articulation
variables

Solving

V
J
J
J
J
J
V
J
J
J
J
J
V
J
J
V
T
T
T
T
T
T
1
1
If J
is not
square,
use
of pseudo
-
inverse of
Jacobian

If J square, compute inverse, J
-
1

V
J
J
V
1
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 in

At each joint, choose update that best gets end
effector to goal position

In 2D

pretty simple

Heuristic solution

EndEffector

Goal

J
i

axis
i

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 Descent

Rotate
our link so that RE falls on RD

Cyclic
-
Coordinate Descent

Move
one link up the chain, and repeat the process

Cyclic
-
Coordinate Descent

The 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 Descent

We’ve reached the root. Repeat the process

Cyclic
-
Coordinate Descent

Cyclic
-
Coordinate Descent

Cyclic
-
Coordinate Descent

Cyclic
-
Coordinate Descent

Cyclic
-
Coordinate Descent

Cyclic
-
Coordinate Descent

We’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

J
i

axis
i

Projected goal

Second

determine angle at joint