ME 4135

Differential Motion and the Robot

Jacobian

Slide Series 6
R. 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 intervals (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
 Kx  i 
vector K: 
K  Ky  j 
 
 K z  k 

 By a general matrix
defined as: Rot ( X , x ) Rot (Y , y ) Rot (Z , z )
These Rotation are given as:

1 0 0 0
0 Cos ( )  Sin( ) 0
Rot ( X ,  x )   x x 
0 Sin( x ) Cos ( x ) 0
 
0 0 0 1
 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:
Cos small  1 Sin small  small
Substituting the Small angle
Approximation:

1 0 0 0
0 1  x 0
Rot ( X ,  x )   
0  x 1 0
 
0 0 0 1

Similarly for Y and Z:

 1 0 y 0 1  z 0 0
 0 1 0 0  1 0 0
Rot (Y ,  y )    Rot ( Z ,  z )   z 
  y 0 1 0 0 0 1 0
   
 0 0 0 1 0 0 0 1
Simplifying the Rotation Matrices
(form their product):

 1  z  y 0
 1  x 0 
Gen.Rot   z 
  y x 1 0
 
 0 0 0 1

Note here: we have neglected higher order

products of the  terms!
 What about Small (general)
Translations?

 We define it as a matrix: 1 0 0 dx 
0 1 0 dy 
Trans (dx, dy, dz )   
0 0 1 dz 
 
0 0 0 1

 1  z  y dx 
General ‘Tiny Motion’ is   x dy 

1
then (including both Rot. Gen _ Movement   z 
and Translation):   y x 1 dz 
 
 0 0 0 1
So using this idea:

 Let’s 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
 Consider Here: T is the original end frame pose
 Substituting for the matrices:
 1  z  y dx 
 1  x dy 
T  T   z  T
  y x 1 dz 
 
 0 0 0 1
Solving for the differential motion (T)

 1  z  y dx 
 1  x dy 
T   z  T T
  y x 1 dz 
 
 0 0 0 1
Factoring T (on the RHS)

 1  z  y dx  1 0 0 0  
    
 
z 1  x dy

0 1 0 0 
 T
T 
   y x 1 dz  0 0 1 0  
   
  0 0 0 1  0 0 0 1  
 Further Simplifying:

 0  z  y dx 
 0  x dy 
T   z  T
  y x 0 dz 
 
 0 0 0 0

We will call this

matrix the del
operator: 
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( )/dx
but now taken with respect to HTM’s!
Lets look into an application:
 Given:
1 0 0 3
0 1 0 5
T0n curr  
0 0 1 0
 
0 0 0 1

 Subject it to 2 simultaneous movements:

– Along X0 (dx) by .0002 units (/unit time)
– About Z0 a Rotation of 0.001rad (/unit
time)
Graphically:

Y0

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

init = Atan2(3,5) =
Xn

R 1.0304 rad


X0
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
Where is the Frame n after one time
step?

 Considering Cos(  )   .9999995 

n   Sin      .000999998
Orientation:
   
 0   0 
  Sin      .000999998
 
o   Cos       .9999995 
 
 0   0 
0 
a  0 
 
1 
After 1 time step, Exact Pose is:

 .9999995 .000999998 0 2.99505 

.000999998 .9999995 0 5.00309 
Tnew   
 0 0 1 0 
 
 0 0 0 1 
Lets Approximate it using this 
operator

 Tnew = Tinit + dT = Tinit + Tinit – the 1st law of differential

calculus
 0 .001 0 .0002  1 0 0 3
 Where: .001 0 0 0  0 1 0 5
dT  Tinit    
 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 
Thus, Tnew is Approximately:

1 0 0 3  0 .001 0 .0048
0 1 0 5  .001 0 0 .003 
Tnew  Tinit  Tinit   
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 
Comparing:

 “Exact”:  .9999995 .000999998 0 2.99505 

.000999998 .9999995 0 5.00309 
Tnew   
 0 0 1 0 
 
 0 0 0 1 

 1 .001 0 2.9952 
 Approximate: .001 1 0 5.003 
Tnew   
 0 0 1 0 
 
 0 0 0 1 

Realistically these are all but equal but using the ‘del’
approximation, but finding it was much easier!
We can (might!) use the ‘del’ approach
to move a robot in space:

 Take a starting POSE (Torig) and 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
n time steps
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):
– dT = T (1)
 However, if the motion is taken w.r.t. another (non-inertia)
space:
– dT = TT (2)
– Here T implies motion w.r.t. itself – a moving frame – but could 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)-1T
Solving for the specific Terms in T

 Positional Change Vector

T
 
dx   d  n  d p n

dy    d  o   d
w.r.t. (any) Tspace:
T
p o
T
dz    d  a   d p a

x  n
T d, n, o & a vectors
are extracts from the
 Angular effects wrt Tspace:
T Matrix
T
 y  o dp is the translation
vector in 
T
 z  a  is the rotational
effects in 
Subbing into a ‘del’ Form:

 0  z
T T
y T
dx 
 T 
 z 0  x
T T
dy 
T
 T
  y T
x 0 T
dz 
 
 0 0 0 0 
An Application of this issue:

Camera
R

TRpart TCamPart
PART
TWCR
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
WC to the robot, so we need part motion “del”
in the robot’s space.
This is a Motion “Mapping” Issue:

 Pa R WC Ca Pa
Pa R  C Pa

 Pa R WC Ca Pa

 Knowns:
 C
 Robot in WC
 Camera in WC
 And of course Part in Camera (But we don’t need it for
now!)
Lets Isolate the “Middle”

 R WC Ca
R  C

 R WC Ca

 To solve for R we make progress from “R to

R” directly (R) and “The long way around”:

R
  TR 1
WC  T  
Cam
WC
Cam
  T
WC  T 
Cam 1 R
WC
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 TWCR 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* TWCCam
Continuing:

 Rewriting, we find that R = “T”-1(Cam) “T”

 R is now shown in the “standard form” for non-

inertial space motion

– the terms: d, n, o & a vectors come from our ‘complex T’
matrix
– the dp 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*TWCR here!
– Its from this “T” product that we extract n, o, a, d vectors
R is given by simplifying:



1st: (TwcCam)-1*TWCR = “T”
Then these Scalars:
R
 
dx   d  n  d p n
R
dy    d  o   d p o
WHERE:

dz    d  a   d
d, n, o & a vectors are R
extracts from the “T” Matrix p a
above

dp is the ‘translation’ vector  x  n

R

in Cam
 y  o
R

 is the vector of ‘rotational

effects’ in Cam  z  a
R
Lets Examine the Jacobian Ideas

 Fundamentally:

D  J Dq
and, If it 'exists' we can define the

Inverse Jacobian as:

1
Dq  J D
In This Model, Ddot & Dq,dot are:
 
x
 
y
 
D   z  ; Cartesian Velocity
 
 x
 y  We state, then, that the
  Jacobian is a mapping
 z 
  tool that relates
 q1  Cartesian Velocities (of
 
q2  the n frame) to the
 
q 
Dq   3  ; Joint Velocity
movement of the
 q4  individual Robot Joints
 
 q5 
 
q 
 6
Lets build one from ‘1st Principles’ –
Here is a Spherical Arm:

Z0

R
Lets start with only
linear motion ----
equations are
 straight forward!

Y0

X0 
 Writing the Position Models:

 Z = R*Sin()
 X = R*Cos()*Cos()
 Y = R*Cos()*Sin()
dz
dt
 R
t 
Sin  R  ( Sin )
t 
 S R  RC 
To find velocity, differentiate
these as seen here: dx
dt
 R
t 
C C  RC  (C )
t    C  
 RC 
 t 

 C C R  RC S   RC S 
dy
dt
 R
t 
C S  RS  (C )
t     S  
 RC 
 t 

 C S R  RS S   RC C 
Writing it as a Matrix:

   
 X    RC S 
 RC S C C 
     
 Y    RC C  RS S S C   

   0 RC S   
Z  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 Frame
Here we could develop an Inverse
Jacobian:

    y '2 x 0   x
   R R '2   
    zx   zy  '
  
      R 2  y
'
 2
R'  R2 R   
  
R R
 
R  x y z  z
   R R R   

R  x  y
' 2

2 .5
It was formed by taking
the partial derivatives of
the IKS equations
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 separations as 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
Separation Leads to:

 A Cartesian Velocity  
Term V0n:  x
 

  0  J v  Dq
n
y V
 
z
 
 An Angular Velocity  x 
Term 0n:     n  J  D
 y 0  q

 z 

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

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 from
angular velocity effects
 To address the problem we will consider
moving a single joint at a time (using DH
separation ideas!)
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:
  ki1 qi  Zi1 qi
i
i 1

– 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 rotoation from it on all subsequent
frames
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!

  ki1 qi  Zi1 qi
i
i 1

 Since Zi-1 is the active direction in a Frame of the model, we

really 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, this
Ai’s products 3rd column is the effect of Joint i as defined in
the (common) base space (note, the ‘qdot’ term is the rate of
rotation of the given joint)
So the Angular Velocity then is:

n
   i  Z i 1 qi
n
0 Note Zi-1 for
Jointi – per DH
i 1
algorithm!
i  1 (revolute joint)
i  0 (prismatic joint)
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
Going Back to the Spherical Device:

 0n  1  Z 0    1  Z1    0

we state:  0  J D q
n

Therefore:

J   Z 0 Z1 0  Notice: 3 columns since

we have 3 joints!
and (always):

0  Here, Z1 depends
Z 0  0  on the Frame
 
1  Skeleton drawn!
Building the Linear Jacobian

 It too will depend on the motion associated

with 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!
Building the Linear Jacobian

n
d 0 is linear velocity of the end frame wrt the base
n
  d0
n
 
d 0   
n
 qi 
i 1 
qi  
 d n
 Jv   
0

 qi i 1 to n
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 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:
A0 * … * Ai-1
 This is as expected based on the model on
the previous slide (and our “move only one
and then normalize it” method)
Building the Linear Jacobian for
Revolute Joints

 This is a dicer problem,

 but then, remembering the idea of prismatic joints on angular
velocity …
 But no that won’t work here – just because its a
rotation, and it changes orientation of the end –
revolute motion also does have a linear contribution
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.
 Building the Linear Jacobian –
Revolute Joints

Using this model we would

expect that a rotation i would
‘lever 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 to
Joint velocity
Building the Linear Jacobian –
Revolute Joints

This is the
directional
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!
Building the Linear Jacobian –
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 = d0n – d0i-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 let’s rewrite equation 3 as: di-1n = On – Oi-1
Building the Linear Jacobian –
Revolute Joints

 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 T0i-1 (A1*… *Ai-1)
 Oi-1 is 4th col. of the T0i-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!
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
 And notice: select Zi-1 and Oi-1 for the frame before the
current joint column
Combining Both Halves of the
Jacobian:

 For Revolute Joints:



 J v   Zi 1  On  Oi 1
J  
 

 J   
 Zi 1 
 For Prismatic Joints:

 J v   Zi 1 
J   
 J   0 
What is the Form of the Jacobian?

 Robot is: (PPRRRR) – a cylindrical machine

with a spherical wrist:

Z Z1 Z 2   O6  O2  Z 3   O6  O3  Z 4   O6  O4  Z 5   O6  O5  
J  0 
0 0 Z2 Z3 Z4 Z5 

 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
Lets try this on the Spherical ARM we
did earlier:

X2
Y1
d3 Z0
Z0

Z1 Y2
2 Z2
Y0 Y0
X1
X0
 X0
1
Xn
The robot indicates
this frame skeleton: Yn

Zn
Lets try this on the Spherical ARM we
did earlier:

LP Table:
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
Lets try this on the Spherical ARM we
did earlier:

 Ai’s: C1 0 S1 0
 S1 0 C1 0
A1   
0 1 0 0
 
0 0 0 1
S 2 0 C2 0
 C2 0 S2 0
A2   
 0 1 0 0
 
 0 0 0 1
1 0 0 0
0 1 0 0
A3   
0 0 1 d3 
 
0 0 0 1
Lets try this on the Spherical ARM we
did earlier:

 T1 = A1!  C1S 2 S1 C1C 2 0

  S1S 2 C1 S1C 2 0
 T2 = A1 * A2 T2   
 C2 0 S2 0
 
 0 0 0 1

 C1S 2 S1 C1C 2 d 3C1C 2 

n
 T0 = T3 = A1*A2*A3   S1S 2 C1 S1C 2 d S1C 2 
T0n  T 3   3 
 C2 0 S2 d3S 2 
 
 0 0 0 1 
Lets try this on the Spherical ARM we
did earlier: THE JACOBIAN

The Jacobian is Of This Form:

 Z 0   O3  O0  Z1   O3  O1  Z 2 
J  
 Z0 Z1 0 
Lets try this on the Spherical ARM we
did earlier: THE JACOBIAN

 Here: 0   d3C1C 2  0 

 
Z 0   O3  O0   0    d3S1C 2   0  
     

1    d3 S 2  0 
 d3S1C 2 
 d C1C 2 
 3 
 0 

 S1    d3C1C 2  0  
 
Z1   O3  O1    C1    d3S1C 2   0   
     
 0    d3 S 2  0  


 d3C1S 2 
 d S1S 2 
 3 
 d3C 2 
After total Simplification, THE Full
JACOBIAN is:

 d3 S1C 2 d3C1S 2 C1C 2 

 d C1C 2 d S1S 2 S1C 2 
 3 3 
 0 d3C 2 S2 
J  
 0 S1 0 
 0 C1 0 
 
 1 0 0 