# Composable Energy Policies for Reactive Motion Generation and Reinforcement
Learning

Julen Urain1, Anqi Li2, Puze Liu1, Carlo D’Eramo1, Jan Peters13  1IAS, TU
Darmstadt, 2 University of Washington, 3MPI for Intelligent Systems  Email:
{urain, liu<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

Reactive motion generation problems are usually solved by computing actions as
a sum of policies. However, these policies are independent of each other and
thus, they can have conflicting behaviors when summing their contributions
together. We introduce Composable Energy Policies (CEP), a novel framework for
modular reactive motion generation. CEP computes the control action by
optimization over the product of a set of stochastic policies. This product of
policies will provide a high probability to those actions that satisfy all the
components and low probability to the others. Optimizing over the product of
the policies avoids the detrimental effect of conflicting behaviors between
policies choosing an action that satisfies all the objectives. Besides, we
show that CEP naturally adapts to the Reinforcement Learning problem allowing
us to integrate, in a hierarchical fashion, any distribution as prior, from
multimodal distributions to non-smooth distributions and learn a new policy
given them. Video in https://sites.google.com/view/composable-energy-
policies/home

Figure 1: Composable Energy Policies generate trajectories reactively in a
wide set of environments, as the ones in the figure. The generated
trajectories are attracted to a target pose, visualized by the green sphere,
and avoid a set of obstacles, shown as red spheres.

##  I Introduction

Many robotic tasks deal with finding a control action satisfying multiple
objectives. An apparently simple task such as watering some plants requires
satisfying multiple objectives to perform it properly. The robot should reach
the targets (the plants) with the watering can, avoid pouring water on the
floor while approaching, and avoid colliding and break plant’s branches with
its arms. In contrast with more sequential tasks [41, 16, 17, 38], in which
the objectives are satisfied concatenating them in time, in the presented
work, we consider tasks in which multiple objectives must be satisfied in
parallel.

The problem has been faced with a spectrum of solutions that balance between
global optimality and computational complexity. Path planning methods [25, 24,
19] find a global

trajectory from start to goal by a computationally intense Monte-Carlo
sampling process. Trajectory optimization methods [44, 31, 18, 36] reduce the
computational burden of planning methods by learning the global trajectory
given an initial trajectory candidate. These methods reshape the global
trajectory to satisfy the objectives. However, they still require solving an
optimization problem over long temporal horizon trajectories. Reactive Motion
Generators, such as Artificial Potential Fields (APF) methods [20, 22, 10, 21,
32, 3] have a very low computational cost, but lack any guarantees of finding
a global trajectory satisfying the objectives. These methods propose a modular
approach. Each component proposes a deterministic policy to satisfy one of the
objectives, and the control action is obtained by the sum of the policy
contributions.

There is a clear gap in methodology. The first (path planning and trajectory
optimization), approach the problem as an optimization or inference problem
over the combined objectives, while the second (reactive motion generation)
assumes a complete independence between objectives, lacking any optimality
guarantees. We frame Reactive Motion Generation as an optimization problem
over a product of expert policies [13, 30, 29, 11, 42]

$\displaystyle{\bm{a}}^{*}=\operatorname*{arg\,max}_{\bm{a}}\prod_{k}\pi_{k}({\bm{a}}|{\bm{s}}).$
(1)

Most of the previous approaches for reactive motion generation, compute each
action maximizing a certain objective independently and then, sum the actions
together [32, 20, 10, 14]. This independence between policies can end up in
conflicting behaviors leading to oscillations or local minima. In our
approach, we first compute the product of a set of stochastic policies, and
then, we compute the action maximizing the product of them. This approach can
be understood as a probabilistic logical conjunction (AND operator) [7, 42]
between the stochastic policies (see Fig. 2 for visual representation).

Figure 2:  Visual Representation of modular control for Goto + Obstacle
Avoidance. In the top box, we show Artificial Potential Field (APF) [20]
policy. In the bottom box, we show Composable Energy Policies (CEP). In
contrast, with artificial potential field method, that sums deterministic
actions (goto, avoid obstacle), CEP computes the product of the policy
distributions. The composition will provide high probability to those actions
that satisfy both components and low to the rest. This approach helps avoiding
conflicts between different components. Robot: blue circle, obstacle: red
circle and target: green cross. Thick dotted line: performed trajectory, light
dotted line: possible future trajectories.

Nevertheless, there are exceptions. Dynamic Window Approach (DWA) [9, 46]
solves the reactive motion generation for a 2D planar robot in a two steps
optimization algorithm. First, the search space of possible actions is reduced
given a set of constraints. Then, given an objective function, the optimal
action is computed solving an optimization problem. In contrast with DWA, in
our work, we compute the optimal action only through an optimization phase.
Additionally, we consider 7-dof robots instead of planar robots and allow to
integrate objectives defined in arbitrarily different task spaces. Finally,
given the flexibility of our method, we can integrate policies from multiple
sources in a single model. We could integrate obstacle avoidance policies with
data-driven learned policies, compute a solution by trajectory optimization
and add reaction to unexpected situations or integrate priors in a
Reinforcement Learning (RL) policy to accelerate learning and ensure safety.

#### Contribution

We present Composable Energy Policies (CEP), a new framework for modular robot
control. In contrast with previous methods [20, 32, 34] that solves reactive
motion generation with a sum of deterministic policies, our method solves
reactive motion generation by maximum likelihood estimation over a product of
expert policies. We claim that optimizing over the composed policy
distributions will increase the guarantees of satisfying all the objectives
jointly w.r.t. adding actions coming from different policies. To validate our
claims, we derive the reactive motion generation problem from control as
inference view and compute the optimality guarantees of applying a product of
expert policies (Appendix A).

In our work, we aim for flexible composition of modular policy distributions.
We propose a framework to integrate stochastic policies represented in
arbitrary state-action spaces. Additionally, these policies might have
multiple sources from data-driven learned multimodal policies to
computationally costly control as inference solutions. In section V, we show
how to integrate a RL policy with a set of prior policies to improve
exploration and guarantee safety exploration.

In the following, we focus on presenting the method’s main blocks while in the
Appendix, we focus on the theoretical interpretation of the method. In section
VI, we validate our algorithm in both reactive motion generation and prior
based RL problems.

#### Notation

As our discussion will involve a set of policies and a set of spaces in which
these policies are represented, we will use superscript ($\pi^{x}$) to
represent the space in which the policy is and subscript ($\pi_{x}$) to
represent the policy index. ${\bm{f}}_{x}^{z}(\cdot)$ represent a
transformation map from space ${\mathcal{X}}$ to space ${\mathcal{Z}}$,
${\bm{s}}^{x}$ is the state in space ${\mathcal{X}}$ and ${\bm{s}}^{z}$, the
state in space ${\mathcal{Z}}$.

##  II Preliminaries

###  II-A Artificial Potential Fields

Reactive motion generation deals with the problem of generating local and
quick motion. The developed methods require to have low computational burden
to give a fast response reacting to unexpected situations. APF [20] propose to
solve the problem by a weighted sum of a set of dynamic systems represented in
a set of task spaces. Let’s assume a set of transformations maps
${\bm{f}}_{x}^{z_{0}},\dots,{\bm{f}}_{x}^{z_{K}}$, that transform a point in
the configuration space, ${\mathcal{X}}$, to a set of task spaces,
${\mathcal{Z}}_{k}$,
$({\bm{z}}_{k},\dot{{\bm{z}}_{k}})=({\bm{f}}_{x}^{z_{k}}({\bm{x}}),{\bm{J}}^{z_{k}}({\bm{x}})\dot{{\bm{x}}})$,
with
${\bm{J}}^{z_{k}}=\frac{\partial{\bm{f}}_{x}^{z_{k}}({\bm{x}})}{\partial{\bm{x}}}$;
and a set of deterministic second-order dynamic systems in the task space,
${\bm{g}}^{z_{0}},\dots,{\bm{g}}^{z_{k}}$. The APF represents the dynamic
system in the configuration space ${\mathcal{X}}$

$\displaystyle{\bm{g}}^{x}=\sum_{k=0}^{K}{\bm{J}}^{z_{k}^{+}}{\bm{\Lambda}}_{k}({\bm{z}}_{k}){\bm{g}}^{z_{k}}$
(2)

with a weighted (${\bm{\Lambda}}_{k}({\bm{z}}_{k})$) sum of the of the
projected dynamics and ${\bm{J}}^{z_{k}^{+}}$ the pseudoinverse Jacobian. Each
dynamic component represent the policy to satisfy a particular objective.
Given that we are looking for the motion that satisfy multiple objectives, the
sum of the dynamics might not be enough as the different components might have
conflicting behaviours and thus lead to local minima or oscillations. In
Appendix C, we show that APF can be rewritten as a particular case of product
of experts and provide an interpretation of their failures.

We aim to solve the conflicts between dynamics by the maximum likelihood
estimation over the product of a set of dynamic distributions. In our work, we
model each dynamic component by a distribution. These distributions can be
understood as a weighted set of possible dynamics for each component. In the
combination of these distributions, our method will search for the dynamics
that better satisfy the combined set of objectives and thus, avoid conflicting
behaviors, as shown in Figure 2.

##  III Composable Energy Policies

Let us assume a set of independent stochastic policies
$\pi_{1}({\bm{a}}|{\bm{s}}),\dots,\pi_{K}({\bm{a}}|{\bm{s}})$ modeled by a
Boltzmann distribution

$\displaystyle\pi_{i}({\bm{a}}|{\bm{s}})=\exp(E_{i}({\bm{a}},{\bm{s}}))\frac{1}{Z_{i}({\bm{s}})}$
(3)

with $E:{\mathcal{S}}\times{\mathcal{A}}\rightarrow\mathbb{R}$ is an
arbitrarily represented energy function and
$Z({\bm{s}})=\int_{\bm{a}}\exp(E({\bm{a}};{\bm{s}}))d{\bm{a}}$ is the
normalization factor. Choosing a Boltzmann distribution is not an arbitrary
choice. Boltzmann distribution allows representing an arbitrary distribution
by a suitable definition of the energy function ($E$) [26]. Additionally,
computing the product of experts

$\displaystyle\pi({\bm{a}}|{\bm{s}})=\prod_{k=0}^{K}\pi_{k}({\bm{a}}|{\bm{s}})^{\beta_{k}}\propto\,\exp\left(\sum_{k}\beta_{k}E_{k}({\bm{a}},{\bm{s}})\right).$
(4)

will end up in a weighted sum over the individual energy components in the
log-space. Linearly combined energies are beneficial for computing the
energies contribution in parallel and thus, in practise, parallelize the
computation by multi-processing, increasing the control frequency. Product of
experts is a natural choice for our policy; given a set of energies
$E_{1},\dots,E_{K}$, product of experts is the posterior of an inference
problem maximizing the energies (See Appendix B).

###  III-A Energy Tree

In the composition proposed in (4), each energy function is considered to be
in the same state-action space. However, in most of the robotics scenarios, we
might be interested in composing together energies defined in different task
spaces. Navigation of the robot’s end-effector towards a target while avoiding
the obstacles, composes skills defined in different task spaces. Each robot
link should avoid obstacles and the end effector should reach a certain target
position. Inspired by APF [20] and Riemannian Motion Policies (RMP) [32], we
propose to model the composition of energies in different task spaces. We
introduce the probabilistic graphical model for our policy in Fig.3.

Our architecture is composed of two main components. First, we have a set of
policies $\pi^{z}({\bm{a}}^{z}|{\bm{s}}^{z})$, defined in different state-
action latent spaces (task space)
$({\bm{s}}^{z},{\bm{a}}^{z})\in{\mathcal{Z}}$. Second, we consider a set of
deterministic mappings that transform the state-actions in the common space
(configuration space) $({\bm{s}}^{x},{\bm{a}}^{x})\in{\mathcal{X}}$ to the
latent state-action spaces ${\mathcal{Z}}$,
${\bm{f}}_{x}^{z}:{\mathcal{X}}\xrightarrow{}{\mathcal{Z}}$.

Applying the change of variable rule in probabilistic density functions [1,
2], given the mapping ${\bm{f}}_{x}^{z}$ is bijective in the actions
(${\bm{a}}^{z}\leftrightarrow{\bm{a}}^{x}$), we can write the policy in the
common space, $\pi^{x}$, in terms of the latent space policy $\pi^{z}$ [45]

$\displaystyle\pi^{x}({\bm{a}}^{x}|{\bm{s}}^{x})$
$\displaystyle=\pi^{z}({\bm{a}}^{z}|{\bm{s}}^{z})\sqrt{\det({\bm{J}}^{\intercal}{\bm{J}})}$
$\displaystyle=\pi^{z}({\bm{f}}_{x}^{z}({\bm{s}}^{x},{\bm{a}}^{x}))\sqrt{\det({\bm{J}}^{\intercal}{\bm{J}})}.$
(5)

with
${\bm{J}}=\partial{\bm{f}}_{x}^{z}({\bm{a}}^{x},{\bm{s}}^{x})/\partial{\bm{a}}^{x}$
is the Jacobian of ${\bm{f}}_{x}^{z}$ in the action ${\bm{a}}^{x}$. The
equality in (5) does not hold for injective or surjective transformations. In
practice, we apply the equality for any possible map ${\bm{f}}_{x}^{z}$
(bijective, surjective, and injective), and we leave for future work the study
of the policy distribution under surjective or injective mappings.

From (5), we can write $\pi^{x}({\bm{a}}^{x}|{\bm{s}}^{x})$ w.r.t. the energy
function defined in the latent space $E^{z}({\bm{a}}^{z};{\bm{s}}^{z})$

$\displaystyle\pi^{x}({\bm{a}}^{x}|{\bm{s}}^{x})=\exp\left(E^{z}({\bm{f}}_{x}^{z}({\bm{a}}^{x},{\bm{s}}^{x}))\right)\frac{1}{Z({\bm{s}}^{x})}$
(6)

with

$\displaystyle
Z=\frac{\int_{{\bm{a}}^{z}}\exp(E^{z}({\bm{a}}^{z},{\bm{s}}^{z}))d{\bm{a}}^{z}}{\sqrt{\det({\bm{J}}^{\intercal}{\bm{J}})}}.$
(7)

This policy representation allows to define the energy function in an easy to
represent task space. Then, for any
${\bm{s}}^{x},{\bm{a}}^{x}\in{\mathcal{X}}$, we can map them to the latent
space ${\mathcal{Z}}$,
$({\bm{s}}^{z},{\bm{a}}^{z})=f_{x}^{z}({\bm{s}}^{x},{\bm{a}}^{x})$ and compute
the un-normalized log-probability of the action in a given latent policy
$E^{z}({\bm{a}}^{z},{\bm{s}}^{z})$.

Our energy tree is inspired by the APF formulation, but it has important
differences. In APF formulation, there is an explicit function from state
${\bm{s}}^{x}$ to action ${\bm{a}}^{x}$

$\displaystyle{\bm{a}}^{x}={\bm{f}}_{x}^{z-1}({\bm{a}}^{z})={\bm{f}}_{x}^{z-1}({\bm{g}}({\bm{s}}^{z}))={\bm{f}}_{x}^{z-1}({\bm{g}}({\bm{f}}_{x}^{z}({\bm{s}}^{x})))$
(8)

where ${\bm{f}}_{x}^{z}$ is the function mapping from common space to latent
space and ${\bm{g}}$ is a deterministic policy in the latent space. In this
approach, ${\bm{f}}_{x}^{z}$ is required to be invertible. In contrast, CEP
assumes an implicit function from ${\bm{s}}^{x}$ to ${\bm{a}}^{x}$

$\displaystyle{\bm{a}}^{x}=\operatorname*{arg\,max}_{{\bm{a}}^{x}}E^{z}({\bm{f}}_{x}^{z}({\bm{a}}^{x},{\bm{s}}^{x})).$
(9)

In this approach, we do not need to compute the inverse of ${\bm{f}}_{x}^{z}$.
Additionally, $E^{z}$ represents the energy in the latent space, so we can
combine distributions instead of combining deterministic dynamics. In Sec. IV
we show that having an implicit function might be beneficial for reactive
motion generation.

In case a set of policies exist in different task spaces
${\mathcal{Z}}_{1},\dots,{\mathcal{Z}}_{N}$, we can compose the policies
together

$\displaystyle\pi^{x}({\bm{a}}^{x}|{\bm{s}}^{x})=\exp\left(\sum_{k=1}^{K}\beta_{k}E^{z_{k}}({\bm{f}}_{x}^{z_{k}}({\bm{a}}^{x},{\bm{s}}^{x}))\right)\frac{1}{Z({\bm{s}}^{x})}$
(10)

Shown in Fig. 3, CEP has a recursive architecture. Our model allows us to
represent the latent policies $\pi^{z}({\bm{a}}^{z}|{\bm{s}}^{z})$ with a set
of policies in an even deeper latent space ${\mathcal{W}}$, and still compute
the optimization in the same way.

${\bm{s}}^{x}$${\bm{f}}_{x}^{z_{1}}$${\bm{f}}_{x}^{z_{2}}$${\bm{f}}_{x}^{z_{3}}$${\bm{s}}^{z_{1}}$${\bm{s}}^{z_{2}}$${\bm{s}}^{z_{3}}$${\bm{a}}^{z_{1}}$${\bm{a}}^{z_{2}}$${\bm{a}}^{z_{3}}$$\pi^{z_{1}}$$\pi^{z_{2}}$${\bm{f}}_{z}^{w_{1}}$${\bm{f}}_{z}^{w_{2}}$${\bm{s}}^{w_{1}}$${\bm{s}}^{w_{2}}$${\bm{a}}^{w_{1}}$${\bm{a}}^{w_{2}}$$\pi^{w_{1}}$$\pi^{w_{2}}$${\bm{f}}_{z}^{w_{1}}$${\bm{f}}_{z}^{w_{2}}$${\bm{f}}_{x}^{z_{1}}$${\bm{f}}_{x}^{z_{2}}$${\bm{f}}_{x}^{z_{3}}$${\bm{a}}^{x}$

Figure 3: Bayesian network for the energy tree. The policy distribution
$\pi^{x}({\bm{a}}^{x}|{\bm{s}}^{x})$ is computed w.r.t. the policies in the
latent spaces ${\mathcal{Z}}_{i}$. Additionally, a latent policy
$\pi^{z}({\bm{a}}^{z}|{\bm{s}}^{z})$ can be represented by a set of policies
in a different latent spaces ${\mathcal{W}}_{i}$. White diamonds represent
deterministic transformations and black square is a probabilistic mapping.

###  III-B Maximum Likelihood Estimation on CEP

In CEP, the optimal action is obtained by Maximum Likelihood Estimation over
the product of expert policies. Given the current state ${\bm{s}}^{x}$

$\displaystyle{\bm{a}}_{\textrm{ML}}^{x}=\operatorname*{arg\,max}_{{\bm{a}}^{x}}\prod_{k=0}^{K}\pi_{k}({\bm{a}}^{x}|{\bm{s}}^{x})^{\beta_{k}}\,,\,\beta_{k}>0.$
(11)

We assume a set of mappings ${\bm{f}}_{x}^{z}$ and a set of energies $E^{z}$
are given. We solve (11), by Cross Entropy optimization [35]. We define a
proposed sampling model $p({\bm{a}}|{\bm{\theta}})$ and iteratively solve

$\displaystyle{\bm{\theta}}_{t+1}$
$\displaystyle=\operatorname*{arg\,max}_{{\bm{\theta}}_{t}}\mathbb{E}_{p({\bm{a}}^{x}|{\bm{\theta}}_{t})}\left[\log\left(\prod_{k=0}^{K}\pi_{k}({\bm{a}}^{x}|{\bm{s}}^{x})^{\beta_{k}}\right)\right]$
$\displaystyle\propto\mathbb{E}_{p({\bm{a}}^{x}|{\bm{\theta}}_{t})}\left[\sum_{k=1}^{K}\beta_{k}E^{z_{k}}({\bm{f}}_{x}^{z_{k}}({\bm{a}}^{x},{\bm{s}}^{x}))\right].$
(12)

The method is presented in Algorithm 1. Each energy ($E^{z}$) component is
linearly dependent and thus, in the optimization process, we evaluate each
energy component in parallel. Also, we consider a batch $N$ of actions
evaluated all in parallel on GPU. This parallelization reduces the
computational time for the optimization process and allows to use CEP for
high-frequency reactive motion generation.

Given : $N$: Number of samples;

${\bm{s}}^{x}$: Current state in common space;

$K$: Number of Energy Components;

$({\bm{f}}_{x}^{z_{1}},E^{z_{1}},\beta^{z_{1}}),\dots,({\bm{f}}_{x}^{z_{K}},E^{z_{K}},\beta^{z_{K}}))$:
Maps, energies and inverse temperatures;

$I$: Optimization steps;

$(\mu_{0},\Sigma_{0})$: Initial sampling distribution mean and variance;

$({\bm{a}}^{*},e^{*})$: Initial optimal action and energy;

for  _$i\leftarrow 0$ to $I-1$_ do

for  _$n\leftarrow 0$ to $N-1$_ do

${\bm{a}}_{n}^{x}\sim{\mathcal{N}}(\mu_{i},\Sigma_{i})$;

for  _$k\leftarrow 1$ to $K$_ do

${\bm{s}}^{z_{k}},{\bm{a}}_{n}^{z_{k}}={\bm{f}}_{x}^{z_{k}}({\bm{s}}^{x},{\bm{a}}_{n}^{x})$;

$e_{n}^{z_{k}}=E^{z_{k}}({\bm{s}}^{z_{k}},{\bm{a}}_{n}^{z_{k}})$;

$e_{n}^{x}=\sum_{k=1}^{K}\beta^{z_{k}}e_{n}^{z_{k}}$

$\mu_{i+1}\leftarrow\textrm{Update}_{\mu}(\mu_{i},{\bm{a}}_{0:N}^{x},e_{0:N}^{x})$;

$\Sigma_{i+1}\leftarrow\textrm{Update}_{\Sigma}(\Sigma_{i},{\bm{a}}_{0:N}^{x},e_{0:N}^{x})$;

${\bm{a}}_{i}^{*},e_{i}^{*}\leftarrow\arg_{\bm{a}}\max_{e}(e_{0:N}^{x})$;

if  _$e^{*} <e_{i}^{*}$_ then

${\bm{a}}^{*}\leftarrow{\bm{a}}_{i}^{*}$;

$e^{*}\leftarrow e_{i}^{*}$;

return ${\bm{a}}^{*}$;

Algorithm 1 Composable Energy Policies

##  IV CEP for Robot Motion Generation

In this section, we will provide further insights on how to model the mapping
functions and the stochastic policies for a robot motion generation problem.
We consider the motion is generated with a second-order dynamic system in the
robot’s configuration space

$\displaystyle\ddot{{\bm{q}}}=g({\bm{q}},\dot{{\bm{q}}})$ (13)

where ${\bm{q}}$ is the robot’s joint state, $\dot{{\bm{q}}}=d{\bm{q}}/dt$
velocity and $\ddot{{\bm{q}}}=d^{2}{\bm{q}}/dt^{2}$, the acceleration.

Our dynamic system is modeled by the maximization in (11). Thus, we represent
our action ${\bm{a}}^{x}=\ddot{{\bm{q}}}$ and the state
${\bm{s}}^{x}=\\{{\bm{q}},\dot{{\bm{q}}}\\}$.

The energy distributions are defined in a set of task spaces. We model the map
between the common space (configuration space) and the latent space (task
space) ${\bm{f}}_{q}^{x}$ by the robot’s kinematics

$\displaystyle{\bm{x}}$ $\displaystyle=\textbf{f}_{\textrm{kin}}({\bm{q}})$
$\displaystyle\dot{{\bm{x}}}$ $\displaystyle={\bm{J}}({\bm{q}})\dot{{\bm{q}}}$
(14) $\displaystyle\ddot{{\bm{x}}}$
$\displaystyle={\bm{J}}({\bm{q}})\ddot{{\bm{q}}}+\dot{{\bm{J}}}({\bm{q}})\dot{{\bm{q}}}\approx{\bm{J}}({\bm{q}})\ddot{{\bm{q}}}$

with forward kinematics $\textbf{f}_{\textrm{kin}}$ to a given task space and
${\bm{J}}({\bm{q}})=\partial{\bm{x}}/\partial{\bm{q}}$, the Jacobian for the
given forward kinematics.

For the particular case in which the mappings are given by (14), we represent
the general policy in (10) as

$\displaystyle\pi^{q}(\ddot{{\bm{q}}}|{\bm{q}},\dot{{\bm{q}}})\propto\exp\left(\sum_{k=1}^{K}\beta_{k}E^{x_{k}}({\bm{J}}_{k}({\bm{q}})\ddot{{\bm{q}}},\textbf{f}_{\textrm{kin}k}({\bm{q}}),{\bm{J}}_{k}({\bm{q}})\dot{{\bm{q}}})\right).$
(15)

We remark, that computing the optimal solution for (15), given we use an
implicit representation, we only require access to the forward kinematics
$\textbf{f}_{\textrm{kin}}$ and the Jacobian ${\bm{J}}$ functions. Instead,
explicit methods [32, 20], require to compute the Jacobian pseudoinverse to
get the action in the configuration space as shown before in (2) and (8).
Jacobian pseudoinverse might be problematic to compute if the robot is in
singularities.

Energy functions allow having a flexible policy distribution representation.
In the following, we provide some energy proposals for the main components the
robot requires for reactive motion generation. Additionally, we show in
Appendix C, that the weighted sum of deterministic policies (Artificial
Potential Fields methods), can be framed as a particular case of CEP.

#### Go To a target

The simplest distribution model for going to a target position is a normal
distribution

$\displaystyle\pi(\ddot{{\bm{x}}}|{\bm{x}},\dot{{\bm{x}}})={\mathcal{N}}({\bm{\mu}}({\bm{x}},\dot{{\bm{x}}}),{\bm{\Sigma}}({\bm{x}}))$
(16)

with

$\displaystyle{\bm{\mu}}({\bm{x}},\dot{{\bm{x}}})=-{\bm{K}}_{p}({\bm{x}}-{\bm{x}}^{*})-{\bm{K}}_{v}\dot{{\bm{x}}}$
(17)
$\displaystyle{\bm{\Sigma}}({\bm{x}})=\alpha({\bm{x}}-{\bm{x}}^{*})^{\intercal}({\bm{x}}-{\bm{x}}^{*}).$
(18)

The distribution proposes as mean ${\bm{\mu}}$, a PD-controller with
${\bm{x}}^{*}$ the target state and ${\bm{K}}_{p}>0$ and ${\bm{K}}_{v}>0$
gains and a covariance matrix ${\bm{\Sigma}}$ represented as the quadratic
distance to the target, scaled by $\alpha>0$. The variance will shrink closer
to the target and, thus, the relevance of this energy component will increase
the closer we are to the desired state.

#### Obstacle Avoidance

We represent obstacle avoidance energy in the unidimensional space represented
by the vector between the cartesian robot position ${\bm{x}}_{r}$ in task
space and the cartesian obstacle position ${\bm{x}}_{o}$.

We first compute the vector pointing to the obstacle

$\displaystyle{\bm{v}}_{r,o}={\bm{x}}_{r}-{\bm{x}}_{o}$ (19)

Then, we project the velocity $\dot{{\bm{x}}}$ and the acceleration
$\ddot{{\bm{x}}}$ vectors in the task space

$\displaystyle\dot{x}_{p}$
$\displaystyle=\dot{{\bm{x}}}\frac{\dot{{\bm{x}}}\cdot{\bm{v}}_{r,o}}{||\dot{{\bm{x}}}||||{\bm{v}}_{r,o}||}$
$\displaystyle\ddot{x}_{p}$
$\displaystyle=\ddot{{\bm{x}}}\frac{\ddot{{\bm{x}}}\cdot{\bm{v}}_{r,o}}{||\ddot{{\bm{x}}}||||{\bm{v}}_{r,o}||}$
(20)

Finally, we compute the energy for the acceleration $\ddot{x}_{p}$,
$\dot{x}_{p}$ and ${\bm{v}}_{r,o}$ as

$\displaystyle
E(\ddot{x}_{p},\dot{x}_{p},{\bm{v}}_{r,o})=\begin{cases}0&\text{{if}
$\dot{x}_{p}<0$ {or} $||{\bm{v}}_{r,o}||>\gamma$ }\\\ -\infty&\text{{if}
$\ddot{x}_{p}>-\alpha\dot{x}_{p}-\beta$}\\\ 0&\textrm{otherwise}\end{cases}$
(21)

with a distance threshold parameter $\gamma>0$ to activate the energy
component. If the projected robot velocity is not pointing to the obstacle or
if the robot’s task space position is too far from the robot, the distribution
is a uniform distribution over the whole acceleration space. In case the robot
is close to the obstacle and the velocity vector points to the obstacle, the
distribution then becomes a uniform distribution between $-\infty$ and
$-\alpha\dot{x}_{p}-\beta$,
${\mathcal{U}}_{p}(-\infty,-\alpha\dot{x}_{p}-\beta)$ with $\alpha>0$ and
$\beta>0$.

#### Avoid Joint Limits

Similarly to the collision avoidance energy, we model the joint limits
avoidance with a uniform distribution that provides high probability to those
accelerations pointing in the opposite direction to the joint limit as long as
the joint is close to the limits. Given the distance to the joint limit

$\displaystyle d_{l}=q-q_{l}$ (22)

with $q_{l}$ the joint limit, the energy can be represented as

$\displaystyle E(\ddot{q};\dot{q},q)=\begin{cases}0&\text{{if}
$\frac{d_{l}}{|d_{l}|}\dot{q}<0$ {or} $|d_{l}|>\gamma$ }\\\ -\infty&\text{{if}
$\frac{d_{l}}{|d_{l}|}\ddot{q}>-\alpha\frac{d_{l}}{|d_{l}|}\dot{q}-\beta$}\\\
0&\text{otherwise}\end{cases}$ (23)

with $\alpha>0$ and $\beta>0$.

#### Imitation Learning, Control-as-Inference and RL

CEP architecture is not only limited to heuristic energies. The architecture
allows to integrate any form distributions. CEP architecture allows to
integrate multimodal policies learned by behavioural cloning [45, 29],
posterior distributions computed by control as inference [23, 48, 49] or a RL
policy.

In the following, we propose a novel hierarchical reinforcement learning
policy to integrate an RL agent with a set of prior distributions.

##  V CEP for Hierarchical Reinforcement Learning

Priors have been widely used in RL, accelerating the learning process and
therefore improving the sample efficiency. Most popular approaches can be
categorized in two groups. On the one hand, methods that first approximate a
parameterized policy to mimic the priors

$\displaystyle\min_{\theta}D_{\mathrm{KL}}(\pi_{\theta}({\bm{a}}|{\bm{s}})||\pi_{q}({\bm{a}}|{\bm{s}}))$

and then, update this policy to maximize for a new task [28, 45].

$\displaystyle\max_{\theta}\mathbb{E}_{p_{\pi_{\theta}}({\bm{s}},{\bm{a}})}[{\mathcal{R}}({\bm{s}},{\bm{a}})].$

Even if this approach is the most popular one, if the parameterized policy is
not sufficiently expressive is not going to cover all the prior information.
Moreover, the parameterized model might forget important information coming
from the prior after some RL updates. On the other hand, Hierarchical
Reinforcement Learning (HRL) methods [15, 39, 40, 5, 30] consider a two-
layered policy.

$\displaystyle\pi^{L}({\bm{a}}|{\bm{s}})=\int_{{\bm{a}}_{H}}\pi_{q}({\bm{a}}|{\bm{s}},{\bm{a}}_{H})\pi_{\theta}^{H}({\bm{a}}_{H}|{\bm{s}})d{\bm{a}}_{H}$
(24)

Given some priors, a policy $\pi_{q}$ is modeled conditioned on a higher-order
action ${\bm{a}}_{H}$. For sampling an action, we first sample an action from
the high-level policy,
${\bm{a}}_{H}\sim\pi_{\theta}^{H}({\bm{a}}_{H}|{\bm{s}})$ and then, we sample
from the conditioned low-level policy,
${\bm{a}}\sim\pi_{q}({\bm{a}}|{\bm{s}},{\bm{a}}_{H})$. Most of the previous
HRL policies assume rather a mixture of skills [30, 12, 28, 8] or a simple sum
of the prior actions [15, 39] as HRL policies. These policy architectures
might not satisfy the desired properties in a real robot RL problem, rather
because they are not safe enough or because they are so limited in their
exploration space.

In a robot learning scenario, a desired HRL policy should consider (1) priors
that guarantee hard constraints to avoid collisions, (2) multimodal/non-normal
priors to provide a rich set of possible exploring regions, (3) allow to
integrate multiple prior sources in the same policy and (4) do not limit the
exploration region to a few set of given skills. In the following, we show
that CEP allows integrating all the desired properties in the HRL policy.

In our work, we propose to model the low level policy with a CEP

$\displaystyle{\bm{\mu}}_{H},{\bm{\Sigma}}_{H}$
$\displaystyle\sim\pi_{\theta}^{H}({\bm{\mu}}_{H},{\bm{\Sigma}}_{H}|{\bm{s}})$
(25)
$\displaystyle\pi^{L}({\bm{a}}|{\bm{s}},{\bm{\mu}}_{H},{\bm{\Sigma}}_{H})$
$\displaystyle=\pi_{q}({\bm{a}}|{\bm{s}}){\mathcal{N}}({\bm{a}}|{\bm{\mu}}_{H},{\bm{\Sigma}}_{H})$

In our model, the action is computed by the maximization over the low-level
policy

$\displaystyle{\bm{a}}=\operatorname*{arg\,max}_{{\bm{a}}}\pi^{L}({\bm{a}}|{\bm{s}},{\bm{\mu}}_{H},{\bm{\Sigma}}_{H}).$
(26)

Our proposed policy combines the given prior policy distribution with a normal
distribution parameterized with the high-level policy. This structure allows
integrating any type of prior distribution with a learnable policy. The prior
could impose hard constraints to avoid actions that lead to collisions by
setting $\pi_{q}({\bm{a}}|{\bm{s}})=0$ or encourage the robot to explore in a
mixture of regions by setting a mixture of distributions as prior.

The variance ${\bm{\Sigma}}_{H}$ control controls the importance of the
learning policy w.r.t. the priors. The bigger ${\bm{\Sigma}}_{H}$ is the less
relevant the learning policy is and thus, the higher the influence of the
priors in the action; in the limit $\lim_{{\bm{\Sigma}}_{H}\rightarrow
0}\pi^{L}({\bm{a}}|{\bm{s}},{\bm{\mu}}_{H},{\bm{\Sigma}}_{H})=\delta({\bm{\mu}}_{H}-{\bm{a}})$,
the prior effect disappear and we only trust in the learning policy.

It is interesting to remark that our model applies RL in the high-level policy
and solves a stochastic search optimization problem in the low-level policy.
With the low-level stochastic search optimization, we can guarantee that some
conditions (obstacle avoidance) will be satisfied in the robot action, while,
a simple residual control could not.

##  VI Experimental Evaluation

The model evaluation is split into two parts. First, we study CEP performance
for reactive motion generation. We evaluate how the different energy
components perform by observing the success rate and the collisions in a set
of obstacle avoidance environments. We compare CEP w.r.t. previous modular
reactive controllers. We exclude trajectory optimization and path planning
algorithms as our evaluation is interested in reactive controllers that can
provide a control response in high control frequencies ($\geq$500Hz).

Second, we evaluate CEP in an RL problem. We want to observe if using CEP as
prior boosts the learning performance of an RL agent. Additionally, we want to
evaluate if the proposed priors are sufficiently strong to impose safety
priors in the policy and avoid collisions. We compare our approach to previous
methods applying both Behavioural Cloning + RL and Hierarchical RL.

Figure 4: Reactive Motion Generation Environments. In the top, from left to
right: 1 Obstacle, 3 Obstacles, Cross. In the bottom, from left to right: Cage
I, Cage II, Double Cross. Green Sphere: target pose, red spheres: obstacles.

### Reactive Motion Generation

We consider the problem of controlling a robot manipulator in a set of
obstacle avoidance environments. We use a 7 dof KUKA-LWR robot manipulator and
the objective is to reach a target pose (3D and 6D) while avoiding colliding
with the obstacles. We consider increasingly difficult environments presented
in Fig. 4. The robot is initialized in a random joint configuration and its
motion is generated by a set of modular reactive motion generators, without
additional global path planning algorithms. We evaluate the success rate and
the collision rate of our method and compare it w.r.t. deterministic modular
reactive motion controllers (Artificial Potential Fields [20] and Riemannian
Motion Policies [32]) as baselines.

We build the CEP model with

  * •

Go-To energy in the end effector’s task space.

  * •

Obstacle Avoidance energy in each robot link’s task space.

  * •

Joint limits avoidance energy in the configuration space.

To guarantee high frequency control actions ($\geq 500Hz$), we solve the
optimization problem (11) in a Nvidia GPU RTX-2800. Each energy component is
parallelized and the action samples are evaluated in batch. With a sufficient
amount of samples per optimization step (10.000 samples), the optimization is
solved in two steps.

Methods | 1 Obstacle | 3 Obstacles | Cross | Double Cross | Cage I | Cage II
---|---|---|---|---|---|---
| Success | Collide | Success | Collide | Success | Collide | Success | Collide | Success | Collide | Success | Collide
Riemannian Motion Policies [32] | 100/100 | 0/100 | 98/100 | 0/100 | 94/100 | 0/100 | 84/100 | 0/100 | 55/100 | 0/100 | 5/100 | 0/100
Artificial Potential Fields [20] | 100/100 | 0/100 | 100/100 | 0/100 | 90/100 | 0/100 | 76/100 | 0/100 | 43/100 | 0/100 | 4/100 | 2/100
Composable Energy Policies(Ours) | 100/100 | 0/100 | 100/100 | 0/100 | 98/100 | 0/100 | 91/100 | 0/100 | 89/100 | 0/100 | 46/100 | 0/100

TABLE I: Results for 3D GoTo + Obstacle Avoidance Task

Methods | 1 Obstacle | 3 Obstacles | Cross | Double Cross | Cage I | Cage II
---|---|---|---|---|---|---
| Success | Collide | Success | Collide | Success | Collide | Success | Collide | Success | Collide | Success | Collide
Riemannian Motion Policies [32] | 100/100 | 0/100 | 91/100 | 0/100 | 75/100 | 0/100 | 63/100 | 0/100 | 18/100 | 0/100 | 1/100 | 0/100
Artificial Potential Fields [20] | 100/100 | 0/100 | 90/100 | 0/100 | 78/100 | 0/100 | 60/100 | 0/100 | 21/100 | 0/100 | 1/100 | 0/100
Composable Energy Policies(Ours) | 100/100 | 0/100 | 100/100 | 0/100 | 91/100 | 0/100 | 87/100 | 0/100 | 43/100 | 0/100 | 14/100 | 0/100

TABLE II: Results for 6D GoTo + Obstacle Avoidance Task

#### Results and analysis

We summarize the obtained results for the 3D Goto problem in table I. Few
obstacles environments are easily solved by all the methods. We observe a
success rate of almost 100% for the first three environments in the three
cases. This result shows that simple scenarios can be easily solved with local
reactive controllers and it is not required to solve a global trajectory
planning problem. In complex scenarios, CEP performs better than the
baselines. We can obtain an 89% success rate in the first cage environment and
46% in the second cage. We think that these results might be related to the
way we compute the robot’s acceleration. While the baselines methods compute
the accelerations provided by the different components(Go-To, Avoid Joint
Limits, Obstacle Avoidance), and do not take into account any possible
conflict of interests between each action, our method first computes the
distribution obtained by the product of all the components and then, computes
the action that maximizes this composed distribution. The built composition
will provide high probabilities to those actions that have high probability in
each component and low to the rest. Thus, the optimal action from the composed
distribution is expected to be the one that better satisfies all the
individual components.

The performance worsens for the 6D GoTo problem (Table II). The orientation
sets an important constraint in the possible final configurations and reduces
the set of trajectories that solves the problem. CEP was able to perform
relatively better than the baselines but it got less than 50% success rate in
both cage environments, suggesting that in complex scenarios an additional
global path planner should be integrated with CEP.

Figure 5: Puck hitting environment. CEP is tested in a RL environment where
the robot needs to learn how to hit the puck. The reward is defined by the
euclidean distance between the puck and the green target. An additional reward
penalizes if the robot hits the table.

### Prior Based Policies for RL

In the following experiment, we evaluate the performance of CEP as a policy
that combines prior policies with a new learnable policy to solve a new task.
We consider the problem of hitting a puck and place it in a target position
Fig. 5. We use a 7 dof LBR-IIWA robot, the action space is defined by the end-
effector’s task space cartesian velocity ${\bm{a}}\in\mathbb{R}^{3}$ and the
state ${\bm{s}}\in\mathbb{R}^{18}$ is represented by the end-effector’s
cartesian position ${\bm{x}}_{\textrm{ee}}$, puck’s position
${\bm{x}}_{\textrm{puck}}$, their relative position
$r_{\textrm{p-ee}}={\bm{x}}_{\textrm{ee}}-{\bm{x}}_{\textrm{puck}}$, the
puck’s velocity ${\bm{v}}_{\textrm{puck}}$, the end effector’s velocity
${\bm{v}}_{\textrm{ee}}$ and the target position ${\bm{x}}_{\textrm{target}}$.
One episode has 1400 steps. We define the reward as

$\displaystyle
r=-||{\bm{x}}_{\textrm{puck}}-{\bm{x}}_{\textrm{target}}||^{2}+r_{T}({\bm{x}}_{\textrm{ee}})$
(27)

with $r_{T}({\bm{x}}_{\textrm{ee}})=-1000$ if the robot’s end effector
position ${\bm{x}}_{\textrm{ee}}$ collides the table. The first term
$-||{\bm{x}}_{\textrm{puck}}-{\bm{x}}_{\textrm{target}}||^{2}$ will encourage
the robot to put the puck close to the target, minimizing the euclidean
distance. The second term will encourage the robot to avoid to collide against
the table. We compare CEP approach presented in (25) with other approaches
that combines prior information with RL. We consider Behavioural Cloning + RL
[28] and Residual Policy Learning [15, 39] as baselines.

To guarantee a fair comparison, we apply the same prior policy in the three
scenarios. We build a prior policy with two components in a CEP. We consider

  * •

Go-To energy in the robot’s end effector’s task space.

  * •

Table avoidance in the robot’s end effector’s task space.

The prior is combined in different forms depending on the applied algorithm:

Behavioural Cloning + RL. We first fit a policy to the prior policy

$\displaystyle{\bm{\theta}}^{*}=\operatorname*{arg\,min}_{{\bm{\theta}}}\mathbb{E}_{p({\bm{s}})}\left[D_{\mathrm{KL}}(\pi_{q}({\bm{a}}|{\bm{s}})||\pi_{\theta}({\bm{a}}|{\bm{s}}))\right]$
(28)

and then directly sample from the fit policy

$\displaystyle{\bm{a}}\sim\pi_{\theta}({\bm{a}}|{\bm{s}}).$ (29)

Residual Policy Learning. We consider a hierarchical structure. We first
sample, from the learnable policy and add the optimal solution from the prior

$\displaystyle{\bm{a}}={\bm{a}}^{*}+{\bm{a}}_{H}\,,\,{\bm{a}}_{H}\sim\pi_{\theta}({\bm{a}}_{H}|{\bm{s}})$
(30)

with

$\displaystyle{\bm{a}}^{*}=\operatorname*{arg\,max}_{{\bm{a}}}\pi_{{\bm{q}}}({\bm{a}}|{\bm{s}}).$
(31)

Composable Energy Policies. We also consider a hierarchical structure. In this
case, we first sample from the learnable policy

$\displaystyle{\bm{a}}_{H}\sim\pi_{\bm{\theta}}({\bm{a}}_{H}|{\bm{s}})$ (32)

and then, optimize over the composition

$\displaystyle{\bm{a}}=\operatorname*{arg\,max}_{{\bm{a}}}\pi_{q}({\bm{a}}|{\bm{s}},{\bm{a}}_{H}).$
(33)

We apply the hierarchical policy introduced in (25). For all the experiments,
we used the Mushroom-RL [6] PPO [37] implementation. We include additional
details in Appendix D.

#### Results and analysis

We show the obtained results in Fig. 6. Our method, CEP-PPO performs on
average better than Residual Learning and Behavioural Cloning + RL methods.
The obtained results show that while CEP can consistently improve its
performance, residual policy learning has a slower learning curve and
behavioral cloning + RL decays in performance. We remark that the three
methods start performing worse than the prior due to the stochasticity of the
policies in contrast with the deterministic prior. The obtained results might
be related to the collisions per episode. CEP based policy allows imposing
hard constraints to avoid collisions. Both, behavioral cloning + RL and
residual learning adds white noise around the optimal prior action and then,
increase the chances of table collision. Fewer collisions mean that
$r_{T}({\bm{x}}_{ee})$ is less activated during the training and then, the
learning can focus on moving the puck close to the target. In the Behavioural
Cloning case, we see that the table collisions push the robot to avoid the
table, and then, it forgets how to reach the target. The Behavioural Cloning +
RL policy ends with a policy that makes the robot neither collide with the
table and neither to hit the puck. In the residual learning case, the policy
is updated by taking both hitting the puck and avoid the table into
consideration, while in CEP, the collisions against the table are less, and
thus, the training is more focused on improving the hitting tactic.

Figure 6: Training curves for Hitting a puck environment. CEP-PPO performs
consistenly better than other prior + RL methods. Collision avoidance prior is
stronger in CEP-PPO and provides a safer training.

####  CEP ablation study

When applying CEP-PPO, the ${\bm{\Sigma}}_{H}$ parameter from (25) controls
how big is the influence of the learnable policy in the overall policy. The
smaller ${\bm{\Sigma}}_{H}$ is; the smaller the entropy for the learnable part
and the higher the influence in the overall policy. In order to study the
influence of the ${\bm{\Sigma}}_{H}$ parameter, we compare CEP-PPO with fixed
${\bm{\Sigma}}_{H}$ and CEP-PPO with learnable ${\bm{\Sigma}}_{H}$. We present
the obtained results in Fig. 7

Figure 7: Training curves for Hitting a puck environment. Learning the
variance of the policy improves the learning curve, but also increases the
probability of collisions.

As expected having the freedom to set the ${\bm{\Sigma}}_{H}$ parameter allows
the robot to learn a better behavior to hit the puck. If the
${\bm{\Sigma}}_{H}$ parameter is fix and too big, then the learned policy has
a low impact on the overall policy and it can not improve a lot. With a very
small ${\bm{\Sigma}}_{H}$, the learned policy will have a very high impact in
the overall policy and then, it might not get benefited by the priors. When
learning ${\bm{\Sigma}}_{H}$ parameter in the high-level policy, the robot
decides when to trust more in the priors and when less. This allows the robot
to get help from priors only if required, but also to decouple from them to
learn a better policy. However, shown in the collisions plot from Fig. 7,
reducing the effect of the priors might increase the probability to go into
dangerous regions and collide more often against the table. Nevertheless,
learning the ${\bm{\Sigma}}_{H}$ parameter seems to provide better results
than keeping it fixed.

##  VII Conclusions

We have presented Composable Energy Policies, a novel approach for modular
robot control. In contrast with the previous method for reactive motion
generation, which computes the optimal action for each component
independently; our method solves an optimization problem over the sum of
components. We have shown that APF policies can be rewritten as a CEP and
provide reasoning of why their method might fail. Through an evaluation phase,
we have also shown that our method can solve a set of reactive motion
generation problems more efficiently than artificial potential field methods.

Our method is flexible and allows us to represent each component in an
arbitrary set of state-action spaces. Additionally, CEP allows integrating
policies from multiple sources and samples from the product of them. In our
work, we show how to integrate a RL agent with a set of priors. The resulting
algorithm combines a high-level RL policy with a low-level optimization
problem. This low-level optimization allows to guarantee that the policy is
safe (if an obstacle avoidance prior is added) or to explore in informative
regions (if guiding priors are added). We have compared our method with two
methods that also combine prior information with an RL agent. Given the low-
level optimization of our method, we can guarantee that some priors such as
obstacle avoidance were satisfied and the learning is more stable.

Additionally, we have introduced in the Appendix, the relations between
optimal control and reactive motion generation. Setting their connections
allows us to compute how the optimal policy for a multi-objective optimal
control problem diverges from the product of the optimal policies for each
objective. These insights allow us to better understand the policy composition
and improve the design of our components.

## References

  * Ben-Israel [1999] Adi Ben-Israel.  The change-of-variables formula using matrix volume.  _SIAM Journal on Matrix Analysis and Applications_ , 21(1):300–312, 1999.
  * Ben-Israel [2000] Adi Ben-Israel.  An application of the matrix volume in probability.  _Linear Algebra and its Applications_ , 321(1-3):9–25, 2000.
  * Cheng et al. [2018] Ching-An Cheng, Mustafa Mukadam, Jan Issac, Stan Birchfield, Dieter Fox, Byron Boots, and Nathan Ratliff.  Rmpflow: A computational graph for automatic motion policy generation.  In _International Workshop on the Algorithmic Foundations of Robotics_ , 2018.
  * Da Silva et al. [2009] Marco Da Silva, Frédo Durand, and Jovan Popović.  Linear bellman combination for control of character animation.  _ACM SIGGRAPH_ , 2009.
  * Daniel et al. [2012] Christian Daniel, Gerhard Neumann, and Jan Peters.  Hierarchical relative entropy policy search.  In _Artificial Intelligence and Statistics_ , pages 273–281. PMLR, 2012.
  * D’Eramo et al. [2020] Carlo D’Eramo, Davide Tateo, Andrea Bonarini, Marcello Restelli, and Jan Peters.  Mushroomrl: Simplifying reinforcement learning research.  https://github.com/MushroomRL/mushroom-rl, 2020.
  * Du et al. [2020] Yilun Du, Shuang Li, and Igor Mordatch.  Compositional visual generation and inference with energy based models.  In _Conference on Neural Information Processing Systems_ , 2020.
  * Florensa et al. [2017] Carlos Florensa, Yan Duan, and Pieter Abbeel.  Stochastic neural networks for hierarchical reinforcement learning.  _In International Conference on Learning Representations_ , 2017.
  * Fox et al. [1997] Dieter Fox, Wolfram Burgard, and Sebastian Thrun.  The dynamic window approach to collision avoidance.  _IEEE Robotics & Automation Magazine_, 4(1):23–33, 1997.
  * Ge and Cui [2002] Shuzhi Sam Ge and Yun J Cui.  Dynamic motion planning for mobile robots using potential field method.  _Autonomous robots_ , 13(3):207–222, 2002.
  * Haarnoja et al. [2018] Tuomas Haarnoja, Vitchyr Pong, Aurick Zhou, Murtaza Dalal, Pieter Abbeel, and Sergey Levine.  Composable deep reinforcement learning for robotic manipulation.  In _IEEE International Conference on Robotics and Automation_ , pages 6244–6251. IEEE, 2018.
  * Heess et al. [2016] Nicolas Heess, Greg Wayne, Yuval Tassa, Timothy Lillicrap, Martin Riedmiller, and David Silver.  Learning and transfer of modulated locomotor controllers.  _arXiv preprint arXiv:1610.05182_ , 2016.
  * Hinton [2002] Geoffrey E Hinton.  Training products of experts by minimizing contrastive divergence.  _Neural computation_ , 14(8):1771–1800, 2002\.
  * Howard et al. [2002] Andrew Howard, Maja J Matarić, and Gaurav S Sukhatme.  Mobile sensor network deployment using potential fields: A distributed, scalable solution to the area coverage problem.  In _Distributed Autonomous Robotic Systems 5_ , pages 299–308. Springer, 2002.
  * Johannink et al. [2019] Tobias Johannink, Shikhar Bahl, Ashvin Nair, Jianlan Luo, Avinash Kumar, Matthias Loskyll, Juan Aparicio Ojea, Eugen Solowjow, and Sergey Levine.  Residual reinforcement learning for robot control.  In _International Conference on Robotics and Automation_ , pages 6023–6029. IEEE, 2019.
  * Kaelbling and Lozano-Pérez [2011] Leslie Pack Kaelbling and Tomás Lozano-Pérez.  Hierarchical task and motion planning in the now.  In _IEEE International Conference on Robotics and Automation_ , pages 1470–1477. IEEE, 2011.
  * Kaelbling and Lozano-Pérez [2013] Leslie Pack Kaelbling and Tomás Lozano-Pérez.  Integrated task and motion planning in belief space.  _The International Journal of Robotics Research_ , 32(9-10):1194–1227, 2013.
  * Kalakrishnan et al. [2011] Mrinal Kalakrishnan, Sachin Chitta, Evangelos Theodorou, Peter Pastor, and Stefan Schaal.  Stomp: Stochastic trajectory optimization for motion planning.  In _IEEE international conference on robotics and automation_ , pages 4569–4574. IEEE, 2011.
  * Kavraki et al. [1996] Lydia E Kavraki, Petr Svestka, J-C Latombe, and Mark H Overmars.  Probabilistic roadmaps for path planning in high-dimensional configuration spaces.  _IEEE transactions on Robotics and Automation_ , 12(4):566–580, 1996.
  * Khatib [1985] O. Khatib.  Real-time obstacle avoidance for manipulators and mobile robots.  In _IEEE International Conference on Robotics and Automation_ , volume 2, pages 500–505, 1985.  doi: 10.1109/ROBOT.1985.1087247.
  * Khatib [1987] Oussama Khatib.  A unified approach for motion and force control of robot manipulators: The operational space formulation.  _IEEE Journal on Robotics and Automation_ , 1987.
  * Koren and Borenstein [1991] Yoram Koren and Johann Borenstein.  Potential field methods and their inherent limitations for mobile robot navigation.  In _IEEE International Conference on Robotics and Automation_ , 1991\.
  * Lambert et al. [2020] Alexander Lambert, Adam Fishman, Dieter Fox, Byron Boots, and Fabio Ramos.  Stein variational model predictive control.  _Conference on Robot Learning_ , 2020.
  * LaValle [2006] Steven M LaValle.  _Planning Algorithms_.  Cambridge University Press, Cambridge, U.K., 2006.
  * LaValle et al. [2001] Steven M LaValle, James J Kuffner, BR Donald, et al.  Rapidly-exploring random trees: Progress and prospects.  _Algorithmic and computational robotics: new directions_ , 5:293–308, 2001.
  * LeCun et al. [2006] Yann LeCun, Sumit Chopra, Raia Hadsell, M Ranzato, and F Huang.  A tutorial on energy-based learning.  _Predicting structured data_ , 1(0), 2006.
  * Levine [2018] Sergey Levine.  Reinforcement learning and control as probabilistic inference: Tutorial and review.  _CoRR_ , abs/1805.00909, 2018.
  * Mülling et al. [2013] Katharina Mülling, Jens Kober, Oliver Kroemer, and Jan Peters.  Learning to select and generalize striking movements in robot table tennis.  _The International Journal of Robotics Research_ , 32(3):263–279, 2013.
  * Paraschos et al. [2013] Alexandros Paraschos, Christian Daniel, Jan R Peters, and Gerhard Neumann.  Probabilistic movement primitives.  In _Advances in Neural Information Processing Systems_ , pages 2616–2624, 2013.
  * Peng et al. [2019] Xue Bin Peng, Michael Chang, Grace Zhang, Pieter Abbeel, and Sergey Levine.  Mcp: Learning composable hierarchical control with multiplicative compositional policies.  _Advances in Neural Information Processing Systems 32_ , 2019.
  * Ratliff et al. [2009] Nathan Ratliff, Matt Zucker, J Andrew Bagnell, and Siddhartha Srinivasa.  Chomp: Gradient optimization techniques for efficient motion planning.  In _IEEE International Conference on Robotics and Automation_ , pages 489–494, 2009.
  * Ratliff et al. [2018] Nathan D Ratliff, Jan Issac, Daniel Kappler, Stan Birchfield, and Dieter Fox.  Riemannian motion policies.  _arXiv preprint arXiv:1801.02854_ , 2018.
  * Rawlik et al. [2012] Konrad Rawlik, Marc Toussaint, and Sethu Vijayakumar.  On stochastic optimal control and reinforcement learning by approximate inference.  _Proceedings of Robotics: Science and Systems VIII_ , 2012.
  * Rimon and Koditschek [1992] Elon Rimon and Daniel E Koditschek.  Exact robot navigation using artificial potential functions.  _Departmental Papers (ESE)_ , page 323, 1992.
  * Rubinstein [1999] Reuven Rubinstein.  The cross-entropy method for combinatorial and continuous optimization.  _Methodology And Computing In Applied Probability_ , 1999.
  * Schulman et al. [2014] John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Jia Pan, Sachin Patil, Ken Goldberg, and Pieter Abbeel.  Motion planning with sequential convex optimization and convex collision checking.  _The International Journal of Robotics Research_ , 33(9):1251–1270, 2014.
  * Schulman et al. [2017] John Schulman, Filip Wolski, Prafulla Dhariwal, Alec Radford, and Oleg Klimov.  Proximal policy optimization algorithms.  _arXiv preprint arXiv:1707.06347_ , 2017.
  * Silver et al. [2017] David Silver, Julian Schrittwieser, Karen Simonyan, Ioannis Antonoglou, Aja Huang, Arthur Guez, Thomas Hubert, Lucas Baker, Matthew Lai, Adrian Bolton, et al.  Mastering the game of go without human knowledge.  _nature_ , 550(7676):354–359, 2017.
  * Silver et al. [2018] Tom Silver, Kelsey Allen, Josh Tenenbaum, and Leslie Kaelbling.  Residual policy learning.  _arXiv preprint arXiv:1812.06298_ , 2018.
  * Singh et al. [2021] Avi Singh, Huihan Liu, Gaoyue Zhou, Albert Yu, Nicholas Rhinehart, and Sergey Levine.  Parrot: Data-driven behavioral priors for reinforcement learning.  _International Conference on Learning Representations_ , 2021.
  * Sutton et al. [1999] Richard S Sutton, Doina Precup, and Satinder Singh.  Between mdps and semi-mdps: A framework for temporal abstraction in reinforcement learning.  _Artificial intelligence_ , 112(1-2):181–211, 1999.
  * Tasse et al. [2020] Geraud Nangue Tasse, Steven James, and Benjamin Rosman.  A boolean task algebra for reinforcement learning.  _Conference on Neural Information Processing Systems_ , 2020.
  * Todorov [2009] Emanuel Todorov.  Compositionality of optimal control laws.  _Advances in neural information processing systems_ , 22:1856–1864, 2009.
  * Toussaint [2009] Marc Toussaint.  Robot trajectory optimization using approximate inference.  In _international conference on machine learning_ , pages 1049–1056, 2009.
  * Urain et al. [2020] Julen Urain, Michele Ginesi, Davide Tateo, and Jan Peters.  Imitationflows: Learning deep stable stochastic dynamic systems by normalizing flows.  In _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2020.
  * Van Den Berg et al. [2011] Jur Van Den Berg, Stephen J Guy, Ming Lin, and Dinesh Manocha.  Reciprocal n-body collision avoidance.  In _Robotics research_ , pages 3–19. Springer, 2011.
  * Van Niekerk et al. [2019] Benjamin Van Niekerk, Steven James, Adam Earle, and Benjamin Rosman.  Composing value functions in reinforcement learning.  In _International Conference on Machine Learning_ , pages 6401–6409, 2019.
  * Watson et al. [2020] Joe Watson, Hany Abdulsamad, and Jan Peters.  Stochastic optimal control as approximate input inference.  In _Conference on Robot Learning_ , pages 697–716. PMLR, 2020.
  * Williams et al. [2017] Grady Williams, Andrew Aldrich, and Evangelos A Theodorou.  Model predictive path integral control: From theory to parallel computation.  _Journal of Guidance, Control, and Dynamics_ , 40(2):344–357, 2017.
  * Ziebart et al. [2010] Brian D Ziebart, J Andrew Bagnell, and Anind K Dey.  Modeling interaction via the principle of maximum causal entropy.  In _International Conference on Machine Learning_ , 2010.

##  Appendix A A Control as Inference view for Reactive motion generation

In the following section, we want to highlight the connections between
reactive motion generation and control as inference to evaluate the optimality
guarantees of composing energies in a multi-objective optimal control problem.

Figure 8: Graphical model for the Optimal Control problem. ${\bm{s}}_{t}$
denoted the state, ${\bm{a}}_{t}$ denotes the action and $o_{t}$ is an
additional variable representing the optimality of the state and action for a
given reward.

${\bm{s}}_{0}$${\bm{s}}_{1}$${\bm{s}}_{2}$${\bm{s}}_{3}$${\bm{s}}_{4}$${\bm{a}}_{0}$${\bm{a}}_{1}$${\bm{a}}_{2}$${\bm{a}}_{3}$$o_{0}$$o_{1}$$o_{2}$$o_{3}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$

We frame optimal control as Bayesian inference problem [33, 27] over the
sequence of actions ${\bm{a}}_{0:T}$. The optimal control problem is
visualized as a graphical model in Fig. 8. The problem is formulated
introducing an auxiliary variable $o_{0:T}$ that represents the optimality of
${\bm{s}}_{t}$ and ${\bm{a}}_{t}$ under a certain reward function,
$p(o_{t}|{\bm{s}}_{t},{\bm{a}}_{t})\propto\exp(r({\bm{s}}_{t},{\bm{a}}_{t}))$.
Given a certain prior distribution $q({\bm{a}}|{\bm{s}}_{0})$ and given
${\bm{s}}_{0}$ is known, the inference problem is

$\displaystyle
p(A_{0}^{T}|{\bm{s}}_{0},O_{0}^{T})=\frac{p(O_{0}^{T}|A_{0}^{T},{\bm{s}}_{0})q(A_{0}^{T}|{\bm{s}}_{0})}{p(O_{0}^{T}|{\bm{s}}_{0})}$
(34)

with

$\displaystyle
p(O_{0}^{T}|A_{0}^{T},{\bm{s}}_{0})=\int_{{\bm{s}}_{1:T}}p(O_{0}^{T}|S_{0}^{T},A_{0}^{T})p(S_{1}^{T}|A_{0}^{T},{\bm{s}}_{0})dS_{1}^{T}$
(35)

where $A_{0}^{T}:\\{{\bm{a}}_{0},\dots,{\bm{a}}_{T}\\}$,
$O_{0}^{T}:\\{o_{0},\dots,o_{T}\\}$ and
$S_{0}^{T}:\\{{\bm{s}}_{0},\dots,{\bm{s}}_{T}\\}$. The are two main directions
to solve the posterior in (34). First, methods that frame the problem as an
Hidden Markov Model (HMM) and solve it in an Expectation-Maximization
approach. Second, methods that compute the posterior in the trajectory level
$A_{0}^{T}$. The first, are computationally demanding as they require several
forward and backward message passing to compute the posterior. The second,
needs to solve the problem in the trajectory level and thus the dimension of
the variables grows linearly with the trajectory length, $T$.

Reactive motion generation instead, solves a one-step-ahead problem. The
solutions in reactive motion generation are local rather than global and thus,
they are computationally more efficient.

We frame reactive motion generation as a one-step control as inference problem

$\displaystyle
p({\bm{a}}_{0}|{\bm{s}}_{0},O_{0}^{T})=\frac{p(O_{0}^{T}|{\bm{a}}_{0},{\bm{s}}_{0})q({\bm{a}}_{0}|{\bm{s}}_{0})}{p(O_{0}^{T}|{\bm{s}}_{0})}$
(36)

with

$\displaystyle p(O_{0}^{T}|{\bm{a}}_{0},{\bm{s}}_{0})=$
$\displaystyle\int_{S_{1:T}}\int_{A_{1:T}}p(O_{0}^{T}|S_{0}^{T},A_{0}^{T})p(S_{1}^{T}|{\bm{a}}_{0},{\bm{s}}_{0})\pi(A_{1}^{T}|S_{1}^{T})dS_{1}^{T}dA_{1}^{T}.$
(37)

In contrast with the classical stochastic optimal control problem that finds
the posterior for the whole trajectory $A_{0}^{T}$, in one-step-ahead control
as inference problem, we aim to find the posterior for only the instant next
control action, ${\bm{a}}_{0}$. Computing the posterior only for
${\bm{a}}_{0}$ requires the likelihood to be defined as the marginal of not
only the state trajectory $S_{1}^{T}$, but also the action trajectory
$A_{1}^{T}$. The graphical model for one-step-ahead control as inference is
presented in Fig. 9.

Figure 9: Graphical model for one-step ahead optimal control problem. In this
approach, the actions $A_{1}^{T}$ are dependant on $S_{1}^{T}$ given a policy
$\pi$.

${\bm{s}}_{0}$${\bm{s}}_{1}$${\bm{s}}_{2}$${\bm{s}}_{3}$${\bm{s}}_{4}$${\bm{a}}_{0}$${\bm{a}}_{1}$${\bm{a}}_{2}$${\bm{a}}_{3}$$o_{0}$$o_{1}$$o_{2}$$o_{3}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$p_{{\bm{s}}{\bm{a}}}^{{\bm{s}}^{\prime}}$$\pi_{{\bm{s}}}^{{\bm{a}}}$$\pi_{{\bm{s}}}^{{\bm{a}}}$$\pi_{{\bm{s}}}^{{\bm{a}}}$

In one-step ahead control as inference, we introduce an additional
distribution $\pi$ that provides us the probability of $A_{1}^{T}$ given
$S_{1}^{T}$. This additional policy $\pi$ is interpreted as the policy the
agent will apply in the future. In this context, given we know the policy the
agent will apply in the future, we are looking for the instant action the
maximized the cumulative reward in the long horizon trajectory. (37) can be
rewritten as the expectation over $p(O_{0}^{T}|S_{0}^{T},A_{0}^{T})$

$\displaystyle\mathbb{E}_{S_{0}^{T},A_{0}^{T}\sim
p^{\pi}(S_{0}^{T},A_{0}^{T}|{\bm{s}}_{0},{\bm{a}}_{0})}\left[p(O_{0}^{T}|S_{0}^{T},A_{0}^{T})\right]\propto$
$\displaystyle\mathbb{E}_{S_{0}^{T},A_{0}^{T}\sim
p^{\pi}(S_{0}^{T},A_{0}^{T}|{\bm{s}}_{0},{\bm{a}}_{0})}\left[\exp(\sum_{t=0}^{T}r({\bm{s}}_{t},{\bm{a}}_{t}))\right]=$
$\displaystyle\exp(Q_{r}^{\pi}({\bm{s}}_{0},{\bm{a}}_{0}))$ (38)

From what follows, the likelihood for our inference problem is proportional to
the $\exp(Q)$ defined over a certain policy $\pi$. Given a $\pi$, the
posterior for our inference problem is given by

$\displaystyle
p({\bm{a}}_{0}|{\bm{s}}_{0},O_{0}^{T})\propto\exp(Q_{r}^{\pi}({\bm{s}}_{0},{\bm{a}}_{0}))q({\bm{a}}_{0}|{\bm{s}}_{0}).$
(39)

The provided $Q$ function depends on $\pi$ and then, the quality of our
reactive motion generator to solve a long horizon optimal control problem
directly depends on the quality of $\pi$. In the optimal case, for $Q^{*}$,
the reactive motion generator can find the optimal trajectory, even if the
optimization is done locally.

###  A-A Optimality Guarantees

In CEP, we propose to model the $Q$ function as the sum of a set of optimal
$Q_{k}^{*}$ functions. Instead, we are aware that
$Q_{\Sigma}=\frac{1}{K}\sum_{k=0}^{K}Q_{k}^{*}$ is not the optimal function
$Q^{*}$ for the sum of the rewards $r=\frac{1}{K}\sum_{k=1}^{K}r_{k}$. The
closer $Q_{\Sigma}$ is from the optimal $Q^{*}$, the closer the product of
experts policy would be from the optimal policy. In this section, we study,
given a certain reward $r=\frac{1}{2}(r_{1}+r_{2})$, how much the sum of the
individual components $Q_{\Sigma}$ diverge from the optimal $Q^{*}$. In [27]
is shown, that the optimal Q function for the control as inference problem can
be computed by recursively solving the soft-value iteration [50]

$\displaystyle Q({\bm{s}},{\bm{a}})$
$\displaystyle=r({\bm{s}},{\bm{a}})+\mathbb{E}_{p({\bm{s}}^{\prime}|{\bm{s}},{\bm{s}})}\left[V({\bm{s}}^{\prime})\right]$
$\displaystyle V({\bm{s}})$
$\displaystyle=\log\int_{\mathcal{A}}\exp(Q({\bm{a}},{\bm{s}})).$ (40)

We assume a finite horizon control as inference problem and evaluate how much
$Q_{\Sigma}$ diverges from $Q*$ when increasing the control horizon

For $t=T$,

$\displaystyle Q^{*T}=\frac{1}{2}(r_{1}+r_{2})$ (41)

and

$\displaystyle Q_{1}^{*T}=r_{1}\,,\,Q_{2}^{*T}=r_{2}.$ (42)

Then,

$\displaystyle Q^{*T}=\frac{1}{2}(Q_{1}^{*T}+Q_{2}^{*T})$ (43)

and the divergence

$\displaystyle\Delta Q^{T}=Q^{*T}-Q_{\Sigma}^{T}=0.$ (44)

From (43), for $T=1$ horizon optimal control problems, the sum of the optimal
components $Q_{\Sigma}$ is equal to the optimal $Q$. For longer horizon
control as inference problems, the optimal $Q$ is computed by recursively
solving the soft-bellman update backward in time. For computing $t=T-1$,

$\displaystyle Q^{*T-1}({\bm{s}},{\bm{a}})$
$\displaystyle=r({\bm{s}},{\bm{a}})+\mathbb{E}_{p({\bm{s}}^{\prime}|{\bm{s}},{\bm{s}})}\left[V^{*T}({\bm{s}}^{\prime})\right]$
$\displaystyle V^{*T}({\bm{s}})$
$\displaystyle=\log\int_{\mathcal{A}}\exp(Q^{*T}({\bm{a}},{\bm{s}})).$ (45)

we do a soft bellman update. The difference betwen $Q^{*T-1}$ and
$Q_{\Sigma}^{T-1}$

$\displaystyle\Delta
Q^{T-1}=Q^{*T-1}-Q_{\Sigma}^{T-1}=\mathbb{E}_{p({\bm{s}}^{\prime}|{\bm{s}},{\bm{a}})}\left[V^{*T}-V_{\Sigma}^{T}\right]$

with

$\displaystyle
V^{*T}-V_{\Sigma}^{T}=\log\frac{\int_{\mathcal{A}}\exp(Q^{*T})}{\int_{\mathcal{A}}(\exp(Q_{1}^{*T})\int_{\mathcal{A}}\exp(Q_{2}^{*T}))^{\frac{1}{2}}}$
$\displaystyle=\log\frac{\int_{\mathcal{A}}\exp(\frac{1}{2}Q_{1}^{*T})\exp(\frac{1}{2}Q_{2}^{*T})}{(\int_{\mathcal{A}}\exp(Q_{1}^{*T})\int_{\mathcal{A}}\exp(Q_{2}^{*T}))^{\frac{1}{2}}}.$
(47)

Then,

$\displaystyle Q^{*T-1}$ $\displaystyle=Q_{\Sigma}^{T-1}+\Delta Q^{T-1}$
$\displaystyle=Q_{\Sigma}^{T-1}+\mathbb{E}_{p({\bm{s}}^{\prime}|{\bm{s}},{\bm{a}})}\left[\log\frac{\int_{\mathcal{A}}\exp(\frac{1}{2}Q_{1}^{*T})\exp(\frac{1}{2}Q_{2}^{*T})}{(\int_{\mathcal{A}}\exp(Q_{1}^{*T})\int_{\mathcal{A}}\exp(Q_{2}^{*T}))^{\frac{1}{2}}}\right]$
(48)

From (A-A) and (A-A), we can obtain the recurrence relation for the divergence
error

$\displaystyle\Delta
Q^{t-1}=\mathbb{E}_{p({\bm{s}}^{\prime}|{\bm{s}},{\bm{a}})}\left[\log\frac{\int_{\mathcal{A}}\exp(\frac{1}{2}Q_{1}^{*t})\exp(\frac{1}{2}Q_{2}^{*t})\exp(\Delta
Q^{t})}{(\int_{\mathcal{A}}\exp(Q_{1}^{*t})\int_{\mathcal{A}}\exp(Q_{2}^{*t}))^{\frac{1}{2}}}\right].$
(49)

From (A-A) and (A-A), we can see that if $Q_{1}^{*}=Q_{2}^{*}$, then $\Delta
Q=0$ and the more they differ, the bigger the divergence error to the optima
$Q^{*}$.

We can also extrapolate interesting results from (49). For any $Q_{1}^{*}$ and
$Q_{2}^{*}$,

$\displaystyle(\int_{\mathcal{A}}\exp(Q_{1}^{*t})\int_{\mathcal{A}}\exp(Q_{2}^{*t}))^{\frac{1}{2}}>\int_{\mathcal{A}}\exp(\frac{1}{2}Q_{1}^{*t})\exp(\frac{1}{2}Q_{2}^{*t})$
(50)

and then,

$\displaystyle\Delta Q<0,\forall\,Q_{1}^{*},Q_{2}^{*}$ (51)

And then, from (49), $\Delta Q$ monotonically decreases. The longer the
horizon T, the bigger the divergence error $\Delta Q$.

In practice, these insights are beneficial for modeling our $Q$ functions. We
can expect the performance of our reactive motion generation to decay in those
environments in which long-horizon planning is required as the $\Delta Q$ will
increase over long horizons. On the other hand, the more overlapping regions
the $Q$ components have, the smaller the divergence error would be and the
closer we would be to the optima.

Similar theoretical studies have been already developed [11, 47, 43]. In
Optimal Control, composable optimality guarantees were proven for linear
dynamics, by the linearly-solvable Markov Decision Processes (LMDP) approach.
In [43, 4], a weighted policy sum was proven to be optimal for the sum of the
rewards, as long as the rewards differ only in the terminal reward.

In [11, 42], the optimality of the composition is studied in a maximum entropy
reinforcement learning problem

$\displaystyle
J(\pi)=\mathbb{E}_{\pi,p({\bm{s}}_{0})}\left[\sum_{t=0}^{T}r({\bm{s}}_{t},{\bm{a}}_{t})+\alpha{\mathcal{H}}(\pi)|{\bm{s}}_{t+1}\sim
p(\cdot|{\bm{s}}_{t},{\bm{a}}_{t})\right]$ (52)

with ${\mathcal{H}}(\pi)$ the entropy of the policy. In [11] is proven, that
optimal soft-Q function for $r=\frac{1}{2}r_{1}+r_{2}$ is bounded

$\displaystyle Q_{\Sigma}({\bm{s}},{\bm{a}})\geq Q^{*}({\bm{s}},{\bm{a}})\geq
Q_{\Sigma}({\bm{s}},{\bm{a}})-{\mathcal{C}}^{*}({\bm{s}},{\bm{a}})$ (53)

with $Q_{\Sigma}=\frac{1}{2}Q_{1}^{*}+Q_{2}^{*}$ and ${\mathcal{C}}^{*}$ is
the fixed point of

$\displaystyle{\mathcal{C}}({\bm{s}},{\bm{a}})\leftarrow\gamma\mathbb{E}_{p({\bm{s}}^{\prime}|{\bm{s}},{\bm{a}})}\left[{\mathcal{D}}_{\frac{1}{2}}(\pi_{1}^{*}({\bm{a}}|{\bm{s}})||\pi_{2}^{*}({\bm{a}}|{\bm{s}}))+\max_{{\bm{a}}^{\prime}}{\mathcal{C}}({\bm{s}}^{\prime},{\bm{a}}^{\prime})\right]$
(54)

and ${\mathcal{D}}_{\frac{1}{2}}$ is the Renyi divergence of order
$\frac{1}{2}$.

##  Appendix B Composable Energy Policies as Inference

The product of experts is the natural expression for the multi-objective
inference problem presented in Fig. 10. We introduce a set of auxiliary
variables $o_{0},\dots,o_{K}$ that represents the optimality of ${\bm{s}}_{0}$
and ${\bm{a}}_{0}$ given a certain energy function
$p(o_{k}=1|{\bm{s}}_{0},{\bm{a}}_{0})\propto\exp(E_{k}({\bm{s}},{\bm{a}}))$.

Figure 10: Graphical model for Composable Energy Policies. $o_{k}$ is an
auxiliary variable that represents the optimality of ${\bm{s}}_{0}$ and
${\bm{a}}_{0}$ for a particular
energy.${\bm{s}}_{0}$${\bm{a}}_{0}$$o_{0}$$o_{1}$$o_{2}$

Then, the likelihood for the graphical model in Fig. 10 can be computed as the
product of the terms

$\displaystyle
p({\bm{s}}_{0},{\bm{a}}_{0},o_{0:2})=q({\bm{a}}_{0})p({\bm{s}}_{0})\prod_{k=0}^{2}p(o_{k}|{\bm{s}}_{0},{\bm{a}}_{0}).$
(55)

We are interested on computing the maximum a posteriori for ${\bm{a}}_{0}$
given ${\bm{s}}_{0}$ and $o_{k}=1\,,\,\forall\,k=(0,1,2)$

$\displaystyle{\bm{a}}_{0}^{*}$
$\displaystyle=\operatorname*{arg\,max}_{{\bm{a}}_{0}}p({\bm{a}}_{0}|{\bm{s}}_{0},o_{0:2}=1)$
$\displaystyle\propto\exp\left(\sum_{k=0}^{2}E_{k}({\bm{a}}_{0},{\bm{s}}_{0})\right)q({\bm{a}}_{0}).$
(56)

From (56), we can see that the maximum a posteriori will search for the
optimal ${\bm{a}}_{0}$ given an exponentiated sum of energies and the prior
distribution $q({\bm{a}}_{0})$. If we assume a uniform distribution for the
prior, we recover (4). Thus, the product of experts represents the
distribution that satisfies best a set of components $E_{0},\dots,E_{k}$. We
highlight that the CEP graphical model in Fig. 10 and in Fig. 3 are
equivalent. The model in Fig. 10, represent the CEP as the distribution
maximizing a set of rewards $E_{0},\dots\ E_{k}$, while the model in Fig. 3
represent the CEP as the distribution maximizing the combination of a set of
policies. Depending on the problem, we might be interested to frame the
problem with one or the other approach.

##  Appendix C Composable Energy Policies and Artificial Potential Fields
Method

In the following section, we will show that the Artificial Potential Fields
Method can be written as a particular case of Composable Energy Policies.

Artificial Potential Field Method assumes a weighted sum of the accelerations
generated by a set of dynamic components

$\displaystyle\ddot{{\bm{q}}}=\sum_{k=0}^{K}{\bm{\Lambda}}_{k}{\bm{g}}_{k}({\bm{q}},\dot{{\bm{q}}}).$
(57)

Let’s consider a particular case of CEP were all the components are
represented with a normal distribution centered in
${\bm{g}}_{k}({\bm{q}},\dot{{\bm{q}}})$,

$\displaystyle\pi_{k}(\ddot{{\bm{q}}}|{\bm{q}},\dot{{\bm{q}}})={\mathcal{N}}({\bm{g}}_{k}({\bm{q}},\dot{{\bm{q}}}),\Sigma_{k}).$
(58)

Then, given $K$ components

$\displaystyle\pi(\ddot{{\bm{q}}}|{\bm{q}},\dot{{\bm{q}}})$
$\displaystyle=\prod_{k=0}^{K}{\mathcal{N}}({\bm{g}}_{k}({\bm{q}},\dot{{\bm{q}}}),\Sigma_{k})={\mathcal{N}}({\bm{g}}_{\Sigma}({\bm{q}},\dot{{\bm{q}}}),\Sigma_{\Sigma})$
(59)

with

$\displaystyle{\bm{g}}_{\Sigma}=\left(\sum_{k=0}^{K}\Sigma_{k}^{-1}\right)^{-1}\left(\sum_{k=0}^{K}\Sigma_{k}^{-1}{\bm{g}}_{k}\right)$
(60)

and

$\displaystyle\Sigma_{\Sigma}=\left(\sum_{k=0}^{K}\Sigma_{k}^{-1}\right)^{-1}.$
(61)

In CEP, the acceleration is obtained by an optimization of

$\displaystyle\ddot{{\bm{q}}}=\operatorname*{arg\,max}_{\ddot{{\bm{q}}}}\pi(\ddot{{\bm{q}}}|{\bm{q}},\dot{{\bm{q}}}).$
(62)

The maximum of 59 is known; given the policy is a gaussian, the maximun is the
mean in 60

$\displaystyle\ddot{{\bm{q}}}=\left(\sum_{k=0}^{K}\Sigma_{k}^{-1}\right)^{-1}\left(\sum_{k=0}^{K}\Sigma_{k}^{-1}{\bm{g}}_{k}({\bm{q}},\dot{{\bm{q}}})\right).$
(63)

We can rewrite (63) to (57)

$\displaystyle{\bm{\Lambda}}_{k}=\left(\sum_{k=0}^{K}\Sigma_{k}^{-1}\right)^{-1}.\Sigma_{k}^{-1}$
(64)

We show that APF policy can be rewritten as a CEP, modelling the dynamics by a
product of normal distributions. Modelling all the components by normal
distributions might be a bad choice for a proper modules integration. normal
distributions assume that (1) there is a unique optimal action (the mean) to
solve the task and (2) the actions quality is related to the mahalanobis
distance to the optima. While tasks like reaching a target might satisfy (1)
and (2); tasks like obstacle avoidance might require richer representations to
properly solve the task.

##  Appendix D Experiments

###  D-A Reactive Motion Generation

The modular components (Go-to, Obstacle Avoidance and joint limits avoidance)
for both baselines APF and RMP were modeled based on [20] and [3]
respectively. CEP were modeled following the energies presented in section IV.
We introduce a table with the hyperparameters for all CEP energies

Energy Modules | Parameters
---|---
Reach Target | ${\bm{K}}_{p}=20.$ | ${\bm{K}}_{v}=30.$ | $\alpha=10.$
Obstacle Avoidance | $\gamma=0.2$ | $\alpha=4.$ | $\beta=0.1$
Joint Limits avoidance | $\gamma=0.3$ | $\alpha=4.$ | $\beta=0.1$

TABLE III: Component parameters for Composable Energy Policies in reactive
motion generation

###  D-B Prior based policies for RL

We introduce a Table with the Hyperparameters for the PPO in Mushroom-RL [6].
We used the same hyperparameters for the three methods (Behavioural Cloning +
RL, Residual RL and CEP for RL).

Hyperparameters |
---|---
horizon | 1400
Policy Net | 18-128-128-3
policy std0 | 1.
policy batch | 256
n_epochs_policy | 2
policy learn rate | 1e-4
Critic Net | 18-256-256-1
critic batch | 256
critic learn rate | 3e-4
n_steps_per_fit | horizon x 30
Discount factor | .997

TABLE IV: Component parameters for Composable Energy Policies in reactive
motion generation