-
HAL Id:
tel-00446757https://tel.archives-ouvertes.fr/tel-00446757
Submitted on 13 Jan 2010
HAL is a multi-disciplinary open accessarchive for the deposit
and dissemination of sci-entific research documents, whether they
are pub-lished or not. The documents may come fromteaching and
research institutions in France orabroad, or from public or private
research centers.
L’archive ouverte pluridisciplinaire HAL, estdestinée au dépôt
et à la diffusion de documentsscientifiques de niveau recherche,
publiés ou non,émanant des établissements d’enseignement et
derecherche français ou étrangers, des laboratoirespublics ou
privés.
Contribution à la planification de mouvement pourrobots
humanoïdes
Oussama Kanoun
To cite this version:Oussama Kanoun. Contribution à la
planification de mouvement pour robots humanoïdes.
Automatic.Université Paul Sabatier - Toulouse III, 2009. English.
�tel-00446757�
https://tel.archives-ouvertes.fr/tel-00446757https://hal.archives-ouvertes.fr
-
THÈSETHÈSEEn vue de l'obtention du
DOCTORAT DE L’UNIVERSITÉ DE TOULOUSEDOCTORAT DE L’UNIVERSITÉ DE
TOULOUSE
Délivré par l'Université Toulouse III - Paul SabatierDiscipline
ou spécialité : Automatique
JURYOussama Khatib RapporteurPhilippe Bidaud Rapporteur
Jean-Paul Laumond ExaminateurEtienne Ferré Examinateur
Pier-Giorgio Zanone ExaminateurFlorent Lamiraux Examinateur
Ecole doctorale : Systèmes (EDSYS)Unité de recherche : CNRS -
Laboratoire d'Analyse et d'Architecture des Systèmes
Directeur(s) de Thèse : Jean-Paul LaumondRapporteurs : Oussama
Khatib (Stanford University) et Philippe Bidaud (UPMC)
Présentée et soutenue par Oussama KanounLe 26 octobre 2009
Titre : Contribution à la planification de mouvement pour robots
humanoïdes
-
To Neila, Foued and Karim
-
Acknowledgements
This work has been part of the research project Zeuxis for which
we gratefully acknowledge a
sponsorship from The EADS Foundation.
I owe the fantastic experience I had during my thesis to people
of great caliber. First, I realize how big
a chance it was to work under the supervision of Dr.Jean-Paul
Laumond whom I thank for his guidance
and accommodating management. I had the chance to work with
Dr.Florent Lamiraux whose scientific
advice was a constant reason behind successes. The fact that I
could easily concentrate on my thesis topic
was largely due to the support of Dr.Anthony Mallet, author of a
great robotics software architecture.
The teamwork we put forward to achieve vision-guided grasp for
HRP-2 was one of the best moments
of this thesis. I would like to thank Dr.Eiichi Yoshida who
directed me to a lot of essential material and
provided friendly support and counsel. I also had the chance to
collaborate with Dr.Pierre-Brice Wieber
from whom I learnt a lot.
Like the legendary singer said, I get by with a little help from
my friends. So thank you Anh, Claudia,
Wael, Gustavo, Thierry, Brice, Ali, Mathieu, Seb, Carlos,
Manish, Pancho, Diego, David, Layale,
Thomas and Duong. Special thanks go to Zen-master Minh :).
Special thanks to you Antoine and Muriel,
for your constant friendship and fantastic meals :)! Grazie
mille Soraya, Jérôme, Virginie, Marrrie and
Diarmait.
Big thanks fly to my adorable family in Tunis and Sfax and
everywhere else in the world.
I owe and dedicate this achievement to my mother Neila, my
father Foued and my brother Karim who
have always had infinite unconditional support, encouragement
and trust. Thank you for your love, far
better than anything in this world.
-
Contents
1 Introduction 1
1.1 Problem statement . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 1
1.2 Related works and contributions . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 2
1.3 Outline of the thesis . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 4
1.4 Associated publications and software . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 5
2 Classical prioritized inverse kinematics 7
2.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 7
2.2 Differential kinematics . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 8
2.3 Local extrema, or singularities . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 10
2.4 Prioritization . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 12
2.5 Handling inequality constraints . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 15
3 Generalizing priority to inequality tasks 19
3.1 Inequality task definition . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 19
3.2 Description of the approach . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 21
3.3 Mapping linear systems to optimization problems . . . . . .
. . . . . . . . . . . . . . . 21
3.3.1 System of linear equalities . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 22
3.3.2 System of linear inequalities . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 23
3.3.3 Mixed system of linear equalities and inequalities . . . .
. . . . . . . . . . . . . 24
3.4 Prioritizing linear systems . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 24
3.4.1 Formulation . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 24
3.4.2 Properties . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 25
3.4.3 Algorithms . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . 25
3.4.4 Dealing with singularities . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . 27
3.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 29
4 Illustration 31
4.1 A humanoid robot . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 31
4.2 Constraints and tasks . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 33
4.3 Prioritization . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 43
5
-
5 Dynamic walk and whole body motion 49
5.1 Dynamic walk generation . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 50
5.1.1 Dynamic constraints . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . 50
5.1.2 A model approximation . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . 51
5.1.3 An optimal controller . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 52
5.1.4 Merger with prioritized inverse kinematics . . . . . . . .
. . . . . . . . . . . . 55
5.2 Experiments . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 57
6 Footsteps planning as an inverse kinematics problem 63
6.1 Principle of the approach . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 63
6.2 Construction of inverse kinematics problems . . . . . . . .
. . . . . . . . . . . . . . . . 64
6.2.1 A single footstep . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 64
6.2.2 Several footsteps . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 67
6.3 Tuning the parameters . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 68
6.3.1 Number of footsteps . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . 68
6.3.2 Starting footstep . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . 70
6.4 Illustration . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 71
6.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . 76
7 Integration 79
7.1 The full algorithm . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . 79
7.2 Illustration . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . 80
8 Conclusion 91
-
1Introduction
1.1 Problem statement
Autonomous robots concretize a Perception-Planning-Action loop.
Perception is the construction of a
useful representation of the robot and its environment. This
process relies on data produced by cameras,
range sensors, force sensors, gyroscopes, etc. Actions designate
the events that the robot can provoke
such as signals (light, voice), motions (involving motors,
pneumatics), etc. Planning is the process that
maps the perception to actions. In this thesis, we will be
exclusively focusing on planning problems for
humanoid robots.
The planning component of a humanoid robot designed after the
human form is a great challenge to
roboticists. In front of a robot with a human shape walking and
moving dexterously, anyone would
rightfully expect a little intelligence in the package.
Intelligence, for a machine, is the capacity to
perform tasks normally requiring human intelligence (Oxford
dictionary). All there seems to do to test
this intelligence is to submit a task and compare the robot’s
actions to one’s own. This is the principle of
The Turing Test. The robot should use all the resources at its
disposal and adopt a course of actions that
a human would expect from his peers.
Intelligence can be perceived at very simple levels. For
instance, to grasp a couple of cups standing
close on a table, the human would probably use both hands
simultaneously if possible. A humanoid robot
with the same possibility taking a cup after the other may
probably be dismissed from the closed circle
of intelligent robots. Intelligence can be perceived on the
level of the action itself: if the table happens
to be a little low, instead of decomposing the action into
leaning forward on the legs then stretching the
arms to grasp the cups, all involved parts of the robot could
move simultaneously to achieve the task.
1
-
In both examples, intelligence is reflected through the
coordination of several resources to save time.
In the second example, the robot used its legs to help a
manipulation. We might have little conscience
of the motion of our lower bodies when performing such
manipulation tasks. We may also have little
conscience of the infinity of ways in which we could have solved
the same problem. One important
aspect of the intelligence of humanoid robots must therefore be
a seamless exploitation of their rich and
redundant mechanical structures.
Unlike humans who might indulge in walking for their pleasure or
because it sustains their health, the
humanoid robot should be motivated by a precise task for obvious
energy-related considerations. How
do we relate the steps that we make to the tasks that we perform
with our hands or any other part of our
bodies? Again, when we think of the number of solutions we
realize that the robot must often choose
from an infinity, which it still has to be aware of. Let us
again consider the simple example of picking
up an object, from the floor this time. We have not yet spoken
of a level of intelligence to build a piece
of furniture from a famous Swedish brand, we will merely ask the
robot to collect the fallen assembly
manual. The robot, nonetheless, faces various difficulties:
where to stand to be able to perform the task?
The object is on the ground, therefore it will probably need to
crouch, so how to take this posture into
account for the motion plan? Is the task feasible at all given
its body shape? We are hinting at a second
seamless form of intelligence: the humanoid robot must decide if
it needs to resort to locomotion in order
to carry out a task and if so, find a suitable one.
In fact, most animals possess this level of intelligence, they
flawlessly move to where an intended
action could be fulfilled. The motion of legged forms on a
terrain could roughly be seen as a monotonous
translation and/or rotation to attain a remote objective.
However, a fine adjustment occurs at the end of
the locomotion so that the posture is adapted to the intended
action. The complexity of actions and
postures is much higher for bipeds than for four-legged animals,
therefore any heuristic-based resolution
of the task-driven locomotion problem is too restrictive.
The planning component of a humanoid robot must rigorously
relate its tasks to its whole body and
locomotion capabilities, otherwise even the simplest possible
tasks could be failed. We aimed in our
work at answering this requirement.
1.2 Related works and contributions
The two major components of this topic, whole body motion
generation and locomotion planning,
have been subject to an extensive research activity. Both
subjects could be considered new in the overall
young field of robotics. This thesis advances a new view for
each subject.
In the context of robot motion control, a task is defined as a
desired kinematic or dynamic property
in the robot (e.g position of hand, forces applied on
manipulated object, direction of cameras, etc). For
robotic arms with a few degrees of freedom, we may find
analytical formulas giving the unique control
satisfying a desired task. For highly-articulated structures
like humanoid robots, the kinematic structure
-
is often redundant with respect to a task, offering an infinity
of possible controls to choose from. One
efficient way of solving the problem of control in redundancy is
by identifying a task to the minimum of
a convex cost function that can be reached using a numerical
algorithm. If we wanted to take advantage
of the redundancy of the system and assigned several tasks at a
time, we could still resort to the same
algorithm and minimize the sum of the cost functions associated
with the tasks. However, if the tasks
became conflicting, the numerical algorithm could lead to a
robot state where none of them are satisfied.
To avoid such situations, it has been proposed to strictly
prioritize tasks so that when such a conflict
occurs, the algorithm should privilege the task with higher
priority. Such a prioritization of a task A over
a task B has been modeled in [Liégeois 1977] as the control for
task B within the set of controls satisfying
task A. The framework was further developed in [Nakamura 1990]
and extended to any number of tasks
in [Siciliano and Slotine 1991; Baerlocher and Boulic 1998],
with illustrations in velocity-based [Yoshida
et al. 2006; Mansard and Chaumette 2007; Neo et al. 2007] and
torque-based [Khatib et al. 2004] control
of humanoid robots.
This classical prioritization algorithm presents an important
shortcoming: its original design is
unable to consider unilateral constraints, inequalities. And
physical systems are always subject to
such constraints, due to bounded control values, limited freedom
of motion because of obstacles, etc...
Sometimes, tasks can also be expressed more rigorously with
inequalities. For example, the task
“Walk through this corridor” is equivalent to “Go forward while
staying in the region delimited by
the walls of the corridor”. Some workarounds have been proposed,
associating for example to each
inequality constraint an artificial potential function which
generates control forces pushing away from
the constraint [Khatib 1986; Marchand and Hager 1998; Chaumette
and Marchand 2001], yet inequality
constraints would appear then to have the lowest priority. To
address this problem, it has been proposed
[Mansard et al. 2008] to calculate a weighted sum of controls,
each correpsonding to a stack of prioritizd
tasks where a subset of inequality constraints is treated as
equality constraints. The main problem with
the algorithm is its exponential complexity in the number of
inequalities. In a third approach, a Quadratic
Program (QP) is used to optimize a sum of cost functions, each
one associated with a desired task, under
strict equality and inequality constraints [Zhao and Badler
1994; Hofmann et al. 2007; Decré et al. 2009;
Salini et al. 2009]. However, this approach can consider only
two priority levels, the level of tasks that
appear as strict constraints of the QP, and the level of tasks
that appear in the optimized cost function.
We strived to overcome this limitation.
The first outcome of this thesis work is a generalized
prioritization algorithm for both equality
and inequality tasks for any type of control parameters.
The second part of the work focused on task-driven locomotion
planning. In the literature, locomotion
planners that are free of ad-hoc strategies are exclusively
relying on probabilistic search algorithms. In
this approach, the parameters of the problem are found by random
exploration of the entire parameter
space. A first application for humanoid robots was shown by
[Kuffner et al. 2002] to build dynamically
stable joint motion with a single foot displacement. [Escande et
al. 2009] used the same class of
algorithms to plan successive contact ports between the robot
and its environment yielding the desired
robot task. With the increase of average precessing power,
applying search algorithms on high-
-
dimensional systems such as humanoid robot has become
affordable. Nonetheless, we believe that these
powerful methods should be saved for complex situations where a
local strategy does not suffice.
Some works attempted a different use of random search
algorithms[Yoshida et al. 2007; Diankov
et al. 2008]. The humanoid is viewed as a wheeled robot that
must join a goal position and orientation
linked to the task in mind. The search algorithm is parametrized
to guarantee collision-free stepping
along the solution path. An independent method takes over to
plan stable stepping motions along the
path. The advantage of this approach is the reduction of the
dimension of the problem down to three
(two translations and one rotation for each node of parameters
in the searched space). It needs, however,
a reliable inference of the goal position and orientation from
the task and the robot’s geometry. In a trial
to compensate for this drawback, a two-time strategy has been
proposed by[Yoshida et al. 2007]: first
infer a gross goal position and orientation for the given task,
then plan a path to it and finally determine if
there is a need to fine-tune the position and orientation by a
single step based on a task-specific strategy.
The advantage of this method is to tackle the problem in
progressive difficulty. Nonetheless, the series
of subproblems suffers from the performance of the weakest link
which is the final local strategy used to
adjust the stance.
The second outcome of this thesis work is a generic task-driven
local footsteps planner that will
advantageously replace any task-specific or robot-specific
strategy on a flat terrain.
1.3 Outline of the thesis
In chapter 2, the classical prioritized inverse kinematics
framework is reviewed for velocity-resolved
control of kinematic structures. The algorithms of this
framework will be closely related to optimization
formulations. Definitions of equality tasks and constraints will
also be given.
In chapter 3, we generalize the prioritization framework to any
set of equality and inequality tasks and
constraints. We will pose the optimization problems and
characterize the corresponding solution sets.
We end this chapter by giving a stage-by-stage resolution
algorithm, as in the classical prioritization
framework.
In chapter 4 we illustrate the generalized framework with two
scenarios for the humanoid robot HRP-
2. The scenarios are based on several tasks and constraints
which are also presented along with useful
calculus for the numerical resolution. Attention will be
especially put on collision avoidance and relaxed
center of mass constraints
In chapter 5, a dynamic walk planning method is reviewed. We
will present a contribution lying in the
merger of two frameworks: numerical inverse kinematics and
Zero-Momentum-Point-based control of
dynamic biped walk. In the robot experiments that illustrate
this work, only dedicated stepping strategies
are designed to help tasks as needed.
In chapter 6, we are ready to formulate and solve the problem of
interest: where to stand and what
footsteps to adopt to carry out the requested tasks. We will use
the algorithm presented in chapter 3 and
-
the tasks and constraints displayed in chapter 4 to formulate an
inverse kinematics problem. We will
illustrate the efficiency of this methods through a variety of
scenarios.
In the final chapter, we exploit whole-body motion generation
reviewed in chapters 3 and 4, the
footsteps planner seen in chapter 6 and the locomotion planner
from chapter 5 to design a local planning
component for humanoid robots on flat terrain.
1.4 Associated publications and software
Publications
Kanoun O., Yoshida E. and Laumond J-P. “An optimization
formulation for footsteps planning” IEEE-
RAS International Conference on Humanoid Robots, 2009
Kanoun O., Lamiraux F., and Wieber P.B. “Prioritizing linear
equality and inequality systems:
application to local motion planning for redundant robots” IEEE
International Conference on Robotics
and Automation, 2009
Yoshida E., Kanoun, O., Esteves Jaramillo C., and Laumond J.P.
“Task-driven support polygon
reshaping for humanoids” IEEE-RAS International Conference on
Humanoid Robots, 2006
Yoshida E., Mallet A., Lamiraux F., Kanoun O., Stasse O.,
Poirier M., Dominey P.F., Laumond J.P.
and Yokoi K “”Give me the purple ball” - he said to HRP-2 N.14”
IEEE-RAS International Conference
on Humanoid Robots 2007
Kanehiro F., Lamiraux F., Kanoun O., Yoshida E. and Laumond J.P.
“A local collision avoidance
method for non-strictly convex objects” Robotics: Science and
Systems Conference 2008
Yoshida E., Poirier M., Laumond J.P., Kanoun O., Lamiraux F.,
Alami R. and Yokoi K.“Whole-body
motion planning for pivoting based manipulation by humanoids”
IEEE International Conference on
Robotics and Automation 2008
Kanehiro F., Yoshida E., Lamiraux F., Kanoun O. and Laumond,
J.P. “A local collision avoidance
method for non-strictly convex object” (in japanese) Journal of
the Robotics Society of Japan 2008
Yoshida E., Laumond J.P., Esteves Jaramillo C., Kanoun O.,
Sakaguchi T., and Yokoi K. “Whole-
body locomotion, manipulation and reaching for humanoids”
Lecture Notes in Computer Science 5277,
Springer, 2008, ISBN 978-3-540-89219-9
-
Yoshida E., Poirier M., Laumond J.P., Kanoun O., Lamiraux F.,
Alami R. and Yokoi K. “Regrasp
planning for pivoting manipulation by a humanoid robot” IEEE
International Conference on Robotics
and Automation 2009
Created software
hppGik : is a software implementing primarily a prioritized
equality system solver (Problem 2.19,
Algorithm 2). A second solver dedicated to prioritized inverse
kinematics is built on top (Algorithm
3). Joint limits are taken into account following (2.22).
Singularities are with using either the regulated
pseudo-inversion (2.10) or the thresholded one (2.9). A set of
equality tasks (body position, orientation,
parallelism, plane, posture, gaze direction and center of mass
position) is also implemented. The
dynamic walk generation based on ZMP trajectory planning
(section 5.1.4) is also integrated. There
are objects that enable the user to program desired tasks on a
time line, including dynamic locomotion
tasks expressed as step tokens, and launch the batch resolution
of the entire time line.
hppHik : is a software that implements the novel linear equality
and inequality prioritization
(Algorithm 4). It is based on the a quadratic program solver
courtesy of AEM-Design[Lawrence et al. ].
It implements the slack variable-based optimization of linear
inequality systems (3.8).
hppLocalStepper : implements the footsteps planner (currently
Block 1 of Algorithm 7 only). All
tasks presented in chapter 4 are implemented, including
collision avoidance and static equilibrium with
a deformable robot support polygon.
-
2Classical prioritized inverse kinematics
2.1 Definitions
Kinematics is a branch of mechanics that studies the motion of
solid particles without consideration of
their inertia and the forces acting on them. To introduce
inverse kinematics let us consider the kinematic
chain composed of three links represented in figure 2.1.
q2
q1
q3
l1
l2 l
3
O
M
x
y
P
Figure 2.1: A three-link kinematic chain
Three rotation joints make the chain move in the plane
(O,~x,~y), where the position of the point M is
:
−−→OM =
(
xM
yM
)
=
(
l1cos(q1)+ l2cos(q1 +q2)+ l3cos(q1 +q2 +q3)
l1sin(q1)+ l2sin(q1 +q2)+ l3sin(q1 +q2 +q3)
)
(2.1)
7
-
Calculating the position of M, tip of the third link, from the
joint configuration (q1,q2,q3) is a
forward kinematics problem. It consists in evaluating a property
of the chain in the work space given the
configuration of the chain in the joint space, whose dimension
equals the number of degrees of freedom
of the kinematic structure. Inverse kinematics designates the
opposite problem consisting in finding a
configuration fulfilling certain properties, for instance
positioning the tip M at the point P.
Considering any kinematic structure with n degrees of freedom,
we define its configuration:
q = (q1, · · · ,qn) ∈ℜn
and express desired values for some kinematic properties, which
can be written without loss of generality
as:
x(q) =~0 (2.2)
where:
x : ℜn −→ ℜmq 7−→ x(q)
is the m-dimensioned vectorial function defining a kinematic
property. For a configuration q of the
structure, if (2.2) is verified and is to be kept, it can be
interpreted as a kinematic equality constraint.
In the opposite case where the relationship is not yet
fulfilled, we may call it a kinematic equality task.
A task expressed as (2.2) is often a non linear function of q
that does not admit a trivial inverse. In
complex kinematic structures such as humanoid robots, the
degrees of freedom relevant to a given task
often exceed in number the dimension of the task. For such
systems we resort to numerical methods to
solve inverse kinematics problems.
2.2 Differential kinematics
By differential kinematics[Nakamura 1990] we refer to the
iterative process of computing small
configuration updates for kinematic structure to converge
towards a state that achieves a given task.
This process is also known as the Newton-Raphson algorithm
applied to inverse kinematics. The tasks
we consider here are differentiable vector functions of the
joint configuration.
By computing the jacobian of the task J = ∂x∂q
(q), we can calculate configuration updates δq to make
the task value converge towards x(q) = 0. These joint updates
are solution of the following linear
differential equation [Liégeois 1977]:
Jδq =−λx(q) (2.3)
where λ is a positive real number. To simplify the notations, we
define:
δx (q,λ ) =−λx(q)
-
0 x(q)q0
x(q0)
q1
x(q1)
q2
q*
Figure 2.2: Newton-Raphson iterations to solve x(q) = 0
and rewrite the previous equation simply as:
Jδq = δx (2.4)
The relationship (2.4) is a system of linear equations for which
a solution might not always exist,
depending on the rank of J. If J keeps a full rank, successive
updates make the configuration converge
to q∗ satisfying x(q∗) = 0. We illustrate this process
(Algorithm 1) in figure 2.2 using a simple function
mapping a 1D configuration space to a 1D workspace.
Algorithm 1 Numerical inverse kinematics for a single task T (q)
= 0
1: Define εp > 0 the minimum task value progression required
to continue
2: Initialize p > εp3: while p > εp do
4: Evaluate T0 ← ‖T (q)‖5: Calculate jacobian of T (q): JT6:
Solve JT δq =−λT (q) in δq7: Update configuration q ← q+δq8:
Evaluate T1 ← ‖T (q)‖9: Calculate task progression: p = T0−T1
10: end while
We suppose that J has full rank (the opposite case is studied in
next section). The solution of the
linear system (2.4) form an affine subspace of ℜn. For instance,
in case n = 3 this affine subspace is
either a point, a line, a plane or the whole space ℜ3. One way
of calculating a point in that subspace is to
orthogonally project the origin of the updates space on the
affine subspace. This projection is the result
of the following minimization:
minδq
|δq‖2
s.t Jδq = δx(2.5)
-
which yields:
δq = J#δx
= JT (JJT )−1δx(2.6)
J# is called the pseudo-inverse of J. A solution to the linear
system (2.4) in general can be written as:
δq = J#δx+(I− J#J)z (2.7)
where z is any vector in ℜn. The operator (I− J#J) is the
projector along the direction of the affinesubspace of solutions.
Figure 2.2 illustrates the components of the general form.
Jδq=δx(I-J#J)z
J#δx
z
Figure 2.3: The plane represents the solutions to a linear
equality in a 3D space
When the linear system (2.4) is over-constrained, the
minimization of ‖Jδq− δx‖2 is consideredinstead, yielding the
solution:
δq = (JT J)−1JT δx (2.8)
2.3 Local extrema, or singularities
Gradient descent methods, such as Newton-Raphson algorithm, are
subject to local extrema. Consider
for example the case represented in figure 2.4. The function x
has a local minimum whose neighborhood
does not intersect the subspace x(q) = 0. At the minimum, the
jacobian becomes singular, the product
JJT gives an ill-conditioned matrix, which results in the
divergence of the numerical method (see how
q2 falls far from the local optimum).
-
0
x(q)
q0
x(q0)
q1q2
Figure 2.4: Divergence of Newton-Raphson algorithm near
singularity
Near singularity, one method[Nakamura 1990] to avoid divergence
is to perform a singular value
decomposition on J and only invert the well conditioned part,
discarding the singular directions. To do
so, we decompose J as:
J = U
σ1
. . .
σm
V T
then cancel the singular values below a chosen threshold and
invert the ones above
J# = V
(
Σ−1 0
0 0
)
UT (2.9)
A second method[Nakamura and Hanafusa 1986] consists in
replacing the optimization problem (2.5)
by the following:
minw,δq
‖w‖2 + k2‖δq‖2
s.t Jδq−δx = w(2.10)
where k is a scalar. Minimizing the first part of the objective
function tends to satisfy the linear constraint
while minimizing the second part tends to keep a null update.
Unless the linear constraint admits the null
vector as solution, in which case it represents the optimum, the
outcome of the optimization is a joint
update with smaller norm than the one from (2.6). The importance
of the second part of the objective
function is scaled with k, directly influencing the norm of the
optimal point, which is given by the
formula:
δq = JT (JJT + k2I)−1δx (2.11)
The factor k2 amplifies all eigen values of JJT thus regulating
the pseudo-inversion. To picture this
process, consider a real function x and the tangent to its curve
at point (x0,y0):
x′(q−q0) = x− x0
-
When the derivative of x is not null, damping the inverse of x
by a factor k2 changes the tangent to:
(x′)2 + k2
x′(q−q0) = x− x0
We illustrate in figure 2.5 the effect of factor k2 on the
tangents near the singularity.
0
x(q)
Figure 2.5: Regulated pseudo-inversion: steeper tangents prevent
large joint updates near singularity.
2.4 Prioritization
Figure 2.6: One target has to be prioritized to prevent failing
both grasps.
Suppose that we seek the completion of two tasks x1(q) = 0 and
x2(q) = 0 and that we cannot assess their
feasibility without resorting to numerical resolution. Suppose,
furthermore, that task 1 is more important
that task 2. We derive the linear equations for differential
kinematics:
J1δq1 = δx1 (2.12)
J2δq2 = δx2 (2.13)
-
Equation (2.12) has an affine subspace of solutions:
{δq ∈ℜn s.t δq = J1#δx1 +(I− J1#J1)z, z ∈ℜn
}(2.14)
To express the absolute priority of task 1 with respect to task
2, δq is replaced in (2.13) giving the linear
system:
δx2− J2J1#δx1 = J2(I− J1#J1)z (2.15)
Defining N1 = (I− J1#J1) and δq1 = J1#δx1, the above linear
system is solved in z:{
minz‖δ z‖2
s.t δx2− J2δq1 = J2N1z(2.16)
yielding:
δq = J1#δx1 +N1(J2N1)
#(x2− J2δq1)
which can be simplified to ([Nakamura 1990]):
δq =
δq2︷ ︸︸ ︷
J1#δx1
︸ ︷︷ ︸
δq1
+(J2N1)#(x2− J2δq1) (2.17)
Figure 2.4 shows a representation of the successive orthogonal
projections that lead to δq. To express
the priorities, the second linear system is solved within the
solutions of the first linear system, hence the
second projection occurring inside the plane defined by the
solutions of the first system. In case the tasks
J1δq=δx
1δq
1
J2δq=δx
2
δq2
δq
Figure 2.7: Compatible tasks. The linear equality systems are
compatible thus a solution satisfying both
is found by successive pseudo-inversions.
are conflicting the union of linear systems is singular. The
joint update δq satisfies the first linear system
and cannot satisfy the second. The regulated (2.11) or truncated
(2.9) pseudo-inversion of the second
linear system ensures that the singularity does not yield a
large update. The effect of both methods is
-
J1δq=δx
1δq1
J2δq=δx
2
δq2
δqreg
δq
(a) Regulation of the second pseudo-inversion gives a
point between the solution to the first linear system
and the solution to the normal pseudo-inversion of the
second linear system.
J1δq=δx
1δq
1
J2δq=δx
2
δq2
δq
(b) Truncation of singular directions. The projection
of the red line onto the plane represents the set of
solutions to the truncated system.
Figure 2.8: Conflicting tasks. The red line represents the
solutions of the linear system corresponding to
the second task. The singularity of second task is represented
by the near parallelism of the red line and
the plane.
shown in figure 2.4.
The prioritization process is straightforwardly generalized to a
set of k tasks. At priority level i, the
solved optimization problem is:
minw,δqi
‖w‖2 + ki2‖δqi‖2
s.t JiNi−1δqi− (δxi− Jiδqi−1) = w(2.18)
Based on this generalization, Algorithm 2 describes the steps
taken to solve a set of prioritized linear
equality systems. We propose the following compact formulation
of the problem solved by this
algorithm:
Find δq∗ solution to the optimization problem:
minδq∈Sk
‖δq‖2
s.t S0 = ℜn
Si = Argminδq∈Si−1
‖Jiδq−δxi‖2(2.19)
Algorithm 2 calculates an update that acts on the values of all
tasks simultaneously. Between two
consecutive stages ranked i and i+1, the joint update is
modified to take into account the tasks at priority
level i without compromising the the tasks from level 1 to i−1.
Like for the single task case, the linearsystems derived from the
prioritized tasks are repeatedly solved by Algorithm 2 until
convergence. The
convergence criterion takes into account all the tasks and
observes their priority. It is shown in Algorithm
3.
-
Algorithm 2 Solve k prioritized linear equality systems
1: n is the dimension of the kinematic structure
2: N0 ← I (n by n identity matrix)3: δq ← 0 (n-sized null
vector)4: for i = 1 to k do5: Calculate linear equality system (Ji,
δxi)
6: Ĵi ← JiNi−17: Calculate pseudo-inverse Ĵ#i8: δqi ← Ĵ#i
(δxi− Jiδqi−1)9: δq ← δq+δqi
10: Ni ← Ni− Ĵ#i Ĵi11: end for
2.5 Handling inequality constraints
Numerical inverse kinematics are used to control the motion of
virtual characters, humanoid robots,
etc. Most actuators on robotic systems only allow for a bounded
range of motion between the connected
bodies. Virtual characters also require joint limits to be
observed for realism. Joint limits are naturally
expressed as inequality constraints on the configuration:
in f (q)≤ q≤ sup(q) (2.20)
and translate to joint update space as:
in f (q)−q≤ δq≤ sup(q)−q (2.21)
which define an admissible convex volume in joint space outside
of which a point corresponds to an
update that would violate the joint limits. Naturally, the
desired bounds for δq may be more constraining
than in (2.21). From a geometrical point of view,
pseudo-inversion is the computation of the orthogonal
projection of a point onto an affine subspace. Nothing prevents
the coordinates of the projection to fall
outside the convex volume allowed by inequalities (2.21).
Nonetheless, pseudo-inversion of a linear
equality system is a fast operation that can be afforded for
online robot control, animation of virtual
figures in movies and video games. Therefore, modified versions
of Algorithm 2 were proposed to
account for joint limits.
One solution is to replace the optimization problem at every
priority stage by the following one:
minδq
δqTWδq
s.t Jδq = δx(2.22)
-
Algorithm 3 Numerical inverse kinematics for k prioritized tasks
{T1(q) = 0, ...,Tk(q) = 0}1: Define εp the minimum task progression
required to continue
2: Call p the measure of task value progression
3: repeat
4:
5: for i from 1 to k do
6: Calculate value of task i: T 0i (q)7: end for
8:
9: Solve the k prioritized linear systems [Algorithm 2]
10: Update configuration q ← q+δq11:
12: for i from 1 to k do
13: Calculate value of task i: T 1i (q)14: Evaluate progression
p = T 1i (q)−T 0i (q)15: if p > εp then
16: break.
17: end if
18: end for
19:
20: until p < εp
W is a positive diagonal n-by-n matrix. The objective function
here affects each degree of freedom in the
configuration q with a weight wi. To emphasize this, the
objective function can be written as:
n
∑i=1
wiδqi2
where δqi
and wi are the i-th component of δq and w respectively. The
larger the weight wi, the more
|δqi| is minimized with respect to other coordinates, hence the
idea: if dqidt
> 0 and qi is close enough
to sup(q) then wi can be gradually increased to reduce the norm
of δqi, making the corresponding joint
brake before violating the upper limit. The same reasoning holds
for the lower limit. The analytical
solution to problem (2.22) is:
δq = J#W δx
= W−1JT (JW−1JT )−1δx(2.23)
The weighted pseudo-inverse J#W is what we would have obtained
from the simple pseudo-inversion of
the system:
J√
W−1δq = δx (2.24)
Expanding above equation into:n
∑i=1
1√wi
∂x
∂qiδqi = δx (2.25)
we could observe that the factor 1√wi
scales the partial derivative of the task x, acting as brakes
or
accelerator on the coordinate qi. The performance of this method
relies essentially on the tuning of
-
the parameters wi with respect to the state of the kinematic
structure.
The above method does not address the general case where any
inequality constraint on δq might be
considered. For example, bounding the velocity of a point P in
the work space along vector d to remain
below a maximal value c is written as:
dT δP(q)≤ c
from which an inequality constraint on the joint update
follows:
dT JPδq≤ c
where JP is the jacobian of the position of point P with respect
to q.
There are solutions proposed by [Baerlocher 2001; Peinado et al.
2005] to account for linear inequality
constraints within Algorithm 2. The inequality constraints are
monitored after each priority stage and
if violated, the stage is re-computed while taking the violated
inequality constraints as hard equality
constraints. This is the principle of active set algorithm.
Supposing that priority stage i sees the system
of inequality constraints Aδq ≤ b violated, the following
optimization problem is solved instead of theusual problem
(2.18):
minw,δqi
‖w‖2 + ki2‖δqi‖2
s.t JiNi−1δqi− (δxi− Jiδqi−1) = wAδq = b
(2.26)
The resolution of 2.26 is repeated until the solution update of
stage i, δqi, satisfies all the inequality
constraints. The algorithm resumes its normal course
afterward.
The main drawback of the method is the nested loop of
pseudo-inversions that must be done at
every stage to enforce the inequality constraints . There are
more efficient algorithms to solve linearly
constrained optimization problems with quadratic costs. As a
matter of fact, other works[Zhao and
Badler 1994; Faverjon and Tournassoud 1987] resorted to these
algorithms. However, the separation of
tasks into discrete priority levels was not available and the
authors resorted to weighted sums of objective
functions corresponding to several simultaneous tasks, such
as:
minδq
∑ki=1 w
i‖Jiδq−δxi‖2
s.t Aδq = b
Bδq≤ c(2.27)
The weighted objective function does not permit strict
enforcement of priority. When two of the
linear systems represented in the objective function cannot be
solved simultaneously, the solution to
the minimization problem is a trade-off update that satisfies
neither of the systems.
-
3Generalizing priority to inequality tasks
3.1 Inequality task definition
Inequality tasks differ from inequality constraints in the way
equality tasks differ from equality
constraints. For a kinematic structure with n degrees of
freedom, we have a differentiable kinematic
property g : q 7−→ g(q) whose values are desired below a certain
threshold, expressed without loss ofgenerality in the following
inequality:
g(q)≤ 0 (3.1)
For a configuration q of the structure, if (3.1) is verified and
is to be kept, it can be interpreted as a
kinematic inequality constraint. In the opposite case where the
relationship is not yet fulfilled, we may
call it a kinematic inequality task.
One trivial example of inequality task can be g(q) = Hz(q)− 0.5
where Hz(q) is the height of therobot hands from the ground. In
this example, the task Hz(q)− 0.5 ≤ 0 requires the hands to be
belowthe horizontal plane of equation z = 0.5. Inequality tasks may
be solved with Newton-Raphson algorithm
like equality tasks through an iterative resolution of a linear
differential system of inequalities. Defining
Jg =∂g∂q
(q), we write the system to solve at a given configuration
q:
Jgδq≤−λg(q) λ ∈ℜ+∗ (3.2)
19
-
The method of resolution of such systems is detailed in the next
section. Continuing with the above
example on the hand, equation (3.2) gives the following
inequality:
∂Hz
∂qδq≤−λ (Hz(q)−0.5)
For a constant value of λ and when the height of hands is below
0.5, the above equation sets a decreasing
upper bound on the vertical hand velocity, a bound that is equal
to 0 for z = 0.5. Above the plane, the
upper bound is negative and the plane z = 0.5 acts as an
attractor.
A linear inequality, when it has solutions, defines a halfspace.
The set of solutions of a system of linear
inequalities is the intersection of the half spaces generated by
its inequalities. This set is a volume of ℜn
bounded by a convex polytope which may be closed or infinite
(for example a half space is infinite). See
figure 3.1 for an illustration of a system of linear
inequalities in ℜ2.
I1
I2
I
x2
x1
Figure 3.1: The linear inequalities y≥ x and y≥−x determine the
filled convex polytope.
One might observe that any equality task f (q) = 0 can be
expressed as two simultaneous inequality
tasks, f (q) ≥ 0 and f (q) ≤ 0. Therefore inverse kinematics
problems expressed as inequalities engulfthose that use equalities.
One attractive aspect of inequalities is the possibility to specify
a range or a
bounding on a kinematic property. What remains to be constructed
is a framework that authorizes the
strict prioritization of inequality and equality tasks in any
order. One may wonder about the scenarios
that require the inequality tasks to be at lower priority than
equalities. Consider for example a humanoid
robot which has to grasp an object seen with embedded cameras.
It is best if its reaching hand does not
come between the cameras and the object too soon. This is
because we would like to keep checking the
visual target to maximize the chance of a successful grasp. In
this scenario, the robot has to accomplish
a primary reaching task and a secondary region-avoidance
task.
-
3.2 Description of the approach
Affecting priorities to linear systems means that we leave some
of the systems unsolved to respect
the ones with higher priorities. Letting L1 and L2 be two linear
systems without common solutions,
prioritizing L1 over L2 means that we retain a solution which
satisfies L1 to the expense of L2.
Nonetheless, to take into account L2, one may select a solution
of L1 which minimizes the euclidean
distance to L2’s solutions set. Euclidean distance is one
example of optimality criterion adapted to
systems of linear equalities. As a matter of fact, a point
realizing this shortest distance belongs to
the orthogonal projection of L2’s solutions on L1’s and can be
obtained analytically. Furthermore, the
entire set of points realizing the shortest distance may also be
determined analytically [Nakamura 1990;
Siciliano and Slotine 1991]. To solve a third system of linear
equalities L3, the resolution is done within
L2’s optimal set.
L1
L2
M
P
Figure 3.2: The primary linear equality L1 and a secondary
system of 3 linear inequalities L2 are without
common solutions. M and P are solutions of L1 minimizing the
euclidean distance to L2’s set, however,
P should be preferred since it satisfies two inequalities out of
three while M satisfies none.
For our problem, we adopt the same approach consisting in
solving every linear system in the optimal
set defined by higher priorities. When we introduce systems of
linear inequalities, however, we introduce
solution sets which are volumes of ℜn bounded by convex
polytopes. In this particular case, euclidean
distance is not a good optimality criterion (Figure 3.2).
Therefore, we start by mapping equality and
inequality tasks to suitable optimality criteria and study the
nature of generated sets of solutions. We
prove that the optimizations result in sets that can be
described with linear systems and we deduce a
resolution algorithm that is relatively easy to implement. This
algorithm intended to prioritize inverse
kinematics tasks can be straightforwardly applied to any problem
involving the prioritization of a set of
linear equality and inequality systems, regardless of the type
of parameters. For this chapter, we will
abandon the notations J and δq to emphasize the generality of
the algorithm.
3.3 Mapping linear systems to optimization problems
In this section we construct optimization problems to solve each
type of linear system. For each problem,
we demonstrate the nature of the optimal set.
-
Let A and C be matrices in ℜm×n and b and d vectors in ℜm with
(m,n) ∈ N2. We will consider inthe following either a system of
linear equalities
Ax = b (3.3)
or a system of linear inequalities
Cx≤ d (3.4)
or both. When m = 1, (3.3) is reduced to one linear equation and
(3.4) to one linear inequality.
3.3.1 System of linear equalities
When trying to satisfy a system (3.3) of linear equalities while
constrained to a non-empty convex set
Ω⊂ℜn, we will consider the set Se of optimal solutions to the
following minimization problem:
minx∈Ω
1
2‖w‖2 (3.5)
with
w = Ax−b. (3.6)
Since the minimized function is coercive, the set Se is
non-empty. We also have the property:
x1,x2 ∈ Se⇔ x1,x2 ∈Ω and Ax1 = Ax2, (3.7)
from which we can conclude that the set Se is convex.
Proof: Let us consider an optimal solution x∗ to the
minimization problem (3.5)-(3.6). The gradient
of the minimized function at this point is
AT (Ax∗−b).
The Karush-Kuhn-Tucker optimality conditions give us that the
scalar product between this gradient and
any vector v not pointing outside Ω from x∗ is non-negative,
w∗T Av≥ 0
with
w∗ = Ax∗−b.
Let us consider now two such optimal solutions, x∗1 and x∗2.
Since the set Ω is convex, the direction x
∗2−x∗1
points towards its inside from x∗1, so we have
w∗T1 A(x∗2− x∗1)≥ 0
which is equivalent to
w∗T1 w∗2−‖w∗1‖2 ≥ 0.
-
The same can be written from x∗2,
w∗T2 w∗1−‖w∗2‖2 ≥ 0,
so that we obtain
‖w∗2−w∗1‖2 = ‖w∗2‖2 +‖w∗1‖2−2w∗T2 w∗1 ≤ 0,
but this squared norm cannot be negative, so it must be zero and
w∗2 = w∗1, what concludes the proof. �
In the unconstrained case, when Ω = ℜn, the solutions of
(3.5)-(3.6) are such that AT Ax∗ = AT b.
This minimization problem corresponds therefore to a constrained
pseudo-inverse solution of the system
of linear equalities (3.3).
3.3.2 System of linear inequalities
When trying to satisfy a system (3.4) of linear inequalities
while constrained to a non-empty convex set
Ω⊂ℜn, we will consider the set Si of optimal solutions to the
following minimization problem:
minx∈Ω,w∈ℜm
1
2‖w‖2 (3.8)
with
w≥Cx−d, (3.9)
where w plays now the role of a vector in ℜm of slack variables.
Once again, since the minimized
function is coercive, the set Si is non-empty. Considering each
inequality cjx ≤ b j of the system (3.4)
separately, we also have the property:
x1,x2 ∈ Si⇔ x1,x2 ∈Ω and
∀ j{
c jx1 ≤ d j⇔ c jx2 ≤ d j,c jx1 > d
j⇒ c jx1 = c jx2,(3.10)
which means that all the optimal solutions satisfy a same set of
inequalities and violate the others by a
same amount, and from which we can conclude that the set Si is
convex.
Proof: Let us consider an optimal solution x∗, w∗ to the
minimization problem (3.8)-(3.9). The
Karush-Kuhn-Tucker optimality conditions give that for every
vector v not pointing outside Ω from x∗,
w∗TCv≥ 0
and
w∗ = max{0,Cx∗−d}. (3.11)
This last condition indicates that if an inequality in the
system (3.4) is satisfied, the corresponding
element of w∗ is zero, and when an inequality is violated, the
corresponding element of w∗ is equal
to the value of the violation.
Let us consider now two such optimal solutions, x∗1, w∗1 and
x
∗2, w
∗2. Since the set Ω is convex, the
-
direction x∗2− x∗1 points towards its inside from x∗1, so we
have
w∗T1 C(x∗2− x∗1)≥ 0
which is equivalent to
w∗T1 (Cx∗2−d)−w∗T1 (Cx∗1−d)≥ 0.
The optimality condition (3.11) gives
w∗T1 w∗2 ≥ w∗T1 (Cx∗2−d)
and
w∗T1 w∗1 = w
∗T1 (Cx
∗1−d),
so we obtain
w∗T1 w∗2−‖w∗1‖2 ≥ 0.
The same can be written from x∗2,
w∗T2 w∗1−‖w∗2‖2 ≥ 0,
so that we obtain
‖w∗2−w∗1‖2 = ‖w∗2‖2 +‖w∗1‖2−2w∗T2 w∗1 ≤ 0,
but this squared norm cannot be negative, so it must be zero and
w∗2 = w∗1, what concludes the proof. �
3.3.3 Mixed system of linear equalities and inequalities
We can observe that systems of linear equalities and systems of
linear inequalities are dealt with
optimization problems (3.5)-(3.6) and (3.8)-(3.9) which have
similar lay-outs and similar properties (3.7)
and (3.10). The generalization of these results to mixed systems
of linear equalities and inequalities is
therefore trivial and we will consider in the following the
minimization problem (in a more compact
form)
minx∈Ω,w∈ℜm
1
2‖Ax−b‖2 + 1
2‖w‖2 (3.12)
with
Cx−w≤ d. (3.13)
The set of solutions to this minimization problem shares both
properties (3.7) and (3.10).
3.4 Prioritizing linear systems
3.4.1 Formulation
Let us consider now the problem of trying to satisfy a set of
systems of linear equalities and inequalities
with a strict order of priority between these systems. At each
level of priority k ∈ {1, . . . p}, both asystem of linear
equalities (3.3) and a system of linear inequalities (3.4) are
considered, with matrices
-
and vectors Ak, bk, Ck, dk indexed by their priority level k. At
each level of priority, we try to satisfy
these systems while strictly enforcing the solutions found for
the levels of higher priority. We propose
to do so by solving at each level of priority a minimization
problem such as (3.12)-(3.13). With levels of
priority decreasing with k, that gives:
S0 = ℜn, (3.14)
Sk+1 = Argminx∈Sk,w∈ℜm
1
2‖Akx−bk‖2 +
1
2‖w‖2 (3.15)
with Ckx−w≤ dk. (3.16)
3.4.2 Properties
A first direct implication of properties (3.7) and (3.10) is
that throughout the process (3.14)-(3.16),
Sk+1 ⊆ Sk.
This means that the set of solutions found at a level of
priority k is always strictly enforced at lower levels
of priority, what is the main objective of all this
prioritization scheme.
A second direct implication of these properties (3.7) and (3.10)
is that if Sk is a non-empty convex
polytope, Sk+1 is also a non-empty convex polytope, the shape of
which is given in properties (3.7) and
(3.10). Figures 3.3 and 3.4.2 illustrate how these sets evolve
in different cases. Classically, these convex
polytopes can always be represented by systems of linear
equalities and inequalities:
∀k, ∃Āk, b̄k,C̄k, d̄k such that x ∈ Sk⇔{
Ākx = b̄k
C̄kx≤ d̄k
With this representation, the step (3.15)-(3.16) in the
prioritization process appears to be a simple
Quadratic Program with linear constraints that can be solved
efficiently.
Note that when only systems of linear equalities are considered,
with the additional final requirement
of choosing x∗ with a minimal norm, the prioritization process
(3.14)-(3.16) boils down to the classical
problem 2.19.
3.4.3 Algorithms
The proposed Algorithm consists in processing the priority
levels from highest to lowest and solving at
every level the corresponding Quadratic Program. The
representation of the sets Sk by systems of linear
equalities and inequalities is efficiently updated then by
direct application of the properties (3.7) and
(3.10).
It is naturally possible to optimize additional criteria over
the final set of solutions. For instance, one
might be interested in the solution with minimal norm, or in the
solution that maximizes the distance to
the boundaries of the optimal set, etc.
-
P1
P2
Optimal set
(a) Priority does not matter, the
prioritized linear systems are
compatible and the optimal sets is
the intersections of their respective
optimal sets.
P1
P2
Optimal set
(b) Equality has priority over
inequality. The optimal set
satisfies all possible inequalities
while minimizing distance to the
unfeasible ones.
P1
Optimal setP2
(c) Inequality has priority over
equality. The optimal set minimizes
the distance to the equality’s solution
set.
Figure 3.3: Illustration of the optimal sets for prioritization
problems involving both linear equality and
inequality systems.
Algorithm 4 Solve prioritized linear systems
1: Initialize the system of equalities Ā0, b̄0 to empty.
2: Initialize the system of inequalities C̄0, d̄0 to empty.
3:
4: for k = 0 to p−1 do5:
6: Solve the Quadratic Program (3.15)-(3.16) to obtain Sk+1.
7:
8: Āk+1←[
ĀkAk
]
, b̄k+1←[
b̄kAkx∗k
]
.
9:
10: C̄k+1← C̄k, d̄k+1← d̄k.11:
12: for all cj
k in Ck do
13: if cj
kx∗k ≤ d
j
k then
14:
15: C̄k+1←[
C̄k+1
cj
k
]
, d̄k+1←[
d̄k+1
dj
k
]
.
16:
17: else
18:
19: Āk+1←[
Āk+1
cj
k
]
, b̄k+1←[
b̄k+1
cj
kx∗k
]
.
20:
21: end if
22: end for
23: end for
-
Optimal setOptimal setOptimal set
Figure 3.4: Equivalent of figure 3.3 in a 3D space.
Note that a similar algorithm has already been described by
[Wolf 1996], but in the setting of
Constraint Programming on discrete variables: the structure and
the logic are similar, but the inner
workings are very different, especially the theoretical analysis
of section 3.3.
For an equality task f (q) = 0, the value of the task was
implicitly defined in previous chapter as the
convex function:
q 7−→ ‖ f (q)‖ (3.17)
For an inequality task g(q)≤ 0, we use the convex value
function:
q 7−→√
∑j
max(0,g j(q))2 (3.18)
with g j being the j-th inequality in g. This is perhaps better
known as an exterior penalty function[Fiacco
and McCormick 1987] in the context of non-linearly constrained
optimization. In case of a mixture of
equality and inequality tasks, for instance { f (q) = 0,g(q)≤
0}, the sum of all value tasks is taken:
q 7−→ ‖ f (q)‖ +√
∑j
max(0,g j(q))2 (3.19)
Apart from this modification of the task values, the iterative
process (see Algorithm 5) that solves k
prioritized equality or inequality tasks stays the same as in
Algorithm 3.
3.4.4 Dealing with singularities
The proposed algorithm inherits the singularity issues from the
classical algorithm based on pseudo-
inversion. Recall that the pseudo-inverse could be regulated or
thresholded to ensure that the joint updates
do not diverge near singularity. In this new framework, we can
take advantage of the same solutions.
Thus, at every priority stage of Algorithm 4, instead of solving
the problem (3.15)-(3.16) we may choose
-
Algorithm 5 Solve k prioritized tasks (equality, inequality or
mixture)
1: Define εp the minimum task value progression required to
continue
2: Call p the measure of task value progression
3: repeat
4:
5: for i from 1 to k do
6: Calculate value of task i: T 0i (q) using (3.17), (3.18) or
(3.19)7: end for
8:
9: Solve prioritized linear systems [Algorithm 4]
10: Update configuration q ← q+δq11:
12: for i from 1 to k do
13: Calculate value of task i: T 1i (q)14: Evaluate progression
p = T 1i (q)−T 0i (q)15: if p > εp then
16: break.
17: end if
18: end for
19:
20: until p < εp
to solve:
Sk+1 = Argminx∈Sk,w∈ℜm
1
2‖Akx−bk‖2 +
1
2‖w‖2 +λ 2‖x‖2 (3.20)
with Ckx−w≤ dk (3.21)
where λ is a scalar. The following steps in Algorithm 4 are not
modified. In figure (3.5) we illustrate the
effect of the proposed method in a 2D space.
Task(t+Δt)
Task(t)
x(t) x(t+Δt)
(a) The red line represents the solution
set of a linear equality system that is
singular with respect to feasible set,
represented as a blue triangle. Slight
variation of the red line may produce a
large difference in the optimal point x.
x(t)
x(t+Δt)
(b) Damping with factor λ 2‖x‖2reduces the norm of x and
augments the dimension of the
optimal set (from the single
red point in (a) to the purple
segment).
x(t) x(t+Δt)
x(t+2Δt)
(c) Next time iteration sees
the optimal point x and the
optimal set change continuously,
provided the task is continuous.
Figure 3.5: Illustration of the effect of damping in problem
(3.20)-(3.21) across successive time steps.
-
3.5 Conclusion
We have introduced inequality tasks as generalization to both
inequality constraints and equality
tasks. We have defined a unified algorithm for linear equality
and inequality system prioritization. This
algorithm can serve as a basis for numerical resolution of
equality and inequality tasks regardless of the
type of parameters: joint velocities, torques, accelerations,
etc.
For humanoid robot, the physical constrains that express as
inequality constraints, such as joint limits,
non-collision with obstacles, static equilibrium, can be
accounted for in a robust manner within our
framework. As for the concept of inequality tasks, it will
permit a more relaxed specification of tasks
and we will give several examples thereof in the following
chapter.
-
4Illustration
4.1 A humanoid robot
The humanoid robot HRP-2 was used for our work. This robot can
be modeled as a kinematic tree
of links, connected by joints. This kinematic model has 28
revolute joints of freedom and extra ones at
each hand to allow one-dimensional opening and closing. A link i
in this model represents a solid body
in the robot with its shape, its mass mi and its inertial matrix
Ii. A joint is the abstract object representing
a single degree freedom between a parent and one child link. The
joint is characterized by a parameter
qk that determines the position and orientation of a child link
k +1 with respect to the parent link k.
Figure 4.1: The humanoid robot HRP-2.
31
-
Call:
• F a frame attached to the workspace• Fk the frame attached to
link k.• Ok the center of frame Fi.
For a parent link k and its child link k +1, define:
• (Ok,ak) the axis of rotation of joint k.• ak the unitary
vector defining the positive rotation of joint k, expressed in Fk.•
bk the constant vector
−−−−→OkOk+1 separating frames Fk and Fk+1, expressed in Fk.
• Rot(w,α) the rotation matrix representing a rotation around
vector w by angle α .• pk the position of point Ok in frame F• Rk
the rotation matrix defining the frame Fk in frame F• wk the
rotation vector equivalent to Rk, such that Rk = Rot( wk‖wk‖
,‖wk‖)
The position and orientation of link i child of link k can be
calculated as:
pk+1 = pk +Rkbk
Rk+1 = Rk +RkRot(Rkak,qk)
With these formulas, any point in the kinematic structure can be
computed recursively from the root
node of the tree of links, which can be any link. The humanoid
robot having complete motion freedom in
space, the root node is considered linked to the environment by
a 6-degree-of-freedom joint authorizing
any transformation. There are applications that require
interruption of contact between the robot’s feet
and the ground, such as jumping and running. In such cases, the
position and the rotation of every link in
the robot is known only if the root body’s are known. In the
scope of this work, the robot is permanently
in non-slippery contact with a horizontal ground by one or both
of its feet. Therefore, defining a root
node other that a support foot was redundant given the knowledge
of the foot’s position and orientation
in the workspace.
When it is not possible to select the root node for a given
reason (e.g constraint of implementation,
etc) Algorithm 6 can be used to compute the position and
orientation jacobian of a frame with respect
to another, in the kinematic structure. Given that a support
foot is considered in non-slippery contact
with the workspace, computing a jacobian with respect to the
frame of a support foot is equivalent to
computing the jacobian with respect to F , the frame attached to
the workspace. The jacobians used for
the differentiation of constraints and tasks presented in the
next section rely on such computations.
-
Algorithm 6 Compute the jacobian of pi and wi in frame F with
respect to frame Fk
1: Find the chain of joints A from root link to link i
2: Find the chain of joints B from root link to link k
3: C = A∪B−A∩B is the set of joints between links i and k4: for
all c ∈C do5: Calculate the rotation vector a of joint c in frame F
according to case:
6: if c ∈ A then7: a = Rcac8: else
9: a =−Rcac10: end if
11: Position part:
12:∂ pi∂qi
= a× (pi−Oc)13: Orientation part:
14:∂wi∂qi
= a15: end for
4.2 Constraints and tasks
Posture and limits
The simplest expressions of a kinematic tasks are:
q−qre f = 0 (4.1)
or
q−qre f ≤ 0 (4.2)
with qre f being a reference vector in ℜn. The posture task in
equality type are usually placed at the lowest
priority level in the stack of prioritized tasks, acting like a
constant attractor for a preferred configuration.
In inequality, it can be used to define a range of preferred
postures, yet the most common use is as
constraint to observe joint limits.
0
qmin
qmaxqmaxsqmaxiqmins
qmini
|δq|max
−|δq|max
Figure 4.2: Bounding joint updates to enforce joint limits.
To enforce the joint limits described by the double inequality,
qmin ≤ q ≤ qmax, we place bounds onthe absolute value of the joint
updates such that the update is gradually decreased to 0 when the
joint
parameter value is almost to its limit. We implement this
approach by setting the differential system of
-
inequalities as a prior constraint for any prioritized
optimization problem:
δq≤{
|δq|max q−qmaxsqmaxi−qmaxs if q > qmaxi|δq|max otherwise
(4.3)
−δq≤{
|δq|max q−qminsqmini−qmins if q < qmini|δq|max otherwise
(4.4)
where qmini , qmaxi , qmins , qmaxs and |δq|amx are as
implicitly defined by figure 4.2.
Position and orientation
The most common tasks and constraints involve positions and
orientations of bodies in the robot. To
express an inequality task on the position and the orientation
of a frame Fk, we write:
pk = pre f
Rk = Rre f
where pre f and Rre f are the desired position and rotation
matrix for Fk. Call wgap the rotation vector such
that RTk Rre f = Rot(wgap‖wgap‖ ,‖wgap‖). The linear equality
system to be fulfilled write as:
Jkδq = λ
[
pre f − pkRkwgap
]
(4.5)
where J is the full jacobian of the non-root foot’s frame
calculated for instance with Algorithm 6. In case
we are considering a constraint to be permanently constrained, λ
is taken equal to 1.
Plane
We are interested in the position of a point P(q) on the
kinematic structure. If we desire this point to
be on a certain plane of the workspace, we write:
〈un|P(q)〉= c (4.6)
where un and c are the normal vector and the scalar defining the
plane. For the numerical resolution, the
linear system to be considered is:
〈un|JP〉δq =−λ (〈un|P〉− c) (4.7)
with JP ∈ℜ3×n being the jacobian of the position of P(q)
expressed in the same frame as P and un.
-
Figure 4.3: While the robot is walking, an inequality task is
set such that a point in the head of the robot
should go below a horizontal plane (the black line).
Parallelism
Figure 4.4: The glass is maintained vertical by setting a
constraint on hand axis to remain parallel to the
workspace’s~z vector.
We are interested in the orientation of an axis (Oi,ul) attached
to a body in the kinematic structure.
We would like to align this axis with a reference axis (O,ur) in
workspace, thus we write:
ur×ul = 0 (4.8)
We differentiate to construct the linear system for numerical
resolution:
ur× (Jωiδq×ul) =−λur×ul (4.9)
where Jωi designates the jacobian of the rotation speed ωi of
link i. Replacing cross products with
equivalent skew-symmetric matrices the above system can be
re-written as:
[ur]×[ul]×Jωiδq = λur×ul (4.10)
-
This task can be used to maintain a plate or a glass vertical in
the hands of a humanoid robot (see figure
4.4). We often use it to maintain the robot straight.
Cone of directions
ur u
l
ul
ur.u
l
min ur.u
l
Figure 4.5: Setting a minimum value for ul.ur constrains ul to
stay within a maximum angle around ur.
A half-empty bottle could be held a little inclined without risk
of spilling. To define such a constraint
on any link i of the robot, we considered a vector ul attached
to the link and a vector ur representing the
reference direction from which we allow ul to deviate (see
figure 4.5). The corresponding inequality task
writes as:
ur.ul− c > 0 (4.11)
where c is the minimum cosine allowed between ur and ul , giving
the differential inequality system:
−ur.[ul]×Jωiδq≤ λ (ur.ul− c) (4.12)
where Jωi designates the jacobian of the rotation speed ωi of
link i, [ul]x the skew-symmetric matrix
representing a prior cross product by ul .
-
Coplanar line segments
u BD
C
A
Consider HRP-2’s hand having to grasp a cup. The sagittal plane
defined by the hand clamps has to
coincide with the axis of the cup. We have this if two non
parallel line segments of the hands’ plane are
coplanar with the axis of the cup.
Let (A,~u) define a reference axis and let B, C and D be three
points defining the constrained plane on
a a robot link i (figure 4.2). We express the task as:
(u×−→AB)× (u×−→AC) = 0(u×−→AB)× (u×−→AD) = 0
(4.13)
Letting JB, JC and JD be the jacobians of the position of points
B, C and D on link i, the differential
system is:
{([−→AC×u]×[u]×)JB− [−→AB×u]×[u]×)JC}δq = −λ (u×
−→AB)× (u×−→AC)
{([−→AD×u]×[u]×)JB− [−→AB×u]×[u]×)JD}δq = −λ (u×
−→AB)× (u×−→AD)
(4.14)
Gaze direction
A variation of the previous task is applied to control the
optical axis of a vision system attached to
the structure. Let O(q) be a point on the optical axis, ug a
unitary vector along the optical axis oriented
towards the vision field, T a fixed point representing the
target of focus in the workspace and ut the
unitary vector defined by ut =−→OT
‖−→OT‖. Writing the following equation, we require the vision
system to
focus on point T :−→OT ×ug = 0 (4.15)
After differentiation the linear system for numerical resolution
is:
([ug]×JO− [ut ]×[ug]×Jω)δq =−λut ×ug (4.16)
Suppose that instead of defining a focus point on which the
optical axis must be constantly aligned, we
prefer to specify a region of focus described by a solid angle.
In this case, we rewrite the task as two
inequalities, as seen in the previous subsection.
-
Figure 4.6: Two simultaneous tasks: a gaze equality task to look
at a point under the table and a plane
inequality task to draw the head out of the occlusion area (in
transparent gray).
Collision avoidance
A
B
A
B
d
d
d
n
n
n
A
B
V1
V2
Forbidding collision between two elements is preventing the
shortest distance between them to become
null. Between a point A and a point B, between a point and a
line or a line segment, or between two
strictly-convex volumes V1 and V2, there is unique pair of
points realizing the minimal distance (figure
4.2). In these cases, collision is avoided if we observe the
constraint:
〈−→AB|n〉−d ≥ 0 (4.17)
where d is the minimal distance to be kept between points A and
B, and n =−→AB
‖−→AB‖.
One may observe that the relative rotation between the objects
in second and third case may still be
the cause of collision (if, for instance, object V1 rotates
around point A byπ
2in the next time sample),
-
however, since the absolute rotation speed is bounded, the
maximal displacement of any point on either
of the elements is also bounded, therefore the distance d could
always be chosen such that violation of
the minimal distance d remains very small.
The differential system following from constraint 4.17 is
thus:
−〈δ (−→AB)|n〉 ≤ λ (〈−→AB|n〉−d) (4.18)
d
Figure 4.7: Inspired from [Kanehiro et al. 2008]. Constraining a
single pair of points realizing the
minimum distance (represented by vertical black line) between
non-strictly convex shapes does not
prevent collision.
This method was applied by [Faverjon and Tournassoud 1987] on
the the pairs of points realizing the
shortest distance between a mobile robot and the obstacles
around, bounded by strictly-convex shapes.
A recent work by [Kanehiro et al. 2008] focused on the general
case where the objects are non strictly-
convex polyhedra. They showed how a collision may occur when
only a single pair of points realizing the
minimal distance is constrained between a segment and a line in
a plane (see figure 4.7). The method they
propose is an extension of the work by [Faverjon and Tournassoud
1987] where they analyze the pairs
of points to be constrained for all configuration cases between
faces, edges and vertices of polyhedra.
For instance, in the example of the segment and the line in a
plane, their analysis leads to constrain the
extremities of the segment as suggested in figure 4.8.
d
Figure 4.8: Inspired from [Kanehiro et al. 2008]. At least two
linear velocity constraints are needed to
prevent collision between a line segment and a plane
The algorithm they propose permits accurate collision avoidance
but can be too costly if the
geometrical model of the robot is complex. We have consequently
designed a simplified geometry for
the robot where every link is wrapped in a cylinder capped with
half spheres. The interesting aspect of
this particular geometrical shape is that it represents a line
segment around which a minimal distance d,
-
corresponding to the radius of the cylinder and the spheres,
need be observed (figures 4.9 and 4.10). All
that remains is to follow the same analysis conducted by
[Kanehiro et al. 2008] to determine the pairs of
points that need be constrained between line segments in 3D
space.
Figure 4.9: Setting simple constraints for self-collision
avoidance: the bodies of the robot are wrapped
in cylinders and spheres. The red lines are examples of pairs
that need to be watched for collisions.
A B
C
Dd2
d2
d
d1
Figure 4.10: The approximation of a link with a cylinder and
half-spherical caps is equivalent to an
approximation by a line segment that must not be approached by
less than the radius of the cylinder.
Let us expose the solution for line segments in a plane, since
it will be very close to the 3D case.
Figure 4.11 shows the three configuration cases that demand
different treatment. The cases are defined
according to intersection of segment [CD] with two type of
regions defined with respect to segment
[AB]: the middle region where orthogonal projections on the line
(AB) fall on the segment [AB], and the
complementary regions containing only points A and B. d
represents the minimal authorized distance
-
between the line segments. In the first case, the forbidden
region (half sphere) is strictly convex, therefore
a single pair of points is enough to maintain [CD] out. In the
second case, [CD] completely inside the
middle region, thus the linear velocities of its extremities are
constrained. In the third case, [CD] is
between two adjacent regions. It is convenient to think of it as
two separate line segments, each in
a region enabling us to apply the constrains found for first and
second case, yielding a total of three
constrains.
A B
C
D
A B
C
D
A
C
D
B
d
Figure 4.11: Pairs of points to constrain to maintain a minimal
distance between two plane segments.
For the 3D case that is in our interest, the region analysis
holds, but we have to take one additional
case into account, depicted in figure 4.12. It occurs when the
minimal distance between the line segments
lies in the middle region, and is realized by a pair of points
excluding the extremities. To obtain the
distance and the pair of points realizing it we can write a
point X on [AB] as:
X = A+λ1−→AB λ1 ∈ [0,1]
and a point Y in [CD] as:
Y = C +λ2−→CD λ2 ∈ [0,1]
and solve the optimization problem:
{
min(λ1,λ2)∈[0,1]2
‖Y −X‖2
We have to pay attention to the fact that we project the points
on the surface defined by the minimal
distance d, whereas we projected on a line segment or on a
circle in the 2D case.
We have now a simplified model of our robot and we can write the
constraints that ensure self-
collision avoidance for any configuration. For HRP-2, there was
no need to constrain all pairs of links
since some would never enter in collision either because of the
geometry or because of the settings of
use.
-
C
D
B
A
Y
X
Figure 4.12: Extra case showing pair of points to constrain
between two line segments in 3D space
com
p
d
c
p
→n
Figure 4.13: The velocity of approach of the center of mass
projection to the edges of the support polygon
is damped.
Static equilibrium
The static equilibrium for a humanoid robot standing on a
horizontal ground is verified when the
projection of the center of mass of the robot is inside its
support polygon.
Call C the projection of the center of mass on the ground. The
constraint that keeps C inside the
support polygon is composed of a variable number of the linear
differential inequalities. One linear
inequality constraint is added when an edge of the support
polygon is directly facing C (see figure 4.13).
We control the maximal velocity of C towards the polygon edges
by observing the following inequality:
−〈δ (−→PC)|~n〉 ≤ λ (〈−→PC|~n〉−d) (4.19)
where ~n is the normal vector to the considered edge and d is
the minimal authorized distance. The
support polygon being fixed when the robot is standing on the
ground, the above inequality boils down
to:
−〈JC|~n〉δq≤ λ (〈−→PC|~n〉−d) (4.20)
-
where JC is the jacobian of C with respect to the joints
configuration.
Figure 4.14: The projection of center of mass is constrained
inside the left foot while the left hand tries
to reach the front target. The right limbs are not controlled,
their motion is caused by their influence on
the position of the center of mass.
4.3 Prioritization
We have applied the proposed algorithm seen in section 3.4.3 to
plan local motions for the humanoid
robot HRP-2[Kaneko et al. 2004]. We show in the following
examples the ability of our algorithm to
treat any order of priority with both equality and inequality
tasks.
The permanent set of constraints
Any task that we require from the robot is subject to a set of
constraints that we list here:
• Joint limits• Static equilibrium• Self-collision and obstacle
avoidance• Fixed position and orientation of feet
We have already presented the form of linear systems applied to
enforce joint limits ((4.3), (4.4)) static
equilibrium (4.19), collision avoidance (4.17) and position and
orientation of a body (4.5). For the
-
kinematic model of our humanoid robot, we defined one support
foot as the root node of the tree due
to its constant fixed contact with the environment (see section
4.1). Therefore, the fourth constraint in
the list is satisfied by constraining the non-support foot only.
As for collision avoidance constraints, for
the following scenarios, we did not resort to the simplified
geometry presented above, we kept the full
model of the robot. The computation of distances between
polyhedra was done with the motion planning
software Kineo[Laumond 2006].
Priority under inequality constraints
In this example, we illustrate the utility of prioritizing
equality tasks after specification of inequality
constraints. The goal of the motion is to reach a ball
underneath an object (blue polyhedron in figure 4.3)
while looking at it. Here is the stack of tasks sorted in
decreasing priority:
1. Permanent set of constraints
2. Reach for the ball
3. Look at the ball
For the reaching we specified a three-dimensional position task
on the center of the left hand. The gaze
task was defined as in section 4.2. For both tasks, we added a
simultaneous optimization criterion as
explained in section 3.4.4 to avoid large updates due to
conflicting tasks.
In the resulting motion, the gaze task could be maintained until
the robot’s head came close to the
border of the table. When simultaneous looking and reaching
became infeasible, the specified priorities
made the robot continue the reaching while its gaze direction
drifted off the target. Task 2) was satisfied
at the end of this motion (figure 4.15(b)).
We tried to achieve the same goal while making the looking and
the reaching tasks share the same
priority. This time the robot was trapped in an intermediate
configuration of the previous motion (frame
4.15(a)) where the looking task was maintained while the head
over the table and the reaching hand stuck
away from the ball.
One other method, presented by [Park et al. 1997] and applied to
move a robotic arm, should also
permit to solve this scenario. For the next one, there is no
available resolution method to our knowledge.
Inequalities below equality tasks
In this example we illustrate the ability of our prioritization
algorithm to account for inequality tasks
at low priority. The goal of the motion is to reach for a ball
while avoiding, when possible, a region
around the ball (represented in figure 4.3 by a green box). Here
is the priority order for this motion:
1. Permanent set of constraints
2. Reach for the ball
3. Look at the ball
4. Avoid the green box
-
(a)
(b)
Figure 4.15: Under collision avoidance constraints (priority 1),
the robot had to reach for the ball (priority
2) and look at it (priority 3). Figure (a) represents the
classical case where all equality tasks are solved
with the same equality under inequality constraints, figure (b)
shows the result for prioritized equality
tasks under inequality constraints allowed by our algorithm.
Towards the end of the motion, the gaze
task could not be maintained because of the hand pulling towards
the ball and the presence of the table.
The strict enforcement of priority enabled the reaching task to
prevail and succeed.
-
The idea behind placing the tall box on the ball is to guide the
hand out of the vision field to avoid the
occlusion of the ball. In a more rigorous but less simple
implementation of this scenario, one would
consider the vision cone linking the robot’s head to the ball.
For the illustration of the method, however,
the green box suffices.
For the box avoidance task, four points in the body of the hand
were checked for collision. The
reaching task placed at priority 2) is different from the first
example as it is one-dimensional only. This
is done by allowing the hand to move on the orthogonal plane to
the vector separating it from the target.
In the resulting motion, the hand was forced by task 4) to stay
behind a face of the green box until
it became incompatible with task 2). Then, the hand
progressively entered the volume of the box and
achieved its goal. As we expected, the inequality task 4) was
maintained as long as possible and ended
unsatisfied to the benefit of equality task 2).
-
(a)
(b)
Figure 4.16: This example illustrates how our algorithm could
serve for target occlusion avoidance.The
robot had to reach for the ball (priority 1), look at it
(priority 2) while avoiding the green region (priority
3). Figure (a) shows the classical case without inequality task,
figure (b) an inequality task is placed as
last task for the hand to avoid the green box. Towards the end
of the motion, reaching the task required
the green region to be crossed, rendering the inequality
unsatisfied. The higher priority task of reaching
the ball was achieved.
-
5Dynamic walk and whole body motion
The human motion of walking is a repeated cycle comprised of two
alternated phases:
• Single support phase: the body is supported by a single leg,
the other is displaced to its new fall
point.
• Double support phase: this is a shorter phase during which the
pressure of the body on the ground
is shifted from a support foot to the next.
The durations of these phases greatly depend on the subject
(skeletal structure, weight, clothi