Vous êtes sur la page 1sur 26

' $

Today's Agenda

 The Physics of Angular Velocities


 The Math of Angular Velocities
 The Jacobian
 Singularities
 Static Forces

This material corresponds to Chapter 5 in Craig's


book.

& %
' $
Di erentiation of Position Vectors
We study the position of the robot as it varies
with time. A natural place to start is the
de nition of the time derivative of a position
vector.
B VQ = dQ = limt!0 B Q(t+t);B Q(t)
dt t
Here we are di ferentiating the vector Q relative
to frame fB g. For example if Q is not changing in
time relative to frame fB g then this velocity will
come out to be 0.

& %
' $
Like any vector the velocity vector can be written
wrt any frame. The above velocity vector written
wrt frame fAg can be written
A (B VQ ) = A dQ
dt
For simplicity B (B VQ ) = B VQ
Also
A (B VQ ) = A R B VQ
B
We will also use a special notation for the velocity
of the origin of a frame fC g relative to some
understood universal frame fU g
vC = U VCORG

& %
' The Angular Velocity Vector
$
Denoted by the symbol
- it is attribute of a
body rather than a point. Since we attach a
frame to a body we can think of angular velocity
as describing rotational motion of a frame.
A
B describes the angular velocity of frame fB g
relative to frame fAg.
As before we introduce a shorthand for angular
velocites wrt to some universal frame fU g
We write
!C = U
C
{B}
{A}

&
where !C is the angular velocity of fC g wrt the
universal frame fU g.
%
' $
Linear Velocity
Given two frames fAg and fB g. Imagine that
fAg is xed and fBg is translating wrt fAg with
some velocity A VBORG .
Also imagine that fB g is rotated wrt fAg by
some xed rotation matrix AB R.
Given a point Q whose velocity B VQ is known in
frame fB g, what is its velocity in frame fAg ?

{B}

{A} B
Q

AP
BORG

& %
A VQ = A VBORG + A R B VQ
B
' $
Rotational Velocity
Given two frames fAg and fB g. Imagine that
fAg is xed and fBg is rotating wrt fAg with
some velocity A
B .
Also imagine that fB g is not translated wrt fAg
at all - i.e. their origins coincide for all time.
Given a point Q whose postion B Q is known in
frame fB g, what is its velocity in frame fAg ?

BQ

{B}

{A}

& %
' $
|Q|sin O

t Q

Q(t) Q(t + t)

jQj = (jA Qsin()j)(jA


B jt)
A VQ = A
B  A Q
In general the vector Q may also be moving in
frame fB g so we add this component
A VQ = A (B VQ ) + A
B  A Q
Which gives us
A VQ = A R B VQ + A
B  A R A Q
B B
We can now allow for the fact that in general the

& %
origins of the two frames may not coincide by
adding on A VBORG resulting in
A VQ = A VBORG + A R B VQ + A
B  A R A Q
B B
' $
A Property of an Orthonormal Matrix
Given an orthonormal matrix R and the identity
matrix I we can write
RRT = I
Di erentiating this we get
_ T + RR_ T = O
RR
Rewrite this as
_ TT = O
_RRT + RR
And set S = RR_ T to write the above equation as
S + ST = O
Clearly S is skew-symmetric and is given by

& %
S = RR _ ;1 .
_ T = RR
' So ??
Consider a xed vector B P unchanging wrt frame
$
fBg - its description in frame fAg is given by
AP = A R B P
B
If frame fB g is rotating wrt fAg i.e. the matrix
A R is time-varying then we have
B
A P_ = A_R B P
B
Or
A VP = A_R B P
B
Now substitute for B P to get
A VP = A_R A R;1 B P
B B
Or
A VP = A S A P
B

& %
Where S is a skew-symmetric matrix which we
will call the angular velocity matrix - it is the
angular velocity matrix associated with the
rotation matrix R.
'
Skew-symmetric Matrices and the Cross Product
$
Now assign elements to the skew-symmetric
matrix S as follows
2 3
0 ;
z
y
6 7
S = 4
z 0 ;
x 75
6
;
y
x 0
and de ne the 3  1 column vector

= [
x
y
z ]T
Now if P is any vector and  denotes the vector
cross product then one can see that
SP =
 P
The 3  1 vector
which corresponds to the 3  3
angular velocity matrix is called the angular
velocity vector.

& %
Now we can rewrite the relation A VP = AB S A P as
A VP = A
B  A P
This is the identical fact that we derived using a
geometrical argument previously.
' $
Velocity Propagation from Link to Link
i+1v
iv i+1 Zi
i
i+1
Zi w
i+1
Yi Yi+1
i
P i+1

iw
i

Xi Xi+1

Starting from the base we compute the velocity of


each link by adding (vectorially) to the velocity of
the previous link.
The velocity of link i + 1 is the velocity of link i
plus whatever new velocity is to be added on due
to the motion of i + 1 relative to i.
Rotations can be added provided they share a

& %
common reference frame, thus
i !i+1 = i !i + ii+1 R _i+1 i+1 Zi^+1
' $
The rotation that is being 'added' on is about the
axis Zi^+1 since that is our convention - this
means that formally the rotation being aded on is
a vector given by
_i+1 i+1 Zi^+1 = i+1 [0 0 _i+1 ]T
We then multiply this vector by the matrix ii+1 R
to express it in the frame fig and then add it to
the existing angular velocity i !i to get i !i+1 .
Now we multiply each side by ii+1 R to get
i+1 !i+1 = i+1 R i !i + _i+1 i+1 Zi^+1
i

& %
' $
For the linear velocity transformation we use the
equation derived earlier
A VQ = A VBORG + A R B VQ + A
B  A R A Q
B B
and note that i Pi+1 is constant in frame fig - this
merely means that the link is of constant lenght
over time.
So we get
i vi+1 = i vi + i !i  i Pi+1
Now premultiply both sides with i+1 Ri to get
i+1 vi+1 = i+1 Ri (i vi + i !i  i Pi+1 )

Applying these equations from link to link


starting with the base we can nd N !N and N vN .

& %
The corresponding equations if the joint i + 1 is
prismatic are left as an exercise (see homework).
' A Two-link Example
$
X

V
3
L2
Y

.
O2
.
O1

L1

What is 3 v3 as a function of _1 and _2 ? Also


what is 0 v3 ?
Lets use the propagation technique. First we
compute all the forward transformations as
2 3
c1 ;s 1 0 0
0 T = 4 s1 c1 0 0
5
1 0 0 1 0

& %
0 0 0 1
2 3
c2 ;s2 0 L1
1 T = 4 s2 c2 0 0
5
2 0 0 1 0
0 0 0 1
' $
2 3
1 0 0 L2
2
3
T =4 0
0
1
0
0
1
0
0
5
0 0 0 1

Now we propagate
 0

1 !1 = 0

_
1
 0

1 v1 = 0
0
 0

2 !2 = 0
1 + _2
_

 c s2 0   0 
2
2v = ; s c2 0 L1 _1
2 2
0 0 1 0

!3 = 2 !2
3

 L1 s2 _1 

& %
3v = L1 c2 _1 + L2 (_1 + _2 )
3
0
' $
And now to nd 0 v3 we use 0 v3 = 0 R3 3 v3 .
0 R3 is given by
2 3
c12 ;s12 0
0 R3 = 0 R1 1 R2 2 R3 6 7
= 64 s12 c12 0 7
5
0 0 1
And thus we have
2 3
;L1 s1 _1 ; L2 s12 (_1 + _2 )
0 v3 = 6
6 L1 c1 _1 + L2 c12 (_1 + _2 )
7
7
4 5
0

& %
' $
The Jacobian
Consider a vector function f : Rn ! Rm . This
function takes a vector of dimension n (say X )
and maps it to a vector of dimension m (say Y ).
Y = f (X )
We know from calculus that the total di erential
dyi can be obtained by the chain rule as follows
Pn @yi
dyi = j=1 @xj dxj
Or in vector notation we can write
dY = @X@Y dX

and dividing both sides by a di erential time


element we may write

& %
Y_ = J (X ) X_
' $
At every instant X changes and so does J (X ),
but instantaneously J provides a linear
transformation between the rate of change of the
input and the rate of change of the output.
In robotics generally we use the Jacobian to stand
for the mapping from rates of change of joint
angles to rates of change of cartesian velocities of
the arm tip.
For a six-jointed robot the Jacobian is 6  6, the
joint rate vector _ is 6  1 and the Cartesian
velocity vector V is 6  1 also. The Cartesian
velocity vector V is the 3  1 velocity vector and
3  1 rotational velocity vector stacked together
as follows
2 3
0v

& %
0V = 4 5
0!
' Example
For the two link arm we just looked at we gured
$
out that  
L1 s2 _1
3 v3 = L1 c2 _1 + L2 (_1 + _2 )
0

and  ;L s _ ; L s (_ + _ ) 
1 1 1 2 12 1 2
0v = L1 c1 1 + L2 c12 (1 + 2 )
_ _ _
3
0

Factoring both of these we see that the Jacobian


is 2 3
3 J () = 4 L1 s2 0 5
L1 c2 + L2 L2
Or written in frame f0g it is
2 3
0 J () = 4 ;L1 s1 ; L2 s12 ;L2 s12 5
;L1c1 + L2c12 L2 c12

& %
Here we have written the matrix which relates the
joint rates to the end e ector velocity. We can
also consider the 3  2 Jacobian which would
include the angular velocity of the end-e ector.
' Singularities
$
The inverse of the Jacobian relates cartesian
velocites to joint rates as follows:
_ = J ;1 () V
The values of  where J ;1 ceases to exist are
called singularites. These points are given by the
equation
jJ j = 0
At these joint con gurations the manipulator has
lost one or more degrees of freedom as viewed
from the Cartesian workspace. This implies that
in this (singular) con guration there is some
direction in Cartesian space along which it is
impossible to move the manipulator tip no matter

& %
what joint rates are used.
There are two kinds of singularities - workspace
boundary and workspace interior.
' Static Forces
$
Imagine a manipulator in static balance exerting
a force on its environment using its tip. what are
the torques (forces) required at the joints to
maintain this equilibrium ?
In this case we can back propagate the (known)
force at the tip through each joint back to the
base.
Denote fi to be the force exerted on link i by link
i ; 1 and ni to be the torque exerted on link i by
link i ; 1.
Since link i is at equilibrium we have a force
balance on it
i fi ; i fi+1 = 0
Since it is also at rotational equilibrium we

& %
balance torques on link i about the origin of
frame fig
i ni ; i ni+1 ; i Pi+1  i fi+1 = 0
'
Rewriting these we have
$
i fi = i fi+1
and
i ni = i ni+1 + i Pi+1  i fi+1
And by using the rotation matrix we get
i fi = i R i+1 fi+1
i+1
and
i ni = i R i+1 ni+1 + i Pi+1  i fi
i+1
So to resist motion the joint must provide a
torque given by
i = i ni T i Z^i

& %
or if the joint is prismatic a force given by
i = i fi T i Z^i
' Example
Consider the same old two link manipulator
$
applying a force 3 F at its tip.
We need to nd the required joint torques as a
function of the robot con guration and of the
applied force.
We propagate 3 F back from the tip to the base as
follows:   fx
2 f2 = fy
0
 f   
x 0
2n = L X^  fy = 0
2 2 2
0 L2 f y
 c ;s2  f   c f ;s f 
1f =
2 0 x 2 x 2 y
1 s2 c2 0 fy = s2 fx + c2 fy
0 0 1 0 0
 0
  0

1 n1 = 0 + L1 X^ 1  1 f1 = 0
L2 fy L1 s2 fx + L1 c2 fy + L2 fy

& %
Therefore we get
1 = L1 s2 fx + (L2 + L1 c2 )fy
and
2 = L2 fy
' $
Which we can rewrite in matrix form as
2 32 3
L s L +L c f
 = 4 1 2 2 1 2 54 x 5
0 L2 fy
Or
 = JT F
We will show that this identity holds in general
for a manipulator at static equilibrium.

& %
' $
The Force-Torque Relation
To show  = J T F formally we use the principle
of virtual work.
We imagine a manipulator at equilibrium and
make in nitesimally small changes to the joint
variables and the end-e ector position.
This results in an in nitesimal amount of work
being done. Since work (energy) is a scalar
invariant of the system it is equal whether it is
calculated in the Cartesian domain or in the joint
domain. i.e.
F  X =   
This can be rewritten as

& %
F T X =  T 
' $
The Jacobian is de ned by
X = J
Substituting we have
F T J =  T 
and since this holds for all  it gives us
FT J = T
By transposing both sides we get the result which
we showed for the two-link planar arm
 = JT F

& %