You are on page 1of 79

autorob.github.

io

Inverse Kinematics:
Manipulator Jacobian

UM EECS 398/598 - autorob.github.io

UM EECS 398/598 - autorob.github.io

Conservatively tuned IK

UM EECS 398/598 - autorob.github.io

Aggressively tuned IK

UM EECS 398/598 - autorob.github.io

How did I do it?


Jacobian Transpose
UM EECS 398/598 - autorob.github.io

Target
endeffector
location

Current
endeffector
location

UM EECS 398/598 - autorob.github.io

Endeffector
error

Can we move the


endeffector to
minimize error?

UM EECS 398/598 - autorob.github.io

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.

UM EECS 398/598 - autorob.github.io

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

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Fixed Frame


v1
r2

angular rotation in frame

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
and passes through frame origin

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Joint Frame


angular velocity
of points in frame
wrt. axis k

angular rotation in frame

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
UM EECS 398/598 - autorob.github.io

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?

UM EECS 398/598 - autorob.github.io

Jacobian Transpose
vector from
joint origin to
endeffector
endeffector
linear velocity

joint rotation axis

This is not what we wanted.


How to obtain joint angular
velocity from endeffector
linear velocity?

UM EECS 398/598 - autorob.github.io

Jacobian Transpose
vector from
joint origin to
endeffector
endeffector
linear velocity

joint rotation axis

This is not what we wanted.


How to obtain joint angular
velocity from endeffector
linear velocity?

UM EECS 398/598 - autorob.github.io

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)

UM EECS 398/598 - autorob.github.io

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)

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


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)
How could we find argminq e(q) if we knew
e(q) in closed form?
UM EECS 398/598 - autorob.github.io

Find global minimum of function


Take derivative

Solve for x where derivative is zero

Verify
UM EECS 398/598 - autorob.github.io

Find global minimum of function


Take derivative

Solve for x where derivative is zero

Verify
UM EECS 398/598 - autorob.github.io

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)
But, do we know e(q) in closed form?
UM EECS 398/598 - autorob.github.io

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)
UM EECS 398/598 - autorob.github.io

Iteration will lead to local


minimization function F

UM EECS 398/598 - autorob.github.io

Derivative assumed to be direction


of steepest ascent away from goal

UM EECS 398/598 - autorob.github.io

f(x)

Derivative assumed to be direction


of steepest ascent away from goal

x
UM EECS 398/598 - autorob.github.io

Minimum (Goal)

Derivative assumed to be direction


of steepest ascent away from goal

Start (Initial)
UM EECS 398/598 - autorob.github.io

Minimum (Goal)

What is the derivative


of robot configuration
wrt. endeffector error?

Start (Initial)
UM EECS 398/598 - autorob.github.io

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
UM EECS 398/598 - autorob.github.io

Jacobian for Planar 2-link Arm

Endeffector
velocity

Configuration
velocity

UM EECS 398/598 - autorob.github.io

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
UM EECS 398/598 - autorob.github.io

3D N-link arm

The Jacobian
A 6xN matrix

consisting of two 3xN matrices

with overall form

UM EECS 398/598 - autorob.github.io

3D N-link arm

The Jacobian
A 6xN matrix

consisting of two 3xN matrices

with overall form


Change in endeffector
variable with respect to
change in joint variable
UM EECS 398/598 - autorob.github.io

3D N-link arm

The Jacobian
A 6xN matrix

consisting of two 3xN matrices


linear
angular

UM EECS 398/598 - autorob.github.io

3D N-link arm

The Jacobian
A 6xN matrix

consisting of two 3xN matrices


linear
angular

UM EECS 398/598 - autorob.github.io

3D N-link arm

The Jacobian
A 6xN matrix

consisting of two 3xN matrices


linear
angular

linear velocity of endeffector


angular velocity of endeffector

vector of
of joint
angle
velocities
UM EECS 398/598 - autorob.github.io

The Jacobian

3D N-link arm

A 6xN matrix

consisting of two 3xN matrices


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

UM EECS 398/598 - autorob.github.io

vectors in
base frame

zi-1: joint axis

The Jacobian
A 6xN matrix

oi-1: joint
origin

linear

on: endeffector

angular

Ji for a rotational joint

Ji for a prismatic joint

UM EECS 398/598 - autorob.github.io

zi-1: joint axis (Jwi)


Jvi

The Jacobian
A 6xN matrix
linear
angular

Ji for a rotational joint

Ji for a prismatic joint

UM EECS 398/598 - autorob.github.io

zi-1: joint axis (Jwi)

The Jacobian
A 6xN matrix
linear
angular

Ji for a rotational joint

Ji for a prismatic joint

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Fixed Frame


v1
r2

angular velocity
of points in frame
wrt. axis k

angular rotation in frame

r1

v2

linear velocity
of points in frame
wrt. axis k

r3

rotation axis

v3

k axis out-of-plane
and passes through frame origin

vector to point
in frame
skew-symmetric
matrix

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Fixed Frame


Note: velocity dependent on frame, not
necessarily linear velocity at point

v1
r2

angular velocity
of points in frame
wrt. axis k

angular rotation in frame

r1

v2

linear velocity
of points in frame
wrt. axis k

r3

rotation axis

v3

k axis out-of-plane
and passes through frame origin

vector to point
in frame
skew-symmetric
matrix

derivative of frame rotation


UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating in Moving Frame

UM EECS 398/598 - autorob.github.io

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
UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating on N-link Arm


Angular Velocity

assuming velocities expressed in the same frame

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating on N-link Arm

consider effect of all frames


(0..n) on endeffector

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating on N-link Arm

Linear Velocity for Rotational Joint

position of endeffector frame


UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating on N-link Arm

Linear Velocity for Rotational Joint

take derivative wrt.


ith joint angle

UM EECS 398/598 - autorob.github.io

Velocity of Point Rotating on N-link Arm

Linear Velocity for Rotational Joint

UM EECS 398/598 - autorob.github.io

Geometric

The Jacobian
A 6xN matrix

Ji for a rotational joint


Procedure restated:

Ji for a prismatic joint

UM EECS 398/598 - autorob.github.io

Geometric

The Jacobian
A 6xN matrix

compute
endpoint
error

Ji for a rotational joint


Procedure restated:

compute step
direction

Ji for a prismatic joint


repeat

perform step
direction
UM EECS 398/598 - autorob.github.io

Geometric

The Jacobian
A 6xN matrix

Ji for a rotational joint


can we invert J(q)?

Ji for a prismatic joint

UM EECS 398/598 - autorob.github.io

The Jacobian
A 6xN matrix

Ji for a rotational joint


when can we invert J(q)?

Ji for a prismatic joint

UM EECS 398/598 - autorob.github.io

Jacobian Transpose

UM EECS 398/598 - autorob.github.io

Jacobian Transpose

UM EECS 398/598 - autorob.github.io

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
UM EECS 398/598 - autorob.github.io

Error Minimization by Jacobian Transpose


Instead, find configuration
displacement that minimizes
endeffector error squared

Define cost function


expressing squared error

UM EECS 398/598 - autorob.github.io

Error Minimization by Jacobian Transpose

Define cost function


expressing squared error

Take cost derivative

Evaluate for configuration convergence

UM EECS 398/598 - autorob.github.io

Matlab example:
Jacobian transpose
Initial configuration

Iterations to goal

UM EECS 398/598 - autorob.github.io

Pseudo Inverse

UM EECS 398/598 - autorob.github.io

Matlab example:
Pseudo Inverse
Initial configuration

Iterations to goal

UM EECS 398/598 - autorob.github.io

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

UM EECS 398/598 - autorob.github.io

Error Minimization by Jacobian Pseudoinverse

Define cost function


expressing squared error

Take cost derivative

Set to zero and solve for configuration displacement

Normal form

UM EECS 398/598 - autorob.github.io

IK Demo

UM EECS 398/598 - autorob.github.io

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.
UM EECS 398/598 - autorob.github.io

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.
UM EECS 398/598 - autorob.github.io

Singularities

Determinant of this matrix?


UM EECS 398/598 - autorob.github.io

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
UM EECS 398/598 - autorob.github.io

Maybe there is a simpler


approach to IK?

UM EECS 398/598 - autorob.github.io

Cyclic Coordinate Descent

http://www.ryanjuckett.com/programming/cyclic-coordinate-descent-in-2d/

UM EECS 398/598 - autorob.github.io

Cyclic Coordinate Descent

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/

UM EECS 398/598 - autorob.github.io

https://www.youtube.com/watch?v=MvuO9ZHGr6k

UM EECS 398/598 - autorob.github.io

Pros and Cons

Cyclic Coordinate Descent

+ 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


UM EECS 398/598 - autorob.github.io

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


UM EECS 398/598 - autorob.github.io

IK assignment details next class

UM EECS 398/598 - autorob.github.io

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
}

UM EECS 398/598 - autorob.github.io

world

IK Assignment

global

Specify target location, endeffector frame,


endeffector point in frame

Form kinematic chain


Transform endeffector into world

Endeffector specified in frame


UM EECS 398/598 - autorob.github.io

IK Assignment

Specify target location, endeffector frame, endeffector point in frame


Form kinematic chain
Transform endeffector into world
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)

UM EECS 398/598 - autorob.github.io

IK Assignment

Specify target location, endeffector frame, endeffector point in frame


Form kinematic chain
Transform endeffector into world
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
UM EECS 398/598 - autorob.github.io

Next class:
Motion Planning

http://people.csail.mit.edu/aperez/www/files/rrt_timelapse.jpg

UM EECS 398/598 - autorob.github.io