













\section*{Appendix}
\input{sections/8_appendix/computation_graph_for_inverted_pendulum.tex}
\input{sections/8_appendix/ablation_notes.tex}
\input{sections/8_appendix/experiment_details.tex}
\input{sections/8_appendix/additional_exp.tex}
\end{document}

\section{Introduction}%
\label{sec:introduction}
Planning is one of the key problems in artificial intelligence~(AI), as it enables intelligent agents to make informed decisions and achieve their objectives in complex and dynamic environments.
This is especially important when the environment is inherently stochastic or when the dynamics model used by the planning algorithm is imperfect.
Planning algorithms should represent and reason about uncertainty to make robust and effective decisions.
As a result, research on planning with stochastic transitions has been multifaceted, encompassing symbolic task-level planning in discrete spaces \cite{kolobov2012reverse,keller2013trial,cui2019stochastic}, robotic motion planning in continuous spaces \cite{kurniawati2011motion,van2012motion,agha2014firm}, integrated task and motion planning \cite{kaelbling2013integrated,vega2018admissible,garrett2021integrated}, and model-based reinforcement learning leveraging probabilistic dynamics \cite{chua2018deep,hafner2019dream,curi2020efficient}.

Markov Decision Processes~(MDPs) provide the theoretical foundation for planning under uncertainty, but scalability remains a challenge, and approximation is often necessary.
Many approaches have been proposed in the literature, including searching in action space, searching in state space, approaches using Monte-Carlo simulation, and approaches using differentiation.
Several gradient-based planners have been proposed, but they are not easily usable as domain independent planners due to scalability \cite{deisenroth2011pilco}, restriction to  deterministic environments \cite{wu2020scalable}, or in the Reinforcement Learning (RL) context -- in most works, the success of the planner depends on the quality of the learned domain-specific value function \cite{tamar2016value,hafner2019dream}.

The paper fills this gap by introducing a novel domain-independent online planner using differentiable probabilistic transition models.
Our work is related to prior algorithms for trajectory optimization, distribution propagation, and differential dynamic programming \cite{chua2018deep,williams2017information,deisenroth2013gaussian,tassa2012synthesis,lenz2015deepmpc}.
However, we introduce a novel symbolic approximation and propagation scheme generalizing work on AI planning that facilitate robustness, better approximation, and optimization \cite{cui2019stochastic}.

The main contribution of this work is in designing DiSProD (\textbf{Di}fferentiable \textbf{S}ymbolic \textbf{Pro}pagation of \textbf{D}istributions), an online planner for environments with probabilistic transitions,
in continuous or hybrid spaces.
The main idea is to create a symbolic graph that encodes the distribution of future trajectories conditioned on a given policy.
The resulting symbolic graph provides an \emph{analytically differentiable representation} of the policy's value, allowing efficient gradient-based optimization of the action variables for long-horizon search.
While distributions over trajectories are too complex to capture exactly, DiSProD uses Taylor's approximation and an independence assumption to facilitate a symbolic propagation of product distributions over future state and reward variables.
The approximate distribution propagation can be viewed as an efficient symbolic aggregation of many trajectories, which differs from sampling algorithms that generate many individual trajectories and aggregate the results.
This approach reduces variance in stochastic environments and facilitates planning with sparse rewards.

Extensive quantitative experiments are conducted to compare DiSProD with state-of-the-art planners in discrete-time planning in OpenAI Gym environments and continuous-time control of simulated robotic systems.
The results show that DiSProD outperforms existing planners in dealing with stochastic environments, sensitivity to search depth, sparsity of rewards, and large action spaces.
Furthermore, we control two real-world robotic systems using DiSProD with an approximate transition model, demonstrating that it can successfully control ground vehicles and surface vessels to navigate around obstacles.
\section{Related Work}\label{sec:related_work}
Planning in continuous spaces has been studied in several sub-fields.
A key distinction in planning methods is between \emph{offline planners}, which compute a complete solution and then apply it, and \emph{online planners}, also known as Model Predictive Control~(MPC)~\cite{borrelli2017predictive}, in which at every decision point, optimization is carried out over a finite horizon and the first action from the optimization is executed.
The online nature of MPC provides some robustness to unexpected outcomes at the cost of increased computation time during action selection.
Another important distinction is planning in \emph{state space} (or configuration space) ~\cite{lavalle2006planning} versus planning in \emph{action space}.
The former seeks an optimal sequence of states, leaving action execution to a low-level controller, while the latter produces executable actions directly.
DiSProD is an \emph{online action planner} but it can also produce a sequence of states as a byproduct, as we discuss in \Cref{sec:exp}.

Within online action planners, DiSProD
is related to two lines of work using differentiable transition models.
First, our approach builds on the algorithm of \cite{cui2018stochastic,cui2019stochastic}, which was developed for discrete task-level AI planning problems.
However, that work is restricted to binary state and action variables.
In addition, DiSProD introduces a new distribution propagation method based on symbolic Taylor expansions.
The second group includes planners using
differentiable transition models in RL and control.
However, deep RL work (e.g., \cite{heess2015learning,depeweg2016learning}) makes use of learned value functions to aid planning performance and cannot plan in a new model without training first, and many approaches (e.g., \cite{wu2020scalable}) use deterministic transition models.
In addition, these approaches
optimize over individual trajectories and do not propagate distributions over trajectories as in DiSProD.
In this realm, iLQG \cite{tassa2012synthesis} and the PILCO family \cite{deisenroth2013gaussian,parmas2018pipps,kamthe2018data} are most related to our method.
iLQG linearizes the dynamics and assumes a linear Gaussian dynamics, as in the Extended Kalman Filter~(EKF), and optimizes over individual trajectories.
PILCO does propagate distributions analytically, albeit with the restricted Gaussian process~(GP) dynamics and the Gaussian kernel.
Follow-up work on PILCO replaces the GP with Bayesian neural networks \cite{gal2016improving}, which cannot propagate distributions analytically and hence requires particle-based planning~\cite{amadio2022model}.
From this perspective, DiSProD can be seen as a generalization of PILCO and iLQG, which uses differentiation over approximate symbolic propagation of distribution.

Our work is also related to sampling-based planners
Model Predictive Path Integral~(MPPI) and Cross-Entropy Method~(CEM) \cite{williams2017information,wagener2019online,chua2018deep}.
These algorithms sample a set of trajectories around the nominal trajectory using a fixed sampling distribution, and update the nominal trajectory based on the ``goodness'' of the trajectories to bias sampling in subsequent iterations.
Our framework uses distribution propagation instead of sampling and naturally optimizes stochastic policies instead of relying on a prefixed sampling distribution.

Finally, we note that our approach can flexibly handle both discrete and continuous variables.
In contrast, other methods for planning in hybrid spaces typically restrict the transitions, for example, to piecewise linear models \cite{li2005lazy,zamani2012symbolic,raghavan2017hindsight}.

\section{Algorithms and Methodology}%
\label{sec:alg}
A Markov Decision Process~(MDP) is specified by $\{\mathcal{S}, \mathcal{A}, {T}, \mathcal{R}, \gamma\}$, where $\mathcal{S}$ is a finite state space, $\mathcal{A}$ is a finite action space, ${T}$ is the transition function, $\mathcal{R}$ is the one-step reward function, and $\gamma$ is the discount factor. A policy $\pi$ is a mapping from states to actions.
Given a policy $\pi$, the action-value function $\mathcal{Q}^\pi(\bm{s}_t,\bm{a}_t)$ represents the expected discounted total reward that can be obtained from state $\bm{s}_t$ if action $\bm{a}_t$ is chosen, and the policy $\pi$ is followed thereafter, where $\bm{s}_t$ and $\bm{a}_t$ are vectors of state and action variables 
at time $t$.

As in prior work, our approach approximates
$\mathcal{Q}^\pi(\bm{s}_t,\bm{a}_t)$ and picks $\bm{a}_t$ that maximizes this value.
We focus on probabilistic policies parameterized by $\theta=\{\theta_t\}$ where each $\theta_t$ picks the action at time step $t$ and is further factorized over individual action variables.
Our goal is to produce an approximation of $\mathcal{Q}^{\theta}(\bm{s}_t,\theta_t)=\mathbb{E}[\sum_{i = 0}^{D-1} \gamma^i \mathcal{R}(\bm{s}_{t+i}, \bm{a}_{t+i})]$
where $\bm{a}_t\sim p(\bm{a}_t| \theta_t)$ and $\bm{s}_{t+1}\sim p(\bm{s}_{t+1}|\bm{s}_t, \bm{a}_{t})$,
optimize $\theta$ and  pick the action using $\theta_t$.
In principle the $\mathcal{Q}$-value can be calculated from the distributions over $\{(\bm{s}_{t+i}, \bm{a}_{t+i})\}$ but these distributions are complex.
Our algorithm approximates these distributions as products of independent distributions over individual state and reward variables.

Our algorithm can be conceptually divided into two parts. 
The first 
calculates an approximate distribution over future states, rewards and $\mathcal{Q}^\pi(\bm{s}_t,\bm{a}_t)$, conditioned on a given policy. The second uses this representation to optimize the policy. These are described in the next two subsections.

\subsection{Analytic Computation Graph}%
\label{sec: analytic_graph}

\mypar{Transition Model}
Typically, given $\bm{s}_t$ and $\bm{a}_t$, a simulator $T_{\text{sim}}$, samples from a distribution over $\bm{s}_{t+1}$.  Formally, $\bm{s}_{t+1} \sim T_{\text{sim}}(\bm{s}_t, \bm{a}_t)$.
For DiSProD, we need to \emph{encapsulate} any internal sampling in $T_{\text{sim}}$ as an input, to separate sampling from computation, similar to the reparameterization trick,
\begin{align}
\bm{s}_{t+1} = T(\bm{s}_t, \bm{a}_t, \bm{\epsilon}_t) \mbox{ where } \bm{\epsilon}_t \sim \mathcal{N}(0, I).
\label{eq:encapsulate}
\end{align}
$T_{\text{sim}}$ and its encapsulation are shown in \Cref{fig:hl-diagram}a and 1b.

\mypar{Representation}
Our construction can handle both discrete and continuous variables. The marginal distribution for any binary random variable $s_k$ at time $t$ (denoted by $s_{t,k}$), can be captured using a single parameter $\hat{s}_{t,k}$. We note that the variance is implicitly given by $\hat{v}_{s,t,k} = \hat{s}_{t,k} (1-\hat{s}_{t,k})$.
When ${s}_{t,k}$ is continuous, we summarize its distribution using its expectation and variance $(\hat{\mu}_{s,t,k},\hat{v}_{s,t,k})$.
Similarly, the distribution over the noise variable $\epsilon_i$ at time $t$ is represented using $(\hat{\mu}_{\epsilon,t,i},\hat{v}_{\epsilon,t,i})$.
While the use of mean and variance suggests a normal distribution, the proposed construction does not assume anything about the form of the distribution.

\mypar{Independence Assumption}
To simplify the computations of propagated distributions, our approximation assumes that for all $t$, the distribution over the trajectories given by $p(\bm{s}_t,\bm{a}_t,\bm{\epsilon}_t)$ is a product over independent factors: $p(\bm{s}_t,\bm{a}_t,\bm{\epsilon}_t)=\prod_k p({s}_{t,k}) \prod_\ell p({a}_{t,\ell}) \prod_i p(\epsilon_{t,i})$.

\mypar{Approximation}
Our main observation is that the above assumption suffices to propagate approximate distributions and further approximate the $\mathcal{Q}$ function.
Specifically, we approximate the transition function using a second order Taylor expansion whose terms are given analytically in a symbolic form.
Let $\bm{z}_t=(\bm{s}_t,\bm{a}_t,\bm{\epsilon}_t)$, $\hat{\bm{z}}_t=(\hat{\mu}_{\bm{s}_t},\hat{\mu}_{\bm{a}_t},\hat{\mu}_{\bm{\epsilon}_t})$ and $\bm{s}_{t+1} = T(\bm{z}_t)$.
To simplify the notation, consider the $j$-th state variable represented by $s_{t+1,j}=T_j(\bm{s}_t, \bm{a}_t, \bm{\epsilon}_t) = T_j(\bm{z}_t)$ and let $\nabla T_j = \nicefrac{\partial T_j}{\partial \bm{z}_t}$ and $H_j=\nicefrac{\partial^2 T_j}{\partial \bm{z}_t \partial \bm{z}_t^\top}$.
We use Taylor's expansion to approximate the encapsulated model in \Cref{eq:encapsulate} by
\begin{align}
    s_{t+1,j}\leftarrow T_j(\bm{z}_t) &\approx T_j(\hat{\bm{z}_t}) + \nabla T_j^\top (\bm{z}_t-\hat{\bm{z}_t}) + \frac{1}{2}(\bm{z}_t-\hat{\bm{z}_t})^\top H_j (\bm{z}_t-\hat{\bm{z}_t}).
\end{align}
This approximation is illustrated in \Cref{fig:hl-diagram}c.
We use this approximation to calculate the mean and variance of $\bm{s}_{t+1}$ via methods of propagating distributions.
Given our independence assumption, the off diagonal covariance terms in $H_j$ are zero, and the expected value of the $s_{t+1, j}$ becomes
\begin{align}
    &\hat{\mu}_{s,t+1,j} =\mathbb{E}\left[s_{t+1, j}\right]\approx{T}_j(\hat{\bm{z}_t})+\frac{1}{2}\Biggl[\sum_k\left(\frac{\partial^2 T_j}{\partial{s}_{t,k}^2}\right)\hat{v}_{s,t,k} + \sum_\ell\left(\frac{\partial^2{T}_j}{\partial{a}_{t,\ell}^2}\right)\hat{v}_{a,t,\ell}+\sum_{i}\left(\frac{\partial^2{T}_j}{\partial{\epsilon}_{t,i}^2}\right)\hat{v}_{\epsilon,t,i}\Biggr].
\end{align}
A similar approximation for the variance yields $4^\text{th}$ order moments.
To reduce complexity we use a first order Taylor approximation for the variance resulting in
\begin{align}
    \hat{v}_{s,t+1,j} &\approx \sum_k \biggl(\frac{\partial T_j}{\partial {s}_{t,k}} \biggr)^2 \hat{v}_{s,t,k} + \sum_\ell \biggl(\frac{\partial T_j}{\partial {a}_{t,\ell}} \biggr)^2 \hat{v}_{a,t,\ell} + \sum_i \biggl(\frac{\partial T_j}{\partial {\epsilon}_{t,i}} \biggr)^2 \hat{v}_{\epsilon,t,i}.
\end{align}
To write these concisely using vector notation, we collect the first order partials and diagonals of the Hessians into matrices as follows:
\begin{align}
  J_{\bm{s}_t} &= \biggl[\frac{\partial T}{\partial \bm{s}_{t}^\top}\biggr],
\qquad{} \ \ \ \mbox{ i.e.,\ \ \ \ }  [J_{\bm{s}_t}]_{j,k}= \frac{\partial T_j}{\partial s_{t,k}},
               \\
  \tilde{H}_{\bm{s}_t} &= \biggl[\frac{\partial^2 T}{\partial \bm{s}_{t}^2}^\top \biggr], 
  \qquad{} \mbox{ i.e.,\ \ \ \ }  [\tilde{H}_{\bm{s}_t}]_{j,k}= \frac{\partial^2 T_j}{\partial s_{t,k}^2}.
\end{align}
Similarly, we define $J_{\bm{a}_t},\tilde{H}_{\bm{a}_t}, J_{\bm{\epsilon}_t}, \text{ and } \tilde{H}_{\bm{\epsilon}_t}$ for action and noise variables, respectively.
Now we can write the vector form of the equations as follows:
\begin{align}
 
    \hat{\mu}_{\bm{s}_{t+1}} &\approx T(\hat{z_t}) + \frac{1}{2}\biggl[ \tilde{H}_{\bm{s}_t} \hat{v}_{\bm{s}_t} + \tilde{H}_{\bm{a}_t} \hat{v}_{\bm{a}_t} + \tilde{H}_{\bm{\epsilon}_t} \hat{v}_{\bm{\epsilon}_t} \biggr],
  \label{eq:expectation_vector_form}
  \\
 
  \hat{v}_{\bm{s}_{t+1}}  &\approx (J_{\bm{s}_t} \odot J_{\bm{s}_t}) \hat{v}_{\bm{s}_t} + (J_{\bm{a}_t} \odot J_{\bm{a}_t}) \hat{v}_{\bm{a}_t} + (J_{\bm{\epsilon}_t} \odot J_{\bm{\epsilon}_t}) \hat{v}_{\bm{\epsilon}_t}.
  \label{eq:variance_vector_form}
\end{align}
The computations of \Cref{eq:expectation_vector_form,eq:variance_vector_form} are illustrated in \Cref{fig:hl-diagram}d.
Stacking these computations over multiple time steps gives us an approximation of the distribution over future states, captured analytically as a computation graph, as shown in \Cref{fig:hl-diagram}e.
Adding the computation of the reward over these distributions in the same manner yields a computation graph for an approximation of the $\mathcal{Q}$-function.
Note that the computation graph propagates distributions and does not sample trajectories.
The computation only requires the mean $(\hat{\mu}_\epsilon=0)$ and variance $(\hat{v}_\epsilon=1)$ of the noise which are known in advance and are absorbed as constants in the graph.

This construction is very general -- only requiring access to $T(\bm{s}_t,\bm{a}_t,\bm{\epsilon}_t)$, and analytic computation of its partial derivatives, e.g., $\nicefrac{\partial T_j}{\partial {s}_{t,k}}$, $\nicefrac{\partial T_j}{\partial {a}_{t,\ell}}$.
In practice, transition functions can have non-differentiable components which can be mitigated by approximating the non-smooth functions with their smoother alternatives.
For instance, a sharp Hyperbolic Tangent function can approximate a step function.

\subsection{Optimization Algorithm}
Thanks to the symbolic representation, once the computation graph is built, it can be reused for all steps  of problem solving and in fact for multiple instances of the same problem as long as the reward function does not change.
The optimization is conceptually simple, initializing all action variables for all time steps and performing gradient based search over the computation graph. However, some important details are discussed next.

\mypar{Multiple Restarts and Loss function}
Following \cite{cui2019stochastic}, we perform gradient search with multiple restarts.
While they performed restarts sequentially, we take advantage of the structure of the computation to perform the restarts in a vectorized form.
If we have $n_a$ action variables at each time step and the search horizon is $d$, then a policy is given by $n_o=2dn_a$ parameters for the mean and variance of each variable.
Feeding the current state and these $n_o$ parameters to the computation graph yields an approximation of the $\mathcal{Q}$-value and hence we can perform the optimization.
To handle multiple restarts we generate a matrix of $n_r \times n_o$ entries where every row represents a possible policy.
Then we define the (negative) loss function to be the sum of the $\mathcal{Q}$-values on all the rows in the matrix.
Therefore, we benefit from evaluating all restarts as a ``batch of examples" relative to the computation graph.
Since we are optimizing with respect to the input matrix (and not computation graph weights), the loss decomposes into a sum of independent functions and gradient search effectively optimizes all restarts simultaneously.

\input{algorithms/disprod.tex}

\mypar{Initialization of Actions}
For discrete action variables, following \cite{cui2019stochastic}, we initialize actions 
parameters for time step $t=0$ to binary values and for steps $t>0$, we initialize the marginals to random values in $[0,1]$.
For continuous action variables, we must constrain actions to be in a valid interval.
For each action variable the policy is given by a   
uniform distribution, initializing $a_\mu \sim \mathcal{U}(a_\text{min}, a_\text{max})$ and the variance of a uniform distribution centered around $a_\mu$. Formally, $a_{v} = \nicefrac{\min(a_\text{max} - a_\mu, a_\mu - a_\text{min})^2}{12}$.

\mypar{Policy Updates}
We use gradient search with Adam \cite{kingma2015adam} to optimize the action variables over all restarts simultaneously.
We make at most $K=10$ updates and stop the search early when the actions converge: $||\bm{a}_\text{new} - \bm{a}_\text{old}||_\infty \le 0.1$ for normalized action ranges.
Gradients are used to update both $a_\mu$ and $a_v$, which implies that we search over a stochastic policy.
This is important for success in sparse-reward scenarios.
If we have sparse rewards and both the transitions and policy are deterministic, then the gradient will be zero because small changes to the action variables will not make a difference for the sparse reward.
With a stochastic policy, there is some probability of obtaining the sparse reward and hence gradient information will be available.
Hence the main role of the stochastic policy is to broaden the search when needed, while also allowing a nearly deterministic choice of actions at convergence.

\mypar{Safe Projected-Gradient Update}
The gradient-based updates have to satisfy two constraints.
The first is the need to constrain variables into valid intervals.
For this, we use the standard projected gradient descent to restrict $a_\mu$ and $a_v$. 
Specifically, $a_\mu$ is constrained to the valid values according to the environment, and $a_v$ is constrained to $\min(\nicefrac{1}{12}, \nicefrac{\min(a_\mu - a_\text{min}, a_\text{max} - a_\mu)^2}{12})$. If the gradient step pushes $a_\mu$ or $a_v$ outside the valid region, it is clipped to the boundary.
Finally, we only take a gradient step on the restart if it improves the $\mathcal{Q}$-value and otherwise we maintain the previous value.
This safe update requires an additional evaluation of the computation graph to check for the $\mathcal{Q}$-value improvement and it increases runtime, but it ensures that the performance is monotonically increasing. 

\mypar{Saving Actions}
Since the gradients are back-propagated through the entire computation graph, action variables at all depths are adjusted according to the corresponding gradients. However, only the action at depth $d=0$ is executed. Formally, at time $t$, we choose the action $a_{t,0}$ for state $s_t$, while $a_{t,1:D}$ are not used.
These updated actions can be used to initialize the action variables when planning at state $s_{t+1}$.
The same idea has been used before in MPPI \cite{williams2017information,wagener2019online}.
Note that MPPI also uses multiple samples, but these samples all contribute to the update of a single action sequence, which can potentially limit the search~\cite{lambert2021stein,barcelos2021dual}.
To allow reuse of old search but add diversity, we initialize one restart using the old actions shifted one time step, and initialize all other restarts randomly.

\mypar{Overall Optimization Algorithm}
These steps are summarized in \Cref{alg:opt},
where search depth, initial step-size and number of restarts serve as hyper-parameters.
When all restarts have converged or $K$ gradient steps have been performed, we choose the \emph{best restart} with the maximum $\mathcal{Q}$-value, breaking ties randomly.
Finally, mean of the first action from the best restart is selected.

\section{Experiments}%
\label{sec:exp}

We experiment with DiSProD on a variety of deterministic and stochastic environments and evaluate its robustness to long horizon planning, its ability to work with sparse rewards, and its performance in high-dimensional action spaces.


For this purpose, we conduct extensive experiments on several simulated and real-world environments with increasing complexity, as shown in \Cref{fig:exp_setup}.
We use three OpenAI Gym \cite{brockman2016openai} environments (Cart Pole, Mountain Car, and Pendulum) to evaluate the \emph{robustness} of the compared planners in terms of stochasticity, planning horizon, and reward sparsity.

In addition, we developed a new Gym environment that models a simplified vehicle dynamics, elaborating over the Dubins' car model \cite{dubins1957curves}. In particular, the agent can control the change in linear $(\Delta v_t)$ and angular velocity $(\Delta \omega_t)$ instead of controlling the velocities directly, and the maximum change $(\overline{\Delta{v}}, \overline{\Delta{\omega}})$ is limited to a small value. This model is still inaccurate because it ignores friction, inertia, actuation noise and localization noise, but it provides an acceptable approximation. We then use this model to plan in a physics simulator with asynchronous execution. Specifically, the planner is used to control a TurtleBot in Gazebo simulation via a Robot Operating System~(ROS) interface.



Finally, to demonstrate robustness and applicability, DiSProD controls two physical robot systems: an Unmanned Ground Vehicle (Jackal) and an Unmanned Surface Vessel (Heron) using the aforementioned model.

OpenAI Gym simulators are deterministic by default.
To support experiments with probabilistic models, we enhance these environments by explicitly adding noise into the model and using it as a part of the dynamics $T(\bm{s},\bm{a},\bm{\epsilon})$. Further details are in the supplementary material. For the experiments, it suffices to note that we parameterize the amount of noise using a scalar $\alpha$ and can therefore evaluate planners' performance as a function of stochasticity in the dynamics.



\subsection{Baselines}
We compare DiSProD to CEM \cite{kobilarov2012cross,chua2018deep} and MPPI \cite{williams2017information,wagener2019online}.
Both CEM and MPPI are shooting-based planners that maintain a sequence of action distributions to sample actions from.
From a given state, they use the sampled actions to generate multiple trajectories and compute the rewards accumulated at the end of each trajectory.
The main difference between CEM and MPPI is how they recompute the action distribution.
While CEM uses the actions corresponding to the top $n$ trajectories to form the new action distribution, MPPI weights the actions belonging to a trajectory by the accumulative reward for that trajectory.

Hyperparameters for the planners were frozen after tuning them on deterministic versions of the environments.
Both CEM and MPPI can potentially benefit from the use of saving actions.
We have found that this is helpful in the basic environments but harms the performance of these algorithms in the simplified vehicle model.
To ensure a fair comparison we use the best setting for the baselines in the experiments whereas DiSProD always saves actions.

\subsection{Evaluation in Basic Gym Environments}

\mypar{Increasing Stochasticity}
First, we explore the performance of the planners with the original deterministic environments and with added stochasticity.
To separate randomness in the environment from randomness in the experiment, we perform 8 repetitions over 6 runs in each environment, calculating means in each repetition and standard deviations of the means across repetitions.
Averages and standard deviations are shown in \Cref{fig:exp_noise_cartpole,fig:exp_noise_pendulum,fig:exp_noise_mountain_car}. We observe that the planners perform similarly in deterministic environments ($\alpha=0$) but 
with increasing amounts of stochasticity,
DiSProD degrades more gracefully and performs better than CEM or MPPI.


\mypar{Increasing Planning Horizon}

Intuitively the deeper the search, the more informative it is. But deeper search can also increase variance when estimating action quality and hence harm performance. 
Results for experiments testing this aspect 
in noisy variants of basic environments 
are shown in \Cref{fig:exp_depth_cartpole_noisy,fig:exp_depth_pendulum_noisy,fig:exp_depth_mountain_car_noisy} and deterministic environments are included in the supplementary material.

We observe that while in the deterministic setting all the planners have similar performance, in some stochastic environments, the performance of CEM and MPPI can degrade with increasing search depth while DiSProD gives better results.
In these environments, beyond a required minimum depth, increasing the depth further does not help performance.
This changes, however, when we change reward sparsity.


\mypar{Increasing Reward Sparsity}
\label{sec:planning_with_sparse_rewards}
Robust planners should be able to work with both sparse and dense rewards.
Intuitively, if the reward is dense then even a short search horizon can yield good performance, but a deeper search is required for sparse reward .
To test this, we evaluate the performance of the planners by varying the sparsity of the rewards in the Mountain Car environment. With the standard reward function, the agent gets a large positive reward when its position and velocity are larger than certain thresholds. We modify the reward function to use a smooth version of greater-than-equal-to function given by $\sigma(10\beta(x-\text{target}))$
where $\beta$ is a sparsity multiplier.
The larger the value of $\beta$, the harder it is to get a reward from bad trajectories.
Results are shown in \Cref{fig:exp_4_depth_100,fig:exp_4_depth_200}.
\Cref{fig:exp_4_depth_100} shows that with the standard search depth of 100, all planners fail to reach the goal once the reward becomes very sparse.
\Cref{fig:exp_4_depth_200} shows that DiSProD can recover and perform well by increasing search depth to 200 but CEM and MPPI fail to do so.


\subsection{Evaluation in High Dimensional Action Space}
\label{sec: experiment_with_high_dim_action_space}

To explore 
high dimensional action spaces, 
we modify the Mountain Car environment by adding redundant action variables.
The dynamics still depend on a single relevant action variable but the reward function includes additional penalties for the redundant actions.
To obtain a high score, the agent must use a similar policy as before for the relevant action and keep the values of the redundant action variables as close to 0 as possible. Details of the model are in the Appendix. We compare the performance of the planners against number of redundant actions, without changing any other hyperparameters.
Results are shown in \Cref{fig:exp_high_dim_scores}, where the MPPI results are omitted since the scores are too low and they distort the plot.
We observe that while CEM/MPPI perform poorly as the action space increases, the performance of DiSProD remains extremely stable.
The performance of CEM improves if we increase the population size (number of samples) by a factor of 100 (from 200 to 2000 or 20,000), but it still lags behind DiSProD. 
Additional results analysing this scenario are given in the Appendix.
We note that despite the inferior reward, CEM is able to achieve the goal. However, as shown in \Cref{fig:exp_high_dim_steps}, it requires a lot more steps to reach the goal location.
This experiment illustrates the potential advantage of planners that use 
the analytic model to identify what causes good outcomes 
as compared to algorithms that estimate this effect through sampling.

\subsection{Evaluation with Hybrid State Space}
While we focus on experiments in continuous spaces, our planner is compatible with hybrid environments.  To illustrate this we modify the Cart Pole environment to include a binary variable which is set to 1 if the cart is to the right of a certain x-coordinate. The agent receives a reward of 3 when this binary variable is set to 1, otherwise it gets a reward of 1. 
\Cref{fig:exp_hybrid_depth,fig:exp_hybrid_noise} 
show the performance of DiSProD and CEM against planning horizon and noise level. 
We observe that a deeper search is required for this problem and that DiSProD can successfully obtain high reward.
\subsection{Evaluation with a Physics Simulator}
\label{sec:evaluation_with_a_physics_simulator}

We control a TurtleBot using a Gazebo simulation and evaluate the performance of DiSProD on 16 maps with varying degrees of difficulty. In these experiments, the analytic transition model does not take into account the obstacles.
The reward function provides a negative reward when the agent collides with obstacles.\footnote{Incorporating obstacles in the transition yields similar or better results, but slows down the planner due to the increased size of the analytical computation graph.}
The obstacle patterns and detailed evaluation results are in the Appendix.
Following \cite{wu2020towards}, we use two metrics to evaluate the performance of the planners - success rate (SR) and success weighted by optimal path length (SL). SR measures the success percentage of the planners across different maps, while SL is the ratio of the planned path length to the euclidean distance from starting position to goal averaged over cases where navigation was successful. Intuitively, the lower the SL value, the faster the agent reaches the goal.



Results are shown in \Cref{tab:turtlebotresults}, averaged over 5 runs for each map and averaged over all instances. We observe that all planners are able to control the TurtleBot but DiSProD performs better in both metrics.
We also evaluated the planners in a Gym environment where the planners' model matches the environment dynamics exactly. In this case, all planners perform similarly. 
This suggests that the main difference in performance in TurtleBot is due to robustness of DiSProD to handle inaccuracy in the model.




\subsection{Experiments with Robotic Systems}
\label{sec: exp_with_robotics_systems}
We use DiSProD with the same analytic model and planner interface as above, to control Jackal and Heron. Real-time control requires high frequency control commands to avoid significant drift. Therefore, one has to
find a good balance between planning horizon $(d)$ and the maximum change of linear velocity $\overline{\Delta{v}}$.
Experiments with low $\overline{\Delta{v}}$ require a large $d$ thus are slow, while experiments with high $\overline{\Delta{v}}$ suffer from drift, as the hardware cannot stop or accelerate instantaneously.
We specified these parameters through initial exploration with the systems. Other parameters are the same as in the TurtleBot simulation experiments.

\input{figures/tex/exp_hardware.tex}
\input{tables/turtlebot/turtlebot_summary_analytic_only.tex}

For experiments with Jackal, DiSProD is used exactly as above, i.e., sending actions to the robot.
For the surface vessel, however, we use our planner in another way because our Heron has a motor issue in one of its thrusters.
Specifically, DiSProD optimizes the action sequence exactly as before.
Instead of sending the actions, it computes intermediate states expected to be reached with the policy (which are available in the computation graph), and sends these as ``waypoints" to a PID controller.
We found that, since our planner works at a fine time granularity, we can send every 5th state to a PID controller and achieve smooth control.
\Cref{fig:exp_hardware_results} visualizes the trajectories generated by DiSProD when controlling Jackal and Heron. 
\subsection{Ablation Studies}

Our planner propagates the state distribution using \Cref{eq:expectation_vector_form,eq:variance_vector_form}.
In this section, we explore the contribution of the different variance terms in these equations ($\hat{\bm{v}}_{S_t}, \hat{\bm{v}}_{A_t}$ and $\hat{\bm{v}}_{\epsilon_t}$)
to the performance of the algorithm.
We use the term \textit{complete mode} when the planner uses the variance terms (DiSProD) and \textit{no-variance mode} when it zeroes them out (DiSProD-NV).

We look into the state distributions produced by DiSProD and DiSProD-NV, and compare them against the empirical state distributions as described below.  We fix the start state and a sequence of action distributions, and compute the next-state distribution for a fixed depth. To obtain the empirical state distribution, we sample actions from the same fixed action distribution and use the dynamics model to compute the next-state.
The trajectory distributions for Pendulum are visualized in \Cref{fig:rollouts_stochastic_simple_env_fixed_alpha}.
We observe that DiSProD gives us a better approximation than DiSProD-NV -- the mean is more accurate and while the variance is underestimated, it has a reasonable shape.
Additional experiments detailed in the Appendix show additional visualization of trajectory distributions, as well as showing that both action and state variance contribute to the improved planning performance.

\section{Discussion and Conclusions}

The paper presents DiSProD, a novel approach for planning in continuous stochastic environments.
The method is general and overcomes limitations of prior work by using an abstracted representation, using a higher order Taylor approximation, and showing how optimization via gradients can be done over propagation of distributions.
Experiments show success across multiple problems, improved handling of stochastic environments, and decreased sensitivity to search depth, reward sparsity, and large action spaces.
Also, DiSProD is compatible with control in real robotic systems.

DiSProD  has some limitations that are important problems for future exploration.
The first is computational complexity where the main bottlenecks are the computation of the diagonal of Hessian matrix 
and the depth of the search.
Alternative formulations of the computation graph might provide speedup.
The second is the approximation quality which requires non-zero values for first and second derivatives, where alternative analytic approximations might allow for improvements. 
The third is the assumption made for this work that the transition function is known. 
A natural approach is to learn the model as in model-based RL and use it for planning.
We leave further exploration of these questions to future work.


\section{Contribution of Sub-Expressions in the Taylor's Expansion}
\label{sec:analysis_of_ablation_modes}

As described in Section \ref{sec: analytic_graph}, the vector form of the equations are as follows:

\begin{align}
    \hat{\mu}_{\bm{s}_{t+1}} &\approx T(\hat{\bm{z}_t}) + 0.5 \biggl[ \tilde{H}_{\bm{s}_t} \hat{v}_{\bm{s}_t} + \tilde{H}_{\bm{a}_t} \hat{v}_{\bm{a}_t} + \tilde{H}_{\bm{\epsilon}_t} \hat{v}_{\bm{\epsilon}_t} \biggr]\\
\hat{v}_{\bm{s}_{t+1}}  &\approx (J_{\bm{s}_t} \odot J_{\bm{s}_t}) \hat{v}_{\bm{s}_t} + (J_{\bm{a}_t} \odot J_{\bm{a}_t}) \hat{v}_{\bm{a}_t}
+ (J_{\bm{\epsilon}_t} \odot J_{\bm{\epsilon}_t}) \hat{v}_{\bm{\epsilon}_t} 
\end{align}
where $\hat{\bm{z}_t} = (\hat{\mu}_{\bm{s}_{t}}, {\mu}_{\bm{a}_{t}}, {\mu}_{\bm{\epsilon}_{t}})$

The equations depend on the distribution of state, action and noise variables. To analyze the impact each set of variables has on the outcome, we consider three cases - no-variance, state-variance and complete.

\mypar{No variance} In this setup, all the variance terms$(\hat{v}_{\bm{s}}, \hat{v}_{\bm{a}}, \hat{v}_{\bm{\epsilon}})$ are zeroed out. So the equations become
\begin{align}
   
    \hat{\mu}_{\bm{s}_{t+1}}  &\approx T_j(\hat{\bm{z}}) \\
   
    \hat{v}_{\bm{s}_{t+1}}  &\approx 0
\end{align}
    
\mypar{State variance} Now, we just zero out action variance $(\hat{v}_{\bm{a}})$, which means we effectively optimize a deterministic policy.

\begin{align}
    \hat{\mu}_{\bm{s}_{t+1}} &\approx T(\hat{\bm{z}_t}) + 0.5 \biggl[ \tilde{H}_{\bm{s}_t} \hat{v}_{\bm{s}_t} + \tilde{H}_{\bm{\epsilon}_t} \hat{v}_{\bm{\epsilon}_t} \biggr]\\
\hat{v}_{\bm{s}_{t+1}}  &\approx (J_{\bm{s}_t} \odot J_{\bm{s}_t}) \hat{v}_{\bm{s}_t} + (J_{\bm{\epsilon}_t} \odot J_{\bm{\epsilon}_t}) \hat{v}_{\bm{\epsilon}_t}
\end{align}

Note that at $t=0$, as the planner knows the state exactly, therefore $\hat{v}_{\bm{s}_0} = 0$. We now consider the possible values the remaining partials might take and analyze the impact. 



\begin{enumerate}
    \item $J_{\bm{\epsilon}_t} = 0$ and $\tilde{H}_{\bm{\epsilon}_t} = 0$. Then,
    \begin{align}
       
        \hat{\mu}_{\bm{s}_1} \approx T(\hat{\bm{z}_0}) 
        \quad \text{and}\quad
       
        \hat{v}_{\bm{s}_1} \approx 0
    \end{align}
    As $\hat{v}_{\bm{s}_t}$ always remains zero, the result is similar to the no-variance mode.
    
    \item $J_{\bm{\epsilon}_t} \ne 0$ but $\tilde{H}_{\bm{\epsilon}_t} = 0$. Then,
    \begin{align}
       
        \hat{\mu}_{\bm{s}_1}  \approx T(\hat{\bm{z}_0})
         \quad \text{and}\quad
       
        \hat{v}_{\bm{s}_1} \approx (J_{\bm{\epsilon}_t} \odot J_{\bm{\epsilon}_t}) \hat{v}_{\bm{\epsilon}_t}
    \end{align}
    As $\hat{v}_{\bm{s}_1} \ne 0$, it influences $\hat{\mu}_{\bm{s}_2}$ and  $\hat{v}_{\bm{s}_2}$ and future timesteps.
    
    \item $J_{\bm{\epsilon}_t} = 0$ but $\tilde{H}_{\bm{\epsilon}_t} \ne 0$. This can happen as we evaluate the partials at the mean of the variables, and we assume zero-mean noise ($\hat{\mu}_{\bm{\epsilon}_t} = 0$). So while $J_{\bm{\epsilon}_t}$ exists, it can evaluate to zero. In such a scenario, we have,
    \begin{align}
       
        \hat{\mu}_{\bm{s}_1}  \approx T(\hat{\bm{z}_0}) + \tilde{H}_{\bm{\epsilon}_t} \hat{v}_{\bm{\epsilon}_t}
         \quad \text{and}\quad
       
        \hat{v}_{\bm{s}_1} = 0
    \end{align}
    As $\hat{v}_{\bm{s}_{t+1}}  \approx (J_{\bm{s}_t} \odot J_{\bm{s}_t}) \hat{v}_{\bm{s}_t}$ and $\hat{v}_{\bm{s}_0} = 0$, the next-state variance is always zero. We can generalize and write 
     \begin{align}
       
        \hat{\mu}_{\bm{s}_{t+1}}  \approx T(\hat{\bm{z}_t}) + \tilde{H}_{\bm{\epsilon}_t} \hat{v}_{\bm{\epsilon}_t}
         \quad \text{and}\quad
       
        \hat{v}_{\bm{s}_{t+1}} = 0
    \end{align}
\end{enumerate}


    

\mypar{Complete} This is the default variant of DiSProD which uses all the variance terms. 
As discussed above, for successful propagation of distribution, DiSProD requires at least some of the $J$ or $\tilde{H}$ terms to be non-zero.


\section{Experimental Results}
\input{figures/tex/exp_appendix_gym.tex}

\myparquestion{Does having a larger planning horizon help in deterministic environments?}
In our experiments with basic Gym environments, we observe that optimal performance, with all the three planners, is achieved with a small planning horizon. Increasing the planning horizon further does not impact the performance. (\Cref{fig:exp_appendix_gym_results}~(a-c)).

\myparquestion{Does sampling more trajectories help?}
Shooting algorithms sample action sequences to generate trajectories. Intuitively, increasing the number of action sequences that a planner is allowed to sample should help with better plans and hence improve performance. 
We observe that in deterministic environments, sampling a small number of trajectories is sufficient to obtain near optimal performance. Making the environments stochastic degrades the planners' overall performance. Interestingly enough, in these environments, the performance remains unaffected on increasing the number of samples. Note that we use the same hyperparameters in deterministic and stochastic environments. 
(\Cref{fig:exp_appendix_gym_results}~(d-i)).


\input{figures/tex/exp_high_dim_cem_convergence.tex}
\myparquestion{How does the performance of shooting algorithms vary when the size of the action space increases?}
When the action space increases, a small number of samples cover a very small volume of the possible trajectories. Intuitively, the performance of agents relying on shooting methods will be poor in such scenarios and performance should improve on increasing the number of samples.  As shown in \Cref{fig:exp_high_dim_scores}, this indeed is the case.  We take a closer look at the performance of CEM as the population size increases. For this, we use the modified version of Mountain Car as detailed in \Cref{sec: experiment_with_high_dim_action_space} with 14 redundant actions. In \Cref{fig:exp_high_dim_cem_convergence}, we plot the values of 5 action variables (A1 to A5) where A1 influences the dynamics while the rest are redundant action variables. For attaining high rewards, an agent must learn to keep the values of action variables A2 through A5 as close to 0 as possible, while using A1 to act optimally. We observe that as the population size increases, CEM does a better job at keeping the values of the redundant variables close to 0. Plots shown in the main paper show that this indeed corresponds to improved performance. 


\myparquestion{Does DiSProD provide a better approximation of the system dynamics than DiSProD-NV?}
As discussed earlier, DiSProD-NV does not account for any uncertainty in the dynamics and ignores the variance terms in the Taylor's expansion while DiSProD uses a 2nd degree Taylor's expansion to approximate the dynamics.
Note that DiSProD-NV does not calculate a distribution but simply propagates the mean.
In addition, its calculated mean is potentially less accurate.
We explore this behaviour in SimpleEnv. For a fixed sequence of action distribution, we compute the state distribution due to DiSProD and DiSProD-NV and compare that to the empirical state distribution visited by the agent when it samples from the same action distribution. In Figure \ref{fig:viz_rollouts} we compare the empirical state distribution of $x$ against the state distributions as computed by DiSProD and DiSProD-NV. 
When $\alpha$ is 0, the approximation using DiSProD overlaps with the empirical distribution for some time before diverging slightly. As the stochasticity of the environment increases (indicated by increasing $\alpha$ values), the state distributions due to DiSProD and DiSProD-NV diverge from the empirical state distribution. But the distribution due to DiSProD is a better approximation than the distribution due to DiSProD-NV.
\input{figures/tex/visualizing_rollouts}


\myparquestion{Do both state and action variance improve planning?}
DiSProD relies on state and action variance terms to propagate distributions. Intuitively, state variance captures the impact of noise on the state variables while having action variance enables DiSProD to search over a stochastic policy. We explore the performance of the three variants of DiSProD discussed in \Cref{sec:analysis_of_ablation_modes}. 
Results are shown in Figure \ref{fig:ablation_cartpole}, \ref{fig:ablation_mountain_car}, \ref{fig:ablation_pendulum}.
We observe that for regions of low stochasticity, all the three modes have similar performance. But with increasing $\alpha$, zeroing out the state variance harms DiSProD. Further, in all of the basic environments, zeroing out action variance but keeping state variance does not harm performance. 
However, as shown in \Cref{fig:ablation_simple_env}, in the Simple Env which has a more complex noise structure (see \Cref{sec:simple_env_details}) action variance leads to further improvement in performance. 

\input{figures/tex/ablation.tex}

\myparquestion{Detailed results for experiments with modified Dubins Car model}
The detailed results for experiments with the modified Dubins Car model in Gym and TurtleBot are given in \Cref{tab:results-gym-dubins,tab:results-dubins-turtlebot}. These are aggregated in the main paper in \Cref{tab:turtlebotresults} by averaging over the 16 maps.  


\input{tables/gym/planning_with_stds.tex}

\input{tables/turtlebot/turtlebot_planning_summary_w_stds.tex}

\section{Example: Computation Graphs with Known Transition Models}

For a concrete example, let us consider the dynamics of the Pendulum from the OpenAI Gym. Let $S_t = [\theta_t, \Dot{\theta}_t]^\top$ be the state vector, $A_t = [a_t]^\top$ be the action vector and $\epsilon_t = [e_t]^\top$ be the noise vector. The transition function is defined as follows: 
\begin{align}
    T(S,A, \epsilon) &= [\theta_{t+1}, \Dot{\theta}_{t+1}]^\top, \\ 
    \text{where }  \Dot{\theta}_{t+1} &= \Dot{\theta}_{t} + \left(-c_1 \sin(\theta_t + \pi) +  c_2 a_t\right) \Delta_t \nonumber \\
    \theta_{t+1} &= \theta_t + (\Dot{\theta}_{t+1} + e_t) \Delta_t \nonumber
\end{align}
In this equation, $c_1 = \frac{3g}{2l}$ and $c_2 = \frac{3}{ml^2}$, are constants.
Then the expectation and variance of the transition function can be written as follows:
\begin{align}
   J_{S_t} 
   &= \frac{\partial S_{t+1}}{\partial S_{t}}
    =     \begin{bmatrix}
         \frac{\partial \theta_{t+1}} {\partial \theta_{t}} &
         \frac{\partial \theta_{t+1}} {\partial \Dot{\theta}_{t}} \\
         \frac{\partial \Dot{\theta}_{t+1}} {\partial \theta_{t}} & 
         \frac{\partial \Dot{\theta}_{t+1}} {\partial \Dot{\theta}_{t}}
    \end{bmatrix} 
    = \begin{bmatrix}
         1 - c_1 \cos(\theta_t + \pi) {\Delta^2_t} &
         \Delta_t \\
         -c_1 \cos(\theta_t + \pi) {\Delta_t} & 
         1
    \end{bmatrix}\\
    J_{A_t} 
    &= \frac{\partial S_{t+1}}{\partial A_{t}}
    = \begin{bmatrix}
         \frac{\partial \theta_{t+1}} {\partial a_{t}}\\
         \frac{\partial \Dot{\theta}_{t+1}} {\partial a_{t}} 
    \end{bmatrix}
    = \begin{bmatrix}
         c_2 \Delta^2_t\\
         c_2 \Delta_t
    \end{bmatrix}\\
    J_{\epsilon_t} 
    &= \frac{\partial S_{t+1}}{\partial \epsilon_{t}}
    = \begin{bmatrix}
         \frac{\partial \theta_{t+1}} {\partial e_{t}} \\
         \frac{\partial \Dot{\theta}_{t+1}} {\partial e_{t}} 
    \end{bmatrix}
    = \begin{bmatrix}
         \Delta_t \\
         0
    \end{bmatrix}\\
    \tilde{H}_{S_t} 
    &= \frac{\partial^2 S_{t+1}}{\partial S_{t}^2} 
    = \begin{bmatrix}
         \frac{\partial^2 \theta_{t+1}} {\partial \theta_{t}^2} &
         \frac{\partial^2 \theta_{t+1}} {\partial \Dot{\theta}_{t}^2} \\
         \frac{\partial^2 \Dot{\theta}_{t+1}} {\partial \theta_{t}^2} & 
         \frac{\partial^2 \Dot{\theta}_{t+1}} {\partial \Dot{\theta}_{t}^2}
        \end{bmatrix} 
    =  \begin{bmatrix}
         c_1 \sin(\theta_t + \pi) \Delta^2_t &
         0 \\
         c_1 \sin(\theta_t + \pi) \Delta_t & 
         0
        \end{bmatrix}\\
    \tilde{H}_{A_t} 
    &= \frac{\partial^2 S_{t+1}}{\partial A_{t}^2}
    = \begin{bmatrix}
             \frac{\partial^2 \theta_{t+1}} {\partial a_{t}^2} \\
             \frac{\partial^2 \Dot{\theta}_{t+1}} {\partial a_{t}^2} 
        \end{bmatrix}
    =  \begin{bmatrix}
             0 \\
             0
        \end{bmatrix}\\
    \tilde{H}_{\epsilon_t} 
    &= \frac{\partial^2 S_{t+1}}{\partial \epsilon_{t}^2}
    = \begin{bmatrix}
             \frac{\partial^2 \theta_{t+1}} {\partial e_{t}^2} \\
             \frac{\partial^2 \Dot{\theta}_{t+1}} {\partial e_{t}^2} 
            \end{bmatrix}
    =   \begin{bmatrix}
             0 \\
             0 
        \end{bmatrix}\\
\hat{\bm{v}}_{S_t} 
    &= \begin{bmatrix}
         \hat{v}_{\theta, t} \\
         \hat{v}_{\Dot{\theta}, t}
        \end{bmatrix} \qquad
\hat{\bm{v}}_{A_t} 
= \begin{bmatrix}
         \hat{v}_{a, t}
        \end{bmatrix} \qquad
\hat{\bm{v}}_{\epsilon_t} 
=  \begin{bmatrix}
         \hat{v}_{e, t}
        \end{bmatrix} 
\end{align}
\begin{align}
    \mathbb{E}[T(S_t,A_t, \epsilon)] &\approx T(\mu_{S_t}, \mu_{A_t}, \mu_\epsilon) + 0.5 \left( 
       
         \tilde{H}_{S_t}
         \hat{\bm{v}}_{S_t}
        +
       
         \tilde{H}_{A_t}
        \hat{\bm{v}}_{A_t}
        +
       
        \tilde{H}_{\epsilon_t} 
       \hat{\bm{v}}_{\epsilon_t}
    \right)  \\
   
    \mathbb{V}[T(S_t,A_t, \epsilon)] &\approx
   
    (J_{S_t} \odot J_{S_t})
   \hat{\bm{v}}_{S_t}
        +
   
    (J_{A_t} \odot J_{A_t})
    \hat{\bm{v}}_{A_t}  + 
   
   (J_{\epsilon_t} \odot J_{\epsilon_t})
    \hat{\bm{v}}_{\epsilon_t}
\end{align}

This illustrates that we can have  analytic gradients for the model as mentioned above. 
Note that, for the sake of simplicity, we have considered just a single noise variable in this model. However, the same approach works with multiple noise variables. 

\input{figures/tex/diagram}

The computation graph for depth 2 for this example is shown in Figure \ref{fig:diagram}. To make the graph simple, certain portions have been abstracted away. More specifically, $T$ signifies the transition function and $r$ specifies the reward function. 
For this example, nodes $g_1, h_1, h_2$ and $h_3$ are used to compute the expectation and variance of next state variables and are defined as follows: $g_1 = \Tilde{H}_{S_t}$;
                            $h_1 = J_{S_t} \odot J_{S_t}$;
                            $h_2 = J_{A_t} \odot J_{A_t}$; 
                          $h_3 = J_{\epsilon_t} \odot J_{\epsilon_t}$

\section{Experiment details}
\subsection{Environments}
In this section, we briefly describe the environments used for our experiments. The basic environments are taken from OpenAI Gym to which we make minor modifications. We rewrite the transition and reward functions by replacing any step-functions with their smooth equivalents. As the environments are deterministic, we add noise explicitly to make them stochastic.  $\epsilon$ denotes noise sampled from a standard Gaussian and $\alpha$  controls the amount of noise being added.
\subsubsection{Cartpole}
\label{sec: env-desc-cartpole}
Cartpole has four state variables - $x, y, \theta, \dot{\theta}$ and one control variable which controls the force acting on the cart. We make the environment stochastic by adding Gaussian noise to the force being applied. Concretely, the updated equation for force becomes $\text{force}_{\text{noisy}} = \text{force} + \alpha \epsilon$.
\subsubsection{Mountain Car}
Mountain Car has two state variables - position $(x)$ and velocity $(v)$ and one control variable ($u$) which is the force to be applied on the car. At a high level, the environment has two functions $f_x$ and $f_v$ that are used to update the position and velocity.
$$v_{t+1} = f_v(x_t, v_t) \text{ and } x_{t+1} = f_x(x_t, v_{t+1})$$
We add noise to $v_{t+1}$ to make the environment stochastic. $v_{t+1} = f_v(x_t, v_t) + \alpha \epsilon$.
\subsubsection{Pendulum}
Pendulum also has two state variables - $\theta$ and $\dot{\theta}$ and the control variable $(u)$ is the torque. First, $\dot{\theta}_{t+1}$ is computed using $\theta_t$, $\dot{\theta}_t$, $u_t$ and some constants, and then $\theta_{t+1}$ is updated using $\dot{\theta}_{t+1}$. We augment this and add noise to the update equation for $\theta_{t+1}$ such that it becomes $\theta_{t+1} = \theta_{t} + (\dot{\theta}_{t+1} + \alpha \exp(\epsilon)) dt$.

\input{figures/tex/exp_continuous_dubins_car_gym}
\input{figures/tex/dubins}
\subsubsection{Dubins Car}
\label{sec: dubins_car_env_details}
The standard Dubins Car model maintains three state variables - $x, y, \theta$ and has two control variables - velocity ($v$) and angular velocity ($\omega$). The dynamics equations are:
\begin{align}
    x_{t+1} &= x_t + v_t \cos \theta_t dt \\
    y_{t+1} &= y_t + v_t \sin \theta_t dt \\
    \theta_{t+1} &= \theta_t + \omega dt
\end{align}
Since this is a discrete time system and ignores acceleration, the motion of the car can be unrealistic. For example, although $v_t = 0$ and $v_{t+1} = v_{\max}$ is a valid control, this cannot be performed by a real vehicle. As Gazebo is a real-physics simulator and considers factors like acceleration, inertia and friction, planning with this model leads to poor performance in Gazebo. In order to obtain a better approximation of the model used by Gazebo, we modify the controls from velocity and angular velocity to delta velocity $(\Delta v)$ and delta angular velocity $(\Delta \omega)$ and compute velocity and angular velocity as
\begin{align}
    v_t &= v_{t-1} + \Delta v_{t} \\
    \omega_t &= \omega_{t-1} + \Delta \omega_{t}
\end{align}
This prevents sudden changes in linear and angular velocity and helps to limit the actions. We experiment with this model on a suite of maps ranging from simple ones with no obstacles to complex ones with obstacles.
All the map configurations are shown in \Cref{fig:Dubins car maps} and the two of the RViz maps are shown in \Cref{fig:dubins}.


\subsubsection{Simple Env}
\label{sec:simple_env_details}
In the Gym environments that we experimented with, some of the required partial derivatives were either zero or not significant enough to make a difference in the planning results. To showcase the difference more clearly, we designed an additional environment called SimpleEnv where all the required partials are non-zero.

The environment has two state variables - $x$ and $y$ and two action variables - $\delta_x$ and $\delta_y$.
The dynamics are as follows:
\begin{align}
    x_{t+1} &= x + \delta_x + \alpha (0.1\epsilon + \epsilon^2) \\
    y_{t+1} &= y + \delta_y
\end{align}
The reward function is a smoothed 0-1 reward where a reward of 1 is given if the agent is sufficiently close to a predefined goal position.

\subsubsection{Cartpole - Hybrid Variant}
To experiment with hybrid environments, we modify Cartpole and introduce an environment variable called \textit{reward marker} (set to 0.5 in the experiment) and a new binary state variable that is set to 1 if the cart is to the left of the reward marker. The reward function is modified such that the agent gets a reward of 3 if the agent is to right of the reward marker and otherwise it continues to get a reward of 1. The initial position of the cart is always to the left of the reward marker. The noise is added in a similar manner as in the original Cartpole environment. (\Cref{sec: env-desc-cartpole})

\subsection{Controlling Sparsity of Rewards}
As mentioned in \Cref{sec:planning_with_sparse_rewards}, we modify the default reward function of Continuous Mountain Car to use a smooth version of greater-than-equal-to function $(f_{ge})$. The function takes as input a variable $x$ and a target $t$ and outputs $\sigma(10\beta(x-t))$ where $\beta$ is the sparsity multiplier. The function is a smooth approximation of $x \ge t$.
 \Cref{fig:smooth_greater_than} shows a plot of $f_{ge}$ against $x-t$.
As the sparsity multiplier increases, the value of the function becomes increasingly similar to that of a step function and rewards become sparse.

\input{figures/tex/smooth_greater_than.tex}


\subsection{Hardware Platforms}
We use two physical robots in our experiments - Jackal and Heron. 

Jackal is an Unmanned Ground Vehicle made by Clearpath Robotics. As mentioned in \Cref{sec:evaluation_with_a_physics_simulator}, the position of obstacles in the environment are unknown to the transition model. The reward function is aware of the obstacles and penalizes trajectories that collide with one. Jackal's position and orientation are obtained through a VICON Motion Capture System, and it is controlled directly by DiSProD through commands for linear and angular velocity. 

Heron is an Unmanned Surface Vessel~(USV), also made by Clearpath Robotics. Similar to Jackal, the position of the obstacles is unknown to the transition model but available to the reward function. Heron's position is obtained by fusing GPS and Inertial Measurement Unit~(IMU) information using EKF.
It is also controlled through commands for linear and angular velocity, but in our experiments this is mediated through a PID controller because one of the USV's thrusters has intermittent motor failures, which requires a high-frequency feedback controller to compensate the significant error.
DiSProD computes an action sequence through its computation graph and then send a list of waypoints -- the means of states on the graph -- to the PID controller.
The list is updated asynchronously at every decision cycle of the planner. 

\subsection{Additional Details}
We list the values of the parameters used for our experiments with OpenAI Gym in \Cref{tab:gym_param_specs} and for our experiments with Jackal and Heron in \Cref{tab:hardware_param_specs}.

\input{tables/gym/parameter_specs.tex}
\input{tables/hardware/parameter_specs.tex}





