Inverse Kinematics

From Position to Angles

 Inverse Kinematics
• Given a desired position
(P) & orientation (R) of
the end-effector z
Y  ( x, y, z , O, A, T )

q  (1 ,  2 ,  n ) y
Find the joint variables
which can bring the robot x
to the desired
configuration.
Solvability
0
Given the numerical value of T we attempt to
N
find values of 1 , 2 ,, n .
The PUMA 560:

Given 06T as 16 numerical values,

solve for6 joint angles, 1 , 2 ,, 6 .

12 equations and 6 unknowns

6 equations and 6 unknowns

(nonlinear, transcendental equations)
 Inverse Kinematics
• More difficult.
– The equations to solve are
nonlinear thus systematic
closed-form solution is not
always available.
– Solution not unique.
• Redundant robot.
• Elbow-up/elbow-down configuration.
– Robot dependent.
2 solutions!
 The Workspace

• Workspace: volume of space which can be

reached by the end effector
• Dextrous workspace: volume of space
where the end effector can be arbitrarily
oriented
• Reachable workspace: volume of space
which the robot can reach in at least one
orientation
Two-link manipulator
If l1 = l2 reachable work
space is a disc of radius
2l1. The dextrous
Workspace is a point:
origin
If l1 l2 there is no
dextrous workspace.
The reachable work
space is a ring of outer
radius l1 + l2 and inner
radius l1 - l2
 Existence of Solutions

• A solution to the IKP exists if the target

belongs to the workspace.
• Workspace computation may be hard.
In practice it is made easy by special
design of the robot.
 Methods of Solutions
• A manipulator is solvable if the joint
variables can be determined by an
algorithm. The algorithm should find all
possible solutions.

closed form solutions

• Solutions
numerical solutions
 Numerical Solutions
• Results in a numerical, iterative solution to system of
equations, for example Newton/Raphson techniques.
• Unknown number of operations to solve.
• Only returns a single solution.
• Accuracy is dictated by user.
• Because of these reasons, this is much less desirable
than a closed-form solution.
• Can be applied to all robots.
Closed-form solutions
Analytical solution to system of equations
Can be solved in a fixed number of
operations (therefore, computationally
fast/known speed)
Results in all possible solutions to the
manipulator kinematics
Often difficult or impossible to find
Most desirable for real-time control
Most desirable overall
 Inverse Kinematic Problem

Given: Position & Orientation Find: joint coordinates

of END-EFFECTOR

0T q1, q2, q3,…, qN

N

Need to solve at most six independent equations in

N unknowns.
 Inverse Kinematic Problem
ISSUES
• Existence of solutions
– Workspace
– Dextrous Workspace
– Less than 6 joints
– Joint limits (practical)
• Multiple solutions
– Criteria Algebraic
– Solvability closed form
numerical Geometric
– number of solutions
= 16 di, ri  0 for six points
 Solution To Inverse
Kinematics
0T = 0T11T22T3…N-1TN = A1A2A3…AN
N

nx ox ax px   cθi -c i sθ i s i sθi d i cθi 

n 
oy ay py  sθ c cθ -s cθ d sθ 
Given: TN 
0  y
Ai   i i i i i i i
nz oz az pz   s i c i ri 
   
   1    1

Find: q = q1, q2, q3, … , qN (joint coordinates)

 Solution To Inverse
Kinematics
n xox a x px 
n oy ay py 
 y  A 1 A 2 A 3 ...A N
nz oz a z pz 
 
    1 
6 independent
12 Equations N unknowns
6 redundant

LHS(i,j) = RHS(i,j)
column
row j = 1, 2, 3, 4
i = 1, 2, 3
 Solution To Inverse
Kinematics
General Approach: Isolate one joint variable at a time

A1-1 0TN = A2A3…AN = 1TN

function of q1 function of q2, … , qN

• Look for constant elements in 1TN

• Equate LHS(i,j) = RHS(i,j)
• Solve for q1
 Solution To Inverse
-1 -10
Kinematics
A A T = A …A = 2T
2 1 N 3 N N

function of q3, … , qN
function of q1, q2
only one unknown q2 since q1 has been solved for
• Look for constant elements of 2TN
• Equate LHS(i,j) = RHS(i,j)
• Solve for q2
• Maybe can find equation involving q1 only
Note:
– There is no algorithmic approach that is
100% effective
– Geometric intuition is required
 A Simple Example

Revolute and
Finding :
Prismatic
y
Joints θ  arctan( )
Combined x
More Specifically:
(x , y)
y arctan2() specifies that it’s in the
θ  arctan 2( ) first quadrant
x
Y

S
1 Finding S:
X

S  (x 2  y 2 )
Inverse Kinematics of a Two Link
Manipulator
(x , y) Given: l1, l2 , x , y

2 Find: 1, 2
l2 Redundancy:
A unique solution to this
problem does not exist. Notice, that
using the “givens” two solutions are
1
l1
possible.
Sometimes no solution is possible.

(x , y)
The Geometric
Solution (x , y)
l2
2

l1

1
The Geometric
Solution (x , y) Using the Law of Cosines:
l2
2
c 2  a 2  b 2  2ab cos C
2 2
( x 2  y 2 )  l1  l2  2l1l2 cos(180  θ 2 )
cos(180  θ 2 )   cos(θ 2 )
2 2
l1 x 2  y 2  l1  l2
 cos(θ 2 ) 
2l1l2
1
 x 2  y 2  l12  l2 2 
θ 2  arccos 

 2l1l2 
Using the Law of Cosines:
Redundant since 2 could be in the
sin B sin C
 first or fourth quadrant.
b c
sin θ1 sin(180  θ 2 ) sin(θ 2 )
  Redundancy caused since 2 has two
l2 x y
2 2
x 2  y2 possible values

θ1  θ1  α  l sin(θ )  y
θ1  arcsin 2 2 
 arctan 2 
y  x 2  y2  x
α  arctan 2   
x
The Algebraic Solution
l2 (x , y) 
2

l1

1
The Algebraic Solution
l2 (x , y) 
2

c 1  cos θ 1
c 1  2  cos( θ 2  θ 1 )
l1 (1) x  l1 c 1  l 2 c 1  2
(2) y  l1 s 1  l 2 sin 1 2
1
(3) θ  θ 1  θ 2

(1) 2  ( 2) 2  x 2  y 2 
 2 2 2
  2 2 2
 l1 c1  l2 (c1 2 ) 2  2l1l2 c1 (c1 2 )  l1 s1  l2 (sin 1 2 ) 2  2l1l2 s1 (sin 1 2 ) 
 l1  l2  2l1l2 c1 (c1 2 )  s1 (sin 1 2 ) 
2 2

2 2
 l1  l2  2l1l2 c 2 Only Note :
Unknown
 x  y  l1  l22 2 2 2
 cos(ab)  (cosa)(cosb)(sin a)(sinb
 θ 2  arccos 
2l1l2  sin(ab)  (sin a)(cosb)(cosa)(sinb
 
 x  l1 c 1  l 2 c 1  2 Note:
 l1 c 1  l 2 c 1 c 2  l 2 s 1 s 2 cos(ab)  (cosa)(cosb) (sina)(sinb)
 c 1 ( l1  l 2 c 2 )  s 1 ( l 2 s 2 ) sin(ab)  (cosa)(sinb) (cosb)(sina)

y  l1 s 1  l 2 sin 1 2 We know what 2 is from the

 l1 s 1  l 2 s 1 c 2  l 2 s 2 c 1 previous slide. We need to solve
 c 1 ( l 2 s 2 )  s 1 ( l1  l 2 c 2 ) for 1 . Now we have two
equations and two unknowns (sin
x  s1 ( l 2 s 2 ) 1 and cos 1 )
c1 
( l1  l 2 c 2 )
x  s1 ( l 2 s 2 ) Substituting for c1 and
y ( l 2 s 2 )  s 1 ( l1  l 2 c 2 ) simplifying many times
( l1  l 2 c 2 )


1
( l1  l 2 c 2 )

x l 2 s 2  s 1 ( l1 2  l 2 2  2 l1 l 2 c 2 )  Notice this is the law of
cosines and can be replaced
by x2+ y2

y ( l1  l 2 c 2 )  x l 2 s 2  y ( l1  l 2 c 2 )  x l 2 s 2 
s1  θ 1  arcsin  

x2 y2  x2 y2 
 Inverse Kinematics
• Analytical solution only works for a fairly
simple structure

• Numerical/iterative solution needed for a

complex structure
 Numerical Approaches
• Inverse kinematics can be formulated as
an optimization problem
 Function Optimization
• Finding the minimum for nonlinear
functions
 Formulation
• So how to convert the IK process in an
optimization function?
arg min F ( )


θ2
l2
l1

θ1 C=(Cx,Cy)

Base (0,0)
 Iterative Approaches
Find the joint angles θ that minimizes the distance between
the hypothesized character position and user specified
position
arg min   f i ( )  Ci 
2

i

hypothesized specified
position position
θ2
l2
l1

θ1 C=(Cx,Cy)

Base (0,0)
 Iterative Approaches
Find the joint angles θ that minimizes the distance between
the hypothesized character position and user specified
position

arg min (l1 cos 1  l2 cos(1   2 )  c1 ) 2  (l1 sin 1  l2 sin(1   2 )  c2 ) 2

1 , 2

θ2
l2
l1

θ1 C=(c1,c2)

Base (0,0)
 Iterative Approaches
Mathematically, we can formulate this as an
optimization problem:

arg min   f i ( )  ci 
2

i

The above problem can be solved by many

nonlinear optimization algorithms:
- Steepest descent
- Gauss-newton
- Levenberg-marquardt, etc
Gradient-based Optimization
 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k

 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k

How can we decide the amount

of update?
 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k


arg min  f i ( k   k )  ci


2

i
 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k


arg min  f i ( k   k )  ci


2

Known!
 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k

arg min
k  f i 
( k
  k
)  ci 
2
Taylor series expansion
i
 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k

arg min
k  f i 
( k
  k
)  ci 
2
Taylor series expansion
i
2
 f i  rearrange
 arg min
 k
i  i
f ( k
) 
 k
 k
 ci

 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k

arg min
k  f i 
( k
  k
)  ci 
2
Taylor series expansion
i
2
 f i  rearrange
 arg min
 k
i  i
f ( k
) 
 k
 k
 ci


Can you solve this optimization problem?

 Gauss-Newton Approach
Step 1: initialize the joint angles with  0

Step 2: update the joint angles:  k 1   k   k

arg min

k  f i (
 k
  k
)  ci 2
Taylor series expansion
i
2
 f i  rearrange
 arg min
 k
i  i
f ( k
) 
 k
 k
 ci

2
 f i k 
 arg min
 k
i   k  k
 ( ci  f i ( ))


This is a quadratic function of 

 Gauss-Newton Approach
• Optimizing an quadratic function is easy
2
 f i k 
arg min
 k
   k
i 
 k
 ( ci  f i ( ))


• It has an optimal value when the gradient is

zero f f
1
...
f
1 1
1  2  N 1 c1  f i (1 ,...,  N )
f 2 f 2 f 2  c  f 2 (1 ,...,  N )
... 2
1  2  N   2
 
   
f M f M f M  N c N  f i (1 ,...,  N )

 N  2  N
 Gauss-Newton Approach
• Optimizing an quadratic function is easy
2
 f 
arg min   ik  k  (ci  f i ( k ))
i   


• It has an optimal value when the gradient is

zero f1f
...1f 1
1  2  N 1 c1  f i (1 ,...,  N )
f 2 f 2 f 2  c  f 2 (1 ,...,  N )
...
b
2
 N   2
1

f M
 2

f M
J 
Δθ 
f M  N c N  f i (1 ,...,  N )

 N  2  N

Linear equation!
 Gauss-Newton Approach
• Optimizing an quadratic function is easy
2
 f 
arg min   ik  k  (ci  f i ( k ))
i   


• It has an optimal value when the gradient is

zero f1f
...1f 1
1  2  N 1 c1  f i (1 ,...,  N )
f 2 f 2 f 2  c  f 2 (1 ,...,  N )
...
b
2
 N   2
1

f M
 2

f M
J 
Δθ 
f M  N c N  f i (1 ,...,  N )

 N  2  N
 Gauss-Newton Approach
• Optimizing an quadratic function is easy
2
 f 
arg min   ik  k  (ci  f i ( k ))
i   


• It has an optimal value when the gradient is

zero f1f
...1f 1
1  2  N 1 c1  f i (1 ,...,  N )
f 2 f 2 f 2  c  f 2 (1 ,...,  N )
...
b
2
 N   2
1

f M
 2

f M
J  
Δθ 
f M  N c N  f i (1 ,...,  N )

 N  2  N

  ( J T J ) 1 J T b