Julius Jankowski, Idiap Research Institute, Rue Marconi 19, 1920 Martigny,
Switzerland.

# Planning for Robust Open-loop Pushing: Exploiting Quasi-static Belief
Dynamics and Contact-informed Optimization

Julius Jankowski1, 21, 2affiliationmark:  Lara Brudermüller33affiliationmark:
Nick Hawes33affiliationmark:  and Sylvain Calinon1, 21, 2affiliationmark:
11affiliationmark:  Idiap Research Institute, Switzerland
22affiliationmark:  Ecole Polytechnique Fédérale de Lausanne, Switzerland
33affiliationmark:  Oxford Robotics Institute, University of Oxford, UK
<EMAIL_ADDRESS>
###### Abstract

Non-prehensile manipulation such as pushing is typically subject to uncertain,
non-smooth dynamics. However, modeling the uncertainty of the dynamics
typically results in intractable belief dynamics, making data-efficient
planning under uncertainty difficult. This article focuses on the problem of
efficiently generating robust open-loop pushing plans. First, we investigate
how the belief over object configurations propagates through quasi-static
contact dynamics. We exploit the simplified dynamics to predict the variance
of the object configuration without sampling from a perturbation distribution.
In a sampling-based trajectory optimization algorithm, the gain of the
variance is constrained in order to enforce robustness of the plan. Second, we
propose an informed trajectory sampling mechanism for drawing robot
trajectories that are likely to make contact with the object. This sampling
mechanism is shown to significantly improve chances of finding robust
solutions, especially when making-and-breaking contacts is required. We
demonstrate that the proposed approach is able to synthesize bi-manual pushing
trajectories, resulting in successful long-horizon pushing maneuvers without
exteroceptive feedback such as vision or tactile feedback.

###### keywords:

Contact-rich Manipulation, Robust Manipulation, Open-loop Pushing, Stochastic
Contact Dynamics.

##  1 Introduction

Enabling robots to physically interact with the world is a key challenge in
robotics. In particular, the ability to move, reorient or localize objects
through contact is a fundamental capability for robots to perform tasks in
unstructured environments. Model-based planning techniques aim to synthesize
robot control actions by using a given model to reason over anticipated
outcomes of actions. However, there are two core challenges of model-based
planning through contacts. First, contact dynamics are inherently
discontinuous, i.e. a robot control action may have no effect on the state of
a target object if the robot does not make contact. This translates into a
vanishing gradient of the associated manipulation objective, making
traditional gradient-based optimization difficult.

Second, the mechanics of non-prehensile contacts are subject to uncertainty.
This is due to the fact that dynamic effects such as friction between surfaces
are difficult to predict, especially when manipulating objects without knowing
their physical properties (Lynch and Mason (1995); Dogar and Srinivasa (2011);
Ha et al. (2020)). Thus, when generating open-loop plans by assuming an
accurate model of the dynamics, those plans are likely to fail as they do not
take into account the underlying uncertainty. In Rodriguez (2021), this effect
is described with an experiment of repeatedly picking-and-placing a queen
chess piece, which fails if the queen is picked from the top. In contrast,
picking the queen from the side stabilizes the repeated pick-and-place
process. This raises the question of how to let robots deal with this
uncertainty autonomously. Deploying fast feedback loops is an implicit
approach to compensating for uncertain dynamics, such as done in model-
predictive control schemes. Yet, modeling uncertainties and reasoning over an
anticipated distribution of outcomes, i.e. a belief, is a more explicit way of
coping with uncertain dynamics.

Figure 1:  Our model-based optimization approach synthesizes bi-manual pushing
trajectories by controlling the variance of the object configuration, without
explicitly modeling contact modes. We show that the robustness of the pushing
trajectories is sufficient to successfully push an object over long horizons.
The red cubes on the table and the yellow content of the can act as additional
perturbations to the contact dynamics.

In this article, we present an approach for modeling the uncertainty of
contact dynamics in order to synthesize robust manipulation behavior. Towards
this end, we make the following contributions:

i) We study how a belief over an object’s configuration propagates through
uncertain contact dynamics. We derive a prediction of the variance of object
configurations upon contact, allowing us to anticipate the reduction of
uncertainty without sampling perturbations to the contact dynamics.

ii) We introduce a contact prior for sampling candidate robot trajectories
that are likely to create contacts between the robot and the object.

iii) Last, we propose a sampling-based trajectory optimization algorithm that
constrains solutions to be robust based on the predicted variance (i)). The
informed trajectory distribution (ii)) serves as a proposal distribution that
guides the sampling-based optimization process.

In a real-world experiment we demonstrate that the proposed approach is able
to synthesize robust bi-manual pushing trajectories in only a few seconds of
planning time, consisting of long-horizon (up to 100 seconds) open-loop
pushing maneuvers that include making and breaking contacts (cf. Fig. 1). The
experimental results show that the interplay of i) and ii) is crucial for
synthesizing robust behavior. To the best of our knowledge, the proposed
algorithm is the first model-based planning approach that is able to
synthesize robust plans for contact-rich manipulation without pre-defined
manipulation primitives.

##  2 Related Work

###  2.1 Contact-rich Manipulation

To plan and control physical interactions such as contacts, model-based
algorithms aim to exploit these models to synthesize manipulation behavior.
Optimization-based approaches have shown successful manipulation planning
capabilities when the desired behavior can be found via local optimization
(Hogan and Rodriguez (2020); Aydinoglu and Posa (2022); Aydinoglu et al.
(2022); Cleac’h et al. (2021)) or when contact modes can be represented as a
small set of discrete decision variables (Marcucci et al. (2017); Migimatsu
and Bohg (2020); Toussaint et al. (2020, 2022); Chen et al. (2021)). In the
case of local optimization, however, most methods rely on gradients which
require a smoothed approximation of the contact dynamics. Moreover, they rely
on good initializations, as control actions that do not result in the robot
making contact with the object will yield a vanishing gradient of the
associated manipulation objective. In contrast, when reformulating the
manipulation problem as finding the optimal sequence of contact modes, the
difficulty lies in an combinatorial explosion when frequent switching of
contact modes is necessary, i.e. making-and-breaking contacts, or if multiple
contact points are involved.

Recently, sampling-based planning and control algorithms have been explored
for contact-rich manipulation tasks as a gradient-free approach to cope with
discontinuous cost landscapes. This offers a framework for combining
stochastic sampling and optimization, supporting the search over contact-rich
manipulation actions. In Pang et al. (2023), the authors propose to use a
quasi-dynamic contact model to efficiently simulate physical interactions
during manipulation. Plans are then synthesized with an adaptation of the RRT
algorithm. While the planning algorithm is able to generate contact-rich
manipulation plans including making-and-breaking contacts, the underlying
contact model is assumed to be accurate, resulting in plans that are not
robust to inaccuracies in the contact model. In another stream of research on
controlling contact-rich manipulation, sampling-based optimization has been
explored in model-predictive control schemes to exploit parallel computing
opportunities (Bhardwaj et al. (2022); Howell et al. (2022); Jankowski et al.
(2023)). In this article, we build upon our previous work on via-point-based
stochastic trajectory optimization (_VP-STO_) as a tool for efficiently
optimizing robot trajectories without requiring gradients of the manipulation
cost with respect to the manipulation action (Jankowski et al. (2023)).

###  2.2 Robust Manipulation

Figure 2: Analogies between active robot localization and robust manipulation.
Both can be formulated as a belief space planning problem, where the objective
is to decrease the uncertainty of the belief over time. While active
localization approaches are based on observation models, robust manipulation
exploits favorable contact dynamics to achieve the same goal.

In robotic manipulation, uncertainty is a dominant aspect that arises from the
complex and hybrid nature of modeling physical interactions (Rodriguez
(2021)). Robust manipulation aims at exploiting particular contact
configurations that naturally reduce errors in manipulation tasks. In Erdmann
and Mason (1988), the authors exploit the geometry of a tray with physical
boundaries to reorient a tool without sensory feedback. Lynch and Mason (1995)
exploit line contacts between a robot and polygonal objects to generate robust
pushing plans. In Dogar and Srinivasa (2011), the geometry of a half-open
gripper is exploited to actively funnel the probability distribution over
object locations between the two fingers and the palm with a push-grasp. Ha et
al. (2020) model the uncertainty in an underactuated system with additive
noise on the robot control actions and approximate the probability
distribution over state trajectories with a normal distribution around a
particular contact mode. Logic geometric programming is then used to find the
most robust contact mode from a set of pre-defined candidate modes. These
strategies have in common that favorable contact geometries are used to create
natural contact dynamics that effectively decrease the uncertainty of the
manipulation system over time without the need for sensors. However, the above
approaches rely on a pre-programmed set of robust behaviors that are tailored
to the robot contact geometry.

In this article, we achieve robust manipulation as the result of optimizing
over an object belief. Belief-space planning in robotics is concerned with
modeling the state of the robot and its work space via probability
distributions, e.g. for active localization. Sensor measurements of the robot
are then used to reject or confirm possible states based on an observation
model to reduce uncertainty. Fig. 2 illustrates the analogy of active
localization and robust manipulation. Both can be formulated as a belief-space
planning problem, where the common objective is to decrease the state-variance
over time. While the goal is to minimize the uncertainty of the robot’s own
state in localization problems, robust manipulation aims to minimize the
uncertainty of the object state. An even more important difference lies in the
way the belief is updated over time. Instead of using observation models to
rule out possible states of the robot, robust manipulation exploits natural
invariances in the contact dynamics to let possible object states converge to
a single state.

###  2.3 Modeling Uncertainty in Contact Dynamics

Modeling the uncertainty in contact dynamics is a key aspect for synthesizing
robust robot behavior. In many reinforcement learning approaches (Haarnoja et
al. (2019); Schulman et al. (2015, 2017)), domain randomization is a natural
way of informing the skill learning process about all the possibilities that
may be encountered when moving from simulation to reality. Domain
randomization may include probability distributions over parameters of the
dynamics model such as friction coefficients, object mass or object geometry
(Andrychowicz et al. (2020); Muratore et al. (2022)). By simulating a large
number of combinations of policy samples and domain samples, the policy
ideally converges to a behavior that is robust against the uncertainty modeled
through domain randomization. We believe that domain randomization as an
interface for modeling uncertainties in physical interactions is the key
reason why reinforcement learning techniques show more advanced manipulation
skills in the real world such as in-hand manipulation (Handa et al. (2023))
compared to model-based planning and control techniques that typically assume
an accurate model. Hence, we aim to bridge the gap between modeling
uncertainty in contact dynamics and ad-hoc planning and control techniques
that do not require data-inefficient offline training cycles by optimizing
over statistical properties of a belief without sampling perturbations.

##  3 Problem Formulation & Approach

We consider an underactuated manipulation system with $n_{\mathrm{dof}}^{r}$
actuated degrees of freedom for the robot, and $n_{\mathrm{dof}}^{o}$
unactuated degrees of freedom for the object to be manipulated. We are
interested in controlling the configuration of the object
$\bm{q}^{o}\in\mathbb{R}^{n_{\mathrm{dof}}^{o}}$ by executing robot control
commands $\bm{u}\in\mathbb{R}^{n_{\mathrm{dof}}^{r}}$. The initial object
configuration and the contact dynamics are subject to uncertainty, such that
the object configuration at time step $k$ is a random variable that is
described through the belief $b_{k}=p(\bm{q}^{o}_{k})$. We formulate the
planning problem as a stochastic optimal control problem over a horizon of $K$
time steps:

$\min_{\bm{u}_{0:K-1}}\mathrm{E}_{b_{K}}\left[\left(\bm{q}^{o}_{K}-\bm{q}^{o}_{\mathrm{des}}\right)^{\scriptscriptstyle\top}\left(\bm{q}^{o}_{K}-\bm{q}^{o}_{\mathrm{des}}\right)\right].$
(1)

We are optimizing for an open-loop control trajectory $\bm{u}_{0:K-1}$ such
that the expected control error at time step $K$ is minimal. In order to
evaluate the expected control error in (1) given a control trajectory, the
belief over object positions is to be propagated through the stochastic
contact dynamics. However, as contact dynamics are inherently non-smooth,
propagating the belief through contacts in closed-form is intractable. Another
way to explicitly evaluate the expected cost is to sample a large number of
stochastic rollouts. Yet, this is problematic due to the fact that the
evaluation of a single robot control trajectory becomes not only inefficient,
but also stochastic.

A key to our approach is the separation of the stochastic optimal control
problem in (1) into a mean control problem and a variance control problem
(Okamoto et al. (2018); Shirai et al. (2023)). The objective of the stochastic
optimal control problem is separated as follows:

$\min_{\bm{u}_{0:K-1}}\left(\mathrm{E}_{b_{K}}\left[\bm{q}^{o}_{K}\right]-\bm{q}^{o}_{\mathrm{des}}\right)^{\scriptscriptstyle\top}\left(\mathrm{E}_{b_{K}}\left[\bm{q}^{o}_{K}\right]-\bm{q}^{o}_{\mathrm{des}}\right)+\mathrm{V}_{b_{K}}\left[\bm{q}^{o}_{K}\right].$
(2)

We provide a more detailed derivation from (1) to (2) in the appendix. The
first term in (2) corresponds to the mean control problem, which refers to
planning for the expected object configuration
$\mathrm{E}_{b_{K}}\left[\bm{q}^{o}_{K}\right]$, i.e. the configuration that
is obtained if the mean initial configuration and the nominal contact dynamics
are used. Evaluating the mean control objective does not require the
propagation of the belief or the sampling of large numbers of stochastic
rollouts of the state. In other words, the mean control problem resembles a
deterministic optimal control problem that assumes an accurate dynamics model,
while the stochasticity in the original problem is captured by the variance
control problem represented by the second term in (2). Note that the variance
control problem, i.e. minimizing
$\mathrm{V}_{b_{K}}\left[\bm{q}^{o}_{K}\right]$, is independent of the desired
object configuration $\bm{q}^{o}_{\mathrm{des}}$. The variance of the object
configuration is defined as:

$\mathrm{V}_{b_{k}}[\bm{q}^{o}_{k}]=\mathrm{E}_{b_{k}}\left[\left(\bm{q}^{o}_{k}-\mathrm{E}_{b_{k}}[\bm{q}^{o}_{k}]\right)^{\scriptscriptstyle\top}\left(\bm{q}^{o}_{k}-\mathrm{E}_{b_{k}}[\bm{q}^{o}_{k}]\right)\right].$
(3)

Thus, the stochastic optimal control problem can be solved by controlling the
nominal object configuration while also steering its variance. The variance is
a scalar measure of the second statistical moment of the belief and can be
interpreted as the uncertainty that the system has about the object’s
configuration. However, note that computing the variance at time step $K$
still requires the propagation of the belief or a Monte-Carlo approximation of
the stochastic dynamics. In Sec. 4, we present an approach which approximates
the variance of the object configuration over time without sampling
perturbations to the contact dynamics, resulting in efficient and
deterministic rollouts of the nominal dynamics and the approximated variance.
In Sec. 5 and Sec. 6, we exploit the approximated variance in a sampling-based
trajectory optimization scheme for synthesizing robust robot trajectories.

##  4 Belief Dynamics through Contacts

Given a robot control action and a belief over an object’s configuration, we
are interested in predicting the mean and the variance of the object’s
configuration at the consecutive time step. For this, we first introduce a
quasi-static model for the object dynamics, i.e. predicting the nominal object
configuration given a robot configuration. We then model the uncertainty of
the object dynamics through additive perturbations on the object configuration
if the robot makes contact with the object. Last, we derive a deterministic
prediction of the variance given the current belief, a control action and the
statistical properties of the perturbations.

###  4.1 Stochastic Quasi-Static Dynamics for Pushing

Quasi-static and quasi-dynamic models have been used to simplify the
prediction of slow physical interactions between robots and objects (Mason
(2001); Koval et al. (2016); Hogan and Rodriguez (2020); Cheng et al. (2021);
Pang (2021); Pang et al. (2023)). Both classes of models assume that effects
that are related to velocities and accelerations can be neglected as they do
not affect the outcome of the prediction. This limits the range of
applications to slow interactions such as pushing tasks, insertion tasks or
in-hand manipulation. Yet, the benefits of quasi-static and quasi-dynamic
models are the lower dimensionality of the system state, i.e. half the number
of states compared to second-order dynamics models (e.g. Todorov et al.
(2012)), and the lower temporal resolution required to compute stable
predictions of the system state. Both aspects effectively allow for faster
simulated rollouts of robot plans and thus for more efficient model-based
trajectory optimization. Such models oftentimes denote the state as
$\bm{q}=\left(\bm{q}^{r},\bm{q}^{o}\right)$. It decomposes into the position
of the actuated degrees of freedom of the robot
$\bm{q}^{r}\in\mathbb{R}^{n_{\mathrm{dof}}^{r}}$, and the position of the
unactuated degrees of freedom of the object(s)
$\bm{q}^{o}\in\mathbb{R}^{n_{\mathrm{dof}}^{o}}$. The discretized dynamics are
consequently given in the form of

$\begin{pmatrix}\bm{q}^{r}_{+}\\\
\bm{q}^{o}_{+}\end{pmatrix}=\bm{f}\left(\begin{pmatrix}\bm{q}^{r}\\\
\bm{q}^{o}\end{pmatrix},\bm{u}\right).$ (4)

$\bm{q}^{r}_{+}$, $\bm{q}^{o}_{+}$ are the predicted robot and object
configurations at the consecutive time step, respectively. In Pang et al.
(2023), the input $\bm{u}\in\mathbb{R}^{n_{\mathrm{dof}}^{r}}$ is defined as
the commanded robot configuration. The robot is assumed to be controlled by a
low-level impedance controller (Hogan (1984)), such that the robot can be
modeled as an impedance for the contact dynamics. In this article, we further
simplify the quasi-dynamic contact dynamics in Pang et al. (2023) by assuming
infinite stiffness of the controlled robot. As a consequence, contacts with
objects are assumed to not affect the robot state itself, but only the
configuration of the object. This assumption is particularly realistic when
pushing lightweight objects with a stiff robot impedance controller. The high-
stiffness assumption induces that the robot is able to reach a desired robot
position even when being in contact with an object, i.e.
$\bm{q}^{r}_{+}=\bm{u}$. The benefit of modeling the contact interactions in
such a way is that the joint robot-object dynamics reduce to solely object
dynamics. This further simplifies the simulation of contacts, as the non-
penetration constraint in the dynamics now only applies to the object. This
turns the quasi-dynamic contact dynamics into quasi-static contact dynamics,
as we remove the dependency on time. We will further exploit this decoupling
of the dynamics in the subsequent derivation of transition probabilities for
the object. Yet, note that this assumption breaks when contacts between the
robot and the environment significantly affect the robot state, e.g. when
trying to move into a solid wall. Furthermore, enclosing grasps, i.e. making
contact with one object from two opposing sides, will also break the high-
stiffness assumption as the two contact points will affect each other. Due to
these limitations, we focus the experiments on planar pushing under
uncertainty.

####  4.1.1 Nominal Object Dynamics.

The discretized quasi-static contact dynamics of the object are

$\begin{pmatrix}\bm{q}^{r}_{+}\\\
\bm{q}^{o}_{+}\end{pmatrix}=\begin{pmatrix}\bm{u}\\\
\bm{q}^{o}+\delta\bm{q}^{o}\end{pmatrix};$ (5a)
$\delta\bm{q}^{o}=\operatorname*{arg\,min}_{\delta\tilde{\bm{q}}^{o}}{\delta\tilde{\bm{q}}^{o}}^{\scriptscriptstyle\top}\bm{M}(\bm{q}^{o})\delta\tilde{\bm{q}}^{o},$
(5b) $\mathrm{s.t.}\quad d(\bm{u},\bm{q}^{o}+\delta\tilde{\bm{q}}^{o})\geq 0.$
(5c)

The control inputs $\bm{u}\in\mathbb{R}^{n_{\mathrm{dof}}^{r}}$ to the system
dynamics are defined by the commanded joint positions of the robot. In (5b),
$\bm{M}(\bm{q}^{o})$ is the inertia matrix of the object with respect to its
current configuration $\bm{q}^{o}$. Thus, the objective in (5b) aims to
minimize the work required to overcome the friction between the object surface
and the surface of the environment, e.g. the table the object is placed on.
$d(\bm{q}^{r},\bm{q}^{o})$ measures the shortest signed distance between the
robot and the object and thus (5c) incorporates the non-penetration
constraint, i.e. the robot does not penetrate a rigid object. As the quasi-
static robot dynamics in (5a) are decoupled from the object configuration, the
non-penetration constraint in (5c) does not depend on the previous robot
state, but only on the control input $\bm{u}$. This simplification allows us
to represent the system dynamics with the next desired robot position as the
control input and the object position as the sole state variable. The quasi-
static contact dynamics of the object in (5a)–(5c) can thus be summarized into
the nominal forward dynamics of the object configuration:

$\bm{q}^{o}_{+}=\bm{f}(\bm{q}^{o},\bm{u}).$ (6)

In the following, we exploit the simplified mathematical structure of the
quasi-static contact dynamics to model uncertainty and to analyze how object
belief states propagate through the contact dynamics.

####  4.1.2 Noisy Object Dynamics.

We propose to model the uncertainty in the contact dynamics as additive noise
that acts as a perturbation to the nominal quasi-static contact dynamics in
(6). Hence, the noisy object dynamics are given by

$\bm{q}^{o}_{+}=\bm{f}(\bm{q}^{o},\bm{u})+\eta\bm{w}.$ (7)

The perturbation $\bm{w}$ is assumed to be sampled from a probability
distribution $p_{\bm{w}}$ such that the resulting object configuration
satisfies the non-penetration constraint in (5c). The noise coefficient $\eta$
encodes if the robot is in contact with the object and is defined as

$\eta=\begin{cases}0\quad\quad\mathrm{if}\,d(\bm{u},\bm{q}^{o})>0\;\mathrm{(no\;contact)},\\\
1\hfill\mathrm{else}\;\mathrm{(contact)}.\end{cases}$ (8)

Hence, the model only considers uncertainty in the prediction of the object
configuration when the robot is manipulating the object. The presented
formulation of the noisy discrete-time contact dynamics in (7) captures two
distinct modes: one when the robot is in contact with the object ($\eta=1$)
and another when the robot is not in contact with the object ($\eta=0$). Note
that we do not model different contact modes. The contact mode $\eta=1$
captures arbitrary numbers of contact points with arbitrary contact
geometries. The object dynamics can thus be formulated with

$\bm{q}^{o}_{+}=\begin{cases}\bm{q}^{o}\quad\quad\quad\mathrm{if}\,\eta=0\;\mathrm{(no\;contact)},\\\
\bm{f}(\bm{q}^{o},\bm{u})+\bm{w}\hfill\mathrm{else}\;\mathrm{(contact)}.\end{cases}$
(9)

The no-contact mode entails that the object configuration does not change.
This mode is not subject to uncertainty.

###  4.2 Object Belief Dynamics

Figure 3: Belief dynamics through contact in a one-dimensional example. a)
Illustration of object samples from the uniformly distributed initial belief
(orange). The ground truth object is depicted in non-transparent orange. b)
After executing a push from left to right, all samples that were on the left
of the push were pushed by the robot. c) The probability mass that was on the
left-hand side of the push (light-blue) is now concentrated in a distribution
at the contact point according to the perturbation distribution (blue). The
probability mass on the right-hand side of the push does not change (blue). d)
The variance of the predicted object position is therefore a function of the
control action, where a robust control action may decrease the variance over
time.

In order to analyze how the belief about an object configuration and its
associated uncertainty propagate through contact dynamics over time, we use
the notation of probabilistic state transitions. Building upon the derived
contact dynamics in (7), the state of the stochastic system is the object
configuration and the action is the next desired robot configuration.
Accordingly, we denote the state transition probability with

$\bm{q}^{o}_{+}\sim p(\cdot|\bm{q}^{o},\bm{u}),$ (10)

describing the probability distribution over object configurations at the next
time step given the commanded robot configuration as well as the object
configuration. Note that the random perturbation $\bm{w}$ that directly acts
on the object dynamics in (7) is captured by the stochasticity of the
transition probability in (10). We furthermore denote the probability
distribution over object configurations as belief $b=p(\bm{q}^{o})$. Given the
transition probability in (10), a control action $\bm{u}$ and the object
belief $b$, the resulting belief $b_{+}$ can be predicted by marginalizing
over the initial object configuration:

$b_{+}=p(\bm{q}^{o}_{+}|\bm{u})=\int_{\mathcal{Q}^{o}}p(\bm{q}^{o}_{+}|\bm{q}^{o},\bm{u})\;b\;\mathrm{d}\bm{q}^{o}.$
(11)

This equation represents the belief dynamics, which can be used not only for
predicting the most likely object configuration, but also the variance of the
belief after executing action $\bm{u}$. However, solving the integral in (11)
is typically intractable for non-linear dynamics and non-Gaussian beliefs.
Yet, in the following, we start by showing an example problem for which we can
obtain a closed-form solution of (11), followed by the introduction of a
general approximation.

Example 1. (1D Box-Pushing). We illustrate the effects of contacts on the
belief with a one-dimensional pushing example as illustrated in Fig. 3. The
hand (i.e. the robot) can be controlled directly while the box on the table
(i.e. the object) can only be moved by making contact. The quasi-static
contact dynamics in this example simplify to

$q^{o}_{+}=\begin{cases}q^{o}\quad\,\mathrm{if}\,q^{o}>u\;\mathrm{(no\;contact)},\\\
u+w\quad\mathrm{else}\hfill\mathrm{(contact)}.\end{cases}$ (12)

These one-dimensional piece-wise linear dynamics move the object to the right-
hand side of the contact. The object does not move if no contact has been
made. In this example, we consider the perturbation of the contact dynamics to
be uniformly distributed, i.e. $w\sim\mathcal{U}_{[0,\alpha]}$. Sub-figures a)
and b) illustrate a particular instance of the object being at $q^{o}$, which
is then moved to $q^{o}_{+}$ due to the robot reaching the commanded position
$u$. However, in this example the initial box position is subject to
uncertainty. The initial belief $b$ over box positions being given as a
uniform distribution on an interval between box position $\underline{q}^{o}$
and $\bar{q}^{o}$, respectively. The interval is indicated by the vertical
dashed lines in all sub-figures. The initial belief is

$b=\mathcal{U}_{[\underline{q}^{o},\bar{q}^{o}]}(q^{o}).$ (13)

Given a control action $u\in[\underline{q}^{o},\bar{q}^{o}]$ as a commanded
robot position, we can now predict the belief after contact by solving (11).
While solving the integral in (11) is typically intractable, this example has
a closed-form solution that is given by

$b_{+}=\frac{u-\underline{q}^{o}}{\bar{q}^{o}-\underline{q}^{o}}\mathcal{U}_{[u,u+\alpha]}(q^{o}_{+})+\frac{\bar{q}^{o}-u}{\bar{q}^{o}-\underline{q}^{o}}\mathcal{U}_{[u,\bar{q}^{o}]}(q^{o}_{+}).$
(14)

We illustrate the belief dynamics in sub-figure c) by visualizing the initial
belief in light-blue and the resulting belief in dark-blue. It can be seen
that the contact dynamics result in a concentration of probability mass around
the contact point. This is due to the fact that all probability mass to the
left of the contact has moved to the same position interval, i.e. the contact
point subject to perturbation. In the extreme case of pushing all the way
through the interval of the uniform distribution, i.e. $u=\bar{q}^{o}$, the
object belief is equivalent to the perturbation distribution with the mean
shifted to the contact point. The closed-form belief dynamics in (14) show
that the probability distribution can be controlled by the actions a robot
takes. This is underlined by sub-figure d) plotting the variance $\mathrm{V}$
of the object position after the push as a function of how far the robot
pushed. Note that while in this specific example any control action $u$ that
makes contact with the object reduces the variance of the belief over its
position, this may not be the case in general. An unfavorable control action
could generally also increase the variance of the belief.

###  4.3 Variance Prediction.

While the 1D-example above showed a closed-form solution to the belief
dynamics, this is typically intractable. Consequently, predicting the variance
with

$\mathrm{V}_{+}=\mathrm{V}_{b_{+}}\left[\bm{q}^{o}\right],$ (15)

in closed-form is also intractable. A common approach is to use a Monte-Carlo
approximation of the belief dynamics in (11) in order to compute the variance
of the approximated belief update (Kappen (2015); Shirai et al. (2023)).
However, this is problematic since this induces stochasticity in the
prediction of the variance $\mathrm{V}_{+}$. Evaluating whether a control
action reduces or increases the variance may thus be subject to uncertainty
itself, which introduces numerical issues in downstream optimization
techniques. The induced noise furthermore depends on the number of samples
used to approximate the belief. Thus, numerical problems and approximation
errors may only be avoided by choosing a high number of samples, rendering the
underlying planning technique inefficient. In the following, we derive a
different way of predicting the variance of object configurations without
depending on the transition probability. In other words, we eliminate the need
to sample from the belief transition distribution in (10) in order to predict
the variance given a robot action.

Instead of predicting the updated variance after taking a control action by
computing the variance of the predicted belief as in (15), we compute the
variance of the noisy object dynamics in (7), i.e.

$\displaystyle\begin{split}\mathrm{V}_{+}&=\mathrm{V}_{b,p_{w}}[\bm{f}(\bm{q}^{o},\bm{u})+\eta\bm{w}]\\\
&=\mathrm{V}_{b}[\bm{f}(\bm{q}^{o},\bm{u})]+\mathrm{V}_{b,p_{w}}[\eta\bm{w}]+\\\
2&\mathrm{E}_{b,p_{w}}\left[(\bm{f}(\bm{q}^{o},\bm{u})-\mathrm{E}_{b}[\bm{f}(\bm{q}^{o},\bm{u})])^{\scriptscriptstyle\top}(\eta\bm{w}-\mathrm{E}_{b,p_{w}}[\eta\bm{w}])\right].\end{split}$
(16)

At this point, we introduce the assumption that the expectation of the
perturbation $\bm{w}$ is zero, i.e. $\mathrm{E}_{p_{w}}[\bm{w}]=\bm{0}$. As a
result, perturbations are modeled as zero-mean noise in the tangential
directions with respect to the contact point and zero noise in the normal
direction of the contact. We furthermore note that the contact indicator
$\eta$ and the perturbation are independent, such that the expectation of the
product of the contact indicator and the perturbation is zero, i.e.
$\mathrm{E}_{b,p_{w}}[\eta\bm{w}]=\mathrm{E}_{b}[\eta]\mathrm{E}_{p_{w}}[\bm{w}]=\bm{0}$.
Consequently, the third term in (16) simplifies to

$\displaystyle\begin{split}&\mathrm{E}_{b,p_{w}}\left[(\bm{f}(\bm{q}^{o},\bm{u})-\mathrm{E}_{b}[\bm{f}(\bm{q}^{o},\bm{u})])^{\scriptscriptstyle\top}(\eta\bm{w}-\mathrm{E}_{b,p_{w}}[\eta\bm{w}])\right]=\\\
&\mathrm{E}_{b,p_{w}}\left[\eta(\bm{f}(\bm{q}^{o},\bm{u})-\mathrm{E}_{b}[\bm{f}(\bm{q}^{o},\bm{u})])^{\scriptscriptstyle\top}\bm{w}\right]=\\\
&\mathrm{E}_{b}\left[\eta(\bm{f}(\bm{q}^{o},\bm{u})-\mathrm{E}_{b}[\bm{f}(\bm{q}^{o},\bm{u})])\right]^{\scriptscriptstyle\top}\mathrm{E}_{p_{w}}\left[\bm{w}\right]=\bm{0}.\end{split}$
(17)

As a result, the predicted variance in (16) is equivalent to the sum of the
nominally predicted variance and the variance of the induced noise. The
predicted variance is thus

$\mathrm{V}_{+}=\mathrm{V}_{b}\left[\bm{f}(\bm{q}^{o},\bm{u})\right]+\mathrm{V}_{b,p_{w}}\left[\eta\bm{w}\right].$
(18)

The left-hand term is computed through the nominal object dynamics $\bm{f}$,
thus not including perturbations. The right-hand term is the variance
contribution from the noise acting on the object when making contact. With the
expectation of $p_{w}$ being zero, i.e.
$\mathrm{E}_{b,p_{w}}[\eta\bm{w}]=\bm{0}$, the variance of the noise is given
with

$\displaystyle\begin{split}&\mathrm{V}_{b,p_{w}}\left[\eta\bm{w}\right]=\mathrm{E}_{b,p_{w}}\left[(\eta\bm{w})^{\scriptscriptstyle\top}(\eta\bm{w})\right]=\\\
&\mathrm{E}_{b,p_{w}}\left[\eta^{2}\bm{w}^{\scriptscriptstyle\top}\bm{w}\right]=\mathrm{E}_{b}\left[\eta^{2}\right]\mathrm{E}_{p_{w}}\left[\bm{w}^{\scriptscriptstyle\top}\bm{w}\right].\end{split}$
(19)

With $\eta=\eta^{2}\in\\{0,1\\}$, the expectation of the squared contact
indicator is equal to the expected contact indicator, i.e.
$\mathrm{E}_{b}\left[\eta^{2}\right]=\mathrm{E}_{b}\left[\eta\right]$. The
expected value of the contact indicator is the probability of the robot making
contact with the object given a belief over object positions and a control
action. Furthermore, note that due to the zero-mean property of our noise
distribution, the expectation of the squared perturbation is equivalent to the
variance of the perturbation, i.e.
$\mathrm{E}_{p_{w}}\left[\bm{w}^{\scriptscriptstyle\top}\bm{w}\right]=\mathrm{V}_{\bm{w}}$.
Inserting these equalities in (19), the variance of the applied perturbation
can be expressed with

$\mathrm{V}_{b,p_{w}}\left[\eta\bm{w}\right]=\mathrm{E}_{b}\left[\eta\right]\mathrm{V}_{\bm{w}}.$
(20)

As a result of this section, we predict the variance of the object
configuration with

$\mathrm{V}_{+}=\mathrm{V}_{b}\left[\bm{f}(\bm{q}^{o},\bm{u})\right]+\mathrm{E}_{b}\left[\eta\right]\mathrm{V}_{\bm{w}}.$
(21)

The result in (21) is central to our contribution, as this allows us to
predict the variance of the object configuration, i.e. the uncertainty, based
on the variance of the perturbation $\mathrm{V}_{\bm{w}}$ that is assumed to
be a constant value. Especially when those perturbations are constrained to
the tangent space of the contact, sampling consistent perturbations involves
expensive computations and makes the prediction of the variance stochastic.

####  4.3.1 Monte-Carlo Approximation of the Nominal Dynamics.

Figure 4: Block diagram depicting one iteration of BS-VP-STO. The algorithm
starts with sampling a population of latent candidate trajectory variables
$\bm{\varepsilon}$. These are then decoded into robot trajectories
$\bm{q}^{r}_{0:K}$ using a contact prior. For each candidate trajectory the
object belief is rolled-out using the nominal object dynamics. The variance
gain together with the mean control cost is then used to compute the fitness
of each candidate trajectory. Finally, the distribution of candidate
trajectory variables weighted by the fitness is used to update the Gaussian
approximation of the probability distribution using CMA-ES. After $M$
iterations, the algorithm returns the best performing candidate trajectory as
solution.

We approximate the variance contribution resulting from the nominal contact
dynamics using a non-parametric representation of the belief, i.e. particles.
We denote the approximated belief as a set of $N_{p}$ particles
$\\{{}^{i}\bm{q}^{o},{}^{i}\alpha\\}_{i=1}^{N_{p}}$, where each particle
consists of a state sample ${}^{i}\bm{q}^{o}$ and a corresponding weight
${}^{i}\alpha$. The weight of a particle, $0\leq{}^{i}\alpha\leq 1$,
represents an approximate belief in the corresponding state sample
${}^{i}\bm{q}^{o}$. A prediction step consists of propagating each particle
through the nominal contact dynamics, i.e.

${}^{i}\bm{q}^{o}_{+}=\bm{f}({}^{i}\bm{q}^{o},\bm{u}),\,\forall
i\in\\{1,2,..,N_{p}\\}.$ (22)

Since we do not take any measurements during planning, we assume that the
particle weights are constant and equally distributed, such that
${}^{i}\alpha=\nicefrac{{1}}{{N_{p}}},\;\forall i$. Based on the particle-
representation of the belief, we estimate the variance of the object
configuration with

$\hat{\mathrm{V}}_{b}[\bm{q}^{o}]=\frac{1}{N_{p}}\sum_{i=1}^{N_{p}}\left({}^{i}\bm{q}^{o}-\bm{\mu}^{o}\right)^{\scriptscriptstyle\top}\left({}^{i}\bm{q}^{o}-\bm{\mu}^{o}\right).$
(23)

The empirical mean object configuration is computed with
$\bm{\mu}^{o}=\nicefrac{{1}}{{N_{p}}}\sum_{i=1}^{N_{p}}{}^{i}\bm{q}^{o}$. The
probability of making contact $\mathrm{E}_{b}\left[\eta\right]$ is
approximated as

$\hat{\mathrm{E}}_{b}\left[\eta\right]=\frac{1}{N_{p}}\sum_{i=1}^{N_{p}}{}^{i}\eta,$
(24)

where ${}^{i}\eta$ indicates if the $i$-th particle has an object
configuration that is in contact with the robot. As a result, we approximate
the overall variance prediction with

$\hat{\mathrm{V}}_{+}=\hat{\mathrm{V}}_{b}\left[\bm{f}(\bm{q}^{o},\bm{u})\right]+\hat{\mathrm{E}}_{b}\left[\eta\right]\mathrm{V}_{\bm{w}}.$
(25)

When comparing the predicted variance $\hat{\mathrm{V}}_{+}$ against the
variance of the current time step $\hat{\mathrm{V}}_{b}[\bm{q}^{o}]$, the same
set of particles is used to compute the empirical variance as in (23). Thus,
evaluating the approximated variance dynamics does not involve sampling and is
thus deterministic. We exploit this approximation in Sec. 5 when optimizing
robot trajectories based on the predicted variance.

##  5 Stochastic Trajectory Optimization for Robust Manipulation

Given our objective to push an object into a desired goal configuration
subject to stochastic contact dynamics (cf. (2)), this section presents a
framework that optimizes for robust robot trajectories directly in the belief
space over possible object configurations. This framework extends our previous
work VP-STO (Jankowski et al. (2023)) to belief-space via-point-based
stochastic trajectory optimization (BS-VP-STO). Our framework exploits the
variance prediction developed in Sec. 4 for synthesizing robust manipulation
behavior. Due to the quasi-static pushing model in (5a) - (5c), a robot
trajectory is equivalent to a control trajectory, i.e.
$\bm{q}^{r}_{1:K}=\bm{u}_{0:K-1}$. Thus, BS-VP-STO is a shooting method aiming
at minimizing an objective that depends solely on the object configuration as
in (1). Due to the non-smooth nature of the contact dynamics and the resulting
non-smooth cost function with respect to the optimization variable, we
approach the optimization problem with a gradient-free, i.e. zero-order,
evolutionary optimization technique.

Fig. 4 illustrates the optimization loop that is based on zero-order
optimization of the variable $\bm{\varepsilon}$, which uniquely encodes a
robot trajectory. At the beginning of the $m$-th iteration, we sample
$N_{\mathrm{cand}}$ candidate trajectories from a latent Gaussian distribution
that represents the current solution to the optimization problem:

$\bm{\varepsilon}^{j}\sim\mathcal{N}(\bar{\bm{\varepsilon}}_{m},\bm{\Sigma}_{m}),\,\forall
j\in\\{1,2,\ldots,N_{\mathrm{cand}}\\}.$ (26)

The latent variable $\bm{\varepsilon}_{j}$ translates to a robot trajectory
through an affine mapping $g$, i.e.
$\bm{u}^{j}_{0:K-1}=g(\bm{\varepsilon}_{j})$. This affine mapping imposes a
contact prior on the sampling of robot trajectories, which is presented in
Sec. 5.2. Given a candidate robot trajectory $\bm{u}^{j}_{0:K-1}$ and an
initial belief over object positions $b_{0}=p(\bm{q}^{o})$, we compute the
nominal belief dynamics. This results in a nominal belief trajectory
$\tilde{b}^{j}_{0:K}$ for each candidate $\bm{\varepsilon}_{j}$. Based on the
nominal belief trajectory we compute the step-wise predicted variance as
developed in Sec. 4. The predicted variance is subsequently used in a cost and
a constraint to the optimization problem, which is further outlined in Sec.
5.1. Together with a cost for controlling the mean of the object
configuration, the total cost of each candidate trajectory is used to update
the parameters of the latent Gaussian distribution, i.e.
$\bar{\bm{\varepsilon}}_{m+1},\bm{\Sigma}_{m+1}$ based on Covariance Matrix
Adaptation (CMA-ES) (Hansen (2016)). After $M$ iterations, we return the best
performing sample that returned the lowest cost.

###  5.1 Variance Gain Control

In Sec. 4 we derive an approximation of the one-step prediction of the
variance of the object configuration. However, this does not enable the
prediction of the variance after multiple time-steps, which is due to the fact
that the prediction of the variance $\hat{\mathrm{V}}_{k+1}$ in (25) requires
the belief of the previous time step $b_{k}$ to be known. Therefore, instead
of directly controlling the variance at the end of the trajectory
$\hat{\mathrm{V}}_{K}$, we propose to control the predicted variance at each
time step. Given a robot trajectory $\bm{u}^{j}_{0:K-1}$, we thus compute the
nominal belief over object configurations at each time step, i.e.
$\tilde{b}_{k}$, via the the nominal object dynamics in (6). Given the
particle set that approximates the initial belief $b_{0}=p(\bm{q}^{o})$ as an
input to BS-VP-STO, the nominal belief rollout is computed by applying the
nominal forward dynamics to all particles:

${}^{i}\bm{q}^{o}_{k+1}=\bm{f}\left({}^{i}\bm{q}^{o}_{k},\bm{u}_{k}\right),\,\forall
i\in\\{1,2,..,N_{p}\\}.$ (27)

The one-step prediction of the variance at each time step can then be computed
as

$\displaystyle\begin{split}\hat{\mathrm{V}}_{k+1}&=\hat{\mathrm{V}}_{\tilde{b}_{k}}\left[\bm{f}(\bm{q}^{o},\bm{u}_{k})\right]+\hat{\mathrm{E}}_{\tilde{b}_{k}}\left[\eta\right]\mathrm{V}_{\bm{w}}\\\
&=\hat{\mathrm{V}}_{\tilde{b}_{k+1}}\left[\bm{q}^{o}\right]+\hat{\mathrm{E}}_{\tilde{b}_{k}}\left[\eta\right]\mathrm{V}_{\bm{w}}.\end{split}$
(28)

In order to quantify the change of uncertainty due to a given control action
$\bm{u}_{k}$, we are interested in the amount of variance that is gained over
one time step. Note that the variance of a continuous random variable is
closely related to its differential entropy. While the variance as defined in
(3) is equivalent to the trace of the covariance matrix, i.e. the sum over all
eigenvalues, the upper bound of the differential entropy is monotonic in the
determinant of the covariance matrix, i.e. the product over all eigenvalues
(Cover and Thomas (2005)). As a result, the variance of a continuous random
variable yields an upper bound for its entropy. Therefore, we introduce a new
metric $\gamma$, which we call variance gain, measuring the relative change of
the variance after applying an action $\bm{u}_{k}$, i.e.

$\gamma_{k}=\frac{\hat{\mathrm{V}}_{k+1}^{+}}{\hat{\mathrm{V}}_{\tilde{b}_{k}}\left[\bm{q}^{o}\right]+\mathrm{V}_{\bm{w}}}=\frac{\hat{\mathrm{V}}_{\tilde{b}_{k+1}}\left[\bm{q}^{o}\right]+\hat{\mathrm{E}}_{\tilde{b}_{k}}\left[\eta\right]\mathrm{V}_{\bm{w}}}{\hat{\mathrm{V}}_{\tilde{b}_{k}}\left[\bm{q}^{o}\right]+\mathrm{V}_{\bm{w}}}.$
(29)

The variance gain $\gamma$ is the ratio of the output variance, i.e. the
predicted variance $\hat{\mathrm{V}}_{k+1}$, to the input variance, i.e.
$\hat{\mathrm{V}}_{k}+\mathrm{V}_{\bm{w}}$. Fig. 5 illustrates three different
robot actions resulting in different variance gains. It shows that the contact
geometry plays a crucial role when planning to make contact between the robot
and an object. The variance gain $\gamma$ reflects that the contact geometry
affects the robustness of a contact, e.g. when pushing. The left sub-figure
shows that using one finger for pushing a circular object with uncertain
location results in an increase of variance ($\gamma>1$). In contrast, the
right sub-figure shows that using two fingers with one finger pushing the
object towards the other finger results in a decrease of variance
($\gamma<1$). Using a flat contact surface to push an object with uncertain
location keeps the variance constant as shown in the middle sub-figure
($\gamma=1$). Note that the variance gain is lower or equal to one for robot
actions that have zero probability of making contact with the object. In this
case, the belief does not change, i.e.
$\hat{\mathrm{V}}_{\tilde{b}_{k+1}}\left[\bm{q}^{o}\right]=\hat{\mathrm{V}}_{\tilde{b}_{k}}\left[\bm{q}^{o}\right]$,
since no perturbation is injected into the belief, i.e.
$\hat{\mathrm{E}}_{\tilde{b}_{k}}\left[\eta\right]\mathrm{V}_{\bm{w}}=0$.
Thus, a no-contact action results in

$\gamma_{\mathrm{no-
contact}}=\frac{\hat{\mathrm{V}}_{\tilde{b}_{k}}\left[\bm{q}^{o}\right]}{\hat{\mathrm{V}}_{\tilde{b}_{k}}\left[\bm{q}^{o}\right]+\mathrm{V}_{\bm{w}}}\leq
1.$ (30)

Figure 5: Belief dynamics through contact in a two-dimensional example. The
three sub-figures on the right illustrate the predicted belief $b_{+}$ via
samples in orange. All three cases started from the same initial belief $b$
that is depicted in the left-most sub-figure. The visualization of the
prediction on the left shows an increase of the variance of the object
position as a consequence of pushing with a single contact point ($\gamma>1$).
The second prediction shows a constant variance as a consequence of pushing
with a flat contact surface ($\gamma=1$). The right-most sub-figure shows a
decrease of the variance of object position as a consequence of pushing with
two contact points ($\gamma<1$).

We propose to enforce robustness in the optimization problem by constraining
the solution to variance gains smaller or equal to one at all steps, i.e.

$\gamma_{k}\leq 1\quad\forall\,k.$ (31)

Due to the zero-order technique that we use for optimizing the robot
trajectory, we deploy the robustness constraint as a barrier cost, i.e. a
discontinuous cost that is zero if the constraint is satisfied and returns a
high value if the constraint is violated. We can further reduce the predicted
variance by adding a cost term that is active if the constraint is already
satisfied. This is encapsulated in the following cost term

$c_{\mathrm{robust}}=\lambda_{c}\prod_{k=0}^{K-1}e^{-\frac{1-\gamma_{k}}{K-1}},$
(32)

with a discontinuous cost weight

$\lambda_{c}=\begin{cases}1\quad\quad\mathrm{if}\max_{k}\gamma_{k}\leq 1,\\\
10^{3}\hfill\mathrm{else}.\end{cases}$ (33)

The total cost of a candidate robot trajectory is computed as the sum of the
variance gain control cost in (32) and a task-specific cost
$c_{\mathrm{task}}$ that computes the deviation of the mean of the object
configuration to the desired goal configuration.

###  5.2 Trajectory Sampling with a Contact Prior

Figure 6: Comparison of uninformed and informed sampling of robot
trajectories. The left sub-figure shows trajectory samples drawn from a
probability distribution computed without a contact prior
($\bm{Q}_{\bm{q}}=\bm{0}$). The right sub-figure shows trajectory samples
drawn from a probability distribution computed with a contact prior
($\bm{Q}_{\bm{q}}>\bm{0}$). The contact prior guides the sampling of robot
trajectories towards regions where the robot is likely to make contact with
the object given the object belief.

The efficiency of sampling-based optimization algorithms depend on the quality
of the generated samples. In this section, we present how the object belief
can be used to inform the sampling of robot trajectories for manipulation
tasks. In general, it is desirable to sample trajectories with a likelihood
that is proportional to the negative cost that can be expected from executing
the trajectory. In the following, we denote a robot trajectory with
$\bm{q}^{r}_{0:K}=\left[\bm{q}^{r}_{0},\bm{q}^{r}_{1},\ldots,\bm{q}^{r}_{K}\right]$,
where $K$ corresponds to the number of discretized steps captured by the
trajectory. Suppose that the cost is given as a function of the robot
trajectory, i.e. $c=f_{c}(\bm{q}^{r}_{0:K})$, then we would like to sample
robot trajectories from a corresponding probability distribution with

$p(\bm{q}^{r}_{0:K})\propto\exp\left(-f_{c}(\bm{q}^{r}_{0:K})\right).$ (34)

While we do not have access to such a generative probability distribution, we
approximate it with a prior that improves the sample-efficiency of the
optimization algorithm compared to sampling initial guesses from an uninformed
probability distribution. The idea is to sample robot trajectories in regions
where the robot is likely to make contact with the object given its belief.
This is motivated by the observation that making contact is a necessary
precondition of manipulating an object.

Figure 6 illustrates the impact of the contact prior on trajectory samples.
Without the contact prior, a large portion of the candidate trajectories
explores regions in which the robot does not move into the object belief, and
thus does not contribute to the optimization process.

####  5.2.1 Via-point-based Trajectory Parameterization.

In order to efficiently synthesize robot trajectories in a low-dimensional
space, we adopt the via-point-based trajectory representation as in Jankowski
et al. (2023). The robot configuration is given with

$\bm{q}^{r}(t)=\bm{\Phi}_{\mathrm{via}}(t)\bm{\theta}+\bm{\phi}_{0}(t,\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r}),$
(35)

where the robot trajectory is parameterized with

$\bm{\theta}=\begin{pmatrix}\bm{q}^{1}_{\mathrm{via}}\\\ \vdots\\\
\bm{q}_{\mathrm{via}}^{N}\end{pmatrix}\in\mathbb{R}^{N\cdot
n_{\mathrm{dof}}^{r}}.$ (36)

The trajectory parameter $\bm{\theta}$ contains $N$ via-points the trajectory
passes through. The basis functions $\bm{\Phi}_{\mathrm{via}}(t)$ enforce that
the trajectory passes exactly through the via configurations while smoothly
interpolating with minimal acceleration. The basis functions furthermore
enforce that the velocity at the end of the trajectory is zero. Note that the
last $n_{\mathrm{dof}}^{r}$ elements of $\bm{\theta}$ are the final robot
configuration at the end of the trajectory, i.e.
$\bm{q}^{r}(T)=\bm{q}_{K}^{r}=\bm{q}_{\mathrm{via}}^{N}$. The basis offset
$\bm{\phi}_{0}(t,\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})$ incorporates the
initial robot position $\bm{q}_{0}^{r}$ and velocity with
$\dot{\bm{q}}_{0}^{r}$. $T$ denotes the duration of the trajectory. We use the
time scaling algorithm in Jankowski et al. (2023) for computing the duration
of a trajectory based on a given parameter $\bm{\theta}$ such that user-
defined velocity and acceleration limits are enforced. For implementation
details on how to compute the basis functions and offsets, please refer to
Jankowski et al. (2022).

In the following, we are interested in computing a Gaussian distribution of
the via-points $\bm{\theta}$ to efficiently sample from. Due to the affine
mapping from via-points to robot trajectories in (35), this corresponds to
sampling from a Gaussian distribution of continuous robot trajectories.

####  5.2.2 Gaussian Contact Prior.

The contact prior is a probability distribution that guides the sampling of
robot trajectories towards regions where the robot is likely to make contact
with the object. To allow for variations in how to approach the contact with
the object, we only consider the final robot configuration of the trajectory
to be subject to the contact prior. This is incorporated by exploiting the
parameterization of the robot trajectory in (35), where the final robot
configuration is explicitly given by the last $n_{\mathrm{dof}}^{r}$ elements
of $\bm{\theta}$.

Suppose that a conditional Gaussian distribution

$p_{\mathrm{c}}(\bm{q}^{r}|\bm{q}^{o})=\mathcal{N}(\bm{f}_{\mathrm{c}}(\bm{q}^{o}),\bm{\Sigma}^{r|o})$
(37)

approximates the probability density of a robot configuration making contact
with the object. Furthermore, suppose that the object configuration is
Gaussian distributed as well with
$\bm{q}^{o}\sim\mathcal{N}(\bm{\mu}^{o},\bm{\Sigma}^{o})$. Practically, we
find the Gaussian distribution of object configurations by approximating the
initial belief $b_{0}$ with a Gaussian distribution for computing the contact
prior. This lets us compute a probability distribution over robot
configurations indicating how likely it is to establish a contact between the
robot and the object:

$p_{\mathrm{c}}(\bm{q}^{r})=\mathcal{N}(\bm{f}_{\mathrm{c}}(\bm{\mu}^{o}),\bm{\Sigma}^{r|o}+\bm{A}\bm{\Sigma}^{o}\bm{A}^{\scriptscriptstyle\top}),$
(38)

with
$\bm{A}=\nicefrac{{\partial\bm{f}_{\mathrm{c}}}}{{\partial\bm{q}^{o}}}|_{\bm{\mu}^{o}}$.
The corresponding prior on the trajectory parameter $\bm{\theta}$ is then
given by

$p_{\mathrm{c}}\left(\bm{\theta}{=}\begin{pmatrix}\bm{q}^{1}_{\mathrm{via}}\\\
\vdots\\\
\bm{q}_{\mathrm{via}}^{N}\end{pmatrix}\right)=\mathcal{N}\left(\begin{pmatrix}\bm{0}\\\
\vdots\\\
\bar{\bm{q}}_{\mathrm{c}}\end{pmatrix},\begin{pmatrix}\bm{0}&\\!\cdots&\\!\\!\bm{0}\\\
\vdots&\\!\ddots&\\!\\!\vdots\\\
\bm{0}&\\!\cdots&\\!\\!\bm{Q}_{\bm{q}}\end{pmatrix}^{-1}\right),$ (39)

where
$\bm{Q}_{\bm{q}}=\left(\bm{\Sigma}^{r|o}+\bm{A}\bm{\Sigma}^{o}\bm{A}^{\scriptscriptstyle\top}\right)^{-1}$
describes the precision matrix of the contact prior with respect to the mean
contact configuration
$\bar{\bm{q}}_{\mathrm{c}}=\bm{f}_{\mathrm{c}}(\bm{\mu}^{o})$. We denote the
contact prior with

$p_{\mathrm{c}}(\bm{\theta})=\mathcal{N}(\bar{\bm{\theta}}_{\mathrm{c}},\bm{Q}_{\bm{\theta}}^{-1}).$
(40)

Note that the resulting covariance matrix $\bm{Q}_{\bm{\theta}}^{-1}$ is
degenerated due to zero-precision values for the via-points except for
$\bm{q}_{\mathrm{via}}^{N}$. For resolving the degeneration, we regularize the
covariance matrix by combining the contact prior with a smoothness prior as
described in the following.

####  5.2.3 Gaussian Contact Prior in Joint Space.

Optimizing in the joint space of an articulated robot such as robot arms may
be beneficial when kinematic and dynamic limitations are to be considered
during planning. Sampling in joint space requires to represent the contact
prior in joint space as well. Suppose that the robot’s end-effector, that is
supposed to manipulate the object, has a configuration given by $\bm{x}^{r}$
which is computed from the robot’s joint positions $\bm{q}^{r}$ via forward
kinematics $\bm{x}^{r}=\bm{f}_{\mathrm{fk}}(\bm{q}^{r})$. We may adopt the
contact prior in (38) to formulate a prior distribution over configurations of
the end-effector, i.e.

$p_{\mathrm{c}}(\bm{x}^{r})=\mathcal{N}(\bm{f}_{\mathrm{c}}(\bm{\mu}^{o}),\bm{\Sigma}^{r|o}+\bm{A}\bm{\Sigma}^{o}\bm{A}^{\scriptscriptstyle\top}).$
(41)

For computing a corresponding Gaussian distribution in joint space, the
forward kinematics are linearized around a mean joint position
$\bar{\bm{q}}_{\mathrm{c}}^{r}$ with

$\bm{x}^{r}\approx\bm{f}_{\mathrm{fk}}(\bar{\bm{q}}_{\mathrm{c}}^{r})+\bm{J}(\bar{\bm{q}}_{\mathrm{c}}^{r})\left(\bm{q}^{r}-\bar{\bm{q}}_{\mathrm{c}}^{r}\right),$
(42)

where
$\bm{J}(\bm{q})=\nicefrac{{\partial\bm{x}^{r}}}{{\partial\bm{q}^{r}}}|_{\bar{\bm{q}}_{\mathrm{c}}^{r}}$
denotes the Jacobian with respect to the end-effector configuration. The mean
joint position $\bar{\bm{q}}_{\mathrm{c}}^{r}$ can be computed via inverse
kinematics with respect to the mean end-effector configuration
$\bm{f}_{\mathrm{c}}(\bm{\mu}^{o})$. Consequently, the Gaussian contact prior
for the end-effector can be locally transformed into the joint space,
resulting in a Gaussian distribution
$p_{\mathrm{c}}(\bm{q}^{r})=\mathcal{N}(\bar{\bm{q}}_{\mathrm{c}}^{r},\bm{Q}_{\bm{q}}^{-1})$.
The joint space contact precision matrix is computed with

$\bm{Q}_{\bm{q}}=\bm{J}(\bar{\bm{q}}_{\mathrm{c}}^{r})^{\scriptscriptstyle\top}\left(\bm{\Sigma}^{r|o}+\bm{A}\bm{\Sigma}^{o}\bm{A}^{\scriptscriptstyle\top}\right)^{-1}\bm{J}(\bar{\bm{q}}_{\mathrm{c}}^{r}).$
(43)

####  5.2.4 Gaussian Smoothness Prior.

The smoothness prior, introduced in our previous work (Jankowski et al.
(2023)), incorporates temporal correlations between via-points by computing a
Gaussian distribution that expresses a high likelihood for low-acceleration
profiles. A typical objective in trajectory optimization is to minimize the
integral over squared accelerations of the candidate trajectory, i.e.

$J_{\mathrm{s}}=\frac{1}{2}\int_{0}^{T}\ddot{\bm{q}}^{r{\scriptscriptstyle\top}}(t)\bm{R}_{\bm{q}}\ddot{\bm{q}}^{r}(t)dt.$
(44)

The positive definite matrix $\bm{R}_{\bm{q}}$ encodes the desired smoothing
for the individual degrees of freedom. Using the parameterization in (35),
this objective can be expressed using the via-point parameter $\bm{\theta}$
and the initial conditions for the trajectory
$\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r}$, i.e.
$J_{\mathrm{s}}(\bm{\theta},\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})$. As a next
step, we express the smoothness prior as a probability distribution
parameterized with the negative objective in (44) with

$p_{\mathrm{s}}(\bm{\theta},\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})\propto
e^{-J_{\mathrm{s}}(\bm{\theta},\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})}.$ (45)

Interestingly, this results in a joint Gaussian distribution over the
trajectory parameter and the initial conditions. We then compute a Gaussian
smoothness prior on the trajectory parameter by conditioning on the initial
conditions, i.e.

$p_{\mathrm{s}}(\bm{\theta}|\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})=\mathcal{N}(\bar{\bm{\theta}}_{\mathrm{s}},\bm{R}_{\bm{\theta}}^{-1}),$
(46)

with the precision matrix being computed with

$\bm{R}_{\bm{\theta}}=\int_{0}^{T}\ddot{\bm{\Phi}}^{\scriptscriptstyle\top}_{\mathrm{via}}(t)\bm{R}_{\bm{q}}\ddot{\bm{\Phi}}_{\mathrm{via}}(t)dt.$
(47)

Please refer to the appendix for the derivation of (47) and for details on how
to compute the smoothness prior mean $\bar{\bm{\theta}}_{\mathrm{s}}$. Note
that the smoothness precision matrix $\bm{R}_{\bm{\theta}}$ can be computed
offline as it does not depend on the trajectory parameter $\bm{\theta}$.

####  5.2.5 Product of Gaussian Priors.

Figure 7: Smooth trajectories sampled from the product of the smoothness prior
and the contact prior. The contact prior is indicated by the orange ellipses
and circles with the mean of the contact prior in the center. The velocity
profile of the trajectories is encoded through color with low velocities in
blue and high velocities in yellow. Note that all trajectories start and end
with exactly zero velocity.

Given the two priors for making contact and smooth trajectories respectively,
we find the informed via-point distribution by fusing the two probabilistic
priors via computing the normalized product of the two priors, such that

$p(\bm{\theta})=\mathcal{N}(\bar{\bm{\theta}},\bm{\Sigma}_{\bm{\theta}})\propto
p_{\mathrm{c}}(\bm{\theta})p_{\mathrm{s}}(\bm{\theta}|\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r}).$
(48)

The product of two multivariate Gaussians is again a multivariate Gaussian,
with the resulting parameters given by

$\bm{\Sigma}_{\bm{\theta}}=\left(\bm{Q}_{\bm{\theta}}+\bm{R}_{\bm{\theta}}\right)^{-1},$
(49a)
$\bar{\bm{\theta}}=\bm{\Sigma}_{\bm{\theta}}(\bm{Q}_{\bm{\theta}}\bar{\bm{\theta}}_{\mathrm{c}}+\bm{R}_{\bm{\theta}}\bar{\bm{\theta}}_{\mathrm{s}}).$
(49b)

Fig. 7 illustrates trajectories sampled from the product of the contact prior,
parameterized by $\bm{Q}_{\bm{q}}$, and the smoothness prior, parameterized by
$\bm{R}_{\bm{q}}$. Within each sub-figure, all trajectories are drawn from a
single Gaussian distribution. Note that all trajectories start and end with
zero velocity.

####  5.2.6 Optimizing and Sampling in Latent Space.

Figure 8: BS-VP-STO optimizing the trajectory of a rectangular robot to push a
circular object into a goal region subject to uncertain initial object
location (represented by a particle-based belief representation) and uncertain
contact dynamics. The sub-figures show the best performing candidate solution
with the corresponding velocity profile, as well as the other trajectory
samples in light gray, after 1, 10, 40 and 120 iterations from left to right.

Given the product of priors, we use the informed generative via-point
distribution $p(\bm{\theta})$ as a probabilistic initial guess for optimizing
robot trajectories with CMA-ES. Instead of directly sampling trajectory
candidates $\bm{\theta}$ from an uninformed distribution, e.g. white noise, we
sample and optimize for
$\bm{\varepsilon}\in\mathbb{R}^{Nn_{\mathrm{dof}}^{r}}$. For a given
$\bm{\varepsilon}$, we compute $\bm{\theta}$ through an affine transformation
as follows:

$\bm{\theta}=\bar{\bm{\theta}}+\bm{L}_{\bm{\theta}}\bm{\varepsilon}.$ (50)

Here, $\bm{L}_{\bm{\theta}}$ is the Cholesky decomposition of the covariance
matrix $\bm{\Sigma}_{\bm{\theta}}$. The parameters $\bar{\bm{\theta}}$ and
$\bm{\Sigma}_{\bm{\theta}}$ incorporate the prior as defined in (49). The idea
of this additional transformation is to decouple the optimization variable
$\bm{\varepsilon}$ from the particular prior. In each iteration $m$ of BS-VP-
STO, we obtain the new population of candidate solutions by sampling
$N_{\mathrm{cand}}$ robot trajectories via

$\bm{\theta}\sim\mathcal{N}\left(\bar{\bm{\theta}}+\bm{L}_{\bm{\theta}}\bar{\bm{\varepsilon}}_{m},\bm{L}_{\bm{\theta}}\bm{\Sigma}_{m}\bm{L}_{\bm{\theta}}^{\scriptscriptstyle\top}\right).$
(51)

When initializing the CMA-ES distribution as white noise, i.e.
$\bar{\bm{\varepsilon}}_{0}=\bm{0}$ and $\bm{\Sigma}_{0}=\bm{I}$, we
effectively sample the first population from the informed distribution in
(49), as inserting the initial parameters into (51) yields

$\bm{\theta}\sim\mathcal{N}\left(\bar{\bm{\theta}},\bm{L}_{\bm{\theta}}\bm{L}_{\bm{\theta}}^{\scriptscriptstyle\top}\right)=\mathcal{N}\left(\bar{\bm{\theta}},\bm{\Sigma}_{\bm{\theta}}\right).$
(52)

Eventually, given a sampled trajectory parameter $\bm{\theta}$, we find the
control trajectory with respect to the system in (6) by discretizing the robot
trajectory in (35), i.e.

$\bm{u}_{k}=\bm{q}^{r}\left(t=T\cdot\frac{k+1}{K}\right).$ (53)

Note that due to the quasi-static model in (6), the dynamics can be rolled out
with an arbitrary temporal resolution.

Example 2. (Single-horizon Robust 2D Object-Pushing). We showcase the BS-VP-
STO pipeline over multiple iterations for a 2D object pushing example,
illustrated in Fig. 8. The task for the robot, a rectangular geometry, is to
push the object, a circular geometry, into a target region. We consider two
sources of uncertainty in this example: a) The initial position of the object
is uncertain, which is reflected by an initial belief; and b) the contact
dynamics are uncertain. This task requires exploring contact modes that are
robust to these uncertainties, as implicitly done by the presented planning
algorithm.

After the first iteration, the best solution corresponds to the robot making
contact with its long side. Note that this solution corresponds to the best
candidate of the initial population sampled from the product of Gaussian
priors, without any CMA-ES updates. Due to the probabilistic contact prior,
almost all of the 30 initial candidates bring the robot into contact with the
object, enabling an informative sampling of the cost landscape. After 10
iterations of BS-VP-STO, the algorithm found a solution making robust contact
with the object while moving it slightly towards the target area. The solution
after 40 iterations enables the robot to almost push the object into the
target area. After 120 iterations, the algorithm found a solution for pushing
the object robustly into the target area, while also optimizing the overall
motion duration which is incorporated into $c_{\mathrm{task}}$. The tight
distribution of candidate solutions after 120 iterations indicates that CMA-ES
has converged.

##  6 Receding-horizon BS-VP-STO

Planning a pushing maneuver over a single long horizon is challenging for two
reasons: i) The dimensionality of the solution space of the optimization
problem grows as the solution requires higher expressiveness. In the presented
algorithm, this is reflected by an increasing number of via-points $N$ that
parameterize the robot trajectory. ii) In BS-VP-STO, the belief is rolled out
using the nominal object dynamics, i.e. without inducing noise. For robust
candidate trajectories, the belief may collapse to a Dirac-delta distribution
after $k_{\mathrm{collapse}}$ steps, i.e.
$\tilde{b}_{k}=\delta(\bm{q}^{o}_{k}),\forall k\geq k_{\mathrm{collapse}}$,
resulting in zero variance. In this case, the variance gain at those time
steps is either $\gamma_{k}=0$ if the robot does not touch the object, or
$\gamma_{k}=1$ if the robot touches the object. Thus, the optimization is
strongly biased towards not touching the object after $k_{\mathrm{collapse}}$
steps.

Input: Robot configuration $\bm{q}^{r}_{0}$ and velocity
$\dot{\bm{q}}^{r}_{0}$, object belief $b_{0}$, receding horizon length $H$.

Output: Robot trajectory $\bm{u}$.

1 $\bm{u}\leftarrow\emptyset$

2 while  _task not solved_ do

3 $\bm{q}^{r*}_{0:K},\dot{\bm{q}}^{r*}_{0:K}\leftarrow$ BS-VP-
STO$(\bm{q}^{r}_{0},\dot{\bm{q}}^{r}_{0},b_{0})$

4 $\bm{u}^{*}_{0:H-1}\leftarrow\bm{q}^{r*}_{1:H}$

5 for  _$k\leftarrow 0$ to $H-1$_ do

// Stochastic rollout

6 ${}^{i}\bm{q}^{o}_{k+1}\sim
p(\cdot|{}^{i}\bm{q}^{o}_{k},\bm{u}^{*}_{k}),\,\forall i\in\\{1,2,..,N_{p}\\}$

7

8 end for

9
$\bm{q}^{r}_{0},\dot{\bm{q}}^{r}_{0}\leftarrow\bm{q}^{r*}_{H},\dot{\bm{q}}^{r*}_{H}$

10 $b_{0}\leftarrow\\{{}^{i}\bm{q}^{o}_{H}\\}_{i=1}^{N_{p}}$

11 $\bm{u}\leftarrow$ concatenate$\left(\bm{u},\bm{u}^{*}_{0:H-1}\right)$

12

13 end while

Algorithm 1 Receding-horizon BS-VP-STO

For these reasons, we propose receding-horizon BS-VP-STO, a planning scheme
for pushing maneuvers over longer horizons. The scheme alternates between
computing a robust push via BS-VP-STO and performing a stochastic rollout of
the solution. This allows to plan pushing maneuvers over multiple shorter
horizons, while optimizing for task progress, i.e. pushing the object towards
the goal, and robustness over a single horizon.

Alg. 1 sketches the receding-horizon procedure for planning pushing maneuvers.
Starting from the initial robot configuration $\bm{q}^{r}_{0}$ and velocity
$\dot{\bm{q}}^{r}_{0}$, a given initial belief $b_{0}$ and the number of time
steps for a receding horizon $H$, BS-VP-STO is used to generate a robust
pushing trajectory $\bm{q}^{r*}_{0:K}$. In order to update the belief for the
subsequent receding-horizon, we perform a stochastic rollout of the robust
pushing trajectory, i.e. sampling from the transition probability in (10).
Note that the update of the belief can be extended by taking observations into
account. For this, line 9 in Alg. 1 may be replaced by a Bayesian state
estimation update, e.g. a particle filter (Arulampalam et al. (2002)), thus
turning the offline planning algorithm into an online re-planning approach.
When planning offline, the optimized pushing trajectories of the receding-
horizons are sequenced to form a single continuous trajectory over a long
horizon. This process may be repeated until a task-specific termination
criterion is satisfied, e.g. the mean object configuration is within bounds of
the target.

Figure 9: Receding-horizon BS-VP-STO optimizing the trajectory of a
rectangular robot to push a circular object into a goal region subject to an
uncertain initial object location and uncertain contact dynamics. The sub-
figures show the robot trajectory optimized over the corresponding receding-
horizon together with a stochastic rollout of the object dynamics. In each
receding-horizon, the algorithm optimizes for a tradeoff between pushing
progress and robustness. The resulting pushing maneuver is more robust than
the solution found when optimizing over a single horizon as in Fig. 8. Figure
10: Setup for the real-world experiment. The two manipulators are considered
as one bi-manual robot with two end-effectors, each equipped with a ball-
shaped end-effector. The object (bottle, glass or can) is placed in front of
the robot and the goal is to push the object along the target path (red
circle). All objects were chosen due to their circular shape for the sake of a
simple implementation of the quasi-static contact dynamics. The initial belief
over the object position is Gaussian distributed with a mean equal to the
initial object position and a covariance that reflects the uncertainty in the
object detection. The contact dynamics are modeled probabilistically to
reflect uncertainty. For the experiments with the can, we add additional
perturbations by placing wooden cubes (red cubes) on the target path and by
changing the center of mass to be off-center by placing a heavy tool in the
can (yellow content of the can).

Example 3. (Receding-horizon Robust 2D Object-Pushing). In this example, we
solve the same problem as in Example 2, while iteratively optimizing over
multiple receding horizons instead of running BS-VP-STO only once over the
full horizon. For this, we only adapt the task-specific cost to reflect the
pushing progress towards the goal. We measure this progress by computing the
distance $d_{k}=||\mathrm{E}[\bm{q}^{o}_{k}]-\bm{q}^{o}_{\mathrm{des}}||_{2}$
between the target area center $\bm{q}^{o}_{\mathrm{des}}$ and the mean object
position at that time step. We compare the distance at the end of the receding
horizon with the distance at the beginning of it in order to make the cost
invariant to absolute distance to the target. Progress is thus defined as
$d_{0}-d_{K}$. Consequently, the task-cost $c_{\mathrm{task}}$ is defined as
follows

$c_{\mathrm{task}}(\bm{u}_{0:K-1})=e^{-\lambda\left(d_{0}-d_{K}\right)}.$ (54)

Fig. 9 illustrates each solution of the optimization over multiple shorter
horizons. It can be seen that a single-horizon push moves the object belief
towards the target area with high probability. The overall pushing maneuver,
obtained by sequencing the robot trajectories of the individual horizons, has
no constraints on the number of parameters, i.e. the number of via-points, as
the number of receding horizon operations is not fixed but rather tied to a
goal check. It is therefore expected to be more expressive than a single-
horizon solution and thus more robust.

##  7 Experiment: Bi-manual Pushing

###  7.1 Experimental Setup

We chose to validate the proposed algorithm with a bi-manual pushing task,
consisting of two Franka robot arms that are equipped with ball-shaped end-
effectors. Note that we treat the two robot arms as one bi-manual robot. We
plan 2D trajectories for the ball-shaped end-effectors in a plane parallel to
the table. The dynamics are modeled in this 2D plane, where the two robots are
abstracted as two independent circles. The objects are abstracted as circles
as well. Fig. 10 shows the experimental setup. Initially, the object (bottle,
glass or can) is placed in front the bi-manual robot and the goal for the
robot is to push the object along a circular target path. The belief over
object positions is initialized with a Gaussian distribution. We furthermore
modeled the noise in the contact dynamics with a Gaussian distribution and we
tuned the covariance to capture the stochasticity in the contact dynamics. To
prevent collisions between the two end-effectors, we include a collision
constraint enforcing that the two end-effectors do not touch. We also add a
constraint that prevents the robot from crossing its arms.

In order to evaluate qualitatively the effect of modeling the uncertainty in
the contact dynamics, we compare the performance of the proposed algorithm
with a deterministic baseline algorithm that is based on the proposed
receding-horizon algorithm. For this, the planner assumes that the initial
object position is known and it uses the nominal object dynamics from Sec. 4
as deterministic dynamics model. Consequently, we run the baseline without the
cost and constraints on the variance gain.

###  7.2 Implementation

Figure 11: Qualitative comparison between the proposed receding-horizon BS-VP-
STO algorithm and the deterministic baseline. We perform 1000 stochastic
rollouts of each generated plan using the proposed stochastic object dynamics.
The left and right end-effectors are visualized with black and blue circles,
respectively. The target path is depicted in light-green. a) For testing the
baseline plan, the object position is initialized with the expected position
(orange circle). For the proposed approach, we initialize the object position
according to the modeled uncertainty. b) Plans generated for an object with 5
cm radius, i.e. the bottle and the can. c) Plans generated for an object with
3 cm radius, i.e. the glass.

We implement the contact dynamics as in (6) for the special case of circular
shapes in two dimensions. In order to approximate the initial belief via
particles, we found that $N_{p}=20$ particles are sufficient to generate
robust plans. For computing the CMA-ES updates to the Gaussian distribution
over candidate trajectories, we use the Python package provided by the authors
of Hansen (2016). We provide additional details on the cost design in the
appendix. The planning algorithm was executed on a laptop with an Intel Core
i9-14900HX CPU and 32GB of RAM. On average, generating a robust pushing plan
for the full target path takes around seven seconds wall-clock time.

###  7.3 Qualitative Planning Results

We illustrate plans generated with the deterministic baseline and with the
proposed receding-horizon BS-VP-STO algorithm in Fig. 11. Each plan is
presented with 1000 stochastic rollouts of the object dynamics used for
optimization. For an evaluation of the real-world performance of the plans,
please refer to Sec. 7.5.

####  7.3.1 Deterministic Baseline.

In all plans generated with the deterministic baseline, the robot uses only
one end-effector at a time for pushing the object. This is not surprising as
the pushing progress, i.e. the mean control problem, is equally optimized when
using one or two end-effectors. Thus, the baseline is not forced to discover
the coordination between the two end-effectors and converges to the simpler
solution, i.e. using one end-effector. When performing open-loop stochastic
rollouts of the baseline plans, the object deviates from the planned
trajectory after some time and thus the robot is not able to successfully push
the object along the whole target path.

####  7.3.2 Receding-horizon BS-VP-STO.

In contrast, we observe that the robot uses both end-effectors to push the
object along the target path when planning with the receding-horizon BS-VP-STO
algorithm. Note that the strategy of using two end-effectors for pushing
deliberately emerges from planning in belief space. The proposed algorithm
discovers the use of two end-effectors by penalizing sampled robot
trajectories that result in an increasing uncertainty about the object
position, i.e. using one end-effector. At the beginning of the pushing
maneuver, the robot performs an action that reduces uncertainty by placing its
end-effectors such that they enclose the initial belief. This effectively
brings the particles closer together, resulting in a decreasing variance. The
robot then starts to make contact with its two end-effectors side-by-side to
push the object along the target path with high probability. After pushing the
object along the first half of the circular target path, the no-collision
constraint between the two end-effectors forces the robot to break the contact
and to find a new contact configuration with which it can continue pushing.
Consequently, the robot has to move its left end-effector around the object
without touching it. Afterwards, the robot makes contact again with the object
and continues pushing until it reaches the initial position again. When
performing open-loop stochastic rollouts of the robust plans, the robot has a
high probability of being successful at pushing the object along the target
path despite the perturbations and the lack of feedback.

###  7.4 Quantitative Planning Results & Ablation Studies

In the following, we present ablation studies of the different components of
receding-horizon BS-VP-STO. We evaluate i) the dependence of the overall
algorithmic performance on the number of iterations taken in each BS-VP-STO
instance within the receding-horizon setting, ii) the relevance of a contact-
prior compared to an uninformed proposal distribution, and iii) how the
receding-horizon BS-VP-STO algorithm scales with the number of degrees of
freedom of the robot. We summarize all findings in Fig. 12 which evaluates the
success rate of the receding-horizon BS-VP-STO algorithm, i.e. if the planning
algorithm finds a valid solution for the robot pushing an object with 5 cm
radius. A plan is considered successful if the mean of the object belief is
within 1 cm tolerance to the target location and if the plan is valid with
respect to the robustness constraints on the variance gain. The planning
algorithm is aborted after 500 iterations of receding-horizon BS-VP-STO and
the plan is considered a failure. Sub-figure b) on the right-hand side in Fig.
11 illustrates a successful plan for the problem considered for the ablations.
We instantiate the planning problem with a varying number $M$ of iterations
for one BS-VP-STO instance within the receding-horizon scheme. For each $M$,
we run receding-horizon BS-VP-STO 50 times and measure the success rate of the
planning algorithm.

####  7.4.1 Impact of the Number of BS-VP-STO Iterations.

Fig. 12 illustrates the statistics of success over the number of BS-VP-STO
iterations. We observe that the success rate increases with the number of
iterations $M$ and reaches around $100\%$ success rate with $M=4$ BS-VP-STO
iterations. Note that running BS-VP-STO for one iteration poses a special case
of the algorithm since no CMA-ES update is performed, resulting in only
sampling an initial candidate population and picking the best performing
candidate. This procedure in fact corresponds to the predictive sampling
algorithm introduced in Howell et al. (2022).

The planning time increases linearly with the number of iterations. For
reference, performing one BS-VP-STO iteration for four degrees of freedom
takes approximately 0.01 seconds wall-clock time. In this setup, we execute
the pushing plan for an execution horizon $H$ that corresponds to 0.2 seconds.
Thus, it would be possible to run the planning algorithm in an online receding
horizon fashion with up to $M=20$ BS-VP-STO iterations per receding-horizon.

####  7.4.2 Impact of the Contact Prior.

Figure 12: Success rates of receding-horizon BS-VP-STO over the number of BS-
VP-STO iterations $M$. We compare the performance of the planning algorithm
with the contact prior $\bm{Q}_{\bm{q}}>\bm{0}$ and without
$\bm{Q}_{\bm{q}}=\bm{0}$; and we evaluate the scalability of the planning
algorithm to many degrees of freedom (14 DoF). A planning run is considered
successful if a valid solution is found within 500 single-horizon
optimizations.

To evaluate the impact of the contact prior, we set the contact precision
matrix in BS-VP-STO to $\bm{Q}_{\bm{q}}=\bm{0}$ (cf. (39)). This corresponds
to uninformed trajectory samples as depicted in the left sub-figure of Fig. 6.
In Fig. 12 we observe that the success rate of the algorithm without the
contact prior is significantly lower than the success rate of the algorithm
with the contact prior. This indicates that the contact prior is improving the
efficiency of the planning algorithm to find robust manipulation actions in
few iterations. Computing the contact prior requires a matrix inversion to
compute $\bm{Q}_{\bm{q}}$ with dimensionality equivalent to the number of
degrees of freedom. Since this operation has to be performed only once for a
single-horizon, the computational overhead of the contact prior is negligible.

####  7.4.3 Scalability to Many Degrees of Freedom.

Last, we evaluate the scalability of the proposed planning algorithm to many
degrees of freedom by moving from planning in the 2D plane to planning in the
joint space of the bi-manual robot, which has 14 degrees of freedom (seven
degrees of freedom per robot arm). Coordinating all degrees of freedom adds
complexity to the planning problem, while at the same time increasing the
dimensionality of the search space. In Fig. 12 we observe that the success
rate for a given number of iterations $M$ drops when increasing the complexity
of the problem, requiring more iterations to discover the required
coordination between the 14 joints. While the contact prior in joint space
(cf. Sec. 5.2.3) imposes coordination between the seven joints of the
individual arms, BS-VP-STO is still required to find joint trajectories such
that the two end-effectors touch the object at the right place at the right
time. The planning time per iteration is not significantly increased compared
to planning for four degrees of freedom, since only an additional forward
kinematics computation for the belief rollout is required. However, note that
the contact dynamics are still modeled using the ball-shaped abstraction of
the end-effector. Modeling the whole kinematic chain of the robot arms as
contact geometries adds additional computational complexity to the planner.

###  7.5 Real-world Pushing Results

Figure 13: Snapshots of the robot behavior synthesized with the receding-
horizon BS-VP-STO algorithm and executed on the real bi-manual system. Each
snapshot shows an overlay of five experiments that were conducted with
different initial positions of the bottle. The robot successfully pushed the
bottle along the target path in all five experiments. At the beginning, i.e.
in the first image, the robot encloses the initial belief with its two end-
effectors such that robust pushing is possible. After moving along the first
half of the circular target path, i.e. in the third image, the robot re-
positions its two end-effectors to continue pushing the bottle along the
target path while avoiding collisions between the two arms.

Fig. 13 shows snapshots of the robot behavior planned with the receding-
horizon BS-VP-STO algorithm. In addition, a full video of the experiment can
be found in the supplementary material. We executed the planned robot
trajectories for each of the three objects (bottle, glass, can), where we
conducted five experiments for each object by placing the object at different
initial positions. Out of 15 executed plans, the robot successfully pushed the
object along the target path in 14 experiments. The robot failed to push the
glass in one experiment, where the object was too far away from the mean
initial position such the robot did not enclose the object during the initial
uncertainty-reducing action.

We furthermore evaluated the deterministic baseline planner with the same
three objects, while placing the objects only at the expected location. In all
three experiments, the actual object position deviated from the planned object
position after a few pushes, resulting in the robot loosing contact with the
object and thus failing to push the object along the target path. We show an
example of the motion generated by the baseline in the supplementary video.

##  8 Discussion

In this article, we investigated the problem of planning robust manipulation
actions subject to stochastic contact dynamics. The quasi-static model used to
predict the contact dynamics is a simplification of the real-world contact
dynamics tailored to the particular problem of slow pushing. In particular,
reducing the dynamics from joint robot-object dynamics to solely object
dynamics enables efficient reasoning over belief dynamics. However, the
provided model excludes other categories of manipulation tasks involving
effects such as grasping objects. We leave it to future work to investigate
how other manipulation dynamics can be reduced to object-only dynamics.

Furthermore, we have shown that informed prior distributions for sampling
candidate actions is beneficial, if not necessary, for sampling-based
optimization for contact-rich manipulation. We used the product of Gaussian
priors to bias the sampling towards smooth and contact-making trajectories. As
the evolutionary optimization algorithm CMA-ES is based on sampling from and
iterating on Gaussian distributions, we incorporated our Gaussian-distributed
prior to initialize CMA-ES. Yet, we see great potential in the use of non-
Gaussian priors that are further optimized with a stochastic optimization
algorithm such as BS-VP-STO. Especially when the system has many degrees of
freedom such as for two robot arms or articulated robotic hands, sampling from
a proposal distribution that captures possibly non-Gaussian correlations
between the degrees of freedom, e.g. correlations between fingers, is expected
to be a key to scalable, realtime control through contacts.

Last, we show that a manipulation task such as pushing, which is typically
approached with high-bandwidth closed-loop control, can also be stabilized by
planning appropriate open-loop actions that deliberately optimize for
robustness. However, if the robot is not able to anticipate perturbations or
the statistical properties of the perturbations, feedback is required to be
able to stabilize the manipulation. This opens the question of the optimal
combination of open-loop robust planning and continuous feedback. Closing the
loop may be done by re-planning robust actions if the observations deviate
from the planned belief, e.g. in a low-bandwidth feedback loop.

## Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the
research, authorship, and/or publication of this article.

## Funding

The author(s) disclosed receipt of the following financial support for the
research, authorship, and/or publication of this article: JJ was supported by
the Swiss National Science Foundation (SNSF) through the CODIMAN project. LB
was supported by an Amazon Web Services Lighthouse scholarship. NH received
EPSRC funding via the “From Sensing to Collaboration” programme grant
[EP/V000748/1].

## References

  * Andrychowicz et al. (2020) Andrychowicz OM, Baker B, Chociej M, Józefowicz R, McGrew B, Pachocki J, Petron A, Plappert M, Powell G, Ray A, Schneider J, Sidor S, Tobin J, Welinder P, Weng L and Zaremba W (2020) Learning dexterous in-hand manipulation.  _The International Journal of Robotics Research_ 39(1): 3–20.  10.1177/0278364919887447.  URL https://doi.org/10.1177/0278364919887447.
  * Arulampalam et al. (2002) Arulampalam MS, Maskell S, Gordon N and Clapp T (2002) A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking.  _IEEE Transactions on signal processing_ 50(2): 174–188.
  * Aydinoglu and Posa (2022) Aydinoglu A and Posa M (2022) Real-time multi-contact model predictive control via admm.  In: _International Conference on Robotics and Automation (ICRA)_. p. 3414–3421.
  * Aydinoglu et al. (2022) Aydinoglu A, Sieg P, Preciado VM and Posa M (2022) Stabilization of complementarity systems via contact-aware controllers.  _IEEE Transactions on Robotics_ 38(3): 1735–1754.  10.1109/TRO.2021.3120931.
  * Bhardwaj et al. (2022) Bhardwaj M, Sundaralingam B, Mousavian A, Ratliff N, Fox D, Ramos F and Boots B (2022) STORM: An Integrated Framework for Fast Joint-Space Model-Predictive Control for Reactive Manipulation.  In: _Conference on Robot Learning (CoRL)_. pp. 750–759.
  * Chen et al. (2021) Chen C, Culbertson P, Lepert M, Schwager M and Bohg J (2021) Trajectotree: Trajectory optimization meets tree search for planning multi-contact dexterous manipulation.  In: _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. pp. 8262–8268.  10.1109/IROS51168.2021.9636346.
  * Cheng et al. (2021) Cheng X, Huang E, Hou Y and Mason MT (2021) Contact mode guided sampling-based planning for quasistatic dexterous manipulation in 2d.  In: _2021 IEEE International Conference on Robotics and Automation (ICRA)_. pp. 6520–6526.  10.1109/ICRA48506.2021.9560766.
  * Cleac’h et al. (2021) Cleac’h SL, Howell T, Yang S, Lee CY, Zhang J, Bishop A, Schwager M and Manchester Z (2021) Fast contact-implicit model-predictive control.  URL https://arxiv.org/abs/2107.05616.
  * Cover and Thomas (2005) Cover TM and Thomas JA (2005) _Differential Entropy_ , chapter 8.  John Wiley & Sons, Ltd.  ISBN 9780471748823, pp. 243–259.  https://doi.org/10.1002/047174882X.ch8.  URL https://onlinelibrary.wiley.com/doi/abs/10.1002/047174882X.ch8.
  * Dogar and Srinivasa (2011) Dogar M and Srinivasa S (2011) A framework for push-grasping in clutter.  In: Hugh Durrant-Whyte NR and Abbeel P (eds.) _Proceedings of Robotics: Science and Systems (RSS ’11)_. MIT Press, pp. 65–73.
  * Erdmann and Mason (1988) Erdmann M and Mason M (1988) An exploration of sensorless manipulation.  _IEEE Journal on Robotics and Automation_ 4(4): 369–379.  10.1109/56.800.
  * Ha et al. (2020) Ha JS, Driess D and Toussaint M (2020) A probabilistic framework for constrained manipulations and task and motion planning under uncertainty.  In: _2020 IEEE International Conference on Robotics and Automation (ICRA)_. pp. 6745–6751.  10.1109/ICRA40945.2020.9196840.
  * Haarnoja et al. (2019) Haarnoja T, Zhou A, Hartikainen K, Tucker G, Ha S, Tan J, Kumar V, Zhu H, Gupta A, Abbeel P and Levine S (2019) Soft actor-critic algorithms and applications.
  * Handa et al. (2023) Handa A, Allshire A, Makoviychuk V, Petrenko A, Singh R, Liu J, Makoviichuk D, Van Wyk K, Zhurkevich A, Sundaralingam B and Narang Y (2023) Dextreme: Transfer of agile in-hand manipulation from simulation to reality.  In: _2023 IEEE International Conference on Robotics and Automation (ICRA)_. pp. 5977–5984.  10.1109/ICRA48891.2023.10160216.
  * Hansen (2016) Hansen N (2016) The CMA evolution strategy: A tutorial.  _arXiv preprint arXiv:1604.00772_ .
  * Hogan and Rodriguez (2020) Hogan FR and Rodriguez A (2020) Reactive planar non-prehensile manipulation with hybrid model predictive control.  _The International Journal of Robotics Research_ 39(7): 755–773.
  * Hogan (1984) Hogan N (1984) Impedance control: An approach to manipulation.  In: _1984 American Control Conference_. pp. 304–313.  10.23919/ACC.1984.4788393.
  * Howell et al. (2022) Howell T, Gileadi N, Tunyasuvunakool S, Zakka K, Erez T and Tassa Y (2022) Predictive sampling: Real-time behaviour synthesis with mujoco.  _arXiv preprint arXiv:2212.00541_ .
  * Jankowski et al. (2023) Jankowski J, Brudermüller L, Hawes N and Calinon S (2023) VP-STO: Via-point-based stochastic trajectory optimization for reactive robot behavior.  _International Conference on Robotics and Automation (ICRA)_ .
  * Jankowski et al. (2022) Jankowski J, Racca M and Calinon S (2022) From Key Positions to Optimal Basis Functions for Probabilistic Adaptive Control.  _IEEE Robotics and Automation Letters_ 7(2): 3242–3249.
  * Kappen (2015) Kappen HJ (2015) Adaptive importance sampling for control and inference.  _Journal of Statistical Physics_ 162: 1244–1266.  URL https://api.semanticscholar.org/CorpusID:6382641.
  * Koval et al. (2016) Koval MC, Pollard NS and Srinivasa SS (2016) Pre- and post-contact policy decomposition for planar contact manipulation under uncertainty.  _The International Journal of Robotics Research_ 35(1-3): 244–264.  10.1177/0278364915594474.  URL https://doi.org/10.1177/0278364915594474.
  * Lynch and Mason (1995) Lynch KM and Mason MT (1995) Stable pushing: Mechanics, controllability, and planning.  _The International Journal of Robotics Research_ 15: 533 – 556.  URL https://api.semanticscholar.org/CorpusID:13041894.
  * Marcucci et al. (2017) Marcucci T, Deits R, Gabiccini M, Bicchi A and Tedrake R (2017) Approximate hybrid model predictive control for multi-contact push recovery in complex environments.  In: _2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids)_. pp. 31–38.  10.1109/HUMANOIDS.2017.8239534.
  * Mason (2001) Mason MT (2001) _Mechanics of Robotic Manipulation_.  MIT Press.
  * Migimatsu and Bohg (2020) Migimatsu T and Bohg J (2020) Object-centric task and motion planning in dynamic environments.  _IEEE Robotics and Automation Letters_ 5(2): 844–851.  10.1109/LRA.2020.2965875.
  * Muratore et al. (2022) Muratore F, Ramos F, Turk G, Yu W, Gienger M and Peters J (2022) Robot learning from randomized simulations: A review.  _Frontiers in Robotics and AI_ 9\.  10.3389/frobt.2022.799893.  URL https://www.frontiersin.org/articles/10.3389/frobt.2022.799893.
  * Okamoto et al. (2018) Okamoto K, Goldshtein M and Tsiotras P (2018) Optimal covariance control for stochastic systems under chance constraints.  _IEEE Control Systems Letters_ 2(2): 266–271.  10.1109/LCSYS.2018.2826038.
  * Pang (2021) Pang T (2021) A convex quasistatic time-stepping scheme for rigid multibody systems with contact and friction.  _2021 IEEE International Conference on Robotics and Automation (ICRA)_ : 6614–6620URL https://api.semanticscholar.org/CorpusID:228095227.
  * Pang et al. (2023) Pang T, Suh HJT, Yang L and Tedrake R (2023) Global planning for contact-rich manipulation via local smoothing of quasi-dynamic contact models.  _IEEE Transactions on Robotics_ 39(6): 4691–4711.  10.1109/TRO.2023.3300230.
  * Rodriguez (2021) Rodriguez A (2021) The unstable queen: Uncertainty, mechanics, and tactile feedback.  _Science Robotics_ 6(54): eabi4667.  10.1126/scirobotics.abi4667.  URL https://www.science.org/doi/abs/10.1126/scirobotics.abi4667.
  * Schulman et al. (2015) Schulman J, Levine S, Abbeel P, Jordan M and Moritz P (2015) Trust region policy optimization.  In: Bach F and Blei D (eds.) _Proceedings of the 32nd International Conference on Machine Learning_ , _Proceedings of Machine Learning Research_ , volume 37. Lille, France: PMLR, pp. 1889–1897.  URL https://proceedings.mlr.press/v37/schulman15.html.
  * Schulman et al. (2017) Schulman J, Wolski F, Dhariwal P, Radford A and Klimov O (2017) Proximal policy optimization algorithms.
  * Shirai et al. (2023) Shirai Y, Jha DK and Raghunathan AU (2023) Covariance steering for uncertain contact-rich systems.  In: _2023 IEEE International Conference on Robotics and Automation (ICRA)_. pp. 7923–7929.  10.1109/ICRA48891.2023.10160249.
  * Todorov et al. (2012) Todorov E, Erez T and Tassa Y (2012) Mujoco: A physics engine for model-based control.  In: _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_. pp. 5026–5033.  10.1109/IROS.2012.6386109.
  * Toussaint et al. (2020) Toussaint M, Ha JS and Driess D (2020) Describing physics for physical reasoning: Force-based sequential manipulation planning.  _IEEE Robotics and Automation Letters_ 5(4): 6209–6216.  10.1109/LRA.2020.3010462.
  * Toussaint et al. (2022) Toussaint M, Harris J, Ha JS, Driess D and Hönig W (2022) Sequence-of-constraints mpc: Reactive timing-optimal control of sequential manipulation.  _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ : 13753–13760URL https://api.semanticscholar.org/CorpusID:247362576.

## Appendix

### Separation of the Expected Cost

Suppose that the state $\bm{x}\in\mathbb{R}^{n_{x}}$ that is to be controlled
is a random variable that is distributed with $\bm{x}\sim p$. Furthermore
suppose that the task is described by a quadratic cost of the state and a
desired state $\bm{x}_{\mathrm{des}}$, i.e.

$J_{\mathrm{det}}(\bm{x})=\left(\bm{x}-\bm{x}_{\mathrm{des}}\right)^{\scriptscriptstyle\top}\left(\bm{x}-\bm{x}_{\mathrm{des}}\right).$
(55)

The corresponding stochastic optimal control objective is given by the
expectation of the quadratic cost of the state:

$\displaystyle\begin{split}J_{\mathrm{sto}}&=\mathrm{E}_{p}\left[J_{\mathrm{det}}(\bm{x})\right]\\\
&=\mathrm{E}_{p}\left[\left(\bm{x}-\bm{x}_{\mathrm{des}}\right)^{\scriptscriptstyle\top}\left(\bm{x}-\bm{x}_{\mathrm{des}}\right)\right]\\\
&=\mathrm{E}_{p}\left[\bm{x}^{\scriptscriptstyle\top}\bm{x}\right]-2\mathrm{E}_{p}\left[\bm{x}\right]^{\scriptscriptstyle\top}\bm{x}_{\mathrm{des}}+\bm{x}_{\mathrm{des}}^{\scriptscriptstyle\top}\bm{x}_{\mathrm{des}}.\end{split}$
(56)

In the following, we denote the mean of the state with

$\bar{\bm{x}}=\mathrm{E}_{p}\left[\bm{x}\right].$ (57)

Furthermore, the variance of the state is defined as the expectation of the
squared deviations from the mean:

$\displaystyle\begin{split}\mathrm{V}_{p}\left[\bm{x}\right]&=\mathrm{E}_{p}\left[(\bm{x}-\bar{\bm{x}})^{\scriptscriptstyle\top}(\bm{x}-\bar{\bm{x}})\right],\\\
&=\mathrm{E}_{p}\left[\bm{x}^{\scriptscriptstyle\top}\bm{x}+\bar{\bm{x}}^{\scriptscriptstyle\top}\bar{\bm{x}}-2\bar{\bm{x}}^{\scriptscriptstyle\top}\bm{x}\right],\\\
&=\mathrm{E}_{p}\left[\bm{x}^{\scriptscriptstyle\top}\bm{x}\right]+\bar{\bm{x}}^{\scriptscriptstyle\top}\bar{\bm{x}}-2\bar{\bm{x}}^{\scriptscriptstyle\top}\mathrm{E}_{p}\left[\bm{x}\right],\\\
&=\mathrm{E}_{p}\left[\bm{x}^{\scriptscriptstyle\top}\bm{x}\right]-\bar{\bm{x}}^{\scriptscriptstyle\top}\bar{\bm{x}}.\end{split}$
(58)

As a result, the stochastic optimal control objective can be rewritten in
terms of the deterministic quadratic cost of the mean state and the variance
of the state with respect to its probability distribution $p$:

$\displaystyle\begin{split}J_{\mathrm{sto}}&=\bar{\bm{x}}^{\scriptscriptstyle\top}\bar{\bm{x}}-2\bar{\bm{x}}^{\scriptscriptstyle\top}\bm{x}_{\mathrm{des}}+\bm{x}_{\mathrm{des}}^{\scriptscriptstyle\top}\bm{x}_{\mathrm{des}}+\mathrm{V}_{p}\left[\bm{x}\right]\\\
&=\left(\bar{\bm{x}}-\bm{x}_{\mathrm{des}}\right)^{\scriptscriptstyle\top}\left(\bar{\bm{x}}-\bm{x}_{\mathrm{des}}\right)+\mathrm{V}_{p}\left[\bm{x}\right]\\\
&=J_{\mathrm{det}}(\bar{\bm{x}})+\mathrm{V}_{p}\left[\bm{x}\right].\end{split}$
(59)

### Smoothness Prior

The goal of this section is to prove that
$p_{\mathrm{s}}(\bm{q}_{\mathrm{via}})$ is the normalized probability density
function of the unnormalized function
$e^{-J_{\mathrm{s}}(\bm{q}_{\mathrm{via}})}$. $J_{\mathrm{s}}$ is defined with

$J_{\mathrm{s}}=\frac{1}{2}\int_{0}^{T}\ddot{\bm{q}}^{r{\scriptscriptstyle\top}}(t)\bm{R}_{\bm{q}}\ddot{\bm{q}}^{r}(t)dt.$
(60)

The acceleration is an affine function of the trajectory parameter, i.e.

$\ddot{\bm{q}}^{r}(t)=\ddot{\bm{\Phi}}_{\mathrm{via}}(t)\bm{\theta}{+}\ddot{\bm{\phi}}_{0}(t,\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r}).$
(61)

Now, suppose that the basis offset is computed as follows:

$\ddot{\bm{\phi}}_{0}(t,\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})=\ddot{\bm{\Phi}}_{\bm{q}_{0}}(t)\bm{q}_{0}^{r}+\ddot{\bm{\Phi}}_{\dot{\bm{q}}_{0}}(t)\dot{\bm{q}}_{0}^{r}.$
(62)

We exploit this affine dependency on the initial position and velocity by
rewriting the acceleration as a linear function in a combined trajectory
parameter $\bm{\xi}$, i.e.

$\ddot{\bm{q}}^{r}(t)=\ddot{\bm{\Phi}}(t)\bm{\xi},$ (63)

where the new basis function matrix and the new trajectory parameter are given
by

$\ddot{\bm{\Phi}}(t)=\begin{pmatrix}\ddot{\bm{\Phi}}_{\mathrm{via}}(t)&\\!\ddot{\bm{\Phi}}_{\bm{q}_{0}}(t)&\\!\ddot{\bm{\Phi}}_{\dot{\bm{q}}_{0}}(t)\end{pmatrix},\quad\bm{\xi}=\begin{pmatrix}\bm{\theta}\\\
\bm{q}_{0}^{r}\\\ \dot{\bm{q}}_{0}^{r}\end{pmatrix}.$ (64)

As a result, the smoothness objective is equivalent to

$\displaystyle\begin{split}J_{\mathrm{s}}&=\frac{1}{2}\int_{0}^{T}\bm{\xi}^{\scriptscriptstyle\top}\ddot{\bm{\Phi}}(t)^{\scriptscriptstyle\top}\bm{R}_{\bm{q}}\ddot{\bm{\Phi}}(t)\bm{\xi}\,dt,\\\
&=\frac{1}{2}\bm{\xi}\int_{0}^{T}\ddot{\bm{\Phi}}(t)^{\scriptscriptstyle\top}\bm{R}_{\bm{q}}\ddot{\bm{\Phi}}(t)\,dt\,\bm{\xi},\\\
&=\frac{1}{2}\bm{\xi}\bm{R}_{\bm{\xi}}\bm{\xi}.\end{split}$ (65)

We obtain a smoothness metric

$\bm{R}_{\bm{\xi}}=\begin{pmatrix}\bm{R}_{\bm{\theta}}&\bm{R}_{\bm{\theta}|\bm{q}_{0},\dot{\bm{q}}_{0}}\\\
\bm{R}_{\bm{\theta}|\bm{q}_{0},\dot{\bm{q}}_{0}}^{\scriptscriptstyle\top}&\bm{R}_{\bm{q}_{0},\dot{\bm{q}}_{0}}\end{pmatrix},$
(66)

that is the result of the integral in the second equation in (65). With this
result, the smoothness objective is in fact a quadratic function in $\bm{\xi}$
and can thus be expressed as a zero-mean Gaussian distribution with

$p_{\mathrm{s}}(\bm{\xi})=p_{\mathrm{s}}(\bm{\theta},\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})=\mathcal{N}\left(\bm{0},\bm{R}_{\bm{\xi}}^{-1}\right).$
(67)

This Gaussian distribution is a joint distribution over the original
trajectory parameter $\bm{\theta}$ and the initial position and velocity. At
the time of constructing the smoothness prior, the initial position and
velocity are given. Consequently, we obtain the smoothness prior for
$\bm{\theta}$ by conditioning on the initial conditions, i.e.

$\displaystyle\begin{split}p_{\mathrm{s}}(\bm{\theta}|\bm{q}_{0}^{r},\dot{\bm{q}}_{0}^{r})=\mathcal{N}\left(\bar{\bm{\theta}}_{\mathrm{s}},\bm{R}_{\bm{\theta}}^{-1}\right),\\\
\bar{\bm{\theta}}_{\mathrm{s}}=\bm{R}_{\bm{\theta}}^{-1}\bm{R}_{\bm{\theta}|\bm{q}_{0},\dot{\bm{q}}_{0}}\begin{pmatrix}\bm{q}_{0}\\\
\dot{\bm{q}}_{0}\end{pmatrix}.\end{split}$ (68)

### Experiment: Implementation Details

In the following, we provide the details of the implementations for running
the experiments as described in Sec. 7.

#### Path Tracking Cost.

In a single-horizon optimization problem, we define the task-specific cost
based on the object belief only. Given a predicted trajectory of particles, we
denote the initial belief with $b_{0}$ and the belief at the end of the
horizon with $b_{T}$. For both beliefs, we compute the expected object states,
i.e. $\bar{\bm{q}}^{o}_{0}=\mathrm{E}_{b_{0}}[\bm{q}^{o}]$ and
$\bar{\bm{q}}^{o}_{T}=\mathrm{E}_{b_{T}}[\bm{q}^{o}]$, for consequently
computing the cost with respect to the candidate solution. The target path is
defined as a circle with

$\bm{q}^{o}_{\mathrm{path}}(s)=r_{\mathrm{path}}\begin{pmatrix}\cos(2\pi s)\\\
\sin(2\pi s)\end{pmatrix},\,s\in[0,1].$ (69)

The radius of the circle is $r_{\mathrm{path}}=0.15$. Next, we find the point
on the target path that is closest to the expected object states with

$s_{k}=\operatorname*{arg\,min}_{s}||\bar{\bm{q}}^{o}_{k}-\bm{q}^{o}_{\mathrm{path}}(s)||_{2},\,\mathrm{s.t.}\,s\in[0,1],$
(70)

for $k=\\{0,T\\}$. Consequently, we compute the path tracking cost with

$c_{\mathrm{path}}=e^{w_{\mathrm{progress}}(s_{0}-s_{T})}+w_{\mathrm{error}}||\bar{\bm{q}}^{o}_{T}-\bm{q}^{o}_{\mathrm{path}}(s_{T})||_{2}^{2}.$
(71)

The first cost term measures the tracking progress by comparing the progress
variable $s$ at the end of the horizon with the beginning of the horizon. We
scale the first cost term with $w_{\mathrm{progress}}=100$. The second cost
term penalizes deviations from the path, where the squared error term is
weighted with $w_{\mathrm{error}}=2000$.