inverting the jacobian and manipulability howie choset (with notes copied from rob platt)
TRANSCRIPT
![Page 1: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/1.jpg)
Inverting the Jacobianand Manipulability
Howie Choset
(with notes copied from Rob Platt)
![Page 2: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/2.jpg)
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 WSInverse is “easier” because linear, but this many to one issue comes up
![Page 3: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/3.jpg)
The Inverse Problem
Quick review of linear algebra
![Page 4: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/4.jpg)
Cartesian control
Let’s control the position (not orientation) of the three link arm end effector:
1q
2q
3q
0x
0y
0z1z
1x
1y 2z
2x2y
3z
3y
3x
1l
2l
3l
23323322
2313233221233221
2313233221233221
0 clclcl
sclclclsclclc
sclclclcclcls
J
2
1
q
qJ
y
x
y
xJ
q
q
1
2
1
We can use the same strategy that we used before:
![Page 5: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/5.jpg)
However, this only works if the Jacobian is square and full rank…
Cartesian control
1J
)(qFK
dx
x
q
x q
dq
q
joint ctlr
joint position sensor
1q
2q
3q
0x
0y
0z1z
1x
1y 2z
2x2y
3z
3y
3x
1l
2l
3l
y
xJ
q
q
1
2
1
• All rows/columns are linearly independent, or
• Columns span Cartesian space, or
• Determinant is not zero
![Page 6: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/6.jpg)
What if you want to control the two-dimensional position of a three-link manipulator?
Cartesian control
x0
y0
1q
z0
2q
3q
x1y1
y2
x2
x3
y3
1l
2l3l
1112211123312211
1112211123312211
clclclclclcl
slslslslslslqJ
3
2
1
q
q
q
qJy
x
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.
![Page 7: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/7.jpg)
If the Jacobian is not a square matrix (or is not full rank), then the inverse doesn’t exist…
• what next?
Generalized inverse
x0
y0
1q
z0
2q
3q
x1y1
y2
x2
x3
y3
1l
2l3l
qJx
We are looking for a matrix such that:#J
We have:
xJq # qJx
![Page 8: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/8.jpg)
Two cases:
• Underconstrained manipulator (redundant)
• Overconstrained
Generalized inverse
Generalized inverse:
• for the underconstrained manipulator: given , find any vector s.t.
• for the overconstrained manipulator: given , find any vector s.t. Is minimizedqJx
qx
x q
![Page 9: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/9.jpg)
This condition must be met when is at a minimum subject to
Psuedoinverse definition: (underconstrained)
Given a desired twist, , find a vector of joint velocities, , that satisfies while minimizing
x0
y0
1q
z0
2q
3q
x1y1
y2
x2
x3
y3
1l
2l3lqJxd q
qqqf T )(
dx
Jacobian Pseudoinverse: Redundant manipulator
Use lagrange multiplier method: )()( zgzf zz
Minimize subject to :
)(zf
0)( zg
0)( zg
)(zf
Minimize joint velocities
![Page 10: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/10.jpg)
qqqf T 21)(
0)( xqJqg
)()( zgzf zz
Tq qqf )(
Jqgq )(
Jq TT
TJq
Minimize
Subject to
Jacobian Pseudoinverse: Redundant manipulator
![Page 11: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/11.jpg)
So, the pseudoinverse calculates the vector of joint velocities that satisfies while minimizing the squared magnitude of joint velocity ( ).
• Therefore, the pseudoinverse calculates the least-squares solution.
TJq
TJJqJ
qJJJ T 1
xJJ T 1
I won’t say why, but if is full rank, then is invertible
JTJJ
TJq
xJJJq TT 1
1# TT JJJJ
xJq #
qJxd
qqT
Jacobian Pseudoinverse: Redundant manipulator
![Page 12: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/12.jpg)
Calculating the pseudoinverse
The pseudoinverse can be calculated using two different equations depending upon the number of rows and columns:
1# TT JJJJ Underconstrained case (if there are more
columns than rows (m<n))
TT JJJJ1#
Overconstrained case (if there are more rows than columns (n<m))
1# JJ 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):
![Page 13: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/13.jpg)
Calculating the pseudoinverse using SVD
Singular value decomposition decomposes a matrix as follows:
TVUJ
TUVJ
n
00000
00000
0000
0000
0000
0000
0000
1
1
1
1
#3
2
1
is a diagonal matrix of singular values:
mm nnnm
TUVJ 1#
T
n
VUJ
000000
000000
000000
000000
000000
3
2
1
![Page 14: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/14.jpg)
Properties of the pseudoinverse
JJ ##
## TTJJ
Moore-Penrose conditions:
JJJJ #
### JJJJ
## JJJJT
JJJJT ##
Generalized inverse: satisfies condition 1
Reflexive generalized inverse: satisfies conditions 1 and 2
Pseudoinverse: satisfies all four conditions
1.
2.
3.
4.
Other useful properties of the pseudoinverse:
![Page 15: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/15.jpg)
#J
)(qFK
dx
x
q
x q
dq
q
joint ctlr
joint position sensor
Controlling Cartesian Position
Procedure for controlling position:
1. Calculate position error:
2. Multiply by a scaling factor:
3. Multiply by the velocity Jacobian pseudoinverse:
errx
errerr xx
errv xJq #
![Page 16: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/16.jpg)
Controlling Cartesian Orientation
#J
)(qFK
dR
cR q
q
dq
q
joint ctlr
joint position sensor
How does this strategy work for orientation control?
• Suppose you want to reach an orientation of
• Your current orientation is
• You’ve calculated a difference:
• How do you turn this difference into a desired angular velocity to use in ?
dR
cR
#Jq
dT
ccd RRR
![Page 17: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/17.jpg)
Controlling Cartesian Orientation
#J
)(qFK
dR
cR q
q
dq
q
joint ctlr
joint position sensor
You can’t do this:
• Convert the difference to ZYZ Euler angles:
• Multiply the Euler angles by a scaling factor and pretend that they are an angular velocity: rJq #
r
r
Remember that in general:q
rJ
![Page 18: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/18.jpg)
The Analytical Jacobian
x0
y0
1q
z0
2q
3q
x1y1
y2
x2
x3
y3
1l
2l3l
If you really want to multiply the angular Jacobian by the derivative of an Euler angle, you have to convert to the “analytical” Jacobian:
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” (we’ll talk more about that in a bit…)
qJrTq
rA
J
c
ssc
scs
JrTJ AA
01
0
0
For ZYZ Euler angles
![Page 19: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/19.jpg)
x0
y0
1q
z0
2q
3q
x1y1
y2
x2
x3
y3
1l
2l3l
The easiest way to handle this Cartesian orientation problem is to represent the error in axis-angle format
qJrk
Controlling Cartesian Orientation
Axis angle delta rotation
Procedure for controlling rotation:
1. Represent the rotation error in axis angle format:
2. Multiply by a scaling factor:
3. Multiply by the angular velocity Jacobian pseudoinverse:
errr
errerr rr
errrJq #
![Page 20: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/20.jpg)
Controlling Cartesian Orientation
Why does axis angle work?
• Remember Rodrigues’ formula from before:
cos1sin 2 kSkSIeR kS
k
axis angle
pSp bbb Compare this to the definition of angular velocity:
The solution to this FO diff eqn is: tS
tb b
eR
Therefore, the angular velocity gets integrated into an axis angle representation
![Page 21: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/21.jpg)
Jacobian Transpose Control
qJx The story of Cartesian control so far:
1.
2. xJq #
![Page 22: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/22.jpg)
Jacobian Transpose Control
Here’s another approach:
errT
err xxe 21
q
xx
q
e Terr
T
q
eq
T
Terr q
xxq
err
T
xq
xq
errT
v xJq
Start with a squared position error function (assume the poses are represented as row vectors)
Gradient descent: take steps proportional to in the direction of the negative gradient.
xxx referr Position error:
![Page 23: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/23.jpg)
orientation error: axis angle orientation of reference pose in the current end effector reference frame:
Jacobian Transpose Control
The same approach can be used to control orientation:
refcurrT kJq
refcurrk
![Page 24: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/24.jpg)
Jacobian Transpose Control
So, evidently, this is the gradient of that
• Jacobian transpose control descends a squared error function.
• Gradient descent always follows the steepest gradient
errT
err xxe 21 err
T xJq
![Page 25: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/25.jpg)
Jacobian Transpose v Pseudoinverse
What gives?
• Which is more direct? Jacobian pseudoinverse or transpose?
TJq #Jq or
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)
![Page 26: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/26.jpg)
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
• Manipulator follows a straight line path in Cartesian space
dx
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
dx
Jacobian Transpose v Pseudoinverse
![Page 27: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/27.jpg)
Up until now, we’ve used the Jacobian in the twist equation,
Using the Jacobian for Statics
qJ
Interestingly, you can also use the Jacobian in a statics equation:
wJ T
Cartesian wrench:
m
fw
force
moment (torque)
Joint torques
![Page 28: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/28.jpg)
When J is not invertible: Case 1
minimizes
![Page 29: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/29.jpg)
When is J not invertible: Case 2
![Page 30: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/30.jpg)
Singularities
for most configurations
![Page 31: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/31.jpg)
Singularities
![Page 32: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/32.jpg)
Manipulability
• A measure of the Jacobian
• Configuration dependent
• Maybe it is a measure of how far away from a singularity
• Norms– Scalar– Vector– Matrix
![Page 33: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/33.jpg)
Yes – imagine the possible instantaneous motions are described by an ellipsoid in Cartesian space.
Can we characterize how close we are to a singularity?
Manipulability Ellipsoid
Can’t move much this way
Can move a lot this way
![Page 34: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/34.jpg)
The manipulability ellipsoid is an ellipse in Cartesian space corresponding to the twists that unit joint velocities can generate:
Manipulability Ellipsoid
1qqT
1## xJxJT
A unit sphere in joint velocity space
111
xJJJJJJx TTT
TTT
11
xJJJJJJx TTTTT
11
xJJx TT
Project the sphere into Cartesian space
The space of feasible Cartesian velocities
![Page 35: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/35.jpg)
You can calculate the directions and magnitudes of the principle axes of the ellipsoid by taking the eigenvalues and eigenvectors of
• The lengths of the axes are the square roots of the eigenvalues
Manipulability Ellipsoid
TJJ
1v2v
12
Yoshikawa’s manipulability measure:
• You try to maximize this measure
• Maximized in isotropic configurations
• This measures the volume of the ellipsoid
TJJdet
![Page 36: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/36.jpg)
Another characterization of the manipulability ellipsoid: the ratio of the largest eigenvalue to the smallest eigenvalue:
• Let 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.
Manipulability Ellipsoid
1v2v
12
1 n
n
k1
![Page 37: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/37.jpg)
Manipulability Ellipsoid
Isotropic manipulability ellipsoid
NOT isotropic manipulability ellipsoid
![Page 38: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/38.jpg)
1 T
You can also calculate a manipulability ellipsoid for force:
1FJFJ TTT
1FJJF TT
Force Manipulability Ellipsoid
FJ TA unit sphere in the space of joint torques
The space of feasible Cartesian wrenches
![Page 39: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/39.jpg)
Principle axes of the force manipulability ellipsoid: the eigenvalues and eigenvectors of:
• has the same eigenvectors as :
• But, the eigenvalues of the force and velocity ellipsoids are reciprocals:
• Therefore, the shortest principle axes of the velocity ellipsoid are the longest principle axes of the force ellipsoid and vice versa…
Manipulability Ellipsoid
1TJJ
1TJJ TJJf
ivi vv
vi
fi 1
![Page 40: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/40.jpg)
Velocity and force manipulability are orthogonal!
Velocity ellipsoid
Force 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
![Page 41: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/41.jpg)
Manipulability Ellipsoid: Example
Solve for the principle axes of the manipulability ellipsoid for the planar two link manipulator with unit length links at
12212211
12212211
clclcl
slslslqJ
4
0
q
21
21
21
21
1qJ
0.1568-
0.3029-11v
221
11
21
21
TqJqJ
Principle axes:
1.8411
0.9530-22v
11v
22v
![Page 42: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/42.jpg)
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
![Page 43: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/43.jpg)
Singular Values
• Eigenvalues are for squares (remember the determinant)
• This square matrix has eigenvalues and vectors
![Page 44: Inverting the Jacobian and Manipulability Howie Choset (with notes copied from Rob Platt)](https://reader035.vdocument.in/reader035/viewer/2022081504/56649f185503460f94c2f4bf/html5/thumbnails/44.jpg)
Singular Value Decomposition