# Inverting the Jacobian and Manipulability

Document Sample

```					 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               x2
Let’s control the position (not orientation) of                                                 q3
y3
the three link arm end effector:                                             y1         z2                  l3

l2         z3
q2                                             x3
x1

  s1 l2 c2  l3c23   c1 l2 c2  l3c23   l3c1s23         z1             l1
                                                                         z0
J   c1 l2 c2  l3c23   s1 l2 c2  l3c23   l3c1s23 
q1
x0
                          l2 c2  l3c23        l3c23 
           0                                                   y0

We can use the same strategy that we used
before:

 x
        q1 
                       q1 
     1  x 

 y   J q                      q   J  y 
        2                     2       
Cartesian control

q                                                        y2               x2
y3
q3
                          
                  x                 q        qd     joint ctlr                    y1         z2                  l3
xd                        J   1

l2         z3
q2                                             x3
x1
                              joint position
x                      q             sensor                z1
FK (q )                                                           l1
z0
q1
x0
 q1 
     1  x 

q   J  y 
y0

 2       
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
Cartesian control
3
x
What if you want to control the two-
dimensional position of a three-link                      3
y
manipulator?                                                                               l3
q3                                l2
2
 l1s1  l2 s12  l3 s123  l1s1  l2 s12    l1s1            x                                                  1
x
J q   
1
y
l1c1 
q2
 l1c1  l2 c12  l3c123    l1c1  l2 c12           
2
y           0
y
 q1 
                                                                                         q1
 x

 J q q2 
0
Two equations of three                                                              x       l1
 y           
 
0
variables each…                                     z
           q3 
 
This is an under-constrained system of equations.
• multiple solutions
• there are multiple joint angle velocities that realize the
same EFF velocity.
Generalized inverse
3
x
If the Jacobian is not a square matrix (or is
not full rank), then the inverse doesn’t    3
y
exist…                                                                       l3
q3                                l2
• what next?                                            2
x                             1
1
x
y
q2

2
y           0
y
We have:      x  Jq
                                                                                  q1
0
x       l1
0
We are looking for a matrix J # such that:                                   z

q  J #x
                 x  Jq
    
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
                   
s.t.   x  Jq
       Is minimized
Jacobian Pseudoinverse: Redundant manipulator
3
x
Psuedoinverse definition: (underconstrained)
3

Given a desired twist, xd , find a vector of            y
joint velocities, q , that satisfies xd  Jq
                                                             l3
l2
while minimizing f (q )  q T q
q3
                              2
x                             1
1
x
y
q2

Minimize joint velocities                              2
y           0
y
Minimize f (z ) subject to g ( z )  0 :
q1
0
x       l1
0
z
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
Jacobian Pseudoinverse: Redundant manipulator
 z f ( z)   z g ( z)

f ( q)  1 q T q
 2                  Minimize

g (q)  Jq  x  0
                        Subject to

 q f (q)  qT
        

 q g ( q)  J
     

q T  T J

q  JT

Jacobian Pseudoinverse: Redundant manipulator
q  JT

 
Jq  JJ T 


  JJ  Jq
T 1
              I won’t say why, but if J is full rank, then
JJ T is invertible
  JJ  x
T 1

q  JT

q  J JJ
      T
  x  T 1

So, the pseudoinverse calculates the
vector of joint velocities that
 J JJ 
#         T      T 1      satisfies xd  Jq while
      
J                           minimizing the squared magnitude
q  J #x
                          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
1
Underconstrained case (if there are more
columns than rows (m<n))
 J J  J
#      T   1    T
J                         Overconstrained case (if there are more rows
than columns (n<m))
J #  J 1          If there are an equal number of rows and columns (n=m)

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:
   is a diagonal matrix of singular values:
J  UV    T

 1 0             0         0       0   0 0
0                0         0       0   0 0
     2                                     
m m m n           n n    J U 0 0              3        0 0         0 0V T
                                           
0 0               0          0         0 0
0 0
                  0         0 n        0 0


 11   0         0       0
0
0      1
0    0 0
       2                   
J #  V 1U T                   0      0         1
3   0 0
                            
J# V 0      0         0     0 U T
0      0         0    0 1n 
                            
0      0         0    0 0
0                     0 0
       0         0          
Properties of the pseudoinverse
Moore-Penrose conditions:

1. J # JJ #  J #
2. JJ # J  J
 
3. JJ #
T
 JJ #
4. J J 
T
#
 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

q

                       
               x              q        qd     joint ctlr
xd                     J   #


                           joint position
x                   q             sensor
FK (q )

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 #x
    v   err
Controlling Cartesian Orientation
How does this strategy work for orientation control?
• Suppose you want to reach an orientation of Rd
• Your current orientation is Rc
• You’ve calculated a difference: R  R T R
cd  c   d
• How do you turn this difference into a desired
angular velocity to use in q  J # ?



q

        
              q        qd       joint ctlr
Rd                       J   #


Rc                   
       q       joint position
FK (q )                   sensor
Controlling Cartesian Orientation
You can’t 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

r
Remember that in general: J  
q

q

r                        
q         qd       joint ctlr
Rd                         J   #


Rc                      
       q        joint position
FK (q )                    sensor
The Analytical Jacobian                         3
x

3
If you really want to multiply the angular             y
l3
Jacobian by the derivative of an Euler                         q3                                l2
2
angle, you have to convert to the                          x                             1
1
x
y
“analytical” Jacobian:                                                                                q2

r
 TA r J  q
2
y           0
y

q                                                                                             q1
0
x       l1

0  s   c s                                 0
z
J A  TA r J   0 c
         s s  J 
                       For ZYZ Euler
1
   0      c                            angles

Gimbal lock: by using an analytical Jacobian instead of the angular
velocity Jacobian, you introduce the gimbal lock problems we
“singularities” (we’ll talk more about that in a bit…)
Controlling Cartesian Orientation
3
x

The easiest way to handle this Cartesian            3
y
orientation problem is to represent the                                            l3
error in axis-angle format                                2
q3                                l2
x                             1
1
x
y
rk  J  q
                                                                                 q2

2
y           0
y
Axis angle delta
rotation                                                                     q1
0
x       l1
0
z
Procedure for controlling 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 #r
            err
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 p            
S  p    b   b

The solution to this FO diff eqn is:
b
Rt  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:
1. x  Jq
     
q  J #x
2.        
Jacobian Transpose Control

function (assume the poses are
represented as row vectors)
e  1 xerr xerr
T
2

e
q
 
  xerr
T x

q
Position error:   xerr  xref  x
T
 e 
q    
        q               Gradient descent: take steps
                    proportional to  in the

 
T
      T x 
direction of the negative
q    xerr

q 
           
x
T
q 
               xerr 
q
q  J v xerr 
T

Jacobian Transpose Control

The same approach can be used to control orientation:

q  J
       T
   curr
kref   
orientation error: axis angle orientation of reference pose in
the current end effector reference frame: currk ref
Jacobian Transpose Control

So, evidently, this is the gradient of that

q  J xerr              e  1 xerr xerr
T                             T
                             2

• Jacobian transpose control descends a squared
error function.
• Gradient descent always follows the steepest
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
xd
in joint space
Using the Jacobian for Statics

Up until now, we’ve used the Jacobian in the twist equation,      Jq


Interestingly, you can also use the Jacobian in a statics
equation:

  JTw

Joint torques        Cartesian         f            force
wrench:       w 
 m
             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.

Can’t 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:

qT q  1
                          A unit sphere in joint velocity space

J x J
  #   T     #
x 1
                Project the sphere into
Cartesian space

 T
x J JJ     J JJ  x  1
T         T 1
T
    T          T 1

xT
     JJ  JJ JJ  x  1
T T
  T         T 1

xT
     JJ  x  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
1             v2
2

Yoshikawa’s manipulability measure:               
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        v1
1        v2
largest eigenvalue to the smallest
eigenvalue:                                                 2
• Let 1 be the largest eigenvalue and let     n
be the smallest.
• Then the condition number of the
ellipsoid is:
1
k
n

• The closer to one the condition number,
the more isotropic the ellispoid is.
Manipulability Ellipsoid

Isotropic manipulability     NOT isotropic
ellipsoid                   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  J
T   T    T
F 1

F T JJ T F  1            The space of feasible Cartesian wrenches
Manipulability Ellipsoid
Principle axes of the force manipulability ellipsoid: the
eigenvalues and eigenvectors of:
JJ 
T 1

•   JJ 
T 1   has the same eigenvectors as JJ T : vi  vi
v       f

• But, the eigenvalues of the force and velocity ellipsoids
are reciprocals:         1
if 
v
i

• 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    2 v2
q  
                                      1 v1
4
 l1s1  l2 s12  l2 s12 
J q   
 l1c1  l2 c12   l2 c12 
  12        1

J q                  2

1  12       1
2    
 1              1   1

J q J q    
T

2

 1  2

1
2  2  

 - 0.3029 
Principle axes:             1 v1  
         

 - 0.1568 
 - 0.9530 
2 v2  
 1.8411  
          
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

```
DOCUMENT INFO
Shared By:
Categories:
Tags:
Stats:
 views: 100 posted: 11/16/2011 language: English pages: 44
How are you planning on using Docstoc?