# Gait Optimization for Legged Systems Through Mixed Distribution Cross-Entropy Optimization

Ioannis Tsikelis, Konstantinos Chatzilygeroudis

► To cite this version:

Ioannis Tsikelis, Konstantinos Chatzilygeroudis. Gait Optimization for Legged Systems Through Mixed Distribution Cross-Entropy Optimization. Humanoids 2024 - IEEE-RAS 23rd International Conference on Humanoid Robots, Nov 2024, Nancy, France. pp.1011 - 1018, 10.1109/Humanoids58906.2024.10769935. hal-05036926

HAL Id: hal-05036926

https://hal.science/hal-05036926v1

Submitted on 23 Apr 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Gait Optimization for Legged Systems Through Mixed Distribution Cross-Entropy Optimization*

Ioannis Tsikelis<fnref n="1" />† and Konstantinos Chatzilygeroudis<fnref n="2" />,<fnref n="3" />†

**Abstract**— Legged robotic systems can play an important role in real-world applications due to their superior load-bearing capabilities, enhanced autonomy, and effective navigation on uneven terrain. They offer an optimal trade-off between mobility and payload capacity, excelling in diverse environments while maintaining efficiency in transporting heavy loads. However, planning and optimizing gaits and gait sequences for these robots presents significant challenges due to the complexity of their dynamic motion and the numerous optimization variables involved. Traditional trajectory optimization methods address these challenges by formulating the problem as an optimization task, aiming to minimize cost functions, and to automatically discover contact sequences. Despite their structured approach, optimization-based methods face substantial difficulties, particularly because such formulations result in highly nonlinear and difficult to solve problems. To address these limitations, we propose CrEGOpt, a bi-level optimization method that combines traditional trajectory optimization with a black-box optimization scheme. CrEGOpt at the higher level employs the Mixed Distribution Cross-Entropy Method to optimize both the gait sequence and the phase durations, thus simplifying the lower level trajectory optimization problem. This approach allows for fast solutions of complex gait optimization problems. Extensive evaluation in simulated environments demonstrates that CrEGOpt can find solutions for biped, quadruped, and hexapod robots in under 10 seconds. This novel bi-level optimization scheme offers a promising direction for future research in automatic contact scheduling.

## I. INTRODUCTION

Legged robotic systems play a pivotal role in robotics due to their superior load-bearing capabilities, enhanced autonomy, and effective navigation on uneven terrain. These systems offer an optimal balance between autonomy and payload capacity, in contrast to flying and wheeled robots. Flying robots excel in diverse environments but face limitations in payload capacity and autonomy. On the other hand, wheeled robots are highly efficient in transporting heavy loads over long periods of time but are primarily restricted to even terrain. The biomimetic design of bipedal and humanoid robots further underscores their significance,

<figure><img src="image_2.png" /><figcaption>Fig. 1. Overview of CrEGOpt method.</figcaption></figure>

as these systems are well-suited for environments typically designed for human mobility and interaction.

However, planning and executing the motion of legged systems has proven to be a challenge. Planning and optimizing the gaits for legged robots is inherently challenging due to the complexity of their dynamic motion and the numerous optimization variables involved [1], [2]. Unlike wheeled robots, legged robots must maintain their balance while transitioning through various phases of movement, thus necessitating precise coordination of multiple joints and actuators. The need to adapt to diverse and unpredictable terrains further complicates gait optimization, as the robot must continually adjust its posture and force distribution to avoid instability and potential falls.

Trajectory optimization (TO) methods [1] address the challenge of gait planning for legged robots by formulating it as an optimization problem. These methods aim to determine the optimal control inputs and joint trajectories that minimize a predefined cost function, such as energy consumption, stability, or smoothness of motion. Although computationally demanding, these methods provide a structured framework to

<footnote>*Full Reference: I. Tsikelis and K. Chatzilygeroudis "Gait Optimization for Legged Systems Through Mixed Distribution Cross-Entropy Optimization" in 2024 IEEE-RAS 23rd International Conference on Humanoid Robots (Humanoids), Nancy, France, 2024, pp. 1011-1018, https://doi.org/10.1109/Humanoids.58906.2024.10769935</footnote>

<footnote>†Equal Contribution</footnote>

<footnote n="1">¹Computer Engineering and Informatics Department, University of Patras, GR-26504 Patras, Greece, tsikelis.i@protonmail.com</footnote>

<footnote n="2">²Laboratory of Automation and Robotics (LAR) in the Department of Electrical & Computer Engineering, University of Patras, GR-26504 Patras, Greece, costashatz@upatras.gr</footnote>

<footnote n="3">³Computational Intelligence Laboratory (CILab), Department of Mathematics, University of Patras, GR-26110 Patras, Greece</footnote>
achieve adaptive and efficient gait strategies, enabling legged robots to navigate complex environments [3], [4], [5].

Despite their success, applying optimization-based methods for selecting the type of gaits in legged robots presents substantial challenges. Contact-implicit methods [6], which incorporate contact dynamics directly into the optimization problem, lead to highly nonlinear and non-convex problems that are very hard to solve effectively. Additionally, methods based on mixed integer programming (MIP), which model gait decisions, such as gait transitions, with discrete variables, are time-consuming due to the combinatorial nature of the problem. The inclusion of integer variables exponentially increases the search space, making it difficult to find optimal solutions within a reasonable timescale, especially for high-dimensional systems, unless strong simplifications are made [7]. Thus, while optimization-based methods offer a powerful framework for gait selection, their practical application is hindered by computational demands and the complexity of accurately modeling dynamic interactions. Consequently, most successful optimization-based methods rely on a hybrid formulation of the dynamics, where we basically predefine the gait sequence by hand [4], [8].

To overcome this limitation, we propose Cross Entropy Gait Optimization for Legged Systems (CrEGOpt), a novel method that effectively combines traditional trajectory optimization with a black-box optimization method, enabling us to solve the gait optimization problem within seconds. CrEGOpt formulates a bi-level optimization scheme. In particular, in the higher-level it employs the Mixed Distribution Cross-Entropy Method (CEM-MD) [9], which optimizes both discrete and continuous variables, to determine gait sequences and the duration of swing/stance phases. The key insight is that fixing the gait sequence and phases' duration simplifies the lower-level trajectory optimization problem significantly. Therefore, each individual in CEM-MD addresses a simpler trajectory optimization problem. Additionally, by leveraging the parallelization capabilities of CEM-MD, we can solve complex problems in a few seconds.

We extensively evaluate our novel methodology in simulated environments focusing on finding gait sequences for biped, quadruped and hexapod robots. The results showcase that CrEGOpt is able to find solutions to difficult optimization problems in less than 10 s.

Overall, this original integration of black-box optimization and trajectory optimization paves the way for future research in automatic contact scheduling.

## II. BACKGROUND

### A. Optimal Control and Trajectory Optimization

The general optimal control problem can be defined as [1]:

$$ \begin{array}{ll} \underset{\mathbf{x}(t),\mathbf{u}(t)}{\operatorname{argmin}} & \mathcal{J}(\mathbf{x}(t), \mathbf{u}(t)) = \int_{t_0}^{t_f} \ell(\mathbf{x}(t), \mathbf{u}(t))dt + \ell_F(\mathbf{x}(t_f)) \\ \text{s.t.} & \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)) \end{array} \quad (1) $$

where<fnref n="1" />

* $\mathbf{x}(t) \in \mathbb{R}^{N_s}$, $\mathbf{u}(t) \in \mathbb{R}^{N_u}$ are the state and control trajectories

* $\mathcal{J}(\mathbf{x}(t), \mathbf{u}(t))$ is the “cost function”

* $\ell(\mathbf{x}(t), \mathbf{u}(t))$ is the “stage cost”

* $\ell_F(\mathbf{x}(t_f))$ is the “terminal cost”

* $\dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t))$ are the “dynamics constraints”

* we can potentially add more constraints

The above formulation results in an infinite-dimensional problem that is generally unsolvable on a computer. Numerous methods exist to transcribe this into a finite-dimensional problem, with discretization and direct collocation being the primary approaches [10], [1]. Discretizing the above problem leads to the following generic formulation:

$$ \begin{array}{ll} \underset{\mathbf{x}_{1:K}, \mathbf{u}_{1:K-1}}{\operatorname{argmin}} & \mathcal{J}(\mathbf{x}_{1:K}, \mathbf{u}_{1:K-1}) = \sum_{k=1}^{K-1} \ell(\mathbf{x}_k, \mathbf{u}_k) + \ell_F(\mathbf{x}_K) \\ \text{s.t.} & \mathbf{x}_{k+1} = f_{\text{discrete}}(\mathbf{x}_k, \mathbf{u}_k) \end{array} \quad (2) $$

where $\mathbf{x}_k \in \mathbb{R}^{N_s}$ and $\mathbf{u}_k \in \mathbb{R}^{N_u}$. Direct collocation optimizes over knot points and links them using splines [10]. Both methods are sufficiently versatile to yield strong solutions to the problem. For discretization, the problem is typically addressed using Differential Dynamic Programming [1], [11], [12], whereas direct collocation employs general non-linear optimization solvers [10].

### B. Contact-Implicit Optimization

Contact-Implicit formulations include the contact forces as optimization variables, and form complementarity problems [6]. A generic formulation can be described as follows:

$$ \begin{array}{ll} \underset{\mathbf{x}_{1:K}, \mathbf{u}_{1:K-1}, \mathbf{\lambda}_{1:K-1}}{\operatorname{argmin}} & \sum_{k=1}^{K-1} \ell(\mathbf{x}_k, \mathbf{u}_k) + \ell_F(\mathbf{x}_K) \\ \text{s.t.} & \mathbf{x}_{k+1} = f_{\text{discrete}}(\mathbf{x}_k, \mathbf{u}_k, \mathbf{\lambda}_k) \\ & \mathbf{\lambda}_k \ge 0 \\ & \phi(\mathbf{x}_k) \ge 0 \\ & \phi(\mathbf{x}_k)^T \mathbf{\lambda}_k = 0 \end{array} \quad (3) $$

where

* we have $N_m$ potential contact points

* $\mathbf{\lambda}_k \in \mathbb{R}^{N_m}$ the constraint forces acting along the surfaces' normal vectors

* $\phi(\mathbf{x}_k)$ are non-penetration constraints

Contact-Implicit methods have the ability to automatically discover contact switches and have produced extraordinary results [13], [14], [15], [3], [6], but the optimization problems are hard to solve consistently and robustly, especially for longer time horizons (i.e. more than 100 ms).

### C. Single Rigid Body Dynamics Model

In this paper, we model walking robots using the Single Rigid Body Dynamics (SRBD) Model [4], [1], [16] (Fig. 2). In this model, we assume that the robot can be modeled as a single rigid body with constant moment of inertia and massless limbs/legs that can generate contact forces. The rigid body has mass $m \in \mathbb{R}^+$ and moment of inertia $I \in \mathbb{R}^{3 \times 3}$.

<footnote n="1">¹We denote time derivatives with an upper dot: e.g. $\dot{x}$.</footnote>
<figure><img src="image_3.png" /><figcaption>Fig. 2. Single rigid body dynamics model with contacts. Image from [16].</figcaption></figure>

The body can be described by its linear position $\mathbf{p}_b \in \mathbb{R}^3$, linear velocity $\dot{\mathbf{p}}_b \in \mathbb{R}^3$, body orientation $\mathbf{R} \in SO(3)$ and angular velocity<fnref n="2" /> $\omega \in \mathbb{R}^3$. Each leg $i$ has a linear position $\mathbf{p}_i \in \mathbb{R}^3$, and can generate contact forces $\mathbf{f}_i \in \mathbb{R}^3$. When leg $i$ is in a swing phase, it does not exert any contact force, $\mathbf{f}_i = \mathbf{0}$. Given all of the above, at each time step the following forces and torques are applied on the body:

$$ 
\begin{aligned}
\mathbf{f}_{\text{total}} &= \sum_{i} \mathbf{f}_{i} + m \mathbf{g}, \\
\tau_{\text{total}} &= \sum_{i} \mathbf{r}_{i} \times \mathbf{f}_{i},
\end{aligned} 
\quad (4) $$

where $\mathbf{g}$ is the gravity vector, $\mathbf{r}_i = \mathbf{p}_i - \mathbf{p}_b$, and $\times$ defines the cross product of two three-dimensional vectors. Given the forces and torques in the above equations, we can compute the linear and angular accelerations of the body as follows:

$$ 
\begin{aligned}
\ddot{\mathbf{p}} &= \frac{\mathbf{f}_{\text{total}}}{m}, \\
\dot{\omega} &= \mathbf{I}^{-1}(\mathbf{R}^{\top}\tau_{\text{total}} - \omega \times \mathbf{I}\omega).
\end{aligned} 
\quad (5) $$

These equations are usually referred to as the Newton–Euler equations for rigid body motion in three-dimensional space.

### III. RELATED WORK

When trying to solve a trajectory optimization problem for a legged system, one has to decide how to solve the scheduling of the contact sequences. The easiest way, perhaps, of achieving this goal is to pre-specify a desired contact sequence [4], [5], [17]: it should be noted that this does not necessarily mean restricting the timings and positions of contact points. In this case, the trajectory optimization problem becomes a hybrid trajectory optimization problem instance. More specifically, the foot trajectory optimization problem is split into modes or phases with each phase being treated as a normal TO problem and connected to its neighboring phases via additional constraints. To make it clearer, let us consider an example with a monopod robot. The contact sequence, in this case, alternates between stance and swing phases for the single leg. During a swing phase,

the foot cannot exert any force and thus a formulation where the robot is modeled without any external forces can be used. On the other hand, during a stance phase, the contact forces must be considered. We define additional constraints (at least in position and velocity) to “link” each phase with the next one. Despite its simplicity and obvious drawbacks (e.g. pre-defined contact sequence), this hybrid formulation is one of the most successful TO formulations for legged systems.

In [4], the authors define a hybrid trajectory optimization problem and use a direct collocation transcription to solve the problem with a general non-linear programming solver. Their method is able to optimize for complex gaits in a few seconds, and the phases’ duration can also be optimized over, which allows for a semi-automatic contact sequence discovery. In [17], the authors combine contact implicit optimization and hybrid TO in order automatically discover the contact sequence, while also keeping low computation times. Despite their success, hybrid TO methods do not allow for automatic contact sequences, and methods such as [17] still require solving a contact implicit optimization problem which is difficult to do in a generic and robust fashion.

Contact implicit optimization methods are able to discover contact sequences automatically and have produced impressive results [13], [14], [15], [3], [6]. Although it is generally considered that contact implicit methods produce difficult to optimize problems, there have recently been serious advances in making them faster and more reliable [14], [15], [18]. This is mostly achieved by intelligent splitting of the main problem (e.g. formulating the contact dynamics as a Linear Complementarity Problem [15] or relaxing the contact constraints [18]) or using variational integrators [14]. Overall, contact implicit methods can discover effective gaits, but the underlying problems are still computationally demanding to solve, and/or require heavy engineering effort per task. CrEGOpt, on the other hand, is able to automatically discover contact sequences in a few seconds on a standard CPU.

Another natural way of scheduling contact sequences is to define integer variables to model how many phases each leg can perform. This, obviously produces a Mixed Integer Programming (MIP) scheme and many methods have been proposed in this direction [19], [20], [21], [22], [7]. The main intuition of this approach is to approximate the non-convex mixed integer constraints with convex equivalents to then be able to use off-the-shelf fast MIP solvers. In [19] the authors incorporate kinematic reachability constraints to plan foot placements for the Atlas robot on uneven terrain. They restrict footstep positions to a convex region close to the robot. Valenzuela [20] builds on this work by employing McCormick Envelopes [23] to approximate bilinear terms in a centroidal dynamics model. Aceituno et al. [21] develop a MIP-based motion planning method for a quadruped robot, but restricts contact points to a region near the center of mass (COM). Finally, the authors in [7] propose a novel MIP formulation where they approximate each stance phase as impulsive and show that this simplification is enough to efficiently generate hopping behaviors for a quadruped robot. Overall, MIP methods can automatically discover contact

<footnote n="2">²We express the angular velocity in the body coordinate frame, while the rest of the variables are expressed in the world/inertial coordinate frame.</footnote>
sequences, but often have to make strong assumptions and simplifications in order to get to a tractable problem. On the contrary, CrEGOpt is able to discover gaits in seconds without the need for extreme simplifications.

A few hybrid methods employing sampling-based planners [24], [25] have been proposed for solving the contact sequence and motion optimization problem for legged systems. These methods, however, depend on the modeling of specific cases by making many assumptions and simplifications, and are computationally demanding.

Overall, despite the efforts of the community, there is still no widely accepted method for automatic contact sequence and motion optimization for legged systems. CrEGOpt aims at filling this gap by providing a versatile method that is able to automatic discover gait sequences along with optimized motions for any legged system in just a few seconds.

# IV. CREGOPT METHOD

## A. Overview

CrEGOpt effectively combines black-box optimization with trajectory optimization by establishing a bi-level optimization framework (Fig. 1). The high-level optimizer, CEM-MD (Sec. IV-B), determines the number of stance and swing phases for each leg as well as the duration of each phase. The low-level optimizer addresses a trajectory optimization problem where the leg phases and durations are fixed. This approach allows CEM-MD to manage the combinatorial complexity of phase selection, while the low-level solver tackles the more manageable hybrid trajectory optimization problems.

## B. Mixed Distribution Cross Entropy Method

We take inspiration from recent work and utilize a Mixed Distribution Cross Entropy Method [9] to solve the mixed integer optimization problem of selecting the number of phases and their duration. CEM-MD follows the same principles as the original CEM algorithm, and employs a categorical distribution, alongside the continuous distribution, for modeling the discrete variables (Algo. 1).

In the original CEM algorithm, the population is modeled via a Gaussian distribution, whereas in CEM-MD we model the population via two independent distributions: one Gaussian for the continuous variables and a categorical one for the discrete variables. The algorithm starts each iteration by sampling from the distributions a population of N candidate solutions (lines 4-6 in Algo. 1). It then evaluates the candidate solutions (line 7 in Algo. 1). It then keeps the M best candidate solutions and uses them to update the distributions. Finally, CEM-MD performs several iterations until some convergence criterion is met.

1) **Parallel Computation:** CEM-MD, like the original CEM algorithm, can take advantage of multi-core computers and compute in parallel the function evaluations (in particular, line 7 in Algo. 1). This parallelization can significantly accelerate the wall-time of the process by enabling simultaneous evaluations of numerous candidate solutions. This scalability and speedup can be particularly beneficial in

# Algorithm 1 Mixed Distribution Cross Entropy Method (CEM-MD)

1: **procedure** CEM-MD(N, K, M, J, C)

2: Initialize $\mu_1, \sigma_1, p_1$

3: **for** $k = 1 \rightarrow K$ **do** ▷ K iterations

4: $\mathcal{P}_{\text{cont}} = \{z_1^c, \dots, z_N^c\}$, where $z_i^c \sim \mathcal{N}(\mu_k, \text{diag}(\sigma_k^2))$ ▷ Generate continuous population of size N

5: $\mathcal{P}_{\text{discrete}} = \{z_1^d, \dots, z_N^d\}$, where $z_i^d \sim \text{Categorical}(p_k)$ ▷ Generate discrete population of size N

6: $\mathcal{P} = \{z_1 = (z_1^c, z_1^d), \dots, z_N = (z_N^c, z_N^d)\}$

7: $\mathcal{F} = \{J(z_1), \dots, J(z_N)\}$ ▷ Evaluate individuals with objective function J

8: $\mathcal{P}_{\text{elite}} = \{z_1^{\text{elite}}, \dots, z_M^{\text{elite}}\}$ ▷ Select M elites

9: $\mu_{k+1} = \frac{1}{M} \sum_{z_i^{\text{elite},c} \in \mathcal{P}_{\text{elite}}} z_i^{\text{elite},c}$ ▷ Update continuous mean

10: $\sigma_{k+1}^2 = \frac{1}{M} \sum_{z_i^{\text{elite},c} \in \mathcal{P}_{\text{elite}}} (z_i^{\text{elite},c} - \mu_k)^2$ ▷ Update continuous variance

11: **for** $i = 1 \rightarrow C$ **do** ▷ C categories

12: $p_{k+1}^i = \frac{\text{count\_occurrences}(\text{Category}_i)}{M}$

realistic applications, where fast responses are crucial, such as in our problem: gait optimization for legged systems.

Moreover, provided that we can run the trajectory optimization process on the GPU (e.g. see [26], [27]), we can foresee an implementation of CrEGOpt completely on the GPU allowing us to increase the parallelization and speedup of the whole pipeline. In this case, we can also considerably increase the population size of CEM-MD and get more optimized results. We leave this endeavor for future work.

2) **Optimizing Gaits:** In order for CEM-MD to produce feasible gaits, we need to make sure that the underlying TO problems are being solved. For this reason, we can use the minimization of the number of violated constraints of the TO problem as an objective function for CEM-MD. In most experiments, we exit the optimization process when CEM-MD finds a solution with zero violated constraints; this means that CEM-MD found a TO initialization that produces a feasible result. We can also choose different objectives for CEM-MD, for example: a) minimizing the number of phases, b) minimizing the feet forces. In all cases, we need to ensure that we have zero violated constraints. For this reason, we use the following generic objective function (we assume that CEM-MD maximizes the objective function):

$$J(z) = f(z) - \alpha_{\text{penalty}} N_{\text{vc}}(z) \quad (6)$$

where $N_{\text{vc}}(z)$ is the number of violated constraints of candidate solution z, $\alpha_{\text{penalty}}$ is a penalty factor to ensure that CEM-MD prioritizes finding feasible solutions, and $f(z)$ is any other objective function that we might care about.

## C. Trajectory Optimization with SRBD

Each candidate solution of CEM-MD solves a trajectory optimization problem with fixed number of phases and
durations (i.e. when calling $J(\cdot)$). We follow the formulation of Winkler et al. [4] and:

a) Model the robot as a single rigid body mass with legs of negligible mass. (Sec. II-C);

b) Adopt Winkler et al. [4] phase-based formulation for contact switching (the phases are coming from CEM-MD, and are thus fixed);

c) Parameterize the body pose, foot positions and foot forces with multiple cubic Hermite polynomials.

Overall, we have the following optimization problem (omitting the cubic polynomials for clarity):

$$ \begin{array}{ll} \text{find} & \mathbf{r}(t), \mathbf{r}: \mathbb{R} \rightarrow \mathbb{R}^3, \qquad \text{(Body positions)} \\ & \boldsymbol{\theta}(t), \boldsymbol{\theta}: \mathbb{R} \rightarrow \mathbb{R}^3, \qquad \text{(Body Euler angles)} \\ & \mathbf{p}(t), \mathbf{p}: \mathbb{R} \rightarrow \mathbb{R}^3, \qquad \text{(Foot position)} \\ & \mathbf{f}(t), \mathbf{f}: \mathbb{R} \rightarrow \mathbb{R}^3, \qquad \text{(Foot force)} \end{array} $$

$$ \begin{array}{ll} \text{s.t.} & \text{srbd}(\mathbf{r}, \boldsymbol{\theta}, \mathbf{p}, \mathbf{f}) = \{\ddot{\mathbf{r}}, \ddot{\boldsymbol{\theta}}\}, \qquad \text{(Dynamics)} \\ & \{\mathbf{r}(0), \boldsymbol{\theta}(0)\} = \{\mathbf{r}_{\text{init}}, \boldsymbol{\theta}_{\text{init}}\}, \qquad \text{(Initial State)} \\ & \{\mathbf{r}(T), \boldsymbol{\theta}(T)\} = \{\mathbf{r}_{\text{goal}}, \boldsymbol{\theta}_{\text{goal}}\}, \qquad \text{(Goal State)} \\ & \mathbf{p}(t) \in \mathcal{B}(\mathbf{r}(t), \boldsymbol{\theta}(t)), \qquad \text{(Bounds wrt body)} \\ & \dot{\mathbf{p}}(t) = \mathbf{0}, \text{ for } t \in \text{Contact}, \qquad \text{(No slip)} \\ & \mathbf{p}(t) \in \mathcal{T}, \text{ for } t \in \text{Contact}, \qquad \text{(Contact on terrain)} \\ & \mathbf{f}(t) \in \mathcal{F}, \text{ for } t \in \text{Contact}, \qquad \text{(Pushing force/friction cone)} \\ & \mathbf{f}(t) = \mathbf{0}, \text{ for } t \notin \text{Contact}, \qquad \text{(No force in air)} \end{array} $$

We use the Ipopt solver [28] to optimize the above problem. It is important to note that even though in this paper we use the SRBD formulation and Winkler et al.'s transcription, CrEGOpt is not limited to those and can be utilized with any other transcription and/or model.

### D. Heuristic Sampling Bias

Although our CEM-MD formulation efficiently finds gait sequences within a few iterations, it encounters difficulties when the number of potential combinations is exceedingly high. To address this, we implement a straightforward yet effective heuristic to reduce the possible combinations. The key idea is that having significantly different numbers of phases per leg generally leads to ill-defined trajectory optimization problems that cannot be solved. Therefore, rather than considering all possible leg phases combinations, we restrict our selection to some of those where the difference in the number of phases between the legs is less than three.

This considerably reduces the number of possible combinations, while keeping useful combinations that can yield good results. For example, let us suppose we have a quadruped robot and want the solver to choose among 7 phases for each leg. We have $7^4 = 2401$ total combinations. In our heuristic, we choose the following triplets: all legs are allowed to have at the same time a) 1, 2 or 3 phases, b) 3, 4 or 5 phases, and c) 5, 6 or 7 phases. Thus we now have $3 \times 3^4 = 243$ total combinations. Even though we have removed many possible combinations, the removed combinations are the ones most likely to produce ill-conditioned TO problems (e.g. one leg having 1 phase, another having 7 phases, etc.). At the same time, the remaining combinations are enough to allow for adequate exploration of the gait space by CrEGOpt.

If not specified otherwise, when we refer to CrEGOpt, we assume that CEM-MD with the heuristic sampling is used.

## V. EXPERIMENTS AND RESULTS

In order to validate CrEGOpt, we devise several simulated experiments. More precisely, we aim at answering the following questions:

1) Is CrEGOpt able to find gait sequences for several scenarios and how does it compare to Contact-Implicit and hybrid methods?

2) Why is CrEGOpt better than computing in parallel all possible gait sequence scenarios and taking the best?

3) Can CrEGOpt find optimal gait sequences and durations?

Overall, the experiments showcase the versatility of CrEGOpt in finding effective gaits for different robots within just a few seconds. Videos of the generated trajectories played forward (i.e visualized, not simulated) are available at https://nosalro.github.io/cregopt.

### A. Practical Considerations

From a practical point of view, we are making the following choices:

1) We model the number of phases per leg as independent categorical variables. This choice was made since the implementation and engineering effort was smaller. Moreover, initial experiments with one big variable (i.e., allowing for correlations) yielded similar results.

2) Since we cannot change the number of optimization variables on the fly, and the duration vector size depends on the number of phases per leg, we create a duration vector with the maximum possible size and specific indexing for each leg and phase.

3) Since each duration should always be positive, we are optimizing them in log space, and we are taking the exponential before passing them to the TO problem.

All experiments are performed on an Intel i7-10700K 16-core CPU @ 3.80GHz. The hyper-parameters used for CEM-MD in our experiments are shown in Table I.

<table><caption>TABLE I
HYPERPARAMETERS FOR CEM-MD</caption><thead><tr><th rowspan="2">Parameter</th><th colspan="3">Hyperparameters per Experiment</th></tr><tr><th>Sec. V-B</th><th>Sec. V-C</th><th>Sec. V-D</th></tr></thead><tbody><tr><td>Population size (N)</td><td>16</td><td>16</td><td>64</td></tr><tr><td>Elites size (M)</td><td>12</td><td>12</td><td>32</td></tr><tr><td>Maximum Iterations (K)</td><td>20</td><td>20</td><td>50</td></tr><tr><td>Termination Condition</td><td colspan="2">Feasible solution</td><td>Convergence</td></tr><tr><td>μ<sub>1</sub></td><td>-1.75 (≈ 0.17 s)</td><td>-1.75</td><td>-1.75</td></tr><tr><td>Minimum value</td><td>-2.5 (≈ 0.1 s)</td><td>-2.5</td><td>-2.5</td></tr><tr><td>Maximum value</td><td>-1 (≈ 0.36 s)</td><td>-1</td><td>-1</td></tr><tr><td>σ<sub>1</sub></td><td>1</td><td>1</td><td>1</td></tr><tr><td>p<sub>1</sub></td><td>1/2</td><td>1/2</td><td>1/2</td></tr><tr><td>Minimum Stance Phases</td><td>5</td><td>5</td><td>3</td></tr><tr><td>Maximum Stance Phases</td><td>13</td><td>13</td><td>11</td></tr><tr><td>α<sub>penalty</sub></td><td>100</td><td>100</td><td>1000</td></tr></tbody></table>
<figure><img src="image_4.png" /><figcaption>Fig. 3. Wall-time performance for experiments of Sec. V-B. Contact-Implicit Optimization always maxed out the maximum wall-time. The box plots show the median (black line) and the interquartile range (25th and 75th percentiles) over 20 replicates; the whiskers extend to the most extreme data points not considered outliers, and outliers are plotted individually.</figcaption></figure>

## B. Automatic Gait Sequence Experiments (Question 1)

In order to answer the first question, we devise three different scenarios: a) a quadruped robot<fnref n="3" /> has to go forward 6 meters on a flat terrain, b) a quadruped robot has to go forward 6 meters while rotating around the z-axis on a flat terrain, and c) a quadruped robot has to go forward 6 meters while rotating around the z-axis and climbing a step. We compare our method to a state-of-the-art hybrid TO method (with time durations optimization), TOWR [4], and a contact-implicit formulation.

In each scenario, we define 20 targets that are all 6 meters away from the starting point and reside on an arc (angle from $-\pi/4$ to $\pi/4$), so that we can define statistics for all methods and provide meaningful results on the success rate of each one. We allow 40 seconds of wall-time for Ipopt to find a solution during the TO step and consider as successful the cases when the optimizer managed to find an optimal solution within the time budget.

<table><caption>TABLE II
SUCCESS RATES FOR EXPERIMENTS OF SEC. V-B</caption><thead><tr><th rowspan="2">Algorithm</th><th colspan="3">Success Rate</th></tr><tr><th>Scenario 1</th><th>Scenario 2</th><th>Scenario 3</th></tr></thead><tbody><tr><td><strong>CrEGOpt</strong></td><td><strong>100%</strong></td><td><strong>100%</strong></td><td><strong>100%</strong></td></tr><tr><td>TOWR</td><td>95%</td><td>95%</td><td>95%</td></tr><tr><td>Contact Implicit</td><td>0%</td><td>0%</td><td>0%</td></tr></tbody></table>

The results showcase that across all scenarios, CrEGOpt is able to always discover effective gait sequences, while TOWR fails around 5% of the time and contact implicit optimization is not able to find a feasible solution within the maximum time budget we allow (Tab. II). Additionally, CrEGOpt outperforms both TOWR and contact implicit optimization in wall-time performance (Fig. 3) and is able to find gait sequences in less than 10 seconds. Interestingly, CrEGOpt with our heuristic sampling was able to find a feasible solution in just 1 or 2 CEM-MD iterations. Overall,

<figure><img src="image_5.png" /><figcaption>Fig. 4. Scaling experiments (Sec. V-C, 20 replicates). CrEGOpt is able to keep similar wall-times performances (≈7 s) even when increasing the number of legs. On the contrary, CrEGOpt without the proposed heuristic sampling fails to find effective gaits as the number of legs increases.</figcaption></figure>

the results of this section showcase that CrEGOpt is able to effectively discover gait sequences in just a few seconds.

## C. Scaling Experiments (Question 2)

In this section, we aim at answering the second question. In essence, since we are randomly sampling candidate solutions for CEM-MD, the following question naturally comes up: *Why not run in parallel all possible candidate gait sequences and select the best one?* There are two main reasons why using CrEGOpt is better than enumerating all possible gait sequences.

<figure><img src="image_6.png" /><figcaption>Fig. 5. CrEGOpt optimization results (Sec. V-D). Solid lines are the median over 10 replicates and the shaded regions are the regions between the 25-th and 75-th percentiles.</figcaption></figure>

1) *Scaling*: CrEGOpt scales much better with the number of legs. This is easy to see with an example. Let us assume that we would like to give a maximum number of 7 phases per leg. For a biped robot, we would have $7^2 = 49$ possible combinations, $7^4 = 2401$ total combinations for a quadruped,

<footnote n="3">³We are using TOWR's [4] SRBD model of the ANYmal robot.</footnote>
<figure><img src="image_7.png" /><figcaption>Fig. 6. Typical behavior returned from CrEGOpt when minimizing the number of steps (Sec. V-D).</figcaption></figure>

and $7^6 = 117649$ possible combinations for a hexapod. While evaluating all possible combinations in parallel might be viable for the biped, it would be prohibitive to do so for a hexapod robot. Moreover, CrEGOpt in combination with our heuristic sampling usually converges in just a few CEM-MD iterations, meaning that we only need to evaluate a few dozen combinations. To experimentally validate this, we devise a set of experiments with a biped<fnref n="4" />, a quadruped and a hexapod robot<fnref n="5" /> and compare the wall-time performance.

<table><caption>TABLE III
SUCCESS RATES FOR EXPERIMENTS OF SEC. V-C</caption><thead><tr><th rowspan="2">Algorithm</th><th colspan="3">Success Rate</th></tr><tr><th>Biped</th><th>Quadruped</th><th>Hexapod</th></tr></thead><tbody><tr><td><strong>CrEGOpt</strong></td><td><strong>100%</strong></td><td><strong>100%</strong></td><td><strong>100%</strong></td></tr><tr><td>CrEGOpt w/out heuristic</td><td><strong>100%</strong></td><td>55%</td><td>50%</td></tr></tbody></table>

The results showcase that CrEGOpt is able to find feasible gait sequences for all robots in about the same wall-time (Fig. 4). Moreover, when not using our heuristic sampling (Sec. IV-D), CEM-MD has significantly lower success rate in the quadruped and hexapod robot cases (Tab. III). Overall, CrEGOpt is able to automatically discover walking gaits for all robots in under 10 s.

2) Duration Optimization: CrEGOpt does not only find the gait sequence, but also optimizes for the duration of each phase. Since time durations are continuous variables, the use of an optimization algorithm, such as CEM-MD, allows CrEGOpt to discover gait patterns. In essence, CrEGOpt provides a versatile framework for gait sequence and motion optimization for legged systems.

### D. Gait Optimization Experiments (Question 3)

In this section, we aim at answering the 3rd question. For this reason, we devise a scenario where a quadruped robot needs to reach a target (≈ 4 meters away) while rotating around the z-axis. Here we are interested in maximizing a specific objective function while ensuring feasible TO problems. We are going to use Eq. (6) and define a few different $f(\cdot)$ functions. Moreover, we will increase the population size of CEM-MD and let it run for more iterations. In this manner, we will be able to observe if CrEGOpt is able to optimize specific objective functions.

1) Minimizing the Number of Steps: We first define an objective function that attempts to minimize the total number of steps for all legs. In other words, $f(z) = - \sum_{i=1}^{C} z_i^c$, where $z_i^c$ is the i-th component of $z^c$, and $z = (z^c, z^d)$. Fig. 5 showcases that CrEGOpt is able to optimize the objective function and refine it until it converges to a (possibly local) minimum. Moreover, CrEGOpt converges much faster than simple CEM-MD. Fig. 6 shows a typical resulting behavior. We see that CrEGOpt is able to discover a fast “galloping”-like gait in order to minimize the number of steps.

2) Minimizing the Contact Forces: We now define an objective function that attempts to minimize the ground reaction forces for all legs. We define $f(z)$ as the negative sum of all contact forces along the ground normal direction; in essence, we get the result from the TO problem at candidate $z$, sample $N_{sample}$ points along the force trajectory, and return the negative sum of the forces acting along the normal direction. Fig. 5 showcases that CrEGOpt is able to optimize this new objective function as well and it converges to a local minimum. Fig. 7 shows a typical behavior produced with this objective function. We see that CrEGOpt is able to produce a completely different gait (compared to the previous one) in order to minimize this new objective function.

## VI. CONCLUSION

CrEGOpt effectively combines black-box optimization and trajectory optimization in order to optimize gait sequences and motions for legged systems. Overall, this novel method is versatile and can find effective gait sequences for multiple legged systems in just a few seconds.

Although effective, CrEGOpt has two main limitations. First, we cannot get any convergence guarantees or even convergence analysis of CEM-MD. Thus, there might be cases where it fails completely to find solutions. Secondly, our current implementation of CrEGOpt cannot be used for real-time sensitive applications. Using the simplified SRBD model and maximum parallelization on our CPU, we weren't able to find solutions faster than 2 s (i.e. 0.5 Hz). The main bottleneck is the optimization time taken by failed TO problems in the lower-level. We aim at investigating this in future work; for example, we can devise criteria to stop earlier or predict failing TO problems before they exceed the maximum iterations/wall-time.

Moreover, we plan to exploit methods that can perform the trajectory optimization process on the GPU (e.g. [26], [27]), and implement CrEGOpt entirely on the GPU, thereby

<footnote n="4">⁴We are using TOWR's [4] SRBD model of the biped HyQ robot.</footnote>

<footnote n="5">⁵We are using an SRBD model of a real servo-based hexapod robot [29].</footnote>
<figure><img src="image_8.png" /><figcaption>Fig. 7. Typical behavior returned from CrEGOpt when minimizing the contact forces (Sec. V-D).</figcaption></figure>

enhancing the parallelization and speed of the entire pipeline. This would also allow for a significant increase in the population size of CEM-MD, leading to more optimized results.

Overall, our novel combination of black-box optimization and trajectory optimization sets the stage for future research in automated contact scheduling. Thus, we also foresee use cases of our approach in non-legged systems; e.g. using CEM-MD and trajectory optimization for automatic contact scheduling in manipulation scenarios.

## ACKNOWLEDGMENT

This work was supported by the Hellenic Foundation for Research and Innovation (H.F.R.I.) under the “3rd Call for H.F.R.I. Research Projects to support Post-Doctoral Researchers” (Project Acronym: NOSALRO, Project Number: 7541). The authors would like to thank Dionis Totsila for helping with the creation of the figures.

## REFERENCES

[1] P. M. Wensing, M. Posa, Y. Hu, A. Escande, N. Mansard, and A. Del Prete, "Optimization-based control for dynamic legged robots," *IEEE Transactions on Robotics*, 2023.

[2] J. Carpentier and N. Mansard, "Multicontact locomotion of legged robots," *IEEE Transactions on Robotics*, vol. 34, no. 6, pp. 1441–1460, 2018.

[3] H. Dai, A. Valenzuela, and R. Tedrake, "Whole-body motion planning with centroidal dynamics and full kinematics," in *IEEE-RAS International Conference on Humanoid Robots*, 2014.

[4] A. W. Winkler, C. D. Bellicoso, M. Hutter, and J. Buchli, "Gait and trajectory optimization for legged systems through phase-based end-effector parameterization," *IEEE Robotics and Automation Letters*, vol. 3, no. 3, pp. 1560–1567, 2018.

[5] M. Posa, S. Kuindersma, and R. Tedrake, "Optimization and stabilization of trajectories for constrained dynamical systems," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2016.

[6] M. Posa, C. Cantu, and R. Tedrake, "A direct method for trajectory optimization of rigid bodies through contact," *The International Journal of Robotics Research*, vol. 33, no. 1, pp. 69–81, 2014.

[7] N. Fey, R. J. Frei, and P. M. Wensing, "3d hopping in discontinuous terrain using impulse planning with mixed-integer strategies," *IEEE Robotics and Automation Letters*, 2024.

[8] C. Mastalli, W. Merkt, G. Xin, J. Shim, M. Mistry, I. Havoutis, and S. Vijayakumar, "Agile maneuvers in legged robots: a predictive control approach," *arXiv preprint arXiv:2203.07554*, 2022.

[9] T. Xue, A. Razmjoo, S. Shetty, and S. Calinon, "Logic-skill programming: An optimization-based approach to sequential skill planning," *arXiv preprint arXiv:2405.04082*, 2024.

[10] M. Kelly, "An introduction to trajectory optimization: How to do your own direct collocation," *SIAM Review*, 2017.

[11] C. Mastalli, W. Merkt, J. Marti-Saumell, H. Ferrolho, J. Solà, N. Mansard, and S. Vijayakumar, "A feasibility-driven approach to control-limited ddp," *Autonomous Robots*, vol. 46, no. 8, pp. 985–1005, 2022.

[12] W. Jallet, A. Bambade, E. Arlaud, S. El-Kazdadi, N. Mansard, and J. Carpentier, "Proxxddp: Proximal constrained trajectory optimization," *Preprint*, 2023.

[13] I. Mordatch, E. Todorov, and Z. Popović, "Discovery of complex behaviors through contact-invariant optimization," *ACM Transactions on Graphics (ToG)*, vol. 31, no. 4, pp. 1–8, 2012.

[14] Z. Manchester, N. Doshi, R. J. Wood, and S. Kuindersma, "Contact-implicit trajectory optimization using variational integrators," *The International Journal of Robotics Research*, vol. 38, no. 12-13, pp. 1463–1476, 2019.

[15] S. Le Cleac'h, T. A. Howell, S. Yang, C.-Y. Lee, J. Zhang, A. Bishop, M. Schwager, and Z. Manchester, "Fast contact-implicit model predictive control," *IEEE Transactions on Robotics*, 2024.

[16] K. I. Chatzilygeroudis, C. G. Tsakonas, and M. N. Vrahatis, "Evolving dynamic locomotion policies in minutes," in *International Conference on Information, Intelligence, Systems & Applications (IISA)*, 2023.

[17] M. R. Turski, J. Norby, and A. M. Johnson, "Staged contact optimization: Combining contact-implicit and multi-phase hybrid trajectory optimization," in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2023.

[18] G. Kim, D. Kang, J.-H. Kim, S. Hong, and H.-W. Park, "Contact-implicit mpc: Controlling diverse quadruped motions without pre-planned contact modes or trajectories," *arXiv preprint arXiv:2312.08961*, 2023.

[19] R. Deits and R. Tedrake, "Footstep planning on uneven terrain with mixed-integer convex optimization," in *IEEE-RAS international conference on humanoid robots*, 2014.

[20] A. K. Valenzuela, *Mixed-integer convex optimization for planning aggressive motions of legged robots over rough terrain*. PhD thesis, Massachusetts Institute of Technology, 2016.

[21] B. Aceituno-Cabezas, C. Mastalli, H. Dai, M. Focchi, A. Radulescu, D. G. Caldwell, J. Cappelletto, J. C. Grieco, G. Fernández-López, and C. Semini, "Simultaneous contact, gait, and motion planning for robust multilegged locomotion via mixed-integer convex optimization," *IEEE Robotics and Automation Letters*, vol. 3, no. 3, pp. 2531–2538, 2017.

[22] Y. Ding, C. Li, and H.-W. Park, "Kinodynamic motion planning for multi-legged robot jumping via mixed-integer convex program," in *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2020.

[23] G. P. McCormick, "Computability of global solutions to factorable nonconvex programs: Part i—convex underestimating problems," *Mathematical programming*, vol. 10, no. 1, pp. 147–175, 1976.

[24] A. Shkolnik, M. Levashov, I. R. Manchester, and R. Tedrake, "Bounding on rough terrain with the littledog robot," *The International Journal of Robotics Research*, vol. 30, no. 2, pp. 192–215, 2011.

[25] I. R. Manchester, U. Mettin, F. Iida, and R. Tedrake, "Stable dynamic walking over uneven terrain," *The International Journal of Robotics Research*, vol. 30, no. 3, pp. 265–279, 2011.

[26] E. Adabag, M. Atal, W. Gerard, and B. Plancher, "MPCGPU: Real-Time Nonlinear Model Predictive Control through Preconditioned Conjugate Gradient on the GPU," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2024.

[27] S. H. Jeon, S. Hong, H. J. Lee, C. Khazoom, and S. Kim, "Cusadi: A gpu parallelization framework for symbolic expressions and optimal control," *arXiv preprint arXiv:2408.09662*, 2024.

[28] A. Wächter and L. T. Biegler, "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming," *Mathematical programming*, vol. 106, pp. 25–57, 2006.
[29] K. Chatzilygeroudis and J.-B. Mouret, "Using parameterized black-box priors to scale up model-based policy search for robotics," in *IEEE International Conference on Robotics and Automation (ICRA)*, 2018.