L6 - Differential Motion and Jacobians 1 v 1

8/9/2019 L6 - Differential Motion and Jacobians 1 v 1

1/65

Differential Motion and the Robot

Jacobian

U

niversitiKualaLumpurMalaysiaFranceInstitute

Originally prepared by: Prof Engr Dr Ishkandar Baharin

Head of Campus & Dean

UniKL MFI


2/65

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 intervals (!t) It is a way to track and explain motion for

different points of viewU



3/65

Considering motion:

We can define a

General Rotation of avector K:

By a general matrixdefined as:

x

y

z

K i

K K j

K k

=

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

U



4/65

These Rotation are given as:

But lets remember for our purposes that

this angle is very small (a tiny rotation) in

radians

If the angle is small we can have use some

simplifications:

1 0 0 0

0 ( ) ( ) 0( , )

0 ( ) ( ) 0

0 0 0 1

x x

x

x x

Cos Sin Rot X

Sin Cos

=

Cos small 1 Sin small smallUniversitiKualaLumpurMalaysiaFrance

Institute


5/65

Substituting the Small angle

Approximation:

1 0 0 0

0 1 0( , )

0 1 0

0 0 0 1

x

x

x

Rot X

1 0 00 1 0 0

( , )0 1 0

0 0 0 1

y

y

y

Rot Y

1 0 01 0 0

( , )0 0 1 0

0 0 0 1

z

z

z Rot Z

Similarly for Y and Z:

U

niversitiKualaLumpurMalaysiaFrance

Institute


6/65

Simplifying the Rotation Matrices(form their product):

1 0

1 0.

1 0

0 0 0 1

z y

z x

y x

Gen Rot

Note here: we have neglected higher order products of the

terms!

ClassExercise !!!

General Small Rotation of a Vector K ( , ) ( , ) ( , ) x y zR X R Y R Z

U


Institute


7/65

What about Small (general)

Translations?

We define it as a

matrix:

General Tiny Motion

is then (including both

Rot. 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 Movementdz

U


Institute


8/65

So using this idea:

Lets define a motion which is due to a robots

joint(s) moving during a small time interval: T+!T = {Rot(K,d)*Trans(dx,dy,dz)}T

Consider Here: T is the original end frame pose

Substituting for the matrices:

1

1

1

0 0 0 1

z y

z x

y x

dx

dyT T T

dz

+

UniversitiKualaLumpurMalaysiaFrance

Institute


9/65

Solving for the differential motion

(!T)

1

1

1

0 0 0 1

z y

z x

y x

dx

dyT T T

dz

U


Institute


10/65

Factoring T (on the RHS)

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

ClassExercise !!!

U


Institute


11/65

Further Simplifying:

0

0

0

0 0 0 0

z y

z x

y x

dx

dy

T Tdz

We will call this

matrix the del

operator: UniversitiKualaLumpurMalaysiaFrance

Institute


12/65

Thus, the Change in POSE (!T or dT) is:

dT (!T) = T

Where: = {[Trans(dx,dy,dz)*Rot(K,d)] I}

Thus we see that this operator is analogous tothe derivative operator d( )/dx but now taken withrespect to Homogeneous Transformation

Matrices !!!

U


Institute


13/65

Lets look into an application:

Given:

Subject it to 2 simultaneous movements: Along X 0 (dx) by .0002 units (/unit time)

About Z0 a Rotation of 0.001rad (/unittime)

0

1 0 0 3

0 1 0 5

0 0 1 00 0 0 1

n

currT

=

U


Institute


14/65

Graphically:

Xn

Yn

X0

Y0

R

Here:

Rinit = (32

+ 52

).5

=5.831 units

init = Atan2(3,5) =1.0304 rad

U


Institute


15/65

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 unit

Effect of Rotation fin = 1.0304 + 0.001 = 1.0314 rad

Therefore: Xf= Cos(fin) * Rf= 2.99505 And: Yf= Sin(fin) * Rf= 5.00309

Class

Exercise !!!

U

niversitiKual

aLumpurMalaysiaFrance

Institute


16/65

Where is the Frame n after one time

step?

Considering

Orientation: ( )

( ) .9999995

.000999998

0 0

Cos

n Sin

= =

r

( )( )

.000999998

.9999995

0 0

Sin

o Cos

= =

r

0

0

1

a

=

r

ClassExercise !!!

U


Institute


17/65

After 1 time step, Exact Poseis:

.9999995 .000999998 0 2.99505

.000999998 .9999995 0 5.00309

0 0 1 0

0 0 0 1

newT

=

U


Institute


18/65

Lets Approximate it using this operator

Tnew

= Tinit

+ dT = Tinit

+ Tinit

the 1st law of differential calculus

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

= =

=

UniversitiKualaLumpurMalaysiaFrance

Institute


19/65

Thus, 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 init

T T T

= + = +

=

U


Institute


20/65

Comparing: Exact:

Approximate:

.9999995 .000999998 0 2.99505

.000999998 .9999995 0 5.00309

0 0 1 0

0 0 0 1

newT

=

1 .001 0 2.9952

.001 1 0 5.003

0 0 1 00 0 0 1

newT

=

Realistically these are all but equal but using the del

approximation, but finding it was much easier!U

niversitiKualaLumpurMa

laysiaFrance

Institute


21/65

We can (might!) use the del approach to

move a robot in space:

Take a starting POSE (Torig) and

a starting motion set (deltas inrotation and translation as

function of unit times)

Form operator for motion Compute dT (Torig)

Form Tnew = Torig + dT

Repeat as time moves forward

over n time steps

U


laysiaFrance

Institute


22/65

Taking Motion W.R.T. other Spaces

(another use for this del operator idea)

Original Model (the motion we seek is defined in an

inertial space), i.e. w.r.t. Base Frame: dT = T (1)

However, if the motion is taken w.r.t. another (non-

inertia) space, i.e w.r.t. New Frame :

dT = T T (2)

Here T implies motion w.r.t. itself a moving frame butcould be motion w.r.t. any other non-inertia space (robot or

camera, etc.)

Consider as well: the pose change (motion that is

happening) itself (dT) is independent of point of

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

T = (T)-1T

Remember Guys !!!Rotation w.r.t. Base Frame, Pre-Multiply,Rotation w.r.t. New Frame, Post-Multiply

U


laysiaFrance

Institute


23/65

Solving for the specific Terms inT

Positional ChangeVector w.r.t. (any)Tspace:

Angular effects wrtTspace:

( )( )

( )

T

p

T

p

T

p

T

T

T

dx d n d n

dy d o d o

dz d a d a

x n

y oz a

= +

= +

= +=

==

uuuuur uur r

uuuuur uur r

uuuuur uur r

r

r

r

d, n, o & a vectors

are extracts from

the T Matrix

dp is the translationvector in is the rotationaleffects in

U


laysiaFrance

Institute


24/65

Subbing into a del Form:

0

00

0 0 0 0

T T T

z y

T T T

T z x

T T T

y x

dx

dydz

=

U


laysiaFrance

Institute


25/65

An Application of this issue:

R

WC

PART

Camera

TWCR

TCamPart

TRpart

If the Part is moving along a conveyor and

we measure its motion in the Camera

Frame (let the camera measure it at

various times) and we would need to pick

the part with the robot, we must track wrt

to the robot, so we need part motion del

in the robots space.U


laysiaFrance

Institute


26/65

This is a Motion Mapping Issue:

Pa R WC Ca PaPa R C Pa

Pa R WC Ca Pa

Knowns: C = del Motion in Camera Space Robot in WC

Camera in WC

And of course Part in Camera (But we dont need it

for now!)

Note: Pa = PART

U


laysiaFrance

Institute


27/65

Lets Isolate the Middle R WC Ca

R C R WC Ca

To solve forR we make progress from Rto R directly (R) and The long wayaround:

( ) ( ) ( ) ( ) ( )1 1

R R Cam Cam Cam R

WC WC WC WC T T T T

=

Class

Exercise !!!

U


laysiaFrance

Institute


28/65

Rewriting into a Standard Form:

It can be shown for 2 Matrices (A & B):

A-1*B = (B-1*A)-1 (1)

Or B-1*A = (A-1*B)-1 (2)

If, on the previous page we consider:

TWCCam as A and TWC

R as B,

and define the form: (TwcCam

)

-1

*TWCR

as T Then, Using the theorem (from matrix math)

stated as (2) above T-1 is: (TWCR)-1* TWC

Cam

Class

Exercise !!!

U


laysiaFrance

Institute


29/65

Continuing:

Rewriting, we find that R = T-1(Cam) T

R is now shown in the standard formfor non-inertial space motion

the terms: d, n, o & a vectors come from our complex Tmatrix

the d p and vectors can be extracted from the Cam These term are required to define motion in the robot space

Of course the T is really: (TwcCam)-1*TWC

R here!

Its from this T product that we extract n, o, a, d vectors

UniversitiKualaLumpurMa

laysiaFrance

Institute


30/65

R

is given by simplifying: 1st: (Twc

Cam)-1*TWCR = T

Then these Scalars:)

( )( )

R

p

R

p

R

p

R

R

R

dx d n d n

dy d o d o

dz d a d a

x n

y o

z a

= +

= += +

==

=

uuuuur uur r

uuuuur uur r

uuuuur uur r

r

r

r

WHERE:d, n, o & a vectors areextracts from the T Matrix

above

dp is the translation vector

in Cam

is the vector of rotational

effects inCam

UniversitiKua

laLumpurMa

laysiaFrance

Institute


31/65

Homework 1 !!!

1. Draw the Motion Mapping for

2. Derive the full equation forGripper where

Part

GripperT

Gripper

Part

CamT

Gripper

WCT

Part

GripperT

WC

Camera

0 0 1 20 1 0 2

1 0 0 2

0 0 0 1

0 1 0 10

1 0 0 10

0 0 1 10

0 0 0 1

Gripper

WC

Camera

WC

T

T

=

=

UniversitiKua

laLumpurMa

laysiaFrance

Institute


32/65

Homework 2 !!!

1. Draw the Motion Mapping for

2. Derive the full equation forGripper where

Part

RobotT

Gripper

Part

Cam

T

Robot

WCT

Part

RobotT

WC

Camera

1 0 0 20 1 0 10

0 0 1 0

0 0 0 1

0 1 0 10

1 0 0 10

0 0 1 10

0 0 0 1

Robot

WC

Camera

WC

T

T

=

=

UniversitiKua

laLumpurMa

laysiaFrance

Institute


33/65

Lets Examine the Jacobian Ideas

Fundamentally:

1

q

q

D J D

D J D

=

=

and, If it 'exists' we can define the

Inverse Jacobian as:

UniversitiKua

laLumpurMa

laysiaFrance

Institute


34/65

In This Model, Ddot & Dq,dot are:

1

2

3

4

5

6

;

;

Cartesian Velocity

Joint Velocity

x

y

z

q

x

y

D z

q

q

qD

q

q

q

=

=

We state, then, that the

Jacobian is a mappingtool that relates

Cartesian Velocities (of

the n frame) to the

movement of the

individual Robot Joints

UniversitiKua

laLumpurMa

laysiaFrance

Institute


35/65

Lets build one from 1st Principles

Here is a Spherical Arm:

X0

Y0

Z0

RLets start with only

linear motion ----

equations are

straight forward!

UniversitiKua

laLumpurMa

laysiaFrance

Institute


36/65

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 RC dt t t t

C C R RC S RC S

Sdy CR C S RS RC dt t t t

C S R RS S RC C

= +

= +

= + +

=

= + +

= +

UniversitiKua

laLumpurMa

laysiaFrance

Institute

To find velocity, differentiate

these as seen here:


37/65

Writing it as a Matrix:

0

X RC S RC S C C

Y RC C RS S S C

RC SZ R

=

This is the Jacobian; It is built as the Matrix of

partial joint contributions (coefficients of the

velocity equations) to Velocity of the End FrameUniversitiKua

laLumpurMa

laysiaFrance

Institute


38/65

Here we could develop an Inverse

Jacobian:

( ) ( )

( )

'2 '2

'

' 2 ' 2 2

.5' 2 2

0y xxR R

zx zy Ry R R R R R

yR zx z R R R

R x y

=

= +

It was formed by taking

the partial derivatives

of the IKS equations

Class

Exercise !!!UniversitiKua

laLumpurMa

laysiaFrance

Institute


39/65

The process we just did is limited to finding

Linear Velocity!

and We need both linear and angular

velocities for full functioned robots!

We can approach the problem by separationsas we

did in the General case of Inverse Kinematics

Here we separate Velocity (Linear from Rotational),not Joints (Arms from Wrists)

Generally speaking, in the Jacobian we will obtain

one Column for each Joint and 6 rows for a full

velocity effect We say the Jacobian is a 6 by n (6xn) matrix

UniversitiKua

laLumpurMa

laysiaFrance

Institute


40/65

Separation Leads to: A Cartesian Velocity

Term V0n

:

An Angular Velocity

Term 0n:

0

n

v q

x

y V J D

z

= =

0

x

ny q

z

J D

= =

Each of these Jis are 3 Row by n Columned MatricesUniversitiKua

laLumpurMa

laysiaFrance

Institute


41/65

Building the Sub-Jacobians: We follow 3 stipulations:

Velocities can only be added if they are defined inthe same space as we know from dynamics

Motion of the end effector (n frame) is taken w.r.t.the base space (0 frame)

Linear Velocity effects are physically separablefrom angular velocity effects

To address the problem we will consider

moving a single joint at a time (using DHseparation ideas!)

UniversitiKua

laLumpurMa

laysiaFrance

Institute


42/65

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 a joint is to drive the very next frame

(frame i)

If Joint i is revolute:

here k(i-1) is the Zi-1 direction (by definition)

This model is applied to each of the joints (revolute) in the

machine (as it rotates the next frame, all subsequent fames,move similarly!)

But if the Joint is Prismatic, it has no angular effect on its

controlled frame and thus no rotation from it on all

subsequent frames

1 1 1

i

i i i i ik q Z q = =

UniversitiKua

laLumpurMa

laysiaFrance

Institute


43/65

Developing the (base) J

We need to add up each of the joint effects

Thus we need to normalize them to base space to do thesum

DH methods allow us to do this!

Since Zi-1 is the active direction in a Frame of the model, wereally need only to extract the 3rd column of the product of A1

* *Ai-1 to have a definition of Zi-1 in base space. Then, thisAis products 3

rd column is the effect of Joint i as defined inthe (common) base space (note, the qdot term is the rate ofrotation of the given joint)

1 1 1ii i i i ik q Z q = =

UniversitiKua

laLumpurMalaysiaFrance

Institute


44/65

So the Angular Velocity then is:

0 1

1

1

0

n

ni i i

i

i

i

Z q

=

=

=

=

(revolute joint)

(prismatic joint)As stated previously, Zi-1 is the 3

rd col. of A1*Ai-1 (rows

1,2 & 3). And we will have a term in the sum for each joint

Note Zi-1 for

Jointi per DH

algorithm!

UniversitiKua

laLumpurMalaysiaFrance

Institute


45/65

Going Back to the Spherical Device:

0 0 1

0

0 1

0

1 1 0

0

0

0

1

we state:

Therefore:

and (always):

n

nq

Z Z

J D

J Z Z

Z

= + +

=

=

=

uur uur r

uur uur r

uurHere, Z1 depends

on the Frame

Skeleton drawn!

Notice: 3 columns since

we have 3 joints!

X 0

Y 0

Z 0

0 1

1

nn

i i i

i

Z q =

=

UniversitiKua

laLumpurMalaysiaFranceInstitute


46/65

Building the Linear Jacobian

It too will depend on the motion associatedwith Zi-1

It too will require that we normalize each joints

linear motion contribution to the base space We will find that revolute and prismatic joints

will make functionally different contributions to

the solution (as if we would think otherwise!) Prismatic joints are Easy, Revolutes are not!

UniversitiKua



47/65


0

00

1

0

n

n n

ni

ii

n

vi i

d

dd qq

dJ q

=

=

=

=

1 to n

is linear velocity of the end frame wrt the base

UniversitiKua


B ildi th Li J bi f


48/65

Building the Linear Jacobian for

Prismatic Joints

When a prismatic jointi moves, all

subsequent links move (linearly) at the

same rate and in the same directionUniversitiKua




49/65


Prismatic Joints

Therefore, for each prismatic joint of amachine, the contribution to theJacobian (after normalizing) is:

Zi-1 which is the 3rd

column of the matrix givenby:A0 * * Ai-1

This is as expected based on the modelon the previous slide (and our moveonly one and then normalize it method)

UniversitiKua




50/65


Revolute Joints

This is a dicer problem, but then, remembering the idea of prismatic joints on angular

velocity

But nothat wont work here just because its arotation, and it changes orientation of the end revolute motion also does have a linearcontribution effect to the motion of the end

This is a levering effect which moves the origin of the n-frame as we saw when discussing the del-operator on the -

R structure. We must compute and account for this effect

and then normalize it too.

UniversitiKua




51/65


Revolute Joints

Using this model we would

expect that a rotation i wouldlever the end by an amount that

is equivalent (in direction) to the

CROSS product of the driver

vector and the connector vector

and with a magnitude equal toJoint velocity

UniversitiKua




52/65


Revolute Joints

This is thedirectional

resultant (DR)

vector given by:

Zi-1 X di-1n

[with Magnitude

equal to joint

speed!]

Note the Green

Vector is equal to

the red DR vector!UniversitiKualaLumpurMalaysiaFranceInstitute



53/65


Revolute Joints

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

the revolute joint i on n-Frame motion It too must be normalized

Notice: di-1n = d0

n d0i-1 (call it eq. 3)

This normalizes the vector di-1n to the basespace

But the d-vectors on the r.h.s. are really origin

position of the various frames (Framei-1 andFramen) i.e. the positions of frame Origins

So lets rewrite equation 3 as: di-1n = On Oi-1

UniversitiKualaLumpurMalaysiaFranceInstitute



54/65


Revolute Joints

The contribution to the Jv due to a revolutejoint is then:

Zi-1 X (On Oi-1)

Where: Zi-1 is the 3

rd col. of the T0i-1 (A1* *Ai-1)

Oi-1 is 4th col. of the T0

i-1 (A1* *Ai-1)

On is 4th col. Of T0n (the FKS!) NOTE when we pull the columns we only need the

first 3 rows!



55/65


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

And notice: select Zi-1 and Oi-1 for the frame before

the current joint column


Combining Both Halves of the


56/65

Combining Both Halves of the

Jacobian:

For Revolute Joints:

For Prismatic Joints:

( )1 1

1

i n iv

i

Z O OJJ

JZ

= =

uuuuuuuuuuuuuuuuuur

uuur uur uuur

uuur

1

0

v iJ Z

JJ

= =

uuur

r



57/65

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-1s and Oi-1s 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 O

J Z Z Z Z

=

UniversitiKualaLumpurM

alaysiaFranceInstitute

Lets try this on the Spherical ARM


58/65


we did earlier:

X0

Y0

Z0

1

2

d3

X0

Y0

Z0

Z1

X1

Y1

Z2

X2

Y2

Zn

Xn

YnThe robot indicatesthis frame skeleton:


alaysiaFranceInstitute

Lets try this on the Spherical ARM we


59/65

Lets try this on the Spherical ARM we

did earlier:

010100d30P32n

C2-S21090002+90R212

S1C11090001R101

S C S C adVarLinkFr

LP Table:


alaysiaFranc

eInstitute



60/65


we did earlier:

Ais:

1

2

3

3

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

=

=

=


alaysiaFranc

eInstitute



61/65


we did earlier: T1 = A1!

T2 = A1 * A2

T0n = T3 = A1*A2*A3

1 2 1 1 2 01 2 1 1 2 0

22 0 2 0

0 0 0 1

C S S C C S S C S C

TC S

=

3

3

0

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 C T T

C S d S

= =


alaysiaFranc

eInstitute



62/65


we did earlier: THE JACOBIAN

) ( )0 3 0 1 3 1 2

0 10

Z O O Z O O Z JZ Z

=

r

The Jacobian is Of This Form:


alaysiaFranc

eInstitute



63/65

y p

we did earlier: THE JACOBIAN Here:

( )

3

0 3 0 3

3

3

3

0 1 2 0

0 1 2 01 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 00 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

= =


alaysiaFranc

eInstitute

C P d t f t t A d B


64/65

Cross Product of two vectors A and B

2 3 3 1 1 2

1 2 3

2 3 3 1 1 2

1 2 3

1 2 3 3 2

2 3 1 1 3 2 3 3 2 3 1 1 3 1 2 2 1

3 1 2 2 1

( ) ( ) ( )

i j ka a a a a a

a a a i j k b b b b b b

b b b

c a b a b

c a b a b a b a b i a b a b j a b a b k

c a b a b

= = = + +

= = = + +

C A B

Ar

Br

Resultant Vector = nr

or

ar

x

y

z

vectorsCoordinate

Frame

A Br r

= Cr

i

j

k


alaysiaFranc

eInstitute

After total Simplification, THE Full


65/65

p ,

JACOBIAN is:

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 S

J S

C

=


alaysiaFranc

eInstitute

L6 - Differential Motion and Jacobians 1 v 1

Documents

Transcript of L6 - Differential Motion and Jacobians 1 v 1