Diff Motion Jacobian Part1 S06

8/2/2019 Diff Motion Jacobian Part1 S06

1/62

ME 4135Differential Motion and the RobotJacobian

Slide Series 6

R. R. Lindeke, Ph.D.


2/62

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 view


3/62

Considering motion:

We can define aGeneral Rotation of avector K:

By a general matrixdefined as:

x

y

z

K i

K K j

K k

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


4/62

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 xx

x x

Cos SinRot X

Sin Cos

Cos small 1 Sin smallsmall


5/62

Substituting the Small angle

Approximation:

1 0 0 0

0 1 0

( , ) 0 1 0

0 0 0 1

x

x

xRot X

1 0 0

0 1 0 0( , )

0 1 0

0 0 0 1

y

y

y

Rot Y

1 0 0

1 0 0( , )

0 0 1 0

0 0 0 1

z

z

zRot Z

Similarly for Y and Z:


6/62

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!


7/62

What about Small (general)

Translations?

We define it as a matrix:

General Tiny Motion isthen (including both Rot.and Translation):

1 0 0

0 1 0

( , , ) 0 0 1

0 0 0 1

dx

dy

Trans dx dy dz dz

1

1_

1

0 0 0 1

z y

z x

y x

dx

dyGen Movement

dz


8/62

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:

11

1

0 0 0 1

z y

z x

y x

dxdy

T T Tdz


9/62

Solving for the differential motion (T)

11

1

0 0 0 1

z y

z x

y x

dxdy

T T Tdz


10/62

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


11/62

Further Simplifying:

00

0

0 0 0 0

z y

z x

y x

dxdy

T Tdz

We will call thismatrix the del

operator:


12/62

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 to the derivative operator d( )/dxbut now taken with respect to HTMs!


13/62

Lets look into an application:

Given:

Subject it to 2 simultaneous movements: Along X0 (dx) by .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

n

currT


14/62

Graphically:

Xn

Yn

X0

Y0

R

Here:Rinit = (3

2 + 52) .5 =5.831 units

init = Atan2(3,5) =1.0304 rad


15/62

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

Therefore: Xf = Cos(fin) * Rf = 2.99505

And: Yf = Sin(fin) * Rf = 5.00309


16/62

Where is the Frame n after one time

step?

ConsideringOrientation:

( ) .9999995

.0009999980 0

Cos

n Sin

.000999998

.9999995

0 0

Sin

o Cos

0

0

1

a


17/62

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


18/62

Lets Approximate it using this operator

Tnew = Tinit + dT = Tinit + Tinit the 1st law of differentialcalculus

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


19/62

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


20/62

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.0030 0 1 0

0 0 0 1

newT

Realistically these are all but equal but using the del

approximation, but finding it was much easier!


21/62

We can (might!) use the del approach

to move a robot in space:

Take a starting POSE (Torig) and astarting motion set (deltas in rotation

and translation as function of unittimes)

Form operator for motion

Compute dT (Torig)

Form Tnew = Torig + dT

Repeat as time moves forward overn time steps


22/62

Taking Motion W.R.T. other Spaces

(another use for this del operator idea)

Original Model (the motion we seek is defined in an inertialspace):

dT = T (1) However, if the motion is taken w.r.t. another (non-inertia)

space:

dT = TT (2)

Here T implies motion w.r.t. itself a moving frame but could bemotion 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


23/62

Solving for the specific Terms inT

Positional Change Vector

w.r.t. (any) Tspace:

Angular effects wrt Tspace:

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 o

z a

d, n, o & a vectorsare extracts from theT Matrixdp is the translationvector in is the rotational

effects in


24/62

Subbing into a del Form:

0

0

00 0 0 0

T T Tz y

T T T

T z x

T T T

y x

dx

dy

dz


25/62

An Application of this issue:

R

WC

PART

Camera

TWCR

TCamPartTR

part

If the Part is moving along a conveyor andwe measure its motion in the Camera

Frame (let the camera measure it atvarious times) and we would need to pickthe part with the robot, we must track wrt

to the robot, so we need part motion delin the robots space.


26/62

This is a Motion Mapping Issue:

Pa R WC Ca PaPa R C Pa

Pa R WC Ca Pa

Knowns: C

Robot in WC

Camera in WC

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

now!)


27/62

Lets Isolate the Middle

R WC CaR C

R WC Ca

To solve for Rwe make progress from R toR directly (R) and The long way around:

1 1

R R Cam Cam Cam R

WC WC WC WC T T T T


28/62

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:

TWCCamas A and TWC

Ras B,

and define the form: (TwcCam)-1*TWCRas T

Then, Using the theorem (from matrix math)stated as (2) above T-1 is: (TWC

R)-1* TWCCam


29/62

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 & avectors come from our complex Tmatrix

the dp and vectors can be extracted from theCam

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


30/62

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

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

above

dpis the translation vectorin Cam

is the vector of rotational

effects inCam


31/62

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:


32/62

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 theJacobian is a mappingtool that relatesCartesian Velocities (of

the n frame) to themovement of theindividual Robot Joints


33/62

Lets build one from 1stPrinciples

Here is a Spherical Arm:

X0

Y0

Z0

RLets start with onlylinear motion ----

equations arestraight forward!


34/62

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

To find velocity, differentiate

these as seen here:


35/62

Writing it as a Matrix:

0

XRC 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 ofpartial joint contributions (coefficients of the

velocity equations) to Velocity of the End Frame


36/62

Here we could develop an Inverse

Jacobian:

' 2 ' 2

'

' 2 ' 2 2

.5' 2 2

0

yx xR R

zx zy R yR R R R R

yR zx zR R R

R x y

It was formed by takingthe partial derivatives of

the IKS equations


37/62

The process we just did is limited to finding Linear Velocity! and We need both linear and angular velocities for fullfunctioned 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 fullvelocity effect

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


38/62

Separation Leads to:

A Cartesian VelocityTerm V0

n:

An Angular VelocityTerm 0n:

0n

v q

x

y V J D

z

0

x

n

y q

z

J D

Each of these Jis are 3 Row by n Columned Matrices


39/62

Building the Sub-Jacobians:

We follow 3 stipulations: Velocities can only be added if they are defined in the

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 separable fromangular velocity effects

To address the problem we will considermoving a single joint at a time (using DHseparation ideas!)


40/62

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 itscontrolled frame and thus no rotoation from it on all subsequent

frames

1 1 1

i

i i i i ik q Z q


41/62

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 the

sum 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 1

i

i i i i ik q Z q


42/62

So the Angular Velocity then is:

0 11

1

0

(revolute joint)

(prismatic joint)

nn

i i ii

i

i

Z q

As stated previously, Zi-1 is the 3rd col. of A1*Ai-1 (rows

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

Note Zi-1 forJointi per DHalgorithm!


43/62


44/62

Building the Linear Jacobian

It too will depend on the motion associatedwith Zi-1

It too will require that we normalize each jointslinear motion contribution to the base space

We will find that revolute and prismatic jointswill make functionally different contributions tothe solution (as if we would think otherwise!)

Prismatic joints are Easy, Revolutes are not!


45/62


0

00

1

0

n

n nn

iii

n

vi i

d

dd q

q

dJ

q

1 to n

is linear velocity of the end frame wrt the base


46/62

Building the Linear Jacobian for

Prismatic Joints

When a prismatic jointi moves, allsubsequent links move (linearly) at the

same rate and in the same direction


47/62


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:

A0* * Ai-1

This is as expected based on the model onthe previous slide (and our move only one

and then normalize it method)


48/62

Building the Linear Jacobian for

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 endrevolute motion also does have a linear contributioneffect 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 -Rstructure.

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


49/62


Revolute Joints

Using this model we wouldexpect that a rotation i wouldlever the end by an amount that

is equivalent (in direction) to theCROSSproduct of the drivervector and the connector vector

and with a magnitude equal toJoint velocity


50/62


Revolute Joints

This is thedirectionalresultant (DR)

vector given by:

Zi-1 X di-1n

[with Magnitudeequal to joint

speed!]

Note the Green

Vector is equal tothe red DR vector!


51/62


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-1

n = d0n d0

i-1 (call it eq. 3)

This normalizes the vector di-1n to the base space

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

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

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


52/62


Revolute Joints

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

Zi-1 X (On Oi-1)

Where:

Zi-1 is the 3rd col. of the T0

i-1 (A1* *Ai-1)

Oi-1 is 4th col. of the T0

i-1 (A1* *Ai-1)

On is 4th col. Of T0

n (the FKS!)

NOTE when we pull the columns we only need the first 3rows!


53/62


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


54/62

Combining Both Halves of the

Jacobian:

For Revolute Joints:

For Prismatic Joints:

1 1

1

i n iv

i

Z O OJJ

JZ

1

0

v iJ Z

JJ


55/62

What is the Form of the Jacobian?

Robot is: (PPRRRR) a cylindrical machinewith a spherical wrist:

Z0 is (0,0,1)T; O0= (0,0,0)Talways, 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 52 3 4 5

0 0

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

Z Z Z Z


56/62

Lets try this on the Spherical ARM we

did earlier:

X0

Y0

Z0

1

2

d3

X0

Y0

Z0

Z1

X1

Y1

Z2

X2

Y2

Zn

Xn

Yn

The robot indicatesthis frame skeleton:


57/62


did earlier:

Fr Link Var d a C S C S

01 1 R 1 0 0 90 0 1 C1 S1

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

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

LP Table:


58/62


did earlier:

Ais:

1

2

3

3

1 0 1 0

1 0 1 0

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


59/62


did earlier:

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

C S S C C

S S C S C T

C S

3

3

0

3

1 2 1 1 2 1 2

1 2 1 1 2 1 2

3 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


60/62


did earlier: THE JACOBIAN

0 3 0 1 3 1 2

0 10

Z O O Z O O ZJ

Z Z

The Jacobian is Of This Form:


61/62


did earlier: THE JACOBIAN

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 01 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


62/62

After total Simplification, THE Full

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

JS

C

Diff Motion Jacobian Part1 S06

Documents

Transcript of Diff Motion Jacobian Part1 S06