

\subsection{{Equality Constraint Manifold Neural Network} (ECoMaNN)}
We propose a novel neural network structure -- called \emph{Equality Constraint Manifold Neural Network} (ECoMaNN) -- to become a learning representation that takes $\mathbf{q}$ as input and outputs the prediction of the implicit function ${h}_{\constraintmanifold}(\mathbf{q})$.
Moreover, we would like to train {ECoMaNN} in a supervised manner, from demonstrations. One of the challenges is that the supervised training dataset is collected \emph{only} from demonstrations of data points which are on the equality constraint manifold ${\configspace}_{\constraintmanifold}$, called the \emph{on-manifold} dataset.
This is a reasonable assumption, since collecting both the on-manifold ${\configspace}_{\constraintmanifold}$ and off-manifold ${\configspace}_{\cancel{\constraintmanifold}} = \{ \mathbf{q} \in \mathcal{C} \mid {h}_{\constraintmanifold}(\mathbf{q}) \neq \vct{0} \}$ datasets for supervised training will be tedious because the implicit function ${h}_{\constraintmanifold}$ values of points in ${\configspace}_{\cancel{\constraintmanifold}}$ are typically unknown and hard to label.
We will show that even though our approach is only provided with data on ${\configspace}_{\constraintmanifold}$, it can still learn a useful representation of the manifold, sufficient for use in the SMP framework.

Our goal is to learn a single global representation of the constraint manifold in form of a neural network. 
A manifold can be defined as a collection of local neighborhoods 
which resemble Euclidean spaces (\citet{Lee00_IntroToSmoothManifolds}). Therefore, a global representation of the manifold can be developed by constructing characterizations for its Euclidean-like local neighborhoods.

Our approach leverages local information on the manifold in the form of the tangent and normal spaces (\citet{Deutsch2015_TensorVotingGraph, GStrangIntroLinearAlgebra}).
With regard to ${h}_{\constraintmanifold}$, the tangent and normal spaces are equivalent to the null and row space,
respectively, of the matrix 
$\jacobian_\constraintmanifold = \left. \frac{\partial {h}_{\constraintmanifold}({\mathbf{q}})}{\partial {\mathbf{q}}}\right|_{{\mathbf{q}} = \Bar{\mathbf{q}}}$, 
and valid in a small neighborhood around the point $\Bar{\mathbf{q}}$.

Using on-manifold data, the local information of the manifold can be analyzed using Local Principal Component Analysis (Local PCA) (\citet{Kambhatla_LocalPCA}). Essentially, for each data point $\mathbf{q}$ in the on-manifold dataset, we establish a local neighborhood using $K$-nearest neighbors ($K$NN) $\hat{\KNNset} = \{\hat{\jointposition}_1, \hat{\jointposition}_2, \dots \hat{\jointposition}_K\}$, with $K \geq d$. 
After a change of coordinates, $\mathbf{q}$ becomes the origin of a new local coordinate frame $\mathcal{F}$, and the $K$NN becomes $\tilde{\KNNset} = \{\tilde{\jointposition}_1, \tilde{\jointposition}_2, \dots \tilde{\jointposition}_K\}$ with $\tilde{\jointposition}_k = \hat{\jointposition}_k - \mathbf{q}$ for all values of $k$. Defining the matrix 
$\mathbf{X} = 
\begin{bmatrix} 
\tilde{\jointposition}_1 & \tilde{\jointposition}_2 & \hdots & \tilde{\jointposition}_K \\
\end{bmatrix}^\top \in \Re^{K \times d}
$, we can compute the covariance matrix $\mathbf{S} = \frac{1}{K-1} \mathbf{X}^\top \mathbf{X} \in \Re^{d \times d}$.
The eigendecomposition of $\mathbf{S} = \righteigmat \diagsingularvalues \righteigmat^\top$ gives us the Local PCA. 
The matrix $\righteigmat$ contains the eigenvectors of $\mathbf{S}$ as its columns in decreasing order w.r.t.\ the corresponding eigenvalues in the diagonal matrix $\diagsingularvalues$. These eigenvectors form the basis of $\mathcal{F}$.

This local coordinate frame $\mathcal{F}$ is tightly related to the tangent and normal spaces of the manifold at $\mathbf{q}$. That is, the $(d - l)$ eigenvectors corresponding to the $(d - l)$ biggest eigenvalues of $\diagsingularvalues$ form a basis of the tangent space, while the remaining $l$ eigenvectors form the basis of the normal space. 
Furthermore, due to the characteristics of the manifold from which the dataset was collected, the $l$ smallest eigenvalues of $\diagsingularvalues$ will be close to zero, resulting in the $l$ eigenvectors associated with them forming the basis of the null space of $\mathbf{S}$. On the other hand, the remaining $(d - l)$ eigenvectors form the basis of the row space of $\mathbf{S}$. 

To this end, we present several methods to define and train ECoMaNN, as follows:

\subsection{Local Tangent and Normal Spaces Alignment}
{ECoMaNN} aims to align the following:
\begin{enumerate}[(a)]
    \item the null space of $\jacobian_\constraintmanifold$ and the row space of $\mathbf{S}$, which both must be equivalent to the tangent space, and
    \item the row space of $\jacobian_\constraintmanifold$ and the null space of $\mathbf{S}$, which both must be equivalent to the normal space
\end{enumerate}
for each local neighborhood of each point $\mathbf{q}$ in the on-manifold dataset. 
Suppose the eigenvectors of $\mathbf{S}$ are $\{\vct{v}_1, \vct{v}_2, \dots, \vct{v}_d\}$ and the singular vectors of $\jacobian_\constraintmanifold$ are $\{\vct{e}_1, \vct{e}_2, \dots, \vct{e}_d\}$, where the indices indicate the decreasing order w.r.t. the eigenvalue/singular value magnitude. The null spaces of $\mathbf{S}$ and $\jacobian_\constraintmanifold$ are spanned by $\{\vct{v}_{d-l+1}, \dots, \vct{v}_d\}$ and $\{\vct{e}_{l+1}, \dots, \vct{e}_d\}$, respectively. The two conditions above imply that the projection of the null space eigenvectors of $\jacobian_\constraintmanifold$ into the null space of $\mathbf{S}$ should be $\vct{0}$, and similarly for the row spaces. 
Hence, we achieve this by training {ECoMaNN} to minimize projection errors $\@ifstar{\oldnorm}{\oldnorm*}{\righteigmat_{\nullspaceid} \righteigmat_{\nullspaceid}^\top \constrjacrighteigmat_{\nullspaceid}}_2^2$ and $\@ifstar{\oldnorm}{\oldnorm*}{ \constrjacrighteigmat_{\nullspaceid} \constrjacrighteigmat_{\nullspaceid}^\top \righteigmat_{\nullspaceid}}_2^2$ with 
$\righteigmat_{\nullspaceid} = 
\begin{bmatrix}
    \vct{v}_{d-l+1} & \dots & \vct{v}_d
\end{bmatrix}$
and 
$\constrjacrighteigmat_{\nullspaceid} =  
\begin{bmatrix}
    \vct{e}_{l+1} & \dots & \vct{e}_d
\end{bmatrix}$.

\subsection{Data Augmentation with Off-Manifold Data}
The training dataset is on-manifold, i.e., each point $\mathbf{q}$ in the dataset satisfies ${h}_{\constraintmanifold}(\mathbf{q}) = \vct{0}$. 
Through Local PCA on each of these points, we know the data-driven approximation of the normal space of the manifold at $\mathbf{q}$. 
Hence, we know the directions where the violation of the equality constraint increases, i.e., the same or opposite direction of any vector from the approximate normal space. 
Since our future use of the learned constraint manifold on motion planning does not require the acquisition of the near-ground-truth value of ${h}_{\constraintmanifold}(\mathbf{q}) \neq \vct{0}$, we can set this off-manifold valuation of ${h}_{\constraintmanifold}$ arbitrarily, as long as it does not interfere with the utility for projecting an off-manifold point onto the manifold. 
Therefore, we can augment our dataset with additional off-manifold data to achieve a more robust learning of {ECoMaNN}. 
For each point $\mathbf{q}$ in the on-manifold dataset, and for each eigenvector $\vct{v}$ that forms the basis of the normal space at $\mathbf{q}$, we can add an off-manifold point $\check{\jointposition} = \mathbf{q} + i \epsilon \vct{v}$ with a non-zero signed integer $i$ and 
a small positive scalar $\epsilon$.
For such an augmented data point $\check{\jointposition}$, we set the label satisfying $\@ifstar{\oldnorm}{\oldnorm*}{{h}_{\constraintmanifold}(\check{\jointposition})}_2 = \@ifstar{\oldabs}{\oldabs*}{i} \epsilon$. During training, we minimize the prediction error $\@ifstar{\oldnorm}{\oldnorm*}{(\@ifstar{\oldnorm}{\oldnorm*}{{h}_{\constraintmanifold}(\check{\jointposition})}_2 - \@ifstar{\oldabs}{\oldabs*}{i} \epsilon)}_2^2$ for each augmented point $\check{\jointposition}$.



\section{Discussion and Future Work}
\label{sec:discussion}
In this paper, we presented ways of learning constraint manifolds for sequential manifold planning. One of them is the novel {Equality Constraint Manifold Neural Network} ({ECoMaNN}). {ECoMaNN} is a method for learning representation for implicit functions, with an emphasis on representing equality constraints, while VAEs can also learn inequality constraints. We showed that {ECoMaNN} has successfully learned equality constraint manifolds and that these manifolds can be used in a sequential motion planning method.

There are several interesting improvements and future directions to pursue.
First, there are still limitations with the current approach; in particular, our approach does not address the sign/polarity assignments of the implicit function value output, which we plan to address.
Second, we plan to do more comprehensive testing on higher-dimensional manifolds, and incorporate multiple learned constraints into a single motion plan with more subtasks. Further, we also plan to integrate manifolds learned with VAE into motion planning algorithms.

\section{Use in planning}
\label{sec:use_in_planning}
\section{Related work}
\label{sec:related_work}

\subsection{Constrained SBMP} \label{csbmp} Sampling-based motion planning
(SBMP) is a broad field which tackles the problem of motion planning by using
randomized sampling techniques to build a tree or graph of configurations (also
called samples). The resulting tree or graph can then be used to plan paths
between configurations. This is in contrast to other motion planning techniques,
which may optimize an objective function over a continuous space of trajectories
(e.g., \citet{kalakrishnan2011stomp}) or use the gradient of a potential field
to guide a path toward a goal configuration (e.g., \citet{khatib1986real}).
Many SBMP algorithms derive from one of the following SBMP methods:
Rapidly-exploring random tree (RRT) (\citet{lavalle1998rapidly,
lavalle2006planning}), probabilistic roadmap (PRM)
(\citet{kavraki1994randomized}), and their optimal counterparts RRT* and PRM*
(\citet{karaman2011sampling}). These methods work by sampling the search space,
connecting samples if the edge between them does not collide with obstacles, and
then querying the tree or graph for a valid path that reaches the goal state. A
collision detector is generally assumed to exist, which reports whether or not a
state is in collision with obstacles in the environment. 

A more challenging and realistic motion planning task is that of constrained
SBMP, where there are motion constraints beyond simply obstacle avoidance.
Constraints will often lead to a free configuration space manifold $C_{free} \in
\mathbb{R}^{d'}$ of lower dimension than the ambient configuration space $C \in
\mathbb{R}^d$. (For example, consider the constraint where an arm must stay at a
particular height. A joint that moves the arm up and down will have an effective
space of only one value, and hence the rest of the configuration space is
"squashed" down a dimension.) Randomized sampling in a $d$-dimensional space
will produce samples on a $d' < d$ -dimensional space with probability 0, so
additional measures must be taken to produce valid samples for planning. Another
complication arises when there is not just one constraint, but many, which may
have a specific order in which they must be met in order to achieve the task
goal.

\subsection{Manifold Learning}
\section{Experiments}
\label{sec:experiments}
We compare the proposed {ECoMaNN} method to a Variational Autoencoder (VAE), which is a popular method for learning a generative
model of a set of data (\citet{chen2016dynamic, kingma2013AutoEncodingVB, park2018multimodal}). Importantly, because
they embed data points as a distribution in the latent space, new latent vectors
can be sampled and decoded into unseen examples which fit the distribution of
the training data. VAEs make use of two neural networks in a neural autoencoder
structure during training, and they only use the decoder during generation. The
key idea that makes VAEs computationally tractable is that the distribution in
the latent space is assumed to be Gaussian. The loss function is a combination of the reconstruction error of the input and the KL divergence of the latent space distribution, weighted by a parameter $\beta$. 

We use the following network structures and parameters: 
For the Nav dataset, the VAE has two hidden layers with 6 and 4 units. The input size is 2 and the embedding size is 2. 
For the Plane dataset, the VAE has three hidden layers with 12, 9, and 6 units. The input size is 3 and the embedding size is 2.
For the Sphere, Orient, Tilt, and Handover datasets, the VAEs have the same structure: Four hidden layers with 8, 6, 6, and 4 units. The input sizes to the networks are 3, 6, 6, and 8, and the embedding sizes are 2, 5, 3, and 7, respectively.
All VAE models have $\beta$ = 0.25 and use batch normalization. We train for 500 epochs for Handover, and 200 otherwise.

\subsection{Evaluate Implicit Functions on Datasets}
\label{sec:experiment1}
We compare the performance of the models using the implicit function value ${h}_{\constraintmanifold}$. In the case of the VAE models, we take the reprojected data $\hat{X}$ and evaluate each configuration with ${h}_{\constraintmanifold}$. In the case of the ECoMaNN, the output of the network is the estimated implicit function value of the input, so we can directly use it. We report the mean and standard deviation of ${h}_{\constraintmanifold}$ for each dataset in Table \ref{table:experiment1}. Note that for Nav and Tilt datasets, $h_M$ does not need to be 0 for a configuration to be valid, since these are inequality constraints. Values less than 1 for Nav and less than 0.1 for Tilt adhere to the constraints.

In Fig.~\ref{sfig:levelset_contour_and_jac_vector_field}, we plot the level set contour as well as the normal space eigenvector field of an {ECoMaNN} after training on a 3D unit sphere constraint dataset. We see that at both cross-sections $y=0$ (left) and $z=0$ (right), the contours are close to circular, which is expected for a unit sphere constraint manifold.

\begin{figure}
    \centering
    \begin{subfigure}[b]{0.66\columnwidth}
        \centering
        \includegraphics[width=\columnwidth]{figures/contourplot_ltsann_epoch_25.png}
        \vspace{-0.8cm}
        \caption{Level set contour plot and the learned {ECoMaNN}'s normal space eigenvector field, after training on a 3D unit sphere constraint dataset.}
        \label{sfig:levelset_contour_and_jac_vector_field}
    \end{subfigure}
    \hfill
    \begin{subfigure}[b]{0.3\columnwidth}
        \centering
        \includegraphics[width=\columnwidth]{figures/hourglass_learned.png}
        \caption{Visualization of a planned path on a learned manifold (sphere).}
        \label{sfig:hourglass_learned}
    \end{subfigure}
\end{figure}

\subsection{Evaluate Implicit Functions on Generated Configurations}
\label{sec:experiment2}
We use the trained VAE models from Sec. \ref{sec:experiment1} to generate 100,000 new, on-manifold configurations for each constraint. We then evaluate these configurations with the implicit function $h_M$ and report the mean and standard deviation in Table \ref{table:experiment2}.

\subsection{Sequential Motion Planning on Learned Manifolds}
\label{sec:experiment3}
In this experiment, we incorporate a learned manifold into the planning framework developed and introduced in \citet{englert2020sampling}. The Sphere dataset (see Section \ref{sec:datasets}) is used to learn a manifold representation with {ECoMaNN}. This learned manifold is combined with two analytical manifolds representing paraboloids. A motion planning problem is defined on these three manifolds where a 3D point has to find a path from a start configuration on one of the paraboloids to a goal configuration on the other. See Figure \ref{sfig:hourglass_learned} for a visualization of the manifolds and a found path with SMP.

\begin{table}
  \caption{Experiment A -- ${h}_{\constraintmanifold}$ on Datasets.}
  \label{table:experiment1}
  \centering
  \resizebox{\columnwidth}{!}{%
  \begin{tabular}{ccc|cc|cc|cc|cc|cc}
    \toprule
         Method & \multicolumn{12}{c}{Dataset} \\
     & \multicolumn{2}{c}{Nav} & \multicolumn{2}{c}{Sphere} & \multicolumn{2}{c}{Plane} & \multicolumn{2}{c}{Orient} & \multicolumn{2}{c}{Tilt} & \multicolumn{2}{c}{Handover} \\
     \midrule
              & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ \\
    \midrule
      VAE     & 0.42 & 0.18 & 0.08 & 0.03 & 0.16 & 0.12 & 0.01 & 0.03 & 0.09 & 0.07 & 0.56 & 0.47 \\
   
      ECoMaNN     & N/A & N/A & 0.04 & 0.06 & 0.46 & 1.39 & -0.05 & 0.93 & N/A & N/A & 0.24 & 0.76 \\
    \bottomrule
  \end{tabular}
  }
\end{table}

\begin{table}
    \caption{Experiment B -- ${h}_{\constraintmanifold}$ on Generated Configurations.}
  \label{table:experiment2}
  \centering
  \resizebox{\columnwidth}{!}{%
  \begin{tabular}{ccc|cc|cc|cc|cc|cc}
    \toprule
         Method & \multicolumn{12}{c}{Dataset} \\
     & \multicolumn{2}{c}{Nav} & \multicolumn{2}{c}{Sphere} & \multicolumn{2}{c}{Plane} & \multicolumn{2}{c}{Orient} & \multicolumn{2}{c}{Tilt} & \multicolumn{2}{c}{Handover} \\
     \midrule
              & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ \\
    \midrule
      VAE     & 0.58 & 0.18 & 0.10 & 0.03 & 0.14 & 0.08 & 0.01 & 0.01 & 0.96 & 0.30 & 0.10 & 0.02 \\
    \bottomrule
  \end{tabular}
  }
\end{table}


\section{Datasets}
\label{sec:datasets}
We use a robot simulator (\citet{todorov2012mujoco}) to generate various datasets. For each dataset, we define ${h}_{\constraintmanifold}(\mathbf{q})$ by hand and randomly sample points in the configuration space and project them onto the manifold. We use six datasets:
\begin{itemize}
    \item \textbf{Nav}: 2D point that has to stay close to a reference point. Defined as an inequality constraint. $N=15000$.
    \item \textbf{Sphere}: 3D point that has to stay on the surface of a sphere. $N=10000$.
   
    \item \textbf{Plane}: Robot arm with 3 rotational DOFs where the end effector has to be on a plane. $N=999$.
   
    \item \textbf{Orient}: Robot arm with 6 rotational DOFs that has to keep its orientation upright (e.g., transporting a cup). $N=21153$.
    \item \textbf{Tilt}: Same as Orient, but here the orientation constraint is relaxed to an inequality constraint. $N=2000$.
    \item \textbf{Handover}: Robot arm with 6 rotational DOFs and a mobile base with 2 translational DOFs. The manifold is defined as an equality constraint that describes the handover of an object between the two robots. $N=2002$.
\end{itemize}



\begin{comment}
\subsection{Data production}
Data for this problem are produced in simulation following
\citet{englert2020sampling}: Constraints are written in the form of equalities in
configuration space. Configurations are sampled randomly in valid configuration
space, and they are subsequently projected onto the constraint manifold using
iterative gradient descent (\citet{kingston2018sampling}). In the case that
projection fails to produce a valid point (e.g., because of local minima
encountered in the configuration space), the sample is rejected and a new point
is sampled.

Configurations that do not adhere to the constraints (``off-manifold data'') can
be computed easily by randomly sampling the configuration space, evaluating the
constraint function at that point, and, if desired, rejecting if it is below a
threshold. Alternatively, near-on-manifold configurations may be kept as a way
to introduce noise to the dataset. In general, the randomly selected
configurations will have probability 0 of being on the manifold, since the
constraint defines an implicit set of measure zero in the configuration space,
and thus producing a large off-manifold dataset can be simple.

The data is represented as an $n$ by $d$ matrix, where $n$ is the number of
samples (configurations) and $d$ is the dimension (DOF) of the configuration
space. Note that for a simple robotic scenario, the environment could consist of
a single arm with just 3 DOFs. In this case, the data matrix is $n$ by $3$.
However, in most practical applications, robots will each have at least 6 and
possibly many more DOFs. In this case, the data matrix will be of size $n$ by
$\sum_a d_a$ where $d_a$ denotes the number of DOFs of robot $a$ in the
environment.

We learn models for the following constraints:
\begin{enumerate}
\item 2DOF: Stay within 1 unit radius of a point of interest (Nav)
\item 3DOF: Stay on a sphere (Sphere)
\item 3DOF: Keep end effector on a 2D line (3DOF)
\item 6DOF: Keep end effector at a specific orientation (6DOF)
\item 6DOF: Keep end effector within a small deviation of an orientation (Tilt)
\item 8DOF: Pass an object between two robots (Handover)
\end{enumerate}

\subsection{Nav dataset}
\label{nav_dataset_section}
This dataset is a 2D inequality constraint where the position of a robot represented as a 2-dimensional vector should stay within a certain distance of a point of interest. This scenario is meant as a simple example of a navigation task where a robot's motion plan may be restricted by the communication limitations of a central tower. The data matrix is of size

\subsection{Sphere dataset}
In the Sphere dataset, the constraint is a geometric one, constraining the configuration to lie on the surface of a sphere in 3D space. The data matrix is of size

\subsection{3DOF dataset}
In the ``3DOF dataset", the constraint is to maintain the end effector of a
planar 3-link robot arm on a line. The data matrix is of size $999$ by
$3$. 


\subsection{6DOF dataset}
The ``6DOF dataset" is created with the constraint to maintain the end
effector of a 6-DOF robot arm pointed down (i.e., keep the orientation constant). The
data matrix is of size $21153$ by $6$. 

\subsection{Tilt dataset}
The Tilt dataset, like Nav, is an inequality constraint. Here, we model the scenario where a robot arm must keep its orientation within an allowable deviation from pointing straight down. The data matrix is of size $2000$ by $6$. 

\subsection{Handover dataset}
The Handover dataset is the most complex task we model in this work. It includes two robots, one mobile and one arm. The arm end effector must meet the top of the mobile robot in the workspace, allowing the arm to hand an object over to the other for transport. The data matrix is of size $2002$ by $8$. 

\end{comment}
\section{Manifold Learning}
\label{sec:manifold_learning}
Learning constraint manifolds from data is attractive for multiple reasons. 
For example, it may be easier for a human to demonstrate a task rather than specifying constraints analytically,
or we may want to reduce the amount of expert information needed. 

\begin{comment}
\subsection{Problem Statement}
Our goal here is to focus on a single manifold as opposed to the entire sequence. We
do not assume the analytical description of the manifold (e.g., $h_M$) is known.
Rather, we have a set of data $x \in \mathbb{R}^d$. These $n$ datapoints are
configurations in $\mathbb{R}^n$ which adhere to the constraint $M$. That is, $x_i \in C_M \forall x_i \in
X$. We seek a model $A_\theta$, learned using our dataset $X$, which encodes an approximate representation $\hat{M}$ of the manifold constraint in a lower-dimensional space in
$\mathbb{R}^{\hat{d}}, \hat{d} \leq d$. 

For the purposes of planning, we are mainly interested in the capability of the learned model to reconstruct
the data accurately. In particular, we would like $\hat{M}$ learned by
$A_\theta$ to be able to transform data points between the original (ambient)
space in $\mathbb{R}^d$ and the learned embedding space in
$\mathbb{R}^{\hat{d}}$. 
To achieve this, we use the mean squared error (MSE) of $\hat{x}$ and $x$ as a
metric, which we call the ``reprojection error" and would like to minimize: 
$$ MSE = \frac{1}{n} \sum_{i}^n (\hat{x}_i - x_i)^2 $$
A low MSE indicates that the learned latent embedding of the manifold estimates
the dataset well, and therefore it is an accurate representation of the manifold
constraint. 

Additionally, we consider the ``?'' metric. Given a point $X$ which originally was on the manifold (i.e., $h(x) = 0$) and its reprojected image $\hat{x}$, we define the metric
as $h(\hat{x})$. This value should ideally also be $0$, indicating that even if the point is not exactly reconstructed as before, it is still a valid configuration for the given constraint.

Because constraints will often be in a lower dimension than the ambient configuration space, this motivates the use of dimensionality reduction techniques on data. 
In general, the constraint manifolds may have arbitrary structure; we assume they
have been sampled sufficiently densely but otherwise make no further
assumptions. 


\subsection{Variational Autoencoders}
Variational Autoencoders (VAEs) are a popular method for learning a generative
model of a set of data (\citep{chen2016dynamic, kingma2013AutoEncodingVB, habibie2017recurrent, park2018multimodal}). Importantly, because
they embed data points as a distribution in the latent space, new latent vectors
can be sampled and decoded into unseen examples which fit the distribution of
the training data. VAEs make use of two neural networks in a neural autoencoder
structure during training, and they only use the decoder during generation. The
key idea that makes VAEs computationally tractable is that the distribution in
the latent space is assumed to be Gaussian. 
\end{comment}

\begin{comment}
\subsection{Singular Value Decomposition}
Principal Component Analysis (PCA) (\citet{tipping1999probabilistic}) and
Singular Value Decomposition (SVD) (\citet{halko2011finding}) are used for linear
dimensionality reduction. PCA is a method whose goal, given a set of $n$ data points $X \in
\mathbb{R}^d$, is to find a set of $k < d$ orthogonal vectors which explain the
most variance in the dataset. Several techniques can be used to find these
orthogonal vectors (also known as principal axes); SVD is one of them.
In essence, SVD decomposes an $n$ by $d$ data matrix $A$ into the diagonal form 
$A = USV^T$
where $S$ is a diagonal matrix that contains the singular values $\sigma_i$ (the
square roots of the eigenvalues) of $A$, and $U$ and $V$ are composed of the
eigenvectors of $AA^T$ and $A^TA$, respectively. With $\sigma$ in decreasing
order, the first $k$ columns of $V$ ($V_k$) can be used as the principal axes
for the data (see \citet{shlens2014tutorial} for a more in-depth explanation).
The data $X$ can be transformed into the new $k$-dimensional space  via
projection: $Y = XV_k$
and, unlike LLE, transformed data can be reprojected easily with $V_k^T$ since
$V_k$ is an orthonormal matrix: $\hat{X} = YV_k^T$.

\subsection{Locally Linear Embedding}

Locally Linear Embedding (LLE) produces a global embedding based on local structure. The idea is to first use a set of $k$ nearest neighbors of each point to linearly reconstruct a point $\vec{X}_i$ as close to its true value as possible.
Secondly, using these linear reconstruction weights $W_{ij}$, LLE computes
lower-dimensional embedding vectors of each point, which are computed such that they minimize the difference between the embedding and a linear reconstruction of the embeddings of its nearest neighbors using the same $W_{ij}$. Thus, LLE assumes that local structure is preserved in the learned embedding.

One of the main drawbacks of LLE for our work is
that it does not encode a method for reprojecting embeddings into the ambient space. Additionally, if the
manifold is tightly folded (imagine a tightly wound spring), points on a loop
above or below $\vec{X}_i$ may be chosen as nearest neighbors when in reality the
geodesic distance between them may be quite large. 




\subsection{Isomap}
Isomap, named for isometric mapping, is a manifold learning technique that
approximates geodesic distances via a graph constructed from the data $X$
(\citet{tenenbaum2000global}). Similarly to LLE, each point $\vec{X}_i$ has a
nearest neighborhood $K_i$ of size $k$. However, this neighborhood is not
used to reconstruct $\vec{X}_i$. Rather, a graph is constructed
where the vertices $V$ represent the data points in $X$, and the edges $E$
connect points in a neighborhood. Then, finding the
shortest path between two nodes using an algorithm such as Dijkstra's is
equivalent to approximating the geodesic distance on the manifold between two
data points. After computing all pairwise distances, the final step to produce
an embedding is to apply Multidimensional Scaling (MDS, \citet{borg2005modern},
\citet{kruskal1978multidimensional}) to $D$. MDS uses eigenvalue decomposition
given a distance or dissimilarity matrix (here, $D$) to produce data $\hat{X}$
that preserve these distances. Like LLE, Isomap lacks the ability to transform
embedded data points back into the original space and may not
produce accurate results if the manifold is very crumpled or folded. 
\end{comment}

\input{ecmnn}

\section{Background on Sequential Manifold Planning}
\label{sec:background}
We focus on learning manifolds that describe kinematic robot tasks. We aim to integrate these learned manifolds into the sequential manifold planning (SMP) framework proposed in \citet{englert2020sampling}.
SMP considers kinematic motion planning problems in a configuration space $\mathcal{C} \subseteq \Re^{d}$. A robot configuration 
$\mathbf{q} \in \mathcal{C}$
describes the state of one or more robots with 
$d$
degrees of freedom in total.
A manifold 
$M$
is represented as an equality constraint 
${h}_{\constraintmanifold}(\mathbf{q}) = \vct{0}$
where 
${h}_{\constraintmanifold}: \Re^{d} \rightarrow \Re^{l}$
and $l$ is the dimensionality of the implicit manifold.
The set of robot configurations that are on a manifold 
$M$
is given by 
\mbox{${\configspace}_{\constraintmanifold} = \{ \mathbf{q} \in \mathcal{C} \mid {h}_{\constraintmanifold}(\mathbf{q}) = \vct{0} \}~.$}
SMP defines the planning problem as a sequence of 
$(n+1)$
such manifolds 
\mbox{$\mathcal{M} = \{M_1, M_2, \dots, M_{n+1}\}$}
and an initial configuration 
$\mathbf{q}_\t{start} \in \mathcal{C}_{{M}_1}$ 
that is on the first manifold.
The goal is to find a path from 
$\mathbf{q}_\t{start}$ 
that traverses the manifold sequence 
$\mathcal{\constraintmanifold}$
and reaches a configuration on the goal manifold 
$M_{n+1}$. 
A path on the $i$-th manifold is defined as 
$\tau_i : [0, 1] \to {{\configspace}_{\constraintmanifold}}_{i}$
and 
$J(\tau_i)$ 
is the cost function of a path
$\mathcal{\cost} : \mathcal{T} \to \Re_{\geq 0}$
where 
$\mathcal{T}$ 
is the set of all non-trivial paths. The problem is formulated as an optimization over a set of paths 
$\vct{\tpath} = (\tau_1, \dots, \tau_{n})$ 
that minimizes the sum of path costs under the constraints of traversing 
$\mathcal{\constraintmanifold}$ 
and of being collision-free:
\begin{align} %
	\begin{alignedat}{2}
	\label{eq:smp_problem}
	&\vct{\tpath}^{\star} = \operatorname*{arg\:min}_{\vct{\tpath}} \sum_{i=1}^{n} J(\tau_i) &&\\
	\t{s.t.}\quad &{\tau}_1 (0) = {\mathbf{q}}_\t{start} &\\
	&\tau_i(1) = \tau_{i+1}(0)  &&\forall_{i=1,\dots,n-1} \\ 
	&\mathcal{C}_{\t{free}, i+1} = \Upsilon(\mathcal{C}_{\t{free}, i}, \tau_{i})~~~~ &&\forall_{i=1,\dots,n}\\
	&\tau_i(s) \in {{\configspace}_{\constraintmanifold}}_{i} \cap \mathcal{C}_{\t{free}, i} &&\forall_{i=1,\dots,n}~ \forall_{s \in [0, 1]}\\
	&\tau_{n}(1) \in {{\configspace}_{\constraintmanifold}}_{n+1} \cap \mathcal{C}_{\t{free}, n+1} &&
	\end{alignedat} %
\end{align}
$\Upsilon$
is an operator that describes the change in the free configuration space (the space of all configurations that are not in collision with the environment)
$\mathcal{C}_\t{free}$ 
when transitioning to the next manifold. The SMP algorithm is able to solve this problem for a certain class of problems. It iteratively applies RRT${}^\star$ to find a path that reaches the next manifold while staying on the current manifold.
For further details of the SMP algorithm, we refer the reader to \citet{englert2020sampling}. 

In this paper, we employ data-driven algorithms to learn manifolds $M$ from data with the goal to integrate them into the SMP framework.

\section{Introduction}
\label{sec:introduction}

Many practical robotic applications require planning robot motions with constraints, such as maintaining an orientation or reaching a particular location. Planning becomes more complicated when the  task consists of many subtasks that must be completed in sequence. In this case, task and motion planning frameworks \cite{kaelbling2013integrated, dantam2016incremental, konidaris2018skills, toussaint2018differentiable, dantam2018task, barry2013hierarchical, cambon2009hybrid} can be used to handle long planning horizons and a wide range of tasks. 
However, some constraints may be difficult to describe analytically, or it may be difficult to sample constraints that adhere to them. For example, if the task is to pour a liquid from a bottle into a cup, it is not immediately clear how to encode the motion constraints for a planning algorithm.

In this work, we focus on learning constraint manifolds for use in constrained motion planning algorithms \cite{englert2020sampling, stilman2010global, berenson2011task, jaillet2013asymptotically, kim2016tangent, kingston2019ijrr, csucan2012motion, cortes2004sampling}.
To this end, we investigate two different approaches: Variational autoencoders (VAE) following \citet{kingma2013AutoEncodingVB}, and {Equality Constraint Manifold Neural Network}  (ECoMaNN), a method we propose which learns the implicit function value of equality constraints. We evaluate these techniques on six datasets of varying size and complexity, and we present preliminary resulting motion plans.

\begin{comment}


To begin, we evaluate several manifold learning techniques on multiple datasets.
The techniques we discuss in this work are:
Variational Autoencoders (VAE); 
and 
{Equality Constraint Manifold Neural Network} ({ECoMaNN}).
We evaluate these models on 6 datasets of varying size and complexity.

First we give a summary of the planning framework. Then, we discuss motivation
for learning manifolds from data, go over the techniques which are
considered, and briefly cover some related work. Next, we describe each dataset.
Then, we show results for each technique. Finally, we discuss how the learned
manifold constraints can be incorporated into a robotic motion planner, and we 
conclude with a summary of the findings and the next steps for this work. 
\end{comment}

\section*{Acknowledgments}


\bibliographystyle{plainnat}


\section{Background on Sequential Manifold Planning}
\label{sec:background}
We focus on learning manifolds that describe kinematic robot tasks. We aim to integrate these learned manifolds into the sequential manifold planning (SMP) framework proposed in \citet{englert2020sampling}.
SMP considers kinematic motion planning problems in a configuration space $\mathcal{C} \subseteq \Re^{d}$. A robot configuration 
$\mathbf{q} \in \mathcal{C}$
describes the state of one or more robots with 
$d$
degrees of freedom in total.
A manifold 
$M$
is represented as an equality constraint 
${h}_{\constraintmanifold}(\mathbf{q}) = \vct{0}$
where 
${h}_{\constraintmanifold}: \Re^{d} \rightarrow \Re^{l}$
and $l$ is the dimensionality of the implicit manifold.
The set of robot configurations that are on a manifold 
$M$
is given by 
\mbox{${\configspace}_{\constraintmanifold} = \{ \mathbf{q} \in \mathcal{C} \mid {h}_{\constraintmanifold}(\mathbf{q}) = \vct{0} \}~.$}
SMP defines the planning problem as a sequence of 
$(n+1)$
such manifolds 
\mbox{$\mathcal{M} = \{M_1, M_2, \dots, M_{n+1}\}$}
and an initial configuration 
$\mathbf{q}_\t{start} \in \mathcal{C}_{{M}_1}$ 
that is on the first manifold.
The goal is to find a path from 
$\mathbf{q}_\t{start}$ 
that traverses the manifold sequence 
$\mathcal{\constraintmanifold}$
and reaches a configuration on the goal manifold 
$M_{n+1}$. 
A path on the $i$-th manifold is defined as 
$\tau_i : [0, 1] \to {{\configspace}_{\constraintmanifold}}_{i}$
and 
$J(\tau_i)$ 
is the cost function of a path
$\mathcal{\cost} : \mathcal{T} \to \Re_{\geq 0}$
where 
$\mathcal{T}$ 
is the set of all non-trivial paths. The problem is formulated as an optimization over a set of paths 
$\vct{\tpath} = (\tau_1, \dots, \tau_{n})$ 
that minimizes the sum of path costs under the constraints of traversing 
$\mathcal{\constraintmanifold}$ 
and of being collision-free:
\begin{align} %
	\begin{alignedat}{2}
	\label{eq:smp_problem}
	&\vct{\tpath}^{\star} = \operatorname*{arg\:min}_{\vct{\tpath}} \sum_{i=1}^{n} J(\tau_i) &&\\
	\t{s.t.}\quad &{\tau}_1 (0) = {\mathbf{q}}_\t{start} &\\
	&\tau_i(1) = \tau_{i+1}(0)  &&\forall_{i=1,\dots,n-1} \\ 
	&\mathcal{C}_{\t{free}, i+1} = \Upsilon(\mathcal{C}_{\t{free}, i}, \tau_{i})~~~~ &&\forall_{i=1,\dots,n}\\
	&\tau_i(s) \in {{\configspace}_{\constraintmanifold}}_{i} \cap \mathcal{C}_{\t{free}, i} &&\forall_{i=1,\dots,n}~ \forall_{s \in [0, 1]}\\
	&\tau_{n}(1) \in {{\configspace}_{\constraintmanifold}}_{n+1} \cap \mathcal{C}_{\t{free}, n+1} &&
	\end{alignedat} %
\end{align}
$\Upsilon$
is an operator that describes the change in the free configuration space (the space of all configurations that are not in collision with the environment)
$\mathcal{C}_\t{free}$ 
when transitioning to the next manifold. The SMP algorithm is able to solve this problem for a certain class of problems. It iteratively applies RRT${}^\star$ to find a path that reaches the next manifold while staying on the current manifold.
For further details of the SMP algorithm, we refer the reader to \citet{englert2020sampling}. 

In this paper, we employ data-driven algorithms to learn manifolds $M$ from data with the goal to integrate them into the SMP framework.

\section{Datasets}
\label{sec:datasets}
We use a robot simulator (\citet{todorov2012mujoco}) to generate various datasets. For each dataset, we define ${h}_{\constraintmanifold}(\mathbf{q})$ by hand and randomly sample points in the configuration space and project them onto the manifold. We use six datasets:
\begin{itemize}
    \item \textbf{Nav}: 2D point that has to stay close to a reference point. Defined as an inequality constraint. $N=15000$.
    \item \textbf{Sphere}: 3D point that has to stay on the surface of a sphere. $N=10000$.
   
    \item \textbf{Plane}: Robot arm with 3 rotational DOFs where the end effector has to be on a plane. $N=999$.
   
    \item \textbf{Orient}: Robot arm with 6 rotational DOFs that has to keep its orientation upright (e.g., transporting a cup). $N=21153$.
    \item \textbf{Tilt}: Same as Orient, but here the orientation constraint is relaxed to an inequality constraint. $N=2000$.
    \item \textbf{Handover}: Robot arm with 6 rotational DOFs and a mobile base with 2 translational DOFs. The manifold is defined as an equality constraint that describes the handover of an object between the two robots. $N=2002$.
\end{itemize}



\begin{comment}
\subsection{Data production}
Data for this problem are produced in simulation following
\citet{englert2020sampling}: Constraints are written in the form of equalities in
configuration space. Configurations are sampled randomly in valid configuration
space, and they are subsequently projected onto the constraint manifold using
iterative gradient descent (\citet{kingston2018sampling}). In the case that
projection fails to produce a valid point (e.g., because of local minima
encountered in the configuration space), the sample is rejected and a new point
is sampled.

Configurations that do not adhere to the constraints (``off-manifold data'') can
be computed easily by randomly sampling the configuration space, evaluating the
constraint function at that point, and, if desired, rejecting if it is below a
threshold. Alternatively, near-on-manifold configurations may be kept as a way
to introduce noise to the dataset. In general, the randomly selected
configurations will have probability 0 of being on the manifold, since the
constraint defines an implicit set of measure zero in the configuration space,
and thus producing a large off-manifold dataset can be simple.

The data is represented as an $n$ by $d$ matrix, where $n$ is the number of
samples (configurations) and $d$ is the dimension (DOF) of the configuration
space. Note that for a simple robotic scenario, the environment could consist of
a single arm with just 3 DOFs. In this case, the data matrix is $n$ by $3$.
However, in most practical applications, robots will each have at least 6 and
possibly many more DOFs. In this case, the data matrix will be of size $n$ by
$\sum_a d_a$ where $d_a$ denotes the number of DOFs of robot $a$ in the
environment.

We learn models for the following constraints:
\begin{enumerate}
\item 2DOF: Stay within 1 unit radius of a point of interest (Nav)
\item 3DOF: Stay on a sphere (Sphere)
\item 3DOF: Keep end effector on a 2D line (3DOF)
\item 6DOF: Keep end effector at a specific orientation (6DOF)
\item 6DOF: Keep end effector within a small deviation of an orientation (Tilt)
\item 8DOF: Pass an object between two robots (Handover)
\end{enumerate}

\subsection{Nav dataset}
\label{nav_dataset_section}
This dataset is a 2D inequality constraint where the position of a robot represented as a 2-dimensional vector should stay within a certain distance of a point of interest. This scenario is meant as a simple example of a navigation task where a robot's motion plan may be restricted by the communication limitations of a central tower. The data matrix is of size

\subsection{Sphere dataset}
In the Sphere dataset, the constraint is a geometric one, constraining the configuration to lie on the surface of a sphere in 3D space. The data matrix is of size

\subsection{3DOF dataset}
In the ``3DOF dataset", the constraint is to maintain the end effector of a
planar 3-link robot arm on a line. The data matrix is of size $999$ by
$3$. 


\subsection{6DOF dataset}
The ``6DOF dataset" is created with the constraint to maintain the end
effector of a 6-DOF robot arm pointed down (i.e., keep the orientation constant). The
data matrix is of size $21153$ by $6$. 

\subsection{Tilt dataset}
The Tilt dataset, like Nav, is an inequality constraint. Here, we model the scenario where a robot arm must keep its orientation within an allowable deviation from pointing straight down. The data matrix is of size $2000$ by $6$. 

\subsection{Handover dataset}
The Handover dataset is the most complex task we model in this work. It includes two robots, one mobile and one arm. The arm end effector must meet the top of the mobile robot in the workspace, allowing the arm to hand an object over to the other for transport. The data matrix is of size $2002$ by $8$. 

\end{comment}
\section{Discussion and Future Work}
\label{sec:discussion}
In this paper, we presented ways of learning constraint manifolds for sequential manifold planning. One of them is the novel {Equality Constraint Manifold Neural Network} ({ECoMaNN}). {ECoMaNN} is a method for learning representation for implicit functions, with an emphasis on representing equality constraints, while VAEs can also learn inequality constraints. We showed that {ECoMaNN} has successfully learned equality constraint manifolds and that these manifolds can be used in a sequential motion planning method.

There are several interesting improvements and future directions to pursue.
First, there are still limitations with the current approach; in particular, our approach does not address the sign/polarity assignments of the implicit function value output, which we plan to address.
Second, we plan to do more comprehensive testing on higher-dimensional manifolds, and incorporate multiple learned constraints into a single motion plan with more subtasks. Further, we also plan to integrate manifolds learned with VAE into motion planning algorithms.

\subsection{{Equality Constraint Manifold Neural Network} (ECoMaNN)}
We propose a novel neural network structure -- called \emph{Equality Constraint Manifold Neural Network} (ECoMaNN) -- to become a learning representation that takes $\mathbf{q}$ as input and outputs the prediction of the implicit function ${h}_{\constraintmanifold}(\mathbf{q})$.
Moreover, we would like to train {ECoMaNN} in a supervised manner, from demonstrations. One of the challenges is that the supervised training dataset is collected \emph{only} from demonstrations of data points which are on the equality constraint manifold ${\configspace}_{\constraintmanifold}$, called the \emph{on-manifold} dataset.
This is a reasonable assumption, since collecting both the on-manifold ${\configspace}_{\constraintmanifold}$ and off-manifold ${\configspace}_{\cancel{\constraintmanifold}} = \{ \mathbf{q} \in \mathcal{C} \mid {h}_{\constraintmanifold}(\mathbf{q}) \neq \vct{0} \}$ datasets for supervised training will be tedious because the implicit function ${h}_{\constraintmanifold}$ values of points in ${\configspace}_{\cancel{\constraintmanifold}}$ are typically unknown and hard to label.
We will show that even though our approach is only provided with data on ${\configspace}_{\constraintmanifold}$, it can still learn a useful representation of the manifold, sufficient for use in the SMP framework.

Our goal is to learn a single global representation of the constraint manifold in form of a neural network. 
A manifold can be defined as a collection of local neighborhoods 
which resemble Euclidean spaces (\citet{Lee00_IntroToSmoothManifolds}). Therefore, a global representation of the manifold can be developed by constructing characterizations for its Euclidean-like local neighborhoods.

Our approach leverages local information on the manifold in the form of the tangent and normal spaces (\citet{Deutsch2015_TensorVotingGraph, GStrangIntroLinearAlgebra}).
With regard to ${h}_{\constraintmanifold}$, the tangent and normal spaces are equivalent to the null and row space,
respectively, of the matrix 
$\jacobian_\constraintmanifold = \left. \frac{\partial {h}_{\constraintmanifold}({\mathbf{q}})}{\partial {\mathbf{q}}}\right|_{{\mathbf{q}} = \Bar{\mathbf{q}}}$, 
and valid in a small neighborhood around the point $\Bar{\mathbf{q}}$.

Using on-manifold data, the local information of the manifold can be analyzed using Local Principal Component Analysis (Local PCA) (\citet{Kambhatla_LocalPCA}). Essentially, for each data point $\mathbf{q}$ in the on-manifold dataset, we establish a local neighborhood using $K$-nearest neighbors ($K$NN) $\hat{\KNNset} = \{\hat{\jointposition}_1, \hat{\jointposition}_2, \dots \hat{\jointposition}_K\}$, with $K \geq d$. 
After a change of coordinates, $\mathbf{q}$ becomes the origin of a new local coordinate frame $\mathcal{F}$, and the $K$NN becomes $\tilde{\KNNset} = \{\tilde{\jointposition}_1, \tilde{\jointposition}_2, \dots \tilde{\jointposition}_K\}$ with $\tilde{\jointposition}_k = \hat{\jointposition}_k - \mathbf{q}$ for all values of $k$. Defining the matrix 
$\mathbf{X} = 
\begin{bmatrix} 
\tilde{\jointposition}_1 & \tilde{\jointposition}_2 & \hdots & \tilde{\jointposition}_K \\
\end{bmatrix}^\top \in \Re^{K \times d}
$, we can compute the covariance matrix $\mathbf{S} = \frac{1}{K-1} \mathbf{X}^\top \mathbf{X} \in \Re^{d \times d}$.
The eigendecomposition of $\mathbf{S} = \righteigmat \diagsingularvalues \righteigmat^\top$ gives us the Local PCA. 
The matrix $\righteigmat$ contains the eigenvectors of $\mathbf{S}$ as its columns in decreasing order w.r.t.\ the corresponding eigenvalues in the diagonal matrix $\diagsingularvalues$. These eigenvectors form the basis of $\mathcal{F}$.

This local coordinate frame $\mathcal{F}$ is tightly related to the tangent and normal spaces of the manifold at $\mathbf{q}$. That is, the $(d - l)$ eigenvectors corresponding to the $(d - l)$ biggest eigenvalues of $\diagsingularvalues$ form a basis of the tangent space, while the remaining $l$ eigenvectors form the basis of the normal space. 
Furthermore, due to the characteristics of the manifold from which the dataset was collected, the $l$ smallest eigenvalues of $\diagsingularvalues$ will be close to zero, resulting in the $l$ eigenvectors associated with them forming the basis of the null space of $\mathbf{S}$. On the other hand, the remaining $(d - l)$ eigenvectors form the basis of the row space of $\mathbf{S}$. 

To this end, we present several methods to define and train ECoMaNN, as follows:

\subsection{Local Tangent and Normal Spaces Alignment}
{ECoMaNN} aims to align the following:
\begin{enumerate}[(a)]
    \item the null space of $\jacobian_\constraintmanifold$ and the row space of $\mathbf{S}$, which both must be equivalent to the tangent space, and
    \item the row space of $\jacobian_\constraintmanifold$ and the null space of $\mathbf{S}$, which both must be equivalent to the normal space
\end{enumerate}
for each local neighborhood of each point $\mathbf{q}$ in the on-manifold dataset. 
Suppose the eigenvectors of $\mathbf{S}$ are $\{\vct{v}_1, \vct{v}_2, \dots, \vct{v}_d\}$ and the singular vectors of $\jacobian_\constraintmanifold$ are $\{\vct{e}_1, \vct{e}_2, \dots, \vct{e}_d\}$, where the indices indicate the decreasing order w.r.t. the eigenvalue/singular value magnitude. The null spaces of $\mathbf{S}$ and $\jacobian_\constraintmanifold$ are spanned by $\{\vct{v}_{d-l+1}, \dots, \vct{v}_d\}$ and $\{\vct{e}_{l+1}, \dots, \vct{e}_d\}$, respectively. The two conditions above imply that the projection of the null space eigenvectors of $\jacobian_\constraintmanifold$ into the null space of $\mathbf{S}$ should be $\vct{0}$, and similarly for the row spaces. 
Hence, we achieve this by training {ECoMaNN} to minimize projection errors $\@ifstar{\oldnorm}{\oldnorm*}{\righteigmat_{\nullspaceid} \righteigmat_{\nullspaceid}^\top \constrjacrighteigmat_{\nullspaceid}}_2^2$ and $\@ifstar{\oldnorm}{\oldnorm*}{ \constrjacrighteigmat_{\nullspaceid} \constrjacrighteigmat_{\nullspaceid}^\top \righteigmat_{\nullspaceid}}_2^2$ with 
$\righteigmat_{\nullspaceid} = 
\begin{bmatrix}
    \vct{v}_{d-l+1} & \dots & \vct{v}_d
\end{bmatrix}$
and 
$\constrjacrighteigmat_{\nullspaceid} =  
\begin{bmatrix}
    \vct{e}_{l+1} & \dots & \vct{e}_d
\end{bmatrix}$.

\subsection{Data Augmentation with Off-Manifold Data}
The training dataset is on-manifold, i.e., each point $\mathbf{q}$ in the dataset satisfies ${h}_{\constraintmanifold}(\mathbf{q}) = \vct{0}$. 
Through Local PCA on each of these points, we know the data-driven approximation of the normal space of the manifold at $\mathbf{q}$. 
Hence, we know the directions where the violation of the equality constraint increases, i.e., the same or opposite direction of any vector from the approximate normal space. 
Since our future use of the learned constraint manifold on motion planning does not require the acquisition of the near-ground-truth value of ${h}_{\constraintmanifold}(\mathbf{q}) \neq \vct{0}$, we can set this off-manifold valuation of ${h}_{\constraintmanifold}$ arbitrarily, as long as it does not interfere with the utility for projecting an off-manifold point onto the manifold. 
Therefore, we can augment our dataset with additional off-manifold data to achieve a more robust learning of {ECoMaNN}. 
For each point $\mathbf{q}$ in the on-manifold dataset, and for each eigenvector $\vct{v}$ that forms the basis of the normal space at $\mathbf{q}$, we can add an off-manifold point $\check{\jointposition} = \mathbf{q} + i \epsilon \vct{v}$ with a non-zero signed integer $i$ and 
a small positive scalar $\epsilon$.
For such an augmented data point $\check{\jointposition}$, we set the label satisfying $\@ifstar{\oldnorm}{\oldnorm*}{{h}_{\constraintmanifold}(\check{\jointposition})}_2 = \@ifstar{\oldabs}{\oldabs*}{i} \epsilon$. During training, we minimize the prediction error $\@ifstar{\oldnorm}{\oldnorm*}{(\@ifstar{\oldnorm}{\oldnorm*}{{h}_{\constraintmanifold}(\check{\jointposition})}_2 - \@ifstar{\oldabs}{\oldabs*}{i} \epsilon)}_2^2$ for each augmented point $\check{\jointposition}$.



\section{Experiments}
\label{sec:experiments}
We compare the proposed {ECoMaNN} method to a Variational Autoencoder (VAE), which is a popular method for learning a generative
model of a set of data (\citet{chen2016dynamic, kingma2013AutoEncodingVB, park2018multimodal}). Importantly, because
they embed data points as a distribution in the latent space, new latent vectors
can be sampled and decoded into unseen examples which fit the distribution of
the training data. VAEs make use of two neural networks in a neural autoencoder
structure during training, and they only use the decoder during generation. The
key idea that makes VAEs computationally tractable is that the distribution in
the latent space is assumed to be Gaussian. The loss function is a combination of the reconstruction error of the input and the KL divergence of the latent space distribution, weighted by a parameter $\beta$. 

We use the following network structures and parameters: 
For the Nav dataset, the VAE has two hidden layers with 6 and 4 units. The input size is 2 and the embedding size is 2. 
For the Plane dataset, the VAE has three hidden layers with 12, 9, and 6 units. The input size is 3 and the embedding size is 2.
For the Sphere, Orient, Tilt, and Handover datasets, the VAEs have the same structure: Four hidden layers with 8, 6, 6, and 4 units. The input sizes to the networks are 3, 6, 6, and 8, and the embedding sizes are 2, 5, 3, and 7, respectively.
All VAE models have $\beta$ = 0.25 and use batch normalization. We train for 500 epochs for Handover, and 200 otherwise.

\subsection{Evaluate Implicit Functions on Datasets}
\label{sec:experiment1}
We compare the performance of the models using the implicit function value ${h}_{\constraintmanifold}$. In the case of the VAE models, we take the reprojected data $\hat{X}$ and evaluate each configuration with ${h}_{\constraintmanifold}$. In the case of the ECoMaNN, the output of the network is the estimated implicit function value of the input, so we can directly use it. We report the mean and standard deviation of ${h}_{\constraintmanifold}$ for each dataset in Table \ref{table:experiment1}. Note that for Nav and Tilt datasets, $h_M$ does not need to be 0 for a configuration to be valid, since these are inequality constraints. Values less than 1 for Nav and less than 0.1 for Tilt adhere to the constraints.

In Fig.~\ref{sfig:levelset_contour_and_jac_vector_field}, we plot the level set contour as well as the normal space eigenvector field of an {ECoMaNN} after training on a 3D unit sphere constraint dataset. We see that at both cross-sections $y=0$ (left) and $z=0$ (right), the contours are close to circular, which is expected for a unit sphere constraint manifold.

\begin{figure}
    \centering
    \begin{subfigure}[b]{0.66\columnwidth}
        \centering
        \includegraphics[width=\columnwidth]{figures/contourplot_ltsann_epoch_25.png}
        \vspace{-0.8cm}
        \caption{Level set contour plot and the learned {ECoMaNN}'s normal space eigenvector field, after training on a 3D unit sphere constraint dataset.}
        \label{sfig:levelset_contour_and_jac_vector_field}
    \end{subfigure}
    \hfill
    \begin{subfigure}[b]{0.3\columnwidth}
        \centering
        \includegraphics[width=\columnwidth]{figures/hourglass_learned.png}
        \caption{Visualization of a planned path on a learned manifold (sphere).}
        \label{sfig:hourglass_learned}
    \end{subfigure}
\end{figure}

\subsection{Evaluate Implicit Functions on Generated Configurations}
\label{sec:experiment2}
We use the trained VAE models from Sec. \ref{sec:experiment1} to generate 100,000 new, on-manifold configurations for each constraint. We then evaluate these configurations with the implicit function $h_M$ and report the mean and standard deviation in Table \ref{table:experiment2}.

\subsection{Sequential Motion Planning on Learned Manifolds}
\label{sec:experiment3}
In this experiment, we incorporate a learned manifold into the planning framework developed and introduced in \citet{englert2020sampling}. The Sphere dataset (see Section \ref{sec:datasets}) is used to learn a manifold representation with {ECoMaNN}. This learned manifold is combined with two analytical manifolds representing paraboloids. A motion planning problem is defined on these three manifolds where a 3D point has to find a path from a start configuration on one of the paraboloids to a goal configuration on the other. See Figure \ref{sfig:hourglass_learned} for a visualization of the manifolds and a found path with SMP.

\begin{table}
  \caption{Experiment A -- ${h}_{\constraintmanifold}$ on Datasets.}
  \label{table:experiment1}
  \centering
  \resizebox{\columnwidth}{!}{%
  \begin{tabular}{ccc|cc|cc|cc|cc|cc}
    \toprule
         Method & \multicolumn{12}{c}{Dataset} \\
     & \multicolumn{2}{c}{Nav} & \multicolumn{2}{c}{Sphere} & \multicolumn{2}{c}{Plane} & \multicolumn{2}{c}{Orient} & \multicolumn{2}{c}{Tilt} & \multicolumn{2}{c}{Handover} \\
     \midrule
              & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ \\
    \midrule
      VAE     & 0.42 & 0.18 & 0.08 & 0.03 & 0.16 & 0.12 & 0.01 & 0.03 & 0.09 & 0.07 & 0.56 & 0.47 \\
   
      ECoMaNN     & N/A & N/A & 0.04 & 0.06 & 0.46 & 1.39 & -0.05 & 0.93 & N/A & N/A & 0.24 & 0.76 \\
    \bottomrule
  \end{tabular}
  }
\end{table}

\begin{table}
    \caption{Experiment B -- ${h}_{\constraintmanifold}$ on Generated Configurations.}
  \label{table:experiment2}
  \centering
  \resizebox{\columnwidth}{!}{%
  \begin{tabular}{ccc|cc|cc|cc|cc|cc}
    \toprule
         Method & \multicolumn{12}{c}{Dataset} \\
     & \multicolumn{2}{c}{Nav} & \multicolumn{2}{c}{Sphere} & \multicolumn{2}{c}{Plane} & \multicolumn{2}{c}{Orient} & \multicolumn{2}{c}{Tilt} & \multicolumn{2}{c}{Handover} \\
     \midrule
              & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ & $\mu$ & $\sigma$ \\
    \midrule
      VAE     & 0.58 & 0.18 & 0.10 & 0.03 & 0.14 & 0.08 & 0.01 & 0.01 & 0.96 & 0.30 & 0.10 & 0.02 \\
    \bottomrule
  \end{tabular}
  }
\end{table}


\section{Introduction}
\label{sec:introduction}

Many practical robotic applications require planning robot motions with constraints, such as maintaining an orientation or reaching a particular location. Planning becomes more complicated when the  task consists of many subtasks that must be completed in sequence. In this case, task and motion planning frameworks \cite{kaelbling2013integrated, dantam2016incremental, konidaris2018skills, toussaint2018differentiable, dantam2018task, barry2013hierarchical, cambon2009hybrid} can be used to handle long planning horizons and a wide range of tasks. 
However, some constraints may be difficult to describe analytically, or it may be difficult to sample constraints that adhere to them. For example, if the task is to pour a liquid from a bottle into a cup, it is not immediately clear how to encode the motion constraints for a planning algorithm.

In this work, we focus on learning constraint manifolds for use in constrained motion planning algorithms \cite{englert2020sampling, stilman2010global, berenson2011task, jaillet2013asymptotically, kim2016tangent, kingston2019ijrr, csucan2012motion, cortes2004sampling}.
To this end, we investigate two different approaches: Variational autoencoders (VAE) following \citet{kingma2013AutoEncodingVB}, and {Equality Constraint Manifold Neural Network}  (ECoMaNN), a method we propose which learns the implicit function value of equality constraints. We evaluate these techniques on six datasets of varying size and complexity, and we present preliminary resulting motion plans.

\begin{comment}


To begin, we evaluate several manifold learning techniques on multiple datasets.
The techniques we discuss in this work are:
Variational Autoencoders (VAE); 
and 
{Equality Constraint Manifold Neural Network} ({ECoMaNN}).
We evaluate these models on 6 datasets of varying size and complexity.

First we give a summary of the planning framework. Then, we discuss motivation
for learning manifolds from data, go over the techniques which are
considered, and briefly cover some related work. Next, we describe each dataset.
Then, we show results for each technique. Finally, we discuss how the learned
manifold constraints can be incorporated into a robotic motion planner, and we 
conclude with a summary of the findings and the next steps for this work. 
\end{comment}

\section*{Acknowledgments}


\bibliographystyle{plainnat}

\section{Manifold Learning}
\label{sec:manifold_learning}
Learning constraint manifolds from data is attractive for multiple reasons. 
For example, it may be easier for a human to demonstrate a task rather than specifying constraints analytically,
or we may want to reduce the amount of expert information needed. 

\begin{comment}
\subsection{Problem Statement}
Our goal here is to focus on a single manifold as opposed to the entire sequence. We
do not assume the analytical description of the manifold (e.g., $h_M$) is known.
Rather, we have a set of data $x \in \mathbb{R}^d$. These $n$ datapoints are
configurations in $\mathbb{R}^n$ which adhere to the constraint $M$. That is, $x_i \in C_M \forall x_i \in
X$. We seek a model $A_\theta$, learned using our dataset $X$, which encodes an approximate representation $\hat{M}$ of the manifold constraint in a lower-dimensional space in
$\mathbb{R}^{\hat{d}}, \hat{d} \leq d$. 

For the purposes of planning, we are mainly interested in the capability of the learned model to reconstruct
the data accurately. In particular, we would like $\hat{M}$ learned by
$A_\theta$ to be able to transform data points between the original (ambient)
space in $\mathbb{R}^d$ and the learned embedding space in
$\mathbb{R}^{\hat{d}}$. 
To achieve this, we use the mean squared error (MSE) of $\hat{x}$ and $x$ as a
metric, which we call the ``reprojection error" and would like to minimize: 
$$ MSE = \frac{1}{n} \sum_{i}^n (\hat{x}_i - x_i)^2 $$
A low MSE indicates that the learned latent embedding of the manifold estimates
the dataset well, and therefore it is an accurate representation of the manifold
constraint. 

Additionally, we consider the ``?'' metric. Given a point $X$ which originally was on the manifold (i.e., $h(x) = 0$) and its reprojected image $\hat{x}$, we define the metric
as $h(\hat{x})$. This value should ideally also be $0$, indicating that even if the point is not exactly reconstructed as before, it is still a valid configuration for the given constraint.

Because constraints will often be in a lower dimension than the ambient configuration space, this motivates the use of dimensionality reduction techniques on data. 
In general, the constraint manifolds may have arbitrary structure; we assume they
have been sampled sufficiently densely but otherwise make no further
assumptions. 


\subsection{Variational Autoencoders}
Variational Autoencoders (VAEs) are a popular method for learning a generative
model of a set of data (\citep{chen2016dynamic, kingma2013AutoEncodingVB, habibie2017recurrent, park2018multimodal}). Importantly, because
they embed data points as a distribution in the latent space, new latent vectors
can be sampled and decoded into unseen examples which fit the distribution of
the training data. VAEs make use of two neural networks in a neural autoencoder
structure during training, and they only use the decoder during generation. The
key idea that makes VAEs computationally tractable is that the distribution in
the latent space is assumed to be Gaussian. 
\end{comment}

\begin{comment}
\subsection{Singular Value Decomposition}
Principal Component Analysis (PCA) (\citet{tipping1999probabilistic}) and
Singular Value Decomposition (SVD) (\citet{halko2011finding}) are used for linear
dimensionality reduction. PCA is a method whose goal, given a set of $n$ data points $X \in
\mathbb{R}^d$, is to find a set of $k < d$ orthogonal vectors which explain the
most variance in the dataset. Several techniques can be used to find these
orthogonal vectors (also known as principal axes); SVD is one of them.
In essence, SVD decomposes an $n$ by $d$ data matrix $A$ into the diagonal form 
$A = USV^T$
where $S$ is a diagonal matrix that contains the singular values $\sigma_i$ (the
square roots of the eigenvalues) of $A$, and $U$ and $V$ are composed of the
eigenvectors of $AA^T$ and $A^TA$, respectively. With $\sigma$ in decreasing
order, the first $k$ columns of $V$ ($V_k$) can be used as the principal axes
for the data (see \citet{shlens2014tutorial} for a more in-depth explanation).
The data $X$ can be transformed into the new $k$-dimensional space  via
projection: $Y = XV_k$
and, unlike LLE, transformed data can be reprojected easily with $V_k^T$ since
$V_k$ is an orthonormal matrix: $\hat{X} = YV_k^T$.

\subsection{Locally Linear Embedding}

Locally Linear Embedding (LLE) produces a global embedding based on local structure. The idea is to first use a set of $k$ nearest neighbors of each point to linearly reconstruct a point $\vec{X}_i$ as close to its true value as possible.
Secondly, using these linear reconstruction weights $W_{ij}$, LLE computes
lower-dimensional embedding vectors of each point, which are computed such that they minimize the difference between the embedding and a linear reconstruction of the embeddings of its nearest neighbors using the same $W_{ij}$. Thus, LLE assumes that local structure is preserved in the learned embedding.

One of the main drawbacks of LLE for our work is
that it does not encode a method for reprojecting embeddings into the ambient space. Additionally, if the
manifold is tightly folded (imagine a tightly wound spring), points on a loop
above or below $\vec{X}_i$ may be chosen as nearest neighbors when in reality the
geodesic distance between them may be quite large. 




\subsection{Isomap}
Isomap, named for isometric mapping, is a manifold learning technique that
approximates geodesic distances via a graph constructed from the data $X$
(\citet{tenenbaum2000global}). Similarly to LLE, each point $\vec{X}_i$ has a
nearest neighborhood $K_i$ of size $k$. However, this neighborhood is not
used to reconstruct $\vec{X}_i$. Rather, a graph is constructed
where the vertices $V$ represent the data points in $X$, and the edges $E$
connect points in a neighborhood. Then, finding the
shortest path between two nodes using an algorithm such as Dijkstra's is
equivalent to approximating the geodesic distance on the manifold between two
data points. After computing all pairwise distances, the final step to produce
an embedding is to apply Multidimensional Scaling (MDS, \citet{borg2005modern},
\citet{kruskal1978multidimensional}) to $D$. MDS uses eigenvalue decomposition
given a distance or dissimilarity matrix (here, $D$) to produce data $\hat{X}$
that preserve these distances. Like LLE, Isomap lacks the ability to transform
embedded data points back into the original space and may not
produce accurate results if the manifold is very crumpled or folded. 
\end{comment}

\input{ecmnn}


\section{Related work}
\label{sec:related_work}

\subsection{Constrained SBMP} \label{csbmp} Sampling-based motion planning
(SBMP) is a broad field which tackles the problem of motion planning by using
randomized sampling techniques to build a tree or graph of configurations (also
called samples). The resulting tree or graph can then be used to plan paths
between configurations. This is in contrast to other motion planning techniques,
which may optimize an objective function over a continuous space of trajectories
(e.g., \citet{kalakrishnan2011stomp}) or use the gradient of a potential field
to guide a path toward a goal configuration (e.g., \citet{khatib1986real}).
Many SBMP algorithms derive from one of the following SBMP methods:
Rapidly-exploring random tree (RRT) (\citet{lavalle1998rapidly,
lavalle2006planning}), probabilistic roadmap (PRM)
(\citet{kavraki1994randomized}), and their optimal counterparts RRT* and PRM*
(\citet{karaman2011sampling}). These methods work by sampling the search space,
connecting samples if the edge between them does not collide with obstacles, and
then querying the tree or graph for a valid path that reaches the goal state. A
collision detector is generally assumed to exist, which reports whether or not a
state is in collision with obstacles in the environment. 

A more challenging and realistic motion planning task is that of constrained
SBMP, where there are motion constraints beyond simply obstacle avoidance.
Constraints will often lead to a free configuration space manifold $C_{free} \in
\mathbb{R}^{d'}$ of lower dimension than the ambient configuration space $C \in
\mathbb{R}^d$. (For example, consider the constraint where an arm must stay at a
particular height. A joint that moves the arm up and down will have an effective
space of only one value, and hence the rest of the configuration space is
"squashed" down a dimension.) Randomized sampling in a $d$-dimensional space
will produce samples on a $d' < d$ -dimensional space with probability 0, so
additional measures must be taken to produce valid samples for planning. Another
complication arises when there is not just one constraint, but many, which may
have a specific order in which they must be met in order to achieve the task
goal.

\subsection{Manifold Learning}
\section{Use in planning}
\label{sec:use_in_planning}