Realtime Limb Trajectory Optimization for Humanoid Running Through Centroidal Angular Momentum Dynamics

Humanoid robot locomotion is mainly composed of two parts. The first part and the main objective of locomotion is translating the overall body mass (center of mass) to a desired point and at a desired rate. The second part is preserving a proper posture that is suitable to the nature of the selected locomotion method. The posture objective also splits into two parts: keeping the torso upright and swinging the feet in the direction of locomotion to prepare for the next step. This paper aims to determine how to swing the limbs so that the robotic system is ready for the next step while preserving a proper posture.

As the influence of the contact forces on the centroidal dynamics is significant [1, 2], motion generation for the center of mass (CoM) through a template or reduced models is very popular in the legged locomotion literature. These models are crafted to capture the ground reaction force patterns and centroidal dynamics well and are used to simplify some multi-body effects and limitations such that online control, planning, and decision-making are computationally feasible. The usage of centroidal or center of mass models in humanoid control appears in push recovery control through capture point [3]; walking control through linear inverted pendulum (LIP) [4], divergent component of motion (DCM) [5], and spring-loaded inverted pendulum (SLIP) based walking [6, 7]; running control through SLIP [8, 9] and biologically inspired deadbeat control (BID) [10]. Control and planning through such models also appear in periodic jumping [11], bipedal backflip [12] and are also widely used in quadruped locomotion literature in single rigid body dynamics (SRBD) form [13]. The locomotion control literature is covered extensively in [14] and [15].

Even though the aforementioned methods showed success in simulation and hardware experiments, they do not capture the limb dynamics. All the methods assume the stance foot is placed at a specific location and do not include any information regarding the torso orientation, swing leg trajectories, and arm trajectories. Whole-body controllers handle this part of the locomotion control and require the user to select and tune these uncaptured trajectories. In the case of the slow pace and small step size walking, which covers the walking of almost all humanoid robots in the current market, the effect of limb swing is not that apparent mainly for two reasons. First, by the nature of walking behaviors, at least one foot is always in contact with the ground. Hence, the ground reaction forces can continuously be used for regulation as long as the friction cone allows. Second, the swing leg velocity is small due to the high step times and small foot displacements. Hence, the effect of leg swinging on the overall posture and dynamics is small. Such behaviors may not even require an arm swing behavior to regulate the angular momentum. As the walking gets more dynamic and the step length increases, the swing leg moves faster and affects on the overall dynamics more significantly. Such behaviors require more precise trajectory tuning both for the swing leg and arms. This problem is addressed in [16] through an optimization for the overall centroidal angular momentum target value.

In the case of running, which is the target of this paper, the aforementioned conditions get even more challenging. As the flight phase does not involve any contact with the ground, limb movement during this phase drastically affects the overall posture due to the conservation of angular momentum. Such behaviors require either very well-tuned limb swing trajectories or have to be limited with very short-lasting flight phases. This problem is more apparent while running through random environments where, due to lack of periodicity, each step may require different running trajectories along with their well-tuned limb swing characteristics. Unprecise tuning of limb trajectories results in ill-defined postural configurations and high body rotational velocities at the touchdown, which cannot be dissipated by the time and friction cone limited stance phase control. Independent from the desired center of mass trajectory tracking performance, such systems inevitably fail to sustain their motions either immediately or as a result of the accumulated body orientation error. We demonstrate such behaviors in the supplemental video.

This paper focuses on online nonlinear constrained limb trajectory optimization through centroidal angular momentum dynamics for humanoid running. The objective of the optimization is to determine the liftoff configuration and flight phase joint trajectories such that at the next touchdown, the body orientation is minimal. The proposed optimization structure is independent of centroidal running trajectory generation methods. It requires only the desired stance leg position w.r.t. the CoM location at the moment of liftoff and touchdown. While doing so, we exploit some properties of the centroidal momentum matrix (CMM), reduce the size of the problem by some minimal simplifications, and construct a nonlinearly constrained optimization problem that can be solved in real-time. We implement our nonlinear optimization solver for the best efficiency and customization. Lastly, we verify our method in a simulation environment on two different robots. We also implement SLIP-based humanoid running trajectories and verify that the optimized flight phase joint trajectories result in minimal body orientation in the next touchdown phase. The running trajectories are constructed for long flight phases, even longer than the stance phase, such that they are more challenging and harder to control. Lastly, we share the computation time report and show that online implementation is possible.

Let $\bm{q}$ be a set of configuration variables and $\bm{\nu}=(\bm{\nu}_{b},\bm{\nu}_{j})$ be the generalized velocity where $\bm{\nu}_{b}=(\bm{v}_{b},\bm{\omega}_{b})\in\mathbb{R}^{6}$ is the linear and angular velocity of the floating base and $\bm{\nu}_{j}\in\mathbb{R}^{n}$ is the generalized velocity of the joints. The well-known robotic system dynamics results in

where $\bm{f}_{c}\in\mathbb{R}^{3n_{c}}$ is the vector of contact forces for $n_{c}$ number of contacts. The first six rows of (1) correspond to the floating base dynamics and are the underactuated portion of the system dynamics, i.e., the base dynamics cannot directly be driven by the instantaneous joint torques. Hence, the base movement is determined by the contact forces and inertial couplings.

Define $\bm{h}_{G}=[\bm{l}_{G};\bm{k}_{G}]\in\mathbb{R}^{6}$ as the centroidal momentum, which is a combination of translational $\bm{l}_{G}\in\mathbb{R}^{3}$ and rotational (angular) $\bm{k}_{G}\in\mathbb{R}^{3}$ momentum. Then, based on [2] and [17], the first six rows of (1) are equivalent to

where $\bm{g}\in\mathbb{R}^{3}$ is the gravitational acceleration vector, $\bm{f}_{c,i}\in\mathbb{R}^{3}$ is the contact force vector originating from $\bm{p}_{c,i}\in\mathbb{R}^{3}$, $\bm{p}_{\text{CoM}}\in\mathbb{R}^{3}$ is the position of center-of-mass, and $m$ is the total mass. The centroidal momentum dynamics (2) shows the significance of the contact forces on the body dynamics and is one of the main motivations of template model based locomotion approaches [14, 1]. The centroidal momentum is related to the generalized velocities through $\bm{h}_{G}=\bm{A}_{G}(\bm{q})\bm{\nu}$ where $\bm{A}_{G}\in\mathbb{R}^{6\times(n+6)}$ is the centroidal momentum matrix (CMM) [1, 17].

In order to focus our discussion on the angular portion of the CMM, we decompose the linear and angular portions:

The centroidal angular momentum matrix $\bm{A}_{k}$ can also be decomposed into

where $\bm{A}_{v}$, $\bm{A}_{\omega}$, and $\bm{A}_{j}$ represent body translational, body rotational, and joint velocity portions, respectively.

The important implication of Property 1 is that the centroidal angular momentum dynamics is completely decoupled from the translational body velocities. Knowing the initial condition, joint trajectories, and flight time, the body orientation evolution is the same for any set of translational body velocities. Hence, the resultant optimization problem does not require any translational trajectory information and is completely independent of running models.

The invertibility of $\bm{A}_{\omega}$ implies that for a given centroidal angular momentum and joint velocities, there exists a finite body rotational velocity at any system configuration. As this work focuses on estimating the body’s rotational velocity and minimizing its integration during the flight phase, Property 2 ensures that there exists a solution to the calculation for any joint configuration and velocity.

The first obvious observation for the flight phase is the conservation of angular momentum. As the contact force $\bm{f}_{c}$ is a zero vector, there is no external force other than gravity acting on the system, i.e., $\dot{\bm{k}}_{G}=\bm{0}_{3\times 1}$. Hence, during the flight phase, the body orientation cannot directly be controlled by the actuators. It can only be controlled by the coupling effects of the actuated links and this information is embedded inside the $\bm{A}_{j}$ matrix. Define $\bm{k}_{Gf}\in\mathbb{R}^{3}$ to be the centroidal angular momentum of the system during the flight phase such that:

Due to the conservation of angular momentum, $\bm{k}_{Gf}$ is constant throughout the flight phase. The consequent body orientation dynamics yields:

where $\bm{A}_{\omega}$ is always invertible by Property 2. Furthermore, due to Property 1, the body orientation dynamics results in

The body orientation dynamics (13) implies a few important aspects. First and most apparent, it shows that the body orientation can be implicitly controlled through the actuated link joints. Second, during the flight phase, the orientation dynamics is completely decoupled from the translational trajectory. This decoupling allows the body rotational velocity estimation to be independent of the robotic system’s flight phase trajectory.

As discussed in the introduction, centroidal model-based running and jumping planners fall short of identifying how the body, arm, and swing leg trajectories should evolve. They usually capture only the essence of the locomotion, i.e., center of mass, stance foot, and contact force evolution of the system. However, as shown in Fig. 1, humanoid robots require much more to control. The stance foot location just before the liftoff phase is known. On the other hand, since the stance leg becomes the next step’s swing leg, the user has to define an appropriate trajectory for its evolution. The same is valid for the swing leg. At the liftoff moment, the swing leg configuration is unknown and must be decided manually. However, at the next touchdown moment, the same leg becomes the stance leg and should be placed at a known location with respect to the center of mass. Nothing is given for arm evolution. They should be used to regulate the robot’s posture. This section works on these unknown aspects and formulates an optimization problem to decide the limb evolution of the system.

Define $\bm{q}^{d}:\mathbb{R}\rightarrow\mathbb{R}^{n}$ to be a vector of polynomials for the desired actuated joint evolution of the system for the flight phase. Hence, $\bm{q}^{d}(0)$ and $\bm{q}^{d}(t_{f})$ represent the desired joint position at the beginning and end of the flight phase, respectively. Similarly, define $\bm{\theta}_{b}$ to be the body orientation. Hence, for a given initial body orientation $\bm{\theta}_{b}(0)$ the consequent final touchdown configuration integrated:

where function $T$ is a mapping from the angular velocities to the body configuration rate (for example Euler or Quaternion rate).

An algorithm that performs a discrete summation of the nonlinear integrator (14) through $N$ number of sampling points is shown in Alg. 1. Starting from a given (desired) liftoff base orientation and rotational velocity, the algorithm integrates for the touchdown configuration. The “computeCentroidalMap” function in the algorithm calculates the forward kinematics, center of mass location, and centroidal momentum matrix together (see documentation of Pinnochio [19] for “computeCentroidalMap”). The given algorithm will construct a nonlinear cost function for the optimization.

The computation cost of the optimization can be reduced via some reasonable assumptions. Typical leg joints of a humanoid robot are shown in Fig. 2. The ankle link is a relatively small component of the robot and constitutes an insignificant portion of the total inertia. Since its body is a small mass with a very short link length, rotation of ankle joints has a negligible effect on the overall inertia shape and angular momentum. Hence, during the optimization, these joints can be assumed not to move, i.e., remain at a constant angle. This assumption still accounts for ankle inertia and mass but neglects the inertia change with respect to the ankle joint angle. Similarly, the hip yaw joint is also another joint that can be simplified. Even though this joint drives the whole leg, as the mass is distributed around its rotation axis, the inertia that this joint drives is still comparably small. Another important aspect of this joint is that, as seen in Fig. 1, this joint is not very active during running and jumping but only during a change of direction. Hence, assuming this joint will stay at its default position during the flight phase is also a reasonable assumption. During the direction change step, where the support leg rotates to the new heading angle during the flight phase, its rotational effect can be accounted for in the optimization. Similar justifications can also be made for the arm joints, e.g., wrist and shoulder yaw joints. On the other hand, hip roll, hip pitch, shoulder pitch, and knee joints are highly inertial and have an important effect on the overall inertial shape. Additionally, these joints move in a wide range at high velocities and generate a significant portion of angular momentum. Hence, these joints are the main focus of the optimization

Let $\bm{\Gamma}\in\mathbb{R}^{n\times(m+1)}$ be the collection of degree $m$ polynomial constants for all desired joint trajectories such that

From this point, we label the upcoming touchdown leg (left foot in Fig. 1) and the upcoming swing leg (right foot in Fig. 1) as stance and swing foot, respectively.

The simulation section includes verification of the proposed method through two different humanoid robots. We present the optimization results and then show the optimized trajectories’ behavior through a running algorithm. As this study composes a subpart of humanoid running or jumping (see Fig. 3), we inherit a SLIP-based trajectory generation method from [9]. The running algorithm is combined with the flight phase limb swing trajectories generated by the proposed optimization problem. The overview of the nonlinear optimization solver algorithm that we implemented in C++ can be found in Appendix.

This study addresses the problem of determining limb swing trajectories for the flight phases of humanoid running. We propose a nonlinear optimization problem to find a set of proper limb swing trajectories that place the stance leg at the desired location and keep the body upright for the next stance phase. We first explore some properties of the angular momentum portion of the centroidal momentum matrix. Then, taking advantage of these properties, we construct a cost function with some constraints for foot placement points. We show that the size of the optimization problem can be significantly reduced through some mild simplifications. We implement the optimized trajectories on a running algorithm from the literature for verification and perform simulations on two different robots. The simulation results verify that once the robotic system lifts off with the optimized configuration and velocities and tracks the optimized flight phase limb trajectories, the robotic system lands with a proper and minimal body orientation. We also share the computational performance results and verify that the proposed optimization problem can be solved in real-time. Lastly, we provide an overview algorithm for the solver that we implement to solve the optimization problem.

We implement our solver, for the given nonlinear optimization problem. For the completeness of the paper, an overview of the solver with the equality constraints is provided in Algorithm 2. The detailed algorithms can be found in [21]. The tool is available at