Forward and Inverse Kinematics

filercaliforniaMechanics

Nov 14, 2013 (3 years and 8 months ago)

71 views

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.

Linkages & Joints


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


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 Kinematics

Input: joint angles

Output:
link positions and orientations


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

Simple linkages


慮慬祴楣i獯汵s楯i

佴桥牷楳i ††††


湵浥物挠楴敲慴楶攠獯汵s楯i

Inverse kinematics


spaces

Configuration
space

Reachable
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
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