Dynamics Computation of Link Mechanisms Employing
COG Jacobian
Takashi Sonoda and Kazuo Ishii

Daigoro Isobe

Department of Brain Science and Engineering
Kyushu Institute of Technology
2-4 Hibikino, Wakamatsu-ku, Kitakyushu 808-0196, Japansonoda-takashi@edu.brain.kyutech.ac.jp
ishii@brain.kyutech.ac.jp

Department of Engineering Mechanics and Energy
University of Tsukuba
1-1-1 Tennodai, Tsukuba-shi, Ibaraki 305-8573, Japan

Abstract - We show the dynamics computation employing
Jacobian that relates the center of gravity (COG) to joints of linkmechanisms, in this paper. COG Jacobian is used for the
behavior planning and the control of humanoids. And, it usually
expresses the relationship between the joints and COG of arobot's whole body. However, in this scheme, it’s calculated
regarding each link and not the robot's whole-body. Moreover,
we can obtain the torques by relationship between COG
Jacobian and the applied forces to COG by using principle ofvirtual work. The loaded forces to COG can be obtained by
employing Newton’s and Euler’s equations of motion. By thescheme, we can calculate the inverse dynamics regardless of
open- and closed-link mechanisms. In addition, the forward
dynamics can be calculated by employing COG Jacobian.
Index Terms -Dynamics, Link Mechanism, COG Jacobian.

I. INTRODUCTION
Machines and robots are modeled into link mechanismsof rigid-body when we compute their dynamics analyses. The
link mechanisms simplify structures of components and are
employed as the kinematic model. Various methods based on
Newton-Euler equation of motion are developed as dynamics
of link mechanisms. In serial mechanisms, Newton-Euler
formulation that is efficient and is able to be solved atcomputation cost proportioned to DOF of a mechanism is
proposed. Also, the laws of an inertial force occurring at thecenter of gravity (COG) of the link that is moving andbalancing with external forces are used in this method.
Moreover, issues of dynamics are application to not only
open- and closed-link mechanisms, but mechanisms
containing redundant actuators, collisions and contacts.
Therefore unified dynamics computation schemes treating
their issues are hoped. A method of numerical analysis solving
simultaneous equations of constraints corresponding to eachjoint is used a lot of present versatile dynamics simulators [1].
However, this method needs very heavy computation cost andlong computation time at the analyses of the complex
structural mechanisms. Therefore, at closed-link mechanisms,
as an efficient method such as Newton-Euler formulation are
also required because their mechanisms are usuallycomplicated. The solution of equation of motion employing
Lagrange multipliers has been known as inverse dynamics for
closed-link mechanisms, but it is not efficient. Afterconsidering closed-link mechanisms to be temporal virtual
open-link mechanisms of tree structure, there is a method of

isobe@kz.tsukuba.ac.jp. .
pi-1, i-1
. .
pi, i

. .
pG0, G0. .
pi+k, i+k
: COG
Fig. 1 Humanoid model. COG Jacobian is usually calculated to COG
of the whole-body of the humanoid. And, it shows the relationshipbetween the joints and COG of the humanoid.

solving inverse dynamics using Jacobian matrix changing
from generalized forces of open-link mechanisms to the onesof closed-link mechanisms [2][3]. However, though thismethod needs to cut some arbitrary joints, to choose thecutting joints and the generalized coordinates is problems incomplex mechanisms. Nakamura and Yamane show themethod that is employed as 6 DOF virtual links to all joints ofeach link as developed method than the above method and it isapplied to mechanisms with discontinuous changes of
constraints [4]. According to this method, these above
problems are solved, but it also needs to compute as virtual
open-link mechanisms. The computation of the change
processes is not necessary at the unified scheme. Isobesuggests a method applying Finite Element Method to inverse
dynamics [5][6]. It is able to represent equations of inverse
dynamics as equations of matrices by coordinatetransformation on dimensions of forces. Therefore, it can beapplied to mechanisms with changes of constraints. The links
are divided to some elements and some nodal points, andforces are applied to the nodal points without distinguishingstatic forces and kinetic forces. As others, there is the methodthat uses the motor algebra, and it enables to derive theequations of motion of open- and closed-link mechanisms [7].
However, we have to solve simultaneous equations of forces
regarding passive joints at closed-link mechanisms.
In this paper, we show a computational method that is
employing Jacobian connecting COG of links with active
joints. This Jacobian usually connects COG of the whole-bodywith active joints, and it is used for motion planning and

i

Fi-1
i-1

.
pGi-1
.

Fi

.
p.Gi

where pGi and θGi are the dimensions of the COG of the link i,θ m ∈ R M is the vector expressing the displacements of theactive joints, N is number of the links and M is number of theactive joints. Thus, the link velocity q&i at the COG of link i iswritten in the following form:

i+1
linki

Gi

linki-1

Gi-1

k+1

q& i = G i (θ m )θ& m ,

.
k

k-1
Ground(3)

where the following matrix is the COG Jacobian matrix of thelink i:k: Passive joint: Active joint

Gi (θm ) =Fig. 2 Multi-rigid-body model. And, the forces and the moments
regarding the inertia by the motion are applied to all rigid-bodies.

∂gi
,
∂θm

(4)

Where

controls of humanoids [8]-[11]. This method is deriving COG
Jacobian matrices that connect COG of each link with active
joints, and calculating the inverse dynamics by using theprinciple of virtual work. In this method, the equation of
motion is derived efficiently because COG Jacobian matrices
can be derived in the derivation process of velocities oraccelerations. Moreover, it has the feature that is able torepresent equations of inverse dynamics as equations ofmatrices. As the examples, we show the applications to an
open-link mechanism, a close-link mechanism, and amechanism with redundant actuators.
II. DYNAMICS EMPLOYENG COG JACOBIAN

(5)

∂g i
G ik =, (k = 1, L , M ) .
∂θ mk

(6)

Similarly, the link acceleration
written in the following form:

at the COG of link i is

q&&i = G i (θ m )θ&&m + G& i (θ m )θ& m ,(7)where G& i = [G& i1 ,L , G& iM ] is the time-derivative of Gi (θ ) .
Equation (7) can be combined with equations of all links asfollowing equation:

A. COG JacobianThis section presents the differences between the general
COG Jacobian and the COG Jacobian of the proposed method.
The orientation of COG of robot is denoted by
qG 0 = [ pGT 0 , θGT 0 ]T , and the orientation vector regarding to all
joints is denoted by q = [ p1T ,L, pnT , θ1T ,L, θ nT ]T , where pG0
and θG0 are the dimensions of COG of the robot, pi and θi are
the dimensions of joint i, n is number of the joints such as inshown Fig.1. The COG Jacobian matrix of whole-body is
described as

Gi = [Gi1 ,L , GiM ] ,⎡ G& 1 ⎤⎡ q&&1 ⎤ ⎡ G1 ⎤⎢ M ⎥ = ⎢ M ⎥θ&& + ⎢ M ⎥θ&⎢ ⎥ ⎢ ⎥ m ⎢ ⎥ m⎢G& ⎥⎢⎣q&&N ⎥⎦ ⎢⎣G N ⎥⎦
⎣ N⎦
&
&
&
&
q&& = Gθ m + Gθ m ,

(8)
(9)

where G = [G1T ,L, G NT ]T ∈ R N ×M is the COG Jacobian matrix
regarding to the all links and G& = [G& T ,L, G& T ]T ∈ R N ×M is
1

N

same.

G 0 (q ) =∂q G 0 (q)
.
∂q

(1)

The COG Jacobian matrix is only one in this case. However,
the COG Jacobian matrices are derived in relation to each linkin the suggested method by us as in shown Fig.2.
At first, the orientations of the mechanisms withholonomic constraint are decided by the active joints only.
The function gi (θ ) is given such that the orientations
T
T T
qi = [ pGi, θGi] of the link i ( i = 1,L, N ) are expressed as

q i = g i (θ m ) ,(2)

B. Inverse Dynamics
The external force vector Fi around the COG of the link i

is expressed as the following equation:
Fi = M i q&&i + b(qi , q&i ) ,(10)where M i is the mass matrix of the link i, b(qi , q&i ) is the term
regarding gravitational forces etc. And,⎡f ⎤Fi = ⎢ Gi ⎥ ,⎣ nGi ⎦

(11)

where f Gi and nGi are the forces of the translation and therotation around the COG of the link i. Equation (9) can be
combined with all links as

0 ⎤ ⎡ q&&1 ⎤ ⎡ b1 ⎤⎡ F1 ⎤ ⎡ M 1⎢ M ⎥=⎢⎥⎢ M ⎥ + ⎢ M ⎥O⎢ ⎥ ⎢⎥⎢ ⎥ ⎢ ⎥⎢⎣ FN ⎥⎦ ⎢⎣ 0
M N ⎥⎦ ⎢⎣q&&N ⎥⎦ ⎢⎣bN ⎥⎦F = Mq&& + b .

(12)

Next, we compute τ mi = [τ 1i ,L,τ Mi ] which is the torquevector regarding to the link i. We obtain τ mi by the product of
the transposed matrix of Gi and Fi like we do in static
dynamics. Thus, τ mi is computed by(14)

And we obtain the torques of all active joints by usingτ m = GT F ,(15)

where τ m = [τ 1T , L , τ TM ]T .
Using (9) and (13), (15) is written asτ m = G T MGθ&&m + G T MG& θ&m + G T b= H (θ )θ&& + B (θ , θ& ),
m

m

m

(19)

C. Forward Dynamics
We show the primitive method of forward dynamics
employing COG Jacobian.
At first, derive the equation of motion using (16). Next,
solve the derived equation for θ&&m . Then, we obtain thesolutions by

(13)

T

τ mi = GiT Fi .

τ m = H (θ m )θ&&m + B (θ m , θ&m ) + J T f e .

(16)

m

θ&&m = H (θm ) −1{τ m − B (θm , θ&m )} .

(20)

However, computation of the inverse matrix of
H (θ ) needs the large computation cost, when a mechanism
has many active joints. Therefore, we suggest using more
efficient methods such as the ones shown in [12] or [13].
D. Features of COG Jacobian Matrices
COG Jacobian matrices have the feature that the elementsof the matrices are decided by a structure of mechanisms. It isshown by the following examples.
Fig. 3 is examples of mechanisms. (a) in Fig.3 shows aprimitive open-link mechanism with N links. If we alreadyobtain G , then the torques of the active joints is expressed asT⎡ τ1 ⎤ ⎡G11T G 21
⎢τ ⎥ ⎢T⎢ 2 ⎥ = ⎢ 0 G 22
⎢M ⎥ ⎢ M
M
⎢ ⎥ ⎢
τ
0
⎣ N ⎦ ⎣⎢ 0

L G NT 1 ⎤ ⎡ F1 ⎤⎥⎢ ⎥L G NT 2 ⎥ ⎢ F2 ⎥.
O
M ⎥⎢ M ⎥⎥⎢ ⎥T
L G NN⎦⎥ ⎣ FN ⎦

(21)

where
H (θ m ) = G MG ,B (θ m , θ&m ) = G T ( MG& θ&m + b) .
T

(17)
(18)

Equation (16) is the form of the generalized equation of
motion. Thus, we find the following relationships: H (θ ) is theinertia matrix, and B (θ , θ& ) is the term regarding centrifugal,
colliolis and gravitational forces.
Computations for inverse dynamics employing COG
Jacobian consist of following three steps:
1. compute q , q& and q&& using (2), (3) and (7),2. compute H (θ ) and B (θ , θ& ) using obtained G and G& in

step 1,compute τ m using H (θ ) and B (θ , θ& ) .
Moreover, in the case that the end-effecters of amechanism is applied the external forces which is denoted byfe, (16) is written the following form by using the Jacobianmatrix J that connects the active joints and the end-effecters:
3.

The COG Jacobian matrix G is expressed as the form
that is upper triangular matrix because the torques havecorrelation: i.e. the torques of the active joints are affected by
the torques of the active joints connected towards the end link
than itself.
Next, (b) in Fig. 3 shows a robot hand picking an object.
If we already obtained G , then the torques of the active joints
is expressed as⎡ τ1 ⎤ ⎡G⎢τ ⎥ = ⎢⎣ 2⎦ ⎣ 0

T
11

0
T
G 22

T
1

J
0

⎡ F1 ⎤⎢ ⎥0 ⎤ ⎢ F2 ⎥,⎥J 2T ⎦ ⎢ f e1 ⎥⎢ ⎥⎣ fe2 ⎦

(22)

where Ji and fei are the Jacobian matrices and the reaction
forces. If fei are known, we can consider as that the left andthe right finger are the independent mechanisms. Therefore,
because the left and the right finger have no correlation, theelements except diagonal ones of G are zero.

F2
2

link2

F1

link1

linkN-1

FN-1⎡cosθ iRz = ⎢⎢ sin θ i
⎢⎣ 0

N

FN

linkN

1
: Active joint

fe1 Object fe2cosθ i0(25)

q&1 =G 1(θ m )θ&m

F2⎡− lG1 sin θ1 0⎤ &⎡θ ⎤ ,= ⎢⎢ lG1 cosθ1 0⎥⎥ ⎢ 1 ⎥θ&⎢⎣10⎥⎦ ⎣ 2 ⎦

2

1

0⎤
0⎥⎥ .
1⎥⎦

Next, we compute the velocities and the accelerations at the
COG of the link i.
The velocities are expressed as(a) Robot arm model of serial mechanisms.

F1− sin θ i

θi

(26)

q& 2 =G 2 (θm )θ&m

: Active joint(b) Robot hand model.
Fig. 3 Examples for showing the features of COG Jacobian matrices
on two types of mechanisms. (a) is a serial link mechanism. (b) is arobot hand model picking a object.

⎡− lG 2 sin(θ1 + θ 2 ) − l1 sin θ1= ⎢⎢ lG 2 cos(θ1 + θ 2 ) + l1 cosθ1⎢⎣1− lG 2 sin(θ1 + θ 2 )⎤⎡θ& ⎤lG 2 cos(θ1 + θ 2 ) ⎥⎥ ⎢ 1 ⎥θ&⎥⎦ ⎣ 2 ⎦1(27)

and the accelerations are expressed similarly as
III. NUMERICAL EXAMPLESAt first, the proposed scheme employing COG Jacobianmatrices is applied to an open-link mechanism with 2 links(see Fig.4(a)). Next, we show the application to a closed-linkmechanisms without redundant actuators (see Fig.4(b)).
Finally, we apply to a mechanism with redundant actuators(see Fig.4(c)).

q&&1 = G1 (θ m )θ&&m + G& 1 (θ m )θ&m ,q&& = G (θ )θ&& + G& (θ )θ& ,
2

m

2

m

m

⎡− lG1θ&1 cosθ1 0⎤⎢⎥
G& 1(θm ) = ⎢ − lG1θ&1 sin θ1 0⎥ ,
⎢
0
0⎥⎦
⎣
⎡ K11 K12 ⎤G& 2 (θ m ) = ⎢⎢ K 21 K 22 ⎥⎥ ,
⎢⎣ 0
0 ⎥⎦

to two together. The vector qi that expresses the orientationof link i is calculated by the following equations:

(23)

(29)

(30)

(31)

K11 = −lG 2 (θ&1 + θ&2 ) cos(θ1 + θ 2 ) − θ&1l1 cosθ1 ,K = −l (θ& + θ& ) cos(θ + θ ),(32)

K 21 = −lG 2 (θ&1 + θ&2 ) sin(θ1 + θ 2 ) − θ&1l1 sin θ1 ,(34)

K 22 = −lG 2 (θ&1 + θ&2 ) sin(θ1 + θ 2 ).

(35)

12

G2

1

2

1

2

(33)

and

(24)

where li is the length of the link i, lGi is the length from the end
to the COG of the link i, and

m

where

A. Open-Link Mechanism
The active joints of the mechanism in the shown Fig. 4(a)are denoted by θm = [θ1 ,θ 2 ]T . The number N and M are equal⎡lG1 ⎤
q1 = g1 (θ m ) = Rzθ1 ⎢⎢ 0 ⎥⎥ ,⎢⎣ θ1 ⎥⎦
⎡lG 2 ⎤
⎡ l1 ⎤
θ1 ⎢ ⎥
θ1 +θ 2 ⎢q 2 = g 2 (θm ) = Rz ⎢ 0 ⎥ + Rz ⎢ 0 ⎥⎥ ,⎢⎣ θ 2 ⎥⎦⎢⎣θ1 ⎥⎦

2(28)

Substituting (28) and (29) for (10), we obtain the forcesas⎡ F1 ⎤ ⎡ M1
⎢F ⎥ = ⎢ 0⎣ 2⎦ ⎣

0 ⎤ ⎡ q&&1 ⎤.
M 2 ⎥⎦ ⎢⎣q&&2 ⎥⎦

(36)

2

2

F2
q2

2

2

link1

F1
1

1

1

q1

F1

q1

1

1

1

q2

3
3

4

q2

q1

F1

q3

1

1

(b) Model of closed link mechanisms without
redundant actuators.

3

4: Passive joint(a) Model of serial link mechanisms with 2
links.

3
3
link3

F3

1

1

3

link2
link1

link3

4

F2

2
3

F3

1

2

3

link2

2

link2

link1

F2

q3
4

: Active joint(c) Model of closed link mechanisms with aredundant actuator.

Fig. 4 Three types of link mechanisms for the dynamic analysis examples employing the COG Jacobian matrices.

Finally, the torques of the active joints is obtained byT⎡τ 1 ⎤ ⎡ G1 ⎤ ⎡ F1 ⎤⎢τ ⎥ = ⎢G ⎥ ⎢ F ⎥ .
⎣ 2⎦ ⎣ 2⎦ ⎣ 2⎦

(37)

Figure 5 shows the target motion of the mechanism(length of each link: 0.10 (m); weight: 1.54 x 10-2 (kg);moment of inertia: 1.29 x 10-5 (kgm2); COG at midpoint).
Then, we obtain the torque curves of the active joints using(37) (see Fig. 6).

B. Closed-Link Mechanism without Redundant Actuators
The active joint of the mechanism are denoted by θ m = θ 3 .
And, N = 3, and M = 1 of the mechanism. The passive joint
angle θ1 and θ 4 can be expressed by θ 3 only: i.e. they are the1

2Fig. 5 Target motionfunctions of θ 3 as

θ1 = θ1 (θ 3 ) and θ 4 = θ 4 (θ 3 ) .

(38)

Therefore, the vector q is expressed by the following
equations:⎡ q1 ⎤ ⎡ g1 (θ3 ) ⎤q = ⎢⎢q 2 ⎥⎥ = ⎢⎢ g 2 (θ3 )⎥⎥ .
⎢⎣ q3 ⎥⎦ ⎢⎣ g3 (θ3 ) ⎥⎦(39)

Fig. 6 Torque curves
T

Next, differentiate (39) with respect to time, then q& isexpressed as⎡ G1 ⎤
q& = G (θ m )θ&m = ⎢⎢G 2 ⎥⎥θ&3 ,⎢⎣G3 ⎥⎦

Gi =

∂g i
.
∂θ3

(40)

(41)

Now we have G , and F is obtained by using (13). Since
the torque τ 3 of the active joint θ 3 is computed by⎡ G1 ⎤ ⎡ F1 ⎤[τ 3 ] = ⎢⎢G2 ⎥⎥ ⎢⎢F2 ⎥⎥ .
⎢⎣G3 ⎥⎦ ⎢⎣ F3 ⎥⎦(42)

C. Closed-Link Mechanism with Redundant Actuators
The active joint of the mechanism are denoted byθ m = [θ1 ,θ 3 ]T . And, N = 3, and M = 2 of the mechanism. The
passive joint angle θ 4 is the functions of θ1 and θ 3 as

θ 4 = θ 4 (θ1 ,θ 3 ) .

(43)

Therefore, q and q& are expressed as follows:⎡ q1 ⎤ ⎡ g1 (θ1 , θ3 ) ⎤q = ⎢⎢q 2 ⎥⎥ = ⎢⎢ g 2 (θ1 , θ3 )⎥⎥ ,⎢⎣q3 ⎥⎦ ⎢⎣ g3 (θ1 , θ3 ) ⎥⎦⎡ G1 ⎤ &⎡θ ⎤&q& = G (θ m )θm = ⎢⎢G 2 ⎥⎥ ⎢ 1 ⎥,⎢⎣θ&3 ⎥⎦⎣⎢G3 ⎦⎥

3

2

(44)
1

(45)

4

Fig. 7 Target motionwhere⎡ ∂g ∂g ⎤Gi = ⎢ i , i ⎥ .
⎣ ∂θ1 ∂θ3 ⎦(46)

And the torque and are computed by⎡ G1 ⎤⎡τ 1 ⎤ ⎢ ⎥⎢τ ⎥ = ⎢G 2 ⎥⎣ 3 ⎦ ⎢G ⎥⎣ 3⎦

T

⎡ F1 ⎤⎢F ⎥ .
⎢ 2⎥⎢⎣ F3 ⎥⎦

(47)

In this paper, we don’t discuss about the optimization of theactuational redundancy. However, it is enabled by using
Jacobian matrices that are obtained from the constraint
equations of joints (hints shown in Ref. [2]-[4]).
Figure 7 shows the target motion for the closed-linkmechanisms of the subsection B and C (length of each link:
0.20 (m); weight: 0.628 (kg); moment of inertia: 2.11 x 10-3
(kgm2); COG at midpoint). We obtain the torque curves of theactive joints using (42) and (47) (see Fig. 8).
IV. CONCLUSION
We propose the unified scheme that is able to be applied
to various link mechanisms for dynamics and show thenumerical examples of the three mechanisms of difference
type together in this paper. The proposed method employs
COG Jacobian matrices for dynamics analyses. We can derive
efficiently the equation of motion by using it.
COG Jacobian matrices are able to be obtained in a
deriving process of velocities or accelerations. They has
feature which COG matrices are affected by structures of amechanism. Moreover, an obtained equation of motion isexpressed as the matrix form. Consequently, those features
show that dynamics computation employing COG Jacobian iseffective as a scheme that expresses the dynamics of linkmechanisms.
REFERENCES[1] E.J. Haug, Computer Aided Kinematics and Dynamics of Mechanical
System. Boston: Allyn and Bacon, 1989.
[2] Y. Nakamura: “Dynamics Computation of Parallel Mechanisms,” Journalof the Robotics Society of Japan, Vol.10, No.6, pp.709-714, 1992.

Fig. 8 Torque curves[3] T. Yoshikawa, Y. Yokokohji and N. Watanabe: “Dynamics Analysis of
Closed Link Mechanisms with Redundant Actuators,” Journal of the
Robotics Society of Japan, Vol.11, No.6, pp.913-917, 1993.
[4] Y. Nakamura, K. Yamane and F. Nagashima: “Dynamics Computation of
Structure-Varying Kinematic Chains and Its Application to Human
Figure,” Journal of the Robotics Society of Japan, Vol.16, No.8, pp.11521159, 1998.
[5] D. Isobe: “A Unified Numerical Scheme for Calculating Inverse
Dynamics of Open/Closed Link Mechanisms,” Proceedings of the 27th
Annual Conference of the IEEE Industrial Electronics Society IECON'01,
pp.341-344, 2001.
[6] D. Isobe, A. Yagi and S. Sato: “General-Purpose Expression of Structural
Connectivity in the Parallel Solution Scheme and Its Application,” JSMEInternational Journal Series C, Vol.49, No.3, pp.789-798, 2006.
[7] K. Sugimoto: “Deviation of Equation of Motion of Closed Loop
Mechanisms,” Journal of the Robotics Society of Japan, Vol.15, No.3,
pp.460-467, 1997.
[8] T. Sugihara and Y. Nakamura: “Whole-body Cooperative Reaction Force
Manipulation on Legged Robots with COG Jacobian involving Implicit
Representation of Unactuated Coordinates,” Journal of the Robotics
Society of Japan, Vol.24, No.2, pp.222-231, 2007.
[9] R. Boulic, R. Mas and D. Thalmann: “Inverse Kinetics for Center of Mass
Position Control and Posture Optimizaion,” Proceedings of European
Workshop on Combined Real and Synthetic Image Processing forBroadcast and video Production, 1994.
[10]P. Baerlocher, Inverse kinematics techniques of the interactive posturecontrol of articulated figures. PhD thesis, Lausanne, 2001.
[11]Y. Tamiya, M. Inaba and H. Inoue: “Realtime Balance Compensation for
Dynamics Motion of Full-Body Humanoid Standing on One Leg,” Journalof the Robotics Society of Japan, Vol.17, No.2, pp.268-274, 1999.
[12]K. Yamane and Y. Nakamura: “Efficient Parallel Dynamics Computationof Human Figure,” Proceedings of the 2002 IEEE International
Conference on Robotics & Automation, pp.530-537, 2002.
[13]R. Featherstone: “A Divide-and-Conquer Articulated-Body Algorithm for
Parallel O(log(n)) Calculation of Rigid-Body Dynamics,” International
Journal of Robotics Research, Vol.18, No.9, pp.867-892, 1999.