Inverse Kinematics

– About the problem

– Method of solution
– Example
 Standard Frames

{W}

{B}
{T}

{G}
{S}
 Inverse Kinematics

• Given a desired position

(P) & orientation (R) of the
end-effector
z
Y  ( x, y, z, ,  ,  )

q  (1 , 2 , n ) y
Find the joint variables which
can bring the robot to the x
desired configuration.
PUMA 560
 Solvability
Given the numerical value of N0T we attempt to
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
 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.
 Multiple Solutions

• The IKP may have more than one

solution. We need to be able to
calculate all the possible solutions.
• The system has to be able to choose
one. 2 solutions!
The closest solution: the
solution which minimizes
the amount that each
joint is required to move.
 More Complicated Example

2
(x,y)
d2 d3

d1 3

1

d3 1-D space

d2
(self-motion
space)
(1,2,3) d1
 Number of Solutions
Depends upon the 8 solutions exits

number and range of

joints and also is a
function of link
parameters a,a,d

Another 4 solution
 4   4  180 ,
5  5 ,

6  6  180 .
 4 Solutions of the PUMA 560
 Inverse Kinematics
– About the problem
– Method of solution
– Example
 Methods of Solutions

In general the IK problem can be solved by

various methods such as:
• Inverse Transform ( Paul, 1981)
• Screw Algebra (Kohli 1975)
• Dual Matrices (Denavit 1956)
• Iterative (Uicker 1964)
• Geometric approach (Lee 1984)
• Decoupling of position and orientation (Pieper
1968)
 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
Method of solutions
 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
 Closed-form solutions
Given a 6 axis robot.
It can be proven that there exists a closed form
solution for inverse kinematics:
 If three adjacent revolute joint axes intersect at
a point (PUMA, Stanford).
 If three adjacent revolute joint axes are parallel
to one another (MINIMOVER).
In any case algebraic or geometric intuition is
required to obtain closed form solutions.
 Closed-form Solutions

We are interested in closed-form solutions:

1. Algebraic methods

2. Geometric methods
 Algebraic solution
Consider a 3-link manipulator

We can derive kinematic

equations:

B
W T T T T T
0
3
0
1
1 2
2 3
 
X 3
Algebraic solution


Y0 Y3

 
Y1
D-H transformation Y2 X 2


X1 
X 0

i ai-1 ai-1 di i

1 0 0 0 1

2 0 L1 0 2

3 0 L2 0 3
 Algebraic Solution

 c1  s1 0 a0  c1  s1 0 0

 s ca c1ca 0  sa 0  sa 0 d1   s1 c1 0 0
0
T   1 0

1
 s1sa 0 c1sa 0 ca 0 ca 0 d1   0 0 1 0
   
 0 0 0 1   0 0 0 1
c 2  s 2 0 L1  c 3  s 3 0 L2 
 s c 2 0 0   s c 3 0 0 
1
T   2 2
T   3
2
 0 0 1 0
3
 0 0 1 0
   
 0 0 0 1  0 0 0 1
 Algebraic Solution
The kinematics of the example seen before are:
c123  s123 0 l1c1  l2 c12 
s c123 0 l1s1  l2 s12 
W T  3T 
B 0  123

 0 0 1 0 
 
 0 0 0 1 
Assume goal point is the
specification of wrist frame, c  s 0 x
specified by 3 numbers: s c 0 
y
WT 
B  
0 0 1 0
 
0 0 0 1
 Algebraic Solution
c  c123 s  s123
By comparison, we get
the four equations: x  l1c1  l2 c12
y  l1s1  l2 s12
Summing the square of
the last 2 equations: x  y  l  l  2l1l2c2
2 2
1
2 2
2

From here we get an x  y l l

2 2 2 2

expression for c2
c2  1 2
2l1l2

And finally: s2   1  c2
2   2  A tan 2(s2 , c2 ).
 Geometric Solution
Two-link Planar Manipulator

a2

a1

x
 Geometric Solution

• Applying the “law of cosines”:

• We should try to avoid using the arccos function

because of inaccuracy.
 Geometric Solution
The second joint angle 2is then found accurately as:

Because of the square root, two solutions result:

 Geometric Solution
1 is determined uniquely given 2
1=-y

Note the two different solutions for 1

corresponding to the elbow-down versus elbow-up
configurations
 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.
 Gradient Descent
• We want to find the value of x that causes f(x) to
equal some goal value g
• We will start at some value x0 and keep taking small
steps:
xi+1 = xi + Δx
until we find a value xN that satisfies f(xN)=g
• For each step, we try to choose a value of Δx that will
bring us closer to our goal
• We can use the derivative to approximate the
function nearby, and use this information to move
‘downhill’ towards the goal
 Gradient Descent for f(x)=g

df/dx

f(xi)

f-axis
g xi+1 xi

x-axis
 Minimization
• If f(xi)-g is not 0, the value of f(xi)-g can be thought of
as an error. The goal of gradient descent is to
minimize this error, and so we can refer to it as a
minimization algorithm
• Each step Δx we take results in the function changing
its value. We will call this change Δf.
• Ideally, we could have Δf = g-f(xi). In other words, we
want to take a step Δx that causes Δf to cancel out
the error
• More realistically, we will just hope that each step will
bring us closer, and we can eventually stop when we
get ‘close enough’
• This iterative process involving approximations is
consistent with many numerical algorithms
 Taking Safe Steps

• Sometimes, we are dealing with non-smooth

functions with varying derivatives
• Therefore, our simple linear approximation is not very
reliable for large values of Δx
• There are many approaches to choosing a more
appropriate (smaller) step size
• One simple modification is to add a parameter β to
scale our step (0≤ β ≤1)
1
 df 
x   g  f xi  
 dx 
 Inverse of the Derivative

• By the way, for scalar derivatives:

1
 df  1 dx
   
 dx   df  df
 
 dx 
 Gradient Descent Algorithm

x0  initial starting value

f 0  f  x0  // evaluate f at x0
while  f i  g 

si 
df
xi  // compute slope
dx
 xi    g  f i  // take step along x
1
{ xi 1 }
si
f i 1  f  xi 1  // evaluate f at new xi 1
 Jacobians

• A Jacobian is a vector derivative with respect to

another vector
• If we have a vector valued function of a vector of
variables f(x), the Jacobian is a matrix of partial
derivatives- one partial derivative for each combination
of components of the vectors
• The Jacobian matrix contains all of the information
necessary to relate a change in any component of x to
a change in any component of f
• The Jacobian is usually written as J(f,x), but you can
really just think of it as df/dx
 Jacobians

 f1 f1 f1 

 x ... 
x2 x N
 1 
f f 2
df  2 ... 
J f , x  
...
  x1 x2 
dx 
... ... ... ... 
 f f M 
 M ... ... 
 x1 x N 
Jacobian Inverse Kinematics
 Jacobians

• Let’s say we have a simple 2D robot arm with two

1-DOF rotational joints:

• e=[ex ey]
φ2

φ1
 Jacobians

• The Jacobian matrix J(e,Φ) shows how each

component of e varies with respect to each joint
angle

 ex ex 
  2 
J e, Φ    1 
 e y e y 
 1 2 
 Jacobians

• Consider what would happen if we increased φ1 by a

small amount. What would happen to e ?

e  ex e y  •
 
1  1 1 

φ1
 Jacobians

• What if we increased φ2 by a small amount?

e  ex e y  •
 
2  2 2 
φ2
 Jacobian for a 2D Robot Arm

 ex ex 
  2 
J e, Φ    1 
 e y e y  •
 1 2 
φ2

φ1
 Jacobian Matrices

• Just as a scalar derivative df/dx of a function f(x)

can vary over the domain of possible values for x,
the Jacobian matrix J(e,Φ) varies over the domain
of all possible poses for Φ
• For any given joint pose vector Φ, we can
explicitly compute the individual components of
the Jacobian matrix
 Incremental Change in Pose

• Lets say we have a vector ΔΦ that represents a

small change in joint DOF values
• We can approximate what the resulting change in
e would be:

 Φ  J e, Φ   Φ  J  Φ
de
e 
dΦ
 Incremental Change in Effector

• What if we wanted to move the end effector by a

small amount Δe. What small change ΔΦ will
achieve this?

e  J  Φ
so :
1
Φ  J  e
 Incremental Change in e

• Given some desired incremental change in end effector

configuration Δe, we can compute an appropriate incremental
change in joint DOFs ΔΦ

Δe •

1
φ2
Φ  J  e

φ1
 Incremental Changes

• Remember that forward kinematics is a nonlinear

function (as it involves sin’s and cos’s of the input
variables)
• This implies that we can only use the Jacobian as an
approximation that is valid near the current
configuration
• Therefore, we must repeat the process of computing
a Jacobian and then taking a small step towards the
goal until we get to where we want to be
 Choosing Δe
• We want to choose a value for Δe that will move e closer to g.
A reasonable place to start is with
Δe = g - e

• We would hope then, that the corresponding value of ΔΦ

would bring the end effector exactly to the goal
• Unfortunately, the nonlinearity prevents this from happening,
but it should get us closer
• Also, for safety, we will take smaller steps:

Δe = β(g - e)
where 0≤ β ≤1