ME 4135 Differential Motion and the Robot Jacobian Fall 2012 R. R. Lindeke, Ph.D

ME 4135Differential Motion and the Robot Jacobian

Fall 2012R. R. Lindeke, Ph.D.

Lets develop the differential Operator – bringing calculus to Robots

The Differential Operator is a way to account for “Tiny Motions” (T)

It can be used to study movement of the End Frame over a short time interval (t)

It is a way to track and explain motion for different points of view

Considering motion:

We can define a General Rotation of a vector K:

By a general matrix defined as:

x

y

z

K i

K K j

K k

( , ) ( , ) ( , )x y zRot X Rot Y Rot Z

These Rotation are given as:

But lets remember, for our purposes that this angle is very small (a ‘tiny rotation’)

If the angle is small we can have some simplifications:

Cos small 1 Sin small small

1 0 0 0

0 ( ) ( ) 0( , )

0 ( ) ( ) 0

0 0 0 1

x xx

x x

Cos SinRot X

Sin Cos

Substituting the Small angle Approximation:

1 0 0 0

0 1 0( , )

0 1 0

0 0 0 1

xx

x

Rot X

1 0 0

0 1 0 0( , )

0 1 0

0 0 0 1

y

yy

Rot Y

1 0 0

1 0 0( , )

0 0 1 0

0 0 0 1

z

zzRot Z

Similarly for Y and Z:

Simplifying the Rotation Matricies (product)

1 0

1 0.

1 0

0 0 0 1

z y

z x

y x

Gen Rot

Here, we have neglected products of the terms!

What about Small (general) Translation?

We define it as a matrix:

General Tiny Motion is then (both considering both Rotation and Translation):

1 0 0

0 1 0( , , )

0 0 1

0 0 0 1

dx

dyTrans dx dy dz

dz

1

1_

1

0 0 0 1

z y

z x

y x

dx

dyGen Movement

dz

So let’s use this idea:

Here we define a motion which is due to a robot’s joint(s) moving during a small time interval:

T+T = {Rot(K,d)*Trans(dx,dy,dz)}T Note, T is the original pose Substituting for the matrices:

1

1

1

0 0 0 1

z y

z x

y x

dx

dyT T T

dz

Solving for the differential motion effect (T)

1

1

1

0 0 0 1

z y

z x

y x

dx

dyT T T

dz

Factoring the T out on the R.H.S.:

1 1 0 0 0

1 0 1 0 0

1 0 0 1 0

0 0 0 1 0 0 0 1

z y

z x

y x

dx

dyT T

dz

Further Simplifying:

0

0

0

0 0 0 0

z y

z x

y x

dx

dyT T

dz

We will call this matrix the del operator:

Thus, the Change in POSE (T)

dT (that is T) = T = {[Trans(dx,dy,dz)*Rot(K,d)] –

I} Here we see that this operator is

analagous to the derivative operator d( )/du when taken wrt HTM’s

Lets look into an application:

Given:

Subject it to 2 simultaneous movements:– Along X0 (dx) a translation of .0002 units

(/unit time)– About Z0 a Rotation of 0.001rad (/unit

time)

0

1 0 0 3

0 1 0 5

0 0 1 0

0 0 0 1

ncurrT

Graphically:

Xn

Yn

X0

Y0

R

Here:Rinit = (32 + 52) .5 = 5.831 units

init = Atan2(3,5) = 1.0304 rad

Where is the Frame ‘n’ after one time step?

Considering Position:– Effect of “Translation”:

X=3.0002 and Y = 5.000 New Rf = (3.00022 + 5.02).5 = 5.83105 u

– Effect of Rotation fin = 1.0304 + 0.001 = 1.0314 rad

Xf = Cos(fin) * Rf = 2.99505& Yf = Sin(fin) * Rf = 5.00309

Where is the Frame n after one time step?

Considering Orientation:

( ) .9999995

.000999998

0 0

Cos

n Sin

.000999998

.9999995

0 0

Sin

o Cos

0

0

1

a

After 1 time step, Exact Position is:

.9999995 .000999998 0 2.9951

.000999998 .9999995 0 5.0031

0 0 1 0

0 0 0 1

newT

Lets Approximate it with the ‘ operator’

Tnew = Tinit + dT = Tinit + Tinit

Where:0 .001 0 .0002 1 0 0 3

.001 0 0 0 0 1 0 5

0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 1

0 .001 0 .0048

.001 0 0 .003

0 0 0 0

0 0 0 0

initdT T

Therefore Tnew is Approximately:

1 0 0 3 0 .001 0 .0048

0 1 0 5 .001 0 0 .003

0 0 1 0 0 0 0 0

0 0 0 1 0 0 0 0

1 .001 0 2.9952

.001 1 0 5.003

0 0 1 0

0 0 0 1

new init initT T T

Comparing:

“Exact”:

Approximate:

.9999995 .000999998 0 2.9951

.000999998 .9999995 0 5.0031

0 0 1 0

0 0 0 1

newT

1 .001 0 2.9952

.001 1 0 5.003

0 0 1 0

0 0 0 1

newT

Realistically these are all but equal and using the ‘del’ approximation is soooo much easier!

We can (might!) use the ‘del’ approach to move a robot in space:

Take a starting POSE (Torig) & a starting motion set (deltas in rotation and translation as function of unit times)

Form operator for motion Compute dT ( Torig) Form Tnew = Torig + dT Repeat as time moves forward

over the time steps …

Taking Motion WRT other Spaces (non inertial basis)

Original Model:• dT = T (1)

Motion Taken WRT another (non-inertia) space:• dT = TT (2)• Here T implies motion wrt its own frame but could

be any other non-inertia space But the change itself (dT) is independent of

point of view so, equating (1) and (2) we can isolate T

T = (T)-1 T

Solving for the specific Terms in T

Origin Change Vector wrt Tspace:

Angular effects wrt Tspace:

Tp

Tp

Tp

T

T

T

dx d n d n

dy d o d o

dz d a d a

x n

y o

z a

RRRRRRRRRRRRRRRRRRRRRRRRRRRR



d, n, o & a vectors are extracts from the T Matrixdp is the translation vector in is the rotational effect in

Finding T -- from earlier example

The Orientation Effects:

Translation Effects:

0 1

0 0 0

.001 0

0 0

0 1 0

.001 0

0 0

0 0 0.001

.001 1

TX

TY

TZ

n

o

a

0 3 1 0.0001 1

0 5 0 0 0 .005 .0001 0.0049

0.001 0 0 0 0

0 3 0

0 5 1

0.001 0 0

TX p

TY p

d d n d n

d d o d o

0.0001 0

0 1 .003 0 0.003

0 0

0 3 0 0.0001 0

0 5 0 0 0 .000 0 0

0.001 0 1 0 1

TY pd d a d a

Subbing into a ‘del’ Form:

0 0.001 0 0.00490

0.001 0 0 0.0030

0 0 0 00

0 0 0 00 0 0 0

T T Tz y

T T TT z x

T T Ty x

dx

dy

dz

Approximating New T Pose with the ‘ T operator’

Tnew = Tinit + dT = Tinit + Tinit*T

Where:1 0 0 3 0 .001 0 .0049

0 1 0 5 .001 0 0 .003

0 0 1 0 0 0 0 0

0 0 0 1 0 0 0 0

0 .001 0 .0049

.001 0 0 .003

0 0 0 0

0 0 0 0

TinitdT T

Comparing:

“Exact” (from earlier):

New Approximation:

.9999995 .000999998 0 2.9951

.000999998 .9999995 0 5.0031

0 0 1 0

0 0 0 1

newT

1 .001 0 2.9951

.001 1 0 5.003

0 0 1 0

0 0 0 1

newT

Realistically these are all but equal – actually closer with this technique

An Application of this issue:

R

WC

PART

Camera

TWCR

TCamPartTR

part

If the Part is moving along a conveyor and we “measure” its motion with a

Camera (in camera frame) but we want to pick up the part with the robot, we

must ‘track’ it so we need to known the part motion in robot’s space (not camera

space!).

This is a Motion “Mapping” Issue:

Pa R WC CaPa Pa R C Pa

Pa R WC CaPa

Knowns: C Robot in WC Camera in WC And of course Part in Camera (But we don’t need it

now!)

Lets Isolate the “Middle”

R WC Ca R C

R WC Ca

To solve for R we “mathematically move” from R to R directly (R) and “The long way around”:

1 1R R Cam Cam Cam RWC WC WC WCT T T T

Rewriting into Standard Form:

It can be shown for any 2 Matrices (A & B): A-1*B = (B-1*A)-1 (1)

If, on the previous page we consider: TWC

R as “A” and TWCCam as “B”,

And define (TWCCam)-1*(TWC

R) as “T” Now, Using the theorem above (from matrix

math), then We find that R = “T”-1(Cam )”T” R is now shown in the standard form of earlier

– the terms: d, n, s & a vectors in the “T” matrix and the dp and vectors from the Cam are then all that is required to define part motion with respect to the robot space

R is given by simplifying!

(TWCCam)-1*(TWC

R) = “T”

Rp

Rp

Rp

R

R

R

dx d n d n

dy d s d s

dz d a d a

x n

y s

z a




HERE:

d, n, s & a vectors are extracts from the “T” Matrix above

dp is the ‘translation’ vector in Cam

is the ‘rotational effects’ in Cam

Thus we would have a means for tracking a moving part directly into robot space!

Now, Lets Examine the Jacobian

Fundamentally:

We use these Jacobians to further our knowledge of Geometric Calculus

1

q

q

D JD

D J D

and, If it 'exists', we can define an Inverse Jacobian as:

In This Model, Ddot & Dq,dot are:

x

y

z

x

y

zD

: Cartesian Velocity

We state, then, that the Jacobian is a mapping tool that relates Cartesian Velocities (of the end frame) to the

movement of the individual Robot Joints

1

2

3

4

5

6

q

q

q

qD

q

q

q

: Joint Velocity

Lets build one from ‘1st Principles’ – Here is a Spherical Arm:

X0

Y0

Z0

RLets start with only linear motion ---- its straight forward!

Writing the Position Models:

Z = R*Sin() X = R*Cos()*Cos() Y = R*Cos()*Sin()

( )

( )

( )

Sindz R Sin Rdt t t

S r RC

CCdx R C C RC RCdt t t t

C C r RC S RC S

Sdy CR C S RS RCdt t t t

C S r RS S RC C

To find velocity differentiate these which leads to:

Writing it as a Matrix:

0

X RC S RC S C C

Y RC C RS S S C

Z RC S r

This is the linear motion Jacobian; It is built as the Matrix of partial joint contributions (coefficients of the velocity

equations) to Velocity of the End Frame

Here we could develop an Inverse Jacobian:

'2 '2

'

' 2 ' 2 2

.5' 2 2

0y xR R x

zx zy R yR R R R R

r zyx zR R R

R x y

It was formed by taking the partial derivatives of the IKS eqns.

The process we did just now is limited to finding Linear Velocity

We need linear and angular velocity for full functional robots!

We can approach the problem by separations as before …

Here we had separated Velocity (Linear from Rotational) but not joints (arms) from joints (wrist)

Generally speaking, in the Jacobian we will obtain one Column for each Joint and 6 rows for full velocity effect

We say the Jacobian is a 6 by n matrix

Seperation Leads to:

A Cartesian Velocity Term V0

n:

An Angular Velocity Term 0

n:

0n

v q

x

y V J D

z

0

xn

y q

z

J D

Each of these “Ji’s” are 3 Row by n Columned Matrices!

Building the Sub-Jacobians (in general):

We define 3 motion stipulations: Velocities can only be added if they are

defined in the same space Motion of the end effecter (n frame) is

taken wrt the base space (0 frame) Linear Velocity effects are (truly)

physically separated from angular velocity effects

To address the problem we will only move one joint at a time (uses the fundamental DH separation principle!)

Lets start with the Angular Velocity (!)

Considering any joint i, its Axis of motion is: Zi-1 (Z in Frame i-1)

The (modeling) effect of the joint is to drive the very next frame (frame i)

If Joint i is revolute:

– here ki-1 is the unit vector of Zi-1– This could be applied to each of the joints (revolute) in the

machine (it rotates the next frame – and of course, all subsequent frames rotate similarly!)

But if the Joint is Prismatic, it has no angular effect on its “controlled” frame (and, thus, subsequent frames all the way to the end frame)

1 1 1ii i i i ik q Z q

Developing the J

We need to add up each of the joint effects BUT, We need to “normalize” them to base space

to added them up DH methods allow us to do this!

Since Zi-1 is in a Frame of the model, we really need only extract the 3rd column of the product of A1 * A2 * …*Ai-1 to have a definition of Zi-1 in base space to add the effect of Joint i in this ‘common space’ (note, the ‘qdot’ term is the rotational velocity of the revolute joint)

1 1 1ii i i i ik q Z q

So J then is just this:

0 11

1

0

nn

i i ii

i

i

Z q

(revolute joint)

(prismatic joint)

As stated previously, Zi-1 is the (1st 3 rows) of the 3rd col. of A0*…Ai-1 And we will have a term in the sum

for each joint

Note Zi-1 for Jointi – per DH algorithm!

For the Spherical Device:

0 0 1 2

0

0 1

0

1 1 0

0

0

0

1

n

nq

Z Z Z r

J D

J Z Z

Z

so, since:

and :

RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR

RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR

RRRRRRRRRRRRRR

always, always, always

All the rest of the Zi’s depend on the Frame Skeleton drawn!

Notice: 3 cols. since we have 3 joints!

Building the Linear Jacobian It too will depend on the movement of

Zi-1 It too will require that we normalize

each joints contribution to the base space

We will also find that revolute and prismatic joints will make functionally different contributions to the solution (as if we would think otherwise!)

Prismatics are “Easy,” Revolutes, not so much!

Building the Linear Jacobian

0

00

1

0

1

is linear velocity of the end frame in base space

as stated earlier!

n

nn n

iii

n

vi i

d

dd qq

dJ q

to n

Building the Linear Jacobian – Prismatic Joints

When the prismatic jointi moves, all subsequent links move

(linearly) at the same rate and in the same direction

Building the Linear Jacobian – Prismatic Joints

Therefore, for each prismatic joint of a machine, the contribution to the Jacobian (after normalizing) is:

Zi-1 which is the 3rd column of the matrix given by:

A1 * … * Ai-1

This is as expected based on the model on the previous slide

Building the Linear Jacobian – Revolute Joints

This is a dicer problem – remembering the idea of prismatic joints on angular velocity …

But no that won’t work here just because its a rotation, and it (primarily) changes

orientation of the end, it does also have a linear contribution effect to the motion of the end as we saw in in our earlier “Del” discussion – that is revolute joints have a “levering effect” which moves the origin of the n-frame (and the levering is clearly a cartesian motion).

We must compute and account for this effect and then normalize it too.


Using this model we would expect a rotation i about Zi-1 would lever the end by an effect that is equivalent to the CROSS product of the driver vector and the connector vector


This is the driver vector given by:

Zi-1 X di-1n

[with a Magnitude equal to joint velocity]


Zi-1 X di-1n is the direction of the linear

contribution of the revolute joint i on n-Frame motion

It must be normalized Notice: di-1

n = d0n – d0

i-1

This equation “normalizes” the vector di-1n

But as we know, the d-vectors are origin position of the various frames (Framei-1 and Framen)

So let’s rewrite the normalizing equation as: di-1

n = On – Oi-1 (cap. letter O for origin!)


The contribution to the Jv due to a revolute joint is then:

Zi-1 X (On – Oi-1)

– Where: Zi-1 is the 3rd col. of the T0

i-1 (A0*… *Ai-1) Oi-1 is 4th col. of the T0

i-1 (A0*… *Ai-1) On is 4th col. Of T0

n (the FKS!) NOTE: when we pull the columns we

only need the first 3 rows

Building the Linear Jacobian

Summarizing:– The Jv is a 3-row by n columned

matrix– Each column is given by joint type:

Revolute Joint: Zi-1 X (On – Oi-1) Prismatic Joint: Zi-1

Combining Both Halves of the Jacobian:

For Revolute Joints:

For Prismatic Joints:

1 1

1

i n iv

i

Z O OJJ

J Z

RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR

RRRRRRRRRRRRRR

1

0

v iJ ZJ

J

RRRRRRRRRRRRRR

What is the Form of the Jacobian

Robot is: (PPRRRR) – a cylindrical machine with a spherical wrist:

Z0 is [0,0,1)T; O0 = (0,0,0)T always, always,

always! Zi-1’s and Oi-1’s are per the frame skeleton

0 1 2 6 2 3 6 3 4 6 4 5 6 5

2 3 4 50 0

Z Z Z O O Z O O Z O O Z O OJ

Z Z Z Z

Lets build one – Here is the Spherical Arm (that we did earlier):

X0

Y0

Z0

1

2

d3

Lets start by making a frame skeleton (just do it!)

Lets try this on the Spherical ARM we did earlier:

X0

Y0

Z0

X0

Y0

Z0

Z1

X1

Y1

Z2

X2

Y2

Zn

Xn

Yn

1

2

d3

The robot indicates this frame skeleton


Fr Link Var d a C S C S

0→1 1 R 1 0 0 90 0 1 C1 S1

1→2 2 R 2+90 0 0 90 0 1 -S2 C2

2→n 3 P 0 d3 0 0 1 0 1 0

LP Table:


Ai’s:

1

2

33

1 0 1 0

1 0 1 0

0 1 0 0

0 0 0 1

2 0 2 0

2 0 2 0

0 1 0 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1

0 0 0 1

C S

S CA

S C

C SA

Ad


T1 = A1 T2 = A1 * A2

T0n = T3 = A1*A2*A3

1 2 1 1 2 0

1 2 1 1 2 02

2 0 2 0

0 0 0 1

C S S C C

S S C S CT

C S

3

30

3

1 2 1 1 2 1 2

1 2 1 1 2 1 23

2 0 2 2

0 0 0 1

n

C S S C C d C C

S S C S C d S CT T

C S d S

Lets try this on the Spherical ARM we did earlier: THE JACOBIAN

0 3 0 1 3 1 2

0 1 0

Z O O Z O O ZJ

Z Z

Of This Form:


Here:

3

0 3 0 3

3

3

3

0 1 2 0

0 1 2 0

1 2 0

1 2

1 2

0

d C C

Z O O d S C

d S

d S C

d C C

3

1 3 1 3

3

3

3

3

1 1 2 0

1 1 2 0

0 2 0

1 2

1 2

2

S d C C

Z O O C d S C

d S

d C S

d S S

d C


3 3

3 3

3

1 2 1 2 1 2

1 2 1 2 1 2

0 2 2

0 1 0

0 1 0

1 0 0

d S C d C S C C

d C C d S S S C

d C SJ

S

C

Documents

ME 4135 Differential Motion and the Robot Jacobian Fall 2012 R. R. Lindeke, Ph.D