Robot Motion Planning in Learned Latent Spaces
Brian Ichter Marco Pavone


Balance functions of (un)identified hadrons in Pb–Pb, p–Pb, and pp collisions at the LHC
    Jinjin Pan (on behalf of the ALICE Collaboration)
    December 30, 2023
========================================================================================

empty
empty



This paper presents Latent Sampling-based Motion Planning (), a methodology towards computing motion plans for complex robotic systems by learning a plannable latent representation.
Recent works in control of robotic systems have effectively leveraged local, low-dimensional embeddings of high-dimensional dynamics. 
In this paper we combine these recent advances with techniques from sampling-based motion planning (SBMP) in order to design a methodology capable of planning for high-dimensional robotic systems beyond the reach of traditional approaches (e.g., humanoids, or even systems where planning occurs in the visual space). 
Specifically, the learned latent space is constructed through an autoencoding network, a dynamics network, and a collision checking network, which mirror the three main algorithmic primitives of SBMP, namely state sampling, local steering, and collision checking. 
Notably, these networks can be trained through only raw data of the system's states and actions along with a supervising collision checker.
Building upon these networks, an RRT-based algorithm is used to plan motions directly in the latent space – we refer to this exploration algorithm as Learned Latent RRT ().
This algorithm globally explores the latent space and is capable of generalizing to new environments. The overall methodology is demonstrated on two planning problems, namely a visual planning problem, whereby planning happens in the visual (pixel) space, and a humanoid robot planning problem.








§ INTRODUCTION


Motion planning is a fundamental problem in robotics whereby one seeks to compute a dynamically-feasible trajectory connecting an initial state to a goal region while avoiding obstacles.
This problem quickly increases in difficulty with the dimensionality of the system and the complexity of the dynamics <cit.>.
For some high-dimensional systems, and even differentially-constrained systems, sampling-based motion planning (SBMP) techniques have emerged as a particularly successful approach. 
SBMP avoids the explicit construction of the state space and instead maintains an implicit representation of it through three algorithmic primitives: sampling the state space, steering (or connecting) to nearby sampled states, and collision checking such connections <cit.>.
This implicit representation is global and increasingly accurate as samples are added  <cit.>.
However, applying SBMP to robotic systems with, say, more than ten dimensions, quickly becomes intractable.
This precludes its application (at least directly) to cases where, for example, plans are computed from visual inputs <cit.> or the dynamics are very high-dimensional and complex, as for humanoids <cit.>.

A promising solution to address this challenge is represented by recent works on learning-based control, which 
leverage learned, low-dimensional manifolds embedded within the state space to which the dynamics of a robotic system are mainly restricted to <cit.>.
For example, a bipedal humanoid walking robot is generally in gait and upright, and a visual representation of a robot has a “true" underlying lower-dimensional dynamical representation. These works, however, primarily focus only in
local regions, and can not be directly applied to compute motion plans whereby one needs to globally explore the state space. Our key insight to scale these techniques to robot motion planning is to leverage the local effectiveness of low-dimensional latent representations to learn a latent space in which one can directly use SBMP. In other words, the idea is to build a global implicit representation of the latent space through only local connections.
The latent space is constructed through an autoencoding network, a dynamics network, and a collision checking network, which mirror the three aforementioned algorithmic primitives of SBMP, namely state sampling, local steering, and collision checking. Specifically, the space is learned through an autoencoding network with learned latent dynamics, which provides
the ability to sample the latent space and connect local states through dynamically-feasible trajectories.
Notably, this network only requires raw data of the system's states and control inputs to be trained.
Separately, a collision checking network is trained to predict whether a trajectory between two latent states is collision free given a specific problem environment. Collectively, these networks provide a methodology to globally search the latent space (in our case, through an RRT-based algorithm), in a way that generalizes well to new environments and is general enough to apply to a broad class of problems, including visual planning and complex dynamics problems.
The final trajectory can then be projected to the full state space through the autoencoding network and used in conjunction with the aforementioned local control methods to effectively control the robotic system along the planned trajectory. We refer to this methodology as Latent Sampling-based Motion Planning ().

Related Work: The field of representation learning seeks to learn representations of data to extract useful information for classifiers or other predictors <cit.>.
This often takes the form of manifold learning, i.e., learning lower-dimensional manifolds embedded within high-dimensional data. 
Representation learning has been applied along the entire robotic stack – from perception <cit.>, to decision making and planning <cit.>, to control and dynamics <cit.>. Recently, these methods have seen particular success in the field of control. Embed to Control <cit.> and Robust Locally-Linear Controllable Embeddings <cit.> present approaches to learning to control robotic systems directly from raw images. 
Both methods learn a locally-linear embedding via a variational autoencoder and directly perform stochastic optimal control on the latent space. 
<cit.> learns to predict the results of actions in visual spaces and uses model-predictive control to plan local manipulator actions. In contrast, our work learns a non-linear latent space (allowing generalization to more complex systems) and a collision checking network (allowing generalization to new environments).

In the context of robot motion planning, Universal Planning Networks (UPN) <cit.> learn a plannable latent representation in which gradient descent can be used to compute a plan that minimizes an imitation loss (learned from an expert planner).
Our methodology, , learns a similar latent space to UPN (by enforcing dynamics in the latent space), but performs a global search of the latent space to find motion plans.
In addition, does not rely on an expert planner, which for many problems may not be available.
<cit.> uses a Gaussian process latent variable model to learn a  low-dimensional representation and then uses inference to solve the planning problem via a particle-based approach. Compared to , the latent variable model must be well initialized and using inference does not allow a global search of the latent space.
Motion Planning Networks (MPNet) <cit.> learns a low-dimensional representation of the obstacle space and uses a feedforward neural network to predict the next step an optimal planner would take given an initial state, goal state, and the obstacle representation (trained via an asymptotically optimal planner).
MPNet approaches the problem of accelerating solutions to planning problems for which solutions to similar problems can be provided by demonstration, whereas our work attempts to solve previously intractable planning problems.
Lastly, within sampling-based planning, learning has been used to replace parts of the algorithmic stack, such as the collision checker <cit.> or the sample distribution <cit.>. 
too learns a collision checking network, though directly in the latent space, and, in a manner, learns where to sample by choosing samples for the exploration algorithm directly from encoded training states.

It is important to note that, in practice, the methodology can be, and should be, combined with many of the aforementioned approaches. 
In many cases, may be an initialization for the latent spaces in <cit.>. 
Furthermore, these approaches can be used to improve the trajectory generated by and to control the motion along it.

Statement of Contributions: The contribution of this paper is threefold. 
First, we present the  Latent Sampling-based Motion Planning () methodology, which comprises an autoencoding network, a dynamics network, and a collision checking network to enable planning through the latent space. 
These networks can be trained through only raw data of the system's states and control inputs along with a supervising collision checker.
Second, building upon , we present an RRT-based algorithm,  termed Learned Latent Rapidly-exploring Random Trees (), to plan motions directly in the latent space. efficiently and globally explores the learned manifold by maintaining an implicit representation of the space formed through sampling encoded states and dynamically propagating nearby trajectories in the tree (under the supervision of the collision checking network). 
The final latent trajectory is then projected back to the full state space through the autoencoding network. Third, we demonstrate our approach on two planning problems that are well beyond the reach of standard SBMP, namely a visual planning problem, whereby planning happens in the visual (pixel) space, and a humanoid robot planning problem.

Organization:  The remainder of the paper is organized as follows. Section <ref> introduces the problem we are approaching and provides necessary background. Section <ref> outlines the methodology for learning the plannable latent space. Section <ref> presents , the sampling-based exploration strategy used to globally search the latent space. Section <ref> demonstrates the performance of on a visual planning problem and a humanoid planning problem. Section <ref> summarizes our findings and suggests several avenues for future work.



§ PROBLEM STATEMENT


In this section we formulate the motion planning problem we wish to solve.
We then briefly overview sampling-based motion planning algorithms in order to give necessary background and to motivate our methodology.
Lastly, we present the problem of learning plannable latent spaces.



 §.§ Robot Motion Planning Problem


Let ⊆^ and ⊆^ denote the state space and control input space, respectively, of a robotic system.
Let the dynamics of the robot be defined by 

    ẋ⃗(τ) = f^(c)_(x⃗(τ),u⃗(τ)),

where x⃗(τ) ∈ and u⃗(τ) ∈ denote the state and control input of the system at time τ and f^(c)_ denotes the continuous-time system dynamics. 
To make the dynamics learning problem tractable we consider a discrete-time formulation.
Note however that we still consider the continuous-time underlying dynamics to determine collisions.
Let the discrete-time dynamics of the robot be defined by  

    x⃗_t+1 = f_(x⃗_t,u⃗_t),

where x⃗_t ∈ and u⃗_t ∈ denote the state and control input of the system at time step t and f_ denotes the discrete-time system dynamics. 

Let ⊆ define the free state space of the robot and let ⊂ define the obstacle space, such that = ∖.
Let us be given an initial state ∈ and a goal region ⊂.
A trajectory π for the robot is defined as a series of states and control inputs (x⃗_0,u⃗_0,…,x⃗_T,u⃗_T), where T is the number of time steps in the trajectory.
Further, let x⃗_0,…,x⃗_T denote the continuous curve traced by the robot's trajectory (connecting x⃗_0,…,x⃗_T).
We make this distinction here to clarify that collision checking should be done along the continuous trajectory connecting discrete states.
A trajectory is said to be collision-free if x⃗_0,…,x⃗_T∩ = ∅.
A trajectory is said to be feasible if it connects the initial state and goal region, i.e., x⃗_0 = and x⃗_T ∈, is collision-free, and is dynamically feasible, i.e., x⃗_t = f_(x⃗_t-1,u⃗_t-1) for t = 1,…,T. In this work, we are interested in the following problem,

[Motion Planning Problem]
Given a motion planning problem (, , ), find a feasible trajectory π. If no such trajectory exists, report failure.




 §.§ Sampling-based Motion Planning


Sampling-based motion planning has emerged as a highly effective algorithmic paradigm for solving complex motion planning problems.
SBMP avoids the explicit construction of the state space and instead maintains an implicit, searchable representation. 
This representation is formed by a set of probing samples which are locally connected and verified via a “black box” collision checking module <cit.>.
This methodology enables a global search and guarantees on probabilistic completeness and often asymptotic optimality <cit.>. 

One particularly successful family of sampling-based motion planning algorithms is the rapidly-exploring random tree (RRT) <cit.>. 
As our exploration algorithm is built from this, we now overview RRT to clarify each algorithmic primitive.
The algorithm begins by initializing a tree 𝒯 with the initial state , and subsequently iteratively builds a tree through the state space.
At each iteration, a state in the free state space is sampled, x⃗_sample∈. 
The nearest node within the tree to x⃗_sample is selected as the expansion node, x⃗_expand.
The node is then expanded towards x⃗_sample, forming a new sample x⃗_new.
If the trajectory connecting x⃗_expand and x⃗_new does not intersect , i.e., is collision-free, then x⃗_new is added as a node to the tree.
This continues until either time elapses or a pre-determined number of samples is reached.
The approach is capable of globally searching the state space only through three algorithmic primitives: (1) sampling, (2) locally connecting, and (3) collision checking.
The key idea in this work is that, by ensuring each of these algorithmic primitives can be applied in a learned, low-dimensional latent space representing the robotic system, we can directly use SBMP to compute plans for the full state space.



 §.§ Plannable Latent Spaces


The robotic systems we consider in this work may have complex-dynamics and high-dimensional state spaces. 
For example, these may be pixel representations of the robot and environment or they may be highly-dynamic, high-dimensional robots. 
In these cases standard approaches to solving motion planning problems are computationally intractable. 
Even approaches like SBMPs, which were initially created for high-dimensional systems, have difficulty scaling well beyond ten dimensions even in the absence of dynamic constraints. 

Our goal is to find the underlying manifold of the problem by learning a low-dimensional latent space model in which SBMP can be performed.
Due to the complexity of the system, this space should be learned only through sequences of states and control inputs of the robot in operation and a separate local collision checker.
Let ⊆^ denote the learned latent space and z⃗_t ∈ a latent state at time step t.
We seek to learn a mapping m from  to  and a mapping n from  to  for which latent dynamics f_ are enforced over .
We further learn a “black-box” collision checking function g in the latent space. 
We thus wish to learn the following:

    m : →,    n : →,   z⃗_t+1 = f_(z⃗_t,u⃗_t), 
       g(z⃗_t,z⃗_t+1, ) = {1  if x⃗_t,x⃗_t+1∩ = ∅,   0  otherwise}.






§ LEARNING A LATENT SPACE FOR ROBOTIC MOTION PLANNING


In order to plan on a low-dimensional latent space model, one needs the properties that: (1) the latent space can be sampled, (2) the dynamics are known, so as to connect nearby latent states, and (3) a local, latent trajectory can be classified as collision-free.
It is also desirable that the latent samples can be mapped to the full space once a solution is found, so that the robotic system can execute the motion plan.
Finally, we do not assume we have access to the true system dynamics f_, i.e., only observations of states and actions over time and a collision check.

The Latent Sampling-based Motion Planning () architecture consists of three networks: (1) an autoencoder, (2) a latent local dynamics model, and (3) a collision classifier. 
The autoencoder projects the high-dimensional full state (x⃗_t) into a lower-dimensional latent state (z⃗_t), 
and also allows decoding to form inputs for a subsequent controller.
The dynamics model enforces local dynamics on the encoded latent space and allows the dynamics to be propagated.
Finally, the collision classifier identifies if the connection between two nearby latent states will be possible given the problem environment. 
The latent space is trained from a series of trajectories from the robotic system in operation; i.e., a sequence of states x⃗_t and actions u⃗_t.
The collision checking network is trained from a data set of nearby states and a label denoting if the connecting trajectory is in collision.



 §.§ Learned Latent Space and Dynamics


The first network in our architecture follows a standard treatment for autoencoders. Given a robotic system with state x⃗_t and latent state z⃗_t, we define an encoding network (x⃗_t) and a decoding network (z⃗_t), with parameters ϕ and θ, respectively. 
These networks project the full state into the lower-dimensional latent space and then reconstruct the full state.
Depending on the robotic system, such networks may be convolutional or fully connected networks.
For this work we use an ℓ_2 reconstruction loss.

The second network in our architecture learns, and subsequently enforces, local dynamics in the latent space. 
Let (z⃗_t,u⃗_t) denote the dynamics network and ψ its parameters. This network predicts the next state in the latent space, ẑ⃗̂_t+1.
The accuracy of this prediction is enforced through two losses. 
First, the prediction is decoded (ẑ⃗̂_t+1), and compared to the full state x⃗_t+1 with an ℓ_2 reconstruction loss.
Second, the prediction is compared in the latent space to the next encoded state z⃗_t + 1 = (x⃗_t+1).
In order to have a dynamically-consistent loss, we linearize the dynamics around z⃗_t and u⃗_t and compute the weighted controllability Gramian G_t (also known as the reachability Gramian) as 

    A_t = ∂/∂z⃗(z⃗_t, u⃗_t),   B_t = ∂/∂u⃗(z⃗_t, u⃗_t),   G_t = A_t B_t B_t^⊤ A_t^⊤.

The loss is then computed as the minimum energy between the prediction and the true next latent state, that is ℒ_t^dyn = (z⃗_t + 1 - ẑ⃗̂_t + 1)^⊤G_t^-1(z⃗_t + 1 - ẑ⃗̂_t + 1).
Essentially this enforces that the loss in prediction is representative of the dynamics of the system, e.g., for a car, missing the latent state prediction forward or backward is less important than missing left and right. 
Note however, that initially, before the dynamics network has been trained, the weighted controllability Gramian from Eq. <ref> may be ill-conditioned. 
We thus begin with an ℓ_2 norm and gradually shift to the G_t^-1 norm.
We also add a small positive term to the diagonal of G_t to ensure invertibility.



 §.§ Learning a Collision Checker


The third network in our architecture learns to identify if the continuous trajectory between two latent states will be in collision; we denote this network as (z⃗_t, z⃗_t+1, ), parameterized by ζ. 
This is a supervised learning process entailing binary classification; meaning, we assume we have a collision checker in the full state space capable of reporting if the trajectory is in collision – arguably, a very mild assumption. 
We note here our use of  as an input to the network. In SBMP  is generally only implicitly defined through the collision checker. We use it here to denote any environmental information available to the state space collision checker. This representation can be quite general as the neural network architecture has the capacity to arbitrarily project this information as necessary. For example, this may be workspace coordinates of the obstacles, an occupancy grid, or an image of the problem.
This network is learned separately, after the encoder and dynamics have been learned.
It is possible to learn this network through unsupervised learning by identifying regions of the state space that are rarely or never connected dependent on the environment. We leave this for future work.





§ PLANNING IN THE LATENT SPACE


We now present the Learned Latent Rapidly-exploring Random Trees () algorithm used to globally search the latent space learned as described in Section <ref>.
is a SBMP algorithm within the rapidly exploring random trees () family of algorithms <cit.>.
Briefly, the algorithm builds a tree, 𝒯, through the latent space by sampling latent samples from the learned manifold and propagating dynamics from nearby tree nodes (under the supervision of the collision checking network).
As the learned dynamics may be quite complex, the algorithm must rely on a dynamics propagation strategy, rather than a steering function, to connect local latent states.

The algorithm is similar to the RRT-BestNear algorithm presented in <cit.>, with samples restricted to the latent space and dynamic propagation through latent dynamics. Specifically, the algorithm is outlined in Alg. <ref> and visualized in Fig. <ref>.
The algorithm takes as input the planning problem in the full state space: the obstacle representation,  initial state, , and goal region, .
Note again, we abuse notation here with . Though traditionally in SBMP,  is only implicitly defined through the collision-checker, we use it here to refer to any environmental information used for the state space collision checker. 


To begin, the initial state and goal region are encoded into the latent space as () = and () =, respectively (line <ref>). 
The tree is initialized with the initial latent state (line <ref>).
The algorithm then begins expanding the tree outwards into the latent space. 
At each iteration a sample, z⃗_sample, is selected from a sample set _samples (line <ref>).
This set is comprised of latent samples generated by encoding a randomly drawn set of the robot in operation. 
That is, given a sequence of states (x⃗_0,…,x⃗_N) (where N is large to well cover the operational state space of the robot), we randomly select a sample set _samples and encode it as _samples = (_samples).
By selecting samples only from this set, ensures that the exploration remains nearby the learned manifold in the latent space.
then selects a node in the tree for propagation by using the  strategy outlined in <cit.> (line <ref>).
(z⃗_sample,δ_G_t^-1) selects the tree node z⃗_prop within a ball radius, (z⃗_prop - z⃗_sample)^⊤ G_t^-1(z⃗_prop - z⃗_sample) < δ_G_t^-1, with the lowest-cost trajectory[This cost can be fairly general, e.g., control cost or the number of time steps.]. 
If no tree nodes are within the ball radius, the nearest tree node is returned.
The ball radius δ_G_t^-1 parameter represents a tradeoff between exploration and exploitation of good trajectories; its value is discussed in the subsequent paragraph and at length in <cit.>.
With z⃗_prop in hand, then forward propagates dynamics through  with a randomly chosen control action u⃗_0,…,T_prop-1∈ for time T_prop, where T_prop is randomly chosen between 1 and T_max, following the Monte Carlo propagation procedure outlined in <cit.> (line <ref>).
This gives a trajectory in latent space (z⃗_0,u⃗_0,…,z⃗_T_prop-1,u⃗_T_prop-1,z⃗_T_prop,0), where z⃗_0=z⃗_prop (line <ref>).
Each pair of waypoints is then collision checked by (z⃗_t,z⃗_t+1,) for t = 0,…,T_prop-1.
The probability that a local connection is collision-free can be computed as sigmoid() (line <ref>). 
Thus, the  acceptable probability threshold α can be tuned to the required level of safety.
Finally, if the propagated trajectory is collision free, z⃗_T_prop is added to the tree (line <ref>).
The exploration ends after a set amount of time or number of samples.
If no trajectories end in the goal region , the algorithm reports failure (line <ref>).
If any trajectories reach the goal region, the best trajectory[1] is then selected and projected back to the full state space through  (line <ref>-<ref>).
This outputs nominal control inputs as well as the full state space trajectory (which may be used as input to a controller or a trajectory optimizer).






Briefly we provide discussion on how to choose values for the algorithm's parameters.
The δ_G_t^-1 ball radius represents a tradeoff between exploiting good trajectories and exploring with new ones.  
This value should be tuned according to the system and from discussion in <cit.>.
The maximum dynamic propagation time T_max controls, roughly, the length of edge.
For subsequent results this was approximately set so the robot with optimal control can traverse the full state space in 4T_max.
The value of α reflects the required level of safety in the collision checking network.
We found most collision classifications yielded probabilities less than 1% or greater than 99%, however, for subsequent results this threshold was set as α = 0.9.
Finally, we also use a technique called goal-biasing, in which we sample more heavily from the goal region. 
For subsequent results we sample a state within the goal region 10% of the time. 

Due to the approximate nature of the learned latent space, does not enjoy formal completeness or optimality guarantees. An interesting avenue for future research is to investigate conditions under which theoretical guarantees for can be derived. 



§ EXPERIMENTAL RESULTS


In this section we demonstrate the effectiveness of our methodology on two planning problems from domains in which traditional motion planning approaches are intractable.
The first is a visual planning problem entailing a point robot with single integrator dynamics navigating a cluttered environment based on visual inputs. The second is a humanoid planning problem where a humanoid robot has to maneuver amidst obstacles. The methodology was implemented in Python and Tensorflow <cit.>. All data was collected on a Unix system with a 3.4 GHz CPU and an NVIDIA GeForce GTX 1080 Ti GPU.
Example code can be found at <github.com/StanfordASL/LSBMP>.



 §.§ Visual Planning Problem


We consider a visual space planar problem similar to the one outlined in <cit.>, though with a more complex obstacle space. This setup allows us to demonstrate the performance of the methodology to generalize to new environments by leveraging the algorithmic prior of separating collision checking from the dynamic propagation process. 
As we have access to the true underlying state, and the underlying planning problem is relatively simple, this problem allows us to compare our methodology against sampling-based motion planning algorithms with access to the true state of the system.

The full state of the visual problem is a 32×32 pixel image rendered in MuJoCo showing both the robot and the obstacles in the scene.
The robot is a point robot with single integrator dynamics.
The obstacles are a random number of circles and squares with randomly generated locations and sizes.
An example problem's initial and goal states are shown in Figs. <ref>-<ref>.
The training data for the autoencoder and dynamics network consists of 10,000 environments, each with a trajectory of 10 successive states and control inputs.
The training data for the collision checking network consists of 25,000 environments, again with 10 pairs of states and a collision label. 

The full state is projected down to a two dimensional latent space.
The methodology applied to an image-based system requires some architectural changes to make the learning problem more tractable.
The encoder network, , is a deep-spatial autoencoder <cit.>, which encourages learning important visual features by using a convolutional neural network followed by a spatial soft arg-max.
To create accurate reconstructions of the full state the decoder network is given as input an image of the environment in addition to the latent state.
This additional input image may be the environment without the robot present or it could be an image with the robot from a different time step (e.g., the initial state for a planning problem).




Fig. <ref> shows the learned latent space for the problem in Fig. <ref>-<ref>. 
The methodology learns a latent space that captures the position of the robot and effectively removes latent regions within obstacles, though this obstacle set has never been seen before. 
Fig. <ref> shows the inverse weighted controllability Gramian in the latent space. 
Because the true system dynamics are consistent throughout the state space, this gives an opportunity to evaluate the weighted loss function.
As desired, in regions of the state space that are more spread (i.e., the same control input allows more movement), errors are penalized less.

Fig. <ref> shows two planning problems for this system, the planned trajectories in the full state space, and the in latent space. 
is able to successfully generalize to these problems and find trajectories even through narrow passages.
These global plans can then be fed into a local controller, such as <cit.>, or a trajectory optimizer <cit.>, to be executed.




Fig. <ref> shows the success rate and cost convergence curve (in terms of control input, i.e., distance) versus the number of samples. The success rate increases quickly at first before leveling below 100% (though all problems are solvable). The cost convergence curve follows a standard curve for SBMP, converging quickly at first before leveling out. The confusion matrix shows nearly 90% of collisions are correctly classified, with only 4% of true collisions classified as free. This false positive ratio could likely be reduced by including conservatism in the full state space collision checker.
With 1000 samples, the algorithm takes on average 9 seconds.



We additionally compare these plans to plans computed in the true state space (given a perfect representation of the robot and obstacles in two dimensions) by the Fast Marching Tree () algorithm <cit.> and by RRT-BestNear <cit.> (which uses the same exploration strategy as , but with knowledge of the true state space). 
We note that relies on a two point boundary value problem solver to make local connections.
Each algorithm is run with 2000 samples and 100 randomly generated planning problems.
This allows to explore the state space more efficiently than or RRT-BestNear (which only use dynamics propagation).
In terms of success rate of finding a solution, normalized to , is able to solve 92% of the problems while RRT-BestNear solves 96%.
In terms of solution trajectory cost, solves these problems with a 13% higher cost than and RRT-BestNear solves with a 5% higher cost than .
From these results we make two observations.
First, planning in the latent space incurs some loss (in terms of success rate and cost), however, this loss is relatively small considering the algorithm is only given a very high-dimensional, inexact, visual representation of the state.
Second, if the latent space were forced into a more restrictive class of dynamical systems (e.g., locally-linear) with a two point boundary value problem solver available, the methodology could leverage algorithms like to more efficiently explore the state space.
We leave this to future work.




 §.§ Humanoid Robot


Our second numerical experiment shows the methodology with a high-dimensional dynamical system, a humanoid in OpenAI Roboschool <cit.>.
The system consists of 50 states with dynamics and a learned controller from OpenAI Roboschool, which takes as input a target position.
The obstacles are randomly generated spheres or cubes of random sizes and positions.
The initial and goal states too are randomly generated through the state space.

The state is encoded into a four dimensional latent space, which was chosen inspired by <cit.>.
The network architecture is fully connected.
The collision checking network takes as input two latent states and a vector of the centers of each obstacle, the type of obstacle (sphere or cube), and the sphere radius or cube width of the obstacle. 
The autoencoder and dynamics network was trained on 1,000,000 data points (states and control input), sampled at 10 Hz. 
This data was obtained by randomly sampling control inputs (target positions) and propagating dynamics with the Roboschool controller <cit.>, essentially randomly exploring the operational state space.
The collision checking network was trained on 1,000,000 data points (pairs of states and the location, type, and size of each obstacle in the workspace).



Fig. <ref> shows the structure learned within the latent space.
The latent space captures the intuitively important dynamics of the problem: the robot's position and yaw.
These states most significantly govern the humanoid's dynamics, i.e., it is often traveling in a straight line towards the control target.
The other dimensions of the state space, such as joint angles and velocities, are fairly structured within the gait of the humanoid and only vary small amounts.



For a single problem, for the humanoid (projected into two and three dimensions) is shown in Fig. <ref>. 
The latent search is able to remain near the learned manifold, and the collision checker has effectively encapsulated the obstacles.
The final trajectory is shown in Fig. <ref> with the control input (the target location) set as a half second ahead on the planned trajectory.
A video is available at <goo.gl/wrm9JR>. 
Fig. <ref> details 's performance in terms of success rate, convergence, and collision network confusion.
After only a few hundreds of samples, the algorithm is able to solve 100% of the planning problems.
The trajectory cost again shows convergence as the number of samples increases.
The collision checking network performs well, classifying nearly 95% correct, with only a 0.7% false positive rate.
With 1000 samples, the algorithm takes on average 15 seconds.
Note we do not provide comparisons to planning in the full state space as traditional planning approaches with the 50 dimensional, dynamical system are intractable (unlike the visual planning problem where an exact lower-dimensional state was available).








§ CONCLUSIONS


We have presented the Latent Sampling-based Motion Planning () methodology, which leverages the effectiveness of local, latent representations for robotic systems with techniques from sampling-based motion planning (SBMP) to compute motion plans for high-dimensional, complex systems.
In particular, this latent space is learned through an autoencoder, a dynamics network, and a separate collision checking network, each of which enforces the main algorithmic primitives of SBMP on the latent space (sampling, local connections, and collision checking).
Given this latent space, we use the Learned Latent RRT () algorithm to globally explore the latent space and compute motion plans directly in it.
Through two experiments, one planning within visual space and one planning with a humanoid robot, we demonstrate the methodology's overall generality and ability to generalize to new environments.

This work leaves several avenues for future work. 
Specific to , we first plan to investigate how much data is required to learn the necessary representation.
Second, we plan to investigate learning more restricted classes of dynamics for which a steering function is available, e.g., locally-linear dynamics. 
This would allow more optimal latent space exploration techniques.
Third, we plan to investigate using unsupervised learning to learn the collision checking network. 
Fourth, we plan to investigate planning problems in which the topology of the latent space is dependent on the environment and obstacle set specific to a planning problem, e.g., if stairs are involved for a humanoid.
Fifth, we plan to use a similar methodology within a task and motion planning framework by state augmentation in the full state space.
Finally, we plan to investigate conditions under which theoretical guarantees for can be derived.
Beyond , the approach of learning plannable latent spaces through enforced algorithmic primitives can be quite powerful. 
We believe a similar methodology could be used to learn a space directly for trajectory optimization, or instead for lower-dimensional problems, combinatorial motion planning. 



§ ACKNOWLEDGMENTS

The authors wish to thank Edward Schmerling and Anirudha Majumdar for their helpful discussions on this work.



IEEEtran-short


