#  RoCUS: Robot Controller
Understanding via Sampling

Yilun Zhou Serena Booth Nadia Figueroa Julie Shah
MIT CSAIL
{yilun, serenabooth, nadiafig<EMAIL_ADDRESS>
###### Abstract

As robots are deployed in complex situations, engineers and end users must
develop a holistic understanding of their behaviors, capabilities, and
limitations. Some behaviors are directly optimized by the objective function.
They often include success rate, completion time or energy consumption. Other
behaviors—e.g., collision avoidance, trajectory smoothness or motion
legibility—are typically emergent but equally important for safe and
trustworthy deployment. Designing an objective which optimizes every aspect of
robot behavior is hard. In this paper, we advocate for systematic analysis of
a wide array of behaviors for holistic understanding of robot controllers and,
to this end, propose a framework, RoCUS, which uses Bayesian posterior
sampling to find situations where the robot controller exhibits user-specified
behaviors, such as highly jerky motions. We use RoCUS to analyze three
controller classes (deep learning models, rapidly exploring random trees and
dynamical system formulations) on two domains (2D navigation and a 7 degree-
of-freedom arm reaching), and uncover insights to further our understanding of
these controllers and ultimately improve their designs.

https://yilunzhou.github.io/RoCUS

> Keywords: Debugging and Evaluation, Algorithmic Transparency

##  1 Introduction

In 2018, after a confluence of failures, an autonomous vehicle (AV) struck and
killed a pedestrian for the first time. In the run-up to this fateful event,
the responsible company had reportedly been trying to improve the AV “ride
experience” by emphasizing non-critical behaviors—such as the smoothness of
the ride [1]. This event reflects the long-standing challenge in robotics:
designing an appropriate objective which considers both safety-critical and
non-critical behaviors. When crafting an objective, it is virtually impossible
to proactively account for all potential controller behaviors, and some
priorities may even be in conflict with one another [2]. In practice, any
given robot behaviors may be specified, unspecified, or even misspecified [3],
so extensive testing and evaluation is a critical component of designing and
assessing robot controllers—especially those using black-box models such as
deep neural networks.

A common testing procedure focuses on finding extreme and edge cases of
controller failure. For example, a tester might use this procedure to find
that the AV swerves very badly when encountering a farm animal while traveling
at 60mph. Finding such extreme and edge cases is well-studied within both
traditional software testing paradigms [4] and more recent adversarial
perturbation testing methods [5]. However, we argue that an equally, if not
more, important form of testing should focus on _representative_ scenarios,
which considers the likelihood of encountering these scenarios. For example,
if this AV is going to be deployed exclusively in New York City, the above
example is largely unhelpful: cars rarely travel at 60mph in the city, and are
very unlikely to encounter farm animals. Instead, the tester may prefer to
know that the car swerves—though not as substantively—at lower speeds when a
pedestrian steps toward it. Finding representative scenarios is often
overlooked, but is especially useful for robotics. This is the focus of this
paper.

Explicit mathematical analysis of robot controllers is implausible given the
high dimensionality of the configuration space and the potential black-box
representation of a learned controller. With access to an environment
simulator, though, a straightforward testing approach is to roll out the
robotic controller on various environments (e.g. road conditions under
different weather and congestion, with or without farm animals or pedestrians,
etc.), and analyze those rollouts that exhibit a specified behavior—like
excessive swerving. However, with too few environments, we risk missing the
condition(s) that triggers the target behavior most saliently. With too many
environments, all the most salient rollouts would be close to the global
maximum at the expense of diversity and coverage. For example, if a farm
animal causes the most swerving, followed by a pedestrian and a dangling tree
branch, using too few environments may only find the pedestrian and the tree
branch while using too many would result in an exclusive focus on the farm
animal. Neither case helps the human develop a correct mental model of the
AV’s behavior.

Figure 1: Two use case demos of RoCUS: 2D navigation (left) and 7DoF arm
reaching (right).

To address this, we introduce Robot Controller Understanding via Sampling
(RoCUS), a method to enable systematic behavior inspection. RoCUS finds
scenarios that are both inherently likely and elicit specified behaviors by
formulating the problem as one of Bayesian posterior inference. Analyzing
these scenarios and the resulting trajectories can help developers better
understand the robot behaviors, and allow them to iterate on algorithm
development if undesirable ones are revealed.

We use RoCUS to analyze three controllers on two common robotics tasks (Fig.
1). For a 2D navigation problem, we consider imitation learning (IL) [6],
dynamical system (DS) [7], and rapidly-exploring random tree (RRT) [8]. For a
7DoF arm reaching problem, we consider reinforcement learning (RL) [9], as
well as the same DS and RRT controllers. For each problem and controller, we
specify several behaviors and visualize representative scenarios and
trajectories that elicit those behaviors. Through this analysis, we uncover
insights that would be hard to derive analytically and thus complement our
mathematical understanding of the controllers. Moreover, we include a case
study on how to improve a controller based on new insights from RoCUS. As
such, RoCUS is a step towards the broader goal of building more accurate human
mental models and enabling holistic evaluation of robot behaviors.

##  2 Related Work

Our work lies at the intersection of efforts to understand complex model
behaviors and those to benchmark robot performance. Methods to understand,
interpret, and explain model behaviors are now commonplace in the machine
learning community. Mitchell et al. [10] introduced Model Cards, a model
analysis mechanism which breaks down model performance for data subsets. In
natural language processing, Ribeiro et al. [11] introduced a checklist for
holistic evaluation of model capabilities and test case generation. Booth et
al. [12] introduced Bayes-TrEx, a Bayesian inference framework for sampling
specified classifier behaviors. In robotics, Fan et al. [13] introduced a
verification framework for assessing machine behavior by sampling parameter
spaces to find temporal logic-satisfying behaviors. Other efforts aim to
summarize robot policies, trading off factors like brevity, diversity and
completeness [14, 15]. All of these works have a shared underlying theme:
treating the black box as immutable and performing downstream analyses of
machine behavior [16]. RoCUS shares this theme and, similar to Bayes-TrEx
[12], searches for instances which exhibit target behaviors to inform accurate
human mental models.

While the need for benchmarking robot performance is often expressed [17, 18,
19], these efforts usually operate on distributions of trajectories or
randomly selected trajectories, and the accompanying metrics are typically
task-completion based without consideration of implicit performance factors.
Anderson et al. [20] put forth a recommendation of using _success weighted by
path length_ for navigation tasks—a task-completion metric. Cohen et al. [21]
and Moll et al. [22] introduced suites of metrics for comparing motion
planning approaches, and Lagriffoul et al. [23] presented a set of task and
motion planning scenarios and metrics. Again, all of these proposed metrics
are based solely on task completion. Lemme et al. [24] proposed a set of
performance measures for reaching tasks, which are either task-completion
based or require a costly human motion ground truth. Our contribution is
distinct in two ways. First, we propose to sample specific trajectories which
communicate controller behaviors instead of reporting metrics averaged over
distributions of trajectories. Second, we introduce metrics which draw on
these prior works while also including essential alternative and typically
emergent quality factors, like motion jerkiness and legibility [25].

##  3 RoCUS

$\displaystyle t$$\displaystyle\tau$$\displaystyle b$$\displaystyle\hat{b}$
Figure 2: The graphical model for the inference problem of finding tasks $t$
and trajectories $\tau$ which exhibit specific behaviors $b$. The dashed box
indicates the relaxed formulation (Eq. 2).

At a high level, RoCUS helps users understand robotic controllers via
representative scenarios that exhibit various specified behaviors. It solves
this by directly incorporating the distribution of scenarios, formally called
tasks, into a Bayesian inference framework as shown in Fig. 2. A robotic
problem is represented by a distribution $\pi(t)$ of individual tasks $t$. For
example, a navigation problem may have $\pi(t)$ representing the distribution
over target locations and obstacle configurations. Given a specific task $t$,
the controller under study induces a distribution $p(\tau|t)$ of possible
trajectories $\tau$. If both the controller and the transition dynamics are
deterministic, $p(\tau|t)$ reduces to a $\delta$-function at the induced
trajectory $\tau$. Stochasticity in either the controller (e.g., RRT) or the
dynamics (e.g., uncertain outcome from an action) can result in $\tau$ being
random. Finally, a behavior function $b(\tau,t)$ computes the behavior value
of the trajectory—for example, the motion jerkiness. Some behaviors only
depend on the trajectory and not the task, but we use $b(\tau,t)$ for
consistency. Sec. 4 presents a list of behaviors.

The discussion on behavior in Sec. 1 is informal and implicitly combines two
related but different concepts. The first concept is the behavior function
$b(\tau,t)$ discussed above. The second is the specified target: for the
swerving example, we are particularly interested in maximal behavior values.
Thus, the target value can be thought of as $+\infty$. This inference problem
uses the maximal mode of RoCUS. In other cases, we are also interested in
tasks and trajectories whose behaviors matches a target. For example, we want
to find road conditions that lead to a daily commute time of an hour, where
the behavior is the travel time. This inference problem uses the _matching_
mode. Since matching mode is conceptually simpler, we present it first,
followed by maximal mode. The sampling procedure is the same for both modes
and presented last in Alg. 1.

###  3.1 Matching Mode

The exact objective is to find tasks and trajectories that exhibit user-
specified behaviors $b^{*}$:

$\displaystyle t,\tau\sim p(t,\tau|b=b^{*})\propto
p(b=b^{*}|t,\tau)\pi(\tau|t)\pi(t).$ (1)

In most cases this posterior does not admit direct sampling, and an envelope
distribution is not available for rejection sampling. Markov-Chain Monte-Carlo
(MCMC) sampling does not work either: since the posterior is only non-zero on
a very small or even measure-zero set, a Metropolis-Hastings (MH) sampler [26]
can get stuck in the zero-density region. Similar to the Bayes-TrEx
formulation [12], we relax it using a normal distribution formulation as shown
in Fig. 2:

$\displaystyle\widehat{b}|b\sim\mathcal{N}(b,\sigma^{2})\,\,\quad t,\tau\sim
p(t,\tau|\widehat{b}=b^{*})\propto
p(\widehat{b}=b^{*}|t,\tau)p(\tau|t)\pi(t).$ (2)

This relaxed posterior is non-zero everywhere $\pi(t)$ is non-zero and
provides useful guidance to an MH sampler. While $\sigma$ is a hyper-parameter
in Bayes-TrEx [12], we instead choose $\sigma$ such that

$\displaystyle\int_{b^{*}-\sqrt{3}\sigma}^{b^{*}+\sqrt{3}\sigma}p(b)\>\mathrm{d}b=\alpha,\,\,\,\mathrm{with}\,\,\,p(b)=\int_{t}\int_{\tau}p(\tau|t)\pi(t)\mathbbm{1}_{b(\tau,t)=b}\>\mathrm{d}{\tau}\>\mathrm{d}t$
(3)

being the marginal distribution of $b(\tau,t)$, which can be estimated by
trajectory roll-outs. This formulation has two desirable properties. First, it
is scale-invariant with respect to $b(\tau,t)$, e.g. measured under different
units like meters vs. centimeters. Second, the hyper-parameter
$\alpha\in[0,1]$ has the intuitive interpretation of the approximate “volume”
of posterior samples $t,\tau\mid\widehat{b}=b^{*}$ under the marginal
$p(t,\tau)=p(\tau|t)\pi(t)$, a notion of their representativeness. Details are
derived in App. A.

###  3.2 Maximal Mode

In this mode, RoCUS finds trajectories that lead to maximal behavior values:
$b^{*}\rightarrow\pm\infty$. It can also be used for finding minimal behavior
values by negating the behavior. The posterior formulation is:

$\displaystyle
b_{0}=\frac{b-\mathbb{E}[b]}{\sqrt{\mathbb{V}[b]}},\quad\beta=\frac{1}{1+e^{-b_{0}}},\quad\widehat{\beta}\sim\mathcal{N}\left(\beta,\sigma^{2}\right),\quad
t,\tau\sim p(t,\tau|\widehat{\beta}=1),$ (4)

where $\mathbb{E}[b]$ and $\mathbb{V}[b]$ are the mean and variance of the
marginal $p(b)$. $\sigma$ is chosen such that

$\displaystyle\int_{1-\sqrt{3}\sigma}^{1}p(\beta)\>\mathrm{d}\beta=\alpha,$
(5)

where $p(\beta)$ is the marginal distribution similar to Eq. 3. If $p(b)$ is
normal, $p(\beta)$ is logit-normal. This formulation is again scale-invariant
and has the same “volume” interpretation for $\alpha$ (App. A).

###  3.3 Posterior Sampling

Input: “Posterior volume” $\alpha$, number of samples $N$, optional burn-in
$N_{B}$ and thinning period $N_{T}$.

1 samples $\leftarrow[\,\,]$;

2 Get $\sigma$ from $\alpha$ by Eq. 3 (matching) or 5 (maximal);

3 Randomly initialize $t$;

4 for  _$i=1,...,N$_ do

5 $t_{\mathrm{new}},p_{\mathrm{for}},p_{\mathrm{rev}}=\mathrm{propose}(t)$

6 Get $p$ from $t$ by Eq. 2 (match) or Eq. 4 (max)

7 Get $p_{\mathrm{new}}$ from $t_{\mathrm{new}}$ by Eq. 2 or Eq. 4;

8 $a\leftarrow(p_{\mathrm{new}}\cdot p_{\mathrm{rev}})/(p\cdot
p_{\mathrm{for}})$;

9 Sample $u\sim\mathcal{U}[0,1]$;

10 if  _$u <a$_ then

11 $t\leftarrow t_{\mathrm{new}}$;

12 Append $t$ to samples;

13 Optionally, discard the first $N_{B}$ burn-in samples and thin the samples
by only keeping every $N_{T}$ samples;

14 return _samples_

Algorithm 1 MH Sampling Procedure

The posterior sampling mechanism depends on the stochasticity of the
controller and dynamics.

Deterministic Controller & Dynamics: When both the controller and the dynamics
are deterministic, so is $\tau|t$, denoted as $\tau(t)$. Eq. 2 reduces to
$t\sim p(t|\widehat{b}=b^{*})\propto p(\widehat{b}=b^{*}|t,\tau(t))\pi(t),$
and similarly for Eq. 4.

Alg. 1 presents the MH sampling procedure. First, $\sigma$ is computed from
$\alpha$ (Line 2). Then we start with an initial task $t$ (Line 3). For each
of the $N$ iterations, we propose a new task $t_{\mathrm{new}}$ according to a
transition kernel and compute the forward and reverse transition probabilities
$p_{\mathrm{for}},p_{\mathrm{rev}}$ (Line 5). We evaluate the posteriors under
$t$ and $t_{\mathrm{new}}$ (Line 6 and 7) and calculate the acceptance
probability using the MH detailed balance principle (Line 8). Finally, we
accept or reject accordingly (Line 9 – 11). Note that if the proposal is
rejected, the current $t$ is left unchanged and appended to the samples. We
can discard the first $N_{B}$ samples as burn-in, and/or thin the samples by a
factor of $N_{T}$ to reduce auto-correlation.

$\displaystyle t$$\displaystyle\tau$$\displaystyle
b$$\displaystyle\hat{b}$$\displaystyle u$

Figure 3: The same graphical model as in Fig. 2, but with the addition of
stochasticity $u$ in the controller such that $\tau|t,u$ is now deterministic.

Stochastic Controller: When the controller and $p(\tau|t)$ are stochastic, the
controller can usually be implemented by sampling a random variable $u$
(independent from $t$), and then producing the action based on the realization
of $u$, as shown in Fig. 3. For instance, a Normal stochastic policy
$\pi(s)\sim\mathcal{N}(\mu(s),\sigma(s)^{2})$ can be implemented by first
sampling $u\sim\mathcal{N}(0,1)$ and then computing
$\pi(s)=\mu(s)+u\cdot\sigma(s)$. In this case, we sample in the combined
$(t,\tau)$-space, with Eq. 2 being $p(t,\tau|\widehat{b}=b^{*})\propto
p(\widehat{b}=b^{*}|t,\tau(e,u))p(u)\pi(t)$, where we overload $\tau(t,u)$ to
refer to the deterministic trajectory given the task $t$ and controller
randomness $u$. It is crucial that for any $u$, we can evaluate $p(u)$.
Concretely, modifying Alg. 1, $u_{\mathrm{new}}$ is proposed alongside with
$t_{\mathrm{new}}$ (Line 5), the detailed balancing factor (Line 8) is
multiplied by $p_{u,\mathrm{rev}}/p_{u,\mathrm{for}}$, and
$t_{\mathrm{new}},u_{\mathrm{new}}$ are accepted or rejected together (Line 10
– 12).

Stochastic Dynamics: Using the same logic, RoCUS can also accommodate dynamics
stochasticity, as long as it can be captured in a random variable $v$ and
$p(v)$ can be evaluated. We leave the details to App. B and use deterministic
dynamics in our experiments.

###  3.4 The Bayesian Posterior Sampling Interpretation

RoCUS uses Bayesian sampling concepts of prior, likelihood, and posterior
quite liberally. Specifically, the task distribution is defined as the prior,
and thus the notion of a task being likely in the deployment context refers to
high probability under the prior. Likelihood refers to the behavior saliency:
how much the exhibited behavior matches the behavior specification. The act of
posterior sampling then finds tasks that strike a balance between these two
objectives.

The choice of explicitly modeling the task distribution is intentional, as it
is not unlikely that the deployment environment will be different than the
development environment. Such a domain mismatch may cause catastrophic
failures, especially for learned controllers whose extrapolation behaviors are
typically undefined. With a suitable task distribution, RoCUS allows more
failures to surface during this testing procedure.

##  4 Behavior Taxonomy

Robot behaviors broadly belong to one of two classes: intentional and
emergent. Intentional behaviors are those that the controller explicitly
optimize with objective functions. For example, the controller for a reaching
task likely optimizes to move the end-effector to the target, by setting the
target as an attractor in DS, using a target-reaching objective configuration
in RRT, or rewarding proximity in RL. Thus, the final distance between the
end-effector and the target is an intentional behavior for all three
controllers. By contrast, _emergent_ behaviors are not explicitly specified in
the objective. For the same reaching problem, an RL policy with reward based
solely on distance may exhibit smooth trajectories for some target locations
and jerky ones for others. Such behaviors may emerge due to robot kinematic
structure, training stochasticity, or model inductive bias.

For trajectory $\tau$, many behavior metrics $b(\tau,t)$ can be expressed as a
line integral $\int_{\tau}V(\mathbf{x})\>\mathrm{d}s$ of a scalar field
$V(\mathbf{x})$ along $\tau$ or its length-normalized version
$\frac{1}{||\tau||}\int_{\tau}V(\mathbf{x})\>\mathrm{d}s$, where
$\>\mathrm{d}s$ is the infinitesimal segment on $\tau$ at $\mathbf{x}$ and
$||\tau||$ is the trajectory length. $\mathbf{x}$ and $\tau$ can be in either
joint space or task space. We introduce six behaviors: length, time
derivatives (velocity, acceleration and jerk), straight-line deviation,
obstacle clearance, near-obstacle velocity and motion legibility, whose
mathematical expressions are in App. C. In addition, custom behaviors can also
be used with RoCUS.

##  5 RoCUS Use Case Demos

In this section, we demonstrate how RoCUS can find “hidden” properties of
various controllers for two common tasks, navigation and reaching. We also
uncover a suboptimal controller design due to bad hyper-parameter choices,
which is improved based on RoCUS insights.

###  5.1 Controller Algorithms

We consider four classes of robot controllers. The imitation learning (IL)
controller uses expert demonstrations to learn a neural network policy which
maps observations to deterministic actions. The reinforcement learning (RL)
controller implements proximal policy gradient (PPO) [27]. While a mean and a
variance is used to parameterize a PPO policy during training, the policy
deterministically outputs the mean action during evaluation. The dynamical
system (DS) controller modulates the linear controller
$\mathbf{u}(\mathbf{x})=\mathbf{x}^{*}-\mathbf{x}$, for the task-space target
$\mathbf{x}^{*}$, into
$\mathbf{u}_{M}(\mathbf{x})=M\cdot\mathbf{u}(\mathbf{x})$ using the modulation
matrix $M$ derived from obstacle configuration, as proposed by Huber et al.
[7]. We give a self-contained review in App. D. The rapidly-exploring random
tree (RRT) controller finds a configuration-space trajectory via RRT and then
controls the robot through descretized segments. Notably, RRT is stochastic,
and we discuss the use of controller stochasticity $u$ (c.f. Fig. 3) in App.
E. The MCMC sampling uses a Gaussian drift kernel, as detailed in App. F.

###  5.2 2D Navigation Task Experiments

Figure 4: RRT, IL and DS controllers on 2D navigation domain. Left: the RRT
controller tree. Middle: smoothed RRT trajectory and lidar sensor (orange
lines) for IL controller training. Right: the modulation by the DS controller.

Setup In a rectangular arena with irregularly shaped obstacles, a point mass
robot needs to move from the lower left to the upper right corner (Fig. 1
left). App. G details the obstacle generation and robot simulation procedures
and contains more environment visualizations.

We consider three controllers for this environment: an RRT planner, a deep
learning IL policy, and a DS (Fig. 4). The RRT planner implements Alg. 2 and
discretizes the path to small segments as control signals at each time step.
The IL controller uses smoothed RRT trajectories as expert demonstrations, and
learns to predict heading angle from its current position and lidar readings.
The DS controller finds an interior reference point for each obstacle, and
converts each obstacle in the environment to be star-shaped.
$\Gamma$-functions are then defined for these obstacles and used to compute
the modulation matrix $M$. App. H contains additional implementation details.

Straight-Line Deviation In most cases, the robot cannot navigate directly to
the target in a straight line. Thus, the collision-avoidance behavior is a
crucial aspect for navigation robots. To understand it, we sample obstacles
that lead to trajectories minimally deviating from the straight line path.
Since the deviation is always non-negative, we use the matching mode in Eq. 2
with target $b^{*}=0$.

Figure 5: Top: Posterior trajectories in orange vs. prior in blue for minimal
straight-line deviation behavior for three controllers. Bottom: Posterior
obstacle distribution relative to the prior. Higher obstacle density regions
are painted in red and lower ones in blue.

In Fig. 5, the top row plots posterior trajectories in orange, with prior
trajectories in blue. The bottom row plots the obstacle distributions compared
to the prior, with red regions being more likely to be occupied by obstacles
and blue ones less likely to be obstructed.

For DS and RRT, the posterior trajectories and obstacle configurations are
mostly symmetric with respect to the straight-line connection, as expected
since both methods are formulated symmetrically with respect to the $x$\- and
$y$-coordinates. The obstacle distribution under RRT is also expected, since
it seeks straight-line connections whenever possible and thus favor a
“diagonal corridor” with obstacles on either side. For DS, however, obstacles
are slightly more likely to exist at the two ends of the above-mentioned
corridor. This behavior is an artifact of the DS _tail effect_ , which drags
the robot around the obstacle (details in App. D). By taking advantage of
anchor-like obstacles at the ends of the corridor, the modulation can reliably
minimize the straight-line deviation.

By comparison, the IL controller saliently exhibits trajectory asymmetry: it
mostly takes paths on the left. It is possible that the asymmetry is due to
“unlucky” samples by the MH sampler, but many independent restarts all confirm
its presence, indicating that the asymmetry is inherent in the learned model.
Since the neural network architecture is symmetric, we conclude that the
stochasticity in the dataset generation and training procedure (e.g.
initialization) leads to such imbalanced behaviors. Furthermore, the obstacle
map suggests that obstacles are distributed very close to the robot path. Why
does the robot seem to drive into obstacles? The answer lies in dataset
generation: the smoothing procedure (Fig. 4 middle) results in most
demonstrated paths navigating tightly around obstacles, and it is thus
expected that the learned IL controller displays the same behavior.

Takeaways RoCUS reveals two unexpected phenomena. First, IL trajectories are
highly asymmetric toward the left of the obstacle due to dataset and/or
training imbalance. Second, both DS and IL models exhibit certain “obstacle-
seeking” behaviors, the former due to the “tail-effect” and the latter due the
dataset generation process. In both cases, such behavior may be undesirable in
deployment due to possibly imprecise actuation, and the controller design may
need to be modified. Additional studies on legibility and obstacle clearance
behaviors are presented in App. I.

###  5.3 7DoF Arm Reaching Task Experiments

Setup A 7DoF Franka Panda arm is mounted on the side of a table with a
T-shaped divider (Fig. 1 right). Starting from the same initial configuration
on top of the table, it needs to reach a random location on either side under
the divider. We simulate this task in PyBullet [28]. We consider three
controllers: an RRT planner, a deep RL PPO agent, and a DS formulation.

RRT again implements Algorithm 2, but uses inverse kinematics (IK) to first
find the joint configuration corresponding to the target location. The RL
controller is a multi-layer perceptron (MLP) network trained using the PPO
algorithm. The DS model outputs the end-effector trajectory in the task space,
which is converted to joint space via IK, with SVM-learned obstacle
definitions. App. J contains additional implementation details for each
method. Overall, RRT and RL are quite successful in reaching the target while
the DS is not due to the bulky robot structure, close proximity to the
divider, and the task-space only modulation.

Figure 6: Left: Minimal end-effector movement samples for RRT and RL. Right:
Posterior samples for minimal distance from end-effector to target for the
original and improved DS controllers. Top: posterior targets locations, with
tabletop + divider in green and target region in orange. Bottom: posterior
trajectories in red, prior trajectories in blue. Robot is mounted on the near
long edge.

End-Effector Movement We find configurations that minimize the total travel
distance of the end-effector for RRT and RL (DS omitted due to high failure
rate). Fig. 6 (left two) shows the posterior target locations and
trajectories. Notably, unlike RL, RRT trajectories are highly asymmetric,
since there are straight-line connections in the configuration space from the
initial pose to some target regions on the left, while every right-side goal
requires at least an intermediate node.

DS Improvement with RoCUS Our initial DS implementation frequently fails to
reach the target. This is understandable, as the DS convergence guarantee [7]
is only valid in task space, in which the modulation is defined. When the
full-arm motion is solved via IK, it is possible that some body parts may
collide and get stuck because of the table divider. To understand the DS
behaviors, we use RoCUS to sample target locations that result in minimal
final distance from the end-effector to the target (i.e., most successful
executions, Fig. 6 center-right). Similar to the RRT case, the samples show
strong lateral asymmetry, with all posterior target locations on the left, due
to the same cause of asymmetric kinematic structure. The result points to a
clear path to improve the DS controller such that it can succeed with right-
side targets: increase the collision clearance of the divider so that the end-
effector navigates farther away from the divider, thus also bringing the whole
arm to be farther away. As detailed in App. K, this modification greatly
improves the controller performance as confirmed by the new symmetry in Fig. 6
(rightmost). In addition, since the issue with DS controller mainly lies in
obstacle avoidance in joint-space or on the body of the robot, additional
techniques [29, 30, 31, 32] could be used and we leave them to future
directions.

Takeaway The set of studies reveal an important implication of the robot’s
kinematic structure: the left side is much less “congested” with obstacles
than the right side in the configuration space. While the RL controller is
able to learn efficient policies for both sides, the design of certain
controllers may need to explicitly consider such factors. App. K includes an
additional study on legibility.

###  5.4 Quantitative Summary

We studied other additional behaviors on both tasks, and Tab. 1 summarizes
prior vs. posterior mean behavior values and shows that RoCUS consistently
finds samples salient in the target behavior.

Domain | Behavior | Target | Prior (DS) | Post. (DS) | Prior (IL/RL) | Post. (IL/RL) | Prior (RRT) | Post. (RRT)
---|---|---|---|---|---|---|---|---
2D Nav | Avg. Jerk | 0 | 1.84e-3 | 1.46e-3 | 6.95e-4 | 3.19e-4 | 4.24e-4 | 2.79e-4
Straight | 0 | 0.256 | 0.084 | 0.378 | 0.301 | 0.470 | 0.162
Legibility | min | 0.819 | 0.650 | 0.877 | 0.784 | 0.798 | 0.669
Obstacle | 0 | 0.309 | 0.229 | 0.262 | 0.218 | 0.312 | 0.241
Obstacle | max | 0.309 | 0.611 | 0.262 | 0.387 | 0.312 | 0.442
Arm | Straight | 0 | 0.980 | 0.913 | 0.858 | 0.762 | 1.223 | 0.897
EE Dist | 0 | 0.934 | 0.623 | 0.958 | 0.691 | 3.741 | 1.192

Table 1: Quantitative results on additional tasks for two domains.

##  6 MCMC Sampling Evaluation

Figure 7: 2D navigation DS min straight-line deviation samples.

After confirming that RoCUS can indeed uncover significant and actionable
controller insights, we evaluate the sampling procedure itself, using tasks
described above as examples.

Mixing Property A potential downside of MCMC sampler is the slow mixing time,
which causes the chain to take a long time to converge from initialization and
causes consecutive samples to be highly correlated. Does this phenomenon
happen for our sampling? Fig. 7 plots the behavior along the MCMC iterations
for the DS minimal straight-line deviation behavior, showing that the chain
mixes well quite fast (additional ones in Fig. 10 of App. F). Thus, a modest
amount of samples, such as several thousand, is typically sufficient to model
the target posterior distribution well.

Baseline: Top-$k$ Selection To the best of our knowledge, RoCUS is the first
work that applies the transparency-by-example formulation [12] to robotic
tasks, and we are not aware of existing methods for the same purpose. Notably,
adversarial perturbation algorithms [5] are not feasible, since stepping in
simulator (or real world) is not typically differentiable. Sec. 1 discusses a
straightforward alternative that runs the controller on $N$ different
scenarios and pick the top-$k$ with respect to the target behavior. We
demonstrate its shortcomings on the minimal straight-line deviation behavior
for the 2D navigation DS controller (RoCUS samples shown in Fig. 5 left).

Fig. 8 (left) shows the trajectories of different values of $k$ for the same
fixed $N$, and vice versa. While a bigger $N/k$ ratio leads to more salient
behaviors in the top-$k$ samples, these examples become more concentrated
around the global maximum and less diverse, making this approach especially
myopic. Further, it is not easy to find the optimal $N$ to trade off between
diversity and saliency of the top-$k$ samples. By contrast, RoCUS offers the
intuitive $\alpha$ hyper-parameter. Fig. 8 (middle) shows that a smaller $N$
fails to highlight the “corridor” pattern while a larger $N$ makes it
completely open and misses the “tail-effect anchors” at the two ends.

In addition, the hard cut-off at the $k$-th salient behavior threshold has two
undesirable implications: first, every trajectory more salient than the
threshold is kept but is given equal importance; second, a trajectory even
slightly under the threshold is strictly discarded. By comparison, RoCUS gives
more importance to more salient samples in a progressive manner, as shown in
Fig. 8 right.

Finally, top-$k$ selection is very computationally inefficient. It discards
all of the unselected $N-k$ samples, while RoCUS is much more efficient in
that all samples after the burn-in up to the thinning factor can be kept since
the posterior concentrated on the salient behavior is directly sampled.

Figure 8: Top-$k$ selection baseline. Left two: trajectory distribution;
middle two: obstacle distribution; right one: probability density function of
behavior values.

##  7 Discussion and Future Work

RoCUS enables humans to build better mental models of robot controllers.
Compared to existing evaluations on task-completion metrics for hand-designed
tasks, RoCUS generates tasks and trajectories that highlight any given
behavior in a principled way. We used it to uncover non-obvious insights in
two domains and help with debugging and improving a controller.

While RoCUS is mainly a tool to analyze robot controllers in simulation as
part of comprehensive testing before deployment, it can help understanding
(anomalous) real world behaviors as well. When an anomaly is observed, RoCUS
can find more samples with the anomaly for developers to identify patterns of
systematic failures. Furthermore, RoCUS is not inherently limited to
simulation: it only requires trajectory roll-out on specific tasks. For the
arm reaching task, this is easy in the real world. For autonomous driving,
“recreating” a traffic condition that involves other vehicles may be hard.
However, a key feature of RoCUS is the decoupling of the task and the
controller algorithm, which allows testing on simpler task variants (e.g. with
props instead of real cars).

There are multiple directions for future work, including evaluation of model
updates [33] by defining behavior functions on two controllers, better
understanding the samples with explainable artificial intelligence (XAI)
methods, and an appropriate interface to facilitate the two-way communication
between RoCUS and end-users, as discussed in detail in App. L.

Overall, RoCUS is a framework for systematic discovery and inspection of
robotic controller behaviors. We hope that the demonstrated utility of RoCUS
sparks further efforts towards the development of other tools for more
holistic understanding of robot controllers.

## Acknowledgement

This research is supported by the National Science Foundation (NSF) under the
grant IIS-1830282. We thank the reviewers for their reviews, which are
available at https://openreview.net/forum?id=5P_3bRWiRsF.

## References

  * Bort [2018] J. Bort.  Inside Uber before its self-driving car killed a pedestrian: Sources describe infighting, ‘perverse’ incentives, and questionable decisions, 2018.  URL https://www.businessinsider.com/sources-describe-questionable-decisions-and-dysfunction-inside-ubers-self-driving-unit-before-one-of-its-cars-killed-a-pedestrian-2018-10.
  * Rasouli and Tsotsos [2019] A. Rasouli and J. K. Tsotsos.  Autonomous vehicles that interact with pedestrians: A survey of theory and practice.  _IEEE Transactions on Intelligent Transportation Systems (T-ITS)_ , 21(3):900–918, 2019.
  * Bobu et al. [2020] A. Bobu, A. Bajcsy, J. F. Fisac, S. Deglurkar, and A. D. Dragan.  Quantifying hypothesis space misspecification in learning from human–robot demonstrations and physical corrections.  _IEEE Transactions on Robotics (T-RO)_ , 36(3):835–854, 2020.
  * Myers et al. [2004] G. J. Myers, T. Badgett, T. M. Thomas, and C. Sandler.  _The Art of Software Testing_ , volume 2.  Wiley Online Library, 2004.
  * Goodfellow et al. [2015] I. J. Goodfellow, J. Shlens, and C. Szegedy.  Explaining and harnessing adversarial examples.  In _International Conference on Learning Representation (ICLR)_ , 2015\.
  * Argall et al. [2009] B. D. Argall, S. Chernova, M. Veloso, and B. Browning.  A survey of robot learning from demonstration.  _Robotics and Autonomous Systems (RAS)_ , 57(5):469–483, 2009.
  * Huber et al. [2019] L. Huber, A. Billard, and J.-J. Slotine.  Avoidance of convex and concave obstacles with convergence ensured through contraction.  _IEEE Robotics and Automation Letters (RA-L)_ , 4(2):1462–1469, 2019.
  * LaValle [2006] S. M. LaValle.  _Planning Algorithms_.  Cambridge University Press, 2006.
  * Sutton and Barto [2018] R. S. Sutton and A. G. Barto.  _Reinforcement Learning: An Introduction_.  MIT press, 2018.
  * Mitchell et al. [2019] M. Mitchell, S. Wu, A. Zaldivar, P. Barnes, L. Vasserman, B. Hutchinson, E. Spitzer, I. D. Raji, and T. Gebru.  Model cards for model reporting.  In _Conference on Fairness, Accountability, and Transparency (FAT $\,{}^{*}$)_, pages 220–229, 2019.
  * Ribeiro et al. [2020] M. T. Ribeiro, T. Wu, C. Guestrin, and S. Singh.  Beyond accuracy: Behavioral testing of nlp models with checklist.  In _58th Annual Meeting of the Association for Computational Linguistics (ACL)_ , pages 4902–4912, 2020.
  * Booth et al. [2021] S. Booth, Y. Zhou, A. Shah, and J. Shah.  Bayes-trex: a bayesian sampling approach to model transparency by example.  In _AAAI Conference on Artificial Intelligence (AAAI)_ , volume 35, pages 11423–11432, 2021.
  * Fan et al. [2020] C. Fan, X. Qin, and J. Deshmukh.  Parameter searching and partition with probabilistic coverage guarantees.  _arXiv preprint arXiv:2004.00279_ , 2020.
  * Hayes and Shah [2017] B. Hayes and J. A. Shah.  Improving robot controller transparency through autonomous policy explanation.  In _ACM/IEEE International Conference on Human-Robot Interaction (HRI)_ , pages 303–312. IEEE, 2017.
  * Lage et al. [2019] I. Lage, D. Lifschitz, F. Doshi-Velez, and O. Amir.  Exploring computational user models for agent policy summarization.  In _International Joint Conference on Artificial Intelligence (IJCAI)_ , pages 1401–1407, 2019.
  * Rahwan et al. [2019] I. Rahwan, M. Cebrian, N. Obradovich, J. Bongard, J.-F. Bonnefon, C. Breazeal, J. W. Crandall, N. A. Christakis, I. D. Couzin, M. O. Jackson, et al.  Machine behaviour.  _Nature_ , 568(7753):477–486, 2019.
  * Mahler et al. [2018] J. Mahler, R. Platt, A. Rodriguez, M. Ciocarlie, A. Dollar, R. Detry, M. A. Roa, H. Yanco, A. Norton, J. Falco, et al.  Guest editorial open discussion of robot grasping benchmarks, protocols, and metrics.  _IEEE Transactions on Automation Science and Engineering (T-ASE)_ , 15(4):1440–1442, 2018.
  * Murali et al. [2019] A. Murali, T. Chen, K. V. Alwala, D. Gandhi, L. Pinto, S. Gupta, and A. Gupta.  PyRobot: An open-source robotics framework for research and benchmarking.  _arXiv preprint arXiv:1906.08236_ , 2019.
  * James et al. [2020] S. James, Z. Ma, D. R. Arrojo, and A. J. Davison.  Rlbench: The robot learning benchmark & learning environment.  _IEEE Robotics and Automation Letters (RA-L)_ , 5(2):3019–3026, 2020.
  * Anderson et al. [2018] P. Anderson, A. Chang, D. S. Chaplot, A. Dosovitskiy, S. Gupta, V. Koltun, J. Kosecka, J. Malik, R. Mottaghi, M. Savva, et al.  On evaluation of embodied navigation agents.  _arXiv preprint arXiv:1807.06757_ , 2018.
  * Cohen et al. [2012] B. Cohen, I. A. Şucan, and S. Chitta.  A generic infrastructure for benchmarking motion planners.  In _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , pages 589–595. IEEE, 2012.
  * Moll et al. [2015] M. Moll, I. A. Sucan, and L. E. Kavraki.  Benchmarking motion planning algorithms: An extensible infrastructure for analysis and visualization.  _IEEE Robotics & Automation Magazine (RA-M)_, 22(3):96–102, 2015.
  * Lagriffoul et al. [2018] F. Lagriffoul, N. T. Dantam, C. Garrett, A. Akbari, S. Srivastava, and L. E. Kavraki.  Platform-independent benchmarks for task and motion planning.  _IEEE Robotics and Automation Letters (RA-L)_ , 3(4):3765–3772, 2018.
  * Lemme et al. [2015] A. Lemme, Y. Meirovitch, M. Khansari-Zadeh, T. Flash, A. Billard, and J. J. Steil.  Open-source benchmarking for learned reaching motion generation in robotics.  _Paladyn, Journal of Behavioral Robotics_ , 6(1), 2015.
  * Dragan et al. [2013] A. D. Dragan, K. C. Lee, and S. S. Srinivasa.  Legibility and predictability of robot motion.  In _ACM/IEEE International Conference on Human-Robot Interaction (HRI)_ , pages 301–308. IEEE, 2013.
  * Hastings [1970] W. K. Hastings.  Monte carlo sampling methods using markov chains and their applications.  _Biometrika_ , 57(1), 1970.
  * Schulman et al. [2017] J. Schulman, F. Wolski, P. Dhariwal, A. Radford, and O. Klimov.  Proximal policy optimization algorithms.  _arXiv preprint arXiv:1707.06347_ , 2017.
  * Coumans and Bai [2016–2021] E. Coumans and Y. Bai.  PyBullet, a python module for physics simulation for games, robotics and machine learning.  http://pybullet.org, 2016–2021.
  * Khatib [1985] O. Khatib.  Real-time obstacle avoidance for manipulators and mobile robots.  In _IEEE International Conference on Robotics and Automation (ICRA)_ , volume 2, pages 500–505. IEEE, 1985.
  * Ratliff et al. [2018] N. D. Ratliff, J. Issac, D. Kappler, S. Birchfield, and D. Fox.  Riemannian motion policies.  _arXiv preprint arXiv:1801.02854_ , 2018.
  * Mirrazavi Salehian et al. [2018] S. S. Mirrazavi Salehian, N. Figueroa, and A. Billard.  A unified framework for coordinated multi-arm motion planning.  _The International Journal of Robotics Research (IJRR)_ , 37(10):1205–1232, 2018.
  * Urain et al. [2021] J. Urain, P. Liu, A. Li, C. D’Eramo, and J. Peters.  Composable Energy Policies for Reactive Motion Generation and Reinforcement Learning.  In _Robotics: Science and Systems (RSS)_ , Virtual, 2021.
  * Bansal et al. [2019] G. Bansal, B. Nushi, E. Kamar, D. S. Weld, W. S. Lasecki, and E. Horvitz.  Updates in human-AI teams: Understanding and addressing the performance/compatibility tradeoff.  In _AAAI Conference on Artificial Intelligence (AAAI)_ , volume 33, pages 2429–2437, 2019.
  * Brooks et al. [2011] S. Brooks, A. Gelman, G. Jones, and X.-L. Meng.  _Handbook of Markov Chain Monte Carlo_.  CRC press, 2011.
  * Khansari-Zadeh and Billard [2012] S. M. Khansari-Zadeh and A. Billard.  A dynamical system approach to realtime obstacle avoidance.  _Autonomous Robots (AuRo)_ , 32(4):433–454, 2012\.
  * Zhou and Hauser [2017] Y. Zhou and K. Hauser.  6DOF grasp planning by optimizing a deep learning scoring function.  In _RSS Workshop on Revisiting Contact – Turning a Problem into a Solution_ , 2017.
  * Xu et al. [2017] H. Xu, Y. Gao, F. Yu, and T. Darrell.  End-to-end learning of driving models from large-scale video datasets.  In _IEEE conference on computer vision and pattern recognition (CVPR)_ , pages 2174–2182, 2017.
  * Hauser [2016] K. Hauser.  Robust contact generation for robot simulation with unstructured meshes.  In _16th International Symposium on Robotics Research (ISRR)_ , volume 114, page 357. Springer, 2016.
  * Karaman and Frazzoli [2011] S. Karaman and E. Frazzoli.  Sampling-based algorithms for optimal motion planning.  _The International Journal of Robotics Research (IJRR)_ , 30(7):846–894, 2011.
  * Hauser and Zhou [2016] K. Hauser and Y. Zhou.  Asymptotically optimal planning by feasible kinodynamic planning in a state–cost space.  _IEEE Transactions on Robotics (T-RO)_ , 32(6):1431–1443, 2016.
  * Greydanus et al. [2018] S. Greydanus, A. Koul, J. Dodge, and A. Fern.  Visualizing and understanding Atari agents.  In _International Conference on Machine Learning (ICML)_ , pages 1792–1801. PMLR, 2018.
  * Zahavy et al. [2016] T. Zahavy, N. Ben-Zrihem, and S. Mannor.  Graying the black box: Understanding DQNs.  In _International Conference on Machine Learning (ICML)_ , pages 1899–1908. PMLR, 2016.
  * Atrey et al. [2019] A. Atrey, K. Clary, and D. Jensen.  Exploratory not explanatory: Counterfactual analysis of saliency maps for deep reinforcement learning.  In _International Conference on Learning Representations (ICLR)_ , 2019.
  * Zhou et al. [2021] Y. Zhou, S. Booth, M. T. Ribeiro, and J. Shah.  Do feature attribution methods correctly attribute features?  _arXiv preprint arXiv:2104.14403_ , 2021.

##  Appendix A Scale-Invariance and “Volume” Interpretation of $\alpha$

We show that Eq. 3 results in the formulation being scale-invariant with
respect to $b$. Consider the same behavior under two different units $b_{1}$
and $b_{2}$ with $b_{1}=c\cdot b_{2}$. For example, $b_{1}$ can be the
trajectory length in centimeters and $b_{2}$ is the same quantity but in
meters, and $c=100$. Thus, $p(c\cdot b_{1})=p(b_{2})$ and $b_{1}^{*}=c\cdot
b_{2}^{*}$. To maintain the same $\alpha$ level in Eq. 3, we need to have
$\sigma_{1}=c\cdot\sigma_{2}$. This implies that

$\displaystyle
p(t,\tau|\hat{b}_{1}=b_{1}^{*})=\frac{\mathcal{N}(b_{1}^{*};b(\tau,t),\sigma_{1}^{2})p(\tau|t)\pi(t)}{p(\hat{b}_{1}=b_{1}^{*})}$
(6) $\displaystyle=$
$\displaystyle\frac{\mathcal{N}(b_{2}^{*};b(\tau,t),\sigma_{2}^{2})p(\tau|e)\pi(t)}{p(\hat{b}_{2}=b_{2}^{*})}=p(t|\hat{b}_{2}=b_{2}^{*})$
(7)

because
$\mathcal{N}(b_{1}^{*};b(\tau,t),\sigma_{1}^{2})=\mathcal{N}(b_{2}^{*};b(\tau,t),\sigma_{2}^{2})$
due to the same scaling of $b_{1}\sim b_{2}$ and $\sigma_{1}\sim\sigma_{2}$,
and $p(\hat{b}_{1}=b_{1}^{*})=p(\hat{b}_{2}=b_{2}^{*})$ as they are the same
event. We conclude that the posterior distribution is scale-invariant with
respect to $b(\tau,t)$.

To motivate the bound of $[b^{*}-\sqrt{3}\sigma,b^{*}+\sqrt{3}\sigma]$ in Eq.
3, we consider a uniform approximation to $\mathcal{N}(b^{*},\sigma^{2})$. To
match the mean $b^{*}$ and standard deviation $\sigma$,
$\mathcal{U}(b^{*}-\sqrt{3}\sigma,b^{*}+\sqrt{3}\sigma)$ is needed. If we use
this uniform distribution in Eq. 2 in lieu of the normal distribution, the
posterior can be instantiated by sampling from the prior and rejecting tasks
for which the trajectory behavior $b(\tau,t)$ falls outside of this bound.
Thus, Eq. 3 specifies that the “volume” of $(\alpha\cdot 100)\%$ under
$p(t,\tau)$ is maintained.

The same invariance and “volume” interpretation holds for Eq. 5 as well. The
former stems from the standardization on $b$ performed in Eq. 4. The latter
uses the same uniform approximation but the bound is one-sided since
$\beta\in(0,1)$ by nature of the sigmoid transformation.

##  Appendix B MCMC Sampling with Stochastic Dynamics

Using the same logic as the case of stochastic controller, RoCUS can also
accommodate stochasticity in transition dynamics (e.g. object position
uncertainty after it is pushed), as long as such stochasticity can be captured
in a random variable $v$ and $p(v|t)$ can be evaluated. This is typically
possible in simulation, and the modification to Alg. 1 is similar to the case
of stochastic controllers. In the real world, we can

  * •

treat a sampled trajectory as the deterministic one;

  * •

restart multiple times to estimate $\mathbb{E}_{\tau}[b(\tau,t)]$; or

  * •

use likelihood-free MCMC methods [34].

We leave these investigations to future work, and use deterministic dynamics
in our experiments.

##  Appendix C Mathematical Definitions of Behaviors

A versatile and general form of a behavior is the (normalized or unnormalized)
line integral of some scalar field along the trajectory. Specifically, we have

$\displaystyle b=\int_{\tau}V(\mathbf{x})\>\mathrm{d}s\quad\mathrm{or}\quad
b=\frac{1}{||\tau||}\int_{\tau}V(\mathbf{x})\>\mathrm{d}s.$ (8)

Using this general definition, we define a list of behaviors in Tab. 2.

Name | Definition | Name | Definition
---|---|---|---
Trajectory Length | $\displaystyle b=\int_{\tau}1\>\mathrm{d}s$ | Straight-Line Deviation | $\displaystyle b=\frac{1}{||\tau||}\int_{\tau}||\mathbf{x}-\mathrm{proj}_{\mathbf{x}_{f}-\mathbf{x}_{i}}\mathbf{x}||\>\mathrm{d}s$
Average Velocity | $\displaystyle b=\frac{1}{||\tau||}\int_{\tau}||\dot{\mathbf{x}}||\>\mathrm{d}s$ | Obstacle Clearance | $\displaystyle b=\frac{1}{||\tau||}\int_{\tau}\min_{\mathbf{x}_{o}\in\mathcal{O}}||\mathbf{x}-\mathbf{x}_{o}||\>\mathrm{d}s$
Average Acceleration | $\displaystyle b=\frac{1}{||\tau||}\int_{\tau}||\ddot{\mathbf{x}}||\>\mathrm{d}s$ | Near-Obstacle Velocity | $\displaystyle b=\frac{\int_{\tau}||\dot{\mathbf{x}}||/\min_{\mathbf{x}_{o}\in\mathcal{O}}||\mathbf{x}-\mathbf{x}_{o}||\>\mathrm{d}s}{\int_{\tau}1/\min_{\mathbf{x}_{o}\in\mathcal{O}}||\mathbf{x}-\mathbf{x}_{o}||\>\mathrm{d}s}$
Average Jerk | $\displaystyle b=\frac{1}{||\tau||}\int_{\tau}||\dddot{\mathbf{x}}||\>\mathrm{d}s$ | Motion Legibility | $\displaystyle b=\frac{1}{||\tau||}\int_{\tau}p(g|\mathbf{x})\>\mathrm{d}s$

Table 2: A list of behavior definitions.

Trajectory length simply measures how long the trajectory is. In most of the
behaviors below, the normalizing factor is also length to decorrelate the
behavior value from it.

Average velocity, acceleration and jerk are useful for a general understanding
about how fast and abruptly the robot moves, which is an important factor to
its safety.

Straight-line deviation measures how much the robot trajectory deviates from
the straight-line path, in either the task space or the state space. A
specific task instance in which the straight-line path is feasible (e.g. with
no obstacles) is typically considered easy. Thus, we can find tasks of varying
difficulty level on the spectrum of deviation values. In the definition,
$\mathbf{x}_{i}$ is the initial state, $\mathbf{x}_{f}$ is the final state,
and $\mathrm{proj}$ is the projection operator.

Obstacle clearance measures the average distance to the closest obstacle.
Finding situations in which the robot moves very close to obstacles is crucial
to understanding the collision risk level. In the definition, $\mathcal{O}$
represents the obstacle space.

Near-obstacle velocity calculates how fast the robot moves around obstacles.
We define it as the average velocity on the trajectory weighted by the inverse
distance to the closest obstacle. Other weighting method can be used, as long
as it is non-negative and monotonically decreasing with distance. This
behavior is correlated with the damage of a potential collision, as high-speed
collisions are usually far more dangerous and costly. Since we want the value
to represent the average velocity, we normalize by the integral of weights
along the trajectory.

Motion legibility measures how well the goal can be predicted over the course
of the exhibited trajectory. In our definition, we use $p(g|\mathbf{x})$, or
the conditional probability of the goal $g$ given at the current robot state
$\mathbf{x}$, but there may be better application-specific definitions.

##  Appendix D Dynamical System Modulation

We review the DS formulation proposed by Huber et al. [7], and present our
problem-specific adaptations for 2D Navigation in App. H.2 and 7DoF arm
reaching in App. J.3. A reader familiar with DS motion controllers may skip
this review.

Given a target $\mathbf{x}^{*}$ and the robot’s current state $\mathbf{x}$, a
linear controller $\mathbf{u}(x)=\mathbf{x}^{*}-\mathbf{x}$ will guarantee
convergence of $\mathbf{x}$ to $\mathbf{x}^{*}$ if there are no obstacles.
However, it can easily get stuck in the presence of obstacles. Huber et al.
[7] proposes a method to calculate a modulation matrix $M(\mathbf{x})$ at
every $\mathbf{x}$ such that if the new controller follows
$\mathbf{u}_{M}(\mathbf{x})=M(\mathbf{x})\cdot\mathbf{u}(\mathbf{x})$, then
$\mathbf{x}$ still converges to $\mathbf{x}^{*}$ but never gets stuck, as long
as $\mathbf{x}^{*}$ is in free space. In short, the objective of the DS
modulation is to preserve the linear controller’s convergence guarantee while
also ensuring that the robot is never in collision.

The modulation matrix $M(\mathbf{x})$ is computed from a list of obstacles,
each of which is represented by a $\Gamma$-function. For the $i$-th obstacle
$\mathcal{O}_{i}$, its associated gamma function $\Gamma_{i}$ must satisfy the
following properties:

  * •

$\Gamma_{i}(\mathbf{x})\leq 1\iff\mathbf{x}\in\mathcal{O}_{i}$,

  * •

$\Gamma_{i}(\mathbf{x})=1\iff\mathbf{x}\in\partial\mathcal{O}_{i}$,

  * •

$\exists\,\mathbf{r}_{i},\mathrm{s.t.}\forall\,t_{1}\geq t_{2}\geq
0,\forall\,\mathbf{u},\Gamma_{i}(\mathbf{r}_{i}+t_{1}\mathbf{u})\geq\Gamma_{i}(\mathbf{r}_{i}+t_{2}\mathbf{u})$.

In words, the $\Gamma$-function value needs to be less than 1 when inside the
obstacle, equal to 1 on the boundary, greater than 1 when outside. This
function must also be monotonically increasing radially outward from a
specific point $\mathbf{r}_{i}$. This point is dubbed the _reference point_.
From this formulation, $\mathbf{r}_{i}\in\mathcal{O}_{i}$ and any ray from
$\mathbf{r}_{i}$ intersects with the obstacle boundary
$\partial\mathcal{O}_{i}$ exactly once. The latter property is also the
definition that $\mathcal{O}_{i}$ is “star-shaped” (Fig. 12). For most common
(2D) geometric shape such as rectangles, circles, ellipses, regular polygons
and regular stars, $\mathbf{r}_{i}$ can be chosen as the geometric center.

We first consider the case of a single obstacle $\mathcal{O}$, represented by
$\Gamma$ with reference point $\mathbf{r}$. Use $d$ to denote the dimension of
the space. We define

$\displaystyle M(\mathbf{x})=E(\mathbf{x})D(\mathbf{x})E^{-1}(\mathbf{x}).$
(9)

We have

$\displaystyle
E(x)=[\mathbf{s}(\mathbf{x}),\allowbreak\mathbf{e}_{1}(\mathbf{x}),\allowbreak...,\allowbreak\mathbf{e}_{d-1}(\mathbf{x})],$
(10)

where

$\displaystyle\mathbf{s}(\mathbf{x})=\frac{\mathbf{x}-\mathbf{r}}{||\mathbf{x}-\mathbf{r}||}$
(11)

is the unit vector in the direction of $\mathbf{x}$ from $\mathbf{r}$, and
$\mathbf{e}_{1}(\mathbf{x}),\allowbreak...,\allowbreak\mathbf{e}_{d-1}(\mathbf{x})$
form a $d-1$ orthonormal basis to the gradient of the $\Gamma$-function,
$\nabla\Gamma(\mathbf{x})$ representing the normal to the obstacle surface.
$D(\mathbf{x})$ is a diagonal matrix whose diagonal entries are
$\lambda_{s},\lambda_{1},...,\lambda_{d-1}$, with

$\displaystyle\lambda_{s}$ $\displaystyle=1-\frac{1}{\Gamma(\mathbf{x})},$
(12) $\displaystyle\lambda_{1},...,\lambda_{d-1}$
$\displaystyle=1+\frac{1}{\Gamma(\mathbf{x})}.$ (13)

each eigenvalue determines the scaling of each direction. Conceptually, as the
robot approaches the obstacle, this modulation decreases the velocity for the
component in the reference point direction (i.e. toward obstacles) while
increases velocity for perpendicular components. The combined effect results
in the robot being deflected away tangent to the obstacle surface.

With $N$ obstacles, we compute the modulation matrix $M_{i}(\mathbf{x})$ for
every obstacle using the procedure above and the individual controllers
$\mathbf{u}_{M_{i}}(\mathbf{x})=M_{i}(\mathbf{x})\cdot\mathbf{u}(\mathbf{x})$.
The final modulation is the aggregate of all the individual modulations.
However, a simple average is insufficient since closer obstacles should have
higher influence to prevent collisions.

Huber et al. [7] proposed the following aggregation procedure. Let
$\mathbf{u}_{i}$ denote the individual modulations, with norms $n_{i}$. The
final aggregate modulation $\mathbf{u}$ is calculated as

$\displaystyle\mathbf{u}=n_{a}\mathbf{u}_{a},$ (14)

where $n_{a}$ and $\mathbf{u}_{a}$ are the aggregate norm and direction.

The aggregate norm is computed as

$\displaystyle n_{a}$ $\displaystyle=\sum_{i=1}^{N}w_{i}n_{i},$ (15)
$\displaystyle w_{i}$ $\displaystyle=\frac{b_{i}}{\sum_{j=1}^{N}b_{j}},$ (16)
$\displaystyle b_{i}$ $\displaystyle=\prod_{1\leq j\leq N,j\neq
i}\Gamma_{j}(\mathbf{x}).$ (17)

The above definition ensures that $\sum_{i=1}^{N}w_{i}=1$, and
$w_{i}\rightarrow 1$ when $\mathbf{x}$ approaches $\mathcal{O}_{i}$ (and only
$\mathcal{O}_{i}$, which holds as long as obstacles are disjoint).

$\mathbf{u}_{a}$ is instead computed using what Huber et al. [7] calls
“$\boldsymbol{\kappa}$-space interpolation.” First, similar to the basis
vector matrix $E(\mathbf{x})$ introduced above, we construct another such
matrix, but with respect to the original controller
$\mathbf{x}^{*}-\mathbf{x}$. We denote it as
$R=[(\mathbf{x}^{*}-\mathbf{x})/||\mathbf{x}^{*}-\mathbf{x}||,\mathbf{e}_{1},...,\mathbf{e}_{d-1}]$,
where $\mathbf{e}_{1},...,\mathbf{e}_{d-1}$ are again orthonomal vectors
spanning the null space.

For each $\mathbf{u}_{i}$, we compute its coordinate in this new $R$-frame as
$\hat{\mathbf{u}}_{i}=R^{-1}\mathbf{u}_{i}$. Its $\boldsymbol{\kappa}$-space
representation is

$\displaystyle\boldsymbol{\kappa}_{i}=\frac{\arccos(\hat{\mathbf{u}}_{i}^{(1)})}{\sum_{m=2}^{d}\hat{\mathbf{u}}_{i}^{(m)}}\left[\hat{\mathbf{u}}_{i}^{(2)},...,\hat{\mathbf{u}}_{i}^{(d)}\right]^{T}\in\mathbb{R}^{d-1},$
(18)

where the superscript $(m)$ refers to the $m$-th entry.
$\boldsymbol{\kappa}_{i}$ is a scaled version of the $\hat{\mathbf{u}}_{i}$
with the first entry removed. We perform the aggregation in this
$\boldsymbol{\kappa}$-space using the weights $w_{i}$ calculated above (19),
transform it back to the $R$-frame (20), and finally transform it back to the
original frame (21):

$\displaystyle\boldsymbol{\kappa}_{a}$
$\displaystyle=\sum_{i=1}^{N}w_{i}\boldsymbol{\kappa}_{i}$ (19)
$\displaystyle\hat{\mathbf{u}}_{a}$
$\displaystyle=\left[\cos(||\boldsymbol{\kappa}_{a}||),\frac{\sin(||\boldsymbol{\kappa}_{a}||)}{||\boldsymbol{\kappa}_{a}||}\boldsymbol{\kappa}_{a}^{T}\right]^{T}$
(20) $\displaystyle\mathbf{u}_{a}$ $\displaystyle=R\hat{\mathbf{u}}_{a}.$ (21)

As mentioned in Eq. 14, the final modulation is
$\mathbf{u}=n_{a}\mathbf{u}_{a}$.

###  D.1 Tail-Effect

An artifact of the above formulation is the “tail-effect,” where the robot is
modulated to go around the obstacle even when it has passed by the obstacle
and the remaining trajectory has no chance of collision under the non-
modulated controller. This effect has been observed by Khansari-Zadeh and
Billard [35] for a related but different type of modulation. Fig. 9,
reproduced from the paper by Khansari-Zadeh and Billard [35, Fig. 7], shows
the tail effect on the left and its removal on the right. This tail effect
induces the placement of obstacles at the end of the “diagonal corridor” as
seen in our straight-line deviation experiments (Fig. 5, left). If desired,
the DS formulation can be modified to remove this effect.

Figure 9: Tail effect (left) and its removal (right), reproduced from Fig. 7
by Khansari-Zadeh and Billard [35]. The target is on the far right side.

##  Appendix E RRT Algorithm Description and Sampling

There are many RRT variants with subtle differences. For clarity, Algorithm 2
presents the version that we use.

Input: Start configuration $s_{0}$, target configuration $s^{*}$.

1 $\mathcal{T}$ $\leftarrow$ tree(root = $s_{0}$);

2 success $\leftarrow$ attempt-grow($\mathcal{T}$, from = $s_{0}$, to =
$s^{*}$);

3 while  _not success_ do

4 $s$ $\leftarrow$ sample-configuration( );

5 $s_{n}$ $\leftarrow$ nearest-neighbor($\mathcal{T}$, $s$);

6 success $\leftarrow$ attempt-grow($\mathcal{T}$, from = $s_{n}$, to = $s$);

7 if  _success_ then

8 success $\leftarrow$ attempt-grow($\mathcal{T}$, from = $s$, to = $s^{*}$);

9

return _path( $\mathcal{T}$, from = $s_{0}$, to = $s^{*}$)_

Algorithm 2 RRT Algorithm

While RRT is stochastic (unlike DS, IL and RL), the entire randomness is
captured by the sequence of C-space samples used to grow the tree, including
failed ones. We call this a growth $g=[s_{1},s_{2},s_{3},...]$. The
probabilistic completeness property of RRT generally assures that the
algorithm will terminate in finite time with probability 1 if a path to the
target exists [8]. Thus, hypothetically, given an infinitely long tape
containing every entry of $g$, we can compute a deterministic trajectory
$\tau=\mathrm{RRT}(s_{0},s^{*},g)$ with a finite number of nodes with
probability 1.

To enable MH inference, we take inspiration from Bayesian nonparametrics: we
instantiate $g$ on an as-needed basis. We start with an empty vector of
$g=[\,]$. When calculating $\mathrm{RRT}(s_{0},s^{*},g)$, if a new point
beyond existing entries of $g$ needs to be sampled, we append it to $g$.
During MH inference, we use a transition kernel that operates element-wise on
instantiated entries of $g$ (i.e. independently perturbing each entry of $g$).
If the transition kernel does not depend on the current $g$ (e.g. drawing
uniformly from the C-space), then past instantiated entries do not even need
to be kept.

Note that RRT trajectories are often smoothed post hoc. Since our main focus
is to evaluate and identify problems for an existing one, we use the original
formulation. Moreover, it is easy to use RoCUS to evaluate model updates (e.g.
original vs smoothed RRT) as discussed in Sec. 7.

##  Appendix F MCMC Sampling Details

We used a truncated Gaussian transition kernel for all experiments. For the
RBF-defined 2D environment, we initialize 15 obstacle points with coordinates
sampled uniformly in $[-0.7,0.7]$. The transition kernel operates
independently on each obstacle coordinate: given the current value of $x$, the
kernel samples a proposal from $\mathcal{N}(\mu=x,\sigma^{2}=0.1^{2})$
truncated to $[-0.7,0.7]$ (and also appropriately scaled). For the arm
reaching task, the target is sampled uniformly from two disjoint boxes, with
the left box at $[-0.5,-0.05]\times[-0.3,0.2]\times[0.65,1.0]$ and the right
box at $[0.05,0.5]\times[-0.3,0.2]\times[0.65,1.0]$. Again, we use the same
transition kernel with $\sigma_{x}=0.1,\sigma_{y}=0.03,\sigma_{z}=0.035$ in
three directions. Again, the distribution is truncated to the valid target
region ($x\in[-0.5,-0.05]\cup[0.05,0.5],y\in[-0.3,0.2],z\in[0.65,1.0]$). In
other words, the transition kernel implicitly allows for the jump across two
box regions.

In addition, the stochastic RRT controller also requires a transition kernel.
As discussed in Sec. 5.1, we initialize its values on an as-needed basis. When
necessary, we sample a configuration uniformly between the lower- and upper-
limit (i.e. $[x_{L},x_{U}]$). For each configuration, the same Gaussian kernel
truncated to $[x_{L},x_{U}]$, and $\sigma=0.1(x_{U}-x_{L})$ is used.

Each sampling run collected 10,000 samples, with the first 5,000 discarded as
burn-in. On a consumer-grade computer with a single GeForce GTX 1080 GPU card
(for neural network-based controllers), the sampling generally takes around 1
to 3 hours. The number of samples and burn-ins are selected fairly
conservatively to ensure representativeness, as Fig. 10 plots the sampled
behavior values in the chain for three analyses and confirms that these
numbers are more than sufficient to ensure proper mixing. Note that RoCUS is
designed to be an offline analysis tool as opposed to be used for real-time
sample generation, and therefore several hours of runtime would be acceptable
in most cases. Furthermore, MCMC sampling is embarrassingly parallel by simply
using multiple chains concurrently, with the only overhead cost being the
discarded burn-in samples.

Figure 10: The sampled behavior values for three MCMC chains. From left to
right, the three panels show DS min straight-line deviation on 2D navigation,
RRT min straight-line deviation on 2D navigation and RL min end-effector
movement on 7DoF arm reaching. The visualization confirms that 10,000
iterations with 5,000 burn-ins are more than sufficient to find representative
samples.

##  Appendix G 2D Environment Details

In this domain, the environment is the area defined as
$[x_{\text{min}},x_{\text{max}}]\times[y_{\text{min}},y_{\text{max}}]$. The
goal is to navigate from $[x_{\text{start}},y_{\text{start}}]$ to
$[x_{\text{goal}},y_{\text{goal}}]$. We define a flexible environment
representation as a summation of radial basis function (RBF) kernels centered
at so-called _obstacle points._ Specifically, given $N_{O}$ obstacle points
$\mathbf{p}_{1},\mathbf{p}_{2},...,\mathbf{p}_{N_{O}}\in\mathbb{R}^{2}$, the
environment is defined as

$\displaystyle
e(\mathbf{p})=\sum_{i=1}^{N_{O}}\exp\left(-\gamma||\mathbf{p}-\mathbf{p}_{i}||_{2}^{2}\right),$
(22)

and each point $\mathbf{p}$ is an obstacle if $e(\mathbf{p})>\eta$, for
$\eta<1$ to ensure each obstacle point $\mathbf{p}_{i}$ is exposed as an
obstacle. Our environments are bounded by $[{-1.2},1.2]\times[{-1.2},1.2]$,
and the goal is to navigate from $[{-1},{-1}]$ to $[1,1]$. $N_{O}=15$ and
$p_{i}$ coordinates are sampled uniformly in $x_{i},y_{i}\in[{-0.7},0.7]$. A
smaller $\gamma$ and $\eta$ makes the obstacles larger and more likely to be
connected; we choose $\gamma=25$ and $\eta=0.9$. Fig. 11 shows random obstacle
configurations demonstrating high diversity in this environment. We also
implement a simple simulator: given the current robot position $[x,y]$ and the
action $[\Delta x,\Delta y]$, the simulator clamps $\Delta x,\Delta y$ to the
range of [-0.03, 0.03], and then moves the robot to $[x+\Delta x,y+\Delta y]$
if there is no collision, and otherwise simulates a frictionless inelastic
collision (i.e. compliant sliding) that moves the robot tangent to the
obstacle. Fig. 11 depicts a randomly selected assortment of 2D environments.
These environments demonstrate the flexibility and diversity of the RBF
environment definition.

Figure 11: An assortment of randomly generated RBF 2D environments, providing
a sense of the diversity generated with this formulation. The green dots are
the environment starting points and the red stars are navigation targets. We
show DS modulation for the first three environments in Fig. 13.

##  Appendix H Implementation Details of 2D Navigation Controllers

###  H.1 IL Controller

The imitation learning controller is a memoryless policy implemented as a
fully connected neural network with two hidden layers of 200 neurons each and
ReLU activations. The input is 18 dimensional, with two dimensions for the
current $(x,y)$ position of the robot, and 16 dimensions for a lidar sensor in
16 equally-spaced directions, with a maximum range of 1. The network predicts
the heading angle $\theta$, and the controller operates on the action of
$[\Delta x,\Delta y]=[0.03\cos\theta,0.03\sin\theta]$.

The network is trained on smoothed RRT trajectories. Specifically, we use the
RRT controller to find and discretize a trajectory. Then the smoothing
procedure repeatedly replaces each point by the mid-point of its two
neighbors, absent collisions. When this process converges, each point on the
trajectory becomes one training data point.

Since only local observations are available and the policy is memoryless, the
robot may get stuck in obstacles, which happens in approximately $10\%$ of the
runs. In addition, while the output target is continuous, a regression
formulation with mean-squared error (MSE) loss is inappropriate, due to
multimodality of the output. For example, when the robot is facing an
obstacle, moving to either left or right would avoid it, but if both
directions appear in the dataset, the MSE loss would drive the prediction to
be the average, resulting in a head-on collision. This problem has been
recognized in other robotic scenarios such as grasping [36] and autonomous
driving [37]. We follow the latter to treat this problem as classification
with 100 bins in the $[0,2\pi]$ range.

###  H.2 DS Controller

For the DS controller, there are two technical challenges in using the
modulation [7] on our RBF-defined environment. First, we need to identify and
isolate each individual obstacle, and second, we need to define a
$\Gamma$-function for each obstacle.

To find all obstacles, we discretize the environment into an occupancy grid of
resolution $150\times 150$ covering the area of $[-1.2,1.2]\times[-1.2,1.2]$.
Then we find connected components using flood fill, and each connected
component is taken to be an obstacle.

To define a $\Gamma$-function for each obstacle, we first choose the reference
point as the center of mass of the connected component. Then we cast 50 rays
in 50 equally spaced directions from the reference point and find the
intersection point of each ray with the boundary of the connected component.
Finally, we connected those intersections in sequence and get a polygon. In
case of multiple intersection points, we take the farthest point as vertex of
the polygon, essentially completing the non-star-shaped obstacle to be star-
shaped, as shown in Fig. 12.

Figure 12: Left: an obstacle which is not star-shaped. Some radial lines
extending from the obstacle’s reference point cross the boundary of the
obstacle twice. Right: the same obstacle, modified to instead be star-shaped.

Given an arbitrary point $\mathbf{x}$, we define

$\displaystyle\Gamma(\mathbf{x})=\frac{||\mathbf{x}-\mathbf{r}||}{||\mathbf{i}-\mathbf{r}||},$
(23)

where $\mathbf{r}$ is the reference point and $\mathbf{i}$ is the intersection
point with the polygon of the ray from $\mathbf{r}$ in $\mathbf{x}-\mathbf{r}$
direction. It is easy to see that this $\Gamma$ definition satisfies all three
requirements for $\Gamma$-functions listed in App. D.

Finally, to compensate for numerical errors in the process (e.g. approximating
obstacles with polygons), we define the control inside obstacle to be the
outward direction, which helps preventing the robot from getting stuck at
obstacle boundaries in practice. Three examples of DS modulation of the 2D
navigation environment are shown in Fig. 13.

Figure 13: Streamlines showing the modulation effect of the dynamical system
for three 2D navigation tasks. The environments correspond to the first three
examples of Fig. 11. Green dots are starting positions and red stars are
navigation targets.

##  Appendix I Additional Results for 2D Navigation

Figure 14: Left: trajectories and obstacle configurations from sampling
minimal DS legibility. Right: obstacle configurations for minimizing and
maximizing DS obstacle clearance. These examples show how obstacle positions
affect the legibility and clearance behaviors.

#### Legibility

We define the instantaneous legibility as the cosine similarity between the
current robot direction and the direction to target $\mathbf{x}^{*}$,
$V(\mathbf{x})=\dot{\mathbf{x}}\cdot(\mathbf{x}^{*}-\mathbf{x})/(||\dot{\mathbf{x}}||\cdot||\mathbf{x}^{*}-\mathbf{x}||)$,
with the intuition that a particular run may be confusing to users if the
robot does not often align to the target. Though this quantity is bounded by
$[-1,1]$, a general legibility definition may not be. Thus, we use the maximal
mode of RoCUS to find DS trajectories and obstacle configurations that achieve
minimal legibility, by negating $V(\mathbf{x})$ first. The left two panels of
Fig. 14 present the samples. As expected, most trajectories take large detours
due to the presence of obstacles in the center.

#### Obstacle Clearance

We take
$V(\mathbf{x})=\min_{\mathbf{x}_{o}\in\mathcal{O}}||\mathbf{x}-\mathbf{x}_{o}||$.
For the DS, we sample two posteriors to maximize and minimize this behavior.
As shown in the right two panels of Fig. 14, when minimizing obstacle
clearance, we see clusters of obstacles in close proximity to the starting and
target positions, such that the robot is forced to navigate around them. When
maximizing obstacle clearance, we instead see central clusters of obstacles,
such that the robot can avoid them by bearing hard left or right.

##  Appendix J Implementation Details of 7DoF Arm Reaching Controllers

###  J.1 RRT Controller

Since the target location is specified in the task space, we first find the
target joint space configuration using inverse kinematics (IK). The initial
configuration starts with the arm positioned down on the same side as the
target. If the IK solution is in collision, we simulate the arm moving to it
using position control, and redefine the final configuration at equilibrium as
the target (i.e. its best effort reaching configuration). We solve the IK
using Klamp’t [38].

###  J.2 RL Controller

The RL controller implements the proximal policy gradient (PPO) algorithm
[27]. The state space is 22-dimensional and consists of the following:

  * •

7D joint configuration of the robot,

  * •

3D position of the end-effector,

  * •

3D roll-pitch-yaw of the end effector,

  * •

3D velocity of the end-effector,

  * •

3D position of the target,

  * •

3D relative position from the end-effector to the target.

The action is 7-dimensional for movement in each joint, which is capped at
$[-0.05,0.05]$.

Both the actor and the critic are implemented with fully connected networks
with two hidden layers of 200 neurons each, and ReLU activations. The action
is parametrized as Gaussian where the actor network predicts the mean, and 7
standalone parameters learns the log variance for each of the 7 action
dimensions. At test time, the policy deterministically outputs the mean action
given a state.

###  J.3 DS Controller

For the DS controller in 7DoF arm reaching, we face the same challenges as in
2D navigation: defining an appropriate $\Gamma$-function for the obstacle
configuration that holds the three properties introduced by Huber et al. [7]
(listed in App. D). Additionally, the DS modulation technique does not
consider the robot’s morphology, end-effector shape, or workspace limits
because it only modulates the state of a point-mass. Thus, we implement
several adaptations. First, we modulate the 3D position of the tip of the end-
effector. The desired velocity of the end-effector tip, given by the modulated
linear controller, is then tracked by the 7DoF arm via the same position-level
IK solver as the RRT controller.

Second, we used a support vector machine (SVM) to learn the obstacle boundary
from a list of points in the obstacle and free spaces, an approach originally
proposed by Mirrazavi Salehian et al. [31]. Then the decision function of the
SVM is used as the $\Gamma$-function. As shown in Fig. 15, we discretize the
3D workspace of the robot and generate a dataset of points in the obstacle
space as negative class and those in the free space as positive class.

Figure 15: Left: the division of 3D space as either containing an obstacle or
free space. This data is used to train an SVM, which acts as an interpolator.
The classification scores of the SVM are used as the $\Gamma$ function for
this 3D reaching task. Right: a 2D slice showing the smoothed $\Gamma$ scores.

Using the radial basis function (RBF) kernel
$K(\mathbf{x}_{1},\mathbf{x}_{2})=e^{-\gamma||\mathbf{x}_{1}-\mathbf{x}_{2}||^{2}}$,
with kernel width $\gamma$, the SVM decision function $\Gamma(\mathbf{x})$ has
the following form:

$\displaystyle\Gamma(\mathbf{x})=\sum_{i=1}^{N_{sv}}\alpha_{i}y_{i}K(\mathbf{x},\mathbf{x}_{i})+b=\sum_{i=1}^{N_{sv}}\alpha_{i}y_{i}e^{-\gamma||\mathbf{x}-\mathbf{x}_{i}||^{2}}+b,$
(24)

and the equation for $\nabla\Gamma(\mathbf{x})$ is naturally derived as
follows:

$\displaystyle\nabla\Gamma(\mathbf{x})=\sum_{i=1}^{N_{sv}}\alpha_{i}y_{i}\dfrac{\partial
K(\mathbf{x},\mathbf{x}_{i})}{\partial\mathbf{x}}=-\gamma\sum_{i=1}^{N_{sv}}\alpha_{i}y_{i}e^{-\gamma||\mathbf{x}-\mathbf{x}_{i}||^{2}}(\mathbf{x}-\mathbf{x}_{i}).$
(25)

In Eq. 24 and 25, $\mathbf{x}_{i}\ (i=1,...,N_{sv})$ are the support vectors
from the training dataset, $y_{i}$ are corresponding collision labels ($-1$ if
position is collided, $+1$ otherwise), $0\leq\alpha_{i}\leq C$ are the weights
for support vectors and $b\in\mathbb{R}$ is decision rule bias. Parameter
$C\in\mathbb{R}$ is a penalty factor used to trade-off between errors
minimization and margin maximization. We empirically set the hyper-parameters
of the SVM to $C=20$ and $\gamma=20$. Parameters $\alpha_{i}$ and $b$ and the
support vectors $\mathbf{x}_{i}$ are estimated by solving the optimization
problem for the soft-margin kernel SVM using scikit-learn. Using this learned
$\Gamma$-function, Fig. 16 shows two examples of the modulated trajectory.

Figure 16: Cross-sections showing streamlines of the dynamical system
modulation effect for two distinct targets in the 3D reaching task. Red
crosses indicate reference points. Green diamond is the initial position of
the end-effector for all experiments.

Finally, given a desired modulated 3D velocity for the end-effector tip,
$\dot{\mathbf{x}}_{M}=\mathbf{u}_{M}(\mathbf{x})$, we compute the next desired
3D position by numerical integration:

$\mathbf{x}_{t+1}=\mathbf{x}_{t}+\mathbf{u}_{M}(\mathbf{x}_{t})\Delta t$ (26)

where $\mathbf{x}_{t},\mathbf{x}_{t+1}\in\mathbb{R}^{3}$ are the current and
next desired 3D position of the tip of the end-effector and $\Delta t=0.03$ is
the control loop time step. $\mathbf{x}_{t+1}$ is then the target in Cartesian
world space coordinates that defines the objective of the position-based IK
solver implemented in Klamp’t [38].

##  Appendix K Additional Results for 7DoF Arm Reaching

#### Details on the DS Improvement

The DS controller provides guarantees of convergence to a target in the space
where modulation is applied (i.e. task-space in our experiments). To adopt
this controller for obstacle avoidance with a robot manipulator, Huber et al.
[7] simplifies the robot to a spherical shape with center at the end-effector
of a 7DOF arm. This translates to considering the robot as a zero-mass point
in 3D space but with the boundaries of the obstacles (described by
$\Gamma$-functions) expanded by a margin with the size of the radius of the
sphere.

Since the shape of the Franka robotic hand is rectangular (6.3 $\times$ 20.7
$\times$ 14cm) fitting a sphere with the radius of the longest axis will over-
constrain the controller and drastically reduce the target regions inside the
table dividers. We thus implemented the obstacle clearances by extruding the
edges of the top table divider by half of the length of the robot’s end-
effector (10cm) and the width of the divider by half of the height (7cm).
Intuitively, this should be enough clearance to avoid the robot’s end-effector
colliding with the table dividers. However, when coupling the DS controller
with the IK solver to control the 7DoF arm, we noticed that the success rate
was below $15\%$, whereas the success rate is $100\%$ when controlling the
end-effector only. We then sampled, via RoCUS, the target locations for the
minimal final end-effector distance to target and noticed that all of the
successful runs were located on the left-side of the partition (Fig. 6 center
right).

Since the DS controller approach does not consider collision avoidance in
joint-space, in a constrained environment, the robot’s forearm or elbow might
get stuck on the edges of the table divider—even though the end-effector is
avoiding collision. Due to the asymmetric kinematic structure of the robot
arm, it is more prone to these situations on the right side of the table
divider. Such an insight is not easy to discover as one must understand how
the robot will behave in joint space based on its kinematic structure and the
low-level controller used (position-based IK). We thus extended the edge
extrusions to 20cm. This change improved the controller success rate and
behavior drastically as shown in (Fig. 6 rightmost).

#### Legibility

We define legibility of reaching to the target on one side of the vertical
divider as the average negative distance that the end effector moves in the
other direction, $V(\mathbf{x})=-\max(\tilde{\mathbf{x}}_{1},0)$, where
$\tilde{\mathbf{x}}_{1}=\mathbf{x}_{1}$ if target is on the left, or
$\tilde{\mathbf{x}}_{1}=-\mathbf{x}_{1}$ otherwise, and $\mathbf{x}_{1}$ is
the $x$-coordinate of the robot end effector with right in the positive
direction. We find target locations that are minimally legible and apply the
maximal inference mode on the maximum distance measure.

Figure 17: Posterior samples showing minimal legibility behavior for RRT.

We did not find any illegible motions from RL controllers for 2,000 targets,
which is mostly expected since the RL reward is distance to the target. For
RRT, however, since we do not use an optimal formulation [e.g. 39, 40] or
perform post-hoc smoothing, the controller is expected to frequently exhibit
low legibility. Fig. 17 plots the posterior target locations and trajectories.
The target locations leading to illegible motions are spread out mostly
uniformly on the right, but concentrated in far-back area on the left,
consistent with our findings on the asymmetry of configuration space. The
trajectory plot confirms the illegibility.

##  Appendix L Future Work

There are multiple directions to extend and complement RoCUS for better
usability and more comprehensive functionality. First, while we only used
RoCUS on individual controllers, future work can readily extend it to compare
two controllers by defining behavior functions that take in the task and two
trajectories, one from each controller, and compute differential statistics.
For example, this could be used to find road conditions that lead to increased
swerving behavior of a new AV controller, compared to the existing one. Such
testing is important to gain a better understanding of model updates [33], and
is particularly necessary for ensuring that these updates do not
unintentionally introduce new problems.

In addition, sometimes it is important to understand particular trajectories
sampled by RoCUS. For example, which sensor input (e.g. lidar or camera) is
most important to the current action (e.g. swerving)? Why does the controller
take one action rather than another (e.g. swerving rather than braking)?
Preliminary investigation into this explainable artificial intelligence (XAI)
problem in the context of temporally extended decision making has been
undertaken [41, 42], but various issues with existing approaches have been
raised [43, 44] and future research is needed to address them.

Finally, an important step before actual deployment is to design appropriate
user interfaces to facilitate the two-way communication between RoCUS and end-
users. In one direction, the user needs to specify the behavior of interest,
and it would be desirable for it to involve as little programming as possible,
especially for non-technical stakeholders. In the other direction, RoCUS needs
to present the sample visualization, and potentially model explanations as
described above, for users to inspect. Here, it is important for the
information to be accurate but at the same time not overwhelming.