Junheng Li, Department of Aerospace and Mechanical Engineering, University of
Southern California, CA 90089, USA.

# Dynamic Loco-manipulation on HECTOR: Humanoid for Enhanced ConTrol and Open-
source Research

Junheng Li11affiliationmark:  Junchao Ma11affiliationmark:  Omar
Kolt11affiliationmark:  Manas Shah11affiliationmark:  and Quan
Nguyen11affiliationmark:  11affiliationmark:  Department of Aerospace and
Mechanical Engineering, University of Southern California, USA
<EMAIL_ADDRESS>
###### Abstract

Despite their remarkable advancement in locomotion and manipulation, humanoid
robots remain challenged by a lack of synchronized loco-manipulation control,
hindering their full dynamic potential. In this work, we introduce a versatile
and effective approach to controlling and generalizing dynamic locomotion and
loco-manipulation on humanoid robots via a Force-and-moment-based Model
Predictive Control (MPC). Specifically, we proposed a simplified rigid body
dynamics (SRBD) model to take into account both humanoid and object dynamics
for humanoid loco-manipulation. This linear dynamics model allows us to
directly solve for ground reaction forces and moments via an MPC problem to
achieve highly dynamic real-time control. Our proposed framework is highly
versatile and generalizable. We introduce HECTOR (Humanoid for Enhanced
ConTrol and Open-source Research) platform to demonstrate its effectiveness in
hardware experiments. With the proposed framework, HECTOR can maintain
exceptional balance during double-leg stance mode, even when subjected to
external force disturbances to the body or foot location. In addition, it can
execute 3-D dynamic walking on a variety of uneven terrains, including wet
grassy surfaces, slopes, randomly placed wood slats, and stacked wood slats up
to $6\>\mathrm{\bf{cm}}$ high with the speed of $0.6\>\mathrm{\bf{m/s}}$. In
addition, we have demonstrated dynamic humanoid loco-manipulation over uneven
terrain, carrying $2.5\>\mathrm{\bf{kg}}$ load. HECTOR simulations, along with
the proposed control framework, are made available as an open-source project.
(https://github.com/DRCL-USC/Hector_Simulation).

##  1 Introduction

###  1.1 Motivations

(a)

(b)

(c)

(d)

(e)

(f)

(g)

Figure 1:  HECTOR experiment results: (a) Dynamically walking over random and
unstable wood slats with 2.5 kg payload; (b) Balancing in-place while carrying
8 kg payload (50$\%$ robot mass); (c) Stepping in-place while carrying 4 kg
payload; (d) Balancing when constantly moving the seesaw; (e) Dynamic turning
with 2.5 kg payload (f) Walking up and down 18∘ slope; (g) Walking on grass
with random wood slats. Full result video: https://youtu.be/-r0QoxQgshk

Studying humanoid locomotion is an endeavor born out of a profound curiosity
about understanding one of the most remarkable feats of nature: human
movement. However, achieving dynamic locomotion and manipulation like humans
in these robots remains a formidable challenge, necessitating revolutionary
innovations in mechanical design, sensors, control, and artificial
intelligence.

A handful of exceptionally crafted humanoid hardware systems are currently
available in the market, catering to a diverse range of business applications
and research endeavors. These remarkable machines have showcased impressive
human-like locomotion and manipulation capabilities, exemplified by feats such
as Boston Dynamics Atlas navigating a parkour course and Agility Robotics
Digit transferring packages within warehouse settings. While often associated
with high costs, these general-purpose robots possess immense future potential
in aiding humans in exploration, disaster response missions, warehousing
logistics, industrial inspections, and many more domains. These applications
all require robust and dynamic control algorithms.

In this study, we investigate the implementation of simplified-model-based
control techniques on humanoid robots. Our goal is to validate the feasibility
and efficiency of reduced-order models in dynamic humanoid loco-manipulation
control.

Moreover, we introduce a humanoid hardware and software solution, HECTOR
project, aiming to advance and expand research in humanoid robotics by
providing a robust testbed for both current and future control algorithms.
This also includes the exploration of reinforcement learning (RL)-based
control, a successful technique implemented in numerous quadrupedal robots for
dynamic motions (e.g. Hoeller et al. (2023); Cheng et al. (2023); Bellegarda
et al. (2022)) but yet to be widely adopted on humanoid robots. HECTOR
humanoid robot hardware will stand apart by being compact in scale, highly
power-dense, cost-effective, and designed for easy operation and integration,
addressing the accessibility and affordability barriers that currently hinder
the advancement of humanoid robotics.

###  1.2 Bipedal Locomotion Control

Dynamic locomotion control has been a significant focal point in researchers’
efforts to unlock the potential for human-like agility in bipedal locomotion.
The primary advantage of bipedal motion over other types of locomotion modes
lies in its capability to navigate human-oriented terrains with uneven
surfaces, various terrain types, and discontinuities.

Recent investigations on versatile bipedal locomotion control rely on
simplified models such as the Zero Moment Point (ZMP), Spring-loaded Inverted
Pendulum (SLIP), and Hybrid Zero Dynamics (HZD), all of which have been
employed on locomotion controllers for bipedal robots (e.g. Kajita et al.
(2003); Westervelt et al. (2003); Hereid et al. (2014); Sreenath et al.
(2011)). Notably, prior work in Nguyen et al. (2018) has studied uneven
discrete terrain bipedal locomotion through HZD-based control schemes and gait
libraries. While this locomotion control framework excels at navigating
discrete terrains, it requires precise terrain data for footstep planning.

Quadratic Programming (QP) based optimal control is another popular avenue in
bipedal locomotion control. By linearizing the complex dynamics of the
humanoid robots, an inverse dynamics problem can be efficiently solved by a QP
solver (e.g. Wensing and Orin (2013); Reher et al. (2020); Herzog et al.
(2016); Xiong and Ames (2018)). Authors in Kim et al. (2020) realized stable
and robust bipedal locomotion on humanoid robots with passive ankles through a
time-to-velocity-reversal (TVR) planner and task-oriented Whole-body
Locomotion Control (WBLC). The proposed framework is versatile and
transferable across different robot platforms. However, it requires the
complex whole-body dynamics of the robot for obtaining task-space commands and
solving inverse-dynamics QP.

With the aim of mitigating the requirement for precise terrain information and
the complexity of whole-body dynamics derivation in the above bipedal
locomotion control schemes, we have introduced a Force-and-moment-based MPC
for dynamic and versatile bipedal locomotion behaviors in Li and Nguyen
(2021), which is a middle ground that consists of reduced-order modeling,
adaptivity to perturbations, and extendability to loco-manipulation.

Model Predictive Control (MPC) stands as a potent approach for achieving real-
time planning and control, whether implemented with a reduced-order model or a
nonlinear dynamics model. Recent implementations of MPC-based control in
legged robots, such as the MIT Cheetah (Kim et al. (2019); Di Carlo et al.
(2018)) and ANYmal quadruped (Sleiman et al. (2021)), have demonstrated the
viability of this scheme. However, in the realm of humanoid robots, the MPC-
based approach has primarily functioned as a high-level planner for generating
trajectories and planning footstep locations (e.g. Scianca et al. (2020,
2016); Brasseur et al. (2015)). A higher-frequency inverse-dynamics-based
optimal low-level control has traditionally been employed together with MPC to
manage locomotion and balance (e.g. Chignoli et al. (2021); Daneshmand et al.
(2021)), utilizing the whole-body dynamics model (e.g., Whole-body Control in
Lee et al. (2016)).

In our work, we rely on the proposed Force-and-Moment-Based MPC as the only
primary optimal control layer, without the need for additional low-level WBC
for enhancing balancing. To our knowledge, HECTOR represents the first
instance of using only the simplified rigid body dynamics (SRBD)-based MPC to
execute dynamic bipedal locomotion and loco-manipulation in hardware, thereby
establishing a robust baseline for dynamic control and promising potential for
further integration and development.

###  1.3 Loco-manipulation on Legged Robots

Control strategies for quadrupedal robots in loco-manipulation have achieved
success in various domains, showcasing proficiency in robust object handling
in Rigo et al. (2023), non-prehensile manipulation in Sleiman et al. (2021),
and dynamic load-bearing in Sombolestan and Nguyen (2023).

Furthermore, there also has been a wide array of studies focused on multi-
contact loco-manipulation on humanoid robots. Many of these studies have
adopted a two-stage approach, which involves using high-level planner multi-
contact preview and trajectory planning, and subsequently employing WBC to
effectively track task-space commands such as in Sentis et al. (2010); Audren
et al. (2014); Murooka et al. (2021). One notable challenge encountered when
dealing with control in these scenarios is the accurate modeling of external
objects’ dynamics. As such, a simplified representation of load dynamics,
incorporating the fundamental aspects of the object’s dynamics, can be derived
for model-based control. For example, object dynamics effects may be accounted
for by external forces and wrenches applied to the robot, as seen in Sleiman
et al. (2021); Agravante et al. (2019). In our approach, we simplified the
object dynamics as the weight of the object applied to the robot SRBD model,
as proposed in the author’s prior work (Li and Nguyen (2023b)). This approach
incorporates the essential dynamic effects of the object, enabling the robot
to compensate for the added mass and inertia within the SRBD model while
maintaining linearity in the dynamics formulation.

Figure 2:  HECTOR leg range of motion is illustrated by the range of motion of
hip roll, thigh, and knee joints.  Figure 3: HECTOR’s arm and leg kinematic
definitions and exploded view of transmission systems.

###  1.4 Humanoid Robot Hardware

Humanoid control research frequently demands rigorous hardware validation,
necessitating a robust and reliable humanoid testbed. Traditional humanoid
robots seen in 2000’s and early 2010’s such as ASIMO (Sakagami et al. (2002)),
HUBO (Park et al. (2005)), and TORO (Englsberger et al. (2014)) rely on rigid
actuators (i.e., high gear ratio harmonic drives) that excel in precise
position control, yet this design limits their capacity for speed and impact
mitigation - essential for dynamic motions.

In contrast, WALK-MAN (Tsagarakis et al. (2017)) and Agility Robotics’ Digit
utilize series-elastic actuators, offering improved impact absorption (Abate
(2018)).

On the other hand, Boston Dynamics’s Atlas showcased unparalleled agility and
complex terrain capabilities through hybrid electric and hydraulic actuators.
However, this system’s high cost and closed platform pose difficulties for
mass adoption in scientific research.

Lately, legged robot design methodology using back-drivable quasi-direct-drive
(QDD) actuators has enabled affordable and high-performance dynamic quadruped
robots that are capable of high-speed motion and high-impact mitigation (Seok
et al. (2015)) such as MIT Cheetah 3 (Bledt et al. (2018)) and mini Cheetah
(Katz et al. (2019)). However, this innovative approach, while prevalent in
quadruped robots, has not yet seen widespread adoption in humanoid robots,
presenting a unique opportunity.

Recent literature presented several small-scale torque-controlled bipedal and
humanoid robots specifically designed to achieve dynamic and agile motions.
Notably, Little HERMES (Ramos et al. (2018)), Bolt (Daneshmand et al. (2021)),
MIT humanoid (Chignoli et al. (2021)), , BRUCE (Liu et al. (2022)), and Tello
leg (Sim and Ramos (2022)) have showcased promising dynamic capabilities.

In our work, we introduce HECTOR, a new ground in the field of small-scale
humanoid robots with a unique approach to achieving dynamic motions.
Leveraging QDD actuators allows HECTOR to excel at compliance and adaptivity
in balancing and loco-manipulation and is best suited for force-based control
frameworks. This novel platform combines portability, modularity, high degrees
of freedom, power density, and ease of operation, paving the way for a new
generation of agile and versatile robots. This paper also provides an in-depth
examination of HECTOR’s dynamic locomotion and loco-manipulation abilities,
offering concrete evidence of HECTOR’s hardware capabilities.

###  1.5 Main Contributions:

This work builds upon the authors’ prior conference papers on Force-and-moment
MPC for humanoid locomotion and loco-manipulation (Li and Nguyen (2021,
2023b)). The additional contributions in this work, with respect to (w.r.t.)
these papers, are as follows:

  * •

We present a generalized Force-and-moment-based MPC approach with simplified
rigid body dynamics (SRBD) for generalizing dynamic locomotion and loco-
manipulation control on humanoid robots.

  * •

To validate our proposed MPC framework, we have developed a portable, power-
dense, cost-effective, and torque-controlled modular humanoid hardware,
HECTOR, with a novel hybrid transmission system for achieving dynamic and
agile motions.

  * •

W.r.t. prior papers, we have successfully demonstrated new and robust
locomotion experiments on humanoid hardware. We have achieved stable, 3-D
dynamic, and disturbance-resistant locomotion behavior with only an SRBD MPC
as the primary control layer.

  * •

We have also successfully demonstrated dynamic loco-manipulation over uneven
terrain on humanoid hardware, carrying a 2.5 kg load and walking over random
and unstable wood piles.

  * •

HECTOR simulation and controllers are made available as an open-sourced
project for developing dynamic humanoid robots.

The rest of the paper is organized as follows. Section 2 presents the design
and hardware specifications of HECTOR. Section 3 is a brief overview of the
system architecture of the robot and control system. Section 4 starts by
introducing the dynamics model of HECTOR and the process of simplification to
SRBD and load dynamics. It follows by presenting the Force-and-moment-based
MPC’s formulation, simplification, low-level control, and swing leg policies
for dynamic loco-manipulation. Lastly, we present the results of the proposed
control schemes, validated both in numerical simulations and hardware
experiments.

##  2 HECTOR Hardware Design

Table 1: HECTOR Physical Properties Property | Symbol | Value
---|---|---
Mass, $\mathrm{kg}$ | $m$ | 16.0
SRBD MoI, $\mathrm{kg}\cdot\mathrm{m}^{2}$ | $\prescript{}{\mathcal{B}}{I_{xx}}$ | 0.541
| $\prescript{}{\mathcal{B}}{I_{yy}}$ | 0.520
| $\prescript{}{\mathcal{B}}{I_{zz}}$ | 0.069
Upper/Lower Leg Length, $\mathrm{m}$ | $l_{leg}$ | 0.22
Foot Length, $\mathrm{m}$ | $l_{t}$ | 0.09
| $l_{h}$ | 0.05
Arm Length, $\mathrm{m}$ | $l_{arm}$ | 0.20

In our work, to validate our proposed control scheme, we present HECTOR, a
torque-controlled humanoid testbed designed to perform highly dynamic
behaviors in hardware experiments. HECTOR is a versatile and robust small-
scale humanoid platform, designed to offer accessible and cost-effective
hardware solutions that are easy to operate, making it an ideal choice for
research and development purposes. Additionally, we offer open-source control
and simulation software for HECTOR in both ROS and MATLAB. This initiative
aims to expedite advancements in the field of humanoid robotics by providing a
standard and available software platform.

###  2.1 Mechanical Design

HECTOR, as illustrated in Figure 3, is primarily divided into three modular
segments: the arms, trunk, and legs. This modular approach enables the robot
to be easily reconfigured between bipedal and humanoid forms to suit various
application needs. Standing at a height of 0.85 meters, HECTOR’S size
resembles that of a child with physical properties summarized in the Table
LABEL:tab:robotProperties. Each modular leg consists of 5 degrees of freedom
including hip yaw, hip roll, thigh, knee, and ankle joints to achieve a large
range of motion as seen in Figure 2. In the design and development phase of
HECTOR’s leg structure, the primary focus was to strike a balance between
maintainability, cost-effectiveness, and performance. The structural
components of the legs are designed around the use of Aluminum 7075 T6 laser-
cut plates and commercially available parts to offer rigidity for robust
dynamic motions while maintaining a simplified design for cost-efficient
prototyping as well as ease of serviceability.

Transitioning to the upper body, the arm module of HECTOR is designed to have
a yaw joint at the shoulder and two pitch joints at the shoulder and elbow.
This design, highlighted in Figure 3, ensures that HECTOR remains lightweight
yet capable of performing a diverse array of tasks.

Figure 4:  System and Control Architecture of HECTOR.  Table 2: HECTOR Joint Configurations  Property | QDD Actuator | QDD + Timing Belt
---|---|---
Gear Ratio | 9.1 | 14.1
Peak Torque, $\mathrm{Nm}$ | 33.5 | 51.9
Peak Speed, $\mathrm{rad/s}$ | 21.0 | 13.6

###  2.2 Joint Actuation

Building on HECTOR’s compact and efficient design, the actuation system is
engineered to fully exploit these attributes, enabling contact-rich, robust,
and agile motions.

To harness the advantages that quasi-direct-drive actuators provide, HECTOR
joints are all driven by identical modular quasi-direct drive actuators
equipped with a single-stage 9.1:1 planetary gearbox. This design ensures
backdrivability and offers advantages in manufacturing simplicity, maintenance
ease, and reduced costs.

In an effort to minimize leg mass and concentrate body mass to the hip area,
all motors have been positioned towards the upper thigh and hip regions. This
increases the agility potential of the hardware platform while allowing for
the possibility of approximating the robot dynamics as a rigid body model.
Power is transmitted to the ankle joint via a 2-stage 4-bar linkage system,
while the knee joint is powered through a timing-belt pulley system. The
pulley system additionally serves to amplify the torque with a gear ratio of
1.55:1, thereby satisfying the torque requirements for various tasks without
compromising agility. Therefore, two sets of torque and speed settings are
implemented with HECTOR, both are summarized in Table
LABEL:tab:Joint_Actuator_Specification.

###  2.3 Hybrid Transsmision Systems

While linkage systems are prevalent in robotics due to their rigidity and
simplicity, they are unable to incorporate mechanical advantage without
introducing nonlinearity that complicates control. On the other hand, timing
belts are particularly well-suited for the increasing torque demands of
bipedal and humanoid platforms, such as in Ramos and Kim (2019); Chignoli et
al. (2021). However, they come with their own set of challenges, especially
when it comes to maintaining compactness and robustness over multi-stage power
transmissions across extended distances.

To address this challenge, we’ve adopted a novel hybrid transmission mechanism
that harnesses the strengths of both linkage systems and timing belts,
achieving transmission efficiency, power density, and compactness.
Specifically, our design integrates a two-stage linkage system forming two
parallelograms, transmitting power from the ankle motor to the joint at a gear
ratio of 1:1. Through the control-mechanical co-design process, where the
increased knee torque requirement is observed to ensure dynamical capabilities
according to simulation results in Li and Nguyen (2021, 2023a, 2023b), we have
decided to use a timing belt-pulley system to both transmit and amplify power
from the knee motor to the joint, achieving a gear ratio of 1.55:1. This
design effectively amplifies the knee joint torque to 51.93 Nm, while
retaining the necessary angular speed for dynamic motions.

##  3 System Architecture

In this section, the overall system and control architecture are presented.
Figure 4 shows the block diagram of the system architecture. In a quick
glance, we use a Logitech F710 wireless gamepad to send user commands to the
controllers, which are executed in real time on an external PC. The computed
torque commands generated by the controllers are subsequently sent to the
robot’s motor control boards. We retrieve information about the motor states
and the robot states from motor feedback and state estimation. The
communication between the external PC and the robot is facilitated through an
Ethernet cable.

In detail, the robot states command $\bm{x}^{cmd}$ includes the desired
velocities $\dot{p}_{c,x}^{cmd},\dot{p}_{c,y}^{cmd}$ in the transverse plane
and desired body yaw rate $\psi^{cmd}$, which are then mapped to a reference
trajectory $\bm{x}^{ref}\in\mathbb{R}^{12\times h}$ with $h$ prediction
horizons at each MPC time step for the controller to track. The reference
trajectory at each horizon consists of robot CoM position
$\bm{p}_{c}\in\mathbb{R}^{3}$, Euler angles $\bm{\Theta}=[\phi,\theta,\psi]$,
CoM velocity $\dot{\bm{p}}_{c}\in\mathbb{R}^{3}$, and angular velocity
$\bm{\omega}\in\mathbb{R}^{3}$.

The contact schedule summarizes time-based periodic contact sequences based on
the desired gait period length (typically in the range of 0.3 to 0.7 s for
HECTOR) and loco-manipulation task. This gait sequence determines, at any time
$t$, whether the leg is in stance phase (contact), or swing phase (no
contact). When leg $m$ is in stance phase, $m=1,2$, the ground reaction forces
and moments for leg $m$ are then optimized by the MPC. When the leg is in
swing phase, a Cartesian-space PD controller commands the foot to swing to a
desired location for the next step. In the context of loco-manipulation tasks,
HECTOR’s arms are primarily employed for object manipulation and handling.
Consequently, they are also controlled using a Cartesian PD controller.

Stance foot control inputs $\bm{u}=[\bm{F}_{stance};\bm{M}_{stance}]$, swing
foot forces $\bm{F}_{swing}$, and arm forces $\bm{F}_{arm}$ are then mapped to
corresponding joint torques $\bm{\tau}_{j}$ in a low-level controller by leg
and arm Jacobian. Subsequently, the joint torque commands are sent to the
motor control boards for executing the motion. HECTOR uses a fused linear
state estimator from the readings of onboard IMU and Intel T265 VIO, both of
which are integrated onboard. The sensor fusion allows HECTOR to obtain
accurate and high-frequency robot state feedback.

##  4 Proposed Approach

This section will provide a comprehensive explanation of the proposed
approach, including the dynamics modeling of the HECTOR humanoid robot, the
SRBD assumptions, and the formulation of Force-and-moment-based MPC for
dynamic locomotion and loco-manipulation.

###  4.1 Robot Dynamics Modelling

Figure 5:  Development of Humanoid SRBD in Locomotion and Loco-manipulation.

Firstly, we present the whole-body dynamics formulation of the HECTOR humanoid
robot in the generalized coordinates. The joint-space generalized states
${\mathbf{q}}\in\mathbb{R}^{22}$ include $\bm{p}_{c},\mathbf{\Theta}$, and
$\bm{q}_{j}$.

$\displaystyle\mathbf{H}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})=\bm{\tau}+\bm{J}(\mathbf{q})^{\intercal}\bm{\lambda}$
(1)

where $\mathbf{H}\in\mathbb{R}^{22\times 22}$ is the mass-inertia matrix and
$\mathbf{C}\in\mathbb{R}^{22}$ is the joint-space bias force. $\bm{\tau}$
represents the joint-space actuation. $\bm{\lambda}$ and $\bm{J}$ represent
the external force applied to the system and corresponding Jacobian matrix.

###  4.2 SRBD Model

####  4.2.1 Locomotion SRBD

The motivation behind the utilization of reduced-order models for legged
robots stems from the need for computational efficiency in online control and
planning tasks, particularly in the context of predictive control (e.g., Caron
and Kheddar (2016); Bledt et al. (2017); Xie et al. (2022)). In the prior work
by the authors (Li and Nguyen (2021)), three SRBD models for bipedal robots
were introduced and analyzed. The aim was to identify an appropriate dynamics
model for bipedal robots with 5 Degrees of Freedom (DoF) legs. It is concluded
that 5-DoF leg configuration represented by HECTOR only requires 3-D ground
reaction forces
$[\prescript{}{\mathcal{F}}{F_{x}};\prescript{}{\mathcal{F}}{F_{y}};\prescript{}{\mathcal{F}}{F_{z}}]$
and 2-D ground reaction moments
$[\prescript{}{\mathcal{F}}{M_{y}};\prescript{}{\mathcal{F}}{M_{z}}]$, all in
the foot frame. In MPC, to conveniently optimize for the control input in 3-D
locomotion, we choose to represent the ground reaction forces and moments in
the world frame for leg $m$ as $\bm{F}_{m}=[{F_{x}};F_{y};F_{z}]$ and
$\bm{M}_{m}=[M_{x};M_{y};M_{z}]$.

Illustrated in Figure 5, HECTOR’s Locomotion SRBD encompasses the body,
shoulders, hips, and upper thighs, all treated as a single combined rigid
body. This combined rigid body captures the main dynamic effect of the system.
The design of HECTOR is centered around the assumption of rigid body dynamics,
whereby the mass of the remaining arm and leg components constitutes only
15$\%$ of the total robot mass. As a result, these components’ inertia effects
have been disregarded in the dynamics model. We assume the location of the
ground reaction force and moment is a contact point on each foot, located at
the projection of ankle joint location on the foot along the z-coordinate. The
complete hardware actuation of the contact point, including 3-D linear
movements, roll, and yaw, is fully achieved through the application of 5-D
forces and moments.

####  4.2.2 Loco-manipulation Dynamics

Loco-manipulation and dynamic handling of heavy loads represent significant
challenges in humanoid hardware and control. Typically, the dynamics of
objects are not included in the humanoid dynamics model due to the variability
in dynamics among different objects. Consequently, it becomes essential to
capture the fundamental dynamics of the object to simplify and enable
efficient and straightforward online utilization.

Our goal is to develop a generalized model for both locomotion and loco-
manipulation. We expand the humanoid SRBD model to encompass dynamic object
carrying and loco-manipulation by integrating the object dynamics into the
robot’s dynamics. In the author’s previous work (Li and Nguyen (2023b)), a
comparison was made between two different formalisms of load dynamics within
the SRBD framework:

  * •

Combined Rigid Body Model: Treating the new SRBD as the combined rigid body of
the robot and the object and updating the system dynamics w.r.t. the new
combined CoM $m_{c}$;

  * •

External Force Model: Treating the object dynamics as an external
gravitational force/weight applied to the robot SRBD.

Upon examination, it was observed that the Combined Rigid Body Model exhibited
significant state changes when transitioning from a no-load phase to a loaded
phase. When an object is loaded on the robot, the dynamics model undergoes
significant changes in parameters such as rigid body mass, inertia, CoM
position, and Euler angles. This introduces instability that is particularly
pronounced when dealing with heavy loads.

In this paper, we will delve into the utilization of the External Force Model,
shown in Figure 5, a formalism that relies only on the knowledge of the
object’s mass and the object’s CoM relative to the robot’s CoM. This approach
aims to address the challenges associated with these state changes and enhance
the robot’s stability with heavy loads.

Consequently, the simplified dynamics model for locomotion and loco-
manipulation can be represented as

$\displaystyle
m(\ddot{\bm{p}}_{c}+\bm{g})=m_{o}\bm{g}+\Sigma^{2}_{m=1}\bm{F}_{m}$ (2)
$\displaystyle\frac{d}{dt}(\prescript{}{\mathcal{G}}{\bm{I}}\bm{\omega})=\bm{r}^{o}\times
m_{o}\bm{g}+\Sigma^{2}_{m=1}\\{\bm{r}_{m}^{f}\times\bm{F}_{m}+\bm{M}_{m}\\}$
(3)

where equation (2) represents the force equilibrium in the world frame. The
robot mass is denoted as $m$, the payload mass is denoted as $m_{o}$, and the
gravity vector $\bm{g}$ is $[0;0;g]$. Equation (3) represents the moment
equilibrium of the rigid body dynamics. $\prescript{}{\mathcal{G}}{\bm{I}}$ is
the rigid body moment of inertia (MoI) in the world frame, which is obtained
from the body frame MoI $\prescript{}{\mathcal{B}}{\bm{I}}$ and body rotation
matrix $\mathbf{R}$,
$\prescript{}{\mathcal{G}}{\bm{I}}=\mathbf{R}\prescript{}{\mathcal{B}}{\bm{I}}\mathbf{R}^{T}$.
$\bm{r}^{o}$ represents the distance vector from robot CoM $\bm{p}_{c}$ to
payload CoM $\bm{p}_{o}$ and $\bm{r}^{f}_{m}$ represents the distance vector
from robot CoM to $m$th foot contact point $\bm{p}_{f,m}$.

###  4.3 Force-and-moment-based MPC

In the following sub-section, the proposed Force-and-moment-based MPC control
framework is presented, including the selection of MPC states, assumptions to
linearize the state-space dynamics, MPC formalism, and low-level control
associated with MPC.

SRBD-based Model Predictive Control (MPC) has been studied and demonstrated as
a viable solution for governing dynamic locomotion in quadrupedal robots (e.g.
Kim et al. (2019); Di Carlo et al. (2018)). In the author’s prior work (Li and
Nguyen (2021)), a 2-D formalism of Force-and-Moment-Based MPC was introduced
for bipedal locomotion in the sagittal and coronal planes. In this section, we
present a developed Force-and-Moment-Based MPC framework for both 3-D
locomotion and loco-manipulation.

MPC is a highly sought-after technique for high-level trajectory and footstep
planning. In humanoid control, MPC is frequently combined with high-frequency
WBC or TSC for instantaneous inverse-dynamics calculations. This often results
in the control inputs from MPC having less influence on the system’s
stability. However, in our work, we embrace a novel approach by allowing MPC
to serve as our primary control scheme, fully harnessing the capabilities of
the SRBD for dynamic locomotion and loco-manipulation.

####  4.3.1 State-space Dynamics Linearization

The robot state is defined as
$\bm{x}=[\bm{p}_{c};\bm{\Theta};\dot{\bm{p}}_{c};\bm{\omega}]$, and the
derivative of the Euler angle and body angular velocity have the following
relationship,

$\displaystyle\omega_{x}=\dot{\phi}\sin{\theta}\sin{\psi}+\dot{\theta}\cos{\psi},$
$\displaystyle\omega_{y}=\dot{\phi}\sin{\theta}\cos{\psi}-\dot{\theta}\sin{\psi},$
$\displaystyle\omega_{z}=\dot{\phi}\cos{\theta}+\dot{\psi}.$ (4)

Equivalently,

$\displaystyle\bm{\omega}=\underbrace{\left[\begin{array}[]{ccc}\sin{\theta}\sin{\psi}&\cos{\psi}&0\\\
\sin{\theta}\cos{\psi}&-\sin{\psi}&0\\\
\cos{\theta}&0&1\end{array}\right]}_{\mathbf{S}_{R}}\left[\begin{array}[]{c}\dot{\phi}\\\
\dot{\theta}\\\ \dot{\psi}\end{array}\right].$ (11)

The left-hand side of equation (3) may be approximated the following relation
with the assumption of small angular velocity during locomotion,

$\displaystyle\frac{d}{dt}(\prescript{}{\mathcal{G}}{\bm{I}}\bm{\omega})=\prescript{}{\mathcal{G}}{\bm{I}}\dot{\bm{\omega}}+(\prescript{}{\mathcal{G}}{\bm{I}}\bm{\omega})\approx\prescript{}{\mathcal{G}}{\bm{I}}\dot{\bm{\omega}},$
(12)

With the above assumptions in linearization, the state-space equation of the
rigid body dynamics model in Figure 5 can be written in the following form,

$\displaystyle\allowdisplaybreaks\frac{d}{dt}\left[\begin{array}[]{c}\bm{p}_{c}\\\
\bm{\Theta}\\\ \dot{\bm{p}}_{c}\\\
\bm{\omega}\end{array}\right]=\underbrace{\left[\begin{array}[]{cccc}\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{I}_{3}&\mathbf{0}_{3}\\\
\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{S}_{R}^{-1}\\\
\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\
\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\end{array}\right]}_{\mathbf{A}_{c}}\left[\begin{array}[]{c}\bm{p}_{c}\\\
\bm{\Theta}\\\ \dot{\bm{p}}_{c}\\\ \bm{\omega}\end{array}\right]+$ (25)
$\displaystyle\underbrace{\left[\begin{array}[]{cccc}\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\
\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\\[3.0pt]
\mathbf{I}_{3}\dfrac{\sigma_{1}}{m}&\mathbf{I}_{3}\dfrac{\sigma_{2}}{m}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\\[6.0pt]
\dfrac{\sigma_{1}[\bm{r}^{f}_{1}]_{\times}}{\prescript{}{\mathcal{G}}{\bm{I}}}&\dfrac{\sigma_{2}[\bm{r}^{f}_{2}]_{\times}}{\prescript{}{\mathcal{G}}{\bm{I}}}&\dfrac{\sigma_{1}}{\prescript{}{\mathcal{G}}{\bm{I}}}&\dfrac{\sigma_{2}}{\prescript{}{\mathcal{G}}{\bm{I}}}\end{array}\right]}_{\mathbf{B}_{c}}\underbrace{\left[\begin{array}[]{c}\bm{F}_{1}\\\
\bm{F}_{2}\\\ \bm{M}_{1}\\\
\bm{M}_{2}\end{array}\right]}_{\mathbf{u}}+\underbrace{\left[\begin{array}[]{c}\mathbf{0}_{3\times
1}\\\ \mathbf{0}_{3\times 1}\\\\[3.0pt]
\dfrac{m_{o}\bm{g}-m\bm{g}}{m}\\\\[6.0pt] \dfrac{\sigma_{o}\bm{r}^{o}\times
m_{o}\bm{g}}{\prescript{}{\mathcal{G}}{\bm{I}}}\end{array}\right]}_{\mathbf{C}_{c}}$
(38)

where $\mathbf{I}_{3}$ denotes 3-by-3 identity matrix. Note that the mapping
between the rate of change of Euler angle and the body angular velocity
$\mathbf{S}_{R}$ is not invertible at $\theta=\pm 90^{\circ}$. We do not
intend to allow the robot to have such a configuration with this MPC.
$\sigma_{1}$, $\sigma_{2}$, and $\sigma_{o}$ are Boolean variables that
represent the time-based foot contact schedules and load contact schedules
($1$ for contact, $0$ for non-contact). The incorporation of contact schedules
enables the MPC to possess information about changes in contact modes within
the prediction horizons, illustrated by the contact schedule block in Figure
4. This, in turn, facilitates improved optimization of the current control
inputs, particularly with regard to potential under-actuation scenarios during
contact mode changes.

With the nonlinearity $\mathbf{C}_{c}$ presented in equation (38), we choose
to modify robot state $\bm{x}$ and introduce the new MPC state variable
selections $\mathbf{x}$ to include constant $1$,
$\mathbf{x}=[\bm{p}_{c};\bm{\Theta};\dot{\bm{p}}_{c};\bm{\omega};1]$.
Consequently, the state-space dynamics equation is modified from equation (38)
with continuous matrices $\mathbf{A}_{c}$ and $\mathbf{B}_{c}$ rewritten as

$\displaystyle\frac{d\mathbf{x}}{dt}=\mathbf{A}_{c}(\mathbf{S}_{R},\sigma_{1,2},\sigma_{o},\bm{r}^{o},m_{o}\bm{g})\mathbf{x}+\mathbf{B}_{c}(\sigma_{1,2},\bm{r}^{f}_{1,2})\mathbf{u}$
(39)

and

$\displaystyle\mathbf{A}_{c}=\left[\begin{array}[]{cc}\mathbf{A}_{c}&\mathbf{C}_{c}\\\
\mathbf{0}_{1\times
12}&0\end{array}\right],\>\mathbf{B}_{c}=\left[\begin{array}[]{c}\mathbf{B}_{c}\\\
\mathbf{0}_{12\times 1}\end{array}\right]$ (44)

To utilize this linearized state-space dynamics equation within the MPC
framework, we transform it into a discrete-time representation at the $k$th
time step with MPC step $dt$:

$\displaystyle\mathbf{x}[k+1]=\mathbf{A}_{k}\mathbf{x}[k]+\mathbf{B}_{k}\mathbf{u}[k]$
(45)
$\displaystyle\mathbf{A}=\mathbf{I}_{13}+\mathbf{A}_{c}dt,\quad\mathbf{B}=\mathbf{B}_{c}dt$
(46)

####  4.3.2 MPC Formulation

Now, we can establish the formal optimal control problem definition for the
MPC that employs the linearized SRBD model. The convex MPC problem definition
with a finite horizon $h$ is as follows

$\displaystyle\underset{\mathbf{x},\mathbf{u}}{\operatorname{min}}\>\mathbf{\Sigma}_{k=0}^{h-1}\>\>\underbrace{||\mathbf{x}[k+1]-\mathbf{x}^{ref}[k+1])||^{2}_{\bm{Q}_{k}}}_{J_{1}}+\underbrace{||\mathbf{u}||^{2}_{\bm{R}_{k}}}_{J_{2}}$
(47)

$\displaystyle\>\>\operatorname{s.t.}\quad\quad\mathbf{x}[k+1]=\mathbf{A}_{k}\mathbf{x}[k]+\mathbf{B}_{k}\mathbf{u}[k]$
(48a) $\displaystyle-\mu^{\prime}{F}_{m,z}\leq
F_{m,x}\leq\mu^{\prime}{F}_{m,z}\quad\quad$
$\displaystyle-\mu^{\prime}{F}_{m,z}\leq
F_{m,y}\leq\mu^{\prime}{F}_{m,z}\quad\quad$ (48b) $\displaystyle
0<{F}_{min}\leq F_{m,z}\leq{F}_{max}\quad\quad$ (48c)
$\displaystyle[1,0,0]\cdot\mathbf{R}_{f,m}^{T}\bm{M}_{m}=\mathbf{0}$ (48d)
$\displaystyle[0,0,-l_{t}]\cdot\mathbf{R}_{f,m}^{T}\bm{F}_{m}\leq[0,1,0]\cdot\mathbf{R}_{f,m}^{T}\bm{M}_{m}$
$\displaystyle\quad\quad\leq[0,0,l_{h}]\cdot\mathbf{R}_{f,m}^{T}\bm{F}_{m}$
(48e)
$\displaystyle[0,-l_{t},-\mu^{\prime}l_{t}]\cdot\mathbf{R}_{f,m}^{T}\bm{F}_{m}+[0,-\mu^{\prime},\>\>\>1]\cdot\mathbf{R}_{f,m}^{T}\bm{M}_{m}\leq\mathbf{0}$
$\displaystyle[0,-l_{t},\>\>\>\>\mu^{\prime}l_{t}]\cdot\mathbf{R}_{f,m}^{T}\bm{F}_{m}+[0,\>\>\>\mu^{\prime},\>\>\>1]\cdot\mathbf{R}_{f,m}^{T}\bm{M}_{m}\leq\mathbf{0}$
$\displaystyle[0,-l_{h},-\mu^{\prime}l_{h}]\cdot\mathbf{R}_{f,m}^{T}\bm{F}_{m}+[0,\>\>\>\mu^{\prime},-1]\cdot\mathbf{R}_{f,m}^{T}\bm{M}_{m}\leq\mathbf{0}$
$\displaystyle[0,-l_{h},\>\>\>\mu^{\prime}l_{h}]\cdot\mathbf{R}_{f,m}^{T}\bm{F}_{m}+[0,-\mu^{\prime},-1]\cdot\mathbf{R}_{f,m}^{T}\bm{M}_{m}\leq\mathbf{0}$
(48f)

Equation (47) describes the objectives of the convex MPC as a quadratic cost
function. $J_{1}$ represents the cost of driving states to the desired
trajectory based on the user commands. $J_{2}$ represents the cost of
minimizing the control input to achieve better energy efficiency for
locomotion. Both of the costs are weighted by the diagonal matrices
$\bm{Q}_{k}$ and $\bm{R}_{k}$ at $k$th time step.

The optimal control problem is subjected to several constraints. Equation
(48a) represents the discrete dynamics constraints derived from equation
(38-46). Equation (48b) describes the contact point friction constraint, which
follows an inscribed friction pyramid approximation,
$\mu^{\prime}=\sqrt{2}\mu/2$, for more conservative lateral foot forces, where
$\mu$ is the contact friction coefficient of the ground.

Equation (48d-48f) represents the line-foot dynamics constraints, where
$\mathbf{R}_{f,m}^{T}$ denotes the foot rotation matrix w.r.t the world frame.
Given the inherent challenges associated with modeling flat humanoid foot
contact for humanoid robots (e.g., Hauser (2014); Caron et al. (2015)), we
have made the decision to simplify the representation of the foot contact on
HECTOR to a line foot contact, which is also a common assumption on humanoid
with 5-DoF legs (e.g., Ahn et al. (2021); Ding et al. (2022)). We modified the
Contact Wrench Cone (CWC) introduced in Caron et al. (2015) to enforce:

  * •

The x-direction ground reaction moment is zero, shown in equation (48d),

  * •

Prevention of toe/heel lift due to the y-direction ground reaction moment,
shown in equation (48e),

  * •

Friction constraints for y-direction reaction forces at line-foot heel and toe
locations, shown in equation (48f).

####  4.3.3 Condensing MPC-QP Problem for Real-time Computation

Despite the extended prediction horizon embedded in the proposed MPC,
resulting in large matrix sizes that demand significant computational
resources for online optimal control problems, we can still optimize the
convex MPC formulation to ensure efficient solutions as a Quadratic
Programming (QP) optimization (e.g. Wang and Boyd (2009); Jerez et al.
(2011)).

In a standard non-condense MPC-QP problem definition, the augmented state
$\mathbf{X}=\bigr{[}\mathbf{x}[1]\dots\mathbf{x}[h];\mathbf{u}[0]\dots\mathbf{u}[h-1]\bigr{]}$
consists of both states $\mathbf{x}$ and decision variables $\mathbf{u}$. A QP
problem definition is,

$\displaystyle\underset{\mathbf{X}}{\operatorname{min}}\>\frac{1}{2}\mathbf{X}^{T}\mathbf{H}\mathbf{X}+\mathbf{f}^{T}\mathbf{X}$
(49)
$\displaystyle\allowdisplaybreaks\>\>\operatorname{s.t.}\quad\mathbf{A}_{eq}\mathbf{X}=\mathbf{b}_{eq}$
(50) $\displaystyle\quad\quad\quad\>\mathbf{C}\mathbf{X}<\mathbf{d}$ (51)

We can express the problem (eqn. (47)) with dynamics constraints (eqn. (48a))
in terms of the above standard non-condensed QP form according to Wright
(1996), where

$\displaystyle\allowdisplaybreaks\mathbf{H}=\text{blkdiag}(\\{\bm{Q}_{i}\\}^{h-1}_{i=0},\\{\bm{R}_{i}\\}^{h-1}_{i=0}),$
(52)
$\displaystyle\mathbf{f}=-\bigr{[}\\{{\bm{Q}_{i}\mathbf{x}^{ref}_{i}}^{T}\\}^{h-1}_{i=0},\\{{\mathbf{0}_{12\times
1}}\\}^{h-1}_{j=0}\bigr{]}^{T};$ (53)

And

$\displaystyle\mathbf{A}_{eq}=\left[\begin{array}[]{cccccccc}\mathbf{I}&&&&-\mathbf{B}_{0}&&&\\\
-\mathbf{A}_{1}&\ddots&&&&\ddots&&\\\ &\ddots&\ddots&&&&\ddots&\\\
&&-\mathbf{A}_{h-1}&\mathbf{I}&&&&-\mathbf{B}_{h-1}\end{array}\right],$ (58)
$\displaystyle\mathbf{b}_{eq}=\bigr{[}\mathbf{A}_{0}\mathbf{X}[0],\mathbf{0},\dots,\mathbf{0}\bigr{]}^{T}.$
(59)

Table 3: Average MPC Total Solve Time∗ Comparisons

Solver$\backslash$Approach | Non-condensed | Condensed
---|---|---
qpOASES (C++) |  |
Standing, $\mathrm{ms}$ | 19.9 | 3.2
Walking, $\mathrm{ms}$ | 12.9 | 1.3
quadprog (MATLAB) |  |
Standing, $\mathrm{ms}$ | 432.5 | 99.3
Walking, $\mathrm{ms}$ | 367.2 | 24.9

∗PC with AMD Ryzen 5600X @ 4.2 GHz on Ubuntu 20.04

However, due to the considerably large size of the equality constraint matrix
$\mathbf{A}_{eq}\in\mathbb{R}^{13h\times
25h},\mathbf{b}_{eq}\in\mathbb{R}^{13h}$, it is computationally expensive to
solve for the solutions online. In addition, the decision variables we need
for actuating the humanoid robot are only the current step control inputs
$\mathbf{u}[0]$. Therefore, the resulting full-state calculation from the non-
condensed approach can be unnecessary. Given that the state-space matrices
$\mathbf{A}_{d}$ and $\mathbf{B}_{d}$ are time-varying and periodic, it is
reasonable to apply a condensed and sparse QP formulation to solve the MPC
problem according to Jerez et al. (2011). A comparison table of the MPC solve
time between non-condensed and condensed formulations on HECTOR hardware is
provided in Table LABEL:tab:solveTime. The condensed approach for solving the
MPC-QP problem allows us to run the hardware MPC very efficiently online with
up to 1.5$\mathrm{s}$ prediction horizon and hyper-sample it up to
300$\mathrm{Hz}$.

(a)

(b)

Figure 6:  HECTOR loco-manipulation simulation snapshots: (a) Multi-contact
loco-manipulation control including 1) Transferring a 4kg package and 2)
dynamically throwing a 2kg ball while staying balanced; (b) Comparison of
approaches in 90-degree turn package transferring.

(a)

(b)

Figure 7:  HECTOR loco-manipulation with time-varying load: (a) Simulation
snapshots; (b) Joint torque plots.

We apply the following formula to translate the $\mathbf{H}$ and $\mathbf{f}$
matrices from equation (52-53) to a condense and sparse QP form to solve
efficiently,

$\displaystyle\mathbf{H}^{cs}=2({\mathbf{B}_{qp}}^{T}\mathbf{Q}{\mathbf{B}_{qp}}+\mathbf{R}),$
(60)
$\displaystyle\mathbf{f}^{cs}=2{\mathbf{B}_{qp}}^{T}\mathbf{Q}({\mathbf{A}_{qp}}\mathbf{x}[0]-\mathbf{x}_{qp}).$
(61)

where $\mathbf{x}_{qp},\>\mathbf{Q}$, and $\mathbf{R}$ are all vertical
concatenations of corresponding matrices from MPC time step 0 (current
instance) to $h-1$. And,

$\displaystyle\allowdisplaybreaks\mathbf{A}_{qp}=[\mathbf{A}_{0},\>\mathbf{A}_{0}\mathbf{A}_{1},\>\dots\>,\mathbf{A}_{0}\mathbf{A}_{1}\dots\mathbf{A}_{h-1}]^{T},$
(62)
$\displaystyle\mathbf{B}_{qp}=\left[\begin{array}[]{cccccc}\mathbf{I}&&&&&\\\\[2.0pt]
\mathbf{A}_{1}&\mathbf{I}&&&&\\\\[2.0pt]
\vdots&\mathbf{A}_{2}&\ddots&&&\\\\[2.0pt]
\mathbf{A}_{h-3}^{h-3}&\vdots&\ddots&\ddots&&\\\\[4.0pt]
\mathbf{A}_{h-2}^{h-2}&\mathbf{A}_{h-1}^{h-3}&\dots&\mathbf{A}_{h-2}&\mathbf{I}&\\\\[4.0pt]
\mathbf{A}_{h-1}^{h-1}&\mathbf{A}_{h-1}^{h-2}&\mathbf{A}_{h-1}^{h-3}&\dots&\mathbf{A}_{h-1}&\mathbf{I}\end{array}\right]\cdot\mathbf{B}^{blkdiag}_{0\rightarrow
h-1}$ (69)

###  4.4 Low-level Control

In contrast to many other strategies for bipedal locomotion control, which
employ a low-level WBC with task-space commands as a high-frequency balancing
controller to manage whole-body dynamics, we thoroughly investigate the
effectiveness of Force-and-moment-based MPC as the primary locomotion
controller. In our approach, we directly utilize the control input from MPC
and map it to joint torques using contact Jacobian, with the assumption of the
SRBD model and negligible leg dynamics. Hence, the $m$th leg joint torque
$\bm{\tau}_{m}$ commands are

$\displaystyle\bm{\tau}_{m}=\left[\begin{array}[]{c}\bm{J}_{v}\\\
\bm{J}_{\omega}\end{array}\right]^{T}\left[\begin{array}[]{c}\mathbf{R}_{f,m}^{T}(\bm{F}_{m}+\bm{F}_{swing,m})\\\
\mathbf{R}_{f,m}^{T}\bm{M}_{m}\end{array}\right],$ (74)

where $\bm{J}_{v}\in\mathbb{R}^{3\times 5}$ and
$\bm{J}_{\omega}\in\mathbb{R}^{3\times 5}$ are linear and angular velocity
parts of foot contact point Jacobian. Note that swing foot force
$\bm{F}_{swing,m}$ and stance foot force cannot be both nonzero at any
instance, and the foot is either under swing or stance phase according to the
gait scheduler. The swing force is acquired by Cartesian-space PD control law
driving the swing foot to the desired heuristic foot position
$\bm{p}_{m}^{des}$ in Raibert (1986) and Kim et al. (2019) and velocity
$\dot{\bm{p}}_{m}^{des}$,

$\displaystyle\bm{p}_{m}^{des}=\bm{p}_{c}+\frac{\dot{\bm{p}}_{c}\Delta
t}{2}+k_{c}(\dot{\bm{p}}_{c}-\dot{\bm{p}}_{c}^{cmd}),$ (75)
$\displaystyle\dot{\bm{p}}_{m}^{des}=2\dot{\bm{p}}_{c}$ (76)

$\Delta t$ denotes the time duration of each foot swing and scalar $k_{c}$
represents the velocity feedback gain to achieve the desired walking speed.

While holding objects with arms, the upper body joint commands are also
calculated by the Cartesian space control and contact (hand) Jacobian with
stiff PD gains.

##  5 Results

In this section, we will present the results of both numerical and hardware
experiments to validate our proposed Force-and-moment-based MPC approach for
dynamic humanoid control. We strongly encourage readers to view the full
result videos111https://youtu.be/-r0QoxQgshk that accompany this paper for
visual aid and a comprehensive understanding of our work.

(a)

(b)

(c)

Figure 8:  HECTOR uneven terrain locomotion experiment snapshots: (a) Walking
over stacked wood slats with 0.6 m/s forward speed; (b) Walking over random
wood slats; (c) Walking over stacked wood slats in humanoid form. Note all
wood slat terrains are unstable.

(a)

(b)

Figure 9: Dynamic turning experiment: (a) Turning snapshots in counter-
clockwise direction; (b) Yaw rate tracking plot

###  5.1 Simulation Results on Dynamic Loco-manipulation

Firstly, we present the numerical simulation results on dynamic and contact-
informed loco-manipulation control on HECTOR. The custom simulation frameworks
of HECTOR are made available as an open-source
project222https://github.com/DRCL-USC/Hector_Simulation. In HECTOR’s Simulink
simulation, our controllers are written in MATLAB scripts. For more efficient
computation, we use qpOASES (Ferreau et al. (2014)) in CasADi toolbox
(Andersson et al. (2019)) to solve the MPC problems online. In HECTOR’s
ROS+Gazebo Simulation, the MPC, state estimation, and low-level control
scripts are written in C++. MPC problem is solved through qpOASES C++
interface.

To demonstrate the effectiveness of the proposed control scheme in loco-
manipulation and the importance of contact schedule in MPC, in Simulink
simulation, we have allowed the robot to perform multi-contact tasks with
heavy objects, shown in Figure 6(a). The robot can stay well-balanced during
stepping even throwing a $2\>\mathrm{kg}$ ball.

In addition, in a logistics setting, we commanded the robot to transfer a
$2\>\mathrm{kg}$ package from a table to the conveyor belt with in-place
walking and turning, illustrated in Figure 6(b). Compared to the traditional
quasi-static approach, where the robot has to perform turning, picking up, and
dropping off separately, our proposed dynamic loco-manipulation control can
allow the robot to synchronize the locomotion and manipulation tasks
altogether, allowing more dynamic and efficient package transferring.

Furthermore, to further demonstrate the effectiveness of the loco-manipulation
SRBD and contact schedule in the proposed MPC, we have leveraged the
convenience of the simulation framework for time-varying load. The robot can
walk while carrying a time-varying load with mass in the range of
$[0.5\>\>4.0]\>\mathrm{kg}$, shown in Figure 7(a). The associated torque plots
are shown in Figure 7(b).

(a)

(b)

Figure 10:  External disturbance rejection in stance: (a) Experiment
snapshots; (b) Robot CoM location plots.  Figure 11: Joint torque plots under
8 kg load in stance.

###  5.2 Experimental Results

We have also demonstrated the feasibility of our proposed Force-and-moment-
based MPC approach in hardware experiments with the HECTOR mini-humanoid
robot.

####  5.2.1 System Integration:

Effective state estimation is crucial for achieving accurate and dynamic
locomotion in robotics. we address this by integrating a 6-axis IMU in the
trunk, positioned near the robot’s center of mass (COM) for precise
measurements. To estimate the COM states, a linear Kalman filter processes the
IMU data and leg kinematics at a high refresh rate of 1 kHz. To enhance the
robustness of state estimation, an Intel Realsense T265 tracking camera is
also located near the COM. This camera, through its onboard VIO algorithm,
outputs the position and orientation of the body at a slower 200 Hz rate. The
system ensures accurate and robust state estimation for the HECTOR platform by
fusing the data from both sources. Additionally, for potential future
applications, HECTOR is equipped with an Intel Realsense D435i camera mounted
on top, designed to capture depth data and other relevant environmental
information. However, for this work, the robot is unaware of the terrain data.

(a)

(b)

Figure 12:  Plots of uneven terrain locomotion experiment results with a
forward speed of 0.6 m/s: (a) Robot CoM plots; (b) MPC forces and moments
solutions.

####  5.2.2 Control Tuning:

Control objective weight/gain tuning on HECTOR hardware is very minimal to
achieve the presented results. The generalized MPC weights used across the
tasks are

$\displaystyle\bm{Q}_{k}=\text{diag}[500\>500\>500\>150\>150\>150\>1\>1\>3\>1\>1\>1\>0],\>$
$\displaystyle\bm{R}_{k}=\text{diag}[1\>1\>1\>1\>1\>1\>5\>5\>5\>5\>5\>5\>]\times
10^{-3}.\quad\quad\quad\>\>\>$

####  5.2.3 Dynamical Turning:

Firstly, we demonstrate the dynamic turning experiments on HECTOR. Figure 9
shows the yaw rate tracking of the robot, in the range of $[-1.75\>\>2]$
$\mathrm{rad/s}$. Figure 1(e) showcases the robot’s turning capability under
the proposed MPC while carrying a 2.5 $\mathrm{kg}$ payload.

####  5.2.4 Handling Heavy Load:

To validate the loco-manipulation and load-carrying capabilities of the
proposed MPC framework and showcase the power density of HECTOR, we
demonstrate the robot’s load-carrying ability in both double-leg standing and
stepping scenarios. Figure 1(b) and 1(c) shows experiment snapshots of
standing while carrying $8\>\mathrm{kg}$ load ($50\%$ robot mass), and
stepping while carrying $4\>\mathrm{kg}$ load. The joint torques associated
with the experiments carrying $8\>\mathrm{kg}$ during stance are shown in
Figure 11. Note that the joint torques in the load-carrying experiment are
well under the peak torque limit of the robot joints.

(a) MPC Forces and Moments

(b) Stance Joint Torques by MPC Forces and Moments

Figure 13:  Plots of uneven terrain loco-manipulation experiment results with
2.5 kg payload: (a) MPC forces and moments solutions. (b) Stance leg joint
torques by MPC forces and moments solutions.

####  5.2.5 Disturbance Rejection in Balancing:

The proposed Force-and-moment-based MPC approach also demonstrates robustness
in recovering from external perturbations including unknown external forces
and unknown terrain changes during standing configuration. Figure 1(d) shows
the snapshots of HECTOR’s robust balancing capability in stance while
constantly moving its feet on a seesaw. Figure 10 shows the experimental
results of balancing while applying unknown forces and impulses to HECTOR. It
can be observed that HECTOR can recover quickly to the desired CoM position
after external forces and impulses are applied.

####  5.2.6 Robust Multi-terrain Locomotion:

The locomotion ability of HECTOR robot with Force-and-moment-based MPC is also
promising. We have demonstrated uneven terrain locomotion on HECTOR hardware
in both biped and humanoid forms. Figure 8 presents experiment snapshots of
uneven terrain locomotion with different terrain setups, including

  * •

Stacked wood slats up to $6\>\mathrm{cm}$, shown in Figure 8(a) and 8(c);

  * •

Randomly placed wood slats, shown in Figure 8(b);

  * •

Randomly placed wood slats on wet grassy terrain, shown in Figure 1(g);

  * •

18∘ slope, shown in Figure 1(f).

Our MPC control scheme on HECTOR can confidently traverse the above terrain
setups. Figure 12 shows the CoM location and MPC forces and moments (in world
frame) plots of the HECTOR walking over stacked wood slats with
$0.6\>\mathrm{m/s}$ forward speed (illustrated in snapshots in Figure 8(a)).
The wood slats have heights of $2\>\mathrm{cm}$, $4\>\mathrm{cm}$, and
$6\>\mathrm{cm}$.

####  5.2.7 Dynamic Loco-manipulation

Furthermore, we have achieved dynamic loco-manipulation on HECTOR humanoid
robot. Figure 1(e) shows the snapshots of HECTOR turning dynamically while
carrying 2.5 $\mathrm{kg}$ payload. Figure 1(a) shows the snapshots of HECTOR
walking over randomly placed and unstable wood slats with a 2.5 $\mathrm{kg}$
payload (composed of 2 Unitree Go1 batteries). The dynamic loco-manipulation
MPC solutions (in world frame) and associated joint torques are shown in
Figure 13. Note that all force solutions comply with the force limit set in
MPC (250 $\mathrm{N}$) and the associated joint torques are well under the
joint torque limits of the hardware.

##  6 Future Work

Having outlined the primary experimental validations for the proposed control
schemes, we aim to highlight future research directions to enhance autonomous
loco-manipulation.

Currently, the controller requires manual inputs of the CoM and mass
properties of the carried object. However, we are exploring a more autonomous
approach involving vision-based object shape detection and object state
estimation to determine the object’s CoM position.

Additionally, we intend to estimate and sense the object’s weight
autonomously, which is particularly important in the problem of carrying an
unknown and time-varying load. The first future integration is by using the
upper-limb motors. The back-drivability feature of these QDD motors plays a
vital role here, allowing us to sense the object’s weight through the back-
drive current/torque of these motors and the kinematic pose of the upper limb.
The second future research direction to tackle this problem revolves around
using L1 adaptive control with MPC for compensating the unknown load.
Successful integration and implementation have been seen on quadrupedal
robots, such as in Sombolestan and Nguyen (2023).

##  7 Conclusions

In summary, this work presents an effective approach to dynamic locomotion and
loco-manipulation control on humanoid robots through a generalized Force-and-
Moment-Based Model Predictive Control (MPC). Our proposed framework involves a
simplified rigid body dynamics (SRBD) model, accounting for both humanoid and
object dynamics in loco-manipulation. we have demonstrated the effectiveness
of the proposed control framework on HECTOR—a dynamic, small-scale, power-
dense, and cost-effective humanoid robot platform. HECTOR demonstrates
exceptional balance in double-leg stance, even under external force
disturbances, and executes dynamic walking across varied terrains at speeds up
to 0.6 m/s. Furthermore, we achieve dynamic loco-manipulation over uneven
terrain, carrying a 2.5 kg load. HECTOR simulation framework and controllers
are made available as an open-source project on GitHub.

##  8 Acknowledgment

The authors would like to thank Yiyu Chen, Zhanhao Le, and Han Gong for
contributing to the HECTOR open-source software.

##  9 Funding

This work is supported by the USC departmental startup fund.

## References

  * Abate (2018) Abate AM (2018) Mechanical design for robot locomotion .
  * Agravante et al. (2019) Agravante DJ, Cherubini A, Sherikov A, Wieber PB and Kheddar A (2019) Human-humanoid collaborative carrying.  _IEEE Transactions on Robotics_ 35(4): 833–846.
  * Ahn et al. (2021) Ahn J, Jorgensen SJ, Bang SH and Sentis L (2021) Versatile locomotion planning and control for humanoid robots.  _Frontiers in Robotics and AI_ : 257.
  * Andersson et al. (2019) Andersson JAE, Gillis J, Horn G, Rawlings JB and Diehl M (2019) CasADi – A software framework for nonlinear optimization and optimal control.  _Mathematical Programming Computation_ 11(1): 1–36.  10.1007/s12532-018-0139-4.
  * Audren et al. (2014) Audren H, Vaillant J, Kheddar A, Escande A, Kaneko K and Yoshida E (2014) Model preview control in multi-contact motion-application to a humanoid robot.  In: _2014 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, pp. 4030–4035.
  * Bellegarda et al. (2022) Bellegarda G, Chen Y, Liu Z and Nguyen Q (2022) Robust high-speed running for quadruped robots via deep reinforcement learning.  In: _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 10364–10370.
  * Bledt et al. (2018) Bledt G, Powell MJ, Katz B, Di Carlo J, Wensing PM and Kim S (2018) Mit cheetah 3: Design and control of a robust, dynamic quadruped robot.  In: _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 2245–2252.
  * Bledt et al. (2017) Bledt G, Wensing PM and Kim S (2017) Policy-regularized model predictive control to stabilize diverse quadrupedal gaits for the mit cheetah.  In: _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 4102–4109.
  * Brasseur et al. (2015) Brasseur C, Sherikov A, Collette C, Dimitrov D and Wieber PB (2015) A robust linear mpc approach to online generation of 3d biped walking motion.  In: _2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids)_. IEEE, pp. 595–601.
  * Caron and Kheddar (2016) Caron S and Kheddar A (2016) Multi-contact walking pattern generation based on model preview control of 3d com accelerations.  In: _2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)_. IEEE, pp. 550–557.
  * Caron et al. (2015) Caron S, Pham QC and Nakamura Y (2015) Stability of surface contacts for humanoid robots: Closed-form formulae of the contact wrench cone for rectangular support areas.  In: _2015 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 5107–5112.
  * Cheng et al. (2023) Cheng X, Shi K, Agarwal A and Pathak D (2023) Extreme parkour with legged robots.  _arXiv preprint arXiv:2309.14341_ .
  * Chignoli et al. (2021) Chignoli M, Kim D, Stanger-Jones E and Kim S (2021) The mit humanoid robot: Design, motion planning, and control for acrobatic behaviors.  In: _2020 IEEE-RAS 20th International Conference on Humanoid Robots (Humanoids)_. IEEE, pp. 1–8.
  * Daneshmand et al. (2021) Daneshmand E, Khadiv M, Grimminger F and Righetti L (2021) Variable horizon mpc with swing foot dynamics for bipedal walking control.  _IEEE Robotics and Automation Letters_ 6(2): 2349–2356.
  * Di Carlo et al. (2018) Di Carlo J, Wensing PM, Katz B, Bledt G and Kim S (2018) Dynamic locomotion in the mit cheetah 3 through convex model-predictive control.  In: _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 1–9.
  * Ding et al. (2022) Ding Y, Khazoom C, Chignoli M and Kim S (2022) Orientation-aware model predictive control with footstep adaptation for dynamic humanoid walking.  In: _2022 IEEE-RAS 21st International Conference on Humanoid Robots (Humanoids)_. IEEE, pp. 299–305.
  * Englsberger et al. (2014) Englsberger J, Werner A, Ott C, Henze B, Roa MA, Garofalo G, Burger R, Beyer A, Eiberger O, Schmid K and Albu-Schäffer A (2014) Overview of the torque-controlled humanoid robot toro.  In: _2014 IEEE-RAS International Conference on Humanoid Robots_. IEEE, pp. 916–923.
  * Ferreau et al. (2014) Ferreau HJ, Kirches C, Potschka A, Bock HG and Diehl M (2014) qpoases: A parametric active-set algorithm for quadratic programming.  _Mathematical Programming Computation_ 6: 327–363.
  * Hauser (2014) Hauser K (2014) Fast interpolation and time-optimization with contact.  _The International Journal of Robotics Research_ 33(9): 1231–1250.
  * Hereid et al. (2014) Hereid A, Kolathaya S, Jones MS, Van Why J, Hurst JW and Ames AD (2014) Dynamic multi-domain bipedal walking with atrias through slip based human-inspired control.  In: _Proceedings of the 17th international conference on Hybrid systems: computation and control_. pp. 263–272.
  * Herzog et al. (2016) Herzog A, Rotella N, Mason S, Grimminger F, Schaal S and Righetti L (2016) Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid.  _Autonomous Robots_ 40: 473–491.
  * Hoeller et al. (2023) Hoeller D, Rudin N, Sako D and Hutter M (2023) Anymal parkour: Learning agile navigation for quadrupedal robots.  _arXiv preprint arXiv:2306.14874_ .
  * Jerez et al. (2011) Jerez JL, Kerrigan EC and Constantinides GA (2011) A condensed and sparse QP formulation for predictive control.  In: _2011 50th IEEE Conference on Decision and Control and European Control Conference_. IEEE, pp. 5217–5222.
  * Kajita et al. (2003) Kajita S, Kanehiro F, Kaneko K, Fujiwara K, Harada K, Yokoi K and Hirukawa H (2003) Biped walking pattern generation by using preview control of zero-moment point.  In: _2003 IEEE international conference on robotics and automation (Cat. No. 03CH37422)_ , volume 2. IEEE, pp. 1620–1626.
  * Katz et al. (2019) Katz B, Di Carlo J and Kim S (2019) Mini cheetah: A platform for pushing the limits of dynamic quadruped control.  In: _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 6295–6301.
  * Kim et al. (2019) Kim D, Di Carlo J, Katz B, Bledt G and Kim S (2019) Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control.  _arXiv preprint arXiv:1909.06586_ .
  * Kim et al. (2020) Kim D, Jorgensen SJ, Lee J, Ahn J, Luo J and Sentis L (2020) Dynamic locomotion for passive-ankle biped robots and humanoids using whole-body locomotion control.  _The International Journal of Robotics Research_ 39(8): 936–956.
  * Lee et al. (2016) Lee Y, Hwang S and Park J (2016) Balancing of humanoid robot using contact force/moment control by task-oriented whole body control framework.  _Autonomous Robots_ 40: 457–472.
  * Li and Nguyen (2021) Li J and Nguyen Q (2021) Force-and-moment-based model predictive control for achieving highly dynamic locomotion on bipedal robots.  In: _2021 60th IEEE Conference on Decision and Control (CDC)_. IEEE, pp. 1024–1030.
  * Li and Nguyen (2023a) Li J and Nguyen Q (2023a) Dynamic walking of bipedal robots on uneven stepping stones via adaptive-frequency MPC.  _IEEE Control Systems Letters_ 7: 1279–1284.  10.1109/LCSYS.2023.3234769.
  * Li and Nguyen (2023b) Li J and Nguyen Q (2023b) Multi-contact mpc for dynamic loco-manipulation on humanoid robots.  In: _2023 American Control Conference (ACC)_. IEEE, pp. 1215–1220.
  * Liu et al. (2022) Liu Y, Shen J, Zhang J, Zhang X, Zhu T and Hong D (2022) Design and control of a miniature bipedal robot with proprioceptive actuation for dynamic behaviors.  In: _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 8547–8553.
  * Murooka et al. (2021) Murooka M, Kumagai I, Morisawa M, Kanehiro F and Kheddar A (2021) Humanoid loco-manipulation planning based on graph search and reachability maps.  _IEEE Robotics and Automation Letters_ 6(2): 1840–1847.
  * Nguyen et al. (2018) Nguyen Q, Agrawal A, Martin W, Geyer H and Sreenath K (2018) Dynamic bipedal locomotion over stochastic discrete terrain.  _The International Journal of Robotics Research_ 37(13-14): 1537–1553.
  * Park et al. (2005) Park IW, Kim JY, Lee J and Oh JH (2005) Mechanical design of humanoid robot platform khr-3 (kaist humanoid robot 3: Hubo).  In: _5th IEEE-RAS International Conference on Humanoid Robots, 2005._ pp. 321–326.  10.1109/ICHR.2005.1573587.
  * Raibert (1986) Raibert MH (1986) _Legged robots that balance_.  MIT press.
  * Ramos et al. (2018) Ramos J, Katz B, Chuah MYM and Kim S (2018) Facilitating model-based control through software-hardware co-design.  In: _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 566–572.
  * Ramos and Kim (2019) Ramos J and Kim S (2019) Dynamic locomotion synchronization of bipedal robot and human operator via bilateral feedback teleoperation.  _Science Robotics_ 4(35): eaav4282.
  * Reher et al. (2020) Reher J, Kann C and Ames AD (2020) An inverse dynamics approach to control lyapunov functions.  In: _2020 American Control Conference (ACC)_. IEEE, pp. 2444–2451.
  * Rigo et al. (2023) Rigo A, Hu M, Gupta SK and Nguyen Q (2023) Hierarchical optimization-based control for whole-body loco-manipulation of heavy objects.  _arXiv preprint arXiv:2311.00112_ .
  * Sakagami et al. (2002) Sakagami Y, Watanabe R, Aoyama C, Matsunaga S, Higaki N and FujiMura K (2002) The intelligent asimo: System overview and integration.  ISBN 0-7803-7398-7, pp. 2478 – 2483 vol.3.  10.1109/IRDS.2002.1041641.
  * Scianca et al. (2016) Scianca N, Cognetti M, De Simone D, Lanari L and Oriolo G (2016) Intrinsically stable mpc for humanoid gait generation.  In: _2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)_. IEEE, pp. 601–606.
  * Scianca et al. (2020) Scianca N, De Simone D, Lanari L and Oriolo G (2020) Mpc for humanoid gait generation: Stability and feasibility.  _IEEE Transactions on Robotics_ 36(4): 1171–1188.
  * Sentis et al. (2010) Sentis L, Park J and Khatib O (2010) Compliant control of multicontact and center-of-mass behaviors in humanoid robots.  _IEEE Transactions on robotics_ 26(3): 483–501.
  * Seok et al. (2015) Seok S, Wang A, Chuah MY, Hyun DJ, Lee J, Otten DM, Lang JH and Kim S (2015) Design principles for energy-efficient legged locomotion and implementation on the mit cheetah robot.  _IEEE/ASME Transactions on Mechatronics_ 20(3): 1117–1129.  10.1109/TMECH.2014.2339013.
  * Sim and Ramos (2022) Sim Y and Ramos J (2022) Tello leg: The study of design principles and metrics for dynamic humanoid robots.  _IEEE Robotics and Automation Letters_ 7(4): 9318–9325.
  * Sleiman et al. (2021) Sleiman JP, Farshidian F, Minniti MV and Hutter M (2021) A unified mpc framework for whole-body dynamic locomotion and manipulation.  _IEEE Robotics and Automation Letters_ 6(3): 4688–4695.
  * Sombolestan and Nguyen (2023) Sombolestan M and Nguyen Q (2023) Adaptive force-based control of dynamic legged locomotion over uneven terrain.  _arXiv preprint arXiv:2307.04030_ .
  * Sreenath et al. (2011) Sreenath K, Park HW, Poulakakis I and Grizzle JW (2011) A compliant hybrid zero dynamics controller for stable, efficient and fast bipedal walking on mabel.  _The International Journal of Robotics Research_ 30(9): 1170–1193.
  * Tsagarakis et al. (2017) Tsagarakis NG, Caldwell DG, Negrello F, Choi W, Baccelliere L, Loc V, Noorden J, Muratore L, Margan A, Cardellino A, Natale L, Mingo Hoffman E, Dallali H, Kashiri N, Malzahn J, Lee J, Kryczka P, Kanoulas D, Garabini M, Catalano M, Ferrati M, Varricchio V, Pallottino L, Pavan C, Bicchi A, Settimi A, Rocchi A and Ajoudani A (2017) Walk-man: A high-performance humanoid platform for realistic environments.  _Journal of Field Robotics_ 34(7): 1225–1259.
  * Wang and Boyd (2009) Wang Y and Boyd S (2009) Fast model predictive control using online optimization.  _IEEE Transactions on control systems technology_ 18(2): 267–278.
  * Wensing and Orin (2013) Wensing PM and Orin DE (2013) Generation of dynamic humanoid behaviors through task-space control with conic optimization.  In: _2013 IEEE International Conference on Robotics and Automation_. IEEE, pp. 3103–3109.
  * Westervelt et al. (2003) Westervelt ER, Grizzle JW and Koditschek DE (2003) Hybrid zero dynamics of planar biped walkers.  _IEEE transactions on automatic control_ 48(1): 42–56.
  * Wright (1996) Wright SJ (1996) Applying new optimization algorithms to more predictive control.  Technical report, Argonne National Lab.(ANL), Argonne, IL (United States).
  * Xie et al. (2022) Xie Z, Da X, Babich B, Garg A and de Panne Mv (2022) Glide: Generalizable quadrupedal locomotion in diverse environments with a centroidal model.  In: _International Workshop on the Algorithmic Foundations of Robotics_. Springer, pp. 523–539.
  * Xiong and Ames (2018) Xiong X and Ames AD (2018) Bipedal hopping: Reduced-order model embedding via optimization-based control.  In: _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 3821–3828.