8/2/2019 Diff Motion Jacobian Part1 S06
1/62
ME 4135Differential Motion and the RobotJacobian
Slide Series 6
R. R. Lindeke, Ph.D.
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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:
8/2/2019 Diff Motion Jacobian Part1 S06
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!
8/2/2019 Diff Motion Jacobian Part1 S06
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/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
9/62
Solving for the differential motion (T)
11
1
0 0 0 1
z y
z x
y x
dxdy
T T Tdz
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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:
8/2/2019 Diff Motion Jacobian Part1 S06
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!
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
14/62
Graphically:
Xn
Yn
X0
Y0
R
Here:Rinit = (3
2 + 52) .5 =5.831 units
init = Atan2(3,5) =1.0304 rad
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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!
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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.
8/2/2019 Diff Motion Jacobian Part1 S06
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!)
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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:
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
33/62
Lets build one from 1stPrinciples
Here is a Spherical Arm:
X0
Y0
Z0
RLets start with onlylinear motion ----
equations arestraight forward!
8/2/2019 Diff Motion Jacobian Part1 S06
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:
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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!)
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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!
8/2/2019 Diff Motion Jacobian Part1 S06
43/62
8/2/2019 Diff Motion Jacobian Part1 S06
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!
8/2/2019 Diff Motion Jacobian Part1 S06
45/62
Building the Linear Jacobian
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
47/62
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 onthe previous slide (and our move only one
and then normalize it method)
8/2/2019 Diff Motion Jacobian Part1 S06
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.
8/2/2019 Diff Motion Jacobian Part1 S06
49/62
Building the Linear Jacobian
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
8/2/2019 Diff Motion Jacobian Part1 S06
50/62
Building the Linear Jacobian
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!
8/2/2019 Diff Motion Jacobian Part1 S06
51/62
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-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
8/2/2019 Diff Motion Jacobian Part1 S06
52/62
Building the Linear Jacobian
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!
8/2/2019 Diff Motion Jacobian Part1 S06
53/62
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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
8/2/2019 Diff Motion Jacobian Part1 S06
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:
8/2/2019 Diff Motion Jacobian Part1 S06
57/62
Lets try this on the Spherical ARM we
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:
8/2/2019 Diff Motion Jacobian Part1 S06
58/62
Lets try this on the Spherical ARM we
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
8/2/2019 Diff Motion Jacobian Part1 S06
59/62
Lets try this on the Spherical ARM we
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
8/2/2019 Diff Motion Jacobian Part1 S06
60/62
Lets try this on the Spherical ARM we
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:
8/2/2019 Diff Motion Jacobian Part1 S06
61/62
Lets try this on the Spherical ARM we
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
8/2/2019 Diff Motion Jacobian Part1 S06
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