KINEMATIC CONTROL OF A SEVEN-JOINT MANIPULATOR

WITH NON-SPHERICAL WRIST

Fabrizio Caccavale

Pasquale Chiacchio

Stefan0 Chiaverini

Dipartimento di Informatica e Sistemistica

Universith degli Studi di Napoli Federico II
via Claudio 21,80125 Napoli, Italy
E-mail: { caccavale,chiacchio,chiaverini} @disna.dis.unina.it

algorithm proposed in [3], where inner joint variables are

solved in the first stage and the outer joint variables are
solved in the second stage. Thanks to its closed-loop
fashion [4], the algorithm is endowed with good tracking
and steady-state performance.

ABSTRACT
This paper focuses on real-time kinematic control of a
seven-joint industrial robot manipulator having a nonspherical wrist. Despite the structure of the wrist, a twostage inverse kinematics algorithm is devised which solves
for the arms inner five joints in the first stage and for the
outer two joints in the second stage. A damped leastsquares technique with varying damping factor is adopted
to provide robustness to kinematic singularities. An additional constraint is introduced and a task priority strategy
is adopted to solve redundancy. Experimental results are
reported in a case study.

Whenever the manipulator has redundant degrees of mobility, the inverse kinematics problem admits infinite solutions and some criterion has to be established to select a
solution. One kind of redundancy occurs when a six-joint
manipulator is placed on a platform or a track to acquire
greater mobility in a larger portion of space. Closed-loop
redundancy resolution techniques have been developed to
exploit the available degrees of freedom to fulfill an additional task besides the end-effector task. Among these, the
task-priority strategy defines an order of priority between
the two tasks by projecting the secondary task in the null
space of the primary task Jacobian [5,6,7].

1. INTRODUCTION
Finding inverse kinematics solutions is of the utmost importance in order to transform motion specification assigned by the user in the operational space into joint references for the robot control system.

The two COMAU SMART-3 S installed in our laboratory are examples of six-joint industrial robot manipulators with a non-spherical wrist whose axes intersect twoby-two. One of them is mounted on a sliding track, and
as such it presents one redundant degree of mobility with
respect to six-degree-of-freedom tasks.

First solving inverse kinematics and then performing joint

space control is commonly known in the literature as kinematic control, as opposed to operational space control
where the control loop is closed directly on end-effector
variables. One advantage of the former is the possibility
of exploiting the joint servos of industrial robots which
normally are not accessible to the user.

The goal of this paper is to report an experimental investigation of kinematic control for the above seven-joint
redundant robot manipulator with non-spherical wrist. A
two-stage closed-loop inverse kinematics algorithm is developed which is based on the pseudo-inverse of a reduced
order Jacobian matrix. Robustness to motion in the neighborhood of kinematic singularities is gained by resorting to
a damped least-squares technique [8,9], where the damping factor is adjusted [lo] on the basis of the smallest
singular value of the Jacobian [ 111. Redundancy resolution is then embedded in the schemes by utilizing the task
priority strategy so as to ensure that the constraint task
does not interfere with the end-effector task.

Most manipulators have a simple kinematic geometry so

that closed-form solutions can be found to the inverse kinematics problem. This is the case of a class of manipulators
having a spherical wrist, i.e. when the three axes of the
wrist revolutejoints intersect at a common point [ 11.
Constructing a spherical wrist may complicate the mechanical design, and thus a few robotic manipulators have
a non-spherical wrist; typically the wrist revolute joint
axes intersect only two-by-two. In spite of the mechanical simplicity, the inverse kinematics problem in general
does not admit closed-form solutions and numerical techniques [2] are to be sought.

Results are illustrated to demonstrate real-time feasibility of the overall kinematic control scheme, i.e. on-line
trajectory generation, inverse kinematics with singularity
robustness and redundancy resolution, and joint servo routines.

A computationally efficient technique for solving inverse

kinematics of this class of manipulators is the two-stage

0-7803-29-1/9 $4.00 0 1995 HEEE

50

2. SEVEN-JOINT INDUSTRIAL MANIPULATOR

form

The SMART-3 S robot is manufactured by COMAU, the

leading national robotics and automation company. Typical industrial applications are arc welding, light handling,
sealant dispensing, and water jet machining. The maximum stroke of the manipulator is 1.4 m. The robot available in our laboratory is mounted on a sliding track which
enlarges the manipulator workspace.

where q is the (7 x 1) vector of joint variables, n, s, a

are the unit vectors of the end-effector frame and p is the
position vector of the origin of such frame with respect to
the origin of the base frame.
In order to devise an inverse kinematics algorithm,the manipulator differential kinematics must be considered. This
can be characterized in terms of either the geometric or
analytical Jacobian. With the former, the relationship between the end-effectorlinear velocity vector i, and angular
velocity vector w and the joint velocity vector q is given
by

where J is the geometric Jacobian matrix.

In view of development of a two-stage inverse kinematics
algorithm, a relevant point of the arm structure is the intersection of the joint axes 6 and 7, denoted by the position
vector p', that can be computed as
p' = p

a;

0
0.15

0.61
0.11

5
6
7

0
0
0

d;
41
0.85
0
0
0.61
-0.113
0.103

19;
0
42
43
44

45
f&j

47

(3)

It can be recognized that p' depends only on the joint variables qp = ( dl 9 2 d3 9 4 d5 )T. Such vector also
determines the orientation of the unit vector of frame 5,
namely z5, which is subject to the geometric constraint [3]

Figure 1 - Sketch of the serial kinematic chain of the

COMAU SMART-3 S robot.
Joint
1
2
3

- d7a.

aT%g= a s Q6.

a;

(4)

Differential kinematics equations involving the above two

vectors can be established in terms of suitable analytical
Jacobians as follows:

-7d2
-~/2
0
-Id2
~ / 2
-TI2
0

where

Table I -Denavit-Hartenbergparameters of the COMAU

SMART-3 S robot.
where
Direct kinematics of the robot manipulator can be found
by adopting the classical Denavit-Hartenbergconvention.
A sketch of the serial kinematic chain is displayed in Figure 1 and its Denavit-Hartenbergparameters expressed in

Furthermore, by denoting with qo = ( 8 8

87

)T the vec-

tor of the two remaining joint variables, the following

analytical Jacobians are of interest:

SI units are listed in Table I. Homogeneous transformation matrices expressing relative position and orientation
between consecutive links can be computed and the resulting direct kinematics equations can be expressedin the

(9)

51

In the neighborhood of singularities of Jp,the pseudoinversion in (17) becomes ill-conditioned and high velocities may occur. Numerical robustness can be gained by
resorting to a damped least-squares of the Jacobian [8,9]

and

J,* = J,T(JpJ,T + X 2 1 ) - '

3. INVERSE KINEMATICS ALGORITHM

where X is a damping factor achieving a trade-off between

solution accuracy and solution feasibility [ 121. The damping factor can be suitably tuned by defining a singular
region in the neighborhood of the singularity on the basis
of an estimate of the smallest singular value of J p [ l l ] ,

A two-stage inverse kinematics algorithm can be devised

by solving first for qp and then for qo following the guidelines in [3].
The first stage of the algorithm must guarantee not only
that p' coincides with p &but also that the sixth-link orientation t 5 is compatible with the assigned a d through
the geometric constraint (4). To the purpose, define the
task-space vector

i.e.

where 1 3is~the estimate of the smallest singular value,

and E defines the size of the singular region; the value of
is at user's disposal to suitably shape the solution in
the neighborhood of a singularity [101.

which in view of ( 5 ) and (7) allows to derive the velocity

mapping
(12)
x p = JPkP)GP +E

4. REDUNDANCY RESOLUTION

where

The presence of redundancy can be utilized to introduce an

additional constraint task; for the manipulator at issue in
the present work which has a single degree of redundancy
this is expressed by a scalar function 2,.

and

E=

a;z5)

As a consequence,the above inverse kinematics algorithm

must be modified to allow redundancy resolution. In order

If p d is the desired end-effector position and n d , S d , a d

describe the desired end-effector frame, the desired taskspace vector is

to avoid conflicts between the end-effector task and the

constraint task, it is worth choosing a task priority strategy [5,6] which consists in projecting the joint velocity
vector satisfying the constraint task onto the null space of
the primary task.

h terms of the above two-stage algorithm, the solution to

the first stage (17) is modified into

where p: is the desired position for p' that, in view of (3),

can be computed as p d - d T a d , and cos a
3 is constant and
equal to - 1. Accordingly, the algorithm error is

G~ = J J ( k p , d
where IC,

The joint velocity solution for the first stage can be thus
computed as
q p

= J;(qp)(*p,d

-I-K p e p

- e)

(17)

where IC,

-t J F ( q p ,

4o)ad)

ec

=xcd

-z c ( q )

(23)

is the constraint error between the desired and actual constraint value.

In view of the kinematic properties of s , a, J s , Ja, the

solution for qo can be computed via [3]
q0)sd

> 0,

is the constraint Jacobian. and

where K pis a suitable positive definite gain matrix.

40= k ( J ? ( q p ,

+ K p e p- 5) + k C ( l- JJJ p ) j c e c (21)

The second stage of the algorithmremains the same as (18).

(18)

5. EXPERIMENTAL RESULTS

> 0.

Experiments have been performed on the six-degree-offreedom robot SMART-3 S with a non-spherical wrist
whose axes intersect two-by-two. The robot is mounted on

Joint positions are then found by numerical integration of

the joint velocities qp and qo.

52

a sliding track so as to realize a hnematically redundant

structure. Each joint is actuated by brushless motors via
gear trains; shaft absolute resolvers provide motor position
measurements. The robot is controlled by an open version
of the C3G 9000 unit whch has a VME-based architecture with a bus-to-bus communication link to a standard
personal computer [ 131.

The initial and final location of the end effector are interpolated using 5th-order polynomial time laws such that null
initial and final velocities and accelerations are obtained.
This results in a straight-line motion for the end-effector
position which corresponds to a displacement of - 1.6 m
along the z axis with an end-effector rotation of 77r/12 rad
around the y axis. The duration of the motion is 6 s.

The open version of the C3G 9000 controller allows seven

different operation modes, of which one is that availableon
the industrial version of the controller. To implement the
proposed inverse kinematics algorithms, we have used the
operation mode number 6 in which the PC takes care of the
joint reference trajectory generation and the control unit
closes its standard PID joint servos at a 2 ms sampling rate.
In this operation mode the joint servos are not accessible,
nevertheless they are factory tuned.

Figure 2 reports the results obtained by using the inverse

kinematics algorithm (17,18). To evaluate the performance of the system, we have chosen to consider the errors
obtained in the task space. In detail, if qr denotes the joint
references output by the inverse kinematics algorithm, the
algorithm error is given by
ea,p

= Pd - P ( ~ P )

for the positional part, and

The inverse kinematics algorithms (17,18) and (21,18)

have been implemented as C modules running on a
486DX2/66 PC. At each sampling interval, the PC computes the joint references by Euler integration of the joint
velocities (ia, 4,; the references are then passed to the
servos through the communication link. For logging purposes, resolver readings and motor currents are downloaded to the PC at each sampling instant.

ea,, = z ( " ( q r ) x

n d

+ s(%)

sd

8 ( Q T )x s d )

for the orientation. If q denotes the vector of actual joint

variables, the servo ermr is given by
es,p

24%)- P ( q )

for the positional part, and

Computation of the joint references requires computation

of the task-space references, direct kinematics functions,
and a single step of the inverse kinematics algorithm. In
the case of algorithm (17,18), this takes 780ps; if algorithm (21,18) is used, 8 3 0 p are needed. Remarkably,
the extra time needed by the latter algorithm is due to
the redundancy resolution and is very small (about 6%) in
comparison to the time needed by the former algorithm. In
any case, the implementation shows that relatively short
computation times are obtained with currently available
low-cost hardware.

for the orientation. Notice that servo errors are read in the
task space but derive from joint space control actions.

To tune the algorithm gains preliminary experiments have

been performed by running the software on the PC without
activating the communication with the robot control unit.
As a result of these preliminary tests, the gain K phas been
chosen equal to 10001 while k , has been set equal to 500
and k , equal to 5. It should be pointed out that a low value
of 1, has been selected to achieve a smooth recovery, thus
avoiding high joint velocities which could be infeasible for
the robot actuators.

To test the above inverse kinematics algorithms, a taskspace motion is assigned from the starting location
COS( - ~ / 3 )
sin( - ~ / 3 ) 0 0.975
T%=(
0
-1 1.288
- s i n ( - ~ / 3 ) cos(-?r/3) 0 2.194
0
0
0
1
to the desired final location
COS(T/~)
s i n ( ~ / 4 ) 0 0.975
0
-1 1.288
Tf= ( - s i l ~ / 4 ) C O S ( T / ~ ) 0 0.594
0
0
0
1

"0

[sec1

Figure 2 -Experimental results for the algorithm (17,18):

a) norm of the position errors (algorithm error: dashed;

servo error: solid); b) norm of the orientation errors (algorithm error: dashed; servo error: solid).

53

It can be recognized that the algorithm error for the po-

i.e. the center of the moving track; a linear interpolation has

been used to achieve a smooth null-space motion. The duration of the constraint-taskis the same as the end-effector
task.

sitional part is about two orders of magnitude below the

servo errors, while algorithm and servo errors on the orientation are comparable. This is due to the higher accuracy
of the velocity mapping in (17), which is based on a Jacobian pseudoinverse, in comparison to the mapping in (18)
based on a Jacobian transpose, which is not a velocity
mapping.

IO-^
I

To evaluate the performance of the system, we must consider the constraint-task error besides the errors obtained
in the task space. Since the inverse kinematics algorithm
is tuned to ensure low tracking errors on the primary (endeffector) task, constraint-task errors are substantially algorithm errors and are computed according to (23).

It can be recognized that the final desired value of the

constraint is achievedwith primary-task errors comparable
to those obtained without redundancy resolution. During
the manipulator motion, however, the constraint error is
large since IC, is low and the task is demanding.

To test the redundancy resolution capabilities of the inverse

kinematics algorithm in the presence of kinematic singularities a second experiment has been developed. The
starting location of the manipulator is

Tz=

-1
0
0
0

0
-0.113
0.801 1.852
0.599 1.050
1
0

that corresponds to a kinematically singular configuration,

and the robot arm must be reconfigured so as to leave
the singularity. Therefore, the desired final location Tf
is equal to the initial one, and the algorithm (21,18) is
used with a suitable choice of the constraint task. It can
be recognized that increasing 43 would help to escape
the singularity; thus, we have assigned as the constraint
task q3 to reach the final value - ~ / 2rad from its starting
value of -2.500. A linear interpolation has been used to
achieve a smooth null-space motion. The duration of the
constraint-taskis 6 s.

Figure 3 -Experimental results for the algorithm (21,lS):

a) norm of the position errors (algorithm error: dashed;
servo error: solid); b) norm of the orientation errors (algorithm error: dashed; servo error: solid).

0.25

0
0.599
-0.801
0

Figure 5 and 6 report the results obtained. It can be recognized that in the starting phase of the motion the endeffector error is larger due to the singularity. However,
the reconfigurationdue to the constraint task allows better
mobility of the arm structure and the tracking accuracy
improves on both the end-effector task and the constraint
task.

6. CONCLUSIONS
This paper has studied the implementation of a kinematic
control scheme for the industrial manipulator COMAU
SMART-3 S. This is a six-joint robot with a non-spherical
wrist whose axes intersect two-by-two. In addition, the
manipulator is mounted on a sliding track which enlarges
its workspace and gives kinematic redundancy.

Figure 4 -Experimental results for the algorithm (21,lS):

norm of the constraint error.

A closed-loop inverse kinematics algorithm has been de-

Figure 3 and 4 report the results obtained by using the

inverse kinematics algorithm (21,lS). In this case, a constraint task must be assigned besides the above end-effector
task. We have assigned q1 to reach the final value 1.05m,

veloped to manage both the presence of the non-spherical

wrist and the available redundant degrees of freedom. Robustness to kinematic singularities has also been provided

54

by resorting to a damped least-squares technique. &nematic properties of the geometric structure of the robot arm
have been exploited to lighten the computational burden
of the algorithm.

-*

x IO

7. ACKNOWLEDGEMENTS

This work was supported by Minister0 delluniversith e della

Ricerca Scientificae Tecnologica under 60% and 40% funds.
8. REFERENCES

[ 11R.P. Paul and H. Zhang, Computationally efficient kinematics for manipulators with spherical wrists based on the homogeneous transformation representation,The International Journal
of Robotics Research, vol. 5, no. 2, pp. 32-44 (1986).

[2]V.I. Lumelsky, Iterativecoordinate transformation procedure

for one class of robots, IEEE Transactionson Systems, Man, and
Cybemetics, vol. 14, pp. 500-505 (1984).
[3] L. Sciavicco and B. Siciliano, Coordinate transformation:A
solution algorithm for one class of robots, IEEE Transactions
on Systems, Man, and Cybemetics, vol. 16, pp. 550-559 (1986).
[4]A. Balestrino, G . De Maria, L. Sciavicco, and B. Siciliano, An algorithmic approach to coordinate transformation for
robotic manipulators, Advanced Robotics, vol. 2, pp. 327-344
(1988).

[5] Y. Nakamura, H. Hanafusa, and T. Yoshikawa, Task-priority

based redundancy control of robot manipulators,Int. J. Robotics
Research, vol. 6, no. 2, pp. 3-15 (1987).
[6] A.A. Maciejewski and C.A. Klein, Obstacle avoidance for
kinematically redundant manipulators in dynamically varying
environments, The Intemational Joumal of Robotics Research.
vol. 4,n. 3, pp. 109-117 (1985).

Figure 5 - Experimental results for the reconfiguration

task: a) norm of the position errors (algorithm error:
dashed; servo error: solid); b) norm of the orientation
errors (algorithm error: dashed; servo error: solid).

[7] P. Chiacchio, S. Chiaverini, L. Sciavicco, and B. Siciliano,

Closed-loop inversekinematics schemes for constrained redundant manipulators with task space augmentation and task priority strategy,The Intemational Journal of Robotics Research,
vol. 10, n. 4, pp. 410-425 (1991).
[8] Y. Nakamura and H. Hanafusa, Inverse kinematic solutions
with singularityrobustness for robot manipulator control, Transactions of the ASME Joumal of Dynamic Systems, Measurement,
and Control, vol. 108, pp. 163-171 (1986).
[9] C.W. Wampler 11, Manipulator inverse kinematic solutions
based on vector formulations and damped least-squares methods, IEEE Transactions on Systems, Man, and Cybemetics,
vol. 16, n. 1, pp. 93-101 (1986).
[lo] S. Chiaverini, 0. Egeland, and R.K. Kanestr@m,Achieving user-defined accuracy with damped least-squares inverse
kinematics, Proc. 5th Intemational Conference on Advanced
Robotics (ICAR91),Pisa, I, pp. 672-677, June 1991.

Figure 6 - Experimental results for the reconfiguration

task: norm of the constraint error.

[ l l ] S. Chiaverim, Estimate of the two smallest singular values of the Jacobian matrix: application to damped least-squares
inversekinematics,Joumal ofRobotic Systems, vol. 10,pp. 9911008 (1993).

The inverse kinematics algorithm runs on a 486DX2/66 PC

connected to the C3G 9000 control unit of the robot
through a bus-to-bus communication link. The system
realizes a kinematic control scheme at a 500 Hz rate.

[12] S . Chiaverini, Inverse differential kinematics of robotic

manipulators at singular and near-singular configurations,I992
IEEE Intemational Conference on Robotics and Automation Tutorial on Redundancy: Performance Indices, Singularities
Avoidance, and Algorithmic Implementations,Nice, F, pp. 2-1-9,

Experimental results show the real-time feasibility of the

kinematic control scheme. Remarkably, despite the complexity of the inverse kinematics algorithm developed which both handles kinematic singularities and allows redundancy resolution-, relatively short computation times
are obtained with currently available low-cost hardware.

May 1992.
[13] E Dogliani, G. Magnani, and L. Sciavicco, An open architecture industrial controller,Newsletter of the IEEE Robotics
and Automation Society, vol. 7, n. 3, pp. 19-21, 1993.

55