An optimization formulation for footsteps planning Oussama Kanoun, Eiichi Yoshida and Jean-Paul Laumond

�bstract— We present a novel method to solve the problem of planning footsteps for a humanoid robot according to an arbitrary set of tasks. In this method� we consider the sequence of footsteps required to solve a task as a virtual kinematic chain that augments the state of the humanoid robot. We introduce this representation to formulate the footsteps planning as an iterative constrainted optimization problem where the footsteps are accounted for as additional degrees of freedom helping the robot in achieving its tasks. We demonstrate the efficiency and the generality of the method through three task scenarios for the humanoid robot HRP-2.

I. INTRODUCTION A� Statement of the problem

B� Related works

Consider the situation where a humanoid robot is given a target object to grasp. The first issue to solve is where to stand to be able to reach for the object, grasp it, then retrieve it. Regardless of the complexity of the environment, this problem is not an easy one to solve since the humanoid robots are complex articulated structures with many degrees of freedom and unique shapes. If we know that the object to grasp will always be lying on a flat surface at comfortable height from the ground then a simple strategy such as approaching to a certain distance and orientation of the object should be enough to solve the problem. Even if the reality of the field were close, the strategy and its parameters would greatly depend on the geometry of the robot and its accessible space for that particular task. Let us think of a second scenario where the object to pick up is lying on the ground between the feet of the robot. Which strategy is adapted to the grasping of the object? Should the robot move sideways or backwards before reaching and by how much? Could it remain at the same position and reach while avoiding arm collision with the legs? Designing multiple strategies fitting this particular situation can quickly become a difficult problem, let alone designing multiple strategies for the whole range of field tasks that we ask from a humanoid robot. Besides, such strategies are too sensitive to the geometry of the robot and of the environment.

In available works, the methods which are free of adhoc strategies are exclusively relying on probabilistic search algorithms. These powerful planning algorithms were applied for humanoid robots in [1] to build dynamically stable joint motion that may require a single foot displacement. They were also used in [2] to find a final stance that is adapted to the tasks by following a sequence of dynamically stable contacts for selected contact ports on the robot. The drawback of such methods is that simple and difficult patterns result from the same algorithm which can be considered inefficient. Some other search methods[16],[3], simplify the problem by planning a collision free planar path over which a separate algorithm places footsteps for the humanoid robot. The humanoid robot is viewed as a wheeled robot all along the path, which allows faster planning to the expense of solution coverage comparing to former algorithms. In simple environments or for short displacements, the above algorithms can be considered over sized. Faster, possibly less powerful methods may be preferred in these situations. To our knowledge, outside global search-based methods, all available footsteps planners infer the final positions of the feet from an a-priori knowledge on the task and the geometry of the robot . We aimed for a low cost method that does not rely on search algorithms nor on geometrical strategies yet delivers task-driven footsteps plans. C� Approach and contribution

This work has been conducted as a joint research in AIST/IS-CNRS/ST2I Joint Japanese-French Robotics Laboratory �JRL). Eiichi Yoshida is with JRL, Research Institute of Intelligent Systems, National Institute of Advanced Industrial Science and Technology �AIST), 1-1-1 Umezono, Tsukuba, Ibaraki 305-8568 Japan. �e.yoshida} @aist.go.jp Oussama Kanoun and Jean-Paul Laumond are with JRL, LAAS-CNRS, University of Toulouse, 7 avenue du Colonel Roche 31077 Toulouse, France. �okanoun� jpl} @laas.fr

Consider a humanoid robot that made a few footsteps to stand where it could fulfill a task. Our approach is based on the following observation: a footstep can be parametrized by a translation and a rotation of the target footprint from the support footprint. As such, two successive footprints can be considered as two virtual bodies joined by a virtual link with three degrees of motion freedom. Connecting the

footprints two by two, we obtain a virtual kinematic chain describing the walk path of the robot as a deformable articulated structure. By linking this chain to the robot at one of its feet, we establish a direct relationship between the tasks and the configuration of the walk path. We have a composite kinematic structure, on which certain constraints need to be observed and for which kinematic tasks are to be completed. All that remains is to apply one of the frameworks of numerical resolution reviewed in [4], [5] or [6]. The contribution of our work is the formulation of the footsteps planning problem as an inverse kinematics problem solved with local optimization techniques. Any task that can be solved with inverse kinematics may be considered an input and prioritized in a stack. We begin by recalling the differential kinematics context where local constrainted optimization is employed to solve the tasks for redundant structures �section II). The formulation of the problem of interest is detailed in section III. As for a robotic arm that takes the shapes that satisfies the desired position and orientation of its end effector, we expected the walk path derived by our method to be shaped directly according to the given tasks. We verified this property on three tasks scenarios �section IV). II. DIFFERENTIAL KINEMATICS BACKGROUND By differential kinematics[7] we refer to the iterative process of computing small configuration updates for an articulated figure to converge towards a state that achieves a given task. The tasks we consider here are differentiable vector functions of the joints configuration of the robot. We present the categories of the tasks that will be used to formulate the footstep planning. A� Equality tasks Let q be the joints configuration defining the posture of the robot, T a vector function of q and T �q) = 0 the equation defining a task on the robot. By computing the jacobian J = ∂T ∂ q �q), one can calculate successive joint velocities q˙ to make the task value converge towards T �q) = 0. These updates are solution of the following linear differential equation [8]: J q˙ = −λ T �q)

�1)

where λ is a positive real. The above equation represents a system of linear equations for which a solution might not always exist, depending on the rank of J. In general, an optimal solution may be defined with respect to an optimality criterion �see [6]). If J keeps a full rank, successive resolutions of equation �1) lead to a configuration q∗ satisfying T �q∗ ) = 0. With appropriate scaling of λ , the sucessive joint updates make a motion that can be applied to control the robot. B� Inequality tasks Inequality tasks on the configuration write as T �q) ≤ 0. The function T �q) can be for instance T �q) = Hz�q) − 0.5 where Hz�q) is the height of the robot hands from the ground. In this exmaple, the task Hz�q) − 0.5 ≤ 0 requires

A di

ds �n B Fig. 1.

the hands to be below the horizontal plane of equation z = 0.5. Inequality tasks may be solved in the same fashion as equality tasks through an iterative resolution of a linear differential inequality: J q˙ ≤ −λ T �q)

�2)

For the above example on the hand, equation �2) gives the following inequality: ∂ Hz q˙ ≤ −λ �Hz�q) − 0.5) ∂q 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. Such velocity bounding was introduced in [9] to maintain a minimal distance between convex moving objects. Let two points A and B represent the positions of such two objects. Define the influence distance di , the minimal distance ds , the unitary vector �n pointing from B to A �figure 1). Call d − → the distance �AB� and ξ the maximal absolute velocity of A relatively to B. The following linear inequality is a velocity damper between A and B: − → d − ds d�AB) |�n� ≤ ξ �3) � dt di − ds − → If activated when �AB� ≤ di , the above equation bounds the velocity of approach of the two objects from ξ when d = di down to 0 when d = ds . We will use velocity dampers to enforce a variety of tasks for our footsteps planner. C� Solving simultaneous tasks Because of the numerous degrees of freedom in a humanoid robot, simultaneous tasks may usually be solved at the same time. When the tasks are conflicting it may be necessary to assign priorities such that a critical task could prevail over a task of less importance. For instance, in the case of a humanoid robot standing in quasi-static motion, the position of the center of mass must project within the support polygon at all times. Solving equality tasks in a given priority order has been formulated by [7],[4] as a hierarchical optimization problem that can be summarized as: Find q˙∗ ∈ Sk such that: S0 = ℜn � � 1 2 Si = Arg min �Ji q˙ − ei � q∈S ˙ i−1 2

�4)

position and orientation of the feet on the ground collision avoidance We enforce the joint limits qmin ≤ q ≤ qmax by applying joint velocity dampers �inequality �3)): •

|q| ˙ max

•

qmini

qmin qmins

qmaxi qmaxs qmax

0 −|q| ˙ max

Fig. 2.

q˙ ≤ |q| ˙ max

Bounding joint velocities to enforce joint limits.

�7)

q − qmins qmini − qmins

�8)

−q˙ ≤ |q| ˙ max

Fig. 3. The velocity of approach of the center of mass projection to the edges of the support polygon is damped.

where Ji = ∂∂Tqi �q), Ti is the equality task at rank i, ei = −λ Ti �q), Si is the set of solutions for priority stage of rank i and k is the rank of the last priority stage. We have recently proposed a rigorous generalization[6] of this formulation to inequality tasks. For kinematic control, the optimization problem will be written as: Find q˙∗ ∈ Sk such that: S0 = ℜn � min w� q∈S ˙ i−1 Si = Arg s.t

q − qmaxs qmaxi − qmaxs

as implicitly defined by figure 2. 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 3). We damp the velocity of C near the polygon edges by observing the following inequality: − → � n� − ds �PC|� ∂ �PC|�n� − → ˙ q˙ ≤ |PC|max �9) − ∂q di − ds �˙ max is the maximum absolute velocity of C with where |PC| respect to an edge of normal �n and ds , di are the minimal and influence distances. Since the support polygon is fixed, the above inequality boils down to: − → ˙ max �PC|�n� − ds −�JC |�n�q˙ ≤ |C| di − ds

In this work, we restrict ourselves to the case where the robot is standing on a flat horizontal ground. Future works may see this condition relaxed. The footprints of the robot are modeled as simple rectangles, which may correspond to the actual shape or to a bounding shape of the robot footprint.

where JC is the jacobian of C with respect to the joints configuration. The constraints responsible for maintaining the feet on the ground are equality constraints that impose null linear and angular velocities to both feet. The collision avoidance constraints depend on the robot and we found different approaches to design them �[9], [10], [11]). We chose the simplest solution consisting in placing a few velocity dampers on end effectors and salient points of the robot[12] on which we tested our planner. Such a solution may lower the volume of accessible joint configurations, it offers, however, the advatanges of quicker implementation and faster calculations. With above constraints, we iterate on algorithm �6) until the tasks are solved or the configuration is trapped in a local minimum which is the limit of local optimization algorithms. The local minimum could be caused by the task being slightly out of the accessible space of the robot, hence the interest in making the robot step precisely such that the accessible task space extends towards the given task.

A� Unchanged stance

B� Single footstep

We start by considering a humanoid robot that must achieve a task without making steps. The constraints to be applied for every iteration of the algorithm �6) are linear systems that enforce: • joint limits • projection of the center of mass within the support polygon

Assuming we chose the stepping foot �we will propose further on an optimization of the choice of this parameter), we relax the feet constraints that we applied in the previous case. Instead of imposing null linear and angular velocities on the stepping foot, we authorize a bounded translation along the ground plane and a bounded yaw rotation around the normal �z. To simplify the constraint, each of these

1 2 2 �w�

Ji q˙ − ei ≤ w

�

�5)

We will denote a call to the algorithm that solves this optimization �described in [6]) by: q˙ = solve��� }� q)

�6)

where �� } is the set of prioritized linear systems �equality or inequality) derived from the tasks. In the following, we will call constraint a task of highest priority that must be satisfied at all times. III. FORMULATION OF FOOTSTEPS PLANNING

∆�

configuration q. ˜ To compute one configuration update for the structure we call:

x

q˙˜ = solve���˜ }� q) ˜

y

x

∆�x� y)

�10)

As one can see, this formulation rids the programmer of any consideration on the nature of task.

y

IV. APPLICATION Fig. 4.

Degrees of freedom for one footstep

�

�

A� Three planning examples We have selected three different scenarios to show the generality of our footstep planner. The feet’s relative position and orientation bounds were chosen as follows �left foot w.r.t right foot, the opposite case is taken symmetrical): −0.22m < ∆x < 0.22m

� Fig. 5.

Virtual kinematic chain of footsteps

degrees of freedom is bounded independently. However, we ensure that footprints authorized by this bounding are quasistatically feasible by the robot. This condition can be easily achieved by shrinking the bounding area below the capacities of the robot. Let us summarize the constraints used in this case: • Joint limits • Collision avoidance • Center of mass inside support polygon • Feet sliding on the ground • Relative feet placement This time the support polygon is a deformable polygon since one of the feet may change its planar position and orientation. The inequality constraint applied for this purpose is �9). The relative feet position and orientation are bounded by applying velocity dampers identically to �7) and �8), this time on ∆x, ∆y and ∆�yaw coordinates of the stepping foot relatively to the support foot �see figure 4). C� Several footsteps Now let us consider the general case where the robot makes several steps to fulfill its tasks. We view two successive footsteps as virtual bodies joined by a virtual link with three degrees of motion freedom �figure 5). We define the configuration �∆x� ∆y� ∆� ) of one footstep by its translations ∆x and ∆y and its rotation ∆� from the support foot. We define the augmented robot state q˜ = �qsteps � qrobot ) where qsteps is the concatenation of successive footstep configurations and qrobot the joints configuration of the robot. The constraints for this new kinematic structure are those we apply on the robot making a single step �previous case) to which we add bounds and collision avoidance constraints for each additional support polygon link. Let ��˜ } be the set of linear differential systems derived from the constraints and prioritized tasks and evaluated at the current composite

0.07m < ∆y < 0.25m π −0.1rad < ∆�yaw < rad 4 These bounds are small enough to guarantee quasi-statical stepping for the robot HRP-2. It is not necessary to estimate the maximal bounds unless one wants to minimize the number of steps to achieve a given task. Scenario 1: the robot stands 2m from the target ball. A simple obstacle is modeled with a disc region and the feet are constrained to avoid the corresponding area placed on the ground. For this we use velocity dampers between the center of the obstacle and its projections on each footprint. Figure 6 shows the state of the kinematic structure at several iterations of algorithm �10) converging to a solution. Initially, the virtual chain is folded down and all support polygons coincide with the start polygon. Iterating �10) makes the chain unfold continuously until the robot has either satisfied its tasks �1:constraints, 2:reach for ball, 3:look at the ball) or the structure has been trapped in a local minimum. The second case may occur when the chosen number of steps is insufficient �see next subsection). For this test scenario, nine steps were needed to accomplish the task. This resulted in a total dimension of 55 for the state parameters. The computation time was 3.2 seconds on a 2.13GHz Intel�R) Core�TM) 2 CPU. A video showing an example of actual motion planned over the obtained footsteps can be found at http://www�laas�fr/˜jpl/humanoids2009�mp4. The dynamic walk motion was generated following [13]. Scenario 2: the robot has to pick up an object that lies on the ground between its feet �1:constraints, 2:reach for object, 3:look at object). To avoid generating a footprint on the object, a virtual obstacle is placed around it and collision avoidance constraints are added between the footprints and the obstacle. Figure 7 shows a progression to the solution for this scenario which took 0.9s to solve. Scenario 3: the robot has performed a motion to look at the pink ball without making steps. It ended in an awkward posture which makes it harder to further track the ball. The robot may recover a comfortable posture by making a few steps. This scenario was considered in [14] with a heuristical method to derive the position of the required footsteps. We used our method to obtain the footsteps without

Fig. 6. States of the full kinematic structure at different steps of the optimization. The task is to grasp the pink ball without stepping on the green region �obstacle). The last view shows the solution footprints retained for the actual robot locomotion.

custom algorithms: we defined a comfortable posture qrest and specified the a desired posture recovery task written as qrobot − qrest = 0. This single task was applied under the constraint of keeping looking at the ball. We expressed this constraint as: −→ −→ OG × OB = 0 −→ where O is a point on the optical axis, OG is a vector lying on the optical axis and B the position vector of the ball. Four steps were needed to achieve a posture close to the intial configuration �see figure 8). The problem was solved in 0.3s. The number of steps and the start footstep were given for each of these tests. We propose hereby two algorithmic extensions to automatically select these important parameters. B� Determining the number of steps Recall that we fixed the number of steps in our formulation of the optimization problem. At a given iteration of the problem, a number of footsteps has to be chosen, yet this does not mean all iterations should have the same number. It is easy to monitor the progress of the tasks after each optimization iteration since the values �T �q)� should be strictly decreasing for the unfinished tasks with the highest priority level. Thus, if the convergence rate slows down below a certain threshold, we decide to augment the state with an extra support polygon. If we do not notice any improvement on the convergence we may conclude that we have reached a local mminimum, which is the limit of all local optimization techniques.

C� Optimizing the starting footstep Recall that we assumed an arbitrary choice of the start footstep for the formulation of our problem. To get rid of this last a-priori, we can make calculations for both available choices, left and right. If the algorithm converges to a solution for both, we may pick the choice that gives the least number of required steps. To avoid an arbitrary decision in case the number of steps is identical for both choices, we suggest to select the start foot based on the result of an extra task that minimizes the deformation of the solution posture with respect to a reference rest posture. This task, seen above as qrobot − qrest = 0, is systematically placed with lowest priority in the stack of tasks. We keep the solution producing the smallest residual task value �qrobot − qrest �. More specialized criteria may naturally be used instead of this criterion intended as default. V. CONCLUSION We presented a novel method to solve the problem of planning footsteps for a humanoid robot according to an arbitrary set of tasks. In this method, we introduced an original representation of the walk sequence of a humaoid robot as a virtual kinematic chain to augment the robot state. We used this representation to formulate the footstep planning problem as an iterated constrainted optimization to directly modify the configuration of the footsteps according to the tasks. We demonstrated the efficiency and the generality of the method for three task scenarios on the humanoid robot HRP-2. As expected, the computation times grew rapidly as the footstep planning problems required higher number of parameters. Our planner may not be suitable for tasks requiring

Fig. 7. Planning footsteps to pick up an object on the floor. A virtual obstacle �green disc) is added around the object to avoid stepping on it.

a long locomotion. For such tasks, one would prefer an algorithm that plans a walk path to a remote goal position and orientation [15], [16]. Nonetheless, our method may still be used as a finetuning algorithm that takes over the end of the planned walk path and reshapes the last few steps precisely according to the tasks. VI. ACKNOWLEDGEMENTS This work has been partly supported by Grant-in-Aid for Scientific Research �B) 21300078 and JST-CNRS Strategic Japanese-French Cooperative Program ”Robot motion planning and execution through online information structuring in real-world environment”. This work has also been supported by the ANR Project Locanthrope and by the FUI Project Romeo. R EFERENCES [1] Kuffner, J.J. Kagami, S. Nishiwaki, K. Inaba, M. and Inoue, H. “Dynamically-stable motion planning for humanoid robots” Autonomous Robots 12, No. 1, pp 105-118, �2002) [2] Escande, A. Kheddar, A. Miossec, S. and Garsault, S. “Planning Support Contact-Points for Acyclic Motions and Experiments on HRP2”, Nonlinear Kalman filtering for force-controlled robot tasks, �2005) [3] Diankov, R. and Ratliff, N. and Ferguson, D. and Srinivasa, S. and Kuffner, J.“Bispace planning: Concurrent multi-space exploration” in Proceedings of Robotics: Science and Systems IV �2008) [4] B. Siciliano, J.-J.E. Slotine: “A general framework for managing multiple tasks in highly redundant robotic systems,” Proc. IEEE ICRA pp. 12111216 �1991) [5] Khatib, O. and Sentis, L. and Park, J. and Warren, J.: “Whole body dynamic behavior and control of human-like robots” International Journal of Humanoid Robotics, Vol 1, pp 29-43 �2004) [6] Kanoun, O. Lamiraux, F. Wieber, P.B. Kanehiro, F. Yoshida, E. Laumond, J.P. “Prioritizing linear equality and inequality systems: application to local motion planning for redundant robots” IEEE International Conference on Robotics and Automation, ICRA �2009) [7] Y. Nakamura, “Advanced Robotics: Redundancy and Optimization,” Addison-Wesley Longman Publishing, Boston �1991)

Fig. 8. Planning footsteps to recover a comfortable posture in the middle of another task �here to look at the ball).

[8] A. Li´egeois, “Automatic supervisory control of the configuration and behavior of multibody mechanisms,”IEEE Trans. Sys., Man, Cybern. 7, No. 12, 868-871 �1977). [9] Faverjon, B. and Tournassoud, P. “A local based approach for path planning of manipulators with a high number of degrees of freedom” IEEE International Conference on Robotics and Automation. Proceedings, 4, �1987) [10] F. Kanehiro, F. Lamiraux, O. Kanoun, E. Yoshida and J.-P. Laumond :“A Local Collision Avoidance Method for Non-strictly Convex Polyhedra” Conf. Robotics Science and Systems �2008) [11] Stasse, O. Escande, A. Mansard, N. Miossec, S. Evrard, P. and Kheddar, A. “Real-time �self)-collision avoidance task on a hrp-2 humanoid robot” IEEE International Conference on Robotics and Automation, pp 3200–3205, �2008) [12] K. Kaneko, F. Kanehiro, S. Kajita, H. Hirukawa, T. Kawasaki, M. Hirata, K. Akachi and T. Isozumi, “Humanoid Robot HRP-2” in Proc. of IEEE ICRA, pp. 10831090 �2004) [13] Kajita, S. and Kanehiro, F. and Kaneko, K. and Fujiwara, K. and Harada, K. and Yokoi, K. and Hirukawa, H. “Biped walking pattern generation by using preview control of zero-moment point”IEEE International Conference on Robotics and Automation, Vol 2, pp 1620–1626, �2003) [14] Sreenivasa, M., Soueres, P., Laumond, J.-P., and Berthoz, A. 2009. “Steering a humanoid robot by its head.” in Proc. of IEEE Int. Conf. on Intelligent Robots and Systems �2009) [15] Kuffner, J. and Kagami, S. and Nishiwaki, K. and Inaba, M. and Inoue, H. “Online footstep planning for humanoid robots” IEEE International Conference on Robotics and Automation, Vol 1, pp 932–937, �2003) [16] Yoshida, E. and Poirier, M. and Laumond, J.P. and Kanoun, O. and Lamiraux, F. and Alami, R. and Yokoi, K. “Whole-body motion planning for pivoting based manipulation by humanoids” IEEE International Conference on Robotics and Automation, pp 3181-3186, �2008)