Hierarchical Planning of Dynamic Movements without Scheduled Contact Sequences

TL;DR

This paper introduces a hierarchical trajectory optimization method for planning dynamic, contact-rich robot movements without predefined contact sequences, enabling complex maneuvers beyond simple kinematic solutions.

Contribution

It presents a novel hierarchical optimization framework using MPCCs for unscheduled contact planning in dynamic robot motions.

Findings

Successfully plans complex dynamic movements with unscheduled contacts.

Outperforms existing methods in challenging tasks.

Demonstrates real-world applicability through experimental trials.

Abstract

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.

Tables

Table 1. TABLE I: Time 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

Equations

$$m\ddot{\mathbf{r}}=\sum _{j=0}^p{\mathbf{\lambda }}_j+m\mathbf{g}$$

$${\dot{\mathbf{H}}}_c(\mathbf{q},\dot{\mathbf{q}})=\sum _{j=0}^p({\mathbf{x}}_j-\mathbf{r})\times {\mathbf{\lambda }}_j$$

$$\underset{\mathbf{b}=ID(model,\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}})}{\underbrace{\mathbf{H}(\mathbf{q})\left[\begin{array}{c}{\ddot{\mathbf{q}}}_b\\ {\ddot{\mathbf{q}}}_q\end{array}\right]+\left[\begin{array}{c}{\mathbf{c}}_b\\ {\mathbf{c}}_q\end{array}\right](\mathbf{q},\dot{\mathbf{q}})-\sum _{j=0}^p\left[\begin{array}{c}{\mathbf{J}}_{b_j}^T\\ {\mathbf{J}}_{q_j}^T\end{array}\right]{\mathbf{\lambda }}_j}}=\left[\begin{array}{c}0\\ {\mathbf{\tau }}_q\end{array}\right]$$

$$0\le {\lambda }_j^{\hat{n}}\perp {\varphi }_j(\mathbf{q})\ge 0$$

$$0\le {\lambda }_j^{\hat{n}}\perp {\dot{\mathbf{x}}}_j^{\hat{t}}(\mathbf{q},\dot{\mathbf{q}})\ge 0$$

$$\underset{\begin{array}{c}\mathbf{q}[k],\dot{\mathbf{q}}[k],\mathbf{r}[k],\dot{\mathbf{r}}[k],\\ {\mathbf{H}}_c[k],{\dot{\mathbf{H}}}_c[k],\mathbf{\lambda }[k],\mathbf{x}[k]\end{array}}{\min }h\sum _{k=1}^N\left(\Vert \mathbf{w}(\mathbf{q}[k])-\mathbf{w}({\mathbf{q}}^{\ast }[k]){\Vert }_{{\mathbf{Q}}_q}\right)$$

$$\mathbf{r}[k-1]-\mathbf{r}[k]+h\dot{\mathbf{r}}[k]=0$$

$${\mathbf{H}}_c[k-1]-{\mathbf{H}}_c[k]+h{\dot{\mathbf{H}}}_C[k]=0$$

$$\displaystyle m(\mathbf{\bm{\dot{r}}}[k]-\mathbf{\bm{\dot{r}}}[k-1])-h\Bigg{(}\sum_{j=0}^{p}\mathbf{\bm{\lambda}}_{j}[k]+m\mathbf{\bm{g}}\Bigg{)}=\mathbf{\bm{0}}$$

$${\dot{\mathbf{H}}}_c[k]-\sum _{j=0}^p({\mathbf{x}}_j[k]-\mathbf{r}[k])\times {\mathbf{\lambda }}_j[k]=0$$

$${\mathbf{x}}_j[k]-{\mathbf{\kappa }}_j(\mathbf{q}[k])=0$$

$${\mathbf{q}}_q^l\le {\mathbf{q}}_q\le {\mathbf{q}}_q^u$$

$${\dot{\mathbf{q}}}_q^l\le {\dot{\mathbf{q}}}_q\le {\dot{\mathbf{q}}}_q^u$$

$${\lambda }_j^{\hat{n}}[k],{\varphi }_j(\mathbf{q}[k])\ge 0$$

$${\lambda }_j^{\hat{n}}[k]{\varphi }_j(\mathbf{q}[k])=0$$

$${\lambda }_j^{\hat{n}}[k]\left({\mathbf{x}}_j^{\hat{t}}[k]-{\mathbf{x}}_j^{\hat{t}}[k-1]\right)=0$$

$$\underset{\begin{array}{c}\mathbf{q}[k],\dot{\mathbf{q}}[k],\\ \mathbf{\tau }[k],\mathbf{\lambda }[k]\end{array}}{\min }h\sum _{k=1}^N\left(\Vert \mathbf{w}(\mathbf{q}[k])-\mathbf{w}({\mathbf{q}}^{\ast }[k]){\Vert }_{{\mathbf{Q}}_q}+\Vert {\mathbf{\tau }}_q[k]{\Vert }_{\mathbf{R}}\right)$$

$$\mathbf{q}[k-1]-\mathbf{q}[k]+h\dot{\mathbf{q}}[k]=0$$

$$\begin{split}\mathbf{\bm{H}}[k]\left(\mathbf{\bm{\dot{q}}}[k]-\mathbf{\bm{\dot{q}}}[k-1]\right)\\ +h\Big{(}\mathbf{\bm{c}}[k]-\sum_{j=0}^{p}\mathbf{\bm{J}}_{j}&[k]^{T}\mathbf{\bm{\lambda}}_{j}[k]\Big{)}-\mathbf{\bm{B}}\mathbf{\bm{\tau}}[k]=\mathbf{\bm{0}}\end{split}$$

$${\mathbf{\tau }}_q^l\le {\mathbf{\tau }}_q\le {\mathbf{\tau }}_q^u$$

Full text

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 [1, 2].

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 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.

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 highly-dynamic 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 [4][5][6][7] and decoupled models [8][9]. 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 [10][11][12] or soft constraints [13][14]. Currently, there are many approaches that reduce the complexity of the problem. Different dynamic formulations have been proposed such as: point-mass, cart-table [5][7], spring-loaded inverted pendulum model [15][16] and contact wrench sum [11][3]. 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 [10][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 [8]. On the other hand, with an unscheduled contact sequence, we can increase the possibilities of locomotion [18][19], 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 [13], 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 [20].

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, highly-dynamic 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.

III-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, $\mathbf{\bm{q}}=(\mathbf{\bm{q}}_{b},\mathbf{\bm{q}}_{q})$. Additionally, the robot has $p$ end-effector or contact points.

The system’s evolution depends on the internal joint torques, $\mathbf{\bm{\tau}}_{q}$, and the contact force, $\mathbf{\bm{\lambda}}_{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.

III-A1 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 [3]. The rate of change of linear and angular momentum is determined by contact forces $\mathbf{\bm{\lambda}}_{j}$, gravitational force $m\mathbf{\bm{g}}$ and the motion of the robot’s links

[TABLE]

where $m$ is the total mass of the robot, $\mathbf{\bm{r}}\in\mathbb{R}^{3}$ is the CoM position, $\mathbf{\bm{\lambda}}_{j}\in\mathbb{R}^{3}$ is the contact force applied at the $j^{th}$ end-effector, $\mathbf{\bm{H}}_{c}\in\mathbb{R}^{3}$ is the centroidal angular momentum and $\mathbf{\bm{x}}_{j}\in\mathbb{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 [3].

III-A2 Full-dynamic model

The full-dynamic model enables us to compute the joint efforts given a motion state $(\mathbf{\bm{q}},\mathbf{\bm{\dot{q}}},\mathbf{\bm{\ddot{q}}})$ subject to contact forces $\mathbf{\bm{\lambda}}_{j}$. We partition the dynamics equation of the robot into the unactuated floating-base DoFs $\mathbf{\bm{q}}_{b}$ ($n_{b}$ equations) and the active robot joints $\mathbf{\bm{q}}_{q}$ ($n_{q}$ equations):

[TABLE]

where $\mathbf{\bm{H}}\in\mathbb{R}^{n\times n}$ is the floating-base inertial matrix, $\mathbf{\bm{c}}=(\mathbf{\bm{c}}_{b},\mathbf{\bm{c}}_{q})\in\mathbb{R}^{n}$ is the force vector that accounts for Coriolis, centrifugal, and gravitational forces, $\mathbf{\bm{\lambda}}_{j}\in\mathbb{R}^{3}$ are the ground contact forces at the $j^{th}$ end-effector (i.e. point feet), and their corresponding Jacobian, $\mathbf{\bm{J}}_{j}=\begin{bmatrix}\mathbf{\bm{J}}_{b_{j}}&\mathbf{\bm{J}}_{q_{j}}\end{bmatrix}\in\mathbb{R}^{3p\times n}$ and $\mathbf{\bm{\tau}}_{q}\in\mathbb{R}^{n_{q}}$ are the joint efforts that we wish to calculate.

The left-hand term $\mathbf{\bm{b}}=(\mathbf{\bm{b}}_{b},\mathbf{\bm{b}}_{q})$ is computed efficiently using the Featherstone implementation of the Recursive Newton-Euler Algorithm (RNEA) [21].

III-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 (4). Additionally, we desire that active contacts do not slide. Such condition implicates an orthogonality between normal contact forces and tangential velocities (5). In the optimization literature, constraints with combinatorial nature, such as the set of contact model equations (4)(5), can be described as complementarity constraints

[TABLE]

where $\lambda_{j}^{\hat{n}}$ is the contact force acting along the surface normal at the $j^{th}$ end-effector (i.e. contact point), $\phi_{j}(\mathbf{q})$ is the signed distance between the $j^{th}$ contact point $\mathbf{\bm{x}}_{j}$ and the surface $\mathcal{S}_{i}$, and $\mathbf{\bm{\dot{x}}}_{j}^{\hat{t}}(\mathbf{\bm{q}},\mathbf{\bm{\dot{q}}})$ is the velocity of the contact point along the tangential surface. Contact-point positions and velocities are calculated efficiently using spatial algebra.

III-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.

III-C1 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 [3]). 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 $\mathbf{\bm{q}}$, the robot velocity $\mathbf{\bm{\dot{q}}}$, the CoM position $\mathbf{\bm{r}}$, the CoM velocity $\mathbf{\bm{\dot{r}}}$, the contact forces $\mathbf{\bm{\lambda}}$, and the end-effector (contact) positions $\mathbf{\bm{x}}$. Our cost function evaluates the trajectory in terms of the desired high-level goal of the task $\mathbf{\bm{w}}(\mathbf{\bm{q}})$ as:

[TABLE]

where $\mathbf{\bm{w}}(\mathbf{\bm{q}})$ constructs a task-specific value from relevant features of the task, and $\|\mathbf{\bm{w}}(\mathbf{\bm{q}}[k])-\mathbf{\bm{w}}(\mathbf{\bm{q}}^{*}[k])\|_{\mathbf{\bm{Q}}_{q}}$ computes its associated quadratic cost given a desired robot position $\mathbf{\bm{q}}^{*}$. Note that $\|\mathbf{x}\|_{\mathbf{\bm{Q}}}$ is an abbreviation for the quadratic cost $\mathbf{x}^{T}\mathbf{\bm{Q}}\mathbf{x}$.

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

[TABLE]

where the centroidal angular momentum is computed from the CMM, $\mathbf{\bm{A}}(\mathbf{\bm{q}})$, as is explained in [3], i.e. $\mathbf{\bm{H}}_{c}[k]=\mathbf{\bm{A}}(\mathbf{\bm{q}}[k])\mathbf{\bm{\dot{x}}}[k]$. Additionally, we impose contact position constraints in order to describe the contact interactions

[TABLE]

where $\mathbf{\bm{\kappa}}_{j}(\cdot)$ is the direct kinematic function which computes the position of the $j^{th}$ end-effector. We also include joint position and velocity limits.

[TABLE]

To describe different possible mode switches, we add contact position and velocity constraints. These constraints are described as complementarity constraints as follows

[TABLE]

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.

III-C2 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 $\mathbf{\bm{q}}$, the robot velocity $\mathbf{\bm{\dot{q}}}$, the joint efforts $\mathbf{\bm{\tau}}_{q}$ and the contact forces $\mathbf{\bm{\lambda}}$.

In this stage, the cost function also considers the joint effort energy of the movement $\mathbf{\bm{\tau}}_{q}$ as

[TABLE]

We apply the same integration rule to the full-dynamic differential equation (3). Additionally, we add a selection matrix $\mathbf{\bm{B}}$ in order to impose a null wrench vector to the floating-base:

[TABLE]

[TABLE]

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 (12)(13), and additionally joint efforts bounds

[TABLE]

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 [22]. We use the Ipopt library [23]. We relax the orthogonality between the complementarities, for example $\lambda_{j}^{\hat{n}}[k]\phi_{j}(\mathbf{\bm{q}}[k])=0$ is posed as $\lambda_{j}^{\hat{n}}[k]\phi_{j}(\mathbf{\bm{q}}[k])\leq 0$. For more information about different interior point methods see [22].

IV Experimental Results

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

IV-A Experimental Setup

We use the hydraulically-actuated robot leg, HyL, in our experiments. HyL weighs approximately 11 kg, is fully-torque controlled and equipped with precision joint encoders, and load cells. HyL is a $1$D floating-base system with 2 actuated joints as is shown in Fig. 2. Controller computations are done on-board in an i7/2.8 GHz PC with a realtime-time 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.

IV-B Results and Discussion

IV-B1 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.

IV-B2 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.

IV-B3 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.