Dynamic Balance Control of Humanoids for
Multiple Grasps and non Coplanar Frictional
Contacts
Cyrille Collette, Alain Micaelli and Claude Andriot
Pierre Lemerle
CEA, LIST
18 route du Panorama, BP6,
FONTENAY AUX ROSES, F- 92265 France
Email: {cyrille.collette , alain.micaelli , claude.andriot}@cea.fr
INRS, MSMP
Avenue de Bourgogne, BP27,
VANDOEUVRE, F- 54501 France
Email: [email protected]
Abstract— Human beings perform complex tasks while even
keeping balance. Though this is evident for a human, it is very
difficult to adapt human motion to humanoid robots, due to
the posture redundancy. In this paper we propose a conceptually
simple framework of human posture control, scoping in a general
way with grasp, task achievement and being on the same time
robust to external disturbances.
We do this by establishing physically meaningful constraints
on the force level. In order to respect all constraints, we solve
for a quadratic minimization problem (quadratic programming),
yielding the required joint torque controls.
Contrarily to most other approaches, we deal also with
unilateral contacts (due to friction) as well as bilateral grasps
which allow for example for arbitrarily steering or pulling on
a handhold. Additionally, and in contrast to classical methods
based on ZMP, we account also for contacts not all being in the
same plane, as hand-wall contact.
Index Terms— Humanoid robot motion, Dynamic balance control, Multiple non coplanar contacts, Grasp control, Friction.
I. I NTRODUCTION
A. Problem statement
A human is able to realize complex tasks and keep its
balance at once. Subject to disturbance, he can carry out many
balance strategies, changing its lean and grasping holds. Since
the kinematical structure of a humanoid robot (HR) is similar
to that of a human, a HR is expected to work in place of a
human in our daily life and have the same behavior in a natural
environment. For instance, in an industrial environment, a HR
locked on to a ladder should be able to realize assembly tasks.
Yet existing control schemes of HR are very limited. Generating consistent motions and behaviors which approximately
look credible is a challenge.
B. Related work
Two usual stability criteria for a HR standing on a horizontal
ground are the Center Of Mass (COM) and the Zero Momentum Point (ZMP). In the static case, we can use the COM. A
HR can remain static if and only if its COM projects vertically
inside the convex hull of the contact points. In the dynamic
case, the ZMP takes into account inertial and Coriolis forces.
A HR can realize a specified movement if and only if the ZMP
Fig. 1. Examples for multiple grasps and non coplanar contacts. A : The
HR keeps its balance after a disturbance. B : On an inclined ground, the HR
leans on fitted bars. C : The HR stands up. D : The HR grasps a paintbrush.
E : The HR holds on a string clumped at one end.
projects vertically inside the convex hull of the contact points.
But, when HR is on a broken ground and a grasp environment,
COM and ZMP are unadapted.
Some authors work on more global criteria of balance in
complex environments. Recently, H. Hirukawa et al. [12] proposed a universal stability criterion of multiple non-coplanar
contacts of legged robots. However, it does not deal with hand
contact, grasp and HR redundancy.
T. Bretl et al. [3] [4] present a general framework for
planning the quasi-static motion of a three-limbed climbing
robot in vertical natural terrain. To prevent the robot from
falling as it moves a limb to reach a new hold, the algorithm
exploits friction at supporting holds and adjusts the robot’s
internal Degrees Of Freedom (DOF). Y. Or and E. Rimon
[15] characterise robust balance in a 3D gravity environment
with multiple non-coplanar contacts. The evolution area of
the COM is a convex vertical prism. It is a global geometrical
approach in the static case and can be applied to HR balance.
However, both approaches are only static; dynamic simulation
and HR posture are not taken into account.
Moreover, various teams work on HR grasping. For instance, Ch. Ott et al. [16] present a humanoid two-arm system
developed as a research platform for studying dexterous twohanded manipulation. However, HR legs are not considered.
K. Harada et al. [10] [11] work on dynamics and balance of
a humanoid robot during manipulation tasks. Their HR can
push an object but not grasp it to keep its balance during
disturbance.
Since the 80’s, O. Khatib et al. [20] uses projection methods
which has been fully tried and tested. However, this method
can provoke passivity issues [18] and don’t deal with all the
problems simultaneously.
Several authors deals with optimization problems. P.B.
Wieber [22] proposes an interesting formulation of optimization problems for walking robots. He computes contact forces
in this control. However, several aspects of the problem
(redundancy, constraint formulation, walking stability) are
unadapted for our problem. Recently, Y. Abe et al. [1] propose
an interactive multiobjective control with frictional contacts.
These works present similitudes with our work. However,
grasp, task and complex stable balance strategies are not
entirely treated.
C. Contribution
The orginality of this paper consists in proposing a single
framework for motion control of a HR with multiple grasps
and non coplanar frictional contacts. The following can be
considered as the key features of this control architecture :
• In section IV-B, we define physically meaningful assumptions on the human motion. These are the core of the
control law since they are used as constraints for the
minimization problem.
• This optimization problem is stated at the force level
of the HR motion problem. Taking into account all
forces effecting the HR, the solution of the quadratic
minimization problem (QP : Quadratic Programming)
induces for sure a realisable set of desired forces acting
on the HR. However in the control law only computed
motors jointed torques will be used.
Integrating the resulting body accelerations yields the HR
posture; no position is clamped a priori.
• Moreover a second QP (named Static QP) is used beforehand the main QP (named Dynamic QP) in order
to compute a feasible set of contact friction forces and
jointly with a robust COM. These values are to be fed
into the dynamic QP.
• Due to the fact that all constraints are considered simultaneously, our control scheme does not require priorizing of
individual control tasks as grasping or balance control and
hence avoids tedious parameter adjustement which usally
is highly task dependent. Our control schemes turns out
to perform well for very different HR configurations and
tasks, yielding in general human-like motion behaviour
(see Fig. 1) without requiring intermediate postures or
even joint trajectories. Note that our control scheme
implies disturbance rejection as well.
• Bilateral grasp contact is a very common every-day
gesture, like approaching and grasping a handhold by
its own hand. Once grasped, a bilateral contact allows
to apply any wrench on the grasped object, as pulling,
pushing, or twisting.
As a particularity of our approach such a bilateral grasp
is considered as a constraint and not as an optimization
goal [1]. This ensures the task to be accomplished, taking
advantage of the strong redundancy of the HR. Note that
already during the phase of approaching, the bilateral
grasp is considered as an acceleration constraint which
in turn is subject to a closed loop control.
• This control scheme emphasizes the computation of instanteneous reference control values in order to fulfill a
given task or maintain a given posture. In this sense,
it is a direct and constructive method. Due to a heavy
disturbance or a non-realisable task, HR may be led to an
unstable configuration. In this case the constraints are still
respected but the solution of the minimzation problem,
ie the computed COM, does not correspond to a stable
posture. Without any further precaution, the HR would
fall down.
This case can be easily detected by a high-level controller
which may change the balance strategy, for instance doing
a step sideward or leaning the hand on same object. Of
course this requires the scene to be known by the highlevel controller.
In section II, we give the physical modelling used for the
simulation engine. Control laws are based on an approximated
modelling, which is detailled in section III. In section IV,
we present our low-level dynamic balance control of HR.
Section V adds a high level control which deals with both
task and advanced balance. Finally, section VI summarizes
the presented control architecture and indicates some possible
future research directions.
II. P HYSICAL M ODELING
A. Dynamic simulation
Our HR is a set of articulated branches of rigid bodies,
organized into a highly redundant ndo f (number of degrees of
freedom) arborescence. Our HR consists of 32 joints (Fig.
1). The skeleton is modeled as a multi-body system. The
root body of the HR tree is the thorax. This root has 6root
DOF and is not controlled. We decided to use human data for
modeling our HR. References come from : D. Miller et al.
[14], E.P. Hanavan [9], W.T. Dempster et al. [5]. Dynamic
model elaboration comes from J. Park [17]. We begin by
C. Grasp modeling
We offer two grasp models. Firstly, as a damped spring
between the hand and the object (Fig. 2(a)). They can be
actived / inactived at any time in the simulation. A damped
spring link is composed of a spring (mapping from SE (3) to
se (3)∗ ) and a damper (mapping from se (3) to se (3)∗ ). We
choose for the spring a formulation derived from a potential
(E. D. Fasse and P. C. Breedveld [7]). Secondly, by modeling
the hand’s kinematics (Fig. 2(b)), the clenching efforts are the
contact efforts between the hand and the object.
(b) 20 DOF hand
(c) Initial pos- (d) Final posture hand
ture hand
(a) Damped spring
Fig. 2.
III. C ONTROL MODELING
Grasp model
To find a control law, we need an adapted model, most often
a simplification of the physical model.
describing the dynamics of the robot in terms of its joint
coordinates, q.
M̃ · (Ṫ − G) + N · T = L · τ +CT ·W
A. Contact modeling
(1)
For this system of ndo f + 6root equations, τ is the set
of generalized joint torques, M̃ is the inertia matrix, N · T
represents the inertial and Coriolis forces and G is the gravity
effect. L formulates τ in generalized coordinates. The vectors
of external forces W is the sum of contact forces, grasp wrench
and other external disturbance forces : W = Wcontact +Wgrasp +
Wdisturbance .
Fig. 3.


τ =
τ1
..
.
τndo f




 G=

0
0
−g

 L=

0(6,ndo f )
Indo f
03+ndo f
Ṫ , T and X are respectively tree acceleration, velocity and
position in generalized coordinates.






V̇root
Vroot
Xroot
 q̈1 
 q̇1 
 q1 






Ṫ =  ..  T =  ..  X =  .. 
 . 
 . 
 . 
q̈ndo f
q̇ndo f
qndo f
C : Matrix transformation between T and velocity ’body’ (J.
Rbody
Park [17]) of all the bodies. Vbody
: twist of body expressed
in its own frame Rbody .
Vroot
V1
. . . Vndo f
Linearized friction cone

T
=C·T
B. Contact modeling
Coulomb friction formalism, extended to polyarticular, is
inspired from T. Liu and M.Y. Wang [13]. Friction can be
formulated through the strict inequality : k fct k < µ · fcn with
fcn : normal contact force, fct : tangent contact force, µ : dryrub factor. It is a non linear model.
The physical simulation uses Coulomb’s friction model,
which is a non linear model. We use a linearized coulomb
model. As J.C. Trinkle et al. [21], we linearize contact cones
into multifaceted friction cones in order to obtain linear
constraints (Fig. 3). The linearized contact force of the k-th
contact is denoted :
k
fc/linearized
= l fck · ξ k
with
ξk =
ξ1k
...
ξnke
T
(2)
with ne : number of edges, l fck : linearized friction cone, ξ k :
forces to every line direction of the k-th linearized friction
cone. The contact forces computed by our control law must
be inside the friction cone which means : ∀i ∈ [1, ne ], ξik ≥ 0.
B. Grasp modeling
Our first grasp model is a damped spring between the hand
and the object (Fig. 2(a)). When the hand is close enough to
the object, the damped spring is activated in the control law
(and simulation), which is similar to a clenching effort. The
second grasp model are based on hand’s kinematics (Fig. 2(b)).
The clenching effort is the contact efforts between hand and
object. The hands’ DOF take part in the grasping effort but
not in any way in the manikin’s balance control. To grasp an
object from an initial posture (Fig. 2(c)), we compute torques
as an articular control towards final posture (Fig. 2(d)).
QP. We make a behavior assessment and then try to express
them as a QP.
B. Behavior assumptions
Fig. 4. Static and Dynamic QP. Thereafter, only control torque result is
useful in the physical simulation.
IV. L OW L EVEL C ONTROL
A. Principle
Our approach to handle dynamic HR lies within a robotic
approach and we focus more precisely on joint control. As
P.B. Wieber [22] and Yeuhi Abe et al. [1], we formulate a constrained optimization problem : QP. This method deals with a
great number of DOF and solves simultaneously all constraint
equations. We introduce the following notation, Y : unknown
vector, Y des : desired but not necessarily accessible solution,
Q : quadratic norm and A, b, C and d matrices and vectors
which express linear equality and inequality constraints.
(
A ·Y + b = 0
des
MinkY −Y kQ such as
(3)
C ·Y + d ≥ 0
The HR control is based on two successive QP (Fig.4).
In the static QP, the HR is brought down to its COM.
Its posture is not taking into account. Its COM is subject to
the acceleration of gravity, contact forces and grasp wrench.
System unknowns are COM position, linearized contact forces
and grasp wrench. We choose the distribution of the HR’s
desired contact forces. Generally, we choose a homogeneous
distribution for a standing posture. Then, we compute a
solution which allows HR to have a consistent static balance,
robust with respect to its contacts. These issues were first
studied by A. Rennuit [19]. To sum up, this first step allows to
compute a consistent goal COM position, robust with respect
to homogenous distribution of desired contact forces and grasp
wrench. Static QP equations are detailed in Appendix.
In the dynamic QP, we consider the HR posture. We
compute the control torques in order to reach the COM goal
position. In the next subsections, we only deal with dynamic
By its own joint activation, HR gets around and interacts
with the environment. The joint control torques (Eq. 11) and
grasp wrench (Eq. 12) are saturated so that our HR cannot
apply unrealistic forces. Working hypotheses of HR :
• Knowledge : State of its contacts and friction (µ) when
it interacts. Position of its COM and its goal COM,
the latest being stable in respect to its contacts layout
(Appendix). Spatial location of environment (ex : ground,
wall). Its dynamic model and gravity (Eq.4).
• Unknowns : Disturbance force.
• Implicit reactions : The highest priority is laid on keeping stable contacts. If its contacts are active, contact
acceleration must be null (Eq.6) and HR has to apply
contact forces inside the friction cones in order to keep its
adhesion (Eq.10). If its contacts are ruptured, it will try to
enter into contact with the nearest environment (Eq.7). It
keeps its balance with the acceleration of gravity (Eq.4).
It tries to reach goal COM (Eq.14) and adapts its posture
(Eq.15).
C. QP formulation
In this part, we formulate previous behavior assumption
(IV-B) as “Dynamic QP” (Fig.4). The goal is to compute
a control torque (τ) to apply to the HR’s joint. Articular
accelerations (Ṫ ), linearized contact forces (ξ ) and grasp
wrench
They are merged into a vector
(WG ) are also unknown.
T
Y = τ Ṫ ξ WG
of dimension 2 · ndo f + nc · ne + 6 ·
ng + 6 (ng : number of grasp taking account, ng ∈ {0, 1, 2}).
The dynamic equation is :
L · τ − M̃ · Ṫ + DCT · ξ + DTG ·WG − N · T + M̃ · G = 0
(4)
Physical simulation (Subsection II-A) computes L, M̃, N &
G. DCT and DTG formulates ξ and WG respectively in generalized
coordinates. Control law does not know disturbance forces, so
Wdisturbance = 0. Moreover, we impose a goal (agoal
c ) to the
nc -th contact acceleration (ac ). We can formulate the velocity
of the nc -th contacts (vc ) as a linear function F of T : vc =
F · T ⇒ ac = F · Ṫ + Ḟ · T . This acceleration gives a command
on Ṫ :
ac = agoal
c
− Ḟ · T
⇒ F · Ṫ = agoal
c
(5)
For the k-th contact (according to subsection IV-B), depending
on wheter contact is active or not, we use the following contact
acceleration commands :
agoal
= 03
c(k)
goal
= kc · (xc(k)
(6)
− xc(k) ) + µc · (vgoal
c(k)
− vc(k) )
(7)
with (kc , µc ) : contact control gain, (xc(k) , vc(k) ) : position
goal
and velocity of contact k, xc(k)
: projection of xc(k) on the
nearest HR’s environment and vgoal
= 03 . In the same way, we
c(k)
impose a goal agoal
grasp to the ng -th hand grasp acceleration
(agrasp ). The velocity of the ng -th hand grasp (vgrasp ) is a
linear function of T :vgrasp = DG ·T ⇒ agrasp = DG · Ṫ + D˙G ·T .
This acceleration provides a command on Ṫ :
agrasp = agoal
grasp
⇒ DG · Ṫ = agoal
grasp − D˙G · T
(8)
goal
goal
agoal
grasp = kgrasp · (Xgrasp − Xgrasp ) + µgrasp · (Vgrasp −Vgrasp ) with
(kgrasp , µgrasp ) : grasp control gain, (Xgrasp ,Vgrasp ) : position
goal
and velocity of hand grasp, Xgrasp
: localization of the grasped
goal
object and Vgrasp = 06 .
We group together equations (4), (5) & (8) in the following
constraint equalities system :
A ·Y + b = 0
with






A=










b=



L
0
0
−M̃
F
0
DCT
0
0
DTG

0 
DG

−N · T + M̃ · G
goal

−ac + Ḟ · T
goal
−agrasp + D˙G · T
(9)
Contacts must be non-sliding (Subsection III-A). And, joint
torques and grasp wrenches are saturated because of motor
limitations :
ξ ≥0
(10)
max
τ ≤τ
|τ| ≤ τ max ⇒
(11)
−τ ≤ τ max
WG ≤ WGmax
max
|WG | ≤ WG ⇒
(12)
−WG ≤ WGmax
C ·Y + d ≥ 0
with
posture which can evolve during simulation. We formulate
all these choices through X des . Finally, Ṫ des is a function
des
(denoted f ) of ades
G and aP . We give different priorities to
the optimization criteria thanks to weighing Q.



−Indo f 0
0
0



0
0
0


 Indo f



C = 
0
0 Inc ·ne
0



0
0
0
I6·ng 



0
0
0
−I6·ng



max
T

τ max 0 WGmax WGmax
d= τ
(13)
We want to minimize the torque, the linearized contact
goal
forces and the grasp wrench. Moreover, we express xG
(computed in the Appendix) through the optimization criteria
Ṫ des . We write COM acceleration ades
as a function of Ṫ .
G
As there is an infinity of HR postures with the same COM,
we add another posture criterion ades
Pos . Hence the following
control :
goal
des
(14)
ades
G = kG · xG − xG + µG · vG − vG
des
ades
− X + µPos · T des − T
(15)
Pos = kPos · X
with (kG , µG ) : COM control gain, vdes
G = 03 , (kPos , µPos ) :
posture control gain, T des = 06+ndo f and X des : posture reference. Lots of authors (Duval-Beaupère et al. [6], N. Gangnet
et al. [8]) work on analyzing standing posture and measure
orientation and localization of the various body segments with
respect to the gravitational vector. Using their results, we
goal
decided that HR tries to place its thorax above xG
level,
vertically oriented. At last, HR is controlled around an initial
MinkY −Y des kQ
with

Q , QṪ , Qξ , QWG
Q = diag


 desτ



τ = 0ndo f






 Ṫ des = f ades , ades 
Pos


G
Y des = 






des


 ξ = 0nc ·ne




des
WG = 06·ng
(16)
We group equations (9), (13) & (16) and solve this Dynamic
QP problem (Eq. 3). τ, Ṫ , ξ and WG are computed but only
τ is used thereafter in the physical simulation.
In a situation of manipulating an object, we need to reassess
the previous arithmetic. WG is no longer an unknown, but
is given by the simulation. Thanks to Eq. 4, we compensate
the object weight this way (damped spring or grasp contact
forces). However, we keep Eq. 8.
In several cases, HR must know its joint end stops; we use
a simple method to manage them. When the joint is too close
to its limit (ε : an arbitrary angle), we impose an articular
acceleration in the opposite direction to take some distance
from it. Thus, we add this inequalities to Eq. 13 :
max
qi ∈ qmin
,
i , qi
|qi − qmin
i |<ε
|qi − qmax
i |<ε
→ q̈i > 0
→ q̈i < 0
(17)
V. H IGH L EVEL C ONTROL
A. Principle
Fig. 5 illustrates our global architecture control. First of
all, HR has an inexact perception of the physical simulation.
For example, HR does not know disturbance force. Then, it
analyses its state and chooses a strategy. We consider two
global strategies : reaction to disturbance and task. In the first
strategy, HR can have different reactions (simple/complex subsection V-B.1). Thanks to stability criteria, HR uses complex
reaction to disturbance if necessary. In task mode, HR can
realize task sequences (Fig. 7). Finally, thanks to subsection
IV-C, we can formulate and solve two QP (static and dynamic)
in order to compute the HR articular torque.
B. Results
1) Towards multi phase balance control: In these examples
(Fig. 6), HR contacts are activated/inactivated as the simulation
carries on. It is a more high-level control. We implement a
two phase balance control. At the beginning of the scene,
the HR is upright and its hand contacts are inactivated. On
account of a disturbance force applied on its back, its COM
goal
moves away from itsgoal position kxG
−xG k > dist (dist : an
arbitrary distance). In the first case (Fig. 6(a)), hand contacts
are activated and try to come in touch with the environment.
Thanks to its hands, the HR can maintain its balance. Finally,
its hand contacts are inactivated in order to meet up with its
Fig. 5.
Global architecture control
starting posture. We note that it is a non-coplanar multi contact
problem and the static QP of our algorithm (Appendix) allows
us to stabilize the motion. It is a HR configuration converging
to a stable posture. In the second case (Fig. 6(b)), contact
accelerations of the right feet have a new goal so that HR
makes one step forward.
(a) Using arms to keep balance
Fig. 6.
(b) One step forward
Balance control in two phases
2) Strategies linking: In this example (Fig. 7), HR must
grasp a brush and realize the task of painting the wall. In
order for its hand to reach the brush, we use the previously
evoked joint limit control (Eq. 17). When HR is close enough,
we activate the damped spring in the simulation (Fig. 7(b)).
A disturbance hits the floor, at an unpredicted time. Though
it does not qualitatively know the nature of the disturbance,
it knows that one was applied. It stops in its task (Fig. 7(e))
and goes back to its initial posture, to possibly make use of
complex reaction (Subsection V-B.1). Once the disturbance is
over and it has recovered its balance, the HR fulfills its task.
VI. C ONCLUSION
We introduced a global architecture to control HR with
multiple grasps and non coplanar frictional contacts. Thanks to
this architecture, we solve simultaneously the problem of tasks
involving grasping and balance control after a disturbance.
Thanks to constraints optimization algorithm (QP), the HR can
be controlled in a large range of situations and is consistent
with the same set of parameters (control gain, weighing). We
notice that this algorithm is adequate for all serial robots,
for every contact configuration (not especially at the endeffectors), and for an important number of DOF.
We are currently planning to use this architecture in an
interactive demo. The basic algorithm is fast enough to be
used in real time. At last, we would like to work on more
complex behaviors with several step controls and predictive
control (C. Azevedo et al. [2]). A better characterization of
balance breakdown would be useful.
Videos illustrating our work are available on cyrillecollette.blogspot.com.
VII. ACKNOWLEDGMENTS
The authors gratefully acknowledge the ANR and the RNTL
PERF-RV2 project. The authors would like to thank Sébastien
Barthélemy (ISIR/LRP) for his precious help.
R EFERENCES
[1] Yeuhi Abe, Marco da Silva, and Jovan Popović. Multiobjective control
with frictional contacts. In Eurographics/ ACM SIGGRAPH Symposium
on Computer Animation, 2007.
[2] Christine Azevedo, Philippe Poignet, and Bernard Espiau. Artificial
locomotion control : from human to robots. Robotics and Autonomous
Systems, 47:203–223, 2004.
[3] Timothy Bretl, Jean-Claude Latombe, and Stephen Rock. Toward
autonomous free-climbing robots. Robotics Research, pages 6–15, 2005.
[4] Timothy Bretl, Stephen Rock, and Jean-Claude Latombe. Motion
planning for a three-limbed climbing robot in vertical natural terrain.
In Proc. IEEE Int. Conf. on Robotics and Automation, Taipei, Taiwan,
2003.
[5] W.T. Dempster and G.R.L. Gaughran. Properties of body segments based
on size and weight. American Journal of Anatomy, 120:33–54, 1967.
[6] Duval-beaupére, G. Schmidt, and C. Cossonp. A barycentrometric study
of sagittal shape of spine and pelvis : the conditions required for an
economic standing position. Annals of Biomedical Engineering, 20:451–
462, 1992.
[7] E. D. Fasse and P. C. Breedveld. Modeling of elastically coupled
bodies: Part 1 - general theory and geometric potential function method.
Transactions of the ASME, 120:496–506, 1998.
(a) Initial position
(b) Catch a paintbrush
(c) Preparation to paint
(d) Apply paint
(e) Ground disturbance : (f) Resuming and finishsimple reaction
ing task
Fig. 7. Strategies linking in order to realize a paint task while keeping balance, especially during disturbance. (Dynamic simulation environment “Arboris”
developped by CEA/LIST in Matlab, 2006)
[8] N. Gangnet, V. Pomero, R. Dumas, W. Skalli, and J.-M. Vital. Variability
of the spine and pelvis location with respect to the gravity line : a threedimensional stereoradiographic study using a force platform. Surgical
and Radiologic Anatomy, 25(5-6):424–433, 2003.
[9] E.P. Hanavan. Mathematical model of the human body. Wright-Patterson
Air Force Base, Ohio, AMRL-TR-64-102, 1964.
[10] Kensuke Harada, Shuuji Kajita, Kenji Kanko, and Hirohisa Hirukawa.
Dynamics and balance of a humanoid robot during manipulation tasks.
IEEE Transactions on Robotics, 22(3):568–575, 2006.
[11] Kensuke Harada, Fumio Kanehiro, Kiyoshi Fujiwara, Kenji Kaneko,
Kazuhito Yokoi, and Hirohisa Hirukawa. Real-time planning of humanoid robot’s gait for force-controlled manipulation. IEEE/ASME
Transations on Mechatronics, 12(1):53–62, 2007.
[12] Hirohisa Hirukawa, Shizuko Hattori, Kensuke Harada, Shuuji Kajita,
Kenji Kaneko, Fumio Kanehiro, Kiyoshi Fujiwara, and Mitsuharu Morisawa. A universal stability criterion of the foot contact of legged robots
- adios zmp. In Proceedings of the 2006 IEEE International Conference
on Robotics and Automation Orlando, Florida, 2006.
[13] Tong Liu and M.Y. Wang. Computation of three-dimensional rigidbody dynamics with multiple unilateral contacts using time-stepping and
gauss-seidel methods,. IEEE Transactions on Automation Science and
Engineering, 2(1):19–31, 2005.
[14] D. Miller and W.E. Morrison. Prediction of segmental parameter using
the hanavan human body model. Medecine and Science in Sports,
7(3):207–212, 1975.
[15] Y. Or and E. Rimon. Computation and graphic characterization of robust
multiple-contact postures in gravitational environments. Department of
Mechanical Engineering, Technion, Israel.
[16] Ch. Ott, O. Eiberger, W. Friedl, B. Bauml, U. Hillenbrand, Ch. Borst,
A. Albu-Schaffer, B. Brunner, H. Hirschmuller, S. Kielhofer, R. Konietschke, M. Suppa, T. Wimbock, F. Zacharias, and G. Hirzinger. A
humanoid two-arm system for dexterous manipulation. In IEEE-RAS
International Conference on Humanoid Robots, pages 276–283, 2006.
[17] Jonghoon Park. Principle of dynamical balance for multibody systems,
multibody system dynamics. 14(3-4):269–299, 2005.
[18] A. Rennuit, A. Micaelli, X. Merlhiot, C. Andriot, F. Guillaume,
N. Chevassus, D. Chablat, and P. Chedmail. Passive control architecture
for virtual humans. In In IEEE/RSJ International Conference on
Intelligent Robots and Sytems, Edmonton, Canada, 2005.
[19] Antoine Rennuit. Contribution au Contrôle des Humains Virtuels
Interactifs. PhD thesis, Ecole Centrale de Nantes, 2006.
[20] Luis Sentis and Oussama Khatib. A whole-body control framework
for humanoids operating in human environnements. In Proceedings of
the 2006 IEEE International Conference on Robotics and Automation
Orlando, Florida, 2006.
[21] J.C. Trinkle, J. Tzitzoutis, and J.S. Pang. Dynamic multi-rigid-body
systems with concurrent distributed contacts: Theory and examples,
philosophical trans. on mathematical, physical, and engineering sciences.
Series A, 359:2575–2593, 2001.
[22] P.B. Wieber. Modélisation et Commande d’un Robot Marcheur Anthropomorphe. PhD thesis, Ecole des Mines de Paris, Décembre 2000.
A PPENDIX
Fig. 8.
Static QP. Thereafter, only COM result is useful.
In this appendix, we detail the Static QP formulation of Fig.
goal
. Our HR is reduced to its COM subject
4 which compute xG
to the acceleration of gravity, contact forces and grasp wrench.
HR posture is not considered.
The unknowns are the COM position (xG ), the linearized
contact forces (ξ ) and the grasp wrench (WG ). They are
T
merged into a vector Y = xG ξ WG
of dimension
3+nc ·ne +6·ng . From a contact forces distribution (generally,
homogeneous for a standing posture), grasp wrench and COM
position of our choice, we compute a solution which respect
several constraints : the static equation, non-sliding contacts
and the maximal grasp wrench max. With the gravity (WMscene ),
contact (WCscene ) and grasp wrenches (WGscene ) applied to HR
and written in the scene coordinates system, the static equation
is :
WMscene +WCscene +WGscene = 0
(18)
We can easily find :

0
0
 0
0


0
0
WMscene = 
 0
−m
·g

 m·g
0
0
0
0
0
0
0
0
0








 · xG + 






0
0
−m · g
0
0
0








which we rewrite WMscene = E1 · xG + E2 . We can also define
a matrix E3 such as WCscene = E3 · ξ and E4 such as WGscene =
E4 ·WG . Hence :
(
A ·Y + b = 0
with
A = E1
b = E2
E3
E4
(19)
Contacts must be non-sliding (Subsection III-A) so ξ ≥ 0.
Moreover, grasp wrench are saturated because of the motor limitations.
These conditions may be rewritten :|WG | ≤
W
≤ WGmax
G
WGmax ⇒
.
−WG ≤ WGmax



0 Inc ·ne
0



C =  0
0
I6·ng 
(20)
C ·Y + d ≥ 0 with
0
0
−I6·ng



T

d = 0 WGmax WGmax
We prioritize the optimization criteria by weighing Q.
(
MinkY −Y des kQ
with
Q = diag(QxG , Qξ , QWG )
des
T
Y = xG
ξ des WGdes
des
(21)
We group equations (19), (20) & (21) and solve this static
QP problem (Eq. 3). xG , ξ and WG are computed but only xG
goal
is used in the dynamic QP (recall xG
) (Eq. 14).
In a situation of manipulating an object, we reassess the
previous arithmetic. WG is no longer an unknown, but is given
by the simulation. Thanks to Eq. 18, we compensate the object
weight this way (damped spring or grasp contact forces).
This method allows us to find a static balance COM location
with multiple coplanar or non coplanar contacts and grasps.
Moreover, by adjusting the weighing, we easily bring the
problem to this one : calculating a consistent static balance
COM position in order to get the desired contact and wrench
forces distribution.