Skip to content

Commit

Permalink
Add blog post.
Browse files Browse the repository at this point in the history
  • Loading branch information
aterenin committed May 3, 2024
1 parent e11e9d2 commit d7fb58c
Show file tree
Hide file tree
Showing 2 changed files with 119 additions and 1 deletion.
118 changes: 118 additions & 0 deletions content/2024-05-02-vGPMP-Motion-Planning-Temporary/index.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,118 @@
+++
title = "A Unifying Variational Framework for Gaussian Process Motion Planning"
[extra]
authors = [
{name = "Lucas Cosier", url = "https://luke-ck.github.io", star = true},
{name = "Rares Iordan", star = true},
{name = "Sicelukwanda Zwane"},
{name = "Giovanni Franzese"},
{name = "James T. Wilson"},
{name = "Marc Peter Deisenroth", url = "https://deisenroth.cc/"},
{name = "Alexander Terenin", url = "https://avt.im/"},
{name = "Yasemin Bekiroğlu", url = "http://yaseminb.github.io" },
]
venue = {name = "AISTATS", date = 2023-05-02, url = "http://aistats.org/aistats2024/"}
buttons = [
{name = "Paper", url = "https://arxiv.org/abs/2309.00854"},
{name = "PDF", url = "https://arxiv.org/pdf/2309.00854"},
{name = "Code", url = "http://github.com/luke-ck/vgpmp"},
]
katex = true
large_card = true
+++

In the field of robot motion planning, one must compute paths through high-dimensional state spaces, while adhering to the physical constraints of motors and joints, ensuring smooth and stable movement, and avoiding obstacles.
This requires a balance of competing computational factors, particularly if one wants to handle this in the presence of uncertainty, including noise, model error, and other complexities arising from real-world environments.
We present a motion planning framework based on variational Gaussian Processes, which supports a flexible family of motion-planning constraints, including equality-based and inequality-based constraints, as well as soft constraints through end-to-end learning.
Our framework is straightforward to implement, and provides both interval-based and Monte-Carlo-based uncertainty estimates.
We evaluate it on a number of different environments and robots, compare against baselines approaches based on feasibility of sampled motion plans and obstacle avoidance quality.
Our results show the proposal achieves a reasonable balance between the motion plan's rate of success and path quality.

# Applying Variational Gaussian Processes to Motion Planning

We begin with a motion planning framework, which we call *variational Gaussian process motion planning (vGPMP)*.
This framework is based on variational Gaussian processes, which were originally introduced for scalability: here, we instead apply them to create a straightforward way to parameterize motion plans.
Let `$\mathcal{T}$` represent time: our motion plan is a map `$f: \mathcal{T} \to \mathbb{R}^d$`, where the output space represents the position of each of the robot's joints.
We parameterize `$f$` as a posterior Gaussian process, conditioned on `$f(\boldsymbol{z}) = \boldsymbol{u}$`, where `$\boldsymbol{z}$` is a set of inducing locations `$\boldsymbol{z} \in \mathcal{T}^m$`, and `$\boldsymbol{u}$` are robot joint states at times `$\boldsymbol{z}$`.
We interpret `$(z_j,u_j)$`-pairs as *waypoints* through which the robot should move.
Our precise formulation in the paper also includes a bijective map which accounts for joint constraints: we suppress this here for simplicity.

To draw motion plans, we apply *pathwise conditioning*,[^efficient-sampling][^pathwise-conditioning] and represent posterior samples as

```
$$
(f \mid \boldsymbol{u})(\cdot) = f(\cdot) + \mathbf{K}_{(\cdot),\boldsymbol{z}} \mathbf{K}_{\boldsymbol{z},\boldsymbol{z}}^{-1} (\boldsymbol{u} - f(\boldsymbol{z})).
$$
```

where the inducing points `$\boldsymbol{z}$` and the variational distribution `$q(\boldsymbol{u})$` of the values `$\boldsymbol{u}$` represent the distribution of possible motion plans.
Computing the motion plan therefore entails optimizing these parameters with respect to an appropriate variational objective.
Once optimized, in practice we can sample from the posterior using efficient sampling, that is, by first approximately sampling the prior `$f(\cdot)$` using Fourier features, then transforming the sampled prior motion plans into posterior motion plans.
This procedure allows us to draw random curves representing the posterior in a way that *resolves the stochasticity once in advance* per sample, after which we can evaluate and differentiate the motion plan at arbitrary time points without any additional sampling.
Compared to prior work, we support general kernels and avoid relying on specialized techniques for stochastic differential equations, thereby enabling explicit control of motion plan smoothness properties.
We also bypass the need to use interpolation to evaluate the posterior in-between a set of pre-specified time points.

Following the framework of variational inference, the resulting variational posterior can be trained by solving the optimization problem

```
$$
\min \mathbb{E}_{q(\boldsymbol{u})} \mathbb{E}_{p(\boldsymbol{f}\mid\boldsymbol{u})} \log p(\boldsymbol{e}\mid\boldsymbol{f}) - D_{\operatorname{KL}}(q(\boldsymbol{u})\mid\mid p(\boldsymbol{u}))
$$
```

where `$p(\boldsymbol{f}\mid\boldsymbol{u})$` is the *log-likelihood* term which in motion planning is used to encode soft constraints.
To apply hard equality-based constraints, we use the fact that the variational posterior is Gaussian, and apply conditional probability to an appropriate set of $(z_i,u_i)$-pairs.
We can also apply hard inequality-based constraints, such as joint constraints, and describe this further in the sequel.

This objective reveals Gaussian-process-based motion planning algorithms to be *stochastic generalizations* of optimization-based planners such as CHOMP[^chomp], STOMP[^stomp], and other variants: compared to these, the main difference is the presence of the Kullback--Leibler divergence, which prevents the posterior from collapsing to a single trajectory, thereby including uncertainty.
For the log-likelihood, we use

```
$$
p(\boldsymbol{e}\mid\boldsymbol{f}) = \exp\left(-\frac{1}{2} \left(\underset{\text{collision term}}{\undergroup{\left\|\operatorname{h}_\varepsilon(\operatorname{sdf}_s(\operatorname{k}_{\operatorname{fwd}}(\sigma(\boldsymbol{f}))))\right\|_{\mathbf\Sigma_{\operatorname{obs}}}^2}} + \underset{\text{soft constraint term}}{\undergroup{\left\|\operatorname{c}(\sigma(\boldsymbol{f}))\right\|^2_{\mathbf\Sigma_{\operatorname{c}}}}} \right)\right)
$$
```

which works as follows.
For the collision term, we first approximate a robot by a set of spheres, and compute the signed distance field `$\operatorname{sdf}_s$` for each sphere.
This is done by composing the forward kinematics map `$\operatorname{k}_{\operatorname{fwd}}$` with the (inverse) constraint map `$\sigma$` which enforces a set of box constraints to account for joint limits.
Then, we compute the hinge loss `$\operatorname{h}_\varepsilon(x) = \max(-x + \varepsilon, 0)$`, where `$\varepsilon$` is the *safety distance* parameter, and calculate its squared norm with respect to a diagonal scaling matrix `$\mathbf\Sigma_{\operatorname{obs}}$` which determines the overall importance of avoiding collisions in the objective.
The soft constraint term, which can be used to encode desired behavior such as a grasping pose, is handled analogously.
Compared to prior work, one of the key differences is the introduction of `$\sigma$`, which guarantees that joint limits are respected without the need for clamping or other post-processing-based heuristics.

# Experiments

Below, we provide a side-by-side comparison with our framework, vGPMP, with the Gaussian-process-based baseline GPMP2, on the WAM robot.
This comparison shows that, on this example, vGPMP produces longer paths, which are more conservative with respect to collision avoidance.

**TODO: paths**

Next, we demonstrate that our framework can enable one to incorporate a grasping pose without further tuning or changing the underlying model assumptions.
This amounts to adding appropriate terms to the soft constraints, and enables the Franka robot, in simulation, to pick up a can.
Further adjustments to can be made to allow the robot to pick up the can and move it to desired places by conditioning end-effector joints to open and close at specific time steps.

**TODO: figure 2**

Finally we present a demonstration of the Franka robot avoiding boxes.
Here, our framework allows sampled paths to be computed at any required resolution without additional interpolation, enabling fine-grained control over smoothness.

**TODO: VIDEO**

# Conclusion

We present a unifying framework which simplifies motion planning with Gaussian processes by applying a formulation based on variational inference.
Through the use of this inducing-point-based framework and pathwise conditioning, we support general kernels that provide explicit control over motion plan smoothness properties.
Our computations are straightforward to implement, and avoid the need for interpolation, clipping, and other post-processing.
The framework also connects Gaussian-process-based motion planning algorithms with optimization-based approaches.
We evaluate the approach on different robots, showing that it accurately reaches target positions while avoiding obstacles, providing uncertainty, and increasing clearance compared to baselines.
We demonstrate the approach on a real robot, executing a randomly sampled trajectory while avoiding obstacles.

[^gpmp2]: TODO

[^chomp]: TODO

[^stomp]: TODO

[^efficient-sampling]: J. T. Wilson, V. Borovitskiy, A. Terenin, P. Mostowsky and M. P. Deisenroth. Efficiently Sampling Functions from Gaussian Process Posteriors. ICML 2020.

[^pathwise-conditioning]: J. T. Wilson, V. Borovitskiy, A. Terenin, P. Mostowsky and M. P. Deisenroth. Pathwise Conditioning of Gaussian Processes. JMLR 2021.
2 changes: 1 addition & 1 deletion themes/academic-paper

0 comments on commit d7fb58c

Please sign in to comment.