Carlos Mastalli

Ioannis Havoutis

Alexander W Winkler

Darwin G Caldwell

Claudio Semini

On-line and On-board Planning and Perception for Quadrupedal Locomotion

come   L'archive ouverte pluridisciplinaire

I. INTRODUCTION

Legged motion planning over rough terrain involves making careful decisions about the footstep sequence, body movements, and locomotion behaviors. Moreover, it should consider whole-body dynamics, locomotion stability, kinematic and dynamic capabilities of the robot, mechanical properties and irregularities of the terrain. Frequently, locomotion over rough terrain is decomposed into: (a) perception and planning to reason about terrain conditions, by computing a plan that allows the legged system to traverse the terrain toward a goal, and (b) control that executes the plan while compensating for uncertainties in perception, modelling errors, etc. In this work, we focus on generating on-line and versatile plans for quadruped locomotion over challenging terrain.

In legged motion planning one can compute simultaneously contacts and body movements, leading to a coupled motion planning approach [START_REF] Tassa | Stochastic Complementarity for Local Control of Discontinuous Dynamics[END_REF][2] [3][4]. This can be posed as a hybrid system or a mode-invariant problem. Such approaches tend to compute richer motion plans than decoupled motion planners, especially when employing modeinvariant strategies. Nevertheless, these approaches are often hard to use in a practical setting. They are usually posed as non-linear optimization problems such as Mathematical Programming with Complementary Constraints [START_REF] Posa | A direct method for trajectory optimization of rigid bodies through contact[END_REF], and are computationally expensive. On the other hand, the legged motion planning problem can be posed as a decoupled approach that is naturally divided into: motion and contact planning [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF][6] [START_REF] Kalakrishnan | Learning locomotion over rough terrain using terrain templates[END_REF]. These approaches avoid the combinatorial search space at the expense of complexity of locomotion. A decoupled motion planner has to explore different plans in email:

{carlos.mastalli, ioannis.havoutis, darwin.caldwell, claudio.semini}@iit.it, alexander.winkler@mavt.ethz.ch Fig. 1. The hydraulically actuated and fully torque-controlled quadruped robot HyQ [START_REF] Semini | Design of HyQ -a hydraulically and electrically actuated quadruped robot[END_REF]. The inset plot shows the on-line environment perception alongside with the planned footholds and the current on-board robot state estimate.

the space of feasible movements (state space), which is often defined by physical, stability, dynamic and task constraints. Nevertheless, the feasibility space is variable since the stability constraints depend on the kind of movement, e.g. static or dynamic walking.

The challenge of decoupled planners lies primarily in reducing the computation time while increasing the complexity of motion generation. To the best of our knowledge, up to now, decoupled approaches are limited in the versatility of movements and computation time, for instance [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF] reduces the computation time but is still limited to small changes of the robot's yaw (heading). Therefore, our main contribution is a planning approach that increases the versatility of plans, based on the definition of footstep search regions and footstep sequence according to a body action plan. Our method computes on-line and on-board plans (around 1 Hz) using the incoming perception information on commodity hardware. We evaluate our planning approach on the Hydraulic Quadruped robot -HyQ- [START_REF] Semini | Design of HyQ -a hydraulically and electrically actuated quadruped robot[END_REF] shown in Fig. 1.

The rest of the paper is structured as follows: after discussing previous research in the field of legged motion planning (Section II). Section III explains, how on-line and on-board versatile plans are generated based on a decoupled planning approach (body action and footstep sequence planners). In Section IV we evaluate the performance of our planning approach in real-world trials before Section V summarizes this work and presents ideas for future work.

II. RELATED WORK

Motion planning is an important problem for successful legged locomotion in challenging environments. Legged systems can utilize a variety of dynamic gaits (e.g. trotting and galloping) in environments with smooth and continuous support such as flats, fields, roads, etc. Such cyclic gaits often assume that contact will be available within the reachable workspace at every step. However, for more complex environments, reactive cyclic gait generation approaches quickly reach their capabilities as foot placement becomes crucial for the success of the behavior.

Natural locomotion over rough terrain requires simultaneous computation of footstep sequences, body movements and locomotion behaviors (coupled planning) [START_REF] Tassa | Stochastic Complementarity for Local Control of Discontinuous Dynamics[END_REF][2][3] [START_REF] Dai | Whole-body motion planning with simple dynamics and full kinematics[END_REF]. One of the main problems with such approaches is that the search space quickly grows and searching becomes unfeasible, especially for systems that require on-line solutions. In contrast, we can state the planning and control problem into a set of sub-problems, following a decoupled planning strategy. For example the body path and the footstep planners can be separated, thus reducing the search space for each component [5][6][7]. This can reduce the computation time at the expense of limiting the planning capabilities, sometimes required for extreme rough terrain. There are two main approaches of decoupled planning: contact-beforemotion [START_REF] Escande | Planning support contactpoints for humanoid robots and experiments on HRP-2[END_REF][10] [START_REF] Vernaza | Search-based planning for a legged robot over rough terrain[END_REF] and motion-before-contact [START_REF] Zucker | An Optimization Approach to Rough Terrain Locomotion[END_REF] [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF]. These approaches find a solution in motion space, which defines the possible motion of the robot.

The motion space of contact-before-motion planner lies in a sub-manifold of the configuration space1 with certain motion constraints, thus, a footstep constraint switches the motion space to a lower-dimensional sub-manifold Q σ 2 [START_REF] Hauser | Randomized multi-modal motion planning for a humanoid robot manipulation task[END_REF] [START_REF] Escande | Planning contact points for humanoid robots[END_REF]. Therefore, these planners have to find a path that connects the initial F σ and goal F σ feasible regions. Note that the planner must find transitions between feasible regions and then compute paths between all the possible stances (feasible regions) that connect F σ and F σ , which is often computationally expensive. On the other hand, motionbefore-contact approaches allow us to shrink the search space using an intrinsic hierarchical decomposition of the problem into; high-level planning, body-path planning and footstep planning, and low-level planning, CoG trajectory and foot trajectory generation. These approaches reduce considerably the search space but can also limit the complexity of possible movements, nevertheless [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF], [START_REF] Kalakrishnan | Learning, planning, and control for quadruped locomotion over challenging terrain[END_REF] and [START_REF] Winkler | Path planning with force-based foothold adaptation and virtual model control for torque controlled quadruped robots[END_REF] have demonstrated that this can be a successful approach for rough terrain locomotion. Nonetheless these approaches were tailored for specific types of trials where goal states are mostly placed in front of the robot. This way most of these frameworks developed planners that use a fixed yaw and only allow forward expansion of the planned paths. Our approach takes a step forward in versatility by allowing motion in all directions and changes in the robot's yaw (heading) as in real environments this is almost always required.

The planning approach described in this paper decouples the problem into: body action and footstep sequence planning (motion-before-contact). The decoupling strategy allows us to reduce the computation time, ensures sub-optimal plans, and computes plans on-line using the incoming information of the terrain. Our body action planner uses a lattice representation based on body movement primitives. Compared to previous approaches our planning approach generates online more versatile movements (i.e. forward and backward, diagonal, lateral and yaw-changing body movements) while also ensure feasibility for every body action. Both planning and perception are computed on-line and on-board.

III. TECHNICAL APPROACH

Our overall task is to plan on-line an appropriate sequence of footholds F that allows a quadruped robot to traverse a challenging terrain toward a body goal state (x, y, θ). To accomplish this, we decouple the planning problem into: body action and footstep sequence planning. The body action planner searches a bounded sub-optimal solution around a growing body-state graph. Then, the footstep sequence planner selects a foothold sequence around an action-specific foothold region of each planned body action.

Clearly, decoupled approaches reduce the combinatorial search space at the expense of the complexity of locomotion. This limits the versatility of the body movement (e.g. discretized yaw-changing movements instead of continuous changes). Nevertheless, our approach manages this trade-off by employing a lattice representation of potentially feasible body movements and action-specific foothold regions.

A. Terrain Reward Map

The terrain reward map quantifies how desirable it is to place a foot at a specific location. The reward value for each cell in the map is computed using geometric terrain features as in [START_REF] Winkler | Path planning with force-based foothold adaptation and virtual model control for torque controlled quadruped robots[END_REF]. Namely we use the standard deviation of height values, the slope and the curvature as computed through regression in a 6cm×6cm window around the cell in question; the feature are computed from a voxel model (2 cm resolution) of the terrain. We incrementally compute the reward map based on the aforementioned features and recompute locally whenever a change in the map is detected. For this we define an area of interest around the robot of 2.5m×5.5m that uses a cell grid resolution of 4 cm. The cost value for each cell of the map is computed as a weighted linear combination of the individual feature R(x, y) = w T r(x, y), where w and r(x, y) are the the weights and features values, respectively. Figure 2 shows the generation of the reward map from the mounted RGBD sensor. The reward values are represented using a color scale, where blue is the maximum reward and red is the minimum one.

B. Body Action Planning

A body action plan over challenging terrain take into consideration the wide range of difficulties of the terrain, Fig. 2. The reward map generation from RGBD (Asus Xtion) camera data. The RBGD sensor is mounted on a PTU unit that scans the terrain. An occupancy map is built with a 2 cm resolution. Then the set of features is computed and the total reward value per pixel of 4 cm is calculated. In addition, a height map of 2 cm resolution in z is created. The reward values are represented using a color scale, where blue is the maximum reward and red is the minimum one.

obstacles, type of action, potential shin collisions, potential body orientations and kinematic reachability. Thus, given a reward map of the terrain, the body action planner computes a sequence of body actions that maximize the cross-ability of the sub-space of candidate footsteps. The body action plans are computed by searching over a body-state graph that is built using a set of predefined body movements primitives. The body action planning is described in Algorithm 1.

1) Graph construction: We construct the body-state graph using a lattice-based adjacency model. Our lattice representation is a discretization of the configuration space into a set of feasible body movements, in which the feasibility depends on the selected sequence of footholds around the movement. The body-state graph represents the transition between different body-states (nodes) and it is defined as a tuple, G = (S, E), where each node s ∈ S represents a body-state and each edge e ∈ E ⊆ S × S defines a potential feasible transition from s to s . A sequence of body-states (or body poses (x, y, θ)) approximates the body trajectory that the controller will execute. An edge defines a feasible transition (body action) according to a set of body movement primitives. The body movement primitives are defined as body displacements (or body actions), which ensure feasibility together with a feasible footstep region. A feasible footstep region is defined according to the body action.

Given a body-state query, a set of successor states are computed using a set of predefined body movement primitives. A predefined body movement primitive connects the current body state s = (x, y, θ) with the successor body state s = (x , y , θ ). The graph G is dynamically constructed due to the associated cost of transition c body (s, s ) depends on the current and next states (or current state and action). In fact, the feasible regions of foothold change according to each body action, which affect the value of the transition cost. Moreover, an entire graph construction could require a greater memory pre-allocation and computation time than available (on-board computation). Figure 3 The resulting states s are checked for body collision with obstacles, using a predefined area of the robot, and invalid states are discarded. For legged locomotion over challenging terrain, obstacles are defined as unfeasible regions to cross, e.g. a wall or tree. In our case, the obstacles are detected when the height deviation w.r.t. the estimated plane of the ground is larger than the kinematic feasibility of the system in question (HyQ). We build the obstacle map with 8 cm resolution, otherwise it would be computationally expensive due to the fact that it would increase the number of collision checks. In fact, the body collision checker evaluates if there are body cells in the resulting state that collide with an obstacle.

2) Body cost: The body cost describes how desirable it is to cross the terrain with a certain body path (or body actions). This cost function is designed to maximize the cross-ability of the terrain while minimizing the path length. In fact, the body cost function c body is a linear combination of: terrain cost c t , action cost c a , potential shin collision cost c pc , and potential body orientation cost c po . The cost of traversing any transition between the body state s and s is defined as follows:

c body (s, s ) = w b t c t (s) + w a c a (s, s ) +w pc c pc (s) + w po c po (s) (1)
where w i are the respective weight of the different costs. For a given current body state (x, y, θ), the terrain cost c t is evaluated by averaging the best n-footsteps terrain reward values around the foothold regions of a nominal stance (nominal foothold positions). The action cost c a is defined by the user according to the desirable actions of the robot, e.g. it is preferable to make diagonal body movements than lateral ones. The potential leg collision cost c pc is computed by searching potential obstacles in the predefined workspace region of the foothold, e.g. near to the shin of the robot. In fact, a potential shin collision is detected around a predefined shin collision region, which depends on the configuration of each leg as shown in Figure 4. This cost is proportional to the height defined around the footstep plane, where red bars represent collision elements. Finally, the potential body orientation c po is estimated by fitting a plane in the possible footsteps around the nominal stance for each leg.

3) Reducing the search space: Decoupling the planning problem into body action and footstep planning avoids the combinatorial search space. This allows us to compute plans on-line, and moreover, to develop a closed-loop planning approach that can deal with changes in the environment. Nevertheless the reduced motion space might span infeasible regions due to the strong coupling between the body action and footstep plans. A strong coupling makes that a feasible movement depends on the whole plan (body action and footstep sequence). In contrast to previous approaches [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF][6][14] [START_REF] Zucker | An Optimization Approach to Rough Terrain Locomotion[END_REF][15], our planner uses a lattice-based representation of the configuration space. The lattice representation uses a set of predefined body movement primitives that allows us to apply a set of rules that ensure feasibility in the generation of on-line plans.

The body movement primitives are defined as 3D motionactions (x, y, θ) of the body that discretize the search space. We implement a set of different movements (body movement primitives) such as: forward and backward, diagonal, lateral and yaw-changing movements.

4) Ensuring feasibility: Our lattice representation allows the robot to search a body action plan according to a set of predefined body movement primitives. A predefined body movement primitive cannot guarantee a feasible plan due to the mutual dependency of movements and footsteps. For instance, the feasibility of clockwise movements depends on the selected sequence of footsteps, and can only be guaranteed in a specific footstep region. In fact, we exploit this characteristic in order to ensure feasibility in each body movement primitive. These footstep regions increase the support triangle areas given a certain body action, improving the execution of the whole-body plan. This strategy also guarantees that the body trajectory generator can connect the support triangles (support triangle switching) [START_REF] Kalakrishnan | Learning, planning, and control for quadruped locomotion over challenging terrain[END_REF]. Figure 5 illustrates the footstep regions according to the type of body movement primitives. These footstep regions are tuned to increase the triangular support areas, improving the execution of the whole-body plan.

5) Heuristic function: The heuristic function guides the search to promising solutions, improving the efficiency of the search. Heuristic-based search algorithms require that the heuristic function is admissibile and consistent. Most of the heuristic functions estimate the cost-to-go by computing the Euclidean distance to the goal. However, these kind of heuristic functions do not consider the terrain conditions. Therefore, we implemented a terrain-aware heuristic function that considers the terrain conditions.

The terrain-aware heuristic function computes the estimated cost-to-go by averaging the terrain cost locally and h Fig. 4. Computation of the potential shin collision. The potential shin collisions are detected a predefined shin collision region (red bars). The computed cost is proportional to the height defined (red lines) around the footstep plane (dashed black lines), where blue height differences do contribute to the cost.

estimating the number of footsteps to the goal:

h(s) = -RF( g -s ) (2)
where R is the average reward (or cost) and F( gs ) is the function that estimates the number of footsteps to the goal. 6) Ensuring on-line planning: Open-loop planning approaches fail to deal adequately with changes in the environment. In real scenarios, the robot has a limited range of perception that makes open-loop planning approaches unreliable. Closed-loop planning considers the changes in the terrain conditions, and uses predictive terrain conditions for non-perceived regions, improving the robustness of the plan. Dealing with re-planning and updating of the environment information requires that the information is managed in an efficient search exploration of the state space. We manage to reduce the computation time of building a reward map by computing the reward values from a voxelized map of the environment. Additionally, we (re-)plan and update the information using ARA * [START_REF] Likhachev | ARA*: Anytime A* with Provable Bounds on Sub-Optimality[END_REF]. ARA * ensures provable bounds on sub-optimality, depending on the definition of the heuristic function. On the other hand, the terrain-aware heuristic function guides the search according to terrain conditions.

C. Footstep Sequence Planning

Given the desirable body action plan, Q = (x d , y d , θ d ), the footstep sequence planner computes the sequence of footholds that reflects the intention of the body action. A local greedy search procedure the optimal footstep target. A (locally optimal) footstep is selected from defined footstep search regions by the body action planner. For every body action, the footstep planner finds a sequence of the 4next footsteps. The footstep sequence planning is described in Algorithm 2.

1) Footstep cost: The footstep cost describes how desirable is a foothold target, given a body state s. The purpose set horizon F for q i=0:F ∈ Q do for i=0:3 do compute the sequence of footstep e given q i generate possible FOOTSTEP(e, q i ) compute the greedy solution f min add f min to F end for end for return F end function of this cost function is to maximize the locomotion stability given a candidate set of footsteps. The footstep cost c f ootstep is a linear combination of: terrain cost c t , support triangle cost c st , shin collision cost c c and body orientation cost c o . The cost of certain footstep f e is defined as follows:

c f ootstep (f e ) = w f t c t (f e ) + w st c st (f e ) +w c c c (f e ) + w o c o (f e ) (3)
where f e defines the Cartesian position of the foothold target e (foot index). We consider as end-effectors: the LF, RF, LH and RH feet of HyQ. The terrain cost c t is computed from the terrain reward value of the candidate foothold, i.e. using the terrain reward map (see Figure 2). The support triangle cost c st depends on the inradii of the triangle formed by the current footholds and the candidate one. As in the body cost computation, we use the same predefined collision region around the candidate foothold. Finally, the body orientation cost c o is computed using the plane formed by the current footsteps and the candidate one. We calculate the orientation of the robot from this plane.

IV. EXPERIMENTAL RESULTS

The following section describes the experiments conducted to validate the effectiveness and quantify the performance of our planning approach.

A. Experimental Setup

We implemented and tested our approach with HyQ, a Hydraulically actuated Quadruped robot developed by the Dynamic Legged Systems (DLS) Lab [START_REF] Semini | Design of HyQ -a hydraulically and electrically actuated quadruped robot[END_REF]. HyQ roughly has the dimensions of a medium-sized dog, weight 90 kg, is fully-torque controlled and is equipped with precision joint encoders, a depth camera (Asus Xtion) and an Inertial Measurement Unit (IMU). We used an external state measuring system, Vicon marker-based tracker system. The perception and planning is done on-board in an i7/2.8GHz PC, and all computations of the real-time controller are done using a PC104 stack. The first set of experiments are designed to test the capabilities of our motion planner. We use a set of benchmarks of realistic locomotion scenarios (see Figure 6): stepping stones, pallet, stair and gap. In these experiments, we compared the cost, number of expansions and computation time of ARA * against A * [16] using our lattice representation. The results are based on 9 predefined goal locations. Second experiment, the robot must plan on-line with dynamic changes in the terrain. Third experiment, the next experiment we show the on-line planning and perception results. Finally, we validate the performance of our planning approach by executing in HyQ.

B. Results and Discussions 1) Initial plan results:

The stepping stones, pallet, stair and gap experiments (see Figure 6) show the initial plan quality (see Table I) of our approach using A * and ARA * . To this end, we plan a set of body actions and footstep sequences for 9 predefined goal locations, approximately 2.0 m away from the starting position, and compare the cost and number of expansions of the body action path, and the planning time of ARA * against A * . Three main factors contribute to the decreased planning time while maintaining the quality of the plan:

First, the lattice-based representation (using body movement primitives) considers versatile movements in the sense that it allows us to reduce the search space around feasible regions (feasible motions) according a certain body action, in contrast to grid-based approaches that ensure the feasibility by applying rules that are agnostic to body actions. Second, our terrain-aware heuristic function guides the tree expansion according to terrain conditions in contrast to a simple Euclidean heuristic. Finally, the ARA* algorithm implements a search procedure that guarantees bounded sub-optimality in the solution given a proper heuristic function [START_REF] Likhachev | ARA*: Anytime A* with Provable Bounds on Sub-Optimality[END_REF]. Figure 7 shows the initial plan of A* and ARA*.

2) On-line planning and perception: Using a movement primitive-based lattice search reduces the size of the search space significantly, leading to responsive planning and re-planning. In our experimental trials, we choose a lattice graph resolution (discretization) of 4 cm for x/y and 1.8 • for θ and goal state is never more that 5 m away from the robot. In these trials, the plan/replan frequency is approximately 0.5 Hz.

The efficient occupancy grid-based mapping allows us to incrementally build up the model of the environment and focus our computations to the area of interest around the robot body, generating plans quickly. This allows us to locally update the computed reward map and incrementally build the reward map as the robot moves with an average response frequency of 2 Hz as is shown in Figure 8.

3) Planning and execution: We generate swift and natural dynamic whole-body motions from an n-step lookahead optimization of the body trajectory that uses a dynamic stability metric, the Zero Moment Point (ZMP). A combination of floating-base inverse dynamics and virtual model control accurately executes such dynamic whole-body motions with an actively compliant system [START_REF] Winkler | Planning and Execution of Dynamic Whole-Body Locomotion for a Hydraulic Quadruped on Challenging Terrain[END_REF]. Figure 9 shows the execution of an initial plan. Our locomotion system is robust due to a combination of on-line planning and compliance control.

V. CONCLUSION We presented a planning approach that allows us to plan on-line, versatile movements. Our approach plans a sequence of footsteps given a body action plan. The body action planner guarantees a bounded sub-optimal plan given a set of predefined movement primitives (lattice representation). In general, to generate versatile movement one has to explore a considerable region of the motion space, making on-line planning a computationally challenging task, especially in practical applications. Here, we reduce the exploration by ensuring the feasibility of every possible body action. In fact, we define the footstep search regions and footstep sequence based on the body action. We showed how our planning approach computes on-line plans given incoming terrain information. Various real-world experimental trials demonstrate the capability of our planning approach to traverse challenging terrain.

  shows the graph construction for the body action planner. The associated cost Algorithm 1 Computes a set of body action over a challenging terrain using an ARA * search. Data: Inflation parameter , time of computation t c Result: a body action plan Q = [q 0 , q 1 , • • • , q l ] function COMPUTEBODYACTIONPLAN(s start , s goal ) set and computation time t c g(s start ) = 0; g(s goal ) = ∞ OPEN = {s start }; INCONS = CLOSED= ∅ while ( ≥ 1 and t c < t) do decrease OPEN = OPEN ∪ INCONS; CLOSED = ∅ // fval(s) = g(s) + h(s) while (fval(s goal ) > minimum fval in OPEN) do remove s with minimum fval from OPEN insert s into CLOSED generate action(s) from body primitives for all u ∈ action(s) do compute s given u compute the footstep regions given s compute c body (s, s ) from footstep regions if g(s ) > g(s) + c body (s, s ) then g(s ) = g(s) + c body (s, s ) add s → s to policy s = π(s) if s is not in CLOSED then insert (s ,fval(s )) into OPEN else insert (s , fval(s )) into INCONS end if end if end for end while end while reconstruct body action plan Q from π(•) return Q end function of every transition c body (s, s ) is computed using the footstep regions. These footstep regions depend on the body action in such a way that they ensure feasibility of the plan, as is explained in III-B.4 subsection. For every expansion, the footstep regions of Left-Front LF (brown squares), Right-Front RF (yellow squares), Left-Hind LH (green squares) and Right-Hind RH (blue squares) legs are computed given a body action.

Fig. 3 .

 3 Fig. 3. Illustration of graph construction of the least-cost path. The associated cost of every transition c body (s, s ) is computed using the footstep regions. For every expansion, the footstep regions of LF (brown squares), RF (yellow squares), LH (green squares) and RH (blue squares) are computed according to a certain body action.

Fig. 5 .

 5 Fig. 5. Different footstep search regions according to the body action. These footstep regions ensure the feasibility of the plan. Moreover, it increase the triangular support areas, improving the execution of the whole-body plan. The brown, yellow, green and blue squares represent the footstep search regions for LF, RF, LH and RH feet, respectively.

Algorithm 2

 2 Computes a footstep sequence given a body action plan. Data: footstep horizon F Result: a footstep sequence F function COMPUTEFOOTSTEPSEQUENCE(Q)

Fig. 6 .

 6 Fig. 6. The planning benchmarks used to analyse the quality of the produced plans. Top left: stepping stones; top right: pallet; bottom left: stair; bottom right: gap.

Fig. 7 .

 7 Fig. 7. Comparison of the body action (green line) and footstep sequence computed plan A * and ARA* given reward map (grey scale). The brown, yellow, green and blue points represent the planned footsteps of the LF, RF, LH and RH feet, respectively. The top image shows the computed plan by A * , and the bottom one the results of ARA * .

Fig. 9 .

 9 Fig.9. Snapshots of pallet trial used to evaluate the performance of our planning approach. From top to bottom: planning and terrain reward map; execution of the plan with HyQ.

Fig. 8 .

 8 Fig. 8. (Re-)planning and perception on-board. The left image presents the plan for a flat terrain, then, the right image reflects the re-planning and updating of the reward map (grey scale map). The green, brown, blue and yellow points represent the planned footholds of the LH, LF, RH, RF legs, respectively. The green line represents the body path according to action plan.

TABLE I COST

 I OF THE PLAN (COST), NUMBER OF EXPANSIONS (EXP.) AND COMPUTATION TIME (TIME) AVERAGED OVER 9 TRIALS OF A * AND ARA * .

			A*			ARA*
	Terrain	Cost	Exp. Time [s]	Cost Exp. Time [s]
	S. Stones 364.7 3191	428.7 597.4	12.1	1.59
	Pallet	269.2	270	271.2 587.7	12.7	1.74
	Stair	306.1	306	308.1 646.4	12.7	1.55
	Gap	325.6	326	327.6 647.0	13.0	2.01

A configuration space defines all the possible configurations of the robot, e.g. joint positions.

Configuration space of a certain stance σ.


Carlos Mastalli

Ioannis Havoutis

Michele Focchi

Darwin G Caldwell

Claudio Semini

Hierarchical Planning of Dynamic Movements without Scheduled Contact Sequences

Most animal and human locomotion behaviors for solving complex tasks involve dynamic motions and rich contact interaction. In fact, complex maneuvers need to consider dynamic movement and contact events at the same time. We present a hierarchical trajectory optimization approach for planning dynamic movements with unscheduled contact sequences. We compute whole-body motions that achieve goals that cannot be reached in a kinematic fashion. First, we find a feasible CoM motion according to the centroidal dynamics of the robot. Then, we refine the solution by applying the robot's full-dynamics model, where the feasible CoM trajectory is used as a warm-start point. To accomplish the unscheduled contact behavior, we use complementarity constraints to describe the contact model, i.e. environment geometry and non-sliding active contacts. Both optimization phases are posed as Mathematical Program with Complementarity Constraints (MPCC). Experimental trials demonstrate the performance of our planning approach in a set of challenging tasks.

I. INTRODUCTION

Legged robots are able to traverse areas that are inaccessible to wheeled vehicles, such as complex and unstructured environments. Such environments are common to search and rescue scenarios, one promising application of legged systems. From the legged locomotion point of view, natural disaster scenarios require planning and execution of complex behaviors in environments with high uncertainty. Complex behaviors require whole-body movements and multiple contact interactions at the same time. Indeed, whenever an articulated body is in contact with the environment (e.g. legged robots) the set of contact forces and joint commands describe the evolution of the motion. Dynamic maneuvers such as jumping and rearing need to consider different sequences of contacts (mode-switching), which cannot be tackled with traditional predefined gaits [START_REF] Barasuol | A reactive controller framework for quadrupedal locomotion on challenging terrain[END_REF], [START_REF] Winkler | Path planning with force-based foothold adaptation and virtual model control for torque controlled quadruped robots[END_REF].

Locomotion in complex environments requires reasoning about terrain conditions, planning and execution of movements through a sequence of contacts, i.e. footholds and/or handholds. These can be posed as separate problems (decoupled approach), i.e. motion and contact planning, and control. Such approaches reduce the combinatorial search space at the expense of the richness of complex behaviors. In contrast, highly-dynamic movements need to consider contact forces and robot dynamics. For instance, contact forces play an important role for predicting the ballistic 1 Department of Advanced Robotics, Istituto Italiano di Tecnologia, via Morego, 30, 16163 Genova, Italy. email: {carlos.mastalli, michele.focchi, darwin.caldwell, claudio.semini}@iit.it. 2 Robot Learning and Interaction Group, Idiap Research Institute, Martigny, Switzerland email: ioannis.havoutis@idiap.ch This work was in part supported by the DexROV project through the EC Horizon 2020 programme (Grant #635491). The proposed hierarchical trajectory optimization reduces the complexity of the motion planning problem by considering two different optimization phases: centroidal and full trajectory optimization. First, the centroidal trajectory optimization phase produces a locally optimal CoM motion using the centroidal dynamics model [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF], which does not consider joint dynamics (i.e. link's CoM). Second, the full trajectory optimization phase refines the CoM trajectory by applying the robot's full-dynamics and joint limits. Both optimization phases use complementarity constraints to model the contact interactions. trajectory in a jumping task. A decoupled motion planner has to explore different plans in the space of feasible movements, which is often defined by physical (friction properties), stability (whole-body balance), dynamic (inertial properties) and task constraints (goal positions and orientations). On the other hand, coupled motion planners compute simultaneously contacts and body movements by posing the problem as a hybrid system or a mode-invariant optimization problem.

Centroidal Trajectory Optimization

In this paper, we are concerned with finding feasible trajectories for complex tasks, i.e. tasks that require the exploration of different mode sequences through highlydynamic movements. We choose a set of jumping tasks as examples, as these highlight the ability to explore the dynamical capabilities of the robot in order to reach goals that are unreachable in a kinematic manner.

The main contribution of this paper is a novel hierarchical trajectory optimization approach based on the principle of divide and conquer ( Fig. 1). Our hierarchical trajectory optimization is capable of producing a wide range of complex behaviors without scheduling a contact sequence by reducing the search space to a fixed sequence of commands, which can be used to find continuous motion plans. The trajectory optimization approach is posed as a MPCC. First, we prune the search space by finding a feasible trajectory according to the robot's centroidal dynamics. Second, we find a feasible trajectory in terms of the full dynamics and joint limits of the robot.

The rest of the paper is structured as follows: after discussing previous research in the field of dynamic motion planning for legged systems (Section II) we describe the direct hierarchical trajectory optimization approach proposed for dynamic motion planning in Section III. In Section IV we evaluate the performance of our trajectory optimization approach in experimental trials with a robotic leg before Section V summarizes this work and presents ideas for future work.

II. RELATED WORK

State-of-the-art legged motion planning methods are often constrained to non-dynamic regimes with scheduled contact sequences [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF][5][6] [START_REF] Winkler | Planning and Execution of Dynamic Whole-Body Locomotion for a Hydraulic Quadruped on Challenging Terrain[END_REF] and decoupled models [START_REF] Mastalli | On-line and On-board Planning and Perception for Quadrupedal Locomotion[END_REF] [START_REF] Dellin | A framework for extreme locomotion planning[END_REF]. For example, most of the current locomotion approaches follow predefined contact sequences, specified by fixed patterns of locomotion. These assumptions reduce the capability of traversing complex environments with legged systems. We believe that natural locomotion in complex environments requires dynamic movements with unscheduled contact sequences. Nevertheless, these approaches are often posed as non-linear optimization problems in which kinematic and dynamic models of the robot, and task information are stated as hard [START_REF] Posa | A direct method for trajectory optimization of rigid bodies through contact[END_REF][11] [START_REF] Tassa | Stochastic Complementarity for Local Control of Discontinuous Dynamics[END_REF] or soft constraints [START_REF] Mordatch | Discovery of complex behaviors through contact-invariant optimization[END_REF] [START_REF] Gabiccini | A Computational Framework for Environment-Aware Robotic Manipulation Planning[END_REF]. Currently, there are many approaches that reduce the complexity of the problem. Different dynamic formulations have been proposed such as: point-mass, cart-table [START_REF] Kalakrishnan | Fast, robust quadruped locomotion over challenging terrain[END_REF][7], spring-loaded inverted pendulum model [START_REF] Poulakakis | The spring loaded inverted pendulum as the hybrid zero dynamics of an asymmetric hopper[END_REF][16] and contact wrench sum [START_REF] Dai | Whole-body motion planning with simple dynamics and full kinematics[END_REF] [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF]. However, most of these approaches use simplified models that do not capture the full-body dynamics of the robot, and reduce the richness of the solutions. On the other hand, using full-body dynamic models [START_REF] Posa | A direct method for trajectory optimization of rigid bodies through contact[END_REF][17], one can produce richer movements but often such formulations require an excessively long amount of time to compute solutions, they are prone to reaching and becoming trapped in local minima and infeasible regions.

Defining a scheduled contact sequence considerably reduces the search space by constraining the plans to a fixed pattern of locomotion [START_REF] Mastalli | On-line and On-board Planning and Perception for Quadrupedal Locomotion[END_REF]. On the other hand, with an unscheduled contact sequence, we can increase the possibilities of locomotion [START_REF] Hauser | Multi-modal motion planning for a humanoid robot manipulation task[END_REF] [START_REF] Escande | Planning contact points for humanoid robots[END_REF], which could in turn be crucial for finding solutions in complex environments. From the optimization point of view, unscheduled contact sequence approaches include the contact forces as decision variables, allowing us to consider friction cone constraints. In the literature there exist two main approaches that incorporate contact forces in the optimization; the first one, smooths the contact forces but has the drawback of allowing the contact to be enabled when there is a distance w.r.t. the contact surface [START_REF] Mordatch | Discovery of complex behaviors through contact-invariant optimization[END_REF], the second one, uses complementarity constraints to model inelastic contacts [10][11]. Complementarity constraints describe inelastic contact forces without resulting in stiff differential equations that require an extremely small time step [START_REF] Stewart | An implicit time-stepping scheme for rigid body dynamics with Coulomb friction[END_REF].

III. HIERARCHICAL DYNAMIC PLANNING

Our work was motivated by the observation that most animal and human locomotion behaviors involve dynamic motions through contact interactions. For instance, kangaroos are dynamic jumpers that use hopping as the main locomotion strategy. Indeed, in kangaroo locomotion, highlydynamic movements and contact forces play an important role for finding efficient locomotion trajectories.

Although such dynamic maneuvers are undoubtedly beneficial, planning and execution of these whole-body trajectories is challenging due to the loss of control authority during flight phases and undefined contact events. We tackle it by generating a whole-body trajectory toward a body goal state (desired body height) that ensures a dynamic motion plan through contact interactions. To accomplish this, we describe the contact model using complementarity constraints which defines our approach as a mode-invariant trajectory planner.

A. Generating Dynamic Motions

Consider a rigid body system with n degrees of freedom, of which n b are floating-base degrees of freedom. The state of the robot is represented by its floating-base and actuated joint components, q = (q b , q q ). Additionally, the robot has p end-effector or contact points.

The system's evolution depends on the internal joint torques, τ q , and the contact force, λ j , applied at the j th end-effector. This evolution is subject to robot and environmental constraints such as: joint limits and environment geometry. Exploring different mode switches (contact events) and dynamic movements could produce unsuccessful locally optimal solutions. We improve the solutions by applying a hierarchical trajectory optimization. In the first phase, we model the system's evolution with the centroidal dynamics, i.e. in the CoM space. Then, we impose joint dynamics and limits using a full-dynamic model. Figure 1 presents an overview of our hierarchical trajectory optimization approach.

1) Centroidal-dynamic model: The robot dynamics can be projected at the CoM, i.e. the centroidal dynamics of the robot. In a full floating-base system (n b = 6 DoF), the centroidal-dynamic model describes the rate of change of linear and angular momentum of CoM with respect to the inertial frame of reference [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF]. The rate of change of linear and angular momentum is determined by contact forces λ j , gravitational force mg and the motion of the robot's links

mr = p j=0 λ j + mg (1) Ḣc (q, q) = p j=0 (x j -r) × λ j ( 2
)
where m is the total mass of the robot, r ∈ R 3 is the CoM position, λ j ∈ R 3 is the contact force applied at the j th end-effector, H c ∈ R 3 is the centroidal angular momentum and x j ∈ R 3 is the end-effector position. The centroidal angular momentum is computed through the computation of the Centroidal Momentum Matrix (CMM) as defined in [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF].

2) Full-dynamic model: The full-dynamic model enables us to compute the joint efforts given a motion state (q, q, q) subject to contact forces λ j . We partition the dynamics equation of the robot into the unactuated floating-base DoFs q b (n b equations) and the active robot joints q q (n q equations):

H(q) qb qq + c b c q (q, q) - p j=0 J T bj J T qj λ j b=ID(model,q, q,q) = 0 τ q (3)
where H ∈ R n×n is the floating-base inertial matrix, c = (c b , c q ) ∈ R n is the force vector that accounts for Coriolis, centrifugal, and gravitational forces, λ j ∈ R 3 are the ground contact forces at the j th end-effector (i.e. point feet), and their corresponding Jacobian, J j = J bj J qj ∈ R 3p×n and τ q ∈ R nq are the joint efforts that we wish to calculate. The left-hand term b = (b b , b q ) is computed efficiently using the Featherstone implementation of the Recursive Newton-Euler Algorithm (RNEA) [START_REF] Featherstone | Rigid Body Dynamics Algorithms[END_REF].

B. Contact Model

In dynamic movements, contact forces play an important role, e.g. a jumping or hopping task. Traditional approaches compute trajectories given a predefined contact sequence. These approaches do not exploit the fact that an optimized mode switching could be required for the success of a certain task.

A contact event occurs when a signed distance to the surface is strictly zero, and additionally, there is a contact force acting along the surface normal. Moreover, it is expected a null normal contact force when the contact is inactive, i.e. a positive signed distance. In other words, normal contact forces and signed distances are orthogonal and positives functions [START_REF] Kolter | A control architecture for quadruped locomotion over rough terrain[END_REF]. Additionally, we desire that active contacts do not slide. Such condition implicates an orthogonality between normal contact forces and tangential velocities [START_REF] Kalakrishnan | Fast, robust quadruped locomotion over challenging terrain[END_REF]. In the optimization literature, constraints with combinatorial nature, such as the set of contact model equations (4)(5), can be described as complementarity constraints

0 ≤ λ n j ⊥ φ j (q) ≥ 0 (4) 0 ≤ λ n j ⊥ ẋt j (q, q) ≥ 0 (5)
where λ n j is the contact force acting along the surface normal at the j th end-effector (i.e. contact point), φ j (q) is the signed distance between the j th contact point x j and the surface S i , and ẋt j (q, q) is the velocity of the contact point along the tangential surface. Contact-point positions and velocities are calculated efficiently using spatial algebra.

C. Trajectory Optimization

Planning problems without scheduled contact sequences are often hard to solve due to the contact forces producing discontinuities in the dynamics. Here, we tackle this issue by applying a hierarchical trajectory optimization scheme, which uses different dynamic models in a two-phase manner. Using a different (simpler) dynamic model in the first optimization phase, we imposes a dynamic relaxation, that helps to explore different mode switches. Thus, we find a feasible CoM motion in terms of the robot's centroidal dynamics. Then, we refine it by applying the full-dynamic model in the second, more complex, trajectory optimization phase.

1) Centroidal trajectory optimization:

The centroidal trajectory optimization step computes a feasible CoM trajectory through the mapping of contact forces inside the centroidal dynamics. The CMM maps the robot's generalized velocities to its spatial momentum (for more details see [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF]). We sample the trajectory in N knot-points with a fixed-time duration h. In this optimization phase, the decision variables of the optimization problem are the robot position q, the robot velocity q, the CoM position r, the CoM velocity ṙ, the contact forces λ, and the end-effector (contact) positions x. Our cost function evaluates the trajectory in terms of the desired high-level goal of the task w(q) as:

min q[k], q[k],r[k],ṙ[k], Hc[k], Ḣc[k],λ[k],x[k] h N k=1 w(q[k]) -w(q * [k]) Qq (6)
where w(q) constructs a task-specific value from relevant features of the task, and w(q[k]) -w(q * [k]) Qq computes its associated quadratic cost given a desired robot position q * . Note that x Q is an abbreviation for the quadratic cost x T Qx.

We transcribe the centroidal dynamics differential equations (2) to algebraic ones by applying an Euler-backward integration rule with a fixed-time step h

r[k -1] -r[k] + hṙ[k] = 0 (7) H c [k -1] -H c [k] + h ḢC [k] = 0 (8) m(ṙ[k] -ṙ[k -1]) -h p j=0 λ j [k] + mg = 0 (9) Ḣc [k] - p j=0 (x j [k] -r[k]) × λ j [k] = 0 (10)
where the centroidal angular momentum is computed from the CMM, A(q), as is explained in [START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF], i.e.

H c [k] = A(q[k]) ẋ[k]
. Additionally, we impose contact position constraints in order to describe the contact interactions

x j [k] -κ j (q[k]) = 0 (11)
where κ j (•) is the direct kinematic function which computes the position of the j th end-effector. We also include joint position and velocity limits.

q l q ≤ q q ≤ q u q (12)

ql q ≤ qq ≤ qu q ( 13
)
To describe different possible mode switches, we add contact position and velocity constraints. These constraints are described as complementarity constraints as follows

λ n j [k], φ j (q[k]) ≥ 0 (14) λ n j [k]φ j (q[k]) = 0 (15) λ n j [k] x t j [k] -x t j [k -1] = 0 (16)
We approximate the contact velocity as contact displacement along the tangential surface. Note that a contact velocity constraint does not guarantee zero displacement between knots.

2) Full trajectory optimization: Once a feasible, and locally optimal, CoM trajectory is computed, we use this CoM trajectory as a warm-start point of the full trajectory optimization phase. We transcribe the full-dynamic model with the same time step value of the centroidal trajectory optimization phase. In this optimization phase, we formulate the problem with the following decision variables: the robot position q, the robot velocity q, the joint efforts τ q and the contact forces λ.

In this stage, the cost function also considers the joint effort energy of the movement τ q as min q

[k], q[k], τ [k],λ[k] h N k=1 w(q[k]) -w(q * [k]) Qq + τ q [k] R (17)
We apply the same integration rule to the full-dynamic differential equation ( 3). Additionally, we add a selection matrix B in order to impose a null wrench vector to the floating-base:

q[k -1] -q[k] + h q[k] = 0 (18) H[k] ( q[k] -q[k -1]) +h c[k] - p j=0 J j [k] T λ j [k] -Bτ [k] = 0 ( 19
)
where the contact forces are determined using the complementarity constraints ( 14)(15)( 16).

In the full trajectory optimization phase, we impose position and velocity bounds [START_REF] Tassa | Stochastic Complementarity for Local Control of Discontinuous Dynamics[END_REF] [START_REF] Mordatch | Discovery of complex behaviors through contact-invariant optimization[END_REF], and additionally joint efforts bounds

τ l q ≤ τ q ≤ τ u q (20)
We derive a continuous motion plan, from the N optimized knot-points, using a polynomial interpolation. Both optimization phases model contact interactions using complementarity constraints. In general, optimization problems with complementarity constraints are difficult to solve because constraint qualifications are hard to satisfy. We solve the MPCC using interior point method as this is faster than a Sequential Quadratic Programming (SQP) algorithm when the number of complementarity constraints increases [START_REF] Raghunathan | An Interior Point Method for Mathematical Programs with Complementarity Constraints (MPCCs)[END_REF]. We use the IPOPT library [START_REF] Wächter | On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming[END_REF]. We relax the orthogonality between the complementarities, for example

λ n j [k]φ j (q[k]) = 0 is posed as λ n j [k]φ j (q[k]) ≤ 0.
For more information about different interior point methods see [START_REF] Raghunathan | An Interior Point Method for Mathematical Programs with Complementarity Constraints (MPCCs)[END_REF].

IV. EXPERIMENTAL RESULTS

This section describes the experiments conducted to validate the effectiveness and quantify the performance of the proposed hierarchical optimization approach.

A. Experimental Setup

We use the hydraulically-actuated robot leg, HyL, in our experiments. HyL weighs approximately 11 kg, is fullytorque controlled and equipped with precision joint encoders, and load cells. HyL is a 1D floating-base system with 2 actuated joints as is shown in Fig. 2. Controller computations 2. HyL: one hydraulically actuated and fully torque controlled leg of the quadruped robot HyQ [START_REF] Semini | Design of HyQ -a Hydraulically and Electrically Actuated Quadruped Robot[END_REF]. The HyL robot has a total number of 3 DOF: 1D floating-base system q b 1 with 2 actuated joints (qq 1 , qq 2 ).

+ - + - + - Fig.
are done on-board in an i7/2.8 GHz PC with a realtimetime kernel. Motion plans are computed off-line using a predefined terrain model.

For each experiment, we specify the goal state of the robot's trunk, and the desired final joint position as a terminal cost. The hierarchical trajectory optimizer finds a sequence of footsteps through dynamic movements without a predefined order, which the controller then executes dynamically. We use a PD controller, using the planned joint efforts as feedforward inputs. We validate the performance of our framework in 3 different examples as seen in Fig. 3, and compare against the full dynamic optimization (Table I) on the same benchmark examples. The first example consists of reaching a goal that is kinematically not feasible, called the jumping task. In the next examples, the step-jumping tasks, the robot has to reach and keep the desired trunk height, which is done through two different steps: a small step (10 cm) and a big step (15 cm). Additionally, the reader is strongly encouraged to view the accompanying video as it provides the most intuitive way to demonstrate the performance of our approach.

B. Results and Discussion

1) Motion planning through dynamical system relaxation: We focus on finding trajectories that are only feasible when dynamics and contact forces are considered. Our motion planning approach describes contact events in a MPCC problem. Since the problem is non-convex, our hierarchical optimization tends to guide the exploration away from infeasible regions through dynamical system relaxation, i.e. centroidal to full dynamics. This dynamical relaxation helps to reduce the computation time and cost value.

Our experiments suggest that dynamical system relaxation is key for finding successful motion plans. Table I shows the time and cost reduction of our approach compared with a single full trajectory optimization. We can see that the hierarchical optimization approach tends to have better performance in complex tasks. Nevertheless, in general, the central tendency (median) of the computation time reduction is decreased, while, on average, we improve the quality of the solution (cost reduction). We define a set of 8 different goal states (i.e. trunk height) for computing the time and cost reduction of our approach. For computing the time and cost reduction, we compare the time and cost of a single full trajectory optimization trial against our hierachical optimization approach in the set of goals defined.

2) Reaching goals that are kinematically not feasible: The jumping task demonstrates the ability of exploring the dynamical capabilities of the robot in order to reach goals that are not kinematically possible. In this particular case, we desire to reach with the trunk, a height of 0.85 cm w.r.t. the ground (or 0.27 cm w.r.t. the initial position), which is kinematically not feasible. Thus, our hierarchical motion planner explores different mode sequences in order to plan a dynamically feasible motion. In Fig. 4, the robot plans a countermovement jump (around 7 cm) without being predefined. In countermovement jumps, a preliminary downward movement is executed which increases the jump height because the robot is carried by its own inertia. Then, a fast movement of the foot is planned considering a desired task behavior, e.g. joint position in the apex point.

3) Discovering of new contacts: For the success of some tasks, it is crucial to exploit the environmental conditions, e.g. reaching and keeping a desired trunk position that is kinematically not reachable. So, imagine that we want to keep a desired trunk position but due to gravitational forces this is not possible with just a vertical jump, for instance we need to climb onto an obstacle to accomplish this. With the hierarchical trajectory optimization, we can plan such kind of maneuvers. In fact, Fig. 5 shows that our motion planner solves these tasks by defining a foolhold on top of an available step. Note that a pre-defined footstep sequence is not required to arrive to such a solution.

V. CONCLUSION In this paper we presented a hierarchical trajectory optimization approach for planning dynamic movements through unscheduled contact sequence. First, the hierarchical trajectory optimization finds a feasible CoM motion according to the centroidal dynamics of the robot. Then, a second phase of optimization considers the full-dynamics of the robot. In both phases we use complementarity constraints for contact modeling. We demonstrated that, with our approach, the robot can plan a wide range of movements that consider the full-dynamics and joint effort limits of the robot. We believe that these considerations are crucial for highly-dynamic locomotion tasks, i.e. step-jumping tasks that are cannot be accomplished in a kinematic fashion. We showed how the hierarchical trajectory optimization improves the solutions and significantly reduces the computation time, compared with the full dynamic optimization. Experimental trials with a robotic leg performing highly-dynamic and challenging tasks demonstrate the capability of our planning approach.

In general, trajectory optimization produces motion plans for a fixed sequence of points, N knot-points. Here, we arrive at a continuous motion plan by applying a polynomial interpolation. Nevertheless, polynomial interpolations cannot predict accurately changes in the contact forces, which generally happen in less than 10 ms. We are currently working on generating motion plans given a library of synthesized motions, improving the quality of the solutions, and permitting on-line computation and execution.

  Fig. 1.The proposed hierarchical trajectory optimization reduces the complexity of the motion planning problem by considering two different optimization phases: centroidal and full trajectory optimization. First, the centroidal trajectory optimization phase produces a locally optimal CoM motion using the centroidal dynamics model[START_REF] Orin | Centroidal dynamics of a humanoid robot[END_REF], which does not consider joint dynamics (i.e. link's CoM). Second, the full trajectory optimization phase refines the CoM trajectory by applying the robot's full-dynamics and joint limits. Both optimization phases use complementarity constraints to model the contact interactions.

Fig. 3 .

 3 Fig.3. Snapshots of three experimental trials used to evaluate the performance of our hierarchical trajectory optimization approach. From top to bottom: jumping task; small step jumping (10 cm of height); big step jumping (15 cm of height).

Fig. 4 .

 4 Fig.4. Optimized CoM and foot trajectory for a jumping task that shows a dynamic movement through different phases: thrust and flight. We can see that the hierarchical optimization maximizes the jump energy by planning a countermovement jump, i.e. a preliminary downward movement.

Fig. 5 .

 5 Fig.5. Optimized CoM and contact sequence for reaching and keeping a desired trunk position (big step jumping), which is not reachable with just a vertical motion. The hierarchical trajectory optimization finds a transitory foothold in order to keep the desired trunk position. After landing in the planned foothold, the trunk moves up until the desired goal.

TABLE I TIME

 I AND COST REDUCTION OVER 8 TRIALS COMPARED TO A SINGLE FULL TRAJECTORY OPTIMIZATION.

		Time reduction [%]	Cost reduction [%]
	Task	Md.	Av.	Md.	Av.
	Jumping	10.36	0.0	0.0	1.44
	Step Jumping 48.29	30.46	0.0	12.91