Unified Dynamics Calculation employing
COG Jacobian for Link Mechanisms
Takashi Sonoda1*, Kazuo Ishii1 and Daigoro Isobe2
1
1

Kyushu Institute of Technology and 2University of Tukuba

2-4 Hibikino, Wakamatsu-ku, Kitakyushu 808-0196, Japan and2

1-1-1 Tennodai, Tsukuba-shi, Ibaraki 305-8573, Japan*e-mail: sonoda-takashi@edu.brain.kyutech.ac.jp

Abstract—We show the unified dynamics computation
employing Jacobian that relates the center of gravity (COG
Jacobian) to joints of link mechanisms. COG Jacobian is used forthe behavior planning and the control of humanoids. Also, it
usually expresses the relationship between the joints and COG ofa robot's whole body. However, this scheme that is suggested byus calculated regarding each link and not the robot's whole-body.
Moreover, we can obtain the torques of the active joints inmechanisms by relating COG Jacobian to the applied forces to
COG and by using principle of virtual work. The COG Jacobian
is obtained via the process calculating the accelerations at COG oflinks and the equation of motion expressed matrix forms by using
COG Jacobian. Therefore, we can compute the equation of
motion of a mechanism effectively, and the forward dynamics
which can be calculated by the obtained equation. We cancalculate the inverse dynamics regardless of open- and closed-linkmechanisms.

I. INTRODUCTION
Machines and robots are modeled into link mechanisms ofrigid-body when we compute their dynamics analyses. The linkmechanisms simplify structures of components and are
employed as the kinematic model. Various methods based to
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. And, 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 not only open- andclosed-link mechanisms, but applications to mechanisms
containing redundant actuators, collisions and contacts are also
expected. Therefore unified dynamics computation schemes
treating their issues are hoped. A Method of equations of
motion solving simultaneous equations of constraints
corresponding to each joint is used in a lot of present versatile
dynamics simulators [1]. However, this method needs very
heavy computation cost and long computation time at theanalyses of the complex structural mechanisms. Therefore, atclosed-link mechanisms, as an efficient method such as
Newton-Euler formulation are also required because theirmechanisms are usually complicated. The solution of equationof motion employing Lagrange multipliers has been known as
inverse dynamics for closed-link mechanisms, but it is notefficient. Then, after considering closed-link mechanisms to be. .
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.

virtual open-link mechanisms of tree structure temporarily andsolving as a problem of open-link mechanisms, there is amethod of solving inverse dynamics using Jacobian matrix
changing from generalized forces of open-link mechanisms to
the ones of closed-link mechanisms [2][3]. This method needs
to cut some arbitrary joints. Therefore, in complex mechanisms,
to choose the cutting joints and the generalized coordinates isproblems. Nakamura and Yamane show the method that is
employed as 6 DOF virtual links to all joints of each link as
developed method than the above method and apply tomechanisms with discontinuous changes of constraints [4], andthis method is the solution to above problems. However, thismethod also needs to compute as virtual open-link mechanisms.
The computation of the change processes is not necessary at
the unified scheme. Isobe suggests a method applying Finite
Element Method to inverse dynamics [5][6]. It is able torepresent equations of inverse dynamics as equations ofmatrices by coordinate transformation on dimensions of forces.
Therefore, it can be applied to mechanisms with changes of
constraints. The links are divided to some elements and somenodal points, and forces are applied to the nodal points without
distinguishing static forces and kinetic forces. As others, thereis the method that uses the motor algebra, and it enables toderive the equations of motion of open- and closed-linkmechanisms [7]. But, we have to solve simultaneous equationsof forces regarding passive joints at closed-link mechanisms.

Fi-1
i-1

.
pGi-1
.

Fi

.
p.Gi

i

link i ( i = 1,L, N ) are expressed asi+1

qi = g i (θ m ) ,

linki

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:

linki-1

Gi-1

k+1
.
k

k-1
Groundq& i = G i (θ m )θ& m ,k: Passive joint: Active joint

Gi (θ m ) =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 andcontrols 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.

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 thedimensions of joint i, n is number of the joints such as in shown
Fig.1. The COG Jacobian matrix of whole-body is described as(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 with holonomic
constraint are decided by the active joints only. The functionT
T T
gi (θ ) is given such that the orientations qi = [ pGi, θGi] of the

∂qi
,
∂θ m

(4)

Where
Gi = [Gi1 , L , GiM ] ,

(5)

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

Similarly, the link acceleration
written in the following form:(6)

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:⎡ G& 1 ⎤⎡ q&&1 ⎤ ⎡ G1 ⎤⎢ M ⎥ = ⎢ M ⎥θ&& + ⎢ M ⎥θ&⎢ ⎥ ⎢ ⎥ m ⎢ ⎥ m⎢G& ⎥⎢⎣q&&N ⎥⎦ ⎢⎣G N ⎥⎦
⎣ N⎦
&
&
&
&
q&& = Gθ + Gθ ,II. DYNAMICS EMPLOYENG COG JACOBIAN∂q G 0 (q)
.
∂q

(3)

where the following matrix is the COG Jacobian matrix of thelink i:Fig. 2 Multi-rigid-body model. And, the forces and the moments
regarding the inertia by the motion are applied to all rigid-bodies.

G 0 (q ) =

(2)

m

(8)

(9)

m

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.
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 ⎥⎦(12)

F = Mq&& + b .

(13)

Next, we compute τ mi = [τ 1i ,L,τ Mi ] which is the torqueTvector regarding to the link i. We obtain τ mi by the product of

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
θ&&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 efficientmethods such as the ones shown in [12].

the transposed matrix of Gi and Fi like we do in static
dynamics. Thus, τ mi is computed byτ mi = GiT Fi .

(14)

And we obtain the torques of all active joints by usingτ m = GT F ,(15)
T⎡ τ1 ⎤ ⎡G11T G 21
⎢τ ⎥ ⎢T⎢ 2 ⎥ = ⎢ 0 G 22
⎢M ⎥ ⎢ MM⎢ ⎥ ⎢0⎣ τ N ⎦ ⎢⎣ 0

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 (θ , θ& ),mmm

D. Features of COG Jacobian Matrices
COG Jacobian matrices have the feature that the elements ofthe 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 as
L G NT 1 ⎤ ⎡ F1 ⎤⎥⎢ ⎥L G NT 2 ⎥ ⎢ F2 ⎥.
O
M ⎥⎢ M ⎥⎥⎢ ⎥T
L G NN⎥⎦ ⎣ FN ⎦

(21)

(16)
The COG Jacobian matrix G is expressed as the form thatis upper triangular matrix because the torques have correlation:

m

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

F2
(17)

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,
3. compute τ m using H (θ ) and B (θ , θ& ) .
Moreover, in the case that the end-effecters of a mechanism
is applied the external forces which is denoted by fe, (16) iswritten the following form by using the Jacobian matrix J thatconnects the active joints and the end-effecters:
τ m = H (θm )θ&&m + B (θ m , θ&m ) + J T f e .

2

link2

(18)

(19)

F1

link1

linkN-1

FN-1

N

FN
linkN

1
: Active joint(a)Robot arm model of serial mechanisms.

fe1 Object fe2
F1

F2
2

1

: 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 a robot
hand model picking a object.

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

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 isexpressed as⎡ τ1 ⎤ ⎡G11T
0⎢τ ⎥ = ⎢TG0⎣ 2⎦ ⎣
22⎡ F1 ⎤⎢ ⎥⎤0 ⎢ F2 ⎥,⎥J 2T ⎦ ⎢ f e1 ⎥⎢ ⎥⎣ f e2 ⎦J 1T
0⎡− lG1 sin θ1 0⎤ &⎡θ ⎤ ,= ⎢⎢ lG1 cosθ1 0⎥⎥ ⎢ 1 ⎥θ&⎢⎣10⎥⎦ ⎣ 2 ⎦(26)

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

(22)

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

⎡− 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
q&&1 = G1 (θm )θ&&m + G& 1 (θ m )θ&m ,q&& = G (θ )θ&& + G& (θ )θ& ,
2

2

m

m

2

m

m

(28)
(29)

where
III. EXAMPLESAt first, the proposed scheme employing COG Jacobianmatrices is applied to an open-link mechanism with 2 links (seeFig.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 (seeFig.4(c)).
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 totwo together. The vector qi that expresses the orientation oflink i is calculated by the following equations:⎡lG1 ⎤
θ ⎢q1 = g1 (θm ) = Rz ⎢ 0 ⎥⎥ ,⎢⎣ θ1 ⎥⎦
⎡lG 2 ⎤
⎡ l1 ⎤
q2 = g 2 (θm ) = Rzθ1 ⎢⎢ 0 ⎥⎥ + Rzθ1 +θ 2 ⎢⎢ 0 ⎥⎥ ,⎢⎣ θ 2 ⎥⎦⎢⎣θ1 ⎥⎦
1i− sin θ icosθ i
0

0⎤
0⎥⎥ .
1⎥⎦

12

G2

1

2

1

2

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

(30)

(31)
(32)
(33)
(34)

and
(23)

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

(35)

Substituting (28) and (29) for (10), we obtain the forces as(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⎡cosθ iRθz = ⎢⎢ sin θ i⎢⎣ 0⎡− 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 ⎥⎦
K11 = −lG 2 (θ&1 + θ&2 ) cos(θ1 + θ 2 ) − θ&1l1 cosθ1 ,K = −l (θ& + θ& ) cos(θ + θ ),⎡ F1 ⎤ ⎡ M 1 0 ⎤ ⎡ q&&1 ⎤(36)⎢ F ⎥ = ⎢ 0 M ⎥ ⎢q&& ⎥ .
2 ⎦⎣ 2 ⎦⎣ 2⎦ ⎣
Finally, the torques of the active joints is obtained byT⎡τ 1 ⎤ ⎡ G1 ⎤ ⎡ F1 ⎤⎢τ ⎥ = ⎢G ⎥ ⎢ F ⎥ .
⎣ 2⎦ ⎣ 2⎦ ⎣ 2⎦

(37)

(25)

Next, we compute the velocities and the accelerations at the
COG of the link i.
The velocities are expressed as

Figure 5 shows the target motion of the mechanism (lengthof each link: 0.10 (m); weight: 1.54 x 10-2 (kg); moment ofinertia: 1.29 x 10-5 (kgm2); COG at midpoint). Then, we obtainthe 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 the

2

2

F2
q2

2

2

q2

link1

F1
1

1

1

2
link1

1

4

3
3
3

link3

F3

q3
4

1

1

3

q2

q1

F1

q3

4

1

F2
link2

3

F3

1

1

1

3
3
link3

q1

F1

q1

2

3

link2

2

link2

link1

F2

4

1

: Passive joint: Active joint(a) Model of serial link mechanisms with 2 (b) Model of closed link mechanisms without (c) Model of closed link mechanisms with alinks.
redundant actuators.
redundant actuator.
Fig. 4 Three types of link mechanisms for the dynamic analysis examples employing the COG Jacobian matrices.

functions 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 = ⎢⎢q2 ⎥⎥ = ⎢⎢ g 2 (θ3 )⎥⎥ .
(39)⎢⎣q3 ⎥⎦ ⎢⎣ g3 (θ3 ) ⎥⎦Next, differentiate (39) with respect to time, then q& isexpressed as⎡ G1 ⎤&q = G (θ m )θ&m = ⎢G2 ⎥θ&3 ,⎢ ⎥⎢⎣G3 ⎥⎦
Gi =

∂gi
.
∂θ3

⎡ G1 ⎤ &⎡θ ⎤&q& = G (θ m )θ m = ⎢⎢G2 ⎥⎥ ⎢ 1 ⎥,θ&⎢⎣G3 ⎥⎦ ⎣⎢ 3 ⎦⎥(45)

⎡ ∂g ∂g ⎤Gi = ⎢ i , i ⎥ .
⎣ ∂θ1 ∂θ3 ⎦

(46)

where

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

T

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

(40)

(41)

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.

Now we have G , and F is obtained by using (13). Since thetorque τ 3 of the active joint θ 3 is computed by

3

2

T

⎡ G1 ⎤ ⎡ F1 ⎤[τ 3 ] = ⎢⎢G2 ⎥⎥ ⎢⎢F2 ⎥⎥ .
⎢⎣G3 ⎥⎦ ⎢⎣ F3 ⎥⎦(42)
1

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

4

Fig. 7 Target motion

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 = ⎢⎢q2 ⎥⎥ = ⎢⎢ g 2 (θ1 , θ3 )⎥⎥ ,⎢⎣q3 ⎥⎦ ⎢⎣ g3 (θ1 , θ3 ) ⎥⎦

(47)

(44)
Fig. 8 Torque curves

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 tovarious link mechanisms for dynamics and show the numericalexamples of the three mechanisms of difference type together
in this paper. The proposed method employs COG Jacobianmatrices for dynamics analyses. We can derive efficiently theequation 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. Those features show thatdynamics computation employing COG Jacobian is effective
as a scheme that expresses the dynamics of link mechanisms.
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.
[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.

1

2

Fig. 5 Target motion

Fig. 6 Torque curves[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.1152-1159, 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 Conferenceon 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.