Ch. 4: Velocity Kinematics Ch. 4: Velocity Kinematics: Tü-Eef Dept. of Control Engineering

İTÜ-EEF Dept.
Of Control Engineering
Ch. 4: Velocity Kinematics
KON 318E : Introduction to Robotics 1

İTÜ-EEF Dept. Of Control Engineering
Velocity Kinematics
• Now we know how to relate the end-effector position and orientation
to the joint variables
• Let’s relate end-effector linear and angular velocities with the joint
velocities
• First we will discuss angular velocities about a fixed axis
• Second we discuss angular velocities about arbitrary (moving) axes
• We will then introduce the Jacobian
– Instantaneous transformation between a vector in Rn representing joint
velocities to a vector in R6 representing the linear and angular velocities
of the end-effector
• Finally, we use the Jacobian to discuss numerous aspects of
manipulators:
– Singular configurations
– Dynamics
– Joint/end-effector forces and torques

Angular velocity: fixed axis

• When a rigid body rotates about a fixed
axis, every point moves in a circle
– Let k represent the fixed axis of rotation,
then the angular velocity is:
ω = θ&k
– The velocity of any point on a rigid body
due to this angular velocity is:
v =ω×r
r : vector from the axis
of rotation to the point
dθ
θ=
&
dt
• When a rigid body rotates about a
fixed axis, every point moves in a
circle
Angular velocity: arbitrary axis

( To develop more general representation of angular velocity )
• Skew-symmetric matrices
– Definition: a matrix S is skew symmetric if: S T + S = 0 ; ST = −S
– Let the elements of S be denoted sij, then by definition:
sij = −s ji for i ≠ j
sij = 0 for i = j
– Thus there are only three independent entries in a skew symmetric matrix
 0 − s3 s2 
S =  s3 0 − s1 
 − s2 s1 0 
– Now we can use S as an operator for a vector a = [ax ay az]T

 0 − az ay 
 
S (a ) =  a z 0 − ax 
− a y ax 0 


• Example:
– Let i, j, k be defined as follows:
1  0  0 
i = 0, j = 1, k = 0
0 0 1
– Then we can define the skew symmetric matrices S(i), S(j), S(k):
0 0 0   0 0 1 0 − 1 0 
S (i ) = 0 0 − 1, S ( j ) =  0 0 0, S (k ) = 1 0 0,
0 1 0  − 1 0 0 0 0 0


• Properties of skew-symmetric matrices
1. The operator S is linear
S (αa + βb ) = αS (a ) + βS (b ), ∀a, b ∈ R 3 ,α , β ∈ R
2. The operator S is known as the cross product operator
– This can be seen by the definition of the cross product:
S (a )p = a × p, ∀a, p ∈ R 3
 a1   b1  a2 b3 − a3 b2 
a = a2 , b = b2  ⇒ a × b =  a3 b1 − a1b3 
a3  b3   a1b2 − a2 b1 
 0 − a3 a2   b1  a2 b3 − a3 b2 
S (a )b =  a3 0 − a1  b2  =  a3 b1 − a1b3 
− a2 a1 0  b3   a1b2 − a2 b1 

• Properties of skew-symmetric matrices

3. For R ∈ SO (3), a ∈ R3
RS (a )R T = S (Ra )
4. For any S ∈ so(n), x ∈Rn
x T Sx = 0


• Derivative of a rotation matrix
– let R be an arbitrary rotation matrix which is a function of a single variable θ
• R(θ)R(θ)T=I
• Differentiating both sides (with respect to θ) gives:
d   d T
R
 dθ  R (θ )T
+ R (θ ) dθ R  = 0
   
• Now define the matrix S as:
d 
S =  R  R (θ )
T
 dθ 
T
 d  T  d 
• Then ST is: S =   R R (θ )  = R (θ ) R T 
T
  dθ    dθ 
• Therefore S + ST = 0 and
d 
SR (θ ) =  R  R (θ ) R (θ ) =
T d
R
 dθ  dθ


• Example: let R=Rx,θ, then using the previous results we have:
0 0 0  1 0 0 
d 
S= R  R (θ ) = 0 − sin θ − cos θ  0 cos θ sin θ 
T
 dθ 
0 cos θ − sin θ  0 − sin θ cos θ 
0 0 0 
= 0 0 − 1 = S (i )
0 1 0 


• Now consider that we have an angular velocity about an arbitrary axis
• Further, let R = R(t)
• Now the time derivative of R is:
R& (t ) = S (ω (t ))R (t )
• Where ω(t) is the angular velocity of the rotating frame

• To show this, consider a point p fixed to a moving frame o1:
d 0 d 0 1
dt
p =
dt
(
R1 p ) p 0 = R10 p1
= R& 10 p1 + R10 p& 1 = 0 , Since p1 is a fixed

vector in frame o1
= R& 0 p1
1
= S (ω (t ))R10 p1
= ω (t ) × R10 p1
= ω (t ) × p 0

Addition of angular velocities

• For most manipulators we will want to find the angular velocity of one frame
due to the rotations of multiple frames
– We assume that there are no translational components: all coordinate frames
have coincident origins
– Consider three frames: o0, o1, o2: 0
R2 (t ) = R1 (t )R2 (t )
0 1
• To illustrate how the rotation of

multiple frames is determined
by the rotations of the
individual frames, take the
derivative of both sides
R2 = R1 R2 + R1 R2
& 0 & 0 1 0 &1
(A) (B) (C)


– By our previous convention:
( )
R& 20 = S ω00, 2 R20 (A)
– Where ω00,2 is the angular velocity corresponding to a rotation of R20 in the
inertial frame
(B) ( ) ( )
R& 10R21 = S ω00,1 R10R21 = S ω00,1 R20
• Now the first term can be derived as follows:
( ) ( )
R& 10R21 = S ω00,1 R10R21 = S ω00,1 R20
– Where ω0,10 is the angular velocity corresponding to a rotation of R10 in the
inertial frame


• The second term is derived using the properties of so(n)
(C) (
R10R& 21 = R10S ω11,2 R21 ) from our definition
= R S (ω
0 1
)(R ) R R
0 T 0 1
1 1,2 1 1 2
RT R = I
= S (R ω0
1
1
1,2 )R R
0
1
1
2
= S (R ω0
1
1
1,2 )R
0
2 because RS (a )R T = S (Ra )
• Thus combining these results: ( A= B+C )
( ) ( ) ( )
S ω00, 2 R20 = S ω00,1 R20 + S R10ω11, 2 R20
= [S (ω ) + S (R ω )]R
0
0 ,1
0
1
1
1, 2
0
2


• Since :
S (a + b ) = S (a ) + S (b ) ω00,2 = ω00,1 + R10ω11,2

• “Therefore angular velocities can be added once they are projected
into the same coordinate frame.”
• This can be extended to calculate the angular velocity for an n-link
manipulator:
– Suppose we have an n-link manipulator whose coordinate frames are related as
follows:
Rn0 = R10R21 ⋅ ⋅ ⋅ Rnn −1
– Now we want to find the rotation of the nth frame in the inertial frame:
( )
R& n0 = S ω00,n Rn0
– We can define the angular velocity of the tool frame in the inertial frame:
ω00,n = ω00,1 + R10ω11,2 + R20ω22,3 + R30ω33,4 + ... + Rn0−1ωnn−−11,n

= ω00,1 + ω10,2 + ω20,3 + ω30,4 + ... + ωn0−1,n
Linear velocities
• The linear velocity of any point on a rigid body is the sum of the
linear velocity of the rigid body and the velocity of the particle due to
rotation of the rigid body
– First, the position of a point p attached to a rigid body is:
p 0 = Rp1 + o
– Where o is the origin of the o1 frame expressed in the inertial frame
– To find the velocity, take the derivative as follows:
p& 0 = R& p1 + Rp& 1 + o&

= S (ω )Rp1 + o& = 0, we assume that p is fixed
with respect to the rigid body
= ω × Rp1 + v
– Where v is the velocity of the rigid body in the inertial frame
– If the point p is moving relative to the rigid body, then we also need to
take this in consideration

The Jacobian
• Now we are ready to describe the relationship between the joint velocities
and the end effector velocities.
• Assume that we have an n-link manipulator with joint variables q1, q2, …, qn
– Our homogeneous transformation matrix that defines the position and orientation
of the end-effector in the inertial frame is:
Rn0 (q ) on0 (q )
T =
n
o

 0 1 
– We can call the angular velocity of the tool frame ω0,n0 and:
( ) ( )
S ω00,n = R& n0 Rn0
T
– Call the linear velocity of the end-effector:
v n0 = o& n0

The Jacobian
• Therefore, we want to come up with the following mappings:
v n0 = Jv q&
ξ = J q&
ωn0 = Jω q&
– Thus Jv and Jω are 3xn matrices
• we can combine these into the following:
– where:
 vn0   Jv 
ξ =  0 ; J =  
ω n   Jω 
• J is called the Jacobian
– 6xn where n is the number of joints

Deriving Jω:
If the ith joint is revolute :
• ith Joint variable : qi → θi
• The axis of rotation of joint i : zi-1
• Angular velocity of link i relative to frame oi-1 : ωii-1
ω i −1
= q& zi −1
= q&i k , k = [0 0 1]
T
i −1,i i i −1
If the ith joint is prismatic :
• the angular velocity of frame oi relative to frame oi-1 is zero

Deriving Jω:
• Thus, forming the equivalent angular velocity of the tool frame with
respect to the base frame:
ω00,n = ω00,1 + R10ω11, 2 + R20ω22,3 + R30ω33, 4 + ... + Rn0−1ωnn−−11,n
= ρ1q&1kˆ + ρ 2 q& 2 R10 kˆ + ρ 3 q&3 R20 kˆ + ρ 4 q& 4 R30 kˆ + ... + ρ n q& n Rn0−1kˆ
n
= ∑ ρ i q&i zi0−1
i =1
Where the term ρi determines if joint i is revolute (ρi =1) or prismatic (ρi =0)
• Now Jω can simply be written as follows:
 vn0 = J v q& 
[
J ω = ρ1 z00 M ρ 2 z10 M L M ρ n z n0−1 ]  0
 ω = J q& 

 n ω 
– There are n columns, each is 3x1, thus Jω is 3xn

Deriving Jv
• Linear velocity of the end effector:
n
∂o 0
o& n0 = ∑ n q& i
i =1 ∂q i
• Therefore we can simply write the ith column of Jv as:

∂on0
Jv i =
∂qi
• However, the linear velocity of the end effector can be due to the motion of
revolute and/or prismatic joints
• Thus the end-effector velocity is a linear combination of the velocity due to
the motion of each joint
– This allows us to examine the end-effector velocity due to the motion of either a
revolute or prismatic joints

Deriving Jv
• End-effector velocity due to prismatic joints
– Assume all joints are fixed other than the prismatic joint di
– The motion of the end-effector is pure translation along zi-1
0
o& n0 = d& i Ri0−1 0 = d& i zi0−1
1
– Therefore, we can write the ith column of the Jacobian:
Jv i = z 0
i −1

Deriving Jv
• End-effector velocity due to revolute joints
– Assume all joints are fixed other than the revolute joint θi
– The motion of the end-effector is given by:
o& n0 = ωi0−1,i × r = θ&i zi0−1 × r

– Where the term r is the distance from the
tool frame on to the frame oi-1
o&n0 = θ&i zi0−1 × (on − oi −1 )

– Thus we can write the ith column of Jv:
Jv i = zi0−1 × (on − oi −1 )
The complete Jacobian

• The ith column of Jv is given by:
zi −1 × (on − oi −1 ) for i revolute

Jv i = 
 zi −1 for i prismatic
• ,The ith column of Jω is given by:
zi −1 for i revolute

J ωi =
 0 for i prismatic

The complete Jacobian

p.s :
• Computation of Jacobian is simple for many
manipulators since many of quantaties are available
once the forward kinematic model is found.
• Only quantaties needed to compute J :
• Unit vectors : zi
• Coordinates of origins : o1, o2 , ….. on
 n1 s1 a1 p1  oi
n s2 a2 p2 
Ti 0 =  2
 n3 s3 a3 p3  zi
 
0 0 0 1
Example: two-link planar manipulator

• Calculate J for the following manipulator:
– Two joint angles, thus J is 6x2
z00 × (o2 − o0 ) z10 × (o2 − o1 )

J (q ) =  0 0 
 z0 z1 
– Where:
0 a1c1  a1c1 + a2c12  0

o0 = 0, o1 = a1s1 , o2 = a1s1 + a2s12  z00 = z10 = 0
0  0   0   1
− a1s1 − a2s12 − a2s12 

 ac +a c a2c12 
 1 1 2 12
 0 0 
J (q ) =  
 0 0 
 0 0 
 
 1 1 
Example: velocity of an arbitrary point

• We can also use the Jacobian to calculate the velocity of any arbitrary point
on the manipulator
z00 × (oc − o0 ) z10 × (oc − o1 ) 0

J (q ) =  0 0 
 z0 z1 0 
– This is identical to placing the tool

frame at any point of the manipulator

Example: Stanford manipulator

• The configuration of the Stanford
manipulator allows us to make the
following simplifications:
zi −1 × (o6 − oi −1 )
Ji =  , i = 1,2
 zi −1 
 z2 
J3 =  
0
zi −1 × (o6 − o )
Ji =   , i = 4,5,6
 zi −1 
• Where o is the common origin of the o3,
o4, and o5 frames


• From the forward kinematics of the Stanford manipulator, we calculated the
homogeneous transformations for each joint:
c1 0 − s1 0 c 2 0 s2 0 1 0 0 0
s 0 c1 0 s 0 − c2 0  0 1 0 0 
A1 =  1 , A2 =  2 , A3 = 
0 − 1 0 0 0 1 0 d2  0 0 1 d3 
     
0 0 0 1 0 0 0 1 0 0 0 1
c 4 0 − s4 0 c5 0 s5 0 c6 − s6 0 0
s 0 c4 0 s 0 − c5 0 s c6 0 0 
A4 =  4 , A5 =  5 , A6 =  6
0 −1 0 0 0 −1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1


• To complete the derivation of the Jacobian, we need the following
quantities: z0, z1, … , z5, o0 , o1, o3 (= o4 =o5 ), o6
– o3 is o and o0 = [0 0 0]T
• We determine these quantities by noting the construction of the T matrices
– oj is the first three elements of the last column of Tj0
– zj is Rj0k, or equivalently, the first three elements of the third column of Tj0
• Thus we can calculate the Jacobian by first determining the Tj0
– Thus the zi terms are given as follows:
0  − s1  c1s2  c1s2 
z0 = 0, z1 =  c1 , z 2 =  s1s2 , z3 =  s1s2 ,
1  0   c2   c2 
 − c1c2 s4 − s1c4  c1c2 c4 s5 − s1s4 s5 + c1s2 c5 

z 4 = − s1c2 s4 + c1c4 , z5 =  s1c2 c4 s5 + c1s4 s5 + s1s2 c5 
 s2 s4   − s2 c4 s5 + c2 c5 

• And the oi terms are given as:
0 0 c1s2d 3 − s1d 2  c1s2d 3 − s1d 2 + d 6 (c1c 2c 4s5 + c1c5s2 − s1s 4s5 )
o0 = 0, o1 =  0 , o3 = s1s2d 3 + c1d 2 , o6 = s1s2d 3 − c1d 2 + d 6 (c1s4s5 + c 2c 4s1s5 + c5s1s2 )
0 d 2   c 2d 3   c 2d 3 + d 6 (c 2c5 − c 4s2s5 ) 
• Finally, the Jacobian can be assembled as follows:
− d y   c1d z  c1s2   s1s2 (d z − o3,z ) + c 2 (d y − o3,y ) 

d   s1d z  s s  − c s (d − o ) + c (d − o )
 x     1 2  1 1 z 3 ,z 2 x 3, x 
 0  − s1d y − c1d x   c2   − c1c 2s 4 − s1c 4 

J1 =  , J 2 =  , J 3 =  , J 4 =  
 0   − s1   0   s s
2 4 
 0   c1   0   0 
       
 1   0   0   0 


• Finally, the Jacobian can be assembled as follows:
 (− s1c2s4 + c1c 4 )(d z − o3,z ) − s2s4 (d y − o3,y ) 


 (− c1c 2 s 4 + s1c 4 )(d z − o 3, z ) + s 2 s 4 (d x − o 3, x ) 

(− c1c 2s 4 − s1c 4 )(d y − o3,y ) + (s1c 2s 4 − c1c 4 )(d x − o3,x )
J5 =  
 − c1c 2c 4 − s1c 4 
 s2s 4 
 
 0 
 (s1c 2c 4s5 + c1s 4s5 + s1s2c5 )(d y − o3,y ) + (s2c 4s5 − c 2c 5 )(d y − o3,y ) 
− (c c c s − s s s + c s c )(d − o ) + (s c s − c c )(d − o )
 1 2 4 5 1 4 5 1 2 5 z 3,z 2 4 5 2 5 x 3,x 
 c1c 2c 4s5 − s1s 4s5 + c1s2c5 

J6 =  
 s1c 2c 4s5 + c1s 4s5 + s1s2c 
 − s 2 c 4 s5 + c 2 c 5 
 
 0 

Example: SCARA manipulator

• Jacobian will be a 6x4 matrix
z00 × (o4 − o0 ) z10 × (o4 − o1 ) z20 z30 × (o4 − o3 )

J= 
 z00 z10 0 z30 
z00 × (o4 − o0 ) z10 × (o4 − o1 ) z20 0
= 
 z00 z10 0 z30 
• Thus we will need to determine the following quantities: z0, z1, … , z3, o0, o1,
o2, o4
– Since all the joint axes are parallel, we can see the following:
z00 = z10 = kˆ, z20 = z30 = −kˆ
– From the homogeneous transformation matrices we can determine the origins of

the coordinate frames

Example: SCARA manipulator

• Thus o0, o1, o2, o4 are given by:
0 a1c1  a1c1 + a2c12 

o0 = 0, o1 = a1s1 , o4 = a1s1 + a2s12 
0  0   d 3 − d 4 
• Finally, we can assemble the Jacobian:
− a1s1 − a2s12 − a2s12 0 0

 ac +a c a2c12 0 0 
 1 1 2 12
 0 0 −1 0 
J= 
 0 0 0 0
 0 0 0 0
 
 1 1 0 − 1

Singularities
• We can now derive the Jacobian as a mapping given by the following:

ξ = J (q )q&
• This means that the columns of J form a basis for the space of possible
end effector velocities
• Thus, for the end effector to be able to achieve any arbitrary body
velocity ξ, J must have rank 6
• We know that J is 6xn and that:
for A ∈ R m×n , rank (A ) ≤ min(m, n )
Thus, rank ( J ) ≤ min ( 6, n )

• For example, for the two link planar manipulator, rank (J ) ≤ 2
• For example, for the Stanford manipulator, rank (J ) ≤ 6
• Note that the columns the Jacobian of a kinematically redundant

manipulator are never linearly independent

Singularities
• But the rank of the Jacobian is not necessarily constant… it will

of course depend upon the configuration
• Definition: we say that any configuration in which the rank of J
is less than its maximum is a singular configuration
– i.e. any configuration that causes J to lose rank is a singular
configuration
• Characteristics of singularities:
– At a singularity, motion in some directions will not be possible
– At and near singularities, bounded end effector velocities would
require unbounded joint velocities
– At and near singularities, bounded joint torques may produce
unbounded end effector forces and torques
– Singularities often occur along the workspace boundary (i.e. when
the arm is fully extended)

Singularities
• How do we determine singularities?

– Simple: construct the Jacobian and observe when it will lose rank
• EX: two link manipulator
– Previously, we found J to be:
− a1s1 − a2s12 − a2s12 
 ac +a c a2c12 
 1 1 2 12
 0 0 
J (q ) =  
 0 0 
 0 0 
 
 1 1 
– This loses rank if we can find some α such that:
J1 = αJ 2 for α ∈ R

Singularities
• This is equivalent to the following:

a1s1 + a2s12 = α (a2s12 )
a1c1 + a2c12 = α (a2c12 )
• Thus if s12 = s1, we can always find an α that will reduce the rank of J
• Thus θ2 = 0,π are two singularities
a1 + a2 a2 − a1
α= α=
a2 a2

Determining Singular Configurations
• In general, all we need to do is observe how the rank of the Jacobian

changes as the configuration changes
• But it is not always as easy as the last example to observe how the
rank changes
• There are some shortcuts for common manipulators: decoupling
singularities
– Analogous to kinematic decoupling
– Assume that we have a 6DOF manipulator and that we can break the
Jacobian into a block form
– Then we can separate singularities into arm singularities and wrist
singularities

Decoupling of Singularities
• Assume that we have a 6DOF manipulator that has a 3-axis arm and a
spherical wrist
– thus the Jacobian is 6x6 and the maximum rank J can have is 6
– Now we can say that the manipulator is in a singular configuration if
det(J(q)) = 0
• For the case of a kinematically decoupled manipulator, we can break

up the Jacobian as follows: J = [J p J o ]
– Where Jp and Jo are represent the position and orientation portions of the
Jacobian
– Jo is given by the following:
z3 × (o6 − o3 ) z4 × (o6 − o4 ) z5 × (o6 − o5 )

Jo =  
 z3 z 4 z5 

Decoupling of Singularities
• Now, one further assumption: o3 = o4 = o5 = o6 = o

– This allows us to note the form of Jo:
0 0 0
Jo =  
z3 z4 z5 
– And we can split the total manipulator Jacobian as follows:
J 0 
J =  11 
J 21 J 22 
– Thus we can say:
det (J ) = det (J11 ) det (J 22 )

Wrist Singularities
• To determine the wrist singularities, we observe the determinant of J22

J 22 = [z3 z4 z5 ]
• Thus the J22 has rank 3 when the three axes are linearly independent
– This is always true, except when two of the axes are collinear
– i.e. θ5 = 0, π are the singularities for a spherical wrist

Arm singularities
• To determine the arm singularities, we observe the determinant of J11

– First, if the ith joint is revolute, the ith column is J11 is given as follows:
J11,i = [zi −1 × (o − oi −1 )]
– First, if the ith joint is prismatic, the ith column is J11 is given as follows:
J11,i = [zi −1 ]
• We will now give examples for the common configurations we have

been using: elbow, spherical, and SCARA manipulators

Ex: elbow manipulator
• To determine the arm singularities, we observe the determinant of J11

– First, J11 is given as follows:
J11 = [z0 × (oc − o0 ) z1 × (oc − o1 ) z2 × (oc − o2 )]
− a2s1c 2 − a3s1c 23 − a2s2c1 − a3s23 c1 − a3c1s23 
=  a2c1c 2 + a3c1c 23 − a2s1s2 − a3s1s23 − a3s1s23 
 0 a2c 2 + a3c 23 a3c 23 
– The determinant of J11 is:

det (J11 ) = a2a3s3 (a2c 2 + a3c 23 )

• The determinant of J11 is:

det (J11 ) = a2a3s3 (a2c 2 + a3c 23 )
– Thus the arm is singular when s3 = 0, i.e. θ3 = 0, π
– This corresponds to the elbow being fully extended or fully retracted:

• The determinant of J11 is:

det (J11 ) = a2a3s3 (a2c 2 + a3c 23 )
– Thus the arm is also singular when a2c2 + a3c23 = 0
– This corresponds to the wrist center intersecting the z0 axis:
– But this is not possible if there is a shoulder offset:

Ex: spherical manipulator
• Since there is no ‘elbow’, the only singularity is when the wrist center
intersects the base axis

Ex: SCARA manipulator
• First, we observe the construction of the Jacobian:

− a1s1 − a2s12 − a1s12 0
J11 =  a1c1 + a2c12 a1c12 0 
 0 0 − 1
• The determinant is:

det (J11 ) = a1 c1s12 − a1 s1c12
2 2
= a1 (c1s12 − s1c12 )
2
= a1 (c1 (s1c 2 + c1s2 ) − s1 (c1c 2 − s1s2 ))

2
= a1 s2
2
• Thus, the SCARA is singular

for s2 = 0, i.e. θ2 = 0, π

Force/torque relationships
• Similar to the relationship between the joint velocities and the end
effector velocities, we are interested in expressing the relationship
between the joint torques and the forces and moments at the end
effector
– Important for dynamics, force control, etc
• Let the vector of forces and moments at the end effector be
represented as: F = [Fx Fy Fz n x ny nz ]
T
• Then we can express the joint torques, τ, as:

τ = J T (q )F
• We will derive this using the principal of virtual work when we discuss
the dynamics of manipulators

• Example: for a force F applied to the end of a planar two-link

manipulator, what are the resulting joint torques?
– First, remember that the Jacobian is:
− a1s1 − a2s12 − a2s12 

 ac +a c a2c12 
 1 1 2 12
 0 0 
J (q ) =  
 0 0 
 0 0 
 
 1 1 

• Example: for a force F applied to the end of a planar two-link

manipulator, what are the resulting joint torques?
– Thus the joint torques are:
Fx 
F 
 y
− a s − a s ac +a c 0 0 0 1  Fz 
τ =  1 1 2 12 1 1 2 12  
 − a2s12 a2c12 0 0 0 1 n x 
n y 
 
 nz 
Fx (− a1s1 − a2s12 ) + Fy (a1c1 + a2c12 )
= 
 F x (− a2 s12 ) + Fy (a 2 c12 ) 

Inverse velocity
• We have developed the Jacobian as a mapping from joint velocities to

end effector velocities:
ξ = Jq&
• Now we want the inverse: what are the joint velocities for a specified
end effector velocity?
• Simple case: if the Jacobian is square and nonsingular,
q& = J −1ξ
• In all other cases, we need another method

• For systems that do not have exactly 6DOF, we cannot directly
invert the Jacobian
• Thus there is only a solution to finding the joint velocities if ξ is in
the range space of J

Inverse Kinematic Algorithm :
ξ q=J ξ
& −1 q&
q& = [q&1 q& 2 L q& n ]
T
• Once is optained, then :
• Jion configuration vector : q = [q1 q2 L qn ]

T
t
q (t ) = ∫ q& (η )dη + q (0)
0
• In numerical implementation :
q (t k +1 ) = q (t k ) + J −1 (q (t k ))ξ (t k )∆t
• However this not completely equavalent to : q& = J ξ
−1
• Hence the solution involves “DRIFT” term

• To solve Drift problem define an operational space error and its time
derivation:
e = ξd − ξ ; e& = ξ&d − ξ&
• According to diferantial kinematics:
e& = ξ&d − J (q )q&

• Choice of :
q& = J −1 (q )( x&d + K e)
• Lead equavalent linear system:
e& + K e = 0
• K : positive definite diagonal matrix

• Block diagram of the Inverse Kinematics algorithm with J-1 :
x&d
+ + q& q
xd
+
-

Inverse velocity
• Take the case of a manipulator with more than 6 joints

– i.e. n > 6
• We can solve for the joint velocities using the right pseudo inverse
• For J ∈ R m×n with m < n ⇒ rank (J ) ≤ m
• If the manipulator is nonsingular, rank(J) = m and (JJT)-1 exists
– Thus we can write:
I = (JJ T )(JJ T )
−1
[ ( )
= J J T JJ T
−1
]
= JJ +
– Where J+ is the right pseudo inverse of J
– Thus the solution for the joint velocities (with minimum norm) is:
q& = J +ξ

Inverse velocity
• How do we construct J+? Using SVD:

– Generalization of methods that we would use for square matrices
– We can write any m x n matrix J as a composition of three matrices:
J = UΣ V T
– Where the matrix U is m x m and contains the eigenvectors of JJT as its
columns and Σ is a matrix that contains the singular values:
σ 1 0
 σ 0 
 2 
Σ=  ⋅ 0
 
 ⋅ 0 
 σ m 0
– And the singular values σi are the square roots of the eigenvalues of JJT:
σ i = λi

Inverse velocity
• Now J+ is given by:

J + = VΣ +U T
– Where Σ+ is:
T
σ 1−1 0
 −1 
 σ2 0
Σ= ⋅ 0
 
 ⋅ 0
 σm
−1
0


Ch. 4: Velocity Kinematics Ch. 4: Velocity Kinematics: Tü-Eef Dept. of Control Engineering

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Ch. 4: Velocity Kinematics Ch. 4: Velocity Kinematics: Tü-Eef Dept. of Control Engineering

Uploaded by

Copyright:

Available Formats

İTÜ-EEF Dept.

Ch. 4: Velocity Kinematics

KON 318E : Introduction to Robotics 1

KON 318E : Introduction to Robotics 2

Angular velocity: fixed axis

Angular velocity: arbitrary axis

– Now we can use S as an operator for a vector a = [ax ay az]T

Angular velocity: arbitrary axis

KON 318E : Introduction to Robotics 5

Angular velocity: arbitrary axis

– This can be seen by the definition of the cross product:

KON 318E : Introduction to Robotics 6

Angular velocity: arbitrary axis

• Properties of skew-symmetric matrices

4. For any S ∈ so(n), x ∈Rn

KON 318E : Introduction to Robotics 7

Angular velocity: arbitrary axis

KON 318E : Introduction to Robotics 8

Angular velocity: arbitrary axis

KON 318E : Introduction to Robotics 9

Angular velocity: arbitrary axis

• Where ω(t) is the angular velocity of the rotating frame

KON 318E : Introduction to Robotics 10

Angular velocity: arbitrary axis

• To show this, consider a point p fixed to a moving frame o1:

= R& 10 p1 + R10 p& 1 = 0 , Since p1 is a fixed

KON 318E : Introduction to Robotics 11

Addition of angular velocities

• To illustrate how the rotation of

(A) (B) (C)

KON 318E : Introduction to Robotics 12

Addition of angular velocities

• Now the first term can be derived as follows:

KON 318E : Introduction to Robotics 13

Addition of angular velocities

KON 318E : Introduction to Robotics 14

Addition of angular velocities

S (a + b ) = S (a ) + S (b ) ω00,2 = ω00,1 + R10ω11,2

ω00,n = ω00,1 + R10ω11,2 + R20ω22,3 + R30ω33,4 + ... + Rn0−1ωnn−−11,n

p& 0 = R& p1 + Rp& 1 + o&

KON 318E : Introduction to Robotics 16

– Call the linear velocity of the end-effector:

KON 318E : Introduction to Robotics 17

KON 318E : Introduction to Robotics 18

If the ith joint is prismatic :

• the angular velocity of frame oi relative to frame oi-1 is zero

KON 318E : Introduction to Robotics 19

• Now Jω can simply be written as follows:

KON 318E : Introduction to Robotics 20

• Therefore we can simply write the ith column of Jv as:

KON 318E : Introduction to Robotics 21

– Therefore, we can write the ith column of the Jacobian:

KON 318E : Introduction to Robotics 22

o& n0 = ωi0−1,i × r = θ&i zi0−1 × r

o&n0 = θ&i zi0−1 × (on − oi −1 )

The complete Jacobian

zi −1 × (on − oi −1 ) for i revolute

• ,The ith column of Jω is given by:

zi −1 for i revolute

KON 318E : Introduction to Robotics 24

The complete Jacobian

Example: two-link planar manipulator

z00 × (o2 − o0 ) z10 × (o2 − o1 )