Autorob 11 Ik Jacobian

autorob.github.
io
Inverse Kinematics:
Manipulator Jacobian
UM EECS 398/598 - autorob.github.io
Conservatively tuned IK
Aggressively tuned IK
How did I do it?

Jacobian Transpose
Target
endeffector
location
Current
endeffector
location
Endeffector
error
Can we move the

endeffector to
minimize error?
Desired
endeffector
displacement
Can we move the
endeffector to
minimize error?
Joint angle
displacements
Yes.
Convert linear velocity of
endeffector to angular
velocities at joints.
Velocity of Point Rotating in Fixed Frame

v1
r2
rotation axis
angular rotation in frame
r1
v2
r3
angular velocity
of points in frame
wrt. axis k
v3
k axis out-of-plane
and passes through frame origin

v1
r2
linear velocity
of points in frame
wrt. axis k
vector to point
in frame
rotation axis
r1
v2
r3
angular velocity
of points in frame
wrt. axis k
v3
k axis out-of-plane
Velocity of Point Rotating in Joint Frame

angular velocity
of points in frame
wrt. axis k
linear velocity
of points in frame
wrt. axis k
vector to point
in frame
endeffector
linear velocity
rotation axis
vector from
joint origin to
endeffector
joint rotation axis
Velocity of Point Rotating in Joint Frame

vector from
joint origin to
endeffector
endeffector
linear velocity
joint rotation axis
This is not what we wanted.

Why?
Jacobian Transpose
vector from
joint origin to
endeffector
endeffector
linear velocity
joint rotation axis

How to obtain joint angular
velocity from endeffector
linear velocity?
Jacobian Transpose
vector from
joint origin to
endeffector
endeffector
linear velocity
joint rotation axis

How to obtain joint angular
velocity from endeffector
linear velocity?
joint rotation axis
Jacobian Transpose
Angular
velocity
for joint i
endeffector
linear velocity
vector from
joint origin to
endeffector
Jacobian
for joint i
Procedure (for each joint):

1) Compute Jacobian
2) Update joint angles using Jacobian transpose
3) Repeat forever (or until error minimized)
Inverse kinematics as error minimization

Define error function e(q) as difference
between current and desired endeffector
poses
e(q)
Error function parameterized by robot

configuration q
Find global minimum of e(q),
ie. argminq e(q)
Example: cos(3x)/x, 0.1 x 1.1

poses
e(q)

configuration q
ie. argminq e(q)
How could we find argminq e(q) if we knew
e(q) in closed form?
Find global minimum of function

Take derivative
Solve for x where derivative is zero
Verify
Find global minimum of function

Take derivative
Solve for x where derivative is zero
Verify

poses
e(q)

configuration q
ie. argminq e(q)
But, do we know e(q) in closed form?
Linearization of nonlinear function

rate of change of f(x) at x0 is J(x0)
function f(x) to approximate
x0-x x0 x0+x
as first order expansion, approximate f(x) at
current configuration x0 and tangent plane J(x0)
Iteration will lead to local

minimization function F
Derivative assumed to be direction

of steepest ascent away from goal
f(x)

x
Minimum (Goal)

Start (Initial)
Minimum (Goal)
What is the derivative

of robot configuration
wrt. endeffector error?
Start (Initial)
Geometric
3D N-link arm
The Jacobian
A 6xN matrix
assuming forward kinematics:
represents partial derivative:

Each column transforms
velocity at the endeffector
to velocity at a DOF
Jacobian for Planar 2-link Arm
Endeffector
velocity
Configuration
velocity
i-1th frame maps to ith column

3D N-link arm
The Jacobian
A 6xN matrix
i=n is endeffector frame

i=0 is base frame
Note: figure taken from Spong et al. textbook, which
assumes D-H parameters and offset column index
3D N-link arm
The Jacobian
A 6xN matrix
consisting of two 3xN matrices
with overall form
3D N-link arm
The Jacobian
A 6xN matrix
with overall form

Change in endeffector
variable with respect to
change in joint variable
3D N-link arm
The Jacobian
A 6xN matrix

linear
angular
3D N-link arm
The Jacobian
A 6xN matrix

linear
angular
3D N-link arm
The Jacobian
A 6xN matrix

linear
angular
linear velocity of endeffector

angular velocity of endeffector
vector of
of joint
angle
velocities
The Jacobian
3D N-link arm
A 6xN matrix

linear
angular
Ji is a single column
of the Jacobian matrix
Z is joint axis in
world coordinates
(overloaded notation)
Ji for a prismatic joint
Ji for a rotational joint
vectors in
base frame
zi-1: joint axis
The Jacobian
A 6xN matrix
oi-1: joint
origin
linear
on: endeffector
angular
zi-1: joint axis (Jwi)

Jvi
The Jacobian
A 6xN matrix
linear
angular
zi-1: joint axis (Jwi)
The Jacobian
A 6xN matrix
linear
angular

v1
r2
angular velocity
of points in frame
wrt. axis k
r1
v2
linear velocity
of points in frame
wrt. axis k
r3
rotation axis
v3
k axis out-of-plane
vector to point
in frame
skew-symmetric
matrix

Note: velocity dependent on frame, not
necessarily linear velocity at point
v1
r2
angular velocity
of points in frame
wrt. axis k
r1
v2
linear velocity
of points in frame
wrt. axis k
r3
rotation axis
v3
k axis out-of-plane
vector to point
in frame
skew-symmetric
matrix
derivative of frame rotation

Velocity of Point Rotating in Moving Frame
Velocity of Point Rotating in Moving Frame

transform to
base to frame 1
Angular Velocity
Linear Velocity
p1 is point in
frame 1
linear velocity of frame

plus linear rotation of point
Velocity of Point Rotating on N-link Arm

Angular Velocity
assuming velocities expressed in the same frame
consider effect of all frames

(0..n) on endeffector
Linear Velocity for Rotational Joint
position of endeffector frame

take derivative wrt.

ith joint angle
Geometric
The Jacobian
A 6xN matrix

Procedure restated:
Geometric
The Jacobian
A 6xN matrix
compute
endpoint
error

Procedure restated:
compute step
direction

repeat
perform step
direction
Geometric
The Jacobian
A 6xN matrix

can we invert J(q)?
The Jacobian
A 6xN matrix

when can we invert J(q)?
Jacobian Transpose
Jacobian Transpose
Error Minimization by Jacobian Transpose

Jacobian gives mapping from
configuration displacement to
endeffector displacement
Inverse of Jacobian maps
endeffector displacement to
configuration displacement
But, inverse of Jacobian is
rarely an option. Why?
Instead, find configuration
displacement that minimizes
endeffector error squared

Instead, find configuration
displacement that minimizes
endeffector error squared
Define cost function

expressing squared error

Take cost derivative
Evaluate for configuration convergence
Matlab example:
Jacobian transpose
Initial configuration
Iterations to goal
Pseudo Inverse
Matlab example:
Pseudo Inverse
Initial configuration
Iterations to goal
Pseudo Inverse,
More Generally
Pseudoinverse of matrix A: A+=(ATA)1AT approximates solution

to linear system Ax=b
The pseudoinverse A+ is a least squares best fit approximate

solution of an overdetermined system Ax=b, where there are
more equations (m) than unknowns (n), or vice versa
Often used for data fitting, as a singular value decomposition
http://lion.cs.uiuc.edu/projects/wmn.html
Error Minimization by Jacobian Pseudoinverse

Take cost derivative
Set to zero and solve for configuration displacement
Normal form
IK Demo
Which Pseudoinverse
For matrix A with dimensions N x M with full rank
Left pseudoinverse, for when N > M, (i.e., less than than 6 DoFs)
s.t.
Right pseudoinverse, for when N < M, (i.e., more than 6 DoFs)

s.t.
Which Pseudoinverse
For matrix A with dimensions N x M with full rank

What happens when
J is not full rank?
Left pseudoinverse, for when N > M, (i.e., less than than 6 DoFs)
s.t.
Right pseudoinverse, for when N < M, (i.e., more than 6 DoFs)

s.t.
Singularities
Determinant of this matrix?

Pseudoinverse by Singular Value Decomposition

NxM matrix
Left singular vectors

NxN matrix
Columns are orthonormal
eigenvectors of AAT
Right singular vectors

MxM matrix
Columns are orthonormal
eigenvectors of ATA
Singular values
Diagonal NxM matrix
eigenvalues of ATA and AAT
Diagonal entries are

reciprocals of diagonal of D
Maybe there is a simpler

approach to IK?
Cyclic Coordinate Descent
http://www.ryanjuckett.com/programming/cyclic-coordinate-descent-in-2d/
Initial
Rotate bone 3
(to place end effector as
close to target as possible)
Rotate bone 2 to place end
Rotate bone 1 to place end
effector as close to target as possible.
effector as close to target as possible.
Repeat until convergence

http://www.ryanjuckett.com/programming/cyclic-coordinate-descent-in-2d/
https://www.youtube.com/watch?v=MvuO9ZHGr6k
Pros and Cons
+ Fast to compute and simple to implement
- Smoothness over time not considered
Jacobian-based methods
+ General transform of velocities and wrenches between frames
- Slower and subject to numerical issues and local minima

IK Assignment
EECS 398-002
IK for mr2 robot with endeffector position
Jacobian transpose and Jacobian pseudoinverse
EECS 598-010
IK for Fetch robot with endeffector position and orientation
Jacobian transpose, Jacobian pseudoinverse, Cyclic Coordinate Descent
Euler angle conversion for endeffector pose orientation

IK assignment details next class
IK Assignment
Specify target location, endeffector frame, endeffector point in frame
home.html
// if requested, perform inverse kinematics control to reach to point
kineval.robotInverseKinematics(kineval.params.ik_target, robot.endeffector.frame, robot.endeffector.position);
kineval_inverse_kinematics.js
kineval.robotInverseKinematics(endeffector_target_world, endeffector_joint, endeffector_position_local) {
. . .
kineval.iterateIK(endeffector_target_world, endeffector_joint, endeffector_position_local);
if (kineval.params.trial_ik_random.execute)
kineval.randomizeIKtrial();
else
kineval.params.trial_ik_random.start = new Date();
}
}
kineval.iterateIK(endeffector_target_world, endeffector_joint, endeffector_position_local) {
// Your code for a single IK iteration
// output sets updated controls for each joint
}
world
IK Assignment
global
Specify target location, endeffector frame,

endeffector point in frame
Form kinematic chain

Transform endeffector into world
Endeffector specified in frame

IK Assignment

Iterate over kinematic chain to update robot.controls
Build Jacobian and compute Jacobian Transpose and Psuedoinverse
Apply updated controls to each joint
Matrix inversion provided by numericjs
numeric.inv(J)
IK Assignment

Iterate over kinematic chain to update robot.controls
Build Jacobian and compute Jacobian Transpose and Psuedoinverse
Apply updated controls to each joint
Ensure your matrix routines work with kineval.js
e.g., generate_translation_matrix takes 3 parameters
Next class:
Motion Planning
http://people.csail.mit.edu/aperez/www/files/rrt_timelapse.jpg

Autorob 11 Ik Jacobian

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Autorob 11 Ik Jacobian

Uploaded by

Copyright:

Available Formats

autorob.github.

UM EECS 398/598 - autorob.github.io

UM EECS 398/598 - autorob.github.io

UM EECS 398/598 - autorob.github.io

UM EECS 398/598 - autorob.github.io

How did I do it?

UM EECS 398/598 - autorob.github.io

Can we move the

UM EECS 398/598 - autorob.github.io

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Fixed Frame

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Fixed Frame

angular rotation in frame

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Joint Frame

angular rotation in frame

Velocity of Point Rotating in Joint Frame

joint rotation axis

This is not what we wanted.

UM EECS 398/598 - autorob.github.io

joint rotation axis

This is not what we wanted.

UM EECS 398/598 - autorob.github.io

joint rotation axis

This is not what we wanted.

UM EECS 398/598 - autorob.github.io

joint rotation axis

Procedure (for each joint):

UM EECS 398/598 - autorob.github.io

Inverse kinematics as error minimization

Error function parameterized by robot

UM EECS 398/598 - autorob.github.io

Example: cos(3x)/x, 0.1 x 1.1

UM EECS 398/598 - autorob.github.io

Inverse kinematics as error minimization

Error function parameterized by robot

Find global minimum of function

Solve for x where derivative is zero

Find global minimum of function

Solve for x where derivative is zero

Inverse kinematics as error minimization

Error function parameterized by robot

Linearization of nonlinear function

function f(x) to approximate

Iteration will lead to local

UM EECS 398/598 - autorob.github.io

Derivative assumed to be direction

UM EECS 398/598 - autorob.github.io

Derivative assumed to be direction

Derivative assumed to be direction

What is the derivative

assuming forward kinematics:

represents partial derivative:

Jacobian for Planar 2-link Arm

UM EECS 398/598 - autorob.github.io

i-1th frame maps to ith column

i=n is endeffector frame

consisting of two 3xN matrices

with overall form

UM EECS 398/598 - autorob.github.io

consisting of two 3xN matrices

with overall form

consisting of two 3xN matrices