Interactive Identification of the Center of Mass Reachable
Space for an Articulated Manipulator*
Ronan Boulic1, Ramon Mas 2, Daniel Thalmann1
(1) Computer Graphics Lab (LIG), Swiss Federal Institute of Technology (EPFL)
CH-1015 Lausanne, Switzerland
(2) Department of Mathematics and Computer Science,UIB, 07071-Palma, Spain
We propose an efficient kinematic position control for the center of mass of an articulated arm. The
algorithm integrates the context of multiple joints reaching their limit value together with a singular
configuration on the remaining joint space. We demonstrate the resulting fine control by interactively
outlining the reachable area borders of the center of mass (in a 2D context for clarity). Moreover, the
identification is also possible under any position or orientation constraint of an end effector. This tool
can easily be integrated in the early conception stage of mobile articulated structures combining strong
requirements on reachability and balance.
Key Words : Inverse Kinematics, Center of Mass, Reachable Space, Singularity, Joint Limits
1
Introduction
In this paper we focus on interactive identification of the center of mass reachable space. For that purpose
we need a fast and robust position control of the center of mass integrating both the singular cases and
joint limits in a unified framework. Our approach only requires the topological, geometrical, and mass
distribution information of the articulated figure. We extend the range of Inverse Kinematics by
incorporating the mass distribution information in the computation of the jacobian matrix. The
underlying control architecture of main and secondary tasks is used to associate the position control of the
center of gravity with other constraints on end effectors.
First, we recall relevant previous work. Then we derive the computation of the so-called Kinetic jacobian
matrix by differentiating the barycentre formula with respect to time. In the fourth section we present the
Inverse Kinetics control scheme and the special case of cascaded control which allows the hierarchical
combination of end effector control with center of mass control. The next section details the pseudoinverse computation algorithm allowing to overcome the artifact encountered with simultaneous
singularity and joint limits. We display several 2D examples highlighting the inherent robustness of our
approach.
2
Background
Numerous approaches have been proposed to improve the Inverse Kinematic control of articulated
structures. We recall here those most relevant to our context. In [1] the manipulator kinetic energy is
minimized when weighting the Jacobian matrix with the inertia matrix (for the control of an end effector).
In [2] another author describes how the naive use of Inverse Kinematics can lead to unrealistic demands
on mechanical performances. He proposes a solution integrating the kinetic energy to alleviate this
problem. Although handling the motion kinetics, these approaches were not dedicaded to the position
control of the center of mass. We present our approach in section 3. The singularity problem has been
nicely tackled in [3] with the so-called Damped Least Square Pseudo-Inverse (in short DLSPI). This
method relies on the Singular Value Decomposition [4] and smoothly guides to zero the norm of the
solution around a singularity. However, the handling of the joint limits is not integrated in the
computation of the DLSPI. We develop our solution in section 4.
3
Derivation of the Kinetic Jacobian Matrix
Let us first recall the concept of the augmented body associated with a joint in an articulated chain. This
body is the imaginary rigid body which is supported by this joint at the current state of the system.
Figure 1 shows the center of mass of all the augmented bodies of an articulated chain and the augmented
body associated with the joint indexed by j. Our method operates in a static context. It consists in
evaluating the instantaneous kinetic influence of each joint by relating an instantaneous unit joint
*Published in Proc. of International Conference of Advanced Robotics ICAR'97, pp 589-594, Monterey
July 1997
Augmented
Body j
Oj
Gaj
G
Center of
of the whole
VGaj
Joint
Center of Mass of one Augmented
Fig. 1. Centers of mass of the augmented bodies for one
variation with the corresponding instantaneous translation of the total center of mass (Direct Kinetics). In
a second stage the kinetic jacobian matrix is inverted as with Inverse Kinematics. However, since it
integrates the mass distribution information, we call it Inverse Kinetics.
3.1
Barycentre Formula Differentiation
Let θ be the parameter vector of an open articulated chain made of n elementary bodies. Each elementary
body i has a mass mi , a local origin Oi and a local center of gravity Gi . For the sake of clarity, the
origin O1 is located on the origin of the reference frame So. The position of the center of gravity G of the
whole chain, with a total mass m, is given by the following vector formula:
O1G =
1 n
∑ m OG
m i =1 i 1 i
(1)
We now introduce the intermediate links between O1 and the Gi :
O1 G =
i −1
1 n
m
(
(O O ) + Oi Gi )
∑
∑
m i=1 i k =1 k k +1
(2)
By differentiating with respect to time, we get the instantaneous translation vector of the center of gravity
VG expressed in frame So :
VG =
i− 1
  dO O    dO G 
1 n
m i ( ∑   k k +1   +  i i  )
∑
dt  S   dt  S
m i =1
k =1  
(3)
o
o
Each vector first derivative with respect to time can be expressed as a function of the instantaneous
rotation vector ω j due to the variation of parameter θj, noted θ˙ j , along the normalized vector rj :
ω j = θ˙j rj
VG =
(4)
1
miOjGi ) )
∑ ω j ×( ∑
m j =1
i =j
(5)
n
n
We can now introduce the position of the center of gravity Gaj of the augmented body (of mass maj)
associated with the parameter θj. The augmented body is the instantaneous rigid body composed of all
the elementary bodies supported by the joint j. Again we have :
1 n
Oj Gaj =
(∑ m O G )
maj i= j i j i
(6)
So, using (4) and (6), (5) becomes :
n

 maj
VG = ∑ 
r j × Oj Gaj )θ˙ j
(

j =1  m
n
Or :
VG = ∑ VG j .θ˙j
j =1
-2-
Autonomous Virtual Humans
with
VG j =
maj
(r × Oj Gaj )
m j
(7)
and finally, under matrix form:
V G = [ JG ]θ˙
where [JG] is the Kinetics Jacobian matrix relating the instantaneous translation of the center of mass to
the instantaneous unit variations on the parameters. In [5] we have also obtained expression (7) by
applying the principle of conservation of momentum on the center of gravity of the augmented bodies.
1.2
Inverse Kinetics
Inverse Kinetics is much more useful than Direct Kinetics because it provides the instantaneous joint
rotation realizing a desired instantaneous translation of the total center of mass. This property is used in
its discrete formulation as an approximation to solve the position control of the center of gravity (under
the hypothesis of small movements). The general expression of discrete Inverse Kinetics is given by :
∆ θ = JG+ ∆xG + (I −J G+ J G )∆z
(8)
where (the number in parenthesis indicates the dimension of the entity): ∆θ is the unknown vector in
the joint variation space (n), ∆xG is the vector expressing the main task as a position variation of the
+
center of mass (m), J G is the Direct Kinetics Jacobian (mxn) and J G is its unique pseudo-inverse
+
(nxm), I is the identity matrix of the joint variation space (nxn), (I − JG J G ) is the projection operator
on the null space of the linear transformation J G (i.e. any joint variation belonging to this sub-space is
mapped by J G into the null position variation of the center of mass), ∆z is the vector expressing the
secondary task in the joint variation vector (n)
Fig. 2. Inverse Kinetics on a 16 dofs articulated chain
Figure 2 illustrates Inverse Kinetic control without secondary task. The center of mass of a 16 dof chain
starts from (a) and has to reach successive goals (b), (c), and (d). The joint limits are in effect and are
most visible on posture (a) and (d). Each position goal is achieved in a few iterations yielding a nearly
straight trajectory between successive goals.
1.3
Cascaded Control
Kinetic and Kinematic control schemes share one common space, i.e. the joint variation space. For this
reason the configuration variation provided by one of these methods can be mapped on the null space of
the other one as a new class of secondary task.
Figure 3 describes the resulting partition of the joint variation space. Two dual expressions for the socalled cascaded control emerge depending on which task is given the highest priority. If the Inverse
Kinematics (Jacobian Je) has a higher priority (Fig 3) then :
∆θ ec = Je+ ∆xe +(In − Je+ J e )( JG+∆x G + (In − JG+ J G)∆z oG ) (9)
Otherwise, if the Inverse Kinetics (Jacobian JG ) has a higher priority :
∆θ Gc = JG+ ∆xG + (In − JG+ J G )(J e+∆xe + (In − J e+ Je )∆zoe ) (10)
A simple example of Equation (9) cascaded control is illustrated on Fig. 4. The main task is a reaching
behavior of the beak (together with a 45 ° orientation constraint) while the secondary task requires the
center of mass to stay on the vertical line projecting on the supporting area (the ∆z0G component was not
exploited). This example is of great interest because two radiographs of the living bird were available (a
mallard bird, p 79 in [6]) to identify the initial posture and to validate the posture predicted with cascaded
control.
Autonomous Virtual Humans
-3-
n
Je
J+
e
(k)
Je
Null Space
Je
null
vector
JG
PeJ +
G
(n-k )
(n-k-m)
k
Pe.JG
Null Space
joint variation space
JG
cartesian variation space
of some end-effector
m
null
vector
cartesian variation space
of the center of mass
Fig. 3. Cascaded partition of the joint variation space in Cascaded control
Fig. 4. rest and predicted postures of the Mallard bird for a "drink and balance" behavior.
4
Conditional Inverse Control (CIC)
The Conditional Inverse Control is our proposal for enforcing the control schemes defined by expressions
(8) to (10) including the conjunction of joints reaching their limit range and a singular configuration.
Such property is required for the control of an end effector or of the center of mass at the limit of its
reachable space. Our solution is valid for either cases.
4.1
Joint limit and Singularity
First let us describe the effect of one joint reaching its limit when the remaining part of the articulated
chain is singular for the desired task. Figure 5 illustrates this problem on the end effector of a simple 2
dofs manipulator for clarity. The first joint reaches its minimum limit for 90° while the second one has
no limited range. The end effector is at the end of the second segment and its task is to reach a black
target (Fig. 5a). This target is unreachable but the DLSPI should allow the solution to stabilize at the
shortest distance to the target (as in Fig. 5b). This is not the case due to the joint limit of the first joint.
We now explain why.
If the pseudo-inverse solution brings a joint over its limit range, the simplest approach would be to
ignore the overshooting contribution. Later, if this joint remains permanently at its limit value, a
dimension is lost in the joint variation space. In a highly redundant system this loss of dimension is
compensated by the contribution of the other joint variations. Their sub-space can handle the completion
of the task as long as the corresponding image space spans the task space [4]. This condition vanishes
when the manipulator exhibits a singularity (Fig. 5b). In such a case the manipulator converges in a few
iterations to the posture maximizing the contribution of the limited joints.
Figure 5c constructs geometrically the corresponding posture. It is the one where the task error vector is
collinear with the end effector instantaneous velocity induced by the limited joint. Such a posture can
generate an enormous error with respect to the completion of the task.
-4-
Autonomous Virtual Humans
unlimited joint range
limited
joint
range
final
configuration
task
end
effector
target
a
reaching
the
limit
final
task
error
task
error
b
c
Fig. 5. the end effector is required to reach the target (a). The first joint reaches its limit (b).
The final stable configuration maximizes the limited joint component of the solution (c)
4.2
Algorithm Overview
The principle of the CIC algorithm is to remove any joint violating its limit range for the current task.
When this happens the solution has to be reevaluated without this (or these) joint(s). It involves
computing a new pseudo-inverse of the jacobian with one or more zeroed columns. For each simulation
step an inner loop ensures that the jacobian contains no joint violating its limit range. Such iterating
mechanism is needed because removing some joint(s) at one iteration may lead to new joints violating
their range limit for the next solution evaluation. Extensive experiments have shown that, when
encountering joint limits, the CIC algorithm provides the solution generally in one iteration per
simulation step.
We present below the pseudo-code for the general case with secondary task corresponding to equation (8).
The Euler integration is used for clarity. In the cascaded control context (equation (9) and (10)) the
algorithm computes both jacobians and tasks at stage
, and both pseudo-inverses and projection
operators at stage . Finally, both jacobians are assigned a null column at stage .
for each simulation step
{
For i=1,n
Over_Range[i] := FALSE
Evaluate_Solution := TRUE
Compute J(dim mxn),∆x(dim m),∆z(dim n)
While (Evaluate_Solution = TRUE)
{
Compute J+ and (I-J+J)
Compute the solution ∆θ (dim n)
Evaluate_Solution := FALSE
For i=1,n
{
If(Over_Range[i] = FALSE)
{
new_θ[i] := θ[i]+∆θ[i]
If(new_θ[i] >Max[i] or <Min[i])
{
Over_Range[i]
:= TRUE
Evaluate_Solution := TRUE
new_θ[i]
:= θ[i]
For j=1,m
J[j,i] := 0
}
}
}
}
For i=1,n
θ[i]=new_θ[i]
}
Autonomous Virtual Humans
-5-
5
Simulation Results
Our approach is better illustrated with 2D models in order to visualize unambiguously the position
control of the center of gravity. However, our method can be used for general tree-structured 3D articulated
chains [7]. The 2D representation of the articulated structures reflects the mass distribution in the
following way: a segment Si is defined between joint i and joint i+1. It has a length L i and a mass mi ;
its width li is only displayed at the end-effector side of the segment and its value is proportional to (mi /
L i). In such a way the final display shows a continuous envelop if all the joint angles belong to the
interval plus or minus π/2. In the present section the center of mass is drawn as a black square (the largest
one) while a small light gray square indicates the location of the used-guided goal location for the center
of mass. In some images they happen to appear superimposed. In all the simulations, the images were
produced interactively on a 100 MHz R4000 CPU (SGI Indigo II workstation).
1.1
A Three dofs Manipulator Arm
This manipulator has three segments of equal length and mass. An additional large mass is attached at
the arm end thus explaining the location of the center of mass. Figure 6 especially highlights the location
of discontinuities along the borders of the center of mass reachable space. Let us denote Maxi and Min i
the state of joint i reaching the corresponding limit value. The joint ranges are from the first to the third :
[-20,20], [-40,40] and [-80,80]. In Figure 6a we have Max 1 and a singular configuration on the remaining
two dofs. In Figure 6b the center of mass lies at the point entering Min1 . In Figure 6c we have Min1 and
we enter Min2 . We are exactly at (Min 1 , Min2 , Min3 ) in Figure 6d and (Max1 , Max 2 , Max 3 ) in Figure
6e. Additionally, Figure 6e outlines the two symmetric regions composing the center of mass reachable
space.
Fig. 6. interactively outlining the reachable space of a three dofs manipulator
1.2
A Twenty dofs Manipulator Arm
This manipulator has twenty segments of equal length and mass, each with a small joint range of [10,10]. Figure 7a shows an example of configuration with all joints in either the Min or Max state. From
such a doubly curved configuration the inner reachable space border is the line appearing within the global
reachable space (Fig. 7b). When all joints reach their Min state (Fig. 7c) the configuration is singly
curved thus allowing to outline the inner border of the reachable space. The conjunction of joints limits
with singularity on the remaining joint (Fig. 7d) defines the outer borders of the reachable space.
1.3
Reachable Area Under Position Constraint for the End Effector
The manipulator is the same as in 5.2 except a wider joint range of [-15,15]. We use the cascaded control
where the end effector is required to remain fixed in space (main task) while the center of mass is moved
interactively (secondary task). This lead to an eight shaped region. Figure 8 displays successive postures
leading the manipulator to displace the center of mass to the lower part of its reachable space.
-6-
Autonomous Virtual Humans
Fig. 7. interactively outlining the reachable area of a twenty dofs manipulator with small joint ranges
Fig. 8. Interactively outlining of the center of mass reachable space
with a position constraint on the end effector
Autonomous Virtual Humans
-7-
6
Conclusion
We have demonstrated the fine position control of the center of mass provided by Inverse Kinetics. The
proposed Conditional Inverse Control algorithm overcomes the severe context of joints reaching their
limit value together with a singular configuration on the remaining joint space. We are able to outline the
reachable space of the center of mass at interactive rates for a 20 dofs manipulator arm (10 images/s).
Moreover, the identification is also possible under any position or orientation constraint of an end effector.
This approach will be tested on multi-legged robots in the near future. It will allow to evaluate
combined requirements in reachability and balance for such mobile structures at their early conception
stage. Other classes of important applications are envisioned in Ergonomics and Computer Animation for
interactive control of articulated figures.
Acknowledgments
The research was partly supported by the Swiss Federal Institute of Technology, the University of
Balearic Islands and by the Swiss National Research Foundation under the grant 21-47025.96 . The
authors are grateful to Luc Emering for sharing his experience on controlling kinematic chains with
singularity and joint limits.
References
[1] Khatib O. "The Operational Space Formulation in the Analysis, Design and Control of Robot
Manipulators, 3rd ISRR, Gouvieux, France, October 1985
[2] Maciejewski A.A. "Kinetic Limitations on the Use of Redundancyin Robotics Manipulators", IEEE
ICRA ‘89, pp 113-118, 1989
[3] Maciejewski A.A. "Dealing with Ill-Conditioned Equations of Motion for Articulated Figures", IEEE
CGA 10(3), pp 63-71, 1990
[4] Press W.H., Teukolski S.A., Wetterling W.T. and Flannery B.P. "Numerical Recipees in C",
Cambridge Press, second edition, Press Syndicate of the University of Cambridge, 1992
[5] Boulic R. and Mas R. "Inverse Kinetics for Center of Mass Position Control and Posture
Optimization", Technical Report 94/68, September 1994, EPFL, DI-LIG, Lausanne CH1015
Switzerland.
[6] Mac Lelland J.,"Form and Function in Birds" vol 4, p79, King & Mac Lelland Editors, Academic
press Publisher, London, 1989
[7 ] Boulic R., Mas R., Thalmann, D. "Position Control of the Center of Mass for Articulated Figures in
Multiple Support", Proc. of the Eurographics Workshop on Computer Animation and Simulation
19 95, Maastricht, pp 130-143, ISBN 3-211-82738-2, Springer Verlag Wien, Sept. 1995
-8-
Autonomous Virtual Humans