me 4135 differential motion and the robot jacobian fall 2012 r. r. lindeke, ph.d
TRANSCRIPT
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
RRRRRRRRRRRRRRRRRRRRRRRRRRRR
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
RRRRRRRRRRRRRRRRRRRRRRRRRRRR
RRRRRRRRRRRRRRRRRRRRRRRRRRRR
RRRRRRRRRRRRRRRRRRRRRRRRRRRR
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.
Building the Linear Jacobian – Revolute Joints
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
Building the Linear Jacobian – Revolute Joints
This is the driver vector given by:
Zi-1 X di-1n
[with a Magnitude equal to joint velocity]
Building the Linear Jacobian – Revolute Joints
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!)
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 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
Lets try this on the Spherical ARM we did earlier:
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:
Lets try this on the Spherical ARM we did earlier:
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
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 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:
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 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
Lets try this on the Spherical ARM we did earlier: THE JACOBIAN
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