Chapter 3

KINEMATICS II: DIFFERENTIAL MOTION

In the previous chapter, the position and orientation of the manipulator end-effector were evaluated in
relation to joint displacements. The joint displacements corresponding to a given end-effector location were
obtained by solving the kinematic equation for the manipulator. This preliminary analysis permitted the
manipulator to place the end-effector at a specified location in space.
In this chapter, we are concerned not only with the final location of the end-effector, but also with the
velocity at which the end-effector moves. In order to move the end-effector in a specified direction at a specified
speed, it is necessary to coordinate the motion of the individual joints. The focus of this chapter is the
development of fundamental methods for achieving such coordinated motion in multiple joint manipulators. As
discussed in the previous chapter, the end-effector position and orientation are directly related to the joint
displacements; hence, in order to coordinate joint motions, we derive the differential relationship between the
joint displacements and the end-effector location, and then solve for the individual joint motions.
3.1. Kinematic Modeling of Instantaneous Motions
3.1.1. Differential Relationships
We begin by considering the two degree-of-freedom planar manipulator shown in Figure 3-1. Here, the
manipulator arm is constrained to the x0-y0 plane. The kinematic equations relating the end-effector position (x,
y) to the joint displacements (θ1, θ2) are given by
x(θ1, θ2) = l1 cos(θ1) + l2 cos(θ1 + θ2)
y(θ1, θ2) = l1 sin(θ1) + l2 sin(θ1 + θ2) (3-1)

Figure 3-1: Two degree-of-freedom planar manipulator.

In this chapter, we are interested in the small motions of the end-effector about its current position. The
infinitesimal motion relationship is determined by differentiating the kinematic equations. For two degree-of-
freedom planar manipulation, the differential relationship is obtained by simply differentiating equation (3-1) so
that
∂x( θ 1 , θ 2 ) ∂x(θ 1 , θ 2 )
dx = dθ 1 + dθ 2
∂θ 1 ∂θ 2 (3-2)
∂y( θ 1 , θ 2 ) ∂y( θ 1 , θ 2 )
dy = dθ 1 + dθ 2
∂θ 1 ∂θ 2

In vector form the above can be written as

θ
dx = J dθ (3-3)
θ are infinitesimal displacement vectors defined as
where dx and dθ
dx = [dx dy]T θ = [dθ1 dθ2]T
dθ (3-4)
and
  ∂x / ∂θ 1 ∂x / ∂θ 2  (3-5)
J= 
∂y / ∂θ 1 ∂y / ∂θ 2 

The matrix J comprises the partial derivatives of the functions x(θ1, θ2) and y(θ1, θ2) with respect to joint
displacements θ1 and θ2. The matrix J is referred to as the manipulator Jacobian. The manipulator Jacobian
represents the infinitesimal relationship between the joint displacements and the end-effector location at the
present position and arm configuration.
From equation (3-1), the Jacobian matrix of the two degree-of-freedom planar manipulator is given by
 − l sin( θ 1 ) − l 2 sin( θ 1 + θ 2 ) − l 2 sin( θ 1 + θ 2 ) (3-6)
J= 1
 l 1 cos(θ 1 ) + l 2 cos( θ 1 + θ 2 ) l 2 cos( θ 1 + θ 2 ) 

Note that the elements of the Jacobian are functions of joint displacements, and therefore vary with the arm
configuration.
Consider the instant when the two joints of the two degree-of-freedom planar manipulator move at joint
velocities θD = [θD 1 , θD 2 ]T , and let v = [xD , yD ]T be the resulting end-effector velocity vector. The Jacobian
represents the relationship between the joint velocities and the resulting end-effector velocities as well as the
infinitesimal position relationship. Indeed, dividing both sides of (3-3) by the infinitesimal time increment dt,
we obtain
dx/dt = J⋅⋅dθ
θ/dt
that is,
v = J ⋅ θD (3-7)
Thus the velocity relationship between the joints and the end-effector is determined by the manipulator
Jacobian.
Let J1 and J2 be two 2x1 vectors consisting respectively of the first and the second columns of the Jacobian
(3-6). Equation (3-7) can then be rewritten as
v = J 1 ⋅ θ 1 + J 2 ⋅ θ 2 (3-8)

The first term on the right-hand side accounts for the end-effector velocity induced by the first joint only,
while the second term represents the velocity resulting from the second joint motion only. The resultant end-
effector velocity is given by the vector sum of the two. Each column vector of the Jacobian matrix represents the
end-effector velocity generated by the corresponding joint motion at unit velocity when all other joints are
immobilized.
3.1.2. Infinitesimal Rotations
In the previous section we dealt with simple planar manipulation and analyzed the infinitesimal translation
and the linear velocity of the endpoint. To generalize the result, we need to include the infinitesimal rotation and
angular velocity for a general spatial manipulator arm.
In Chapter 2, we developed mathematical tools for representing the spatial orientation of a rigid body.
Those methods utilize 3x3 rotation matrices and Euler angles and allow us to represent rotations and
orientations of finite angles. However, infinitesimal rotations or time derivatives of orientations are substantially
different from finite angle rotations and orientations. As a result, the methods for representing finite rotations
and orientations are not appropriate when infinitesimal motions are considered. In this section, we investigate
the difference between finite and infinitesimal rotations, and then develop an appropriate mathematical tool for
infinitesimal rotations.
We begin by writing the 3x3 rotation matrix representing infinitesimal rotation dφx about the x axis:
1 0 0  1 0 0 
(3-9)
R x (dφ x ) = 0 cos(dφ x ) − sin(dφ x ) ≈ 0 1 − dφ x 

0 sin(dφ x ) cos(dφ x )  0 dφ x 1 
 Figure 3-2: Infinitesimal rotation vector.
Note that, since dφx is infinitesimal, cos(dφx) = 1 and sin(dφx) = dφx. For infinitesimal rotations about the y
and z axes, similar matrix expressions can be obtained in the same way as equation (3-9). Let Ry(dφ φy) be the 3x3
infinitesimal rotation matrix about the y axis; then the result of consecutive rotations about the x and y axes is
given by

1 0 0   1 0 dφ y   1 0 dφ y   1 0 dφ y 
      (3-10)
R x (dφ x ) ⋅ R y (dφ y ) = 0 1 − dφ x  ⋅  0 1 0  = dφ x dφ y 1 − dφ x  =  0 1 − dφ x 
 
0 dφ x 1  − dφ y 0 1   − dφ y dφ x 1  − dφ y dφ x 1 

where the higher order quantity dφxdφy is neglected. We now change the order of rotations Ry(dφ φy) and Rx(dφ
φx).
Calculating the matrix product in the same way as before, we find that the two results are identical. Namely,
φx) Ry(dφ
Rx(dφ φy) = Ry(dφ
φy) Rx(dφ
φx) (3-11)
Therefore, infinitesimal rotations do not depend on the order of rotations; in other words, they commute. In
general, infinitesimal rotations about the x, y, and z axes shown in Figure 3-2 can be represented by
 1 − dφ z dφ y 
  (3-12)
R(dφ x , dφ y , dφ z ) =  dφ z 1 − dφ x 
− dφ y dφ x 1 

The rotation matrix depends only on the three infinitesimal angles, but is independent of the order of
rotations.
Let R(dφ φy, dφ
φx, dφ φz) and R(dφ
φx’, dφ
φy’, dφ
φz’) be two infinitesimal rotation matrices, then the consecutive
rotations of the two yield
 1 − (dφ z + dφ z ' ) (dφ y + dφ y ' ) 
 
R(dφ x , dφ y , dφ z ) ⋅ R(dφ x ' , dφ y ' , dφ z ' ) =  (dφ z + dφ z ' ) 1 − (dφ x + dφ x ' ) = (3-13)
− (dφ y + dφ y ' ) (dφ x + dφ x ' ) 1 
 
= R (dφ x + dφ x ' , dφ y + dφ y ' , dφ z + dφ z ' )

where higher order quantities are neglected. Thus, the rotation resulting from two arbitrary infinitesimal
rotations is simply given by the algebraic sum of the individual components for each axis, in other words
infinitesimal rotations are additive. This is another important property of infinitesimal rotations.
Let us write the infinitesimal rotations about the three axes in vector form:
φ = [dφx dφy dφz]T
dφ (3-14)
In general, expressions such as (3-14) can be treated as vectors if they are additive and commutative. As
shown above, infinitesimal rotations are additive and commutative. We treat, therefore, the infinitesimal
rotations denoted by the expression (3-14) as a vector, because it possesses all the properties that vectors in a
φ can be represented by an arrow,
vector field must satisfy. Geometrically, the infinitesimal rotation vector dφ
shown in Figure 3-2. The direction of the arrow represents the axis of rotation, and the length represents the
magnitude of the rotation.
 It should be noted that vector representation is not allowed for finite rotations, but is valid only for
infinitesimal rotations. Figure 3-3 demonstrates that finite rotations cannot be treated as vectors. The rectangular
box shown is rotated about the x and y axes in different orders. When we rotate it about the x axis first and then
about the y axis, the resultant orientation is given by the top figure. If the consecutive rotations are carried out in
the opposite order, the orientation shown by the bottom figure is obtained, which is completely different from
the other one. However, if we represented the individual rotations by the three-dimensional vectors, φ1 and φ2,
and used vector addition, the resultant vectors corresponding to the two different orders of rotations would be
identical. Thus finite rotations cannot be treated as vectors.

Figure 3-3: Finite angle rotations.

3.1.3. Computation of the Manipulator Jacobian
In this section we deal with a general n degree-of-freedom manipulator arm and compute the manipulator
Jacobian associated with the infinitesimal translation and rotation of the end-effector. As shown in Figure 3-4,
we denote the infinitesimal end-effector translation by a three-dimensional vector dxe and we represent the
infinitesimal end-effector rotation by a three-dimensional vector dφ φe. Both vectors are represented with
reference to the base coordinate frame O0-x0y0z0. For convenience, we combine the two vectors and define the
following six-dimensional vector dp:
dx  (3-15)
dp =  e 
dφ e  6x1

Figure 3-4: Infinitesimal displacements of end-effector generated by individual joints.

 Dividing both sides by the infinitesimal time increment dt, we obtain the velocity and angular velocity of
the end-effector:
v  (3-16)
pD =  e 
 ω e  6 x1
As before, the end-effector velocity and angular velocity can be written as a linear function of joint
velocities using the manipulator Jacobian:
pD = J ⋅ qD (3-17)

where qD = [qD 1 , qD 2 , l , qD n ]T is the nx1 joint velocity vector. The dimension of the Jacobian matrix is now 6xn;
the first three row vectors are associated with the linear velocity ve, while the last three correspond to the
angular velocity ωe. Each column vector, on the other hand, represents the velocity and angular velocity
generated by the corresponding individual joint. Let us determine each column vector of the Jacobian matrix as
functions of link parameters and arm configuration. Let JLi and JAi be 3x1 column vectors of the Jacobian
matrix associated with the linear and angular velocities, respectively. Namely, we partition the Jacobian matrix
so that
J J L 2 l J Ln  (3-18)
J =  L1
 J A1 J A 2 l J An 

Using the JLi, we can write the linear velocity of the end-effector as
v e = J L1qD 1 + J L 2 qD 2 + l + J Ln qD n (3-19)

If joint i is prismatic, it produces a linear velocity at the end-effector in the same direction as the joint axis.
Let bi−−1 be the unit vector pointing along the direction of the joint axis i, as shown in Figure 3-4, and let d i be
the scalar joint velocity in this direction, then we obtain
J Li qD i = b i −1dD i (3-20)

Note that in the Denavit-Hartenberg notation the joint velocity d i is measured along the zi−1 axis. If the
joint is revolute, as shown in the figure, it rotates the composite of distal links from links i to n at the angular
velocity ωi given by
ω i = b i −1 θD i (3-21)

This angular velocity produces a linear velocity at the end-effector. Let ri−−1,e be the position vector from Oi−1
to the end-effector as shown in the figure, then the linear velocity generated by the angular velocity ωi is given
by
J Li qD i = ω i × ri −1,e = (b i −1 × ri −1,e ) ⋅ θD i (3-22)

where a × b represents the vector product of two vectors a and b. Thus the end-effector velocity is determined
by either (3-20) or (3-22) depending on the type of joint.
Similarly, the angular velocity of the end-effector can be expressed as a linear combination of the column
vectors JAi in equation (3-18),
ω e = J A1qD 1 + J A 2 qD 2 + l + J An qD n (3-23)

When joint i is a prismatic joint, it does not generate an angular velocity at the end-effector, hence
J Ai qD i = 0 (3-24)
If, on the other hand, the joint is a revolute joint, the angular velocity is given by
J Ai qD i = ω i = b i −1 θD i (3-25)

Equations (3-20), (3-22), (3-24) and (3-25) determine all the elements of the manipulator Jacobian. To
summarize:
 J Li  b i −1  for a prismatic joint (3-26)
J  =  0 
 Ai   
  J Li  b i −1 × ri −1,e  for a revolute joint (3-27)
J  =  b i −1 
 Ai   
Vectors bi−−1 and ri−−1,e in the above expressions are functions of joint displacements. These vectors can be
computed using the coordinate transformations discussed in the previous chapter. The direction of joint axis i−1
is represented by b = [0, 0, 1]T with reference to coordinate frame i−1, because the joint axis is along the zi−1
axis. This vector b can be transformed to a vector which is defined with reference to the base frame, that is bi−−1,
using 3x3 rotation matrices R jj−1 (q j ) as:

b i −1 = R 10 (q 1 ) ⋅ R 12 (q 2 ) ⋅  ⋅ R ii −−12 (q i −1 ) ⋅ b (3-28)

Position vector ri−−1,e can be computed by using 4x4 matrices A j−1 (q j ) . Let Xi−−1,e be the 4x1 augmented
j
vector of ri−−1,e, and X = [0, 0, 0, 1]T be the augmented position vector representing the origin of its coordinate
frame, then the position vector ri−−1,e is derived from

X i −1,e = A 10 (q 1 ) ⋅ A 12 (q 2 ) ⋅  ⋅ A nn−1 (q n ) ⋅ X − A 10 (q 1 ) ⋅ A 12 (q 2 ) ⋅  ⋅ A ii −−12 (q i −1 ) ⋅ X (3-29)

where the first term accounts for the position vector from the origin O0 to the end-effector and the second term is
from O0 to Oi−1.
Example 3-1: Three Degree-of-Freedom Polar Coordinate Manipulator.
Let us work out an example of the manipulator Jacobian computation. The skeleton structure of a 3 degree-
of-freedom manipulator is shown in Figure 3-5. The joint displacements θ1, θ2 and d3 defined in the figure are
equivalent to polar coordinates; hence this manipulator arm is called a polar coordinate manipulator. To find the
Jacobian matrix, we begin by determining the directions of the joint axes. From the figure these are given by
0 − s 1  c 1s 2 
(3-30)
b 0 = 0 b 1 =  c 1  b 2 = s 1s 2 
   
1  0   c 2 

Figure 3-5: Three degree-of-freedom spherical coordinate manipulator.

For revolute joints, we need to find position vector ri−−1,e. They are
r1,e = d3⋅b2
r0,e = l0⋅b0 + d3⋅b2 (3-31)
 Substituting (3-30) and (3-31) into (3-18), (3-26) and (3-27),
− d 3 s 1s 2 d 3 c 1c 2 c 1s 2 
d c s d 3 s 1c 2 s 1s 2 
 3 1 2 
 0 − d 3s 2 c2  (3-32)
J= 
 0 − s1 0 
 0 c1 0 
 
 1 0 0 

Note that the elements of the manipulator Jacobian are functions of the joint displacements, hence the
Jacobian is configuration-dependent.
∆∆∆
3.2. Inverse Instantaneous Kinematics
3.2.1. Resolved Motion Rate
Equation (3-17) in the previous section provided the velocity and angular velocity of the end-effector as a
linear function of joint velocities. Using this expression we now discuss the inverse problem. Namely, given a
desired end-effector velocity, we find the corresponding joint velocities that cause the end-effector to move at
the desired velocity.
As mentioned in Chapter 2, a manipulator arm must have at least six degrees of freedom in order to locate
its end-effector at an arbitrary position with an arbitrary orientation. Similarly, six degrees of freedom are also
necessary to move the end-effector in an arbitrary direction with an arbitrary angular velocity. In this section, we
discuss the inverse problem for six degree-of-freedom manipulators. In Section 3.2.2. we will extend the
derivation to general n degree-of-freedom manipulators.
For a six degree-of-freedom manipulator, the Jacobian matrix J is a 6x6 square matrix. If the matrix is non-
singular at the current arm configuration, the inverse matrix J−1 exists. We then obtain from (3-17)

qD = J −1pD (3-33)

Equation (3-33) determines the velocities required at the individual joints in order to obtain a given end-
effector velocity pD - this scheme is referred to as resolved motion rate control and is attributed to (Whitney,
1969). Since the manipulator Jacobian varies with the arm configuration, it may become singular at certain
configurations. In such cases the inverse Jacobian does not exist, hence solution (3-33) is not valid. The
corresponding arm configuration is then itself called singular. At a singular configuration, the matrix J is not of
full rank; hence, its column vectors are linearly dependent, and thus do not span the whole six-dimensional
vector space of pD . Therefore, there exists at least one direction in which the end-effector cannot be moved no
matter how we choose joint velocities q 1 through q 6 . Let us work out an example of this effect.

Example 3-2
Consider the two degree-of-freedom planar manipulator shown in Figure 3-6. The length of each link is 1.0,
and the endpoint velocity is denoted by v = [vx, vy]T.
1. Given a desired endpoint velocity, find joint velocities that produce the desired endpoint velocity.
2. Find singular configurations, and determine in which direction the endpoint cannot move for each singular
configuration.
3. Find profiles of joint velocities when the endpoint is required to track the trajectory shown in Figure 3-7 at a
constant tangential speed.
 Figure 3-6: Endpoint velocities of the two d.o.f. planar manipulator.
1. The Jacobian matrix for this planar manipulator has been derived in equation (3-6). Replacing l1 and l2 by 1.0,
we obtain
− sin θ 1 − sin( θ 1 + θ 2 ) − sin( θ 1 + θ 2 ) (3-34)
J=
 cos θ 1 + cos(θ 1 + θ 2 ) cos(θ 1 + θ 2 ) 

Inverting the Jacobian matrix and substituting into (3-33), the joint velocities are given by
v x cos( θ 1 + θ 2 ) + v y sin( θ 1 + θ 2 )
θD 1 = (3-35)
sin θ 2

v x [cos θ 1 + cos( θ 1 + θ 2 )] + v y [sin θ 1 + sin( θ 1 + θ 2 )]

θD 2 = − (3-36)
sin θ 2

2. Singularity occurs when the determinant of the manipulator Jacobian is zero. Now from expression (3-34)
det (J) = sin θ2 (3-37)
Therefore, singular configurations occur for θ2 = 0 or θ2 = π, i.e. when the arm is fully extended or fully
contracted. This corresponds to the endpoint positions shown in Figure 3-7, so that the origin O0 and the
boundary of the reachable space are singular. At the singular configuration, equation (3-19) becomes
 v x  − 2 sin θ 1  D − sin θ 1  D − sin θ 1  D D (3-38)
v  =   ⋅ θ 1 +  cos θ  ⋅ θ 2 =  cos θ  ⋅ ( 2θ 1 + θ 2 )
 y  2 cos θ 1   1   1 

i.e., the two column vectors of the Jacobian matrix become parallel. The endpoint can then be moved only in the
direction perpendicular to the arm links, but not in any other direction.
 Figure 3-7: Trajectory tracking near singular points.
3. To find the velocity profile for tracking the specified trajectory, we first obtain the joint angles that
correspond to each endpoint position on the trajectory. Then we substitute the joint angles into (3-35) and (3-
36), and determine the joint velocities required. The result is shown in Figure 3-8, where the two joint velocities
are plotted with respect to time. Note that both velocities are excessively large near the singular points, A and D.
To generate the endpoint velocity in the radial directions, OA and OD, excessively large velocities are required
for both joints. In this region, the denominators in equations (3-35) and (3-36) are almost zero. Also, the
velocity of the first joint becomes excessively large between points B and C, since the arm links must rotate
quickly in this region. Again, this region is near the singular point. Thus, even if the inverse of the manipulator
Jacobian exists, the joint velocities become excessively large in the vicinity of singular points.

Figure 3-8: Profiles of joint velocities.

∆∆∆
3.2.2. Redundancy
We have seen that there are a finite number of solutions to the kinematic equation of a six degree-of-
freedom manipulator arm. For the instantaneous kinematic equation, a unique solution exists if the arm
configuration is non-singular. However, when a manipulator arm has more than six degrees of freedom, we can
find an infinite number of solutions that provide the same motion required for the end-effector. Consider for
instance the human arm, which has seven degrees of freedom excluding the joints at the fingers. As seen in
Section 2.3.1, when the hand is placed on a desk and fixed in its position and orientation, the elbow position can
still vary continuously without moving the hand. This implies that a certain ratio of joint velocities exists that
does not cause any velocity at the hand. This specific ratio of joint velocities does not contribute to the resultant
endpoint velocity. Even if these joint velocities are superimposed to other joint velocities, the resultant end-
effector velocity is the same. Consequently, we can find different solutions of the instantaneous kinematic
equation for the same end-effector velocity. In this section, we investigate the fundamental properties of the
instantaneous kinematics when additional degrees of freedom are involved.
 To formulate the instantaneous kinematic equation for a general n degrees-of-freedom manipulator arm, we
begin by modifying the definition of the vector dp representing the end-effector motion. In equation (3-15), dp
was defined as a six-dimensional vector that represents the infinitesimal translation and rotation of an end-
effector. However, the number of variables that we need to specify for performing a task is not necessarily six.
In arc welding, for example, only five independent variables of torch motion need be controlled. Since the
welding torch is usually symmetric about its center line, we can locate the torch with an arbitrary orientation
about the center line. Thus five degrees of freedom are sufficient to perform arc welding operations. In general,
we describe the end-effector motion by m independent variables that must be specified to perform a given task.
Let dp = [dp1, dp2, …, dpm]T be the mx1 vector which represents the infinitesimal displacements of the end-
effector, then the instantaneous kinematic equation for a general n degree-of-freedom manipulator arm is given
by
dp = J⋅⋅dq (3-39)
where the dimension of the manipulator Jacobian J is mxn. When n is larger than m and J is of full rank, there
are (n−m) arbitrary variables in the general solution of (3-39). The manipulator is then said to have (n−m)
redundant degrees of freedom for the given task.

Figure 3-9: Linear mapping diagram of instantaneous kinematics.

The Jacobian matrix also determines the relationship between the end-effector velocity pD and joint
velocities qD :

pD = J ⋅ qD (3-40)

Equation (3-40) can be regarded as a linear mapping from n-dimensional vector space Vn to m-dimensional
space Vm. To characterize the solution to equation (3-40), let us interpret the equation using the linear mapping
diagram shown in Figure 3-9. The subspace R(J) in the figure is the range space of the linear mapping. The
range space represents all the possible end-effector velocities that can be generated by the n joints at the present
arm configuration. If the rank of J is of full row rank, the range space covers the entire vector space Vm.
Otherwise, there exists at least one direction in which the end-effector cannot be moved. The subspace N(J) of
Figure 3-9 is the null space of the linear mapping. Any element in this subspace is mapped into the zero vector
in Vm: J ⋅ q = 0 . Therefore, any joint velocity vector qD that belongs to the null space does not produce any
velocity at the end-effector. Recall the human arm discussed before. The elbow can move without moving the
hand. Joint velocities for this motion are involved in the null space, since no end-effector motion is induced. If
the manipulator Jacobian is of full rank, the dimension of the null space, dim N(J), is the same as the number of
redundant degrees of freedom (n−m). When the Jacobian matrix is degenerate, i.e. not of full rank, the
dimension of the range space, dim R(J), decreases, and at the same time the dimension of the null space
increases by the same amount. The sum of the two is always equal to n:
dim R(J) + dim N(J) = n (3-41)
Let qD * be a solution of (3-40) and qD 0 be a vector involved in the null space, then the vector of the form
qC = qC * + k ⋅ qC 0 is also a solution of (3-40), where k is an arbitrary scalar quantity. Namely,

J ⋅ qD = J ⋅ qD * +k ⋅ J ⋅ qD 0 = J ⋅ qD * = pD (3-42)

Since the second term k ⋅ qD 0 can be chosen arbitrarily within the null space, an infinite number of solutions
exists for the linear equation, unless the dimension of the null space is zero. The null space accounts for the
arbitrariness of the solutions. The general solution to the linear equation involves the same number of arbitrary
parameters as the dimension of the null space.
3.2.3. Optimal Solutions
 In this section we discuss an optimal solution to the velocity relationship (3-40). We fix the manipulator
Jacobian at an appropriate arm configuration, and find the optimal solution to the linear equation (3-40),
assuming that the Jacobian matrix is of full row rank. We evaluate solutions to the linear equation by the
quadratic cost function of the joint velocity vector given by

G(qD ) = qD T ⋅ W ⋅ qD (3-43)

where W is an nxn symmetric positive definite weighting matrix. The problem is to find the qD that satisfies
equation (3-40) for a given pD and J while minimizing the cost function G(q ) . Let us solve this problem using
Lagrange multipliers. To this end we use a modified cost function of the form

G(qD , λ ) = qD T ⋅ W ⋅ qD − λT ⋅ ( J ⋅ qD − pD ) (3-44)

where λ is an mx1 unknown vector of Lagrange multipliers. The necessary conditions that the optimal solution
must satisfy are
∂G T
= 0 , that is, 2 ⋅ W ⋅ qD − J ⋅ λ = 0 (3-45)
∂qD

∂G
= 0 , that is, J ⋅ qD − pD = 0 (3-46)
∂λ
which is of course identical to (3-40). Now matrix W is positive definite, hence invertible. Thus, we obtain from
equation (3-45)
1 −1 T (3-47)
qD = W J λ
2
Substituting the above into (3-46) yields

( J ⋅ W −1 J T ) ⋅ λ = 2pD (3-48)

Since J is assumed to be of full row-rank, matrix product J⋅⋅W−1JT is a full-rank square matrix, and is
therefore invertible. Eliminating the Lagrange multiplier vector λ in equations (3-47) and (3-48), we obtain the
optimal solution

qD optimal = W −1 J T ( J ⋅ W −1 J T ) −1 ⋅ pD (3-49)

Clearly, the above solution satisfies the original velocity relationship (3-40). Indeed, we can obtain equation
(3-40) by premultiplying equation (3-49) by the Jacobian matrix J. When the weighting matrix W is the mxm
identity matrix, the above solution reduces to

qD optimal = J T ( J ⋅ J T ) −1 ⋅ pD (3-50)

The matrix product pinv( J ) ≡ J T ( J ⋅ J T ) −1 is known as the pseudo-inverse of the Jacobian matrix.

3.3. Research Topics

The computation of the manipulator Jacobian is time-intensive, which is a crucial problem for real-time
control. Efficient computation algorithms are suggested by (Orin and Schrader, 1984; Pennock and Yang, 1985-
b). (Sugimoto, 1984) studies the derivation of joint velocities from endpoint velocities without explicit
computation of the Jacobian.
Singularity is a critical problem for articulated manipulator arms. (Waldron, Wang and Bolin, 1985; Litvin
and Castelli, 1985) analyze arm singularities, while (Paul and Stevenson, 1983; Asada and Cro Granito, 1985)
address wrist singularities and methods for avoiding them. (Hollerbach, 1984) discusses the kinematic structures
of redundant manipulators that are appropriate for avoiding the singular points internal to the workspace.
The study of redundant manipulators is an important research topic in advanced manipulation, particularly
for obstacle and singularity avoidance. The application of pseudo-inverse matrices to obtain optimal joint
velocities has traditionally been a central tool for the redundant arm problem (Hanafusa et al., 1981;
Konstantinov et al., 1982; Klein, 1983; Nakamura and Hanafusa, 1984; Hollerbach, 1984-b).
Resolved motion rate control has been extended to resolved acceleration control by several researchers.
Computational efficiency is again a central theme (Luh, Walker and Paul, 1980-b; Hollerbach, 1983).
 The kinematic analysis and design problems have been extended to closed-loop mechanisms, in particular
mechanical fingers (Hanafusa and Asada, 1976; Asada, 1979; Salisbury, 1982 and 1984; Salisbury and Craig,
1982; Okada, 1979; Kobayashi, 1984; Bakar et al., 1985; Holzmann and McCarthy, 1985) The kinematic
problems associated with the manipulation of rigid bodies with mechanical fingers has been studied in the
contexts of automatic assembly (Ohwovoriole and Roth, 1981) and workpart fixturing (Asada and By, 1984,
1985).