# Trajectory Optimization under Contact Timing Uncertainties

Haizhou Zhao1 , Majid Khadiv1 1Munich Institute of Robotics and Machine
Intelligence, Technical University of Munich, Germany
<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

Most interesting problems in robotics (e.g., locomotion and manipulation) are
realized through intermittent contact with the environment. Due to the
perception and modeling errors, assuming an exact time for establishing
contact with the environment is unrealistic. On the other hand, handling
uncertainties in contact timing is notoriously difficult as it gives rise to
either handling uncertain complementarity systems or solving combinatorial
optimization problems at run-time. This work presents a novel optimal control
formulation to find robust control policies under contact timing
uncertainties. Our main novelty lies in casting the stochastic problem to a
deterministic optimization over the uncertainty set that ensures robustness
criterion satisfaction of candidate pre-contact states and optimizes for
contact-relevant objectives. This way, we only need to solve a manageable
standard nonlinear programming problem without complementarity constraints or
combinatorial explosion. Our simulation results on multiple simplified
locomotion and manipulation tasks demonstrate the robustness of our
uncertainty-aware formulation compared to the nominal optimal control
formulation.

##  I Introduction

Intermittent contact with the world renders locomotion and object manipulation
problems hybrid. When using optimal control to generate plans for these
systems, the resulting problem to solve would be a mixed-integer optimization
problem [1, 2]. Several works have tried to solve the problem by relaxing the
hybrid nature, e.g., smoothing the contact transition by regularizing the
Delasus matrix [3], handling physical consistency as a soft constraint [4], or
relaxing contact with complementarity slackness in the solver [5]. Most recent
efforts to implement MPC for locomotion and manipulation have focused on
solving a hierarchical problem instead of the holistic one and could achieve
impressive behaviors on real hardware [6, 7, 8, 9, 10]. These approaches
consider a fixed contact plan and control the whole body motion for the given
plan.

Figure 1: Illustration of Uncertain Hybrid Systems

They also assume that contact events happen at exact times, i. e., the
predefined switching times. However, in reality, this is a very restrictive
assumption. For instance, the robot’s perception of the environment is always
with some errors. Furthermore, the tracking error of the end-effector
establishing contact can also lead to a mismatch between the planned and
realized time of contact. To handle these situations, the whole-body MPC
frameworks available in the literature either use heuristics [8] or rely on
the intrinsic robustness of MPC through fast replanning to handle
uncertainties in contact events [6, 9, 7]. However, these approaches are very
limited and a more systematic approach is required.

Recently, [11, 12, 13, 14] investigated the use of robust and stochastic
optimal control for contact-rich robotics problems. While these approaches
provide a very concrete understanding of the problem and interesting safety
guarantees, they generally fall short in handling contact timing uncertainty.
[12] has shown that adjusting the end-effector impedance as a function of
disturbances can mitigate the problem of impact when the contact event is
uncertain. In this framework, the contact event is considered to be uncertain
with a known distribution, and the impact is mitigated using a risk-sensitive
optimal controller. However, not adapting desired trajectories can highly
limit the capability of the controller in handling different situations such
as late foot touch-down during locomotion.

The primary contribution of this work is to provide a deterministic re-
formulation of the stochastic hybrid optimal control problem with uncertainty
in the switching event that does not add run-time computational complexity
compared to the deterministic optimal control problem. In doing so, we propose
a robust optimal control formulation that accounts for a trajectory of
possible switching states over the uncertainty set. The proposed approach can
be adapted for general contact dynamics from locomotion to manipulation.
Through several simplified examples on locomotion and manipulation problems,
we demonstrate the robustness of our approach compared to the standard nominal
optimal control problem.

The rest of the paper is structured as follows: in Section II, we provide the
necessary ingredients to formulate the problem. In section III, we detail our
proposed formulation. In section IV, we present the results of applying our
formulation to several simplified locomotion and manipulation problems.
Finally, Section V presents the concluding remarks and future work.

##  II Preliminaries

In this section, we first define the terminology required for describing our
problem. Then, we present a deterministic optimal control formulation for
hybrid dynamical systems.

###  II-A Deterministic Hybrid Systems

Locomotion and manipulation are realized through intermittent contact with the
environment. One way to formalize this problem is through the framework of
hybrid dynamical systems [15]. In this work, we consider the following
definition of hybrid systems [16, 17]

$\mathcal{H}:\bigg{\\{}\begin{array}[]{ll}\dot{\mathbf{x}}=\mathcal{F}_{I}(\mathbf{x},\mathbf{u}),&\mathbf{x}\in\mathcal{D}_{I}\backslash\mathcal{G}_{I}^{J},\mathbf{u}\in\mathcal{U}_{I},\\\
\mathbf{x}^{+}=\mathcal{R}_{I}^{J}(\mathbf{x}^{-}),&\mathbf{x}^{-}\in\mathcal{G}_{I}^{J},\mathbf{x}^{+}\in\mathcal{D}_{J},\\\
\end{array}$ (1)

with $\mathcal{J}=\\{I,J,...\\}$ being the finite set of discrete modes such
that for a mode $I\in\mathcal{J}$,

  * •

$\mathcal{F}_{I}$ is the continuous dynamics,

  * •

$\mathcal{D}_{I}$ is the domain of states,

  * •

$\mathcal{U}_{I}$ is the set of admissible input,

  * •

$\mathcal{G}_{I}^{J}:=\\{\mathbf{x}\in\mathcal{D}_{I}|g_{I}^{J}(\mathbf{x})\leq
0\\}$ is the guard (Fig. 1),

  * •

$\mathcal{R}_{I}^{J}:\mathcal{G}_{I}^{J}\to\mathcal{D}_{J}$ is the reset map
that projects states in $D_{I}$ to $D_{J}$ when the guard condition
$g_{I}^{J}(\mathbf{x}^{-})\leq 0$ is met.

A simple example of a hybrid robotic system is a jumping 1D hopper. Upon
landing, the robot’s states enter the guard from the aerial phase to stance,
undergoing a reset by an impulsive impact force.

###  II-B From Hybrid to Switching systems

Given the sequence of contacts for a hybrid system, the problem can be
simplified to a switching system. In this formulation, the system’s dynamics
are smooth between consecutive switches, while the time of the switch can
still be optimized. Recently, many fast solvers [18, 19, 20] have been
developed for real-time resolution of (2). In the following, we present the
multiple-shooting transcription of the switching system.

Let $\mathcal{S}$ be the set of shooting node indices where a switch is
expected. For a given initial state $\mathbf{x}_{0}$, the time-based direct-
multiple-shooting optimal control problem can be formulated as

$\displaystyle\min_{\mathbf{x},\mathbf{u}}\quad$ $\displaystyle
L_{N}(\mathbf{x}_{N})+\sum_{i=0}^{N-1}L_{i}(\mathbf{x}_{i},\mathbf{u}_{i})$
(2a) s.t. $\displaystyle\forall i\notin\mathcal{S}:$
$\displaystyle\quad\mathbf{f}_{i}(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{x}_{i+1},\Delta
t_{i})=\mathbf{0},$ (2b) $\displaystyle\quad g_{i}(\mathbf{x}_{i+1})>0,$ (2c)
$\displaystyle\forall i\in\mathcal{S}:$
$\displaystyle\quad\mathbf{f}_{i}(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{x}_{i+1}^{-},\Delta
t_{i})=\mathbf{0},$ (2d)
$\displaystyle\quad\mathbf{x}_{i+1}=\mathcal{R}_{i}(\mathbf{x}_{i+1}^{-}),$
(2e) $\displaystyle\quad g_{i}(\mathbf{x}_{i+1}^{-})=0,$ (2f)
$\displaystyle\mathbf{h}(\mathbf{x}_{i},\mathbf{u}_{i})\leq 0,$ (2g)

where $\Delta t_{i}$ is the phase-wise timestep, $N$ is the number of shooting
nodes, $L_{N}$ is the terminal cost, $L_{i}$ is the running cost, (2b) is the
non-switching implicit dynamics, $\eqref{eq:impdynpre}$ is the pre-switching
continuous dynamics derived from $\mathcal{F}_{(\cdot)}$ in (1), $x^{-}_{i+1}$
denotes the pre-reset state, (2c),(2f) ensure switching consistency, (2e) is
the state reset equation at the switch, and (2g) is the state-input inequality
constraints.

##  III Uncertainty-Aware Optimal Control

The formulation in (2) assumes that contact happens at a certain time and
state (where the distance between the end-effector and the environment goes to
zero). However, due to uncertainties in the environment perception and end-
effector tracking errors, it is highly unlikely that the end-effector touches
the ground at the exact pre-defined time. to formalize this situation, we
introduce the following uncertain guard as illustrated in Fig. 1:

$\hat{\mathcal{G}}_{I}^{J}(\delta)=\\{\mathbf{x}\in\mathcal{D}_{I}|{g}_{I}^{J}(\mathbf{x})\leq\delta\\},\delta\in[-d,d],$
(3)

where $\delta$ is the guard uncertainty bounded by $d$. With
$\hat{\mathcal{G}}$, a state cannot be deterministically predicted to incur
switching, leading to uncertain contact timing and thus the switching time
between modes. This is naturally incompatible with the deterministic structure
of (2).

###  III-A Issues of the Nominal Approach

Trajectories generated from nominal time-based optimal control with a nominal
guard ($\delta=0$) only ensure that the nominal switching state is feasible.
If the switching does not happen as planned (i.e., early or late contact), the
system may evolve unexpectedly. Typical issues include:

  * •

For late contact, the controller is unknown after the nominal contact timing.
Problem-specific solutions include reference spreading [21] or simplistic
zero-order-hold of the last input.

  * •

For early contact, the system usually encounters unfavorable impact forces. In
such cases, the system may fail due to failures in the mechanical structure or
bouncing of the end-effector impacting the ground.

  * •

Since the nominal problem is only concerned with the exact contact event, it
can lead to highly aggressive motions before or after contact. An example is
that when a trajectory is aggressive for performance, states near its nominal
switching time may be outside the feasibility set of the post-impact problem.

In this section, we introduce our main contribution: an uncertainty-aware
optimal control formulation that resolves the above issues.

###  III-B A Deterministic Transcription

Figure 2: Illustration of the difference between (a) the nominal optimal
control and (b) the proposed approach. The proposed method does not switch the
mode but generates a trajectory of feasible switching states over the
uncertainty set.

Intuitively, a robust time-based trajectory that solves the problem in Sec.
III-A should be deterministic until a switch is triggered. Since the switching
may happen at any moment, when the states are within the uncertain region, all
these candidate pre-switching states should not harm the system safety or the
feasibility of the post-switch optimal control. Based on this intuition, we
consider that for a single phase, an uncertain sub-phase (namely the robust
phase) is appended to the pre-switching phase. For its index set
$\mathcal{K}$, the following constraints must be satisfied:

$\displaystyle i=\mathcal{K}_{0},~{}$ $\displaystyle g_{i}(\mathbf{x}_{i})=d,$
(4a) $\displaystyle i=\mathcal{K}_{-1},~{}$ $\displaystyle
g_{i}(\mathbf{x}_{i})=-d,$ (4b) $\displaystyle\forall i\in\mathcal{K},~{}$
$\displaystyle\dot{g}_{i}(\mathbf{x}_{i})\leq 0,$ (4c)

where $\mathcal{K}_{0},\mathcal{K}_{-1}$ denote the earliest and latest
indices in $\mathcal{K}$, respectively. An uncertainty-aware optimal control
problem can then be formulated as a parameterized optimization problem as
introduced in [22]:

$\displaystyle\min_{\mathbf{x},\mathbf{u},\Delta t,d}\quad$
$\displaystyle\sum_{i=0}^{N-1}L_{i}(\mathbf{x}_{i},\mathbf{u}_{i})+\sum_{i\in\mathcal{K}}L_{K}(\mathbf{x}_{i},\mathbf{p}_{i})$
(5a) s.t.
$\displaystyle\mathbf{h}_{K}(\mathbf{x}_{i},\mathbf{p}_{i})\leq\mathbf{0},\forall
i\in\mathcal{K},$ (5b) $\displaystyle\Delta t_{i}\in[\Delta t_{\min},\Delta
t_{\max}],d\in[d_{\min},d_{\max}],$ (5c) $\displaystyle\forall
i\notin\mathcal{K},\eqref{eq:guardpreineq},$ $\displaystyle\forall
i\in\mathcal{K},\eqref{eq:uncertainconstr},$
$\displaystyle\eqref{eq:impdyn},\eqref{eq:ineqnormal}$

where $\mathcal{K}_{0}=N$, $L_{K}$ is the contact-related objective,
$\mathbf{h}_{K}$ is the contact-related constraint, and $\mathbf{p}_{i}$ is
the collection of auxiliary variables including the timesteps and uncertainty.
Notice that $\delta t$ and $d$ are decision variables in this new formulation,
bounded by (5c), which can be crucial for the feasibility and convergence of
the optimization problem. For instance, depending on the problem if $d$ is set
to a large fixed value, there might be no feasible solution that can satisfy
all the constraints for all the possible contact events. Also, since the
number of nodes in the robust phase is fixed, optimizing time is important to
regulate the robot’s behavior in the uncertain region.

The _robust phase_ in (4) ensures that the trajectory traverses the uncertain
region, constituting a continuous collection of possible switching states. To
show the effectiveness of our proposed formulation in (5) in generating robust
trajectories, we can adapt $L_{K},\mathbf{h}_{K}$ in (5b) in the following
ways: we can model safety-related or feasibility-related criteria as
inequality constraints $\mathbf{h}_{K}$; we can also adapt $L_{K}$ to reach
various goals such as robustness maximization and impact minimization. We will
show the flexibility of our formulation in different case studies in the next
section.

###### Remark 1

(Uncertainty optimization) In (5), uncertainty $d$ is also a decision
variable. Depending on the specific problem setting, this handling enables
finding the maximum possible uncertainty, where either the uncertainty can be
increased to gain better robustness or decreased to show the maximum feasible
value.

###### Remark 2

(Optimality) For long-term optimality, the formulation in (5) can further be
extended to a parallelizable tree-structured optimal control problem [23] that
branches at each shooting node in $\mathcal{K}$. Nevertheless, we only focus
in this paper on the transcription of the uncertainty into a robust phase
without trying to achieve long-term optimality.

##  IV Case Studies

In this section, we show case studies of various locomotion and manipulation
tasks based on the proposed optimal control formulation. We also compare the
results of our proposed robust formulation to the nominal case. All examples
are implemented using the Opti stack of CasADi [24] and IPOPT [25].

###  IV-A Impact Minimization of a Hopping Robot

Figure 3: Illustration of the planar two-link point-footed robot. (a) The
robot has two joints (hip and knee) and a 2-DoF base joint. The black dot
denotes the whole-body center of mass (CoM). (b) When landing, the ground
position is uncertain. Figure 4: Simulation data during the robust phase. The
weights for maximizing the uncertainty in (a),(c) are respectively 1000x that
in (b),(d). ’mi’ denotes the impact minimization over the given uncertainty
[-0.05, 0.05]m. ’mu_(x)’ denotes uncertainty maximization for known impact
limits x (unit: N), where the flat region denotes the uncertainty. ’nom’
denotes the nominal optimal control data. Flat parts of ’mu_(x)’ denote the
optimized uncertainty region where the impact limits are satisfied.

One of the classical examples in robot locomotion control is impact
minimization for jumping robots [26]. In this task, a planar two-link point-
feet hopper jumps continuously while the height of the support surface can
suddenly change within a bound, as shown in Fig. 3. The robot has a 2-DoF X-Y
base joint The task is to perform in-place hopping to reach a desired height.

####  IV-A1 Dynamic Model

Let $\mathbf{q}=[y_{b},\theta_{h},\theta_{k}]^{\top}$, and
$\dot{\mathbf{q}}=[\dot{y}_{b},\dot{\theta}_{h},\dot{\theta}_{k}]^{\top}$.
$y_{b}$ is the base height, and $\theta_{h},\theta_{k}$ are the hip and knee
angles, respectively. The dynamics of the system can be written as

$\displaystyle\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{H}(\mathbf{q},\dot{\mathbf{q}})$
$\displaystyle=\mathbf{S}\bm{\tau}+\mathbf{J}_{c}^{T}\mathbf{F}_{c},\,$ (6a)

where $\mathbf{M}\in\mathbb{R}^{3\times 3}$ is the joint-space inertia matrix,
$\mathbf{H}\in\mathbb{R}^{3}$ is the nonlinear effects,
$\mathbf{S}=\begin{bmatrix}\mathbf{0}_{2\times
2}&\mathbf{I}_{2}\end{bmatrix}^{\top}$ is the selection matrix,
$\bm{\tau}\in\mathbb{R}^{2}$ is the joint torques,
$\mathbf{J}_{c}\in\mathbb{R}^{2\times 3}$ is the foot contact jacobian,
$\mathbf{F}_{c}=[F_{y},F_{x}]^{\top}\in\mathbb{R}^{2}$ is the contact force
subject to the following constraints:

$\displaystyle 0<F_{y}$ $\displaystyle~{}\bot~{}y_{f}-y_{g}>0,$ (7a)
$\displaystyle F_{y}$ $\displaystyle\geq\mu|F_{x}|,$ (7b)

where (7a) is the contact complementary constraints, $y_{f},y_{g}$ are the
foot and the ground height, respectively. Equation (7b) encodes the planar
friction cone constraint. We assume purely inelastic impact, i.e., zero post-
impact foot velocity. Based on maximum dissipation principle, the impact
impulse $\bm{\lambda}=[\lambda_{x},\lambda_{y}]^{\top}$ can be modeled as

$\displaystyle\bm{\lambda}=$
$\displaystyle\operatorname*{arg\,min}~{}||\mathbf{J}_{c}^{T}\dot{\mathbf{q}}^{+}||^{2}$
(8a) s.t.
$\displaystyle\mathbf{M}(\dot{\mathbf{q}}^{+}-\dot{\mathbf{q}}^{-})=\mathbf{J}_{c}^{\top}\bm{\lambda}+[\mathbf{S}\bm{\tau}-H(q,\dot{\mathbf{q}}^{-})]\Delta
t,$ (8b) $\displaystyle\lambda_{y}\geq\mu|\lambda_{x}|,$ (8c)
$\displaystyle\mathbf{J}_{c}^{N}\dot{\mathbf{q}}^{+}=0,$ (8d)

where the superscription $N$ and $T$ denote normal and tangential components
of velocity w.r.t. the ground, $\Delta t$ denotes the impact duration, which
is set to be 2ms in our tests. Note that impulse is used instead of force to
improve the numerical conditioning.

####  IV-A2 Nominal Optimal Control

In the form of (2), a hopping loop is divided into three phases: take-off
(stance), ascendance, and falling. A terminal constraint of the base height is
added to the ascendance phase to ensure the base reaches the desired position.
The guard is chosen as

$g_{i}:=y_{f}-y_{g}.$ (9)

Let $\mathbf{r}_{f}=[x_{f},y_{f}]^{\top}$. To maintain the discretized contact
constraint during the stance phase, we add the velocity-level stabilization at
each shooting node:

$k_{f}\dot{\mathbf{r}}_{f}+\mathbf{r}_{f}=\mathbf{r}_{f}^{0},$ (10)

where $k_{f}=1e3$ in our setting, $\mathbf{r}_{f}^{0}$ is the initial foot
position. The center-of-mass (CoM) of the robot is set to be right above the
foot during the whole procedure for in-place hopping. Upon switching, (8) is
added and the horizontal post-impact velocity of the foot is constrained to be
zero as a terminal constraint of the falling phase to avoid slip. Torques,
joint positions, and velocities are also constrained according to the hardware
implementation of the robot for realistic settings.

####  IV-A3 Robust Formulation

The robust formulation is the same as the nominal one except that an extra
robust phase is added. The guard (9) is used in the form of (4). Two realistic
scenarios are tested to show the flexibility of our method:

  * •

Minimizing impact force for the worst-case uncertainty within a given range.
In this case, we minimize the upper bound of vertical impact
$\bar{\lambda}_{y}$ in the robust phase, i.e.,

$\displaystyle L_{K}$ $\displaystyle:=w_{\lambda}\bar{\lambda}_{y},$ (11a)
$\displaystyle h_{K}$ $\displaystyle:=\lambda_{y}<\bar{\lambda}_{y}.$ (11b)

Note that $d$ is a parameter and $\bar{\lambda}_{y}$ is a decision variable.

  * •

Maximizing uncertainty based on the worst-feasible impact force. This is the
safety-critical case when the maximum tolerable impact by the structure of the
robot $\bar{\lambda}_{y}$ is obtained from mechanical design. In this case,
for the robust phase, we have:

$\displaystyle L_{K}$ $\displaystyle:=-w_{d}d,$ (12a) $\displaystyle h_{K}$
$\displaystyle:=\lambda_{y}<\bar{\lambda}_{y}.$ (12b)

Note that $\bar{\lambda}_{y}$ is a parameter and $d$ is a decision variable.

####  IV-A4 Result and Discussion

Two desired heights (0.65m, 0.8m) are tested for the nominal approach and the
two scenarios of the robust approach. The friction coefficient is set at 0.7.
The data is shown in Fig. 4. In terms of impact minimization, it can be
observed that the robust method can have approximately up to 30%-50%
improvement over the nominal method for about 70% of the uncertain region.

For uncertainty maximization, a wide range for the weight $w_{d}$ is
considered to generate diverse solutions. For low $w_{d}$, as the impact limit
$\bar{\lambda}\to 0$, the robust solution converges to the nominal case. For
high $w_{d}$, the feasible uncertainty can be larger, at the cost of higher
impact force outside the uncertain region. The low $w_{d}$ cases can also be
interpreted as reducing the uncertainty to obtain better average improvements
over the nominal method i.e., the percentage of the original uncertain region
with lower impact forces than the nominal solution.

###  IV-B Object Catching

Figure 5: Illustration of how the manipulator catches a free-falling object.
The manipulator (a) lifts its end-effector (EF) to a high position and then
(b) lowers its EF to reduce the velocity w.r.t. the object.

Figure 6: Optimization and simulation data of the manipulator object-catching
task. (a) The (solved) optimized uncertainty w.r.t. the initial
$x_{\text{obj}}$ with differential initializations. (b),(c) are the y-position
and -velocity trajectories of the object and the EF with the ’init L’
initialization, where the number denotes the $x_{\text{obj}}$. The manipulator
follows the strategy of reducing velocity difference at possible impacts.

This task shows a torque-controlled manipulator catching an object, of which
the shape is uncertain. It is a typical safety-critical case as an object can
be fragile and may break if the impact upon contact is high. The setup is
shown in Fig. 5. For simplicity, instead of using impulse as safety criteria,
it is assumed that the object will crack if the impact velocity difference
between the EF and the object exceeds a maximal value.

Let $y_{\text{object}},y_{\text{EF}}$ be respectively the y-position of the
nominal bottom of the object and the EF. The uncertain guard in (4) is chosen
as

$g_{i}:=y_{\text{obj}}-y_{\text{EF}},$ (13)

with the following constraints on their x-positions to ensure consistent
geometry during catching

$\forall i\in\mathcal{K},x_{\text{EF}}=x_{\text{obj}},\dot{x}_{\text{EF}}=0.$
(14)

The initial state of the manipulator is set the same for all tests. The
shoulder joint (the first joint attached to the fixed base) is located at the
origin. The object falls from $y_{\text{obj}}=1$m and different
$x_{\text{obj}}$. The results are shown in Fig. 6 for the following optimizer
initializations:

  * •

’init L’: initialization using the initial state where $y_{\text{EF}}<0$,
i.e., the EF is lower than the shoulder joint.

  * •

’init H’: initialization using the state where $y_{\text{EF}}>0$, i.e., the EF
is higher than the shoulder joint.

which lead to distinct optimized uncertainty as in Fig. 6(a). Optimization
with ’init L’ is infeasible with low $x_{\text{obj}}$. Solutions to the two
initializations diverge from each other for $x_{\text{obj}}\lessapprox 0.28$
as represented by the discontinuity (black dashed line). The y-position and
velocity plots can further illustrate it as in Fig. 5(b,c) where for ’init L’,
the trajectories with $x_{\text{obj}}\in\\{0.1,0.2\\}$ are different from the
ones with $x_{\text{obj}}\in\\{0.3,0.4,0.5\\}$. This indicates that the
uncertainty optimization is affected by the non-convexity of the original
problem. As can be seen in 5(b,c), the manipulator reduces the velocity
difference between its end-effector and the object to reduce the impact force.

###  IV-C Cart-Pole With a Rigid Wall

Figure 7: Illustration of the cart-pole system recovering balance. (a) The
pole angular velocity is disturbed. Since the cart input is limited, it moves
to the wall to seek impact that will reverse the direction of pole velocity.
(b) After Impact, the cart-pole can recover its balance and position.

In this case, we test a task similar to [27] where a cart-pole system can use
contact with the wall to stabilize itself under disturbance. As shown in Fig.
7, the pole will bounce when colliding with a rigid wall, and the cart
position is limited.

####  IV-C1 Dynamic Model

Let $\mathbf{q}=[x,\theta]$ where $x$ is the cart position and $\theta$ is the
counterclockwise pole angle. Similar to the hopping robot, the equation of
motion of the cart-pole can be written as

$\displaystyle\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{H}(\mathbf{q},\dot{\mathbf{q}})=[1,0]^{\top}\tau+\mathbf{J}_{c}^{T}\mathbf{F}_{c}$
(15a) $\displaystyle\mathbf{M}=\begin{bmatrix}m_{c}+m_{p}&m_{p}lc_{\theta}\\\
m_{p}lc_{\theta}&m_{p}l^{2}\end{bmatrix},\mathbf{H}=-m_{p}ls_{\theta}\begin{bmatrix}\dot{\theta}^{2}\\\
g\end{bmatrix},$ (15b)

where $m_{c},m_{p}$ are respectively the mass of the cart and the pole, $l$ is
the pole length (its CoM is assumed to be at the end), $c_{\theta},s_{\theta}$
are cosine and sine of the pole angle, $\tau$ is the cart linear driving
force, $\mathbf{J}_{c}\in\mathbb{R}^{2\times 2}$ is the contact jacobian of
the pole and $\mathbf{F}_{c}\in\mathbb{R}^{2}$ is the wall reaction force.
When the pole is upright, $\theta=\pi$. Its impact model is similar to (8)
except that for the normal velocity w.r.t. the wall $v_{N}$, we assume a
restitution coefficient $C$, such that

$v_{N}^{+}=-Cv_{N}^{-}.$ (16)

####  IV-C2 Nominal Optimal Control

The nominal optimal control comprises the pre-impact and post-impact phases.
The pole is constrained to collide with the wall at the terminal node of the
pre-impact phase. In the cases of early contact, the nominal optimal control
is degraded into a single-phase problem with the post-impact states as its
initial state. For late contact cases, the wall position is updated to the
actual value if the nominal contact is not triggered.

####  IV-C3 Proposed Method

(a) Convex hull

(b) Fitting error (m)

Figure 8: Approximation of the feasible set of the cart-pole system. The
colors represent (a) the stopping distance (unit: m) and (b) the fitting error
of the quadratic approximation. Figure 9: Optimized uncertainty for difference
disturbed angular velocities and position limits. Only feasible solutions are
plotted.

Since the cart-pole is an unstable and constrained system, it is important
that the robot’s state after the impact remains in a set from which there
exists a solution to stabilize the system under the constraints (a.k.a
viability). In general, finding this set is very difficult and out of the
scope of this paper. Here, we present a simple brute-force approach to
approximate this set. We used grid search to sample a small batch of pre-
impact states $\mathbf{x}=[\dot{x},\theta,\dot{\theta}]^{\top}$ and
approximated the feasible ones as a convex hull as shown in Fig. 8a. The
stopping distance, i.e., the maximum position of the cart during the
balancing, is approximated by a quadratic function $\phi$ of the pre-impact
states as shown in Fig. 8b. These two approximations are sufficient for robust
optimization with different $x_{\max}$.

Let $\mathbf{A}\mathbf{x}+\mathbf{b}\leq\mathbf{0}$ be the convex hull. The
constraints of the robust phase can be designed as

$\displaystyle\ \mathbf{h}_{K}^{\text{cvxh}}$
$\displaystyle:=\mathbf{A}\mathbf{x}+\mathbf{b}+\mathbf{s},$ (17a)
$\displaystyle h_{K}^{\text{dist}}$
$\displaystyle:=\phi(\mathbf{x})-x_{\max},$ (17b)

where $\mathbf{s}$ is the conservativeness parameter, (17a) is the convex hull
constraint and (17b) is the maximum stopping distance constraint.

###### Remark 3

(Conservativeness) Since the convex hull is merely an approximation of the
feasible sample set, states close to its boundary may still be infeasible. The
conservativeness parameters shrink the boundary to push the states into the
interior to improve robustness.

####  IV-C4 Results and Discussion

Figure 10: Success-failure plot for the comparison experiment.
’NominalSuccess’ denotes the success achieved by purely the nominal approach.
’RobustSuccess’ denotes the success achieved by the robust method in addition
to the ’NominalSuccess’. Note that the robust method will also succeed in
’NominalSuccess’ settings. The dashed lines denote that the nominal method can
find a nominal solution for the given setting. Note that the blocks do not
include the nominal settings (zero uncertainty).

The nominal and robust methods are tested on various $\dot{\theta}(0)$ and
$x_{\max}$ settings. The restitution coefficient in (16) is 0.8 and the
friction coefficient is 0.7. The optimized uncertainties are shown in Fig. 9
where the monotonicity w.r.t. $\dot{\theta}(0)$ and $x_{\max}$ can be
summarized as respectively negative and positive.

The comparison results are shown in Fig. 10. The robust approach has a higher
success rate for both early and late contact, while the nominal approach could
fail. This phenomenon shows that the robustness of the nominal approach is
limited by its potentially aggressive solution. Nevertheless, the robust
approach cannot always ensure success since the feasibility of the original
problem can vary between settings, which is irrelevant to uncertainty.

##  V Conclusions and future work

In this work, we present an uncertainty-aware optimal control formulation that
takes the uncertainty in contact events into account using the notion of
guards in hybrid systems and enables tractable resolution of the problem. Our
proposed formulation features constraint satisfaction and uncertainty
optimization within a robust phase, making it applicable to various problems
in robotics with uncertain contact events. Several case studies showed that,
in addition to generating robust trajectories, uncertainty optimization is
important to avoid failure.

In the future, we plan to extend the uncertainty-aware approach to
parallelized tree-structure optimal control for applications that emphasize
long-term optimality. We also plan to implement a fast parameterized and
constrained optimal control solver for real-world experiments. Real-world
experiments are also part of our future vision for this work.

## References

  * [1] R. Deits and R. Tedrake, “Footstep planning on uneven terrain with mixed-integer convex optimization,” in 2014 IEEE-RAS international conference on humanoid robots, pp. 279–286, IEEE, 2014.
  * [2] M. A. Toussaint, K. R. Allen, K. A. Smith, and J. B. Tenenbaum, “Differentiable physics and stable modes for tool-use and manipulation planning,” 2018.
  * [3] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4906–4913, IEEE, 2012.
  * [4] I. Mordatch, E. Todorov, and Z. Popović, “Discovery of complex behaviors through contact-invariant optimization,” ACM Transactions on Graphics (ToG), vol. 31, no. 4, pp. 1–8, 2012.
  * [5] M. Posa, C. Cantu, and R. Tedrake, “A direct method for trajectory optimization of rigid bodies through contact,” The International Journal of Robotics Research, vol. 33, no. 1, pp. 69–81, 2014.
  * [6] M. Toussaint, J. Harris, J.-S. Ha, D. Driess, and W. Hönig, “Sequence-of-constraints mpc: Reactive timing-optimal control of sequential manipulation,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 13753–13760, IEEE, 2022.
  * [7] C. Mastalli, W. Merkt, G. Xin, J. Shim, M. Mistry, I. Havoutis, and S. Vijayakumar, “Agile maneuvers in legged robots: a predictive control approach,” IEEE Transactions on Robotics, 2023.
  * [8] R. Grandia, F. Jenelten, S. Yang, F. Farshidian, and M. Hutter, “Perceptive locomotion through nonlinear model predictive control,” IEEE Transactions on Robotics, 2023.
  * [9] A. Meduri, P. Shah, J. Viereck, M. Khadiv, I. Havoutis, and L. Righetti, “Biconmp: A nonlinear model predictive control framework for whole body motion planning,” IEEE Transactions on Robotics, 2023.
  * [10] H. Zhu, A. Meduri, and L. Righetti, “Efficient object manipulation planning with monte carlo tree search,” in 2023 IEEE/RSJ international conference on intelligent robots and systems (IROS), IEEE, 2023.
  * [11] L. Drnach and Y. Zhao, “Robust trajectory optimization over uncertain terrain with stochastic complementarity,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1168–1175, 2021.
  * [12] B. Hammoud, M. Khadiv, and L. Righetti, “Impedance optimization for uncertain contact interactions through risk sensitive optimal control,” IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 4766–4773, 2021.
  * [13] A. Gazar, M. Khadiv, S. Kleff, A. Del Prete, and L. Righetti, “Nonlinear stochastic trajectory optimization for centroidal momentum motion generation of legged robots,” in Robotics Research, pp. 420–435, Springer Nature Switzerland Cham, 2023.
  * [14] A. Gazar, M. Khadiv, A. Del Prete, and L. Righetti, “Multi-contact stochastic predictive control for legged robots with contact locations uncertainty,” arXiv preprint arXiv:2309.04469, 2023.
  * [15] E. R. Westervelt, J. W. Grizzle, and D. E. Koditschek, “Hybrid zero dynamics of planar biped walkers,” IEEE transactions on automatic control, vol. 48, no. 1, pp. 42–56, 2003.
  * [16] A. M. Johnson, S. A. Burden, and D. E. Koditschek, “A hybrid systems model for simple manipulation and self-manipulation systems,” The International Journal of Robotics Research, vol. 35, no. 11, pp. 1354–1392, 2016.
  * [17] N. J. Kong, C. Li, G. Council, and A. M. Johnson, “Hybrid iLQR Model Predictive Control for Contact Implicit Stabilization on Legged Robots,” IEEE Transactions on Robotics, vol. 39, no. 6, pp. 4712–4727, 2023.
  * [18] F. Farshidian et al., “OCS2: An open source library for optimal control of switched systems.”  [Online]. Available: https://github.com/leggedrobotics/ocs2.
  * [19] C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Hammoud, M. Naveau, J. Carpentier, L. Righetti, S. Vijayakumar, and N. Mansard, “Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 2536–2542, 2020.
  * [20] C. Mastalli, S. P. Chhatoi, T. Corbéres, S. Tonneau, and S. Vijayakumar, “Inverse-dynamics mpc via nullspace resolution,” IEEE Transactions on Robotics, vol. 39, no. 4, pp. 3222–3241, 2023.
  * [21] J. v. Steen, G. v. d. Brandt, N. v. d. Wouw, J. Kober, and A. Saccon, “Quadratic programming-based reference spreading control for dual-arm robotic manipulation with planned simultaneous impacts,” IEEE Transactions on Robotics, pp. 1–14, 2024.
  * [22] A. Oshin, M. D. Houghton, M. J. Acheson, I. M. Gregory, and E. A. Theodorou, “Parameterized Differential Dynamic Programming,” in Robotics: Science and Systems 2022, 2022.
  * [23] G. Frison and M. Diehl, “HPIPM: a high-performance quadratic programming framework for model predictive control,” IFAC-PapersOnLine, vol. 53, no. 2, pp. 6563–6569, 2020.
  * [24] J. Andersson, J. Gillis, G. Horn, J. Rawlings, and M. Diehl, “CasADi: a software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation, vol. 11, 07 2018.
  * [25] A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Mathematical Programming, vol. 106, pp. 25–57, 2006.
  * [26] M. Bogdanovic, M. Khadiv, and L. Righetti, “Learning variable impedance control for contact sensitive tasks,” IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 6129–6136, 2020.
  * [27] A. Aydinoglu and M. Posa, “Real-time multi-contact model predictive control via admm,” in 2022 IEEE/RAS International Conference on Robotics and Automation (ICRA), pp. 3414–3421, 05 2022.