of 7
Nonlinear Model Predictive Control of a 3D Hopping Robot:
Leveraging Lie Group Integrators for Dynamically Stable Behaviors
Noel Csomay-Shanklin, Victor D. Dorobantu, and Aaron D. Ames
Abstract
— Achieving stable hopping has been a hallmark
challenge in the field of dynamic legged locomotion. Controlled
hopping is notably difficult due to extended periods of un-
deractuation combined with very short ground phases wherein
ground interactions must be modulated to regulate global state.
In this work, we explore the use of hybrid nonlinear model
predictive control paired with a low-level feedback controller
in a multi-rate hierarchy to achieve dynamically stable motions
on a novel 3D hopping robot. In order to demonstrate richer
behaviors on the manifold of rotations, both the planning and
feedback layers must be designed in a geometrically consistent
fashion; therefore, we develop the necessary tools to employ
Lie group integrators and appropriate feedback controllers.
We experimentally demonstrate stable 3D hopping on a novel
robot, as well as trajectory tracking and flipping in simulation.
I. I
NTRODUCTION
Hopping has been a benchmark challenge in the field of
robotic locomotion dating back to the seminal work of Marc
Raibert in the 1980’s [1]. The lessons learned during hopping
have inspired generations of researchers, and have enabled
complex behaviors such as walking and running on bipedal
[2]–[4] and quadrupedal robots [5], [6]. Since the original
work of Raibert, the landscape of control has changed
dramatically, and has recently been fueled by advances
in computation power allowing for previously prohibitively
costly methodologies to be employed in real-time on robotic
platforms. The goal of this work is to investigate the extent
to which this new modern computation can be leveraged in
the context of the canonical example of hopping.
The control of hopping robots is particularly challenging
due to intermittent continuous and discrete dynamics, periods
of extreme underactuation, and exceptionally short ground
phases during which the robot can apply forces to regu-
late its global position. These pose unique difficulties for
conventional control algorithms, and necessitate the ability
to decide control actions based on predictions of where
the robot will be in the future. Raibert addressed these
challenges through the use of reduced order models, e.g.,
the spring loaded inverted pendulum (SLIP) model [7], which
has since proven effective in walking and running generation
on bipedal robots [8]. Besides the work of Raibert, many
methods have been developed to stabilize hopping robots
[9], including reinforcement learning based approaches [10],
[11], nonholonomic motion planning [12], a mix of offline
and online hierarchical motion planning strategies [13], [14],
This research was supported by NSF Graduate Research Fellowship No.
DGE-1745301 and Raytheon, Beyond Limits, JPL RTD 1643049.
Authors are with the Department of Computing and Mathematical Sci-
ences, California Institute of Technology, Pasadena, CA 91125.
Fig. 1: Nonlinear MPC running in real-time on the 3D robot
ARCHER, demonstrating hopping on hardware.
and model predictive control of simplified models [15], [16].
Compared to the abundance of work that exists for planar
hopping robots, the literature for the control of 3D hopping
robots is comparatively sparse.
Recently, model predictive control (MPC) [17] has been
used effectively for the control of dynamic robotic systems,
including hybrid systems (systems with continuous dynamics
and discrete impacts [18]) like legged robots [19]–[21]. MPC
has been brought into the realm of real-time dynamic robotic
control due to modern computing power and increased algo-
rithmic efficiency [22]; however, the implementation of MPC
on hardware platforms, as well as theoretical justifications
of its performance for nonlinear systems remains an active
area of research. As nonlinear MPC is predicated on taking
local approximations of system dynamics, its success heavily
relies on correctly constructing these approximations and
remaining within the regions in which they are valid.
When the system states are manifold-valued, these lo-
cal approximations must be carefully constructed. We will
specifically be concerned with Lie groups, groups with
smooth manifold structure whose operator and inverse are
also smooth (see [23], [24]), as they are often used in the
field of robotics to model the space of orientations. Lie
groups have additionally been studied from the perspectives
of discrete mechanics [25] and numerical analysis [26], [27].
The estimation of robotic systems on Lie groups was outlined
in [28], and the control of legged robotic systems on Lie
groups has recently been investigated in [29]. Finally, the
application of optimal control techniques over differentiable
manifolds to the control of hybrid systems was explored in
[30] with experimental results achieved in [31], [32].
The goal of this work is to experimentally realize dynamic
hopping behaviors on a novel 3D hopping robot through
arXiv:2209.11808v3 [cs.RO] 8 Mar 2023
the use of nonlinear MPC. To this end, we: (1) develop
a framework for hybrid nonlinear MPC in a geometrically
consistent fashion via Lie group integrators, and (2) exper-
imentally demonstrate on a novel 3D robot that MPC is an
effective tool for accomplishing highly dynamic behaviors
such as hopping.
Theoretically, we consider both the manifold structure of
the configuration space and the hybrid nature of the dynamics
(Sec. II), wherein an MPC problem is synthesized through
the use of sequential linearizations that leverage Lie group
and Lie algebra structures (Sec. III). The MPC problem is
translated to hardware via a multi-rate control paradigm. To
experimentally verify this framework (Sec. IV), we leverage
a novel hardware platform: ARCHER [33] (which builds
upon earlier generations of hopping robots [34]–[36]). This
robot has three reaction wheels for attitude control, and
one motor connected via a rope to control foot spring
compression. We first show the capabilities of this robot
in a high-fidelity simulation wherein a variety of dynamic
motions are demonstrated: from path following to flipping.
Finally, we experimentally realize sustained 3D hopping on
this new hardware platform, marking the first demonstration
of 3D hopping using online motion planning strategies.
II. P
RELIMINARIES
A. Hybrid System Dynamics
The configuration of the hopping robot is given by
q
=
(
p
,q,
θ
,`
)
∈ Q
, where
p
R
3
is the Cartesian position,
q
S
3
is the unit quaternion representing the orientation,
θ
R
3
represents the flywheel angles, and
`
R
is the foot
deflection. Next, let
v
= (
̇
p
,
ω
,
̇
θ
,
̇
`
)
∈V
,
R
3
×
s
3
×
R
3
×
R
,
where
ω
s
3
is a purely imaginary quaternion representing
the angular rate of the body. The complete robot state can
then be written as
x
= (
q
,
v
)
∈X
,
Q×V
.
Hopping consists of alternating sequences of continuous
and discrete dynamics; therefore, it is naturally modeled as a
hybrid system. Two distinct continuous phases of dynamics
exist, the
flight
phase
f
when the robot is in the air, and the
ground
phase
g
when the foot is contacting the floor. We can
construct a directed graph with vertices
v
V
,
{
f,g
}
and
edges
e
E
,
{
f
g,g
f
}
to characterize how the
robot traverses the hybrid modes, as shown in Figure 2.
For each vertex
v
V
, let
D
v
⊂ X
represent the
admissible domain in which the system state evolves, and
n
v
denote the number of holonomic constraints restricting
the motion of the robot. Note that
n
f
= 0
, and
n
g
= 3
,
which pin the foot to the ground. Omitting the details for
manifold value variables (which can be found in [37]), let
J
v
:
Q →
R
n
v
×
n
denote the Jacobian of the holonomic
constraints where
n
=
|Q|
. We can define the dynamics as:
D
(
q
)
̈
q
+
H
(
q
,
̇
q
) =
Bu
+
J
>
v
(
q
)
λ
v
,
̇
J
v
(
q
,
̇
q
)
̇
q
+
J
v
(
q
)
̈
q
=
0
.
where
D
:
Q→
R
n
×
n
is the mass-inertia matrix,
H
:
X →
R
n
contains the Coriolis and gravity terms,
B
R
n
×
m
is the
actuation matrix,
u
R
m
is the control input, and
λ
v
R
n
v
Fig. 2: The robot traversing the various hybrid domains.
are the Lagrange variables describing the constraint forces.
These equations can be rearranged and forces
λ
v
solved for
in order to produce the constraint implicit dynamics:
̇
x
=
f
v
(
x
,
u
)
,
(1)
where
f
v
:
X ×
R
m
R
2
n
is of control-affine form.
Each hybrid transition
e
E
occurs when the system state
intersects the
guard
, defined as:
S
f
=
{
x
∈D
f
:
p
z
(
q
) = 0
,
̇
p
z
(
x
)
<
0
}
e
=
f
g,
S
g
=
{
x
∈D
g
:
`
= 0
,
̇
` <
0
}
e
=
g
f,
where
p
z
:
Q →
R
returns the vertical position of the foot.
For an edge
e
=
v
1
v
2
, upon striking the guard
S
v
1
the
system undergoes a discrete jump in states as described by:
x
+
=
e
(
x
)
,
where
e
:
S
v
1
→ D
v
2
is the
reset map
describing the
momentum transfer though impact, and
x
∈ D
v
1
and
x
+
∈ D
v
2
are the pre and post-impact states, respectively.
Collecting the various objects
D
=
{D
v
}
v
V
,
S
=
{S
v
}
v
V
,
=
{
e
}
e
E
and
F
=
{
f
v
}
v
V
, we can describe the
hybrid control system of the hopping robot via the tuple:
HC
= (
V,E,
D
,
S
,
,F
)
.
B. Lie Group Integrators
In this section, we focus our attention to the orientation
coordinates of the robot and discuss how to perform one form
of Lie group integration. We represent the orientation of a
rigid body via a unit quaternion
q
S
3
=
{
q
H
:
|
q
|
=
1
}
; quaternionic representations of orientation have been
extensively explored in attitude control of spacecrafts [38],
[39]. As opposed to Euler angles, quaternions do not suffer
from issues of singularities, and provide a straightforward
interpolation method – this will be helpful when construct-
ing continuous-time signals from the discrete points that
MPC produces. Importantly, quaternions and their associated
quaternion multiplication define a Lie group structure on
S
3
.
The angular rate of the body is given by an element of the
associated Lie algebra
ω
s
3
, the tangent space of
S
3
at
the identity element
q
E
,
the elements of which are purely
imaginary quaternions.
Fig. 3: A depiction of Lie groups, Lie algebras, and the
log
operation. a) The trajectory
q
k
, b) pulling the trajectory back
to the identity element via
q
1
0
, and c) taking the
log
map near identity to obtain elements in the Lie algebra.
The time rate of change of a unit quaternion at a point
q
is
given by an element of the tangent space at
q
, i.e.
̇
q
T
q
S
3
.
Given the angular rate of the body
ω
, we can calculate
̇
q
as:
̇
q
=
q
ω
,
(2)
using standard quaternion multiplication. The formulation (2)
is possible since the tangent map of left multiplication by
a quaternion is also given by left multiplication, mapping
from the Lie algebra to the tangent space at
q
.
1
Integrating
equation (2) results in:
q
(
t
) =
q
(0) exp(
ω
(
t
))
(3)
where
exp :
s
3
S
3
is termed the exponential map and
maps elements of the Lie algebra
s
3
back to the Lie group
S
3
. This map is injective for imaginary quaternions with
magnitude less than
π
; over this neighborhood, the inverse
map is denoted as
log :
S
3
s
3
. If instead of directly
integrating we take a
Lie-Euler step
, (3) becomes:
q
k
+1
=
q
k
exp(
ω
k
h
)
(4)
for a (small) time step
h
R
. This is a simple example of
a
Lie group integrator
.
III. G
EOMETRIC
MPC
AND
M
ULTI
-R
ATE
C
ONTROL
A. Linearized Dynamics
In order to avoid the nonlinearity present in (4), we
propose the following change of coordinates:
ξ
k
= log(
q
1
0
q
k
)
,
(5)
which first pulls the variables back to the vicinity of
q
ε
, and
then to the Lie algebra as shown in Figure 3. Substituting in
the Lie-Euler step from (4) into the above expression for the
first few values of
k
, we have:
ξ
0
=
0
ξ
1
= log(
q
1
0
q
1
) = log(
q
1
0
q
0
exp(
ω
0
h
))
=
ω
0
h
ξ
2
= log(
q
1
0
q
2
) = log(
q
1
0
q
1
exp(
ω
1
h
))
= log(
q
1
0
q
0
exp(
ω
0
h
) exp(
ω
1
h
))
=
ω
0
h
+
ω
1
h
+
1
2
[
ω
0
h,
ω
1
h
] +
O
(
h
3
)
1
Often, the equation (2) is reported as
̇
q
=
1
2
q
ω
. This is because the
isomorphism between
ω
s
3
and
R
3
is given by
φ
(
ω
) =
1
2
ω
if the
generators of
s
3
are taken to be the canonical basis of imaginary 3-vectors,
which arises from the fact that the action of quaternions parameterized by
a rotation angle
θ
on vectors rotates them by an angle of
2
θ
.
where
[
·
,
·
]
represents the Lie bracket, the last line follows
from the Campbell-Baker-Hausdorff theorem [40], and the
higher order terms consist of linear combinations of iterated
Lie brackets, which due to linearity are multiplied by terms
of
h
3
or higher
2
. This means that, neglecting terms of
O
(
h
2
)
or higher, we are able to write our dynamics update law as:
ξ
k
+1
=
k
i
=0
ω
k
h
=
ξ
k
+
ω
k
h,
which is linear and will therefore be straightforward to
include in the MPC program.
The next challenge concerns the construction of local
approximations of the acceleration dynamics. As most of
the coordinates lie in Euclidean space and therefore have
straightforward Taylor approximations, we will limit our
attention on the manifold valued variables. Specifically, in a
continuous domain
v
consider a function
f
:
S
3
×
s
3
×
R
m
s
3
satisfying:
̇
ω
=
f
(
q,
ω
,
u
)
.
Given a perturbation
η
s
3
, a local representation of
f
in
exponential coordinates
̃
f
:
S
3
×
s
3
×
R
m
×
s
3
R
3
can
be defined as:
̃
f
(
q,
ω
,
u
,
η
)
,
f
(
q
exp(
η
)
,
ω
,
u
)
.
Given
q
S
3
,
ω
s
3
, and
u
R
m
, as well as additional
perturbations
ω
R
3
and
u
R
m
, we can compute
a Taylor expansion of
̃
f
about the point
(
q,
ω
,
u
,
0
)
at a
perturbed point
(
q,
ω
+ ∆
ω
,
u
+ ∆
u
,
η
)
via:
f
(
q
exp(
η
)
,
ω
+ ∆
ω
,
u
+ ∆
u
)
f
(
q,
ω
,
u
) +
̃
f
η
·
η
+
∂f
ω
·
ω
+
∂f
u
·
u
.
Then, we can write the continuous-time linearized dynamics
of
ω
about the point
(
q,
ω
,
u
)
as:
d
dt
δ
ω
=
̃
f
η
(
q,
ω
,
u
,
0
)
·
η
+
∂f
ω
·
δ
ω
+
∂f
u
·
δ
u
.
(6)
Next, we consider the dynamics of the variables
ξ
k
around
a reference trajectory
̄
q
k
S
3
,
̄
ω
k
s
3
, and
̄
u
k
R
m
.
Define
̄
ξ
k
= log( ̄
q
1
0
̄
q
k
)
with
̄
ξ
0
=
0
and suppose the
reference trajectory satisfies the Lie-Euler step, i.e.:
̄
ξ
k
+1
=
̄
ξ
k
+
̄
ω
k
h.
2
If quaternion multiplication commuted, then the iterated Lie brackets
would be zero – but alas, no such pleasantries exist.
For a trajectory
ξ
k
s
3
similarly satisfying the Lie-Euler
step, and vectors
ω
k
s
3
, and
u
k
R
m
, we have:
(
ξ
k
+1
̄
ξ
k
+1
) = (
ξ
k
̄
ξ
k
)+(
ω
k
̄
ω
k
)
h
+
̄
ξ
k
+
̄
ω
k
h
̄
ξ
k
+1
︷︷
=
0
,
yielding continuous-time linearized dynamics:
d
dt
δ
ξ
=
δ
ω
.
(7)
Combining expressions (6) and (7), we obtain:
d
d
t
[
δ
ξ
δ
ω
]
=
[
0
I
̃
f
η
(
q,
ω
,
u
,
0
)
∂f
ω
]
︷︷
A
[
δ
ξ
δ
ω
]
+
[
0
∂f
u
]
︷︷
B
δ
u
.
(8)
We can similarly construct local approximations of the
impact maps. On an edge
e
, consider a function
∆ :
S →
s
3
which satisfies:
ω
+
= ∆(
q
,
ω
)
.
(9)
Here,
S
represents the guard as a submanifold of
S
3
×
s
3
(though the guard is actually a submanifold of
Q
). As
defined, this reset map is the restriction of the momentum
transfer of the system at impact to the guard. Therefore,
we can naturally extend the domain of the reset map by
considering the same momentum transfer applied anywhere
in the state space, yielding
ext
:
S
3
×
s
3
s
3
. This
is needed because in our Talyor expansion of the discrete
dynamics, we consider perturbations of the full system state
(not just perturbations tangent to the guard)
3
. As before, we
can locally approximate the function
ext
via:
ext
(
q
exp(
η
)
,
ω
+ ∆
ω
)
ext
(
q
,
ω
)
+
̃
ext
η
(
q
,
ω
,
0
)
·
η
+
ext
ω
·
ω
,
where again we can represent
ext
locally as:
̃
ext
(
q
,
ω
,
η
)
,
ext
(
q
exp(
η
)
,
ω
)
.
Noting that the
q
+
=
q
, we can represent the linearization
of the discrete map as:
[
δ
ξ
+
δ
ω
+
]
=
[
I
0
̃
ext
η
(
q
,
ω
,
0
)
ext
ω
]
︷︷
D
[
δ
ξ
δ
ω
]
.
(10)
B. Geometric Model Predictive Control
This section represents the mid-level of the control hier-
archy, as shown in Figure 4. High level target base positions
p
ref
R
3
are provided by the user, and MPC produces ref-
erence trajectories to pass to the low level. This architecture
maintains the benefit of having a horizon and is paired with a
low level feedback controller which adds robustness to model
error and delays induced by computation time.
For a given MPC horizon
N
N
, we begin by con-
structing a vertex sequence
v
k
V
for
k
= 0
,...,N
1
describing the continuous modes that the robot will be in
3
This extension allows us to abscond from having to only consider
perturbations along the guard, which is an interesting area of future work.
Fig. 4: The multi-rate architecture employed for the robot.
at various points along the horizon. These are defined
a
priori
by estimating the time to impact of the robot. We
also construct a sequence of
e
k
=
v
k
v
k
+1
E
∪{
0
}
,
where
e
k
= 0
if no discrete transition is expected. Consider
a discrete (manifold valued) state trajectory
̄
x
k
and input
trajectory
̄
u
k
. We introduce the variables:
̄
z
k
= (
̄
p
k
,
̄
ξ
k
,
̄
θ
k
,
̄
`
k
,
̄
v
k
)
R
20
,
with
̄
ξ
k
defined as in (5), and whereby
z
k
will represent our
decision variables in the MPC program. At each index
k
,
compute the linearizations of the dynamics in the vertex
v
k
:
̇
z
k
=
A
v
k
z
k
+
B
v
k
u
k
+
f
v
k
(
̄
x
k
,
̄
u
k
)
A
v
k
̄
z
k
B
v
k
̄
u
k
︷︷
,
C
v
k
,
(11)
where the
q
k
and
ω
k
elements are linearized as in (8), the
Euclidean elements are linearized in the standard way. From
(11), we can produce a discrete-time linear system over a
time interval
h
R
>
0
by taking an Euler step (in Euclidean
space), or by using the matrix exponential in
R
20
×
20
to
produce the discrete time dynamics:
z
k
+1
=
A
d
v
k
z
k
+
B
d
v
k
u
k
+
C
d
v
k
,
(12)
z
+
k
=
D
e
k
z
k
+
e
k
(
̄
x
k
)
D
e
k
̄
z
k
︷︷
,
E
e
k
.
(13)
We now introduce the finite-time optimal control problem
(FTOCP), i.e., the
geometric model predictive controller
:
min
u
k
,
z
k
N
1
k
=0
(
z
k
z
ref
)
>
Q
(
z
k
z
ref
) +
u
>
k
Ru
k
+
z
>
N
Vz
N
s.t.
z
k
+1
=
A
d
v
k
z
k
+
B
d
v
k
u
k
+
C
d
v
k
,
if
e
k
= 0
(14a)
z
k
+1
=
D
e
k
z
k
+
E
e
k
,
if
e
k
6
= 0
(14b)
z
0
=
z
(
t
)
,
(14c)
u
k
∈U
(14d)
where
Q
S
2
n
>
0
and
R
S
2
n
>
0
are symmetric, positive
definite state and input gain matrices, respectively,
V
S
2
n
>
0
is a quadratic approximation of the cost-to-go,
U
is an input
constraint set, and where the initial condition
ξ
0
=
0
is
enforced, as previously discussed. The above optimal control
problem is solved in an SQP fashion, where the solution from
the previous iteration is used to produce the linearizations
for the next. Specifically, we can take
(
̄
z
k
,
̄
u
k
) = (
z
k
,
u
k
)
where the asterisk indicates the optimal solution, and
̄
x
k
can
be produced from
̄
z
k
via inverting (5).
Fig. 5: The planned elements
q
S
3
and
ω
s
3
, as well
as the low level feedback controller. The multi-rate nature of
the methodology can be seen in the difference of time scales
between when MPC produces trajectories and when the low
level controller updates.
C. Quaternionic Feedback
Once MPC produces a solution, a desired trajectory and
feedforward input can be produced as:
q
d
(
τ
) = ̄
q
0
exp(
ξ
1
)
,
ω
d
(
τ
) =
ω
1
,
u
ff
(
τ
) =
u
0
,
for
τ
[0
,dt
)
R
. An interesting area of future work
is using the MPC signals to produce dynamically admissible
trajectories in the inter-MPC times, but this was not explored
due to communication bandwidth limitations. The FTOCP is
implemented in a receding horizon fashion, where the low
level controller only ever receives the first control input and
desired trajectory.
Given the measured quaternion
q
a
, measured angular rate
ω
a
, desired quaternion
q
d
, and desired angular rate
ω
d
of
the robot, we can construct our actuation as:
u
(
x
,t
) =
K
p
I
m
(
q
d
(
t
)
1
q
a
)
K
d
(
ω
a
ω
d
(
t
)) +
u
ff
(
t
)
,
where
K
p
,
K
d
S
3
>
0
are positive definite gain matrices.
The product
q
d
(
t
)
1
q
a
represents a “difference” between
elements of
S
3
; if
q
a
is in a small neighborhood of
q
d
, then
the product is in a small neighborhood of the identity element
q
E
. The map
I
m
:
S
3
s
3
takes the purely imaginary
component of the error signal, and can be viewed as the
Euclidean projection of the Lie group onto the Lie algebra,
allowing us to base the control input over a vector valued
error. Alternatively, the
log
operation could be used instead
of
I
m, but the
I
m operator was empirically found to work
more reliably.
Fig. 6: (Above) Positions and velocities of the robot tracking
a global reference setpoint in simulation. Note the planning
through hybrid events and constant velocity in flight phase
due to underactuation. (Below) Two reference trajectories.
IV. I
MPLEMENTATION AND
R
ESULTS
A. Hardware
The ARCHER hardware platform consists of three KV115
T-Motors with 250 g flywheel masses attached for orientation
control, and one U10-plus T-Motor attached to a 3-1 gear
reduction to the foot via a cable and pulley system. The robot
is powered by two 6 cell LiPo betteries connected in series,
which can supply up to 50.8 V at over 100 A of current to
the four ELMO Gold Solo Twitter motor controllers. The
robot has two on-board Arduino Teensy microcontrollers
for the low level feedback control, which run at 1kHz and
communicate over WiFi via an ESP module to a desktop
running the mid-level controller.
The MPC program runs at 100 Hz on an Ubuntu 20.04
machine with AMD Ryzen 5950x @ 3.4 GHz and 64 Gb
RAM. The Pinocchio C++ library [22] is used, specifically
the pinocchio3 preview branch, to produce fast evaluation
of the system dynamics (constrained, unconstrained, contin-
uous, and discrete), as well as their associated Jacobians, and
the manif C++ library [28] is used to handle all Lie group
operations (such as
log
and
exp
). As seen in Figure 5 as well