Inverse Kinematics
1. Set goal configuration of end effector
2. calculate interior joint angles
Analytic approach – when linkage is simple enough, directly
calculate joint angles in configuration that satifies goal
Numeric approach – complex linkages
At each time slice, determine joint movements that take
you in direction of goal position (and orientation)
Rick Parent - CIS682
Forward Kinematics - review
Articulated linkage – hierarchy of joint-link pairs
Pose – linkage is a specific configuration
Pose Vector – vector of joint angles for linkage
Degrees of Freedom (DoF) – of joint or of whole figure
Types of joints: revolute, prismatic
Tree structure – arcs & nodes
Recursive traversal – concatenate arc matrices
Push current matrix leaving node downward
Pop current matrix traversing back up to node
Rick Parent - CIS682
Inverse Kinematics
End Effector
L1
q2 L
2
q3
L3
q1
Goal
Rick Parent - CIS682
Inverse Kinematics
Underconstrained – if fewer constraints than DoFs
Many solutions
Overconstrained – too many constraints
No solution
Reachable workspace – volume the end effector can reach
Dextrous workspace – volume end effector can reach in
any orientation
Rick Parent - CIS682
Inverse Kinematics - Analytic
Given arm configuration (L1, L2, …)
Given desired goal position (and orientation) of
end effector: [x,y] or [x,y,z, y1,y2, y3]
Analytically compute goal configuration (q1,q2)
Interpolate pose vector from initial to goal
Rick Parent - CIS682
Analytic Inverse Kinematics
q2
L1
q1
L2
Goal
(X,Y)
Rick Parent - CIS682
Analytic Inverse Kinematics
Multiple solutions
Goal
(X,Y)
Rick Parent - CIS682
Analytic Inverse Kinematics
L1
180- q2
q1
L2
(X,Y)
qT
Rick Parent - CIS682
Analytic Inverse Kinematics
L1
180- q2
q1
x2  y2
qT
L2
(X,Y)
Y
X

Rick Parent - CIS682
Law of Cosines
C
A
a
B
A2  B 2  C 2
cos(a ) 
2 AB
Rick Parent - CIS682
Analytic Inverse Kinematics
L2
L1
q1
(X,Y)
180- q2
x2  y2
qT
cos(qT ) 


2
2 
 X Y 
qT  cos 1 
cos(q1  qT ) 

L1  L2  X 2  Y 2
cos(180  q 2 ) 
2 L1 L2
2
2

q1  cos (
1


2
2
1 L1  L2  X  Y
q 2  180  cos (
)
2 L1 L2
2
2
X 2 Y 2

Y
X

X
X
L1  X 2  Y 2  L2
2
2
2 L1 X 2  Y 2
L1  X 2  Y 2  L2
2
2 L1 X  Y
2
2
2
)  qT
Rick Parent - CIS682
Analytic Inverse Kinematics
Rick Parent - CIS682
Iterative Inverse Kinematics
When linkage is too complex for analytic methods
At each time step, determine changes to joint
angles that take the end effector toward goal
position and orientation
Need to recompute at each time step
Rick Parent - CIS682
Inverse Jacobian Method
a2
d2=EF-J2
q2
End Effector
a2 x d2
- Compute instantaneous effect of each joint
- Linear approximation to curvilinear motion
- Find linear combination to take end effector
towards goal position
Rick Parent - CIS682
Inverse Jacobian
Method
Instantaneous linear change in
end effector for ith joint
= (EF - Ji) x ai
Rick Parent - CIS682
What is the change in
orientation of end effector
induced by joint i that has
axis of rotation a i
and position Ji?
Inverse Jacobian
Method
Angular velocity
ai  i
Rick Parent - CIS682
Inverse Jacobian Method
Solution only valid for an
instantaneous step
Angular affect is really
curved, not straight line
Once a step is taken, need
to recompute solution
Rick Parent - CIS682
Inverse
Jacobian
Method
- Mathematics
Set up equations
yi: state variable
xi : system parameter
fi : relate system parameters to state variable
y1  f1 (x1, x 2 , x 3 , x 4 , x 5 , x 6 )
y 2  f 2 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
y 3  f 3 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
y 4  f 4 (x1, x 2 , x 3 , x 4 , x 5 , x 6 )
y 5  f 5 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
y 6  f 6 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
Rick Parent - CIS682
Inverse Jacobian
Method
- Mathematics
Matrix Form

y1  f1 (x1, x 2 , x 3 , x 4 , x 5 , x 6 )
y 2  f 2 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
y 3  f 3 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
y 4  f 4 (x1, x 2 , x 3 , x 4 , x 5 , x 6 )
y 5  f 5 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
y 6  f 6 (x1, x 2, x 3 , x 4 , x 5 , x 6 )
Y  F(X)
Rick Parent - CIS682
Inverse Jacobian Method
- Mathematics
yi  f1 ( x1 , x2 , x3 , x4 , x5 , x6 )
Use chain rule to differentiate equations to relate changes in
system parameters to changes in state variables
f i
f i
f i
f i
f i
f i
dyi 
dx1 
dx2 
dx3 
dx4 
dx5 
dx6
x1
x2
x3
x4
x5
x6
Rick Parent - CIS682
Inverse Jacobian Method
- Mathematics
f i
f i
f i
f i
f i
f i
yi 
dx1 
dx2 
dx3 
dx4 
dx5 
dx6
x1
x2
x3
x4
x5
x6
Matrix Form
Y  F(X)
F
dY 
dX
X
Rick Parent - CIS682
Inverse Jacobian Method
F
dY 
dX
X
Change in position (and
orientation) of end effector
Change in joint angles
Linear approximation that relates change in
joint angle to change in end effector position
(and orientation)
Rick Parent - CIS682
Inverse Jacobian Method
F
dY 
dX
X
V  Jq
Rick Parent - CIS682
Inverse
Jacobian
Method
F
dY 
dX
X
 p x
 q
 1
 p y
 vx  
 v   q1
 y   p z
 v z   q
  1
 x   a x
 y   q1
   a
 z   y
 q1
 a z

 q1
p x
p x

q 2
q n
p y
p y

q 2
q n

















 q 
 1 
 q2 
 
 
 qn 






Rick Parent - CIS682
Inverse Jacobian Method
= (S - J1) x a1
 p x
 q
 1
 p y
 vx  
 v   q1
 y   p z
 v z   q
  1
 x   a x
 y   q1
   a
 z   y
 q1
 a z

 q1
p x
p x

q 2
q n
p y
p y

q 2
q n

















 q 
 1 
 q2 
 
 
 qn 






= 1
Rick Parent - CIS682
The Matrices

q  q
vy
vz
 x  y  z T
q
q
... q n
V  vx
1
 p x
 q
 1
 p y
J   q
 ...1
 a
 z
 q1
2
3

T
p x
q 2
p y
q 2
p x 
...
q n 

p y 
...
q n 
...
a z
...
q 2
V  Jq
... 
a z 

...
q n 
Rick Parent - CIS682
The Matrices
N DoFs
V  Jq
3x1, 6x1
V – desired linear and angular velocities
3xN, 6xN
J – Jacobian
Matrix of partials
Nx1
q – change to joint angles (unknowns)
Rick Parent - CIS682
Pseudo Inverse of the Jacobian
V  Jq
J V  J Jq
T
T
( J J ) J V  ( J J ) J Jq

J V  q
T
1
T
T
1
T
J   (J T J)1 J T  J T (J J T )1

Rick Parent - CIS682
Solving using the Pseudo Inverse
J V  q
J T ( JJ T ) 1V  q

  ( JJ ) V
T 1
( JJ )   V
T
LU decomposition
J T   q
Rick Parent - CIS682
Adding a Control Term
A solution of this form
q  ( J  J  I ) z
When put into this formula
V  Jq
V  J (J  J  I )z
Like this
After some manipulation,
you can show that it…
…doesn’t affect the
desired configuration
V  ( JJ  J  J ) z
V  (J  J )z
V  0z
V 0
But it can be used to bias
The solution vector
Rick Parent - CIS682
Form of the Control Term
Bias to desired angles
(not the same as hard joint limits)
Desired angles and
corresponding
gains are input
n
H  a i (q i  q ci )y
‘z’ is H differentiated
i1
n
dH
z  q H 
 y a i (q i  q ci )y 1
dq
i1
Where the deviation is large, you bump up the solution vector
in such a way that you don’t disturb the desired effect
Rick Parent - CIS682
Some Algebraic Manipulation
Include this in equation
V  Jq  J ( J  J  I )q H
Isolate vector of unknown
q  J V  ( J  J  I )q H
q  J V  ( J  J  I )q H
Rearrange to
isolate the inverse
q  J V  J  Jq H  q H
q  J  (V  Jq H )  q H
q  J T ( JJ T ) 1 (V  Jq H )  q H
q  J T [( JJ T ) 1 (V  Jq H )]  q H
Rick Parent - CIS682
Solving the Equations
  ( JJ ) (V  Jq H )
T 1
T

q  J   q H
V  Jq H  ( JJ T ) 
LU decomp.
Rick Parent - CIS682
Control Term
Use to bias to desired mid-angle
Does not enforce joint angles
Does not address “human-like” or “natural” motion
Only kinematic control – no forces involved
Rick Parent - CIS682
Other ways to numerically IK
Jacobian transpose
Alternate Jacobian – use goal position
HAL – human arm linkage
Damped Least Squares
CCD
Rick Parent - CIS682
Jacobian Transpose
Use projection of effect vector onto desired movement
Rick Parent - CIS682
Jacobian Transpose
(( S  J i )  ai )  (G  S )
S
J q  V
T
Rick Parent - CIS682
Jacobian Transpose
(( S  J1 )  a1 )  (G  S )
(( S  J 2 )  a2 )  (G  S )
…
(( S  J n )  an )  (G  S )
(( S  J1 )  a1 ) x
(( S  J )  a )
1
1 y

 (( S  J1 )  a1 ) z


(( S  J 2 )  a2 ) x
...
(( S  J n )  an ) x 

...
...


...


T
(G  S ) x 
(G  S ) 
y

(G  S ) z 
 J V  q
T
Rick Parent - CIS682
Alternate Jacobian
G
Use the goal postion instead of the end-effector!!??
((G  J1 )  a1 )  (G  S )
((G  J 2 )  a2 )  (G  S )
…
J
Alt
!?
((G  J n )  an )  (G  S )
Rick Parent - CIS682
Damped Least Squares
G
( J J   I )q  J V
T
2
1 T
q  ( J J   I ) J V
T
T
2
1
q  J ( JJ   I ) V
T
2
1
f  ( JJ   I ) V
T
2
( JJ   I ) f  V
T
J f  q
T
substitution
Solve
2
T
Rick Parent - CIS682
Hueristic Human-Like Linkage (HAL)
7 DoF linkage
RA (q1,q2,q3 , RB ( q4), RC(q5,q6,q7
3 DoF
G
Decompose into simpler subproblems
1 DoF
3 DoF
Set hand position and rotation
based on relative position of
Goal to shoulder
Fix wrist position – use as Goal
Rick Parent - CIS682
Hueristic Human-Like Linkage (HAL)
s
L
L1
e
L2
w
Set q4 based on distance
between shoulder and wrist
Assume axis of elbow is
perpendicular to plane defined
by s, e, w use law of cosines
L1  e  s
L2  w  e
L  ws
L1  L2  L2
cos(q t ) 
2 L1 L2
2
2
q 4  180  q t
Rick Parent - CIS682
Hueristic Human-Like Linkage (HAL)
s
q4
w
e
Elbow lies on circle defined by w, s & q4
Determine elbow position based on heuristics
For example: project forearm straight from hand orientation
if arm intersects torso or a shoulder angle
exceeds joint limit (or exceeds comfort zone) –
Clamp to inside of limits
Rick Parent - CIS682
Hueristic Human-Like Linkage (HAL)
s
q4
w
e
From e and s, determine RA
From e and w and hand orientation, determine RB
Rick Parent - CIS682
Cyclic-Coordinate Descent
Traverse linkage from distal joint inwards
Optimally set one joint at a time
Update end effector with each joint change
At each joint, minimize difference between end effector and goal
Easy if only trying to match position; heuristic if orientation too
Use weighted average of position and orientation.
Rick Parent - CIS682
Cyclic-Coordinate Descent
E (q)  E p (q)  Eo (q)
E p (q )  ( Pd  Pc )
2
3
Eo (q)   ((u jd  u jc )  1)
2
.
j 1
Rick Parent - CIS682
Cyclic-Coordinate Descent
Rotational joint:
Pic ( )  Raxisi ( ) Pic
g p ( )  Pic ( )  Pid
3
g o ( )   u jd  ujc ( )
j 1
g ( )  w p g p ( )  wo g o ( )
.
Rick Parent - CIS682
Cyclic-Coordinate Descent
Rotational joint:
g ( )  w p g p ( )  wo g o ( )
w p  a (1   )
wo  1
a  k /W

min( Pid , Pic )
max( Pid , Pic )
.
Rick Parent - CIS682
Cyclic-Coordinate Descent
Rotational joint:
g ( )  k1 (1  cos( ))  k2 cos( )  k3 sin(  )
3
k1  wp ( Pid  axisi )( Pic  axisi )  wo  (u jd  axisi )(u jc  axisi )
j 1
3
k2  wp ( Pid  Pic )  wo  (u jd u jc )
j 1
3
k3  axisi  [ wp ( Pid  Pic )  wo  (u jd u jc )]
.
j 1
Rick Parent - CIS682
Cyclic-Coordinate Descent
Rotational joint:
g ( )  k1 (1  cos( ))  k2 cos( )  k3 sin(  )
(k1  k2 ) sin(  )  k3 cos( )  0
k3
  tan (
)
(k 2  k1 )
1
c  
c  
qi  qi  wi
.
Rick Parent - CIS682
Cyclic-Coordinate Descent
Translational joint:
  ( Pid  Pic )  axisi
..
Rick Parent - CIS682
IK w/ constraints
Chris Welman, “Inverse Kinematics and Geometric Constraints for Articulated
Figure Manipulation,” M.S. Thesis, Simon Fraser University, 2001.
Basic idea:
Constraints are geometric, e.g., point-topoint, point-to-plan, specific orientation, etc.
Assume starting out in satisfied configuration
Forces are applied to system
Detect, and cancel out, force components that would
violate constraints.
Rick Parent - CIS682
IK w/ constraints
Point-on-a-plane constraint
Fa
Fc
Ft
Given: geometric constraints & applied forces
Determine: what constraints will be violated & what
(minimal) forces are needed to counteract the components
of the applied forces responsible for the violations.
Rick Parent - CIS682
Constraints
C (q)  0
To maintain constraints, need:
Notation:
C

C (q) 
q  0
q
ci
Jc 
q j
.
Rick Parent - CIS682
Constraints
C

C (q) 
q  0
q
q  KJ F  Kg
T
IK Jacobian
C  J c Kg
Constraint Jacobian
Generalized force
.
Rick Parent - CIS682
example constraint
Geometric constraint on point that is function of pose
( R  P(q))  0
2

C (q )  c1 ( Px (q )), c2 ( Py (q )), c3 ( Pz (q ))

 [( Rx  Px ) , ( R y  Py ) , ( Rz  Pz ) ]
2
ci ci Pk
Jc 

q j Pk q j
2
2
Usually sparse
.
Rick Parent - CIS682
Computing the constraint force
g  ga  gc
Applied force
To counteract ga’s
affect on constraints:
Yet to be determined constraint force
J c Kgc   J c Kg a
g should lie in the nullspace of JcK
J c Kg  0
.
Rick Parent - CIS682
Computing the constraint force
The system is usually underconstrained
Restrict gc to move the system in a direction it may not go
g c  J c
J c KJ c    J c Kga
T
Solve linear system to find Lagrange multiplier vector.
.
Rick Parent - CIS682
Solving for Lagrange Multipliers
  [1 ,..., m ]
J c KJ c    J c Kga
T
gradient
Shortest distance from A to B
passing through a point P
Constrain P to lie on g
Points on ellipse are set of points for
which sum of distances to foci is
equal to some constant
Direction of
gradients are
equal
A
B
g
Rick Parent - CIS682
Solving for Lagrange Multipliers
J c KJ c    J c Kga
T
A  b
Use truncated SVD with backsubstitution on
J c KJ c
T
A  UDV T
Range basis
diagonal
Nullspace basis.
Rick Parent - CIS682
Solving for Lagrange Multipliers
UDV   A  b
T
UDV   b
T
DV   U b
T
T
V D U b
1
T
T
  VD U b
1
T
Ui b
 
Vi
wi
Rick Parent - CIS682
Feedback term
q  K ( g a  gc )  kCJc
T
Spring that penalizes deviation from constraints.
Rick Parent - CIS682
Implementation
Handles on skeletons
Point handle
orientation handle
Center-of-mass handle
Each handle must know how to its value from q
Each handle must know how to compute the Jacobian.
Rick Parent - CIS682
Constraints on handles
Constraining a point handle to a location
Constraining a point handle to a plane
Constraining a point handle to a line
Constraining an orientation handle to an orientation
.
Rick Parent - CIS682
Dataflow approach
f
q
f
Constraint
function block
f (x )
Knows how to compute
Its function in term of x
Knows its Jacobian wrt x
x
f f x

q x q
x
q
..
Rick Parent - CIS682
Example network
Jc
C
c1
h1
c2
h2
h3
h4
..
q
Rick Parent - CIS682