Inverting the Jacobian

and Manipulability
Howie Choset
(with notes copied from Rob Platt)

The Inverse Problem

Quick review of linear algebra

Forward Kinematics is in general many to one (if more joint angles than DOF in WS),
non linear
Inverse Kinematics is in general one to many (have a choice), non linear
Forward differential kinematics, linear, many to one, etc. based on # of DOF and WS
Inverse is easier because linear, but this many to one issue comes up

The Inverse Problem

Quick review of linear algebra

Cartesian control
y2

Lets control the position (not orientation) of

the three link arm end effector:

q3

We can use the same strategy that we used

before:

q 1
x
y J q

2

q 1
1 x
q J y

2

z1

l2

x0
y0

x1
z0

l1
q1

y3

l3

z2

y1
q2

s1 l2 c2 l3c23 c1 l2 c2 l3c23 l3c1s23

J c1 l2 c2 l3c23 s1 l2 c2 l3c23 l3c1s23

0
l
c

l
c
l
c
2 2
3 23
3 23

x2

z3

x3

Cartesian control

xd

y2

qd

q3

joint position
sensor

z1

l2

x1
z0

z3

l1
q1

x0

q 1
1 x
q J y

2

y3

l3

z2

y1

joint ctlr
q2

FK (q )

x2

y0

However, this only works if the Jacobian is

square and full rank

All rows/columns are

linearly independent, or

Columns span
Cartesian space, or

Determinant is not zero

x3

Cartesian control
3

What if you want to control the twodimensional position of a three-link

manipulator?

l1s1 l2 s12 l3 s123

J q
l1c1 l2 c12 l3c123
x
y

q 1
J q q 2
q 3

l1s1 l2 s12
l1c1 l2 c12

y
l3

l1s1
l1c1

l2

q3
1

q2

y
q1

Two equations of three

variables each

This is an under-constrained system of equations.

multiple solutions

there are multiple joint angle velocities that realize the

same EFF velocity.

l1

Generalized inverse
3

If the Jacobian is not a square matrix (or is

not full rank), then the inverse doesnt
exist

what next?

y
l3
2

We have:

l2

q3

x Jq

x Jq

q2

y
q1

We are looking for a matrix J # such that:

q J # x

l1

Generalized inverse
Two cases:
Underconstrained manipulator (redundant)
Overconstrained

Generalized inverse:

for the underconstrained manipulator: given x , find any vector q

s.t.

for the overconstrained manipulator: given x , find any vector q

x s.t.
Jq

Is minimized

Jacobian Pseudoinverse: Redundant manipulator

3

Psuedoinverse definition: (underconstrained)

Given a desired twist, x d , find a vector of
joint velocities, q , that satisfies x d Jq
while minimizing f ( q ) q T q
Minimize joint velocities

y
l3
2

l2

q3
1

q2

y
q1

Minimize f (z ) subject to g ( z ) 0 :
Use lagrange multiplier method: z f ( z ) z g ( z )

This condition must be met when f (z ) is at a minimum

subject to g ( z ) 0

l1

Jacobian Pseudoinverse: Redundant manipulator

z f ( z ) z g ( z )
f (q ) 12 q T q
g (q ) Jq x 0

q f (q ) q T
q g (q ) J
q T T J
q J T

Minimize
Subject to

Jacobian Pseudoinverse: Redundant manipulator

q J T

Jq JJ T

JJ
JJ

T 1
T 1

q J T

q J JJ
T

I wont say why, but if J is full rank, then

JJ T is invertible

T 1

J J JJ
q J # x
#

Jq

T 1

So, the pseudoinverse calculates the

vector of joint velocities that
satisfies x d Jq while
minimizing the squared magnitude
of joint velocity ( q T q ).

Therefore, the pseudoinverse

calculates the least-squares
solution.

Calculating the pseudoinverse

The pseudoinverse can be calculated using two different
equations depending upon the number of rows and columns:

J # J T JJ T

Underconstrained case (if there are more

columns than rows (m<n))
1 T
#
T
Overconstrained case (if there are more rows
J J J J
than columns (n<m))
If there are an equal number of rows and columns (n=m)
J # J 1

These equations can only be used if the Jacobian is full rank;

otherwise, use singular value decomposition (SVD):

Calculating the pseudoinverse using SVD

Singular value decomposition decomposes a matrix as follows:

J UV

m m m n

n n

is a diagonal matrix of singular values:

1 0
0
2

J U 0

0
0

0
0
1
1

J# V 0
0

0
0

0
0

0
0

3 0 0
0 0
0 0 n

J # V 1U T

0
0

1
2

1
3

0
0

0
0

0 0
0 0
0 0 V T

0 0
0 0

0
0
0

0 UT
1
n

0
0

Properties of the pseudoinverse

Moore-Penrose conditions:

1. J # JJ # J #
2. JJ # J J

4. J J
3. JJ

# T

JJ #

J #J

Generalized inverse: satisfies condition 1

Reflexive generalized inverse: satisfies conditions 1 and 2
Pseudoinverse: satisfies all four conditions
Other useful properties of the pseudoinverse:

J J
J J
# #
# T

T #

Controlling Cartesian Position

xd

J#

FK (q )

qd

joint ctlr

joint position
sensor

Procedure for controlling position:

1. Calculate position error: xerr
2. Multiply by a scaling factor:

xerr xerr

3. Multiply by the velocity Jacobian pseudoinverse: q J v #xerr

Controlling Cartesian Orientation

How does this strategy work for orientation control?
Suppose you want to reach an orientation of
Your current orientation is

Rd

Rc

Youve calculated a difference:

Rcd Rc Rd

How do you turn this difference into a desired

#
angular velocity to use in q J ?

Rd

Rc

FK (q )

qd

joint ctlr

joint position
sensor

Controlling Cartesian Orientation

You cant do this:

Convert the difference to ZYZ Euler angles: r

Multiply the Euler angles by a scaling factor and

pretend that they are an angular velocity: q J # r

Remember that in general: J

r
q

Rd

Rc

FK (q )

qd

joint ctlr

joint position
sensor

The Analytical Jacobian

If you really want to multiply the angular

Jacobian by the derivative of an Euler
angle, you have to convert to the
analytical Jacobian:

r
q

TA r J q

0 s
J A TA r J 0 c
1
0

y
l3
2

l2

q3
1

q2

y
q1

c s
s s J
c

For ZYZ Euler

angles

Gimbal lock: by using an analytical Jacobian instead of the angular

velocity Jacobian, you introduce the gimbal lock problems we
talked about earlier into the Jacobian this essentially adds
singularities (well talk more about that in a bit)

l1

Controlling Cartesian Orientation

3

The easiest way to handle this Cartesian

orientation problem is to represent the
error in axis-angle format

y
l3
2

l2

q3
1

rk J q
Axis angle delta
rotation

1. Represent the rotation error in axis angle format: rerr

2. Multiply by a scaling factor:

rerr rerr

3. Multiply by the angular velocity Jacobian

pseudoinverse: q J #rerr

q2

y
q1

Procedure for controlling rotation:

l1

Controlling Cartesian Orientation

Why does axis angle work?

Remember Rodrigues formula from before:

Rk e S k I S k sin S k 1 cos
2

axis

angle

Compare this to the definition of angular velocity: b

The solution to this FO diff eqn is:

p S b b p

Rt e

S b t

Therefore, the angular velocity gets integrated into an

axis angle representation

Jacobian Transpose Control

The story of Cartesian control so far:

x Jq
#

J
x
2.
1.

Jacobian Transpose Control

Heres another approach:
T

e 12 xerr xerr

e
T x
xerr
q
q

Position error:

x
q xerr T

x
q
q

xerr xref x

Start with a squared position error

function (assume the poses are
represented as row vectors)

xerr

T
q J v xerr

Gradient descent: take steps

proportional to in the
direction of the negative
gradient.

Jacobian Transpose Control

The same approach can be used to control orientation:
T
q J

curr

k ref

orientation error: axis angle orientation of reference pose in

the current end effector reference frame: curr k ref

Jacobian Transpose Control

So, evidently, this is the gradient of that

q J

xerr

e 12 xerr xerr

Jacobian transpose control descends a squared

error function.
Gradient descent always follows the steepest
gradient

Jacobian Transpose v Pseudoinverse

What gives?

Which is more direct? Jacobian pseudoinverse or

transpose?

q J T

or

q J #

They do different things:

Transpose: move toward a reference pose as quickly as
possible

One dimensional goal (squared distance meteric)

Pseudoinverse: move along a least squares reference twist

trajectory

Six dimensional goal (or whatever the dimension of the

relevant twist is)

Jacobian Transpose v Pseudoinverse

The pseudoinverse moves the end effector in
a straight line path toward the goal pose
using the least squared joint velocities.

The goal is specified in terms of the

reference twist

xd

Manipulator follows a straight line path in

Cartesian space

The transpose moves the end effector toward

the goal position
In general, not a straight line path in
Cartesian space
Instead, the transpose follows the gradient
in joint space

xd

Using the Jacobian for Statics

Up until now, weve used the Jacobian in the twist equation,

Jq

Interestingly, you can also use the Jacobian in a statics

equation:

JTw
Joint torques

Cartesian
wrench:

f
w
m

force
moment (torque)

When J is not invertible: Case 1

minimizes

When is J not invertible: Case 2

Singularities

for most configurations

Singularities

Manipulability
A measure of the Jacobian
Configuration dependent
Maybe it is a measure of how far away
from a singularity
Norms
Scalar
Vector
Matrix

Manipulability Ellipsoid
Can we characterize how close we are to a singularity?
Yes imagine the possible instantaneous motions are described by
an ellipsoid in Cartesian space.
Cant move much this way
Can move a lot this way

Manipulability Ellipsoid
The manipulability ellipsoid is an ellipse in
Cartesian space corresponding to the twists
that unit joint velocities can generate:

q T q 1

J x
T

A unit sphere in joint velocity space

J # x 1

x J JJ

JJ

x T JJ T
x T

Project the sphere into

Cartesian space

JJ x 1
JJ x 1

T
T 1

T 1

JJ T

x 1

T 1

T 1

The space of feasible

Cartesian velocities

Manipulability Ellipsoid
You can calculate the directions and magnitudes
of the principle axes of the ellipsoid by taking
the eigenvalues and eigenvectors of JJ T

The lengths of the axes are the square roots

of the eigenvalues

v1

Yoshikawas manipulability measure:

v2

det JJ T

You try to maximize this measure

Maximized in isotropic configurations

This measures the volume of the ellipsoid

Manipulability Ellipsoid
Another characterization of the
manipulability ellipsoid: the ratio of the
largest eigenvalue to the smallest
eigenvalue:

Let 1 be the largest eigenvalue and let

be the smallest.

Then the condition number of the

ellipsoid is:

The closer to one the condition number,

the more isotropic the ellispoid is.

v1

v2

Manipulability Ellipsoid

Isotropic manipulability
ellipsoid

NOT isotropic
manipulability ellipsoid

Force Manipulability Ellipsoid

You can also calculate a manipulability ellipsoid
for force:

T 1

A unit sphere in the space of joint torques

JTF

J F
T

JTF 1

F T JJ T F 1

The space of feasible Cartesian wrenches

Manipulability Ellipsoid
Principle axes of the force manipulability ellipsoid: the
T 1
eigenvalues and eigenvectors of:

JJ

JJ

T 1

has the same eigenvectors as JJ T : vi vi

v

But, the eigenvalues of the force and velocity ellipsoids

are reciprocals:
1
f

vi

Therefore, the shortest principle axes of the velocity

ellipsoid are the longest principle axes of the force
ellipsoid and vice versa

Velocity and force manipulability are orthogonal!

Force ellipsoid

Velocity ellipsoid

This is known as force/velocity duality

You can apply the largest forces in the same

directions that your max velocity is smallest

Your max velocity is greatest in the directions where

you can only apply the smallest forces

Manipulability Ellipsoid: Example

Solve for the principle axes of the manipulability
ellipsoid for the planar two link manipulator with unit
length links at
0
q
4
l1s1 l2 s12 l2 s12
J q

l
c

l
c
l
c
2 12
1 1 2 12

12
J q
1
1

1
2
1
2

J q J q
T

1
2

Principle axes:

1
2

2 2
- 0.3029

- 0.1568
- 0.9530

2 v2
1.8411

1 v1

2 v2
1 v1

Eigenvalues and Vectors

Eigenvectors are the directions that a
linear mapping just scales a vector
Ax = sx where A is a matrix, s is a scalar
Det (Ax) = Det (sx)

Eigenvectors
Ellipsoid analogy

Singular Values
Eigenvalues are for squares (remember
the determinant)

This square matrix has eigenvalues and

vectors

Singular Value Decomposition