N
ϕ

a
i

⁢

ϕ
i

(

x
t

)

=

x
t

+

A
T

⁢

ϕ
t

(
2
)

The remaining difference to the Euclidean case is the basis vector matrix V_x_t∈ [Image Omitted] ^(D^x^+1)×D^x. This basis vector matrix is used to represent hyperbolic tangent space vectors in local coordinates according to the Lorentz model. In this setting, Δ^Tϕ_t∈^D^xdenotes a tangent space vector at x_trepresented in local coordinates. By linearly combining the basis vectors weighted by the local coordinates of the tangent space a change of basis is performed and the corresponding Lorentz representation V_x_tA^Tϕ_t∈T_x_t [Image Omitted] is obtained.

Besides the linear dynamics model, noise is introduced to make the model probabilistic as follows:

$\begin{matrix} x_{t + 1} = {Exp}_{f_{A} (x_{t})} (V_{f_{A} (x_{t})} {\tilde{ϵ}}_{t}), {\tilde{ϵ}}_{t} ~ 𝒩 (0, {\tilde{Σ}}_{x}) . & (3) \end{matrix}$

It should be noted that the noise vector {tilde over (ϵ)}_t [Image Omitted] ^D^xis also given in local coordinates. To represent the noise as a hyperbolic tangent space vector it is multiplied with the basis vector matrix V₁₇_A_(x_t₎∈^(D^x^+1)×D^x. To simplify the notation, the degenerate noise covariance matrix is defined as

$Σ_{x} = V_{f_{A} (x_{t})} {\tilde{Σ}}_{x} V_{f_{A} (x_{t})}^{⊤} \in ℝ^{(D_{x} + 1) \times (D_{x} + 1)} .$

Having introduced noisy observations, the next step is to compute the probability density function of the state x_t+1given x_tas follows:

$\begin{matrix} p (x_{t + 1} ❘ x_{t}, A) = 𝒩_{ℍ_{ℒ}^{D_{x}}} (x_{t + 1} ❘ f_{A} (x_{t}), Σ_{x}) & (4) \end{matrix}$

$\begin{matrix} \approx 𝒩 ({Log}_{x_{t}} (x_{t + 1}) ❘ V_{x_{t}} A^{⊤} ϕ_{i}, Γ_{f_{A} (x_{t}) \to x_{t}} (Σ_{x})) & (5) \end{matrix}$

$\begin{matrix} \approx 𝒩 ({\tilde{x}}_{t + 1} ❘ A^{⊤} ϕ_{t}, {\tilde{Σ}}_{x}) & (6) \end{matrix}$

More precisely, in equation (4), the PDF (probability density function) is formulated in terms of a Riemannian Gaussian distribution. Then, an approximation of the hyperbolic PDF by the Euclidean PDF on the tangent space w.r.t the current state x_tis used. To achieve this, the logarithmic map of the mean ƒ_A(x_t) and the parallel transport of the degenerate covariance matrix Σ are used. It should be noted that the Lorentzian (hyperbolic) tangent space vectors are represented in local coordinates and the definition {tilde over (x)}_t+1: =V_x_t^TG Log_x_t(x_t+1)∈ [Image Omitted] ^D^x(and an analogous definition of {tilde over (x)}_t) is used.

It should also be noted that as the basis vector matrix V_x_tconsists of orthonormal basis vectors of the corresponding tangent space, the Lorentz product of two of such matrices equals the identity matrix V_x_t^TGV_x_t=I_D_x.

Now the hyperbolic dynamics prior can be derived by marginalizing out the parameters A, similar to the Euclidean case. For a single trajectory X of N latent variables x₁, . . . , x_N∈ [Image Omitted] the hyperbolic dynamics prior, following a first-order Markov chain assumption, is given as follows

$\begin{matrix} p (X) = \int p (X ❘ A) p (A) dA & (7) \end{matrix}$

$\begin{matrix} = p (x_{1}) \int \overset{N}{\prod_{t = 2}} p (x_{t} ❘ x_{t - 1}, A) p (A) dA & (8) \end{matrix}$

$\begin{matrix} = p (x_{1}) \int \overset{N}{\prod_{t = 2}} 𝒩_{ℍ_{ℒ}^{D_{x}}} (x_{t} ❘ f_{A} (x_{t - 1}), Σ_{x}) p (A) dA & (9) \end{matrix}$

$\begin{matrix} = p (x_{1}) \int \overset{N}{\prod_{t = 2}} 𝒩 ({\tilde{x}}_{t} ❘ A^{⊤} ϕ_{t - 1}, {\tilde{Σ}}_{x}) p (A) dA & (10) \end{matrix}$

$\begin{matrix} = p (x_{1}) \overset{D_{x}}{\prod_{d = 1}} \int \overset{N}{\prod_{t = 2}} p ({\tilde{x}}_{t, d} ❘ ϕ_{t - 1}^{⊤} A_{d}, σ_{x, d}^{2}) & (11) \end{matrix}$

$𝒩 (A_{d} ❘ 0, I_{N_{ϕ}}) {dA}_{d}$

$\begin{matrix} \approx 𝒩_{ℍ_{ℒ}^{D_{x}}} (x_{1} ❘ μ_{0}, V_{μ_{0}} V_{μ_{0}}^{⊤}) & (12) \end{matrix}$

$𝒩_{ℍ_{ℒ}^{D_{x}}} (X_{2 : N} ❘ X_{1 : N - 1}, V_{1 : N - 1} (K_{1 : N - 1} + {\tilde{Σ}}_{X}) V_{1 : N - 1}^{⊤})$

Like in the Euclidean case, the marginalization integral over the parameters A is first built and then the Markov property is applied to compute the conditional single-step distributions. It should be noted that equation (11) handles only terms that lie on Euclidean tangent spaces, hence the next steps are the same as in the Euclidean case. Further, it should be noted that since Log_μ₀(x) is a tangent space vector at the origin, the metric tensor is not needed to represent it in local coordinates. Finally, equation (12) represents the final hyperbolic dynamics prior model.

The Gaussian Process Hyperbolic Dynamical Model (GPHDM) now combines the hyperbolic dynamics prior of (12) with the GPHLVM. The goal of the model is to embed each high-dimensional observed motions (i.e. trajectory, i.e. sequence of poses in “real” space, e.g. joint space)

Y=[y₁. . . y_N]^T∈ [Image Omitted] ^N×D^y

into the hyperbolic latent space [Image Omitted] such that a low-dimensional latent variable x_t∈ (i.e. pose embedding) is obtained for each corresponding observed pose y_t. Additionally, in the training of the model, the latent embeddings are required to preserve the trajectory structure of the high-dimensional motions while simultaneously resembling the graph structure of a robotics taxonomy.

This means that it is assumed that the robotics taxonomy is given as an undirected graph G=(V,E) with nodes V={c¹, . . . , c^N^c} and edges E between the nodes, as well as a distance measure dG between graph nodes (e.g. reflecting a similarity of poses represented by the graph nodes) as for example described in reference [2].

Further, it is assumed that fully-labelled training data {(y_t, c_t)}_t=1^Nis available, i.e., that for each observed pose y_t∈ [Image Omitted] ^D^ythe corresponding taxonomy node c_l∈V is known (or can be determined from the taxonomy that is used).

Then, the optimization problem for determining the GPHDM optimization process can be written as follows:

$\begin{matrix} W, Θ = \underset{W, Θ}{\arg \max} β_{1} \log p (Y ❘ X, Θ) + β_{1} \log p (X ❘ Θ) - β_{3} ℒ_{stress} (X), & (13) \end{matrix}$

$s . t . X = gw (Y)$

where the latent mapping, dynamics mapping, stress loss, and back-constraints are given by

$\begin{matrix} p (Y ❘ X, Θ) = 𝒩 (Y ❘ 0, K_{X} + Σ_{Y}) & (14) \end{matrix}$

$\begin{matrix} p (X ❘ Θ) = 𝒩_{ℍ_{ℒ}^{D_{x}}} (x_{1} ❘ μ_{0}, V_{μ_{0}} V_{μ_{0}}^{⊤}) & (15) \end{matrix}$

$𝒩_{ℍ_{ℒ}^{D_{x}}} (X_{2 : N} ❘ X_{1 : N - 1}, V_{1 : N - 1} (K_{1 : N - 1} + Σ_{X}) V_{1 : N - 1}^{⊤})$

$\begin{matrix} ℒ_{stress} (X) = \sum_{i < j} {(d_{G} (c_{i}, c_{j}) - d_{ℍ_{ℒ}^{D_{x}}} (x_{i}, x_{j}))}^{2} & (16) \end{matrix}$

$\begin{matrix} g_{W} (Y) = Exp μ_{0} (V_{μ_{0}} (K_{Y} \cdot K_{G}) W) & (17) \end{matrix}$

It should be noted that p(Y|X, Θ) denotes the latent mapping, or likelihood, of the model. For given observations Y, the embeddings X and hyperparameters Θ are optimized, wherein the latter include the noise variance, the kernel outputscale, and the lengthscale. In other words, latent variables X and hyperparameters Θ should be find that describe the given data Y as close as possible in the sense of maximum likelihood estimation. However, the model would have no incentive to structure its latent space by solely optimizing the likelihood.

Therefore, the dynamics mapping p(X|Θ) is added as a prior to the objective function of the optimization problem (13), turning it into a maximum a posteriori estimation. It should be noted that the dynamics mapping induces consecutive latent points x_t, x_t+1to keep close together and consequently form smooth latent trajectories. Additionally to the dynamics mapping, the stress loss [Image Omitted] _stressis subtracted, which can be viewed as a second prior.

It should be noted that that maximizing the negative stress loss minimizes the difference between the pairwise node distance on the taxonomy graph (i.e. pose similarities according to the taxonomy) and the distance of the corresponding embeddings in the latent space. This induces the latent embeddings to resemble the taxonomy graph. The three individual losses, i.e., the latent mapping, the dynamic mapping, and the stress loss, are weighted using scalar weights β₁, β₂, β₃∈(0, 1) and are summed up to obtain the final GPHDM loss.

While it is possible to optimize this loss directly over the latent variables X, the parameters (e.g. weights) W of an encoder function g_W(e.g. a neural network) are optimized. This also referred to as back-constraints. That allows embedding new observations after the training directly into the latent space without additional training using the encoder (function) trained in this manner. Furthermore, the encoder can be adapted to incorporate information about the taxonomy structure.

According to one embodiment, point-wise multiplication is used to combine the SE (squared exponential) kernel on the observations K_Ywith the graph-Matérn kernel K_Gon the taxonomy nodes.

In total, according to various embodiments, the GPHDM relies on four different kernel functions. One multi-output hyperbolic heat kernel for the latent mapping K_X∈ [Image Omitted] ^ND^y^×ND^yand one for the dynamics mapping K_1:N−1∈^(N−1)D^x^×(N−1)D^xone Euclidean SE kernel for the back-constraints K_Y∈^N×Nand one graph-Matern kernel on the taxonomy nodes also for the back-constraints K_G∈^N×N.

It should be noted that if one kernel function is shared across dimensions, the size of the kernel matrices can be significantly reduced to

K_X∈ [Image Omitted] ^N×Nand K_1:N−1∈^{(N−1)×(N=1)}which allows for lower memory requirements and faster training. It should further be noted that the graph-Matérn kernel K_Gcan only be constructed when fully-labelled training data is available.

To optimize Equation (13), Riemannian Adam may for example be used, which is a first-order optimization method. First-order optimization methods rely on an initialization of the optimized variables. A typical choice for the initialization of the latent variables X_initis the Principal Component Analysis (PCA) which spans a linear subspace such that the retained amount of variance in the data is maximized. The problem is that, typically, high-dimensional motions are highly non-linear. Thus, the projection into the linear subspace cannot preserve the data's structure. Since GPLVM models require a good initialization to avoid getting stuck in local optima, initializing the latent variables with something other than PCA might offer an improved model performance. In view of that, according to various embodiments, the latent variables are initialized by optimizing the stress loss

$\begin{matrix} X_{init} = \underset{X}{\arg \min} \sum_{i = 1}^{N} \sum_{j = i + 1}^{N} {(d_{G} (c_{i}, c_{j}) - d_{ℍ_{ℒ}^{D_{x}}} (x_{i}, x_{j}))}^{2} & (18) \end{matrix}$

This initialization is referred to as stress loss initialization. The minimization in equation (18) itself also requires an initialization for the latent variables, for which PCA can be used or random latent variables can be chosen.

In summary, the optimization process involves three steps:

- (i) obtaining the initial latent variables from PCA.
- (ii) optimizing the stress loss on these latent variables, which typically converges after a few seconds.
- (iii) optimizing the full GPHDM as detailed in equation (13) on the stress loss initialized latent variables.

Similarly as the GPHLVM latent space, the GPHDM latent space can be exploited to plan motions by following trajectories in the low-dimensional latent space. The latent space geometry can be exploited to plan motions by following geodesics, i.e., shortest paths, between two embeddings in the hyperbolic latent space. However, such trajectories do not account for the uncertainty of the model and may pass through regions of the latent space where no training data is available to support motion prediction, causing the prediction to revert to the non-informative default case, i.e., to the Gaussian process (GP) mean. Therefore, according to various embodiments, geodesics in the hyperbolic latent space are computed according to a metric related to the high-dimensional motion metric, the so-called pullback metric. Geodesics computed according to the pullback metric tend to avoid regions with high uncertainty, thus leading to the generation of motions that follow the training data. Assuming a Euclidean observation space, the pullback metric G∈ [Image Omitted] ^D^x^×D^xcorresponding to a deterministic mapping function ƒ: ^D^x→^D^yis computed as

$\begin{matrix} G = J^{⊤} J & (19) \end{matrix}$

with J∈ [Image Omitted] ^D^y^×D^xthe Jacobian of f, i.e. the Gaussian process which maps embeddings (i.e. elements of the latent space) to real space poses. In the case of a GPLVM, the conditional distribution over the Jacobian is a Gaussian distribution

$\begin{matrix} p (J) = \prod_{d = 1}^{D_{y}} 𝒩 (J_{d} ❘ μ_{J, d}, Σ_{J}) & (20) \end{matrix}$

with mean and covariance

$\begin{matrix} μ_{J, d} = \partial k (x^{*}, X) {(K_{X} + σ_{y}^{2} I_{N})}^{- 1} Y_{d}, & (21) \end{matrix}$

$Σ_{J} = \partial^{2} k (x^{*}, x^{*}) - \partial k (x^{*}, X) {(K_{X} + σ_{y}^{2} I_{N})}^{- 1} \partial k (X, x^{*})$

This distribution induces the metric tensor to follow a non-central Wishart distribution

$\begin{matrix} G = J^{⊤} J ~ p (G) = W_{d_{x}} (D_{y}, Σ_{J}, {𝔼 [J]}^{⊤} 𝔼 [J]) & (22) \end{matrix}$

with mean prediction for the metric tensor given by

$\begin{matrix} 𝔼 [G] = {𝔼 [J]}^{T} 𝔼 [J] + D_{y} Σ_{J} & (23) \end{matrix}$

According to one embodiment, the computation of the pullback metric G is adapted to the hyperbolic latent space. This is achieved by considering the fact that the kernel k in (21) is a hyperbolic kernel and by adapting the computation of the first and second derivative of the kernel accordingly. The resulting pullback metric G is then used within the classical geodesic equation, which is solved to compute geodesics in the hyperbolic latent space. The resulting geodesics follow the transitions between classes defined in the taxonomy, while avoiding uncertain regions of the latent space. More precisely, once the pullback metric has been estimated, the motion trajectory generation boils down to first compute a geodesic on the GPHDM latent space, and decode this geodesic via the Gaussian process of the GPDM. Specifically, two different points x,y∈ custom-character