Learning Riemannian Manifolds forGeodesic Motion Skills
Hadi Beik-Mohammadi1,2 , Søren Hauberg3 , Georgios Arvanitidis4 , Gerhard Neumann2 , and Leonel Rozo1
1

Bosch Center for Artificial Intelligence (BCAI), Renningen, Germany.
Autonomous Learning Robots Lab, Karlsruhe Institute of Technology (KIT), Karlsruhe, Germany.
3
Section for Cognitive Systems, Technical University of Denmark (DTU), Lyngby, Denmark.
4
Max Planck Institute for Intelligent Systems (MPI-IS), Tübingen, Germany.
Emails: hadi.beik-mohammadi@de.bosch.com, sohau@dtu.dk, gear@tuebingen.mpg.de2

gerhard.neumann@kit.edu, leonel.rozo@de.bosch.com

Abstract—For robots to work alongside humans and perform
in unstructured environments, they must learn new motion skills
and adapt them to unseen situations on the fly. This demands
learning models that capture relevant motion patterns, while
offering enough flexibility to adapt the encoded skills to new
requirements, such as dynamic obstacle avoidance. We introduce
a Riemannian manifold perspective on this problem, and propose
to learn a Riemannian manifold from human demonstrations on
which geodesics are natural motion skills. We realize this witha variational autoencoder (VAE) over the space of position andorientations of the robot end-effector. Geodesic motion skills let a
robot plan movements from and to arbitrary points on the data
manifold. They also provide a straightforward method to avoid
obstacles by redefining the ambient metric in an online fashion.
Moreover, geodesics naturally exploit the manifold resulting frommultiple–mode tasks to design motions that were not explicitly
demonstrated previously. We test our learning framework using
a 7-DoF robotic manipulator, where the robot satisfactorily
learns and reproduces realistic skills featuring elaborated motion
patterns, avoids previously–unseen obstacles, and generates novel
movements in multiple-mode settings.

Fig. 1: From demonstrations we learn a variational autoencoder that spansa random Riemannian manifold. Geodesics on this manifold are viewed as
motion skills.

I. I NTRODUCTION
Robot motion generation has been actively investigatedduring the last decades, where motion planners and movement
primitives have led to significant advances. When a robot
moves in obstacle-free environments, the motion generation
problem can be easily solved by off-the-shelf motion planners [13]. However, the problem is significantly more involved
in unstructured environments when (static and dynamic) obstacles occupy the robot workspace [27]. Moreover, if the robot
motion depends on variable targets, or requires to consider
multiple-solution tasks, the motion generation problem exacerbates. Some of the aforementioned problems have been
recently addressed from a learning-from-demonstration (LfD)
perspective, where a skill model is learned by extracting therelevant motion patterns from human examples [28].
LfD approaches are advantageous as they do not necessarily
require a model of the environment, and can easily adapt tovariable targets on the fly [28]. Three main lines of work
stand out in the LfD field, namely, (1) dynamical-system-based
approaches [20] which focus on capturing motion dynamics,(2) probabilistic methods [6, 29, 19] which exploit data
variability and model uncertainty, and more recently, (3) neu-

ral networks [37, 4] which address generalization problems.
Despite their significant contributions (see Section II), several
challenges are still open: encoding and reproduction of fullpose end-effector movements, skill adaptation to unseen or
dynamic obstacles, handling multiple-solution (a.k.a. multiplemode) tasks, generalization to unseen situations, among others.
In this paper we provide an LfD approach that addresses
several of these problems, through a Riemannian perspective
for learning robot motions from demonstrations. Unlike previous works [17, 26], where skill manifolds are built from locallysmooth manifold learning [11], we leverage a Riemannian
formulation. Specifically, we develop a variational autoencoder(VAE) that learns a Riemannian submanifold of R3 × S 3 fromhuman demonstrations. We exploit geodesics (i.e. shortest
paths) on this learned manifold as the robot motion generation
mechanism, from which full-pose end-effector trajectories are
generated.
These geodesics reproduce motions starting from arbitrary
initial points on the manifold, and they can adapt to avoid
dynamic obstacles in the robot environment by redefining theambient metric.

Our approach can learn manifolds encoding multiplesolution tasks, from which novel (unobserved) robot motions
may naturally arise when computing geodesics.
We illustrate our approach with a simple example, and wetest our method in robotic experiments using a 7-DoF robotic
manipulator.
The experiments show how our approach learns and reproduces realistic robot skills featuring complex motion patterns
and obstacle avoidance. Moreover, we demonstrate how ourapproach can discover new solutions for unseen task setups ina multiple–mode setting.
In summary, we contribute a new view on motion skill
learning that navigates along geodesics on a manifold learned
via a novel VAE over position-orientation space. We show how
this allows the robot to generate useful movements beyond thedemonstration set, while avoiding dynamic obstacles that were
not part of the learning procedure.
II. BACKGROUND AND RELATED WORK
We first briefly review some relevant work on learningfrom demonstrations, variational autoencoders (VAEs), andRiemannian geometry. We also introduce some recent connections between VAEs and Riemannian geometry, which isthe backbone of our work.
A. Learning robot motion from demonstrations
Learning from demonstrations (LfD) provides a framework
for a robot to quickly learn tasks by observing several demonstrations provided by a (human) expert [28]. The demonstrations are then used to learn a model of the task (e.g., amovement primitive, a reward, or plan) which is then used
to synthesize new robot motions [32]. In particular, movement
primitives (MPs) describe complex motion skills, and represent
an alternative solution to classic motion planners [13] forgenerating robot motions. We here exploit LfD to learn a
Riemannian skill manifold, which we later employ to drive
the robot motion generation.
LfD approaches can be categorized into: (1) dynamicalsystem-based approaches [20] which capture the demonstrated
motion dynamics [36], (2) probabilistic methods [6, 29, 19]that take advantage of data variability and model uncertainty,and (3) neural networks [37, 4] aimed at generalization problems. Our method leverages a neural network (VAE) to learn a
Riemannian metric that incorporates the network uncertainty.
This metric allows us to generate motions that resemble thedemonstrations. Unlike the aforementioned approaches, ourmethod allows for online obstacle avoidance by rescaling thelearned metric. Although obstacle avoidance might still bepossible by defining via-points in methods like [29, 37, 19],this problem was not explicitly considered in any of them.
Finally, human demonstrations may show different solution
trajectories for the same task [33, 37], which is often tackled
through hierarchical approaches [24, 14]. In this context, ourmethod permits to encode multiple-solution tasks into thelearned Riemannian manifold, which is exploited to not only
reproduce the demonstrated solutions but also to come up

with a hybrid solution built on a synergy of a subset of thedemonstrations. These novel solutions naturally arise fromour geodesic motion generator. Note that previous learning
frameworks generate robot motions that are restricted to theprovided solutions for the task at hand.
B. Variational autoencoders (VAEs)
A variational autoencoder (VAE) [23] is a generative modelthat captures the data density p(x) through a latent variable
z that generally has a significantly lower dimension thanx. In the interest of simplicity, we consider Gaussian VAEs
corresponding to the generative modelp(z) = N (z|0, Id ) ,pθ (x|z) = N

z|µθ (z), ID σθ2 (z),z ∈ Z;(1)x ∈ X.

(2)

Here µθ : Z → X and σθ : Z → RD
+ are deep neural networks
with parameters θ that estimate the mean and the variance ofthe posterior distribution pθ (x|z).
When the generative process is nonlinear, exact inference
becomes intractable, and VAEs apply a variational approximation of the evidence (marginal likelihood). The corresponding
evidence lower bound (ELBO) is then
LELBO = Eqφ (z|x) [log(pθ (x|z))]− KL (qφ (z|x)||p(z)) ,(3)2
where qφ (z|x) = N (x|µφ (x), Id σφ(x)) approximates theposterior distribution p(z|x) by two deep neural networksµφ (x) : X → Z and σφ (x)) : X → Rd+ . This approximate
posterior is often denoted the inference or encoder distribution,
while the generative process pθ (x|z) is known as the decoder.
As mentioned previously, we use VAEs to learn a robot skill.

C. Riemannian geometry
Riemannian manifolds can be intuitively seen as curved ddimensional surfaces that are described by smoothly-varying
positive-definite inner products, characterized by the Riemannian metric M [25]. These manifolds locally resemble a
Euclidean space Rd , and have a globally defined differential
structure. For our purposes, it suffices to consider manifolds
as defined by a mapping function
f : Z → X,

(4)where both Z and X are open subsets of Euclidean spaces
with dim Z < dim X . We then say that M = f (Z) is amanifold immersed in the ambient space X .
Given a curve c : [0, 1] → Z, we can measure its length on
the manifold as
Z 1
Lc =k∂t f (c(t))kdt.
(5)0

By applying the chain-rule, we see that this can be equivalently
expressed as
Z 1qċ(t)T M (c(t))ċ(t)dt.
(6)
Lc =
0

Here ċt = ∂t ct is the curve derivative and where we haveintroduced the Riemannian metric
M (z) = Jf (z)T Jf (z),(7)

where Jf is the Jacobian of f that we evaluate at z ∈ Z.
We may think of the metric as forming a local inner productin Z that inform us how to measure lengths locally. This
construction relies on the Euclidean metric of X ; we will later
extend this to also form a Riemannian metric. Having defineda notion of curve length (5), we can trivially define shortest
paths, or geodesics, as curves of minimal length. Geodesics
are the generalization of straight lines on the Euclidean space
to Riemannian manifolds. They will serve as our motion
generation mechanism as explained in Section IV. Note thatgeodesics have been recently used as solutions of trajectory
optimizers for quadrotors control [35].
D. Variational autoencoders as Riemannian manifolds
To make the link between VAEs and Riemannian geometry[1], we may write the generative process of a VAE (2) asfθ (z) = µθ (z) + diag()σθ (z), ∼ N (0, ID ).

(8)This is also known as the reparametrization trick [23] and is
illustrated in Fig. 2. We see that this is similar to the mapping
function (4) that defined a manifold in the previous section.
The difference being that now fθ is stochastic. We can rewrite
this stochastic mapping as [12] µθ (z)fθ (z) = ID , diag()= P g(z),(9)σθ (z)
where P is a random matrix, and g(z) is the concatenation ofµθ (z) and σθ (z). In this notation, we can view the VAE as arandom projection of a deterministic manifold spanned by g,and the metric under this mapping isM̄ (z) = Jµθ (z)T Jµθ (z) + Jσθ (z)T Jσθ (z).

(10)

Geodesics under this metric have been shown to be faithful tothe data used for training the VAE [1]. Hauberg [15] argues
that this is due to the contribution from σ to the metric and that
disregarding this term gives an almost flat manifold geometry.
As mentioned, the definition of curve length relies on the
Euclidean metric of the ambient space X , but this is not a strict
requirement. Arvanitidis et al. [3] argue that there is value
in giving the ambient space a manually defined Riemannian
metric and including that into the definition of curve length.
Curve length is then defined as
Z 1qċ(t)T Jfθ (c(t))T MX (fθ (c(t)))Jfθ (c(t))ċ(t)dt,
Lc =
0

(11)
where MX is the ambient space metric, which can now varysmoothly across X . The corresponding Riemannian metric of
Z is thenM̄ (z) = Jµθ (z)T MX (µθ (z))Jµθ (z)+ Jσθ (z)T MX (µθ (z))Jσθ (z).

(12)

With this construction, it is straightforward to push geodesics
away from certain regions of X by increasing MX there.
Note that geodesics do generally not follow a closed-form
expression in these models, and numerical approximations are
in order. This can be done by direct minimization of curve
length [38, 22], A∗ search [9], integration of the associated
ODE [2], or various heuristics [8].
III. R IEMANNIAN MANIFOLD LEARNING ON R3 × S 3
Learning complex robot motion skills requires models thathave enough capacity to learn and synthesize the relevant
patterns of a motion while being flexible enough to adapt to
new conditions. In this section, we describe how we tackle thisproblem by bringing a Riemannian manifold perspective tothe robot learning problem. First, we explain how we exploit
VAEs to access a low-dimensional learned manifold of the
motion data where an ambient-space Riemannian metric islearned. This metric will be later used to generate robot motion
trajectories via geodesics, as detailed in Section IV. In orderto learn elaborated motion skills, which may display complex
position and orientation trajectories, we represent the robot
state as the full pose of the robot end-effector, i.e. its positionx ∈ R3 and orientation q ∈ S 3 . We then seek a VAE
that models a joint density of this state. We retain the usual
Gaussian prior p(z) = N (z|0, Id ), but alter the generative
distribution pθ,ψ (x, q|z) to suit our needs. We will assume
that position and orientation are conditionally independent,pθ,ψ (x, q|z) = pθ (x|z)pψ (q|z),(13)

such that all correlations between the two must be capturedby the latent variable z.
A. Position encoding on R3
To model the conditional distribution of end-effector positions x, we opt for simplicity and choose this to be Gaussian,pθ (x|z) = N (x|µθ (z), I3 σθ2 (z)),(14)

where µθ and σθ are neural networks parametrized by θ.
One could argue that pθ (x|z) should have zero probability
mass outside the workspace of the robot, but we disregard
such concerns as σθ2 tends to take small values due to limited
data noise. This implies that only a negligible fraction of theprobability mass falls outside the robot workspace.
B. Orientation encoding on S 3
Complex robot motions often involve elaborated orientation trajectories which require a suitable representation formotion learning and control. There exist several orientation
representation such rotation matrices, Euler angles, and unit
quaternions. Euler angles and rotation matrices are commonlyused for reasons of simplicity. Unfortunately, Euler angles
suffer from gimbal lock [18] which makes them an inadequate
representation of orientation in robotics, and rotation matrices
are a redundant representation requiring a high number ofparameters. Unit quaternions are a convenient way to represent an orientation since they are compact, not redundant,

Fig. 2: In a Gaussian VAE, samples are
generated by a random projection of the
manifold jointly spanned by µ and σ.

and prevent gimbal lock. Also, they provide strong stability
guarantees in closed–loop orientation control [7], they havebeen recently exploited in robot skills learning [34], and fordata-efficient robot control tuning [21] under a Riemannianmanifold formulation.
We choose to represent orientations q as a unit quaternion,such that q ∈ S 3 with the additional antipodal identification that q and −q correspond to the same orientation.
Formally, a unit quaternion q lying on the surface of a 3sphere S 3 can be represented using a 4-dimensional unit
vector q = [qw , qx , qy , qz ] ∈ S 3 , where the scalar qw andvector (qx , qy , qz ) represent the real and imaginary parts of
the quaternion, respectively. To cope with antipodality, one
could opt to model q as a point in a projective space, but
for reasons of simplicity we let q live on the unit sphere S 3 .
We then choose a generative distribution pψ (q|z) such thatpψ (q|z) = pψ (−q|z).
To construct a suitable distribution pψ (q|z) over the unit
sphere, we turn to the von Mises-Fischer (vMF) distribution,
which is merely an isotropic Gaussian constrained to lie on
the unit sphere [41]. This distribution is described by a mean
direction µ with kµk = 1, and a concentration parameterκ ≥ 0. Its density function takes the formvMF(q|µ, κ) = CD (κ) exp κµT q ,kµk= 1,(15)

where CD is the normalization constant

C. Variational inference
Our VAE model can be trained by maximizing the conventional evidence lower bound (ELBO) (3), which now is
LELBO = αLx + βLq − KL (qφ (z|x)||p(z)) ,(18)

Lx = Eqφ (z|x) [log pθ (x|z)] ,(19)

Lq = Eqφ (z|x) [log pψ (q|z)] ,(20)

where x ∈ R3 and q ∈ S 3 represent the position andquaternion of the end-effector, respectively. To balance thelog-likelihood of position and orientation components, α > 0and β > 0 are proportionally scaled. Note that due to theantipodal nature of quaternions, raw demonstration data may
contain negative or positive values for the same orientation. So,
we avoid any pre-processing step of the data by considering
two von Mises-Fischer distributions that encode the sameorientation at both sides of the hypersphere. Practically, we
double the training data, by including qn and −qn for allobservations qn .
D. Induced Riemannian metric
Our generative process is parametrized by a set of neuralnetworks. Specifically, µθ and σθ are position mean andvariance neural networks parameterized by θ, while µψ andκψ are neural networks parameterized by ψ that represent
the mean and concentration of the quaternion distribution.
Following Sec. II-D the Jacobians of these functions govern
the induced Riemannian metric asDCD (κ) =κ 2 −1D2(2π) I D −1 (κ)

,

(16)

2

(21)

with

with I D −1 (κ) being the modified Bessel function of the first2
kind. Like the Gaussian, from which the distribution wasconstructed, the von Mises-Fischer distribution is unimodal. To
build a distribution that is antipodal symmetric, i.e. pψ (q|z) =pψ (−q|z), we simply form a mixture of antipodal von MisesFischer distributions [16],1
vMF(q|µψ (z), κψ (z))
2
1
+ vMF(q|−µψ (z), κψ (z)),
2

M (z) = Mµx (z) + Mσx (z) + Mµq (z) + Mκq (z)pψ (q|z) =(17)

where µ and κ are parametrized as neural networks. This
mixture model is conceptually similar to a Bingham distribution [41], but is easier to implement numerically.

Mµx (z) = Jµθ (z)T Jµθ (z),Mσx (z) = Jσθ (z)T Jσθ (z),Mµq (z) = Jµψ (z)T Jµψ (z),Mσq (z) = Jσψ (z)T Jσψ (z),where Jµθ , Jσθ , Jµψ , Jσψ are the Jacobian of functions
representing the position mean and variance, as well as thequaternion mean and concentration, respectively.
In practice, we want this metric M (z) to take large values
in regions with little or no data, so that geodesics avoid passing
through them. Following Arvanitidis et al. [1] we achieve thisby using radial basis function (RBF) networks as our variance
representation, whose kernels reliably extrapolate over thewhole space. Since one of the main differences between Gaussian and von Mises-Fischer distributions lies on the way they
represent data dispersion, the RBF network should consider areciprocal behavior when estimating variance for positions. In
summary, the data uncertainty is encoded by the RBF networks
representing σθ−1 (z) and κψ (z), which affect the Riemannian
metric through their corresponding Jacobians as in (21).
IV. G EODESIC M OTION S KILLSAs mentioned previously, geodesics follow the trend of thedata, and they are here exploited to reconstruct motion skills
that resemble human demonstrations. Moreover, we explain
how new geodesic paths, that avoid obstacles on the fly, can beobtained by a metric scaling process. In particular, we exploit
ambient space metrics defined as a function of the obstacles
configuration to locally deform the original learned Riemannian metric. Last but not least, our approach can encode
multiple-solution skills, from which new hybrid trajectories(not previously shown to the robot) can be synthesized. We
elaborate on each of these features in the sequel.
A. Geodesic motion generation
Geodesic curves generally follow the trend of the training
data, due to the role of uncertainty in the metric. Specifically,
Eq. (10) tells us that geodesics are penalized for crossing
through regions where the VAE predictive uncertainty grows.
This implies that if a set of demonstrations follows a circularmotion pattern, geodesics starting from arbitrary points on thelearned manifold will also generate a circular motion. Thisbehavior is due to the way that the metric M is defined, as
M is characterized by low values where data uncertainty islow (and vice-versa). Since the geodesics minimize the energyof the curve between two points on M, which is calculated
as a function of M , they tend to stay on the learned manifold
and avoid outside regions. This property makes us suggestthat geodesics form a natural motion generation mechanism.
Note that when using an Euclidean metric (i.e., an identity
matrix), geodesics correspond to straight lines. Such geodesics
certainly neglect the data manifold geometry.
Formally, we compute geodesics on M by approximatingthem by cubic splines c ≈ ωλ (zc ), with zc = {zc0 , . . . , zcN },where zcn ∈ Z is a vector defining a control point of the
spline over the latent space Z. Given N control points, N − 1
cubic polynomials ωλi with coefficients λi,0 , λi,1 , λi,2 , λi,3have to be estimated to minimize its Riemannian length
Z 1phω̇λ (zc ), M (ωλ (zc ))ω̇λ (zc )idt.
(22)
Lωλ (ĉ) =0Then, the final geodesic c computed in Z is used to generate
the robot motion through the mean decoder networks µθ andµψ . The resulting trajectory can be executed on the robot arm
to reproduce the required skill.
To illustrate, we consider a simple experiment where thedemonstration data at each time point is confined to R2 × S 2 ,i.e. only two dimensional positions and orientations are considered. We create synthetic position data that follows a Jshape and orientation data that follows a C-shape projectedon the sphere (see Fig. 3). We fit our VAE model to thisdata, and visualize the corresponding latent space in Fig. 4.

Fig. 3: As an illustration, we consider synthetic data that belong to R2 ×
S 2 . The left panel depicts the J-shaped position data in R2 and the rightpanel shows the C-shaped orientation data on S 2 . The yellow and red curves
show the geodesics computed based on Riemannian and Euclidean metrics,
respectively.

Here the top panel shows the latent mean embeddings of the
training data with a background color corresponding to thepredictive uncertainty. We see low uncertainty near the data,and high otherwise. The bottom panel of Fig. 4 shows thesame embedding
but with a background color proportional√
to log det M . This quantity, known as the magnification
factor [5], will generally take large values in regions where
distances are large, implying that geodesics will try to avoid
such regions. In the figure, we notice that the magnification
factor is generally low, except on the ‘boundary’ of the data
manifold, i.e. in regions where the predictive variance grows.
Consequently, we observe that Riemannian geodesics (yellow
curves in the figure) stay within the ‘boundary’ and are hence
near the training data. In contrast, Euclidean geodesics (redcurves in the figure) fail to stay in the data manifold. Our
proposal is to use Riemannian geodesics to generate new
motions for the robot.
B. Obstacle avoidance using ambient space metrics
Often human demonstrations do not include any notion of
obstacles in the environment. As a result, obstacle avoidanceis usually treated as an independent problem when generating
robot motions in unstructured environments. A possible solution to integrate both problems is to provide obstacle–aware
demonstrations, where the robot is explicitly taught how to
avoid known obstacles. The main drawback here is that the
robot is still unable to avoid unseen obstacles on the fly.
The Riemannian approach provides a natural solution tothis problem. The natural metric in latent space (10) measuresthe length of a latent curve under the Euclidean space of theambient space X . We can easily modify this to take obstacles
into account. Intuitively, we can increase the length of curves
that intersect obstacles, such that geodesics are less likely to gonear the obstacles. Formally, we propose to alter the ambient
metric of the end-effector position to be−kx − ok2x
MX (x) = 1 + η exp
I3 , x ∈ R3 , (23)2r2

Latent graphRiemannian-metric-based Geodesic
Euclidean-metric-based Geodesic
Encoded Demonstrations

Decoded graph

12

10

8

Obstacle
6

Fig. 5: Concept drawing. Left: The latent space is discretized to form agrid graph consisting of linearly spaced nodes, with edge weights matching
Riemannian distances. Right: To efficiently handle obstacles, the graph is
decoded, such that obstacles can easily be mapped to latent space.

42

with Mµx (z) = Jµθ (z)T MXx (µθ (z))Jµθ (z),Mσx (z) = Jσθ (z)T MXx (µθ (z))Jσθ (z),22Mµq (z) = Jµψ (z)T MXq (µψ (z))Jµψ (z),Mκq (z) = Jκψ (z)T MXq (µψ (z))Jκψ (z),20

where MXx and MXq represent the position and orientation
components of the obstacle–avoidance metric MX , respectively. Here we emphasize that as the object changes position,
the VAE does not need to be re-trained as the change is onlyin the ambient metric.

18

C. Real time motion generation

16

Fig. 4: Top: the variance measure, bottom: magnification factor of the
Riemannian manifold learned from trajectories based on J and C English
alphabet characters defined in R2 × S 2 . The semi-transparent white points
depict the encoded training set and the yellow curve depicts the geodesic in
the latent space. The resulting manifold is composed of two similar clusters
due to the antipodal encoding of the quaternions, where each cluster represents
one side of the hyper-sphere. The yellow and red curves show the geodesics
computed based on Riemannian and Euclidean metrics, respectively.

Having phrased motion generation as the computation ofgeodesics, we evidently need a fast and robust algorithm
for computing geodesics. As we work with low–dimensional
latent spaces, we here propose to simply discretize the latent
space on a regular grid and use a graph–based algorithm forcomputing shortest paths.
Specifically, we create a uniform grid over the latent space,and assign a weight to each edge in the graph corresponding
to the Riemannian distance between neighboring nodes (seeFig. 5). Geodesics are then found using Dijkstra’s algorithm[10]. This algorithm selects a set of graph nodes,
Gc = {g0 , g1 , . . . , gN −1 , gN } ,where η > 0 scales the cost, o ∈ R3 and r > 0 represent
the position and radius of the obstacle, respectively. For theorientation component, we use MXq (x) = I4 . Under thisambient metric, geodesics will generally avoid the object,
though we emphasize this is only a soft constraint. Thisapproach is similar in spirit to CHOMP [31] except ourformulation works along a low-dimensional learned manifold,whose solution is then projected to the task space of the robot.
Under this ambient metric, the associated Riemannian metricof the latent space Z becomesM (z) = Mµx (z) + Mσx (z) + Mµq (z) + Mκq (z),(24)

gn ∈ R D ,where g0 and gN represent the start and the target of thegeodesic in the graph, respectively. To select these points, theshortest path on the graph is calculated by minimising theaccumulated weight (cost) of each edge connecting two nodes
calculated as in (6). To ensure a smooth trajectory, we fit a
cubic spline ωλ to the resulting set Gc by minimizing themean square error. The spline computed in Z is finally used togenerate the robot motion through the mean decoder networksµθ and µψ . The resulting trajectory can be executed on the
robot arm to reproduce the required skill.
One issue with this approach is that dynamic obstacles
imply that geodesic distances between nodes may change
dynamically. To avoid recomputing all edge weights every
time an obstacle moves we do as follows. Since the learned

manifold does not change, we can keep a decoded version of
the latent graph in memory (Fig. 5). This way we need notquery the decoders at run-time. We can then find points on
the decoded graph that are near obstacles and rescale theirweights to take the ambient metric into account. Once the
obstacle moves, we can reset the weights of points that are
now sufficiently far from the obstacle. The center panel ofFig. 9 provides an example showing how the metric of theleft panel is represented as a discrete graph.
V. E XPERIMENTS
We evaluate the performance and capabilities of our method
in two different scenarios1 : a simulated pouring task, and areal-world grasping scenario both in R3 ×S 3 . In particular, thepouring task showcases a multiple-solution setting. For the experiments, we discuss the design of each experiment regarding
manifold learning and geodesic computation. We also provide
a visualization of the learned Riemannian metrics and geodesic
representation in the latent space Z. Furthermore, the code isavailable at: https://sites.google.com/view/geodesicmotion.
A. Setup description
We consider simulated and real robot demonstrations involving a 7-DoF Franka Emika Panda robot arm with a
two–finger gripper. The demonstrations were recorded usingkinesthetic teaching in the real grasping scenario meanwhile
simulated pouring dataset was collected using the Franka ROS
Interface [40] on Gazebo [39]. In both scenarios, the robot is
controlled by an impedance controller.
We calculate geodesic on a 100 × 100 grid graph, andour straightforward Python implementation runs at 100Hz on
ordinary PC hardware. The approach readily runs in real time.
B. VAE architecture
Our VAE architecture is implemented using PyTorch [30].
The decoder and encoder networks have two hidden layerswith 200 and 100 neuron units. The same architecture isused for all the experiments. The RBF variance/concentration
networks use 500 kernels calculated by k-means over the
training dataset [1] and predefined bandwidth. The latent spaceof the VAE is 2-dimensional, while the ambient space is 7dimensional, corresponding to R3 × S 3 .
We employ a single neural network to represent both theposition and orientation decoder means, meaning that our final
metric is defined as
M (z) = Mµx,q (z) + Mσx (z) + Mκq (z),(25)

with Mµx,q (z) = Jµθ (z)T MX Q (z)Jµθ (z),MX (µθ (z))
0
MX Q (z) =,0
MQ (µψ (z))
Mσx (z) = Jσθ (z)T MX (µxθ (z))Jσθ (z),Mκq (z) = Jκψ (z)T MQ (µψ (z))Jκψ (z).
1 We extensively evaluated our method on simulation settings as the COVID19 pandemic prohibited access to our robotic labs. We still tested our learning
approach with real robot data to validate that our insights apply in realistic
scenarios. We plan to run more real experiments whenever possible.

where Jµθ ∈ R(DX +DQ )×d is the Jacobian of the joint decoder
mean network (position and quaternion), and Jσθ ∈ RDX ×dand Jσψ ∈ RDQ ×d are the Jacobians of the decoder varianceand concentration RBF networks. Since the position andquaternion share the same decoder mean network, the output
vector is split into two parts, accordingly. The quaternion partof the decoder mean is projected to the S 3 to then define thecorresponding von Mises-Fischer distribution (15).
The ELBO parameters α and β in (18) are found experimentally to guarantee good reconstruction of both positionand quaternion data. It is worth pointing out that we manuallyprovided antipodal quaternions during training, which leads tobetter latent space structures and reconstruction accuracy.
C. Real grasping task
The first set of experiments is based on a dataset collected
while a human operator performs kinesthetic demonstrationsof a grasping skill. This particular grasping motion includes
a 90° rotation when approaching the object for performinga side grasp [34]. The demonstration trajectories start fromdifferent end-effector poses, and they reach the same target
position with slightly different orientations.
To reproduce the grasping skill, we computed a geodesic in
Z which leads to a continuous trajectory in X , that closely reproduces the rotation pattern observed during demonstrations.
Figure 6 depicts the magnification factor related to thelearned manifold. The semi–transparent white points correspond to the latent representation of the training set, and theyellow curves are geodesics between points assigned from thestart and endpoints of the demonstrations. The left panel inFig. 6 shows geodesics in two different scenarios: The onesin the top cluster start from different poses and end up at
the same target, and the geodesics in the bottom cluster startand end in different random poses. The target points on themost right side of each cluster represent the same position but
due to their slightly different orientation, they are encoded on
different latent points.
The results show that the method can successfully generate
geodesics that respect the geometry of the manifold learned
from demonstration. Interestingly, as shown by the magnification factor plot (Fig. 6-left), the resulting manifold is composedof two similar clusters, similarly to the illustrative example
of Fig. 4. We observed that this behavior emerged due tothe antipodal encoding of the quaternions, where each cluster
represents one side of the hyper-sphere. It is worth highlightingthat this encoding alleviates any kind of post-processing of raw
quaternion data during training or reproduction phases.
Figure 6-middle shows one of the demonstrated trajectories
on real robot, and Fig. 6-right displays the reconstructed
geodesic using the decoder network and applied on a simulated
robot arm. From the results, it is clear the motion generated
by the geodesic leads to a motion pattern similar to thedemonstrations. Note how the end-effector orientation evolves
on the decoded geodesic in the ambient space, showing thatthe 90° rotation is properly encoded and reproduced.

Encoded Demonstrations
18

16

14

12

10

Fig. 6: Left: The yellow curves in the top cluster show geodesics starting from random points and ending up at the same target, and the curves in the bottom
cluster connect random points on the manifold. The background depicts magnification factor derived from the learned manifold, and the semi-transparent white
points show the encoded training dataset. Middle: It illustrates one of the demonstrations on the real robot, where the yellow curve depicts the end-effector
trajectory. Right: It shows the reconstructed geodesic using the decoder network applied on the simulated robot. The yellow curve depicts the decoded geodesic
computed on the learned manifold.
Encoded Demonstration Set 1
Encoded Demonstration Set 2
Encoded Demonstration Set 3

20

18

16

14

12

10

D. Simulated pouring task
To evaluate our model on a more complicated scenario,
we collected a dataset of pouring task demonstrations on asimulator. The task involves grasping 3 cans from 3 different
positions and then pouring at 3 different cups placed at different locations. The demonstrated trajectories cross each other,
therefore providing a multiple-solution setting. Figure 7 shows
how the geodesic, depicted as a yellow curve, is constructedin the latent space. This geodesic starts from a point on thesecond demonstration group (blue dots) and switches to thefirst group (white dots) to get to the given target locatedin the third group (green dots). As a result, with 3 sets ofdemonstrations, all 9 permutations for grasping any can fromthe table and then pouring any cup are feasible.
To evaluate the method in the presence of an unseen
obstacle, the start and target points of the geodesic path
are selected such that it follows one of the demonstrationgroups. To ensure that the obstacle is in the way, we selectits position from the training set. Figure 8-left shows thegeodesic performing obstacle avoidance while following thegeometry of the manifold. The circular obstacle representation
depicted as red and yellow circles in the latent space is justfor the sake of visualization. The red and yellow curves

Fig. 7: Left: The geodesic depicted as theyellow curve takes advantage of the different
sets of demonstrations (blue, green and whiledots) to generate a hybrid solution which was
not explicitly demonstrated to the robot. Right:
Robot configurations considered in the leftplot, from the top perspective in the ambient
space. The decoded geodesics are depicted asa black curve and the orientations are visualized by coordinate systems

represent geodesics avoiding the red and yellow obstacles,
correspondingly. These curves correspond to one time frameof the adapted geodesics, showing how our method can deal
with dynamic obstacles. The middle and right plots in Fig. 8
show the decoded geodesics executed on the simulated robot
in the ambient space. The black trajectory shows the decoded
geodesic, the dot clusters in red, green, and blue depict thedemonstration sets, and the blue sphere depicts the obstacle.
The middle plot shows the robot configuration 15 frames
earlier than the plot at the right, displaying the obstacle
dynamics during the task execution. Figure 9-middle illustrates
the graph computed from the corresponding data manifold in
Fig. 9-left. The graph-based geodesic (red curve in Fig. 9middle) is then decoded and executed on the simulated robot(see Fig. 9-right). Although gradient-based and graph-based
geodesic computation are both viable options, the latter isfaster and thus more suitable for real-time motion generation.
VI. D ISCUSSION AND CONCLUSIONS
We have proposed a novel LfD approach that learns a Riemannian manifold from human demonstrations and computes
geodesics to recover and synthesize learned motion skills. Our
proposed geodesic motion generation is capable of planningmovements from and to arbitrary points on the data manifold,Encoded Demonstration Set 1
Encoded Demonstration Set 2
Encoded Demonstration Set 3

26

24

22

20

18

16

14

12

10

Fig. 8: Left: Red and yellow curves depict geodesics at two different time frames of the same motion, showing how the method computes paths that avoid
dynamic obstacles (depicted as red and yellow circles). The background depicts magnification factor derived from the learned manifold. The dot clusters in
red, green and blue depict the encoded demonstration sets. Middle and right: Robot configurations for the two time frames considered in the left plot, fromthe top perspective in the ambient space. The decoded geodesics are depicted as black curves and the orientations are visualized by coordinate systems.

Encoded Demonstration Set 1
Encoded Demonstration Set 2
Encoded Demonstration Set 3

26

24

22

20

18

16

14

12

10

Fig. 9: Left: Geodesic computed on the graph is depicted as yellow curve avoiding the obstacle depicted as yellow circle. The background depicts magnification
factor derived from the learned manifold. The dot clusters in red, green and blue depict the encoded demonstration sets. Middle: The graph computed frommetric learned from data. Right: Robot configurations from the top perspective in the ambient space. The decoded geodesic is depicted as a black curve andthe orientations are visualized by coordinate systems.

while avoiding obstacles on the fly. We realize the idea witha variational autoencoder (VAE) over the space of positionand orientations of the robot end–effector. Motion is generated
with graph–based geodesic computation for real–time motion
generation and adaptation. Through extensive evaluation in
the simulation, we show geodesic motion generation performs
well in different scenarios such as grasping and pouring.
The proposed methodology can be extended and improvedin several directions. A consequence of learning a manifold
skill using VAEs is that data lying outside the manifold may bearbitrarily misrepresented in the latent space Z. Consequently,
any reconstruction in the ambient space X may be inaccurate.
This may give rise to problems when conditioning on points,
e.g. new targets, that are located outside the learned manifold.
We did not explore this setting in the present paper. One
possible solution may involve learning a bijective mapping
between old demonstrations and new conditions, and thenuse this function to transform the learned manifold (e.g., byexpanding or rotating) to fit another region of the space.
We introduce dynamical obstacle avoidance as a soft con-straint through the ambient metric. If an approach based on
hard constraints is to be preferred then one may opt to remove
nodes near an obstacle from the graph instead of re-weighting
edges. This could provide a computational saving, but one
would lose the ‘complete’ Riemannian picture that we findelegant. It would also be straightforward to direct geodesics
to go through select via-points by slight modifications to thegraph algorithm. We did not explore these approaches here.
Our obstacle avoidance formulation only considered simple
obstacles at this point, but the strategy can be extended tomultiple dynamic obstacles. Instead of working with single
Gaussian balls, one can imagine extending the approach tocomplex obstacle shapes represented as point clouds. This may
increase the implementation demands in order to remain realtime, but such an extension seems entirely reasonable. It is
worth pointing out that to execute a motion safely the obstacle
avoidance should be considered for all robot links and not justthe end-effector. This requires a more informative manifoldthat is embedded in the joint space of the robot and an ambient
space metric that combines the obstacle information in theEuclidean and joint space simultaneously.
R EFERENCES[1] Georgios Arvanitidis, Lars Kai Hansen, and Søren
Hauberg. Latent space oddity: on the curvature ofdeep generative models. In International Conference
on Learning Representations (ICLR), 2018. URL https://arxiv.org/abs/1710.11379.
[2] Georgios Arvanitidis, Søren Hauberg, Philipp Hennig,
and Michael Schober. Fast and robust shortest paths on
manifolds learned from data. In International Conferenceon Artificial Intelligence and Statistics (AISTATS), pages
1506–1515, 2019. URL http://proceedings.mlr.press/v89/
arvanitidis19a.html.
[3] Georgios Arvanitidis, Søren Hauberg, and Bernhard
Schölkopf. Geometrically enriched latent spaces. In
International Conference on Artificial Intelligence andStatistics (AISTATS), 2021. URL https://arxiv.org/abs/
2008.00565.
[4] Shikhar Bahl, Mustafa Mukadam, Abhinav Gupta, andDeepak Pathak. Neural dynamic policies for end-to-end
sensorimotor learning. In Advances in Neural Information Processing Systems (NeurIPS), pages 5058–5069,
2020. URL https://proceedings.neurips.cc/paper/2020/
file/354ac345fd8c6d7ef634d9a8e3d47b83-Paper.pdf.
[5] Christopher M Bishop, Markus Svensén, and Christopher KI Williams. Magnification factors for the SOM and
GTM algorithms. In Workshop on Self-Organizing Maps,
1997.
URL https://research.aston.ac.uk/files/113377/
NCRG 97 008.pdf.
[6] Sylvain Calinon.
A tutorial on task-parameterized
movement learning and retrieval. Intelligent Service
Robotics, 9(1):1–29, 2016. URL https://doi.org/10.1007/
s11370-015-0187-9.
[7] Ricardo Campa and Karla Camarillo. Unit quaternions:
A mathematical tool for modeling, path planning andcontrol of robot manipulators. In Robot Manipulators,
chapter 2. IntechOpen, Rijeka, 2008. URL https://doi.
org/10.5772/6197.
[8] Nutan Chen, Alexej Klushyn, Richard Kurle, Xueyan
Jiang, Justin Bayer, and Patrick Smagt. Metrics fordeep generative models. In International Conferenceon Artificial Intelligence and Statistics (AISTATS), pages
1540–1550, 2018. URL http://proceedings.mlr.press/v84/
chen18e.html.
[9] Nutan Chen, Francesco Ferroni, Alexej Klushyn, Alexandros Paraschos, Justin Bayer, and Patrick van der
Smagt. Fast approximate geodesics for deep generative models. In International Conference on Artificial Neural Networks (ICANN), pages 554–566,
2019.
URL https://link.springer.com/content/pdf/10.
1007%2F978-3-030-30484-3 45.pdf.
[10] Thomas H Cormen, Charles E Leiserson, Ronald L
Rivest, and Clifford Stein. Introduction to algorithms.
MIT press, 2009.

[11] Piotr Dollár, Vincent Rabaud, and Serge Belongie. Nonisometric manifold learning: Analysis and an algorithm. In International Conference on Machine Learning(ICML), pages 241–248, 2007. doi: 10.1145/1273496.
1273527.
[12] David Eklund and Søren Hauberg. Expected path length
on random manifolds. In arXiv preprint, 2019. URL
https://arxiv.org/abs/1908.07377.
[13] Mohamed Elbanhawi and Milan Simic. Sampling-based
robot motion planning: A review. IEEE Access, 2:56–77,
2014. doi: 10.1109/ACCESS.2014.2302442.
[14] Marco Ewerton, Gerhard Neumann, Rudolf Lioutikov,
Heni Ben Amor, Jan Peters, and Guilherme Maeda.
Learning multiple collaborative tasks with a mixture ofinteraction primitives. In IEEE International Conferenceon Robotics and Automation (ICRA), pages 1535–1542,
2015. URL https://doi.org/10.1109/ICRA.2015.7139393.
[15] Søren Hauberg. Only Bayes should learn a manifold.
2019.
[16] Søren Hauberg, Aasa Feragen, Raffi Enficiaud, andMichael J. Black. Scalable robust principal component
analysis using Grassmann averages. IEEE Transactions
on Pattern Analysis and Machine Intelligence (TPAMI),
38(11):2298–2311, 2016. URL https://doi.org/10.1109/
TPAMI.2015.2511743.
[17] Ioannis Havoutis and Subramanian Ramamoorthy. Motion planning and reactive control on learnt skill
manifolds.
International Journal of Robotics Research (IJRR), 32(9-10):1120–1150, 2013. doi: 10.
1177/0278364913482016. URL https://doi.org/10.1177/
0278364913482016.
[18] Evan G. Hemingway and Oliver M. O’Reilly. Perspectives on Euler angle singularities, gimbal lock, and theorthogonality of applied forces and applied moments.
Multibody System Dynamics, 44(1):31–56, mar 2018.
doi: 10.1007/s11044-018-9620-0. URL https://doi.org/
10.1007%2Fs11044-018-9620-0.
[19] Yanlong Huang, Leonel Rozo, João Silvério, and Darwin G Caldwell. Kernelized movement primitives.
International Journal of Robotics Research (IJRR),
38(7):833–852, 2019.
URL https://doi.org/10.1177/
0278364919846363.
[20] Auke Jan Ijspeert, Jun Nakanishi, Heiko Hoffmann, Peter
Pastor, and Stefan Schaal. Dynamical movement primitives: Learning attractor models for motor behaviors.
Neural Computation, 25:328–373, 2013.
[21] Noémie Jaquier, Leonel Rozo, Sylvain Calinon, andMathias Bürger. Bayesian optimization meets Riemannian manifolds in robot learning. In Proceedings of theConference on Robot Learning, pages 233–246, 2020.
URL http://proceedings.mlr.press/v100/jaquier20a.html.
[22] Dimitris Kalatzis, David Eklund, Georgios Arvanitidis,
and Søren Hauberg. Variational autoencoders with
Riemannian Brownian motion priors. In International
Conference on Machine Learning (ICML), volume 119,
pages 6789–6799, 2020. URL http://proceedings.mlr.

press/v119/kalatzis20a/kalatzis20a.pdf.
[23] Diederik P. Kingma and Max Welling. Auto-encoding
variational Bayes. In International Conference on Learning Representations (ICLR), 2014. URL https://arxiv.org/
abs/1312.6114.
[24] George Konidaris, Scott Kuindersma, Roderic Grupen,and Andrew Barto. Robot learning from demonstrationby constructing skill trees. The International Journal of
Robotics Research (IJRR), 31(3):360–375, 2012. URL
https://doi.org/10.1177/0278364911428653.
[25] John Lee.
Introduction to Riemannian Manifolds.
Springer, 2 edition, 2018.
[26] Mao Li, Kenji Tahara, and Aude Billard. Learning
task manifolds for constrained object manipulation. Autonomous Robots, 42:159–174, 2018. doi: https://doi.org/
10.1007/s10514-017-9643-z.
[27] M.G. Mohanan and Ambuja Salgoankar. A surveyof robotic motion planning in dynamic environments.
Robotics and Autonomous Systems, 100:171–185, 2018.
ISSN 0921-8890. URL https://www.sciencedirect.com/
science/article/pii/S0921889017300313.
[28] Takayuki Osa, Joni Pajarinen, Gerhard Neumann, J. Andrew Bagnell, Pieter Abbeel, and Jan Peters. An algorithmic perspective on imitation learning. Foundations andTrends in Robotics, 7(1–2):1–179, 2018. ISSN 19358253. doi: 10.1561/2300000053.
[29] Alexandros Paraschos, Christian Daniel, Jan Peters, andGerhard Neumann. Using probabilistic movement primitives in robotics. Autonomous Robots, 42:529–551, 2018.
[30] Adam Paszke, Sam Gross, Francisco Massa, Adam Lerer,
James Bradbury, Gregory Chanan, Trevor Killeen, Zeming Lin, Natalia Gimelshein, Luca Antiga, Alban Desmaison, Andreas Kopf, Edward Yang, Zachary DeVito,
Martin Raison, Alykhan Tejani, Sasank Chilamkurthy,
Benoit Steiner, Lu Fang, Junjie Bai, and Soumith Chintala. Pytorch: An imperative style, high-performance
deep learning library. In Advances in Neural Information
Processing Systems (NeurIPS), pages 8024–8035. 2019.
[31] Nathan Ratliff, Matthew Zucker, J. Andrew (Drew)
Bagnell, and Siddhartha Srinivasa. CHOMP: Gradient
optimization techniques for efficient motion planning. In
International Conference on Robotics and Automation(ICRA), pages 489–494, 2009. URL https://doi.org/10.
1109/ROBOT.2009.5152817.
[32] Harish Ravichandar, Athanasios S Polydoros, Sonia
Chernova, and Aude Billard. Recent Advances in
Robot Learning from Demonstration.
Annual Review of Control, Robotics, and Autonomous Systems,
3(1):297–330, 2020.
URL https://doi.org/10.1146/
annurev-control-100819-063206.
[33] Leonel Rozo, Pablo Jimenez, and Carme Torras. Robotlearning from demonstration of force-based tasks withmultiple solution trajectories. In International Conference on Advanced Robotics (ICAR), pages 124–129,
2011. URL https://doi.org/10.1109/ICAR.2011.6088633.
[34] Leonel Rozo, Meng Guo, Andras G. Kupcsik, MarcoTodescato, Philipp Schillinger, Markus Giftthaler,
Matthias Ochs, Markus Spies, Nicolai Waniek, Patrick
Kesper, and Mathias Bürger. Learning and sequencingof object-centric manipulation skills for industrial tasks.
In IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS), pages 9072–9079, 2020.
URL https://doi.org/10.1109/IROS45743.2020.9341570.
[35] Aidan Scannell, Carl Henrik Ek, and Arthur Richards.
Trajectory Optimisation in Learned Multimodal Dynamical Systems Via Latent-ODE Collocation. In IEEE
International Conference on Robotics and Automation(ICRA), 2021.
[36] Stefan Schaal. Dynamic Movement Primitives -A Framework for Motor Control in Humans and Humanoid
Robotics. In Adaptive Motion of Animals and Machines, pages 261–280. Springer-Verlag, Tokyo. doi:
10.1007/4-431-31381-8 23. URL http://link.springer.
com/10.1007/4-431-31381-8{ }23.
[37] M. Yunus Seker, Mert Imre, Justus H. Piater, and EmreUgur. Conditional neural movement primitives. In
Robotics: Science and Systems (R:SS), 2019.
[38] Hang Shao, Abhishek Kumar, and P. Thomas Fletcher.
The Riemannian geometry of deep generative models.
In IEEE/CVF Conference on Computer Vision and Pattern Recognition Workshops (CVPRW), pages 428–4288,
2018. URL https://doi.org/10.1109/CVPRW.2018.00071.
[39] Saif Sidhik. panda simulator: Gazebo simulator for
Franka Emika Panda robot supporting sim-to-real codetransfer, 2020.
URL https://doi.org/10.5281/zenodo.
3747459.
[40] Saif Sidhik. justagist/franka ros interface: Franka ROS
Interface version for franka-ros v0.7.1-release, 2020.
URL https://doi.org/10.5281/zenodo.4320612.
[41] Suvrit Sra. Directional statistics in machine learning: A
brief review. In Christophe Ley and Thomas Verdebout,editors, Applied Directional Statistics. 2018. 1st edition.