Geometry-aware manipulability learning, tracking, and transfer

Abstract

Body posture influences human and robot performance in manipulation tasks, as appropriate poses facilitate motion or the exertion of force along different axes. In robotics, manipulability ellipsoids arise as a powerful descriptor to analyze, control, and design the robot dexterity as a function of the articulatory joint configuration. This descriptor can be designed according to different task requirements, such as tracking a desired position or applying a specific force. In this context, this article presents a novel manipulability transfer framework, a method that allows robots to learn and reproduce manipulability ellipsoids from expert demonstrations. The proposed learning scheme is built on a tensor-based formulation of a Gaussian mixture model that takes into account that manipulability ellipsoids lie on the manifold of symmetric positive-definite matrices. Learning is coupled with a geometry-aware tracking controller allowing robots to follow a desired profile of manipulability ellipsoids. Extensive evaluations in simulation with redundant manipulators, a robotic hand and humanoids agents, as well as an experiment with two real dual-arm systems validate the feasibility of the approach.

1. Introduction

When we perform a manipulation task, we naturally place our arms (and body) in a posture that is best suited to carry out the task at hand (see Figure 1). Such posture variation is a means through which the motion and strength characteristics of the arms are made compatible with the task requirements. For example, human arm kinematics plays a central role when humans plan point-to-point reaching movements, where joint trajectory patterns arise as a function of the visual target (Morasso, 1981), indicating that the task requirements influence the human arm posture. This insight was also identified in more complex situations, where not only kinematic but also other biomechanic factors affect the task planning (Cos et al., 2011). For example, the human central nervous system plans arm movements considering its directional sensitivity, which is directly related to the arm posture (Sabes and Jordan, 1997). This allows humans to be mechanically resistant to potential perturbations coming from obstacles occupying the workspace. Interestingly, directional preferences of human arm movements are characterized by a tendency to exploit interaction torques for movement production at the shoulder or elbow, indicating that the preferred directions are largely determined by biomechanical factors (Dounskaia et al., 2014).

Fig. 1.

Illustration of pushing (a) and pulling (b) tasks for which the posture of the human significantly influences their ability to carry out the task.

The robotics community has also been aware of the impact of robot posture on reaching movements and manipulation tasks (e.g., pushing, pulling, reaching). It is well known that by varying the posture of a robot, we can change the optimal directions for generating motion or applying specific forces. This has direct implications in hybrid control, because the controller capability can be fully realized when the optimal directions for controlling velocity and force coincide with those dictated by the task (Chiu, 1987). In this context, the so-called manipulability ellipsoid (Yoshikawa, 1985b) serves as a geometric descriptor that indicates the ability to arbitrarily perform motion and exert a force along the different task directions in a given joint configuration.

Manipulability ellipsoids have been used to measure the compatibility of robot postures with respect to fine and coarse manipulation (Chiu, 1987), and to improve minimum-time trajectory planning using a manipulability-aware inverse kinematics algorithm (Chiacchio, 1990). Vahrenkamp et al. (2012) proposed a grasp selection process that favored high manipulability in the robot workspace. Other works have focused on maximizing the manipulability ellipsoid volume in trajectory generation algorithms (Guilamo et al., 2006), and task-level robot programming frameworks (Somani et al., 2016), to obtain singularity-free joint trajectories and high task-space dexterity. Nevertheless, as stated in Lee (1989), solely maximizing the ellipsoid volume to achieve high dexterity in motion may cause a reverse effect on the flexibility in force.

The aforementioned approaches do not specify a desired robot manipulability for the task. In contrast, Lee and Oh (2016) proposed an optimization method to find reaching postures for a humanoid robot that achieved desired (manually specified) manipulability volumes. Similarly, a series of desired manipulability ellipsoids was predefined according to Cartesian velocity and force requirements in dual-arm manipulation tasks (Lee, 1989). Note that both Lee (1989) and Lee and Oh (2016) predetermined the task-dependent robot manipulability, which required a meticulous and demanding analysis about the motion that the robot needed to perform, which becomes impractical when the robot is required to carry out a large set of different tasks. Furthermore, these approaches overlooked an important characteristic of manipulability ellipsoids, namely, the fact that they lie on the manifold of symmetric positive-definite (SPD) matrices. This may influence the optimal robot joint configuration for the task at hand.

Other geometric descriptors have been proposed in the literature to evaluate the velocity or force performance of robots at a given joint configuration. In contrast to manipulability ellipsoids that do not fully account for boundary limits in the space of joint velocities or torques, manipulability polytopes provide a linear estimate of the exact joint constraints in task space (Chiacchio et al., 1997; Lee, 1997). Moreover, Ajoudani et al. (2015) introduced the concept of a stiffness feasibility region (SFR) to represent the non-polytopic boundary where the realization of a desired Cartesian stiffness matrix is feasible. While the polytope approaches provide a more accurate estimate of the velocity or force generation capabilities of the robot compared with manipulability ellipsoids, their calculation is computationally expensive. SFR is a particular Cartesian stiffness descriptor and therefore does not generalize to other robot control settings. Manipulability ellipsoids are easy to compute, while representing an intuitive estimate of the robot ability to perform velocities, accelerations or exert forces along the different task directions.

In this article we introduce the novel idea that manipulability-based posture variation for task compatibility can be addressed from a robot learning from demonstration perspective. Specifically, we cast this problem as a manipulability transfer between a teacher and a learner. The former demonstrates how to perform a task with a desired time-varying manipulability profile, while the latter reproduces the task by exploiting its own redundant kinematic structure so that its manipulability ellipsoid matches the demonstration. Unlike classical learning frameworks that encode reference position, velocity, and force trajectories, our approach offers the possibility of transferring posture-dependent task requirements such as preferred directions for motion and force exertion in operational space, which are encapsulated in the demonstrated manipulability ellipsoids.

This idea opens two main challenges, namely, (i) how to encode and retrieve a sequence of manipulability ellipsoids, and (ii) how to track a desired time-varying manipulability either as the main task of the robot or as a secondary objective. To address the former problem, we propose a tensor-based formulation of Gaussian mixture model (GMM) and Gaussian mixture regression (GMR) that takes into account that manipulability ellipsoids lie on the manifold of SPD matrices (see Section 3 for a full description of the model). The latter challenge is solved through a manipulability tracking formulation inspired by the classical inverse kinematics problem in robotics, where a first-order differential relationship between the robot manipulability ellipsoid and the robot joints is established, as explained in Section 4. This relationship also demands to consider that manipulability ellipsoids lie on the SPD manifold, which is here tackled by exploiting tensor-based representations and differential geometry (see Section 2). The geometry awareness of our formulations is crucial for achieving successful manipulability transfer, as shown in Section 5. Note that Riemannian geometry has also been successfully exploited in robot motion optimization (Ratliff et al., 2015) and manipulability analysis of closed chains (Park and Kim, 1998). For the sake of clarity, different aspects of the proposed learning and tracking approaches are illustrated with simple examples using simulated planar robots throughout the article.

The proposed approach can be straightforwardly applied to different types of kinetostatic and dynamic manipulability measures. This opens the door to manipulability transfer scenarios with various types of robots where different task requirements at kinematic and dynamic levels can be learned and successfully transferred between agents of different embodiments. The functionality of the proposed approach is evaluated in different simulated manipulability tracking tasks involving a 16-degree-of-freedom (DoF) robotic hand and two legged robots. The full manipulability transfer is showcased in a bimanual setup where an unplugging task is kinesthetically demonstrated to a 14-DoF dual-arm robot, which then transfers the learned model to a different dual-arm system that reproduces the unplugging task successfully, as described in Section 6.

Early contributions on our learning and tracking frameworks were presented in Rozo et al. (2017) and Jaquier et al. (2018), respectively. In Rozo et al. (2017), the learning approach provided a sequence of desired manipulability ellipsoids that a learner robot reproduced using gradient-based nullspace commands. Existing approaches built on the optimization of manipulability-based indices are not suitable as they do not allow the tracking of specific manipulability ellipsoids. In Jaquier et al. (2018), the tracking framework used manually specified robot manipulability ellipsoids for the task. As mentioned previously, this may be tedious and cumbersome when the robot needs to carry out different and complex tasks. Therefore, the integration of the proposed learning and tracking approaches solves the aforementioned problems and offers a complete geometry-aware manipulability transfer framework where manipulability ellipsoid profiles are learned from demonstrations and reproduced accurately. This opens the possibility to transfer posture-dependent task requirements between agents of dissimilar kinematic structures. In particular, this framework also permits to transfer other velocity, force, or impedance specifications with any priority order with respect to the manipulability tracking controller.

Beyond the combination of our early contributions on manipulability learning and tracking, the other contributions of this article are: (i) analyzing the role of the proposed differential geometry formulation of the geometry-aware tensor-based GMM/GMR adapted to manipulability ellipsoids; (ii) extending the geometry-aware manipulability tracking control scheme initially designed for kinetostatic manipulability measures to dynamic measures; (iii) demonstrating the exponential stability of the proposed manipulability tracking controllers; (iv) introducing various novel types of geometry-aware manipulability tracking schemes and methodologies to consider the robot actuators contribution and variability-based tracking precision; (v) analyzing the importance of the geometry-awareness of the manipulability tracking controllers by comparison against state-of-the-art manipulability-based optimization methods.

A summary video, as well as videos of the illustrative planar examples and simulated and real experiments accompany the article and can be found at https://sites.google.com/view/manipulability. Related source codes are available at https://github.com/NoemieJaquier/Manipulability.

2. Background

2.1. Manipulability ellipsoids

Velocity and force manipulability ellipsoids introduced in Yoshikawa (1985b) are kinetostatic performance measures of robotic platforms. They indicate the preferred directions in which force or velocity control commands may be performed at a given joint configuration. More specifically, the velocity manipulability ellipsoid describes the characteristics of feasible motion in Cartesian space corresponding to all the unit norm joint velocities. The velocity manipulability of an n-DoF robot can be found by using the kinematic relationship between task velocities $\stackrel{·}{\mathit{x}}$ and joint velocities $\stackrel{·}{\mathit{q}}$,

$$\stackrel{·}{\mathit{x}}=\mathit{J}(\mathit{q})\stackrel{·}{\mathit{q}}$$ (1)

where $\mathit{q}\in {\mathbb{R}}^n$ and $\mathit{J}\in {\mathbb{R}}^{6\times n}$ are the joint position and Jacobian of the robot, respectively. Moreover, consider the set of joint velocities of constant (unit) norm $\parallel \stackrel{·}{\mathit{q}}{\parallel }^2=1$ describing the points on the surface of a hypersphere in the joint velocity space, which is mapped into the Cartesian velocity space ${\mathbb{R}}^6$ with¹

$$\parallel \stackrel{·}{\mathit{q}}{\parallel }^2={\stackrel{·}{\mathit{q}}}^{\top }\stackrel{·}{\mathit{q}}={\stackrel{·}{\mathit{x}}}^{\top }(\mathit{J}{\mathit{J}}^{\top }{)}^{-1}\stackrel{·}{\mathit{x}}$$ (2)

by using the least-squares inverse kinematics relation $\stackrel{·}{\mathit{q}}={\mathit{J}}^{†}\stackrel{·}{\mathit{x}}={\mathit{J}}^{\top }(\mathit{J}{\mathit{J}}^{\top }{)}^{-1}\stackrel{·}{\mathit{x}}$. Equation (2) represents the robot manipulability in terms of motion, indicating the flexibility of the manipulator in generating velocities in Cartesian space.²

In the literature, the velocity manipulability ellipsoid is usually defined as $(\mathit{J}{\mathit{J}}^{\top }{)}^{-1}$, where the principal axes of the ellipsoid coincide with the eigenvectors and their length is equal to the inverse of the square root of the corresponding eigenvalues, i.e., $\frac{1}{\sqrt{{\lambda }_i}}$ (see, e.g., Chiu, 1987). For the sake of consistency, we here use an alternative definition of the velocity manipulability ellipsoid given by ${\mathit{M}}^{\stackrel{·}{\mathit{x}}}=\mathit{J}{\mathit{J}}^{\top }$. In this case, the major axis of the manipulability ellipsoid is aligned to the eigenvector associated with the maximum eigenvalue ${\lambda }_{\max }$ of ${\mathit{M}}^{\stackrel{·}{\mathit{x}}}$, whose length equals the square root of ${\lambda }_{\max }$. Thus, the interpretation and representation of the manipulability ellipsoid from the corresponding matrix are facilitated. Note that the major axis of the velocity manipulability ellipsoid ${\mathit{M}}^{\stackrel{·}{\mathit{x}}}=\mathit{J}{\mathit{J}}^{\top }$ indicates the direction in which the greater velocity can be generated, which is also the direction in which the robot is more sensitive to perturbations. This occurs owing to the principal axes of the force manipulability being aligned with those of the velocity manipulability, with reciprocal lengths (eigenvalues) caused by the duality of velocity and force (for details see Chiu, 1987).

Other forms of manipulability ellipsoids exist, such as the dynamic manipulability (Yoshikawa, 1985a), which gives a measure of the ability of performing end-effector accelerations along each task-space direction for a given set of joint torques. This has been shown to be useful when the robot dynamics cannot be neglected in highly dynamic manipulation tasks (Chiacchio et al., 1991b). Recent works have extended this measure to analyze the robot capacity to accelerate its center of mass (CoM) for locomotion stability (Azad et al., 2017; Gu et al., 2015), showing the applicability of the aforementioned tools beyond robotic manipulation.

As mentioned previously, any manipulability ellipsoid $\mathit{M}$ belongs to the set of SPD matrices ${\mathcal{S}}_{++}^D$ that describes the interior of the convex cone. Consequently, our manipulability transfer formulation must consider this particular characteristic to properly encode, reproduce, and track manipulability ellipsoids. To do so, we here propose geometry-aware formulations of both learning and tracking problems by exploiting Riemannian manifolds and tensor representations, which are introduced next.

2.2. Riemannian manifold of SPD matrices

The set of $D\times D$ SPD matrices ${\mathcal{S}}_{++}^D$ is not a vector space because it is not closed under addition and scalar product (Pennec et al., 2006), and thus the use of classical Euclidean space methods for treating and analyzing these matrices is inadequate. A compelling solution is to endow these matrices with a Riemannian metric so that these form a Riemannian manifold.³ This metric permits to define geodesics, which are the generalization of straight lines to Riemannian manifolds. Similarly to straight lines in Euclidean space, geodesics are the minimum-length curves between two points on the manifold.

Intuitively, a Riemannian manifold $\mathcal{M}$ is a mathematical space for which each point locally resembles a Euclidean space. For each point $\mathbf{\Sigma }\in \mathcal{M}$, there exists a tangent space ${\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$ equipped with a positive-definite inner product. In the case of the SPD manifold, the tangent space at any point $\mathbf{\Sigma }\in {\mathcal{S}}_{++}^{\mathbf{D}}$ is identified by the space of symmetric matrices $\mathrm{Sy}{\mathrm{m}}^D$ and the inner product between two matrices ${\mathit{T}}_1$, ${\mathit{T}}_2\in {\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$ is

$$〈{\mathit{T}}_1,{\mathit{T}}_2{〉}_{\mathbf{\Sigma }}=\mathrm{tr}({\mathbf{\Sigma }}^{-\frac{1}{2}}{\mathit{T}}_1{\mathbf{\Sigma }}^{-1}{\mathit{T}}_2{\mathbf{\Sigma }}^{-\frac{1}{2}})$$ (3)

The space of SPD matrices can be represented as the interior of a convex cone embedded in its tangent space $\mathrm{Sy}{\mathrm{m}}^D$. To utilize these tangent spaces, we need mappings back and forth between ${\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$ and $\mathcal{M}$, which are known as exponential and logarithmic maps.

The exponential map $\mathrm{Ex}{\mathrm{p}}_{\mathbf{\Sigma }}:{\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}\to \mathcal{M}$ maps a point $\mathit{L}$ in the tangent space to a point $\mathbf{\Lambda }$ on the manifold, so that it lies on the geodesic starting at $\mathbf{\Sigma }$ in the direction $\mathit{L}$ and such that the distance between $\mathbf{\Sigma }$ and $\mathbf{\Lambda }$ is equal to the norm of $\mathit{L}$ in the tangent space. The inverse operation is called the logarithmic map $\mathrm{Lo}{\mathrm{g}}_{\mathbf{\Sigma }}:\mathcal{M}\to {\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$. Both operations are illustrated in Figure 2(a).

Fig. 2.

SPD manifold ${\mathcal{S}}_{++}^2$ embedded in its tangent space $\mathrm{Sy}{\mathrm{m}}^2$. One point corresponds to a matrix $\left(\begin{array}{cc}{T}_{11}\hfill & T_{12}\hfill \\ \multicolumn{1}{c}{T_{12}}& T_{22}\hfill \end{array}\right)$$\in \mathrm{Sy}{\mathrm{m}}^2$. Points inside the cone, such as $\mathbf{\Sigma }$ and $\mathbf{\Lambda }$, belong to ${\mathcal{S}}_{++}^2$. In (a) $\mathit{L}$ lies on the tangent space of $\mathbf{\Sigma }$ such that $\mathit{L}=\mathrm{Lo}{\mathrm{g}}_{\mathbf{\Sigma }}(\mathbf{\Lambda })$. The shortest path between $\mathbf{\Sigma }$ and $\mathbf{\Lambda }$ is the geodesic represented as a red curve, which differs from the Euclidean path depicted in yellow. In (b) $\tilde{\mathit{T}}\in {\mathcal{T}}_{\mathbf{\Lambda }}\mathcal{M}$ is the result of the parallel transport of $\mathit{T}\in {\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$ from ${\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$ to ${\mathcal{T}}_{\mathbf{\Lambda }}\mathcal{M}$. The coordinate axes of the tangent spaces ${\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}$ and ${\mathcal{T}}_{\mathbf{\Lambda }}\mathcal{M}$ are represented in dark gray.

Specifically, the exponential and logarithmic maps on the SPD manifold corresponding to the affine-invariant distance

$$d(\mathbf{\Lambda },\mathbf{\Sigma })=\parallel \mathbf{log}({\mathbf{\Sigma }}^{-\frac{1}{2}}\mathbf{\Lambda }{\mathbf{\Sigma }}^{-\frac{1}{2}}){\parallel }_{\mathrm{F}}$$ (4)

are computed as (see (Pennec et al., 2006) for details)

$$\mathbf{\Lambda }=\mathrm{Ex}{\mathrm{p}}_{\mathbf{\Sigma }}(\mathit{L})={\mathbf{\Sigma }}^{\frac{1}{2}}\mathbf{exp}({\mathbf{\Sigma }}^{-\frac{1}{2}}\mathit{L}{\mathbf{\Sigma }}^{-\frac{1}{2}}){\mathbf{\Sigma }}^{\frac{1}{2}}$$ (5)

$$\mathit{L}=\mathrm{Lo}{\mathrm{g}}_{\mathbf{\Sigma }}(\mathbf{\Lambda })={\mathbf{\Sigma }}^{\frac{1}{2}}\mathbf{log}({\mathbf{\Sigma }}^{-\frac{1}{2}}\mathbf{\Lambda }{\mathbf{\Sigma }}^{-\frac{1}{2}}){\mathbf{\Sigma }}^{\frac{1}{2}}$$ (6)

where $\exp (·)$ and $\log (·)$ are the matrix exponential and logarithm functions.

Another useful operation over manifolds is the parallel transport ${\mathrm{\Gamma }}_{\mathbf{\Sigma }\to \mathbf{\Lambda }}:{\mathcal{T}}_{\mathbf{\Sigma }}\mathcal{M}\to {\mathcal{T}}_{\mathbf{\Lambda }}\mathcal{M}$, which moves elements between tangent spaces such that the angle between two elements in the tangent space remains constant (see Figure 2(b)). The parallel transport of $\mathit{T}\in {\mathcal{T}}_{\mathbf{\Sigma }}{\mathcal{S}}_{++}^D$ to ${\mathcal{T}}_{\mathbf{\Lambda }}{\mathcal{S}}_{++}^D$ is given by

$$\tilde{\mathit{T}}={\mathrm{\Gamma }}_{\mathbf{\Sigma }\to \mathbf{\Lambda }}(\mathit{T})={\mathit{A}}_{\mathbf{\Sigma }\to \mathbf{\Lambda }}{\mathit{TA}}_{\mathbf{\Sigma }\to \mathbf{\Lambda }}^{\top }$$ (7)

with ${\mathit{A}}_{\mathbf{\Sigma }\to \mathbf{\Lambda }}={\mathbf{\Lambda }}^{\frac{1}{2}}{\mathbf{\Sigma }}^{-\frac{1}{2}}$ (for details see Sra and Hosseini, 2015). This operation is exploited when it is necessary to move SPD matrices along a curve on the nonlinear manifold. Finally, for a complete introduction to differential geometry and Riemannian manifolds, we refer the interested reader to, e.g., do Carmo (1992) and Lee (2012).

In this article, we first exploit the Riemannian manifold framework to propose a probabilistic learning model that encodes and retrieves manipulability ellipsoids considering that these belong to ${\mathcal{S}}_{++}^D$. Second, we take advantage of the Riemannian geometry to compute the difference between manipulability ellipsoids in the tracking problem, and consequently propose novel velocity- and acceleration-based controllers. This geometry-aware approach proves to be crucial for learning and tracking manipulability ellipsoids in terms of accuracy, stability, and convergence, beyond providing an appropriate mathematical treatment of both problems.

2.3. Tensor representation

Tensors are generalization of matrices to arrays of higher dimensions (Kolda and Bader, 2009), where vectors and matrices may respectively be seen as first- and second-order tensors. Tensor representation permits to represent and exploit data structure of multidimensional arrays. In this article, such representation is first used in the learning process to encode a distribution of manipulability ellipsoids (as explained in Section 3). Then, tensor representation is also exploited in the proposed manipulability tracking formulation to find the first-order differential relationship between the robot joints and the robot manipulability ellipsoid (first- and second-order tensors, respectively), which results in a third-order tensor (see Section 4). To do so, we first introduce the tensor operations needed for our mathematical treatment.

2.3.1. Tensor product

The tensor product is a multilinear generalization of the outer product of two vectors $\mathit{x}\otimes \mathit{y}=\mathit{x}{\mathit{y}}^{\top }$. The tensor product of two tensors $\mathcal{X}\in {\mathbb{R}}^{I_1\times \cdots \times I_M}$, $\mathcal{Y}\in {\mathbb{R}}^{J_1\times \cdots \times J_N}$ is $\mathcal{X}\otimes \mathcal{Y}\in {\mathbb{R}}^{I_1\times \cdots \times I_M\times J_1\times \cdots \times J_N}$ with elements

$$(\mathcal{X}\otimes \mathcal{Y}{)}_{i_1,\dots ,i_M,j_1,\dots ,j_N}=x_{i_1,\dots ,i_M}{y}_{j_1,\dots ,j_N}$$ (8)

2.3.2. n-mode product

The multiplication of a tensor $\mathcal{X}\in {\mathbb{R}}^{I_1\times \dots \times I_n\times \dots \times I_N}$ by a matrix $\mathit{A}\in {\mathbb{R}}^{J\times I_n}$, known as the n-mode product is defined as

$$\mathcal{Y}=\mathcal{X}{\times }_n\mathit{A}\Leftarrow {\mathit{Y}}_{(n)}=\mathit{A}{\mathit{X}}_{(n)}$$ (9)

where ${\mathit{X}}_{(n)}\in {\mathbb{R}}^{I_n\times I_1{I}_2\dots I_N}$ is the n-mode matricization or unfolding of tensor $\mathcal{X}$. Element-wise, this n-mode product can be written as $(\mathcal{X}{\times }_n\mathit{A}{)}_{i_1\dots i_{n-1}{j}_n{i}_{n+1}\dots i_N}=\sum _{i_n}{a}_{j_n{i}_n}{x}_{i_1\dots i_{n-1}{i}_n{i}_{n+1}\dots i_N}$.

2.3.3. Tensor contraction

As described in Tyagi and Davis (2008), we denote the element $(i,j,k,l)$ of a fourth-order tensor $\mathcal{S}$ by ${\mathcal{S}}_{\mathrm{ij}}^{\mathrm{kl}}$ with two covariant indices i, j and two contravariant indices k, l. The element (k,l) of a matrix $\mathit{X}$ is denoted by ${\mathit{X}}_{\mathrm{kl}}$ with two covariant indices k, l. A tensor contraction between two tensors is performed when one or more contravariant and covariant indices are identical. For example, the tensor contraction of $\mathcal{S}\in {\mathbb{R}}^{D\times D\times D\times D}$ and $\mathit{X}\in {\mathbb{R}}^{D\times D}$ is written as

$$\mathcal{S}\mathit{X}=\sum _{k=1}^D\sum _{l=1}^D{\mathcal{S}}_{\mathrm{ij}}^{\mathrm{kl}}{\mathit{X}}_{\mathrm{kl}}$$ (10)

2.3.4. Tensor covariance

Similarly to the covariance of vectors, the $2M$th-order covariance tensor $\mathcal{S}\in {\mathbb{R}}^{I_1\times \cdots \times I_M\times I_1\times \cdots \times I_M}$ of centered tensors ${\mathcal{X}}_n\in {\mathbb{R}}^{I_1\times \cdots \times I_M}$ is given by

$$\mathcal{S}=\frac{1}{N-1}\sum _{n=1}^N{\mathcal{X}}_n\otimes {\mathcal{X}}_n$$ (11)

where N is the total number of data points. This definition is used in the formulation of tensor-variate normal distributions.

2.3.5. Normal distribution of symmetric matrices

The tensor-variate normal distribution of a random second-order symmetric matrix $\mathit{X}\in \mathrm{Sy}{\mathrm{m}}^D$ with mean $\mathbf{\Xi }\in \mathrm{Sy}{\mathrm{m}}^D$ and covariance $\mathcal{S}\in {\mathbb{R}}^{D\times D\times D\times D}$ is defined as (Basser and Pajevic, 2007)

$$\mathcal{N}(\mathit{X}|\mathbf{\Xi },\mathcal{S})=\frac{1}{\sqrt{{(2\pi )}^{\tilde{D}}|\mathcal{S}|}}{e}^{-\frac{1}{2}(\mathit{X}-\mathbf{\Xi }){\mathcal{S}}^{-1}(\mathit{X}-\mathbf{\Xi })}$$ (12)

with $\tilde{D}=D+D(D-1)/2$. This formulation is used in Section 3 to formulate a normal distribution of SPD matrices necessary to adapt the formulations of GMM and GMR to encode and retrieve manipulability ellipsoids.

2.3.6. Derivative of a matrix with respect to a vector

In the following identities, the matrix $\mathit{Y}\in {\mathbb{R}}^{I\times J}$ is a function of $\mathit{x}\in {\mathbb{R}}^K$, while $\mathit{A}\in {\mathbb{R}}^{L\times I}$ and $\mathit{B}\in {\mathbb{R}}^{J\times L}$ are constant matrices. The derivative of a matrix function $\mathit{Y}$ with respect to a vector $\mathit{x}$ is a third-order tensor $\frac{\partial \mathit{Y}}{\partial \mathit{x}}\in {\mathbb{R}}^{I\times J\times K}$ such that

$${\left(\frac{\partial \mathit{Y}}{\partial \mathit{x}}\right)}_{\mathrm{ijk}}=\frac{\partial y_{\mathrm{ij}}}{\partial x_k}$$ (13)

Note that when the matrix function $\mathit{Y}$ is multiplied by a constant matrix, the partial derivatives of $\mathit{Y}$ are given as follows.

Left multiplication by a constant matrix

$$\frac{\partial \mathit{AY}}{\partial \mathit{x}}=\frac{\partial \mathit{Y}}{\partial \mathit{x}}{\times }_1\mathit{A}$$ (14)

Right multiplication by a constant matrix

$$\frac{\partial \mathit{YB}}{\partial \mathit{x}}=\frac{\partial \mathit{Y}}{\partial \mathit{x}}{\times }_2{\mathit{B}}^{\top }$$ (15)

Finally, another useful operation for our manipulability tracking formulation is the derivative of the inverse of the matrix $\mathit{Y}$ with respect to the vector $\mathit{x}$, which results in a third-order tensor, namely

$$\frac{\partial {\mathit{Y}}^{-1}}{\partial \mathit{x}}=-{\frac{\partial \mathit{Y}}{\partial \mathit{x}}}^{\top }{\times }_1{\mathit{Y}}^{-1}{\times }_2{\mathit{Y}}^{-\top }$$ (16)

Note that the proposed geometry-aware manipulability tracking, introduced in Section 4, takes inspiration from the computation of the robot Jacobian, which is computed from the first-order time derivative of the robot forward kinematics. We use the tensor representation to similarly compute the first-order derivative of the function that describes the relationship between a manipulability ellipsoid $\mathit{M}$ and the robot joint configuration $\mathit{q}$. Mathematical proofs for (14), (15), and (16) are given in Appendix A.

3. Learning manipulability ellipsoids

The first open problem in manipulability transfer is to appropriately encode sequences of demonstrated manipulability ellipsoids and subsequently retrieve a desired manipulability profile that encapsulates the patterns observed during the demonstrations. In order to describe how we tackle this problem, we first introduce the mathematical formulation of a GMM that encodes a set of demonstrated manipulability ellipsoids over the manifold of SPD matrices. This probabilistic formulation models the trend of the demonstrated manipulability sequences along with their variability, reflecting their dispersion through the different demonstrations. After, we describe how a distribution of the desired manipulability ellipsoids can be retrieved via GMR on the SPD manifold.

3.1. GMM on SPD manifolds

Similarly to multivariate distribution (see Dubbelman, 2011; Simo-Serra et al., 2017; Zeestraten et al., 2017), we can extend the normal distribution (12) to the SPD manifold. Thus, a tensor-variate distribution maximizing the entropy in the tangent space is approximated by

$${\mathcal{N}}_{\mathcal{M}}(\mathit{X}|\mathbf{\Xi },\mathcal{S})=\frac{1}{\sqrt{{(2\pi )}^{\tilde{D}}|\mathcal{S}|}}{e}^{-\frac{1}{2}\mathrm{Lo}{\mathrm{g}}_{\mathbf{\Xi }}(\mathit{X}){\mathcal{S}}^{-1}\mathrm{Lo}{\mathrm{g}}_{\mathbf{\Xi }}(\mathit{X})}$$ (17)

where $\mathit{X}\in \mathcal{M}$, $\mathbf{\Xi }\in \mathcal{M}$ is the origin in the tangent space and $\mathcal{S}\in {\mathcal{T}}_{\mathbf{\Xi }}\mathcal{M}$ is the covariance tensor.

Similarly to the Euclidean case, a GMM on the SPD manifold is defined by

$$p(\mathit{X})=\sum _{k=1}^K{\pi }_k{\mathcal{N}}_{\mathcal{M}}(\mathit{X}|{\mathbf{\Xi }}_k,{\mathcal{S}}_k)$$ (18)

with K being the number of components of the model, and ${\pi }_k$ representing the priors such that $\sum _k{\pi }_k=1$.

The parameters of a GMM on the manifold of SPD matrices are estimated by expectation–maximization (EM) algorithm. Specifically, the responsibility of each component k is computed in the E-step as

$$p(k|{\mathit{X}}_i)=\frac{{\pi }_k{\mathcal{N}}_{\mathcal{M}}({\mathit{X}}_i|{\mathbf{\Xi }}_k,{\mathcal{S}}_k)}{\sum _{j=1}^K{\pi }_j{\mathcal{N}}_{\mathcal{M}}({\mathit{X}}_i|{\mathbf{\Xi }}_j,{\mathcal{S}}_j)}$$ (19)

$$N_k=\sum _{i=1}^{N}p(k|{\mathit{X}}_i)$$ (20)

During the M-step, the mean ${\mathbf{\Xi }}_k$ is first updated iteratively until convergence for each component. The covariance tensor ${\mathcal{S}}_k$ and prior ${\pi }_k$ are then updated using the new mean:

$${\mathbf{\Xi }}_k\leftarrow \frac{1}{N_k}\mathrm{Ex}{\mathrm{p}}_{{\mathbf{\Xi }}_k}(\sum _{i=1}^{N}p(k|{\mathit{X}}_i)\mathrm{Lo}{\mathrm{g}}_{{\mathbf{\Xi }}_k}({\mathit{X}}_i))$$ (21)

$${\mathcal{S}}_k\leftarrow \frac{1}{N_k}\sum _{i=1}^{N}p(k|{\mathit{X}}_i)\mathrm{Lo}{\mathrm{g}}_{{\mathbf{\Xi }}_k}({\mathit{X}}_i)\otimes \mathrm{Lo}{\mathrm{g}}_{{\mathbf{\Xi }}_k}({\mathit{X}}_i)$$ (22)

$${\pi }_k\leftarrow \frac{N_k}{N}$$ (23)

3.2. GMR on SPD manifolds

GMR computes the conditional distribution $p({\mathit{X}}_{\mathcal{O}\mathcal{O}}|{\mathit{X}}_{\mathcal{I}\mathcal{I}})$ of the joint distribution $p(\mathit{X})$, where the subindices $\mathcal{I}$ and $\mathcal{O}$ denote the sets of dimensions that span the input and output variables. We use the following block decomposition of the data points, means, and covariances:

$$\mathit{X}=\left(\begin{array}{cc}{\mathit{X}}_{\mathcal{I}\mathcal{I}}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& {\mathit{X}}_{\mathcal{O}\mathcal{O}}\hfill \end{array}\right),\mathbf{\Xi }=\left(\begin{array}{cc}{\mathbf{\Xi }}_{\mathcal{I}\mathcal{I}}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& {\mathbf{\Xi }}_{\mathcal{O}\mathcal{O}}\hfill \end{array}\right)$$

$$\mathcal{S}=\left(\begin{array}{cccc}{\mathcal{S}}_{\mathcal{I}\mathcal{I}}^{\mathcal{I}\mathcal{I}}\hfill & \mathit{0}\hfill & \mathit{0}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& {\mathcal{S}}_{\mathcal{I}\mathcal{I}}^{\mathcal{O}\mathcal{O}}\hfill & \mathit{0}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& \mathit{0}\hfill & {\mathcal{S}}_{\mathcal{O}\mathcal{O}}^{\mathcal{I}\mathcal{I}}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& \mathit{0}\hfill & \mathit{0}\hfill & {\mathcal{S}}_{\mathcal{O}\mathcal{O}}^{\mathcal{O}\mathcal{O}}\hfill \end{array}\right)$$ (24)

where we represent the fourth-order tensor by separating the components of the third and fourth modes with horizontal and vertical bars, respectively. With this decomposition, manifold functions can be applied individually on input and output parts, for example the exponential map would be

$$\mathrm{Ex}{\mathrm{p}}_{{\mathbf{\Xi }}_k}(\mathit{X})=\left(\begin{array}{cc}\mathrm{Ex}{\mathrm{p}}_{{\mathbf{\Xi }}_{\mathcal{I}\mathcal{I}}}({\mathit{X}}_{\mathcal{I}\mathcal{I}})\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& \mathrm{Ex}{\mathrm{p}}_{{\mathbf{\Xi }}_{\mathcal{O}\mathcal{O}}}({\mathit{X}}_{\mathcal{O}\mathcal{O}})\hfill \end{array}\right)$$

Similarly to GMR in Euclidean space (Rozo et al., 2016) and in manifolds where data are represented by vectors (Zeestraten et al., 2017), GMR on SPD manifold approximates the conditional distribution by a single Gaussian

$$p({\mathit{X}}_{\mathcal{O}\mathcal{O}}|{\mathit{X}}_{\mathcal{I}\mathcal{I}})~\mathcal{N}({\hat{\mathbf{\Xi }}}_{\mathcal{O}\mathcal{O}},{\hat{\mathcal{S}}}_{\mathcal{O}\mathcal{O}}^{\mathcal{O}\mathcal{O}})$$ (25)

where the mean ${\hat{\mathit{X}}}_{\mathcal{O}\mathcal{O}}$ is computed iteratively until convergence in its tangent space using

$${\mathbf{\Delta }}_k=\mathrm{Lo}{\mathrm{g}}_{{\hat{\mathbf{\Xi }}}_{\mathcal{O}\mathcal{O}}}({\mathbf{\Xi }}_{\mathcal{O}\mathcal{O},k})-{\tilde{\mathcal{S}}}_{\mathcal{O}\mathcal{O},k}^{\mathcal{I}\mathcal{I}}{\tilde{\mathcal{S}}}_{\mathcal{I}\mathcal{I},k}^{\mathcal{I}\mathcal{I}-1}\mathrm{Lo}{\mathrm{g}}_{{\mathit{X}}_{\mathcal{I}\mathcal{I}}}({\mathbf{\Xi }}_{\mathcal{I}\mathcal{I},k})$$ (26)

$${\hat{\mathbf{\Xi }}}_{\mathcal{O}\mathcal{O}}\leftarrow \mathrm{Ex}{\mathrm{p}}_{{\hat{\mathit{X}}}_{\mathcal{O}\mathcal{O}}}(\sum _k{h}_k{\mathbf{\Delta }}_k)$$ (27)

with $h_k$ describing the responsibilities of the GMM components in the regression, namely

$$h_k=\frac{{\pi }_k\mathcal{N}({\mathit{X}}_{\mathcal{I}\mathcal{I}}|{\mathbf{\Xi }}_{\mathcal{I}\mathcal{I},k},{\mathcal{S}}_{\mathcal{I}\mathcal{I},k}^{\mathcal{I}\mathcal{I}})}{\sum _{j=1}^K{\pi }_j\mathcal{N}({\mathit{X}}_{\mathcal{I}\mathcal{I}}|{\mathbf{\Xi }}_{\mathcal{I}\mathcal{I},j},{\mathcal{S}}_{\mathcal{I}\mathcal{I},j}^{\mathcal{I}\mathcal{I}})}$$ (28)

The covariance ${\hat{\mathcal{S}}}_{\mathcal{O}\mathcal{O}}^{\mathcal{O}\mathcal{O}}$ is then computed in the tangent space of the mean

$$\begin{array}{c}{\hat{\mathcal{S}}}_{\mathcal{O}\mathcal{O}}^{\mathcal{O}\mathcal{O}}=\sum _k{h}_k({\tilde{\mathcal{S}}}_{\mathcal{O}\mathcal{O},k}^{\mathcal{O}\mathcal{O}}-{\tilde{\mathcal{S}}}_{\mathcal{O}\mathcal{O},k}^{\mathcal{I}\mathcal{I}}{\tilde{\mathcal{S}}}_{\mathcal{I}\mathcal{I},k}^{\mathcal{I}\mathcal{I}-1}{\tilde{\mathcal{S}}}_{\mathcal{I}\mathcal{I},k}^{\mathcal{O}\mathcal{O}}+{\mathbf{\Delta }}_k\otimes {\mathbf{\Delta }}_k)\hfill \\ -{\hat{\mathbf{\Xi }}}_{\mathcal{O}\mathcal{O}}\otimes {\hat{\mathbf{\Xi }}}_{\mathcal{O}\mathcal{O}}\hfill \end{array}$$ (29)

where $\tilde{\mathcal{S}}$ is the parallel transported covariance tensor

$$\tilde{\mathcal{S}}={\mathrm{\Gamma }}_{\mathbf{\Xi }\to \hat{\mathit{X}}}(\mathcal{S})\mathrm{with}\hat{\mathit{X}}=\left(\begin{array}{cc}{\mathit{X}}_{\mathcal{I}\mathcal{I}}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& {\hat{\mathit{X}}}_{\mathcal{O}\mathcal{O}}\hfill \end{array}\right)$$ (30)

This covariance has been typically used to define the controller gains of robotic systems for trajectory tracking problems (see also Section 4.4). Note that the definition of the tangent space ${\mathcal{T}}_{\mathbf{\Xi }}\mathcal{M}$ (which has the structure of a Euclidean vector space) is what allow us to compute the conditional distribution above. Also note that to parallel transport a fourth-order covariance tensor $\mathcal{S}\in {\mathbb{R}}^{D\times D\times D\times D}$, the covariance is first converted into a second-order tensor $\mathbf{\Sigma }\in {\mathbb{R}}^{\tilde{\mathbf{D}}\times \tilde{\mathbf{D}}}$ with $\tilde{D}=D+D(D-1)/2$, as proposed in Basser and Pajevic (2007). We can then compute its eigentensors ${\mathit{V}}_k$, which are used to parallel transport the covariance matrix between tangent spaces (Freifeld et al., 2014). Let ${\tilde{\mathit{V}}}_k={\mathrm{\Gamma }}_{\mathbf{\Xi }\to \hat{\mathit{X}}}({\mathit{V}}_k)$ be the kth parallel transported eigentensor with (7) and ${\lambda }_k$ the kth eigenvalue. The parallel transported fourth-order covariance tensor is then obtained with (for more details see Jaquier and Calinon, 2017)

$${\mathrm{\Gamma }}_{\mathbf{\Xi }\to \hat{\mathit{X}}}(\mathcal{S})=\sum _k{\lambda }_k{\tilde{\mathit{V}}}_k\otimes {\tilde{\mathit{V}}}_k$$ (31)

3.3. Manipulability learning example with two planar robots

In order to illustrate the functionality of the proposed learning approach, we carried out an experiment using a couple of simulated planar robots with dissimilar embodiments and a different number of joints. The central idea is to teach a redundant robot to track a reference trajectory in Cartesian space with a desired time-varying manipulability ellipsoid. For the demonstration phase, a 3-DoF teacher robot follows a C-shape trajectory four times, from which we extracted both the end-effector position ${\mathit{x}}_t$ and robot manipulability ellipsoid ${\mathit{M}}_t(\mathit{q})$, at each time step t. The collected time-aligned data were split into two training datasets of time-driven trajectories, namely Cartesian position and manipulability. We trained a classical GMM over the time-driven Cartesian trajectories and a geometry-aware GMM over the time-driven manipulability ellipsoids, using models with five components, i.e., $K=5$ (the number was selected by the experimenter).

During the reproduction phase, a 5-DoF student robot executed the time-driven task by following a desired Cartesian trajectory ${\hat{\mathit{x}}}_t$ computed from a classical GMR as ${\hat{\mathit{x}}}_t~p(\mathit{x}|t)$. As secondary task, the robot was also required to vary its joint configuration for matching desired manipulability ellipsoids ${\hat{\mathit{M}}}_t~p(\mathit{M}|t)$, estimated by GMR over the SPD manifold.

Figure 3(a) shows the four demonstrations carried out by the 3-DoF robot, where both the Cartesian trajectory and manipulability ellipsoids are displayed. Note that the recorded manipulability ellipsoids slightly change across demonstrations as a side effect of the variation observed in both the initial end-effector position and the generated trajectory. Figure 3(b) displays the demonstrated ellipsoids (in gray) along with the center ${\mathbf{\Xi }}_k$ of the five components of the GMM encoding $\mathit{M}$. These are centered at the end-effector position recovered by the classical GMR for the corresponding time steps represented in the geometry-aware GMM. Figure 4 shows the desired Cartesian trajectory and manipulability ellipsoid profile respectively estimated by classical GMR and GMR in the SPD manifold. Both manipulability and Cartesian path are references to be tracked by the student robot.

Fig. 3.

(a) Four demonstrations of a 3-DoF planar robot tracking a C-shape trajectory. The end-effector path (light gray solid lines) and the manipulability ellipsoids at different time steps are shown for all the demonstrations. (b) Demonstrated manipulability ellipsoids (in gray) and centers ${\mathbf{\Xi }}_k$ of the 5-state GMM in the SPD manifold. Position $\mathit{x}$ and time t are given in centimeters and seconds, respectively.

Fig. 4.

(a) Desired execution of a C-shape tracking task. The desired Cartesian trajectory and manipulability profile are depicted as a black curve and green ellipsoids. (b) The top shows the desired manipulability ellipsoids estimated by GMR and the bottom shows the influence of GMM components on the time-driven GMR. The colors match the distributions shown in Figure 3(b).

These results validate that the proposed learning framework permits to learn and plan the reproduction of reference trajectories, while fulfilling additional task requirements encapsulated in a profile of desired manipulability ellipsoids. In Section 4, we develop a manipulability tracking formulation that will then be used by the 5-DoF student robot to track the desired manipulability profile obtained in the learning phase.

4. Tracking manipulability ellipsoids

Several robotic manipulation tasks may demand the robot to track a desired trajectory with certain velocity specifications, or apply forces along different task-related axes. These requirements are more easily achieved if the robot adopts a posture that suits velocity or force control commands. In other tasks, the robot may be required to adopt a posture that comply several aligned velocity or force requirements. These problems can be viewed as matching a set of desired manipulability ellipsoids that are compatible with the task requirements. In this section, we introduce an approach that addresses this problem by exploiting the mathematical concepts presented in Section 2.

4.1. Manipulability Jacobian

Given a desired profile of manipulability ellipsoids, the goal of the robot is to adapt its posture to match the desired manipulability, either as its main task or as a secondary objective. We here propose a formulation inspired by the classical inverse kinematics problem in robotics, which permits to compute the joint angle commands to track a desired manipulability ellipsoid.

First, the manipulability ellipsoid is expressed as a function of time

$$\mathit{M}(t)=f(\mathit{J}(\mathit{q}(t)))$$ (32)

for which we can compute the first-order time derivative by applying the chain rule as

$$\frac{\partial \mathit{M}(t)}{\partial t}=\frac{\partial f(\mathit{J}(\mathit{q}))}{\partial \mathit{q}}{\times }_3{\frac{\partial \mathit{q}(t)}{\partial t}}^{\top }=\mathcal{J}(\mathit{q}){\times }_3{\stackrel{·}{\mathit{q}}}^{\top }$$ (33)

where $\mathcal{J}\in {\mathbb{R}}^{6\times 6\times n}$ is the manipulability Jacobian of an n-DoF robot, representing the linear sensitivity of the changes in the robot manipulability ellipsoid $\stackrel{·}{\mathit{M}}=\frac{\partial \mathit{M}(t)}{\partial t}$ to the joint velocity $\stackrel{·}{\mathit{q}}=\frac{\partial \mathit{q}(t)}{\partial t}$. Note that the computation of the manipulability Jacobian depends on the type of manipulability ellipsoid that is used. We develop here the expressions for the force, velocity, and dynamic manipulability ellipsoids.

The derivation of the manipulability Jacobian ${\mathcal{J}}^{\stackrel{·}{\mathit{x}}}$ corresponding to the velocity manipulability ellipsoid ${\mathit{M}}^{\stackrel{·}{\mathit{x}}}=\mathit{J}{\mathit{J}}^{\top }$ is straightforward by using (14) and (15)⁴

$${\mathcal{J}}^{\stackrel{·}{\mathit{x}}}=\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\mathit{J}+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\mathit{J}$$ (34)

Similarly, the manipulability Jacobian ${\mathcal{J}}^{\mathit{F}}$ corresponding to the force manipulability ellipsoid ${\mathit{M}}^{\mathit{F}}=(\mathit{J}{\mathit{J}}^{\top }{)}^{-1}$ is obtained using (14), (15), and (16),

$${\mathcal{J}}^{\mathit{F}}=-(\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\mathit{J}+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\mathit{J}){\times }_1{\mathit{M}}^{\mathit{F}}{\times }_2{\mathit{M}}^{\mathit{F}}$$ (35)

In a similar fashion, the manipulability Jacobian ${\mathcal{J}}^{\stackrel{··}{\mathit{x}}}$ corresponding to the dynamic manipulability ellipsoid ${\mathit{M}}^{\stackrel{··}{\mathit{x}}}=\mathbf{\Upsilon }{\mathbf{\Upsilon }}^{\top }$ with $\mathbf{\Upsilon }=\mathit{J}\mathbf{\Lambda }(\mathit{q}{)}^{-1}$ (as defined in (Yoshikawa, 1985a), where $\mathbf{\Lambda }(\mathit{q})$ is the robot inertia matrix), is computed as follows

$${\mathcal{J}}^{\stackrel{··}{\mathit{x}}}=\frac{\partial \mathbf{\Upsilon }}{\partial \mathit{q}}{\times }_2\mathbf{\Upsilon }+\frac{\partial {\mathbf{\Upsilon }}^{\top }}{\partial \mathit{q}}{\times }_1\mathbf{\Upsilon }$$ (36)

where

$$\begin{array}{c}\frac{\partial \mathbf{\Upsilon }}{\partial \mathit{q}}=\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2{\mathbf{\Lambda }}^{-\top }+\frac{\partial {\mathbf{\Lambda }}^{-1}}{\partial \mathit{q}}{\times }_1\mathit{J}\hfill \\ =\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2{\mathbf{\Lambda }}^{-\top }-\frac{\partial \mathbf{\Lambda }}{\partial \mathit{q}}{\times }_1\mathbf{\Upsilon }{\times }_2{\mathbf{\Lambda }}^{-\top }\hfill \end{array}$$

Details on the computation of the derivative of the Jacobian and inertia matrix with respect to the joint angles are given in Appendices B and C.

4.2. Geometry-aware manipulability tracking formulation

4.2.1. Velocity-based controller

A solution to control a robot so that it tracks a desired end-effector trajectory is to compute the desired joint velocities using the inverse kinematics formulation derived from (1). We use here a similar approach to compute the joint velocities $\stackrel{·}{\mathit{q}}$ to track a desired manipulability profile. More specifically, by minimizing the ${\ell }^2$ -norm of the residuals

$$\underset{\stackrel{·}{\mathit{q}}}{min}\parallel \stackrel{·}{\mathit{M}}-\mathcal{J}{\times }_3{\stackrel{·}{\mathit{q}}}^{\top }\parallel =\underset{\stackrel{·}{\mathit{q}}}{min}\parallel \mathrm{vec}(\stackrel{·}{\mathit{M}})-{\mathcal{J}}_{(3)}^{\top }\stackrel{·}{\mathit{q}}\parallel$$

we can compute the required joint velocities of the robot to track a profile of desired manipulability ellipsoids as its main task with

$$\stackrel{·}{\mathit{q}}=({\mathcal{J}}_{(3)}^{†}{)}^{\top }\mathrm{vec}(\stackrel{·}{\mathit{M}})$$ (37)

where $\mathrm{vec}(\stackrel{·}{\mathit{M}})$ is the vectorization of the matrix $\stackrel{·}{\mathit{M}}$.

Note that (37) allows us to define a controller to track a reference manipulability ellipsoid as main task, similarly as the classical velocity-based control that tracks a desired task-space velocity. To do so, we propose to use a geometry-aware similarity measure to compute the joint velocities necessary to move the robot towards a posture where the match between the current manipulability ellipsoid ${\mathit{M}}_t$ and the desired one ${\hat{\mathit{M}}}_t$ is maximum. Specifically, the difference between manipulability ellipsoids is computed using the logarithmic map (6) on the SPD manifold. Therefore, the corresponding controller is given by

$${\stackrel{·}{\mathit{q}}}_t=({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\mathrm{Lo}{\mathrm{g}}_{{\mathit{M}}_t}({\hat{\mathit{M}}}_t))$$ (38)

where ${\mathit{K}}_{\mathit{M}}$ is a gain matrix.

Alternatively, for the case in which the main task of the robot is to track reference trajectories in the form of Cartesian positions or force profiles, the tracking of a profile of manipulability ellipsoids is assigned a secondary role. Thus, the robot task objectives are to track the reference trajectories while exploiting the kinematic redundancy to minimize the difference between current and desired manipulability ellipsoids. In this situation, a manipulability-based redundancy resolution is carried out by computing a nullspace velocity that similarly exploits the geometry of the SPD manifold. Thus, the corresponding controller is given by

$$\begin{array}{c}{\stackrel{·}{\mathit{q}}}_t={\mathit{J}}^{†}{\mathit{K}}_{\mathit{x}}({\hat{\mathit{x}}}_t-{\mathit{x}}_t)\hfill \\ +(\mathit{I}-{\mathit{J}}^{†}\mathit{J})({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\mathrm{Lo}{\mathrm{g}}_{{\mathit{M}}_t}({\hat{\mathit{M}}}_t))\hfill \end{array}$$ (39)

Note that matricization and vectorization operations can be defined using Mandel notation to alleviate the computational cost of the controllers using tensor representations, such that

$$\begin{array}{c}{\mathcal{X}}_{(3)}=\left(\begin{array}{c}\mathrm{vec}{\left({\mathcal{X}}_{:,:,1}\right)}^{\top }\hfill \\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{\mathrm{vec}{\left({\mathcal{X}}_{:,:,K}\right)}^{\top }}\end{array}\right)\mathrm{and}\hfill \\ \mathrm{vec}\left(\left(\begin{array}{cc}\alpha \hfill & \beta \hfill \\ \multicolumn{1}{c}{\beta }& \gamma \hfill \end{array}\right)\right)=\left(\begin{array}{c}\alpha \hfill \\ \multicolumn{1}{c}{\gamma }\\ \multicolumn{1}{c}{\sqrt{2}\beta }\end{array}\right)\hfill \end{array}$$ (40)

for $2\times 2\times K$ third-order tensors and $2\times 2$ matrices.

In order to show the functionality of the proposed approach where the goal of the robot is to reproduce a given manipulability ellipsoid either as its main task or as a secondary objective, we carried out experiments with a simulated 4-DoF planar robot. In the first case, the robot is required to vary its joint configuration to make its manipulability ellipsoid ${\mathit{M}}_t$ coincide with the desired one $\hat{\mathit{M}}$, without any task requirement at the level of its end-effector. In the second case, the robot needs to keep its end-effector at a fixed Cartesian position while moving its joints to match the desired manipulability ellipsoid. Figure 5 shows how the manipulator configuration is successfully adjusted so that ${\mathit{M}}_t\approx \hat{\mathit{M}}$ when the manipulability ellipsoid tracking is considered as the main task or as a secondary objective (see Table 1). These results show that our geometry-aware controllers inspired by the inverse kinematics formulation are suitable to solve the manipulability ellipsoid tracking problem.

Fig. 5.

(a) Manipulability tracking as the main task. (b) Manipulability-based redundancy resolution with Cartesian position control. The robot color goes from light gray to black to show the evolution of the posture. Initial, final, and desired manipulability ellipsoids are depicted in yellow, dark purple, and green, respectively. The top rows show close-up plots corresponding to the initial and final manipulability.

Table 1.

Initial and final distances $d(\hat{\mathit{M}},{\mathit{M}}_t)$ between the current and desired manipulability for the experiments illustrated in Figure 5

	Initial	Final
Main task	$1.342$	$0.199$
Redundancy resolution	$2.194$	$0.955$

4.2.2. Stability analysis

We here analyze the stability properties of the proposed manipulability tracking controller given the geometry of the underlying manifold. First, note that the dynamical system operated by the controller (38) corresponds to

$$\stackrel{·}{\mathit{M}}=k_{\mathit{M}}\mathrm{Lo}{\mathrm{g}}_{\mathit{M}}(\hat{\mathit{M}})$$ (41)

where the controller gain is assumed to be a positive scalar value for sake of simplicity. Then, we select the Lyapunov function V as

$$V(\mathit{M})=〈\mathit{F},\mathit{F}{〉}_{\hat{\mathit{M}}}$$ (42)

where $\mathit{F}=\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M})$ is a vector field composed of the initial velocities of all geodesics departing from the origin $\hat{\mathit{M}}$, and $〈·,·{〉}_{\hat{\mathit{M}}}$ is the inner product (3). As proved by Pait and Colon (2010), the function (42) is a Lyapunov function for a dynamical system $\stackrel{·}{\mathit{M}}=h(\mathit{M})$ such that $h(\hat{\mathit{M}})=0$ if the Lie derivative ${\mathcal{L}}_{h}V(\mathit{M})=2〈h,\mathit{F}{〉}_{\hat{\mathit{M}}}$ is negative everywhere except at the origin $\hat{\mathit{M}}$. To verify this condition, we first express the velocity of the dynamical system (41) in the tangent space of $\hat{\mathit{M}}$ using parallel transport as

$${\mathrm{\Gamma }}_{\mathit{M}\to \hat{\mathit{M}}}(\stackrel{·}{\mathit{M}})=-k_{\hat{\mathit{M}}}\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M})$$ (43)

The Lie derivative ${\mathcal{L}}_{h}V$ of the proposed Lyapunov function for the dynamical system (43) is given by

$$\begin{array}{c}\multicolumn{1}{c}{{\mathcal{L}}_{h}V(\mathit{M})=2〈-k_{\hat{\mathit{M}}}\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M}),\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M}){〉}_{\hat{\mathit{M}}}}\\ =-2k_{\hat{\mathit{M}}}〈\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M}),\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M}){〉}_{\hat{\mathit{M}}}\hfill \\ =-2k_{\hat{\mathit{M}}}V\hfill \end{array}$$ (44)

Therefore, we have

$$V(\mathit{M})>0,{\mathcal{L}}_{h}V(\mathit{M})<0\forall \mathit{M}\ne \hat{\mathit{M}}$$

$$V(\mathit{M})={\mathcal{L}}_{h}V(\mathit{M})=0\Leftarrow \mathit{M}=\hat{\mathit{M}}$$

so that the function (42) is a valid Lyapunov function. Moreover, by observing that $V(\mathit{M})=d^2(\mathit{M},\hat{\mathit{M}})$ with $d(·,·)$ the affine-invariant distance (4), we have

$$c_1{d}^2(\mathit{M},\hat{\mathit{M}})\le V(\mathit{M})\le c_2{d}^2(\mathit{M},\hat{\mathit{M}})$$

$${\mathcal{L}}_{h}V(\mathit{M})\le -c_3{d}^2(\mathit{M},\hat{\mathit{M}})$$

where $0<c_1\le 1$, $c_2\ge 1$, and $c_3=2k_{\hat{\mathit{M}}}>0$. This implies that the controller (38) is exponentially stable (see, e.g.,Wu, 2020). It can be easily shown that this result holds with $c_3=2{\lambda }_{\min }({\mathit{K}}_{\hat{\mathit{M}}})$ for a positive-definite controller gain matrix ${\mathit{K}}_{\hat{\mathit{M}}}$, where ${\lambda }_{\min }(·)$ returns the minimum eigenvalue of the matrix.

Note that the Lyapunov function (42) is similar to that usually defined to demonstrate the exponential stability of the classical inverse kinematic-based velocity controller ${\stackrel{·}{\mathit{q}}}_t={\mathit{J}}^{†}{\mathit{K}}_{\mathit{x}}({\hat{\mathit{x}}}_t-{\mathit{x}}_t)$. In that case, the Lyapunov function is defined as $V(\mathit{x})=(\hat{\mathit{x}}-\mathit{x}{)}^{\top }(\hat{\mathit{x}}-\mathit{x})$, which is equivalent to the inner product $〈\mathit{e},\mathit{e}〉$ with the error $\mathit{e}=\hat{\mathit{x}}-\mathit{x}$. In the case of manipulability tracking, the inner product 〈·,·〉 is defined in the SPD manifold and the error $\mathit{e}$ is computed as $\mathrm{Lo}{\mathrm{g}}_{\hat{\mathit{M}}}(\mathit{M})$. Finally, it is worth highlighting that when the manipulability tracking is assigned a secondary role, the controller (39) does not influence the stability of the main task of the robot as the manipulability-based redundancy resolution is carried out in the corresponding nullspace.

4.2.3. Acceleration-based controller

Similarly to the velocity-based controller, we propose a geometry-aware acceleration-based controller that allows the computation of the joint accelerations $\stackrel{··}{\mathit{q}}$ required to track a desired manipulability trajectory (i.e., desired manipulability and manipulability velocity profiles). The approach is inspired by the inverse kinematics formulation and its differential relationships used to compute the joint accelerations necessary to track desired end-effector positions and velocities.

To formalize the acceleration-based controller, let us first define the second-order time derivative of the manipulability ellipsoid computed from (33) by applying the product rule

$$\frac{{\partial }^2\mathit{M}(t)}{\partial t^2}=\mathcal{J}(\mathit{q}){\times }_3{\stackrel{··}{\mathit{q}}}^{\top }+\stackrel{·}{\mathcal{J}}(\mathit{q}){\times }_3{\stackrel{·}{\mathit{q}}}^{\top }$$ (45)

(see Appendix D for details on the computation of $\stackrel{·}{\mathcal{J}}(\mathit{q})$). Thus, by minimizing the ${\ell }^2$-norm of the residuals, we can compute the required joint accelerations of the robot to track a desired trajectory of manipulability ellipsoids as its main task with

$$\stackrel{··}{\mathit{q}}=({\mathcal{J}}_{(3)}^{†}{)}^{\top }(\mathrm{vec}(\stackrel{··}{\mathit{M}})-{\stackrel{·}{\mathcal{J}}}_{(3)}^{\top }\stackrel{·}{\mathit{q}})$$ (46)

Similarly as in the classical acceleration-based controller that tracks a desired end-effector trajectory, we can define a controller to track a reference manipulability ellipsoid trajectory based on (46). To do so, we exploit the geometry of the SPD manifold to compute the difference between the current manipulability ellipsoid ${\mathit{M}}_t$ and the desired one ${\hat{\mathit{M}}}_t$, as previously specified for the velocity-based controller. Moreover, because the first-order time derivative of manipulability ellipsoids lies on the tangent space of the SPD manifold (i.e., the space of symmetric matrices $\mathrm{Sy}{\mathrm{m}}^D$), the difference between the current manipulability velocity ${\stackrel{·}{\mathit{M}}}_t$ and the desired one ${\widehat{\stackrel{·}{\mathit{M}}}}_t$ is computed as a subtraction in the Euclidean space. Therefore, a reference manipulability acceleration command can be specified by

$$\mathrm{vec}({\stackrel{··}{\mathit{M}}}_t)={\mathit{K}}_p\mathrm{vec}(\mathrm{Lo}{\mathrm{g}}_{{\mathit{M}}_t}({\hat{\mathit{M}}}_t))+{\mathit{K}}_d\mathrm{vec}(\hat{{\stackrel{·}{\mathit{M}}}_t}-{\stackrel{·}{\mathit{M}}}_t)$$ (47)

which resembles a proportional–derivative controller where ${\mathit{K}}_p$ and ${\mathit{K}}_d$ are gain matrices. Then, the reference joint acceleration $\stackrel{··}{\mathit{q}}$ can be computed using (46) and (47). Note that this reference joint acceleration can correspond to a main task of the robot or to a secondary tracking objective. In the latter case, a manipulability-based redundancy resolution can also be implemented in a similar way as (39).

4.3. Actuators contribution

In many practical applications, the joint velocities of the robot are limited. The definition of manipulability ellipsoid can then be extended to include these actuation constraints, as shown in Lee (1997). We here provide the definition of the force, velocity, and dynamic manipulability ellipsoids and the corresponding manipulability Jacobians considering joint actuation constraints.

To include the joint velocity constraints of the robot in the definition of the velocity manipulability ellipsoid, we use the following weighted forward kinematics formulation

$$\stackrel{·}{\mathit{x}}=\underset{\tilde{\mathit{J}}}{\underbrace{(\mathit{J}{\mathit{W}}^{\stackrel{·}{\mathit{q}}})}}\underset{\widetilde{\stackrel{·}{\mathit{q}}}}{\underbrace{({\mathit{W}}^{\stackrel{·}{\mathit{q}}-1}\stackrel{·}{\mathit{q}})}}$$ (48)

where ${\mathit{W}}^{\stackrel{·}{\mathit{q}}}=\mathrm{diag}({\stackrel{·}{q}}_{1,\max },\dots ,{\stackrel{·}{q}}_{n,\max })$ is a diagonal matrix whose elements correspond to the maximum joint velocities of the robot. Then, considering the set of joint velocities of constant unit norm $\parallel \widetilde{\stackrel{·}{\mathit{q}}}\parallel =1$ mapped into the Cartesian velocity space through

$$\parallel \widetilde{\stackrel{·}{\mathit{q}}}{\parallel }^2={\widetilde{\stackrel{·}{\mathit{q}}}}^{\top }\widetilde{\stackrel{·}{\mathit{q}}}={\stackrel{·}{\mathit{x}}}^{\top }(\tilde{\mathit{J}}{\tilde{\mathit{J}}}^{\top }{)}^{-1}\stackrel{·}{\mathit{x}}$$ (49)

the velocity manipulability ellipsoid is given by ${\tilde{\mathit{M}}}^{\stackrel{·}{\mathit{x}}}=\tilde{\mathit{J}}{\tilde{\mathit{J}}}^{\top }=\mathit{J}{\mathit{W}}^{\stackrel{·}{\mathit{q}}}{\mathit{W}}^{\stackrel{·}{\mathit{q}}\top }{\mathit{J}}^{\top }$, which represents the flexibility of the manipulator in generating velocities in Cartesian space considering its maximum joint velocities as illustrated in Figure 6(a). Note that the actuators contribution ${\mathit{W}}^{\stackrel{·}{\mathit{q}}}{\mathit{W}}^{\stackrel{·}{\mathit{q}}\top }$ also has a geometrical interpretation based on the fact that the robot joint position $\mathit{q}$ lies on the flat n-torus manifold (Park, 1995).

Fig. 6.

Illustration of the contribution of actuators. (a) Velocity manipulability ellipsoids obtained when setting a maximum joint velocity, for each joint, five times higher than the rest. The manipulability corresponding to equal maximum joint velocity is shown in gray. (b) Joint trajectories obtained with manipulability tracking (as in Figure 5(a)) for equal maximum joint velocities (top), highest velocity limit for $q_1$ (middle), and lowest velocity limit for $q_1$ (bottom).

By following the methodology of Section 4.1, the change in the robot manipulability ellipsoid is related to the joint velocity via

$$\frac{\partial \tilde{\mathit{M}}(t)}{\partial t}=\tilde{\mathcal{J}}(\mathit{q}){\times }_3{\stackrel{·}{\mathit{q}}}^{\top }$$ (50)

Therefore, the velocity manipulability Jacobian including joint velocity limits is given by

$${\tilde{\mathcal{J}}}^{\stackrel{·}{\mathit{x}}}=\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\mathit{J}{\mathit{W}}^{\stackrel{·}{\mathit{q}}}{\mathit{W}}^{\stackrel{·}{\mathit{q}}\top }+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\mathit{J}{\mathit{W}}^{\stackrel{·}{\mathit{q}}}{\mathit{W}}^{\stackrel{·}{\mathit{q}}\top }$$ (51)

Figure 6(b) shows the effect of including the actuator contribution when tracking a velocity manipulability ellipsoid. Note that the robot joint $q_1$ moves significantly when given the highest velocity limit. In contrast, its influence on the manipulability tracking task is minimal when given the lowest velocity limit. This demonstrates the importance of considering the robot actuator specifications when tracking manipulability ellipsoids in real platforms.

In a similar way, the force manipulability ellipsoid considering the maximum joint torques is defined as ${\tilde{\mathit{M}}}^{\mathit{F}}=(\mathit{J}\mathbf{\Omega }\mathbf{\tau }{\mathit{J}}^{\top }{)}^{-1}$, where $\mathbf{\Omega }\mathbf{\tau }=({\mathit{W}}^{\top }{\mathit{W}}^{\mathbf{\tau }\top }{)}^{-1}$ and ${\mathit{W}}^{\mathbf{\tau }}=\mathrm{diag}({\tau }_{1,\max },\dots ,{\tau }_{n,\max })$. Then, the corresponding manipulability Jacobian is given by

$${\tilde{\mathcal{J}}}^{\mathit{F}}=-(\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\mathit{J}\mathbf{\Omega }\mathbf{\tau }+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\mathit{J}\mathbf{\Omega }\mathbf{\tau }){\times }_1{\tilde{\mathit{M}}}^{\mathit{F}}{\times }_2{\tilde{\mathit{M}}}^{\mathit{F}}$$

Finally, the dynamic manipulability ellipsoid considering the maximum joint torques is ${\tilde{\mathit{M}}}^{\stackrel{··}{\mathit{x}}}=\Upsilon {\mathbf{\Omega }\mathbf{\tau }}^{-1}{\Upsilon }^{\top }$ with corresponding manipulability Jacobian defined as

$${\tilde{\mathcal{J}}}^{\stackrel{··}{\mathit{x}}}=\frac{\partial \mathbf{\Upsilon }}{\partial \mathit{q}}{\times }_2\mathbf{\Upsilon }{{\mathbf{\Omega }}^{\mathbf{\tau }}}^{-1}+\frac{\partial {\mathbf{\Upsilon }}^{\top }}{\partial \mathit{q}}{\times }_1\mathbf{\Upsilon }{\mathbf{\Omega }\mathbf{\tau }}^{-1}$$ (52)

4.4. Exploiting fourth-order precision matrix as controller gain

An open problem regarding the proposed tracking approach is how to specify the values of the gain matrix ${\mathit{K}}_{\mathit{M}}$, which basically determines how the manipulability tracking error affects the resulting joint velocities. In this sense, we propose to define ${\mathit{K}}_{\mathit{M}}$ as a precision matrix, which describes how accurately the robot should track a desired manipulability ellipsoid. In learning from demonstration applications, such gain matrix would typically be set as proportional to the inverse of the observed covariance $\mathcal{S}$ (see Section 3.2). This encapsulates variability information of the task to be learned. Our goal here is to exploit this information to demand the robot a high precision tracking for directions in which low variability is observed, and vice versa.

We therefore introduce the required precision ${\mathcal{S}}^{-1}$ for a given manipulability tracking task into the controllers defined in Section 4.2. To do so, we define the gain matrix ${\mathit{K}}_{\mathit{M}}$ as a function of the precision tensor. Specifically, we define the controller gain matrix as a full SPD matrix, which is computed from the matricization of the precision tensor ${\mathcal{S}}^{-1}$ along its two first dimensions, with a proportion defined by

$${\mathit{K}}_{\mathit{M}}\propto {\mathcal{S}}_{(1,2)}^{-1}$$ (53)

To show how precision matrices work as controller gains in our manipulability tracking problem, we tested different forms of ${\mathit{K}}_{\mathit{M}}$ aimed at reproducing a given manipulability ellipsoid as a main task with a simulated 4-DoF planar robot. The robot is required to move its joints to track a desired manipulability ellipsoid, where the controller gain matrix ${\mathit{K}}_{\mathit{M}}$ is a diagonal matrix with the diagonal elements of (53) to take into account the variation of each component of the manipulability ellipsoid. We tested four different precision tensors. First, equal variability for all components of the manipulability ellipsoid matrix is given. Then, the variability along the first or the second main axis of the manipulability ellipsoid, corresponding to the first and second diagonal elements of the gain matrix ${\mathit{K}}_{\mathit{M}}$, is reduced. This means that the robot needs to prioritize the tracking of one of the ellipsoid main axes over the other. In the fourth test, the variability of the correlation between the two main axes of the manipulability ellipsoid is lowered. In this last case, the manipulability controller prioritizes the tracking of the ellipsoid orientation over the shape.

Figure 7 shows how the manipulator posture is adapted to track the desired manipulability ellipsoid with a priority on the component with the lowest variability. Note that when high tracking precision is required for one of the main axes of the ellipsoid, the robot initially seeks to fit the shape of the ellipsoid along that specific axis, and subsequently matches the whole manipulability ellipsoid. In this case, the precision ratio between the prioritized and the rest of components of the gain matrix is 10:1. When high tracking precision is assigned to the correlation of the ellipsoid axes, the robot first tries to align its manipulability with the orientation of the desired ellipsoid, and afterwards the whole manipulability is matched. In this case, the precision ratio between the prioritized correlation and the other components of the gain matrix is 3:1. Note that the precision tensor naturally affects the computed joint velocities required to track a given ellipsoid, which consequently influences the resulting motion of the end-effector as a function of the precision constraints, as shown in Figure 7(e). After convergence, the desired manipulability ellipsoid is successfully matched for all experiments. These results show that our geometry-aware tracking permits to take into account the variability information of a task to define the manipulability tracking precision.

Fig. 7.

Manipulability tracking as the main task with diagonal gain matrices defined from different precision tensors. The top plots depict the end-effector trajectory (solid colored line) and the posture of the robot along with the corresponding manipulability at time $t=0$, $0.25$, and 1 s. The evolution of the manipulability along time is shown in the bottom plots. (a) Equal tracking precision for all components. (b), (c) Tracking precision is 10:1 higher for $x_1$ and $x_2$, respectively. (d) Correlation between $x_1$ and $x_2$ axes is assigned a high tracking accuracy. (e) Evolution of the robot manipulability and end-effector trajectory for the gain matrices used in (a)–(d). The colors match those of the previous graphs. Initial and desired manipulability ellipsoids are depicted in dark blue and green on all graphs. Time t is in seconds.

Therefore, our manipulability tracking approach may be readily combined with the manipulability learning framework introduced in Section 3. In order to illustrate this, we show the reproduction phase of the experiment carried out in Section 3.2. The 5-DoF student robot was requested to track a desired Cartesian trajectory as main task, while varying its joint configuration for matching desired manipulability ellipsoids as secondary task. The student robot used the geometry-aware controller defined by (39), where ${\mathit{K}}_{\mathit{M}}$ was defined either as a scalar value or as a diagonal matrix with the diagonal elements of (53) with the precision tensor being equal to the inverse of the covariance tensor ${\hat{\mathcal{S}}}_{\mathcal{O}\mathcal{O}}^{\mathcal{O}\mathcal{O}}$ retrieved by GMR (29). Our goal here was to exploit the learned variability information of the task to demand the robot a high precision tracking where low variability was observed in the demonstrations, and vice versa. Successful reproductions of the demonstrated task using our manipulability-based redundancy resolution controller with scalar and variability-based matrix gains are shown in Figures 8(a) and (b), respectively. Note that the variability-based matrix gain changes the required tracking precision, where higher precision is enforced only at the beginning and the end of the task, which results in lower control efforts in between. These results validate that the proposed approach allows the robot to reproduce reference profiles of desired manipulability ellipsoids while adapting the tracking precision according to the demonstrated requirements of the task.

Fig. 8.

Reproductions of a learned C-shape tracking task with desired manipulability ellipsoids. The end-effector trajectory is shown in black solid line, while the desired and reproduced manipulabilities are depicted in green and dark purple, respectively: (a) ${\mathit{K}}_{\mathit{M}}$ is a scalar value; (b) ${\mathit{K}}_{\mathit{M}}$ is the diagonal of the precision tensor retrieved by GMR. The required tracking precision is higher at the start and end of the task as a consequence of the low observed variability.

4.5. Nullspace of the manipulability Jacobian

As traditionally done when designing redundancy resolution controllers, the nullspace of the manipulability Jacobian can also be exploited to fulfill secondary objectives when manipulability tracking is the main task. More specifically, a joint velocity ${\stackrel{·}{\mathit{q}}}_N$, aimed at fulfilling secondary objectives, can be projected into the nullspace of our manipulability tracking controller (38) using the nullspace operator $(\mathit{I}-({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathcal{J}}_{(3)}^{\top })$. Therefore, the resulting redundancy resolution controller is given by

$$\begin{array}{c}{\stackrel{·}{\mathit{q}}}_t=({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\mathrm{Lo}{\mathrm{g}}_{{\mathit{M}}_t}({\hat{\mathit{M}}}_t))\hfill \\ +(\mathit{I}-({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathcal{J}}_{(3)}^{\top }){\stackrel{·}{\mathit{q}}}_N\hfill \end{array}$$ (54)

In order to show the functionality of this nullspace operator, we carried out experiments with a simulated 6-DoF planar robot. The main task of the robot is to track a desired manipulability ellipsoid while keeping a desired pose for its first joint $q_0$, which is considered as secondary task. Thus, the nullspace velocity is defined as a simple proportional controller ${\stackrel{·}{\mathit{q}}}_N={\mathit{K}}_{\mathit{q}}^{\mathcal{P}}(\hat{\mathit{q}}-{\mathit{q}}_t)$ where $\hat{\mathit{q}}$ is the desired joint configuration and ${\mathit{K}}_{\mathit{q}}^{\mathcal{P}}$ is a matrix gain defined so that only joint position errors in the first joint are compensated. Figure 9 shows that the black manipulator configuration is adjusted to track the desired manipulability ellipsoid and keep, as accurately as possible, the desired joint position for $q_0$. Note that the black robot is able to find an alternative joint configuration that permits not only to closely track the desired manipulability, but also fulfill secondary objectives projected into its nullspace, in contrast to the blue robot which exclusively implements a manipulability tracking task. These results show that the nullspace of the manipulability Jacobian is suitable to carry out a secondary task along with manipulability tracking as main objective.

Fig. 9.

Use of the nullspace of the manipulability Jacobian. Two 6-DoF planar robots are required to track a desired manipulability ellipsoid as main task. The black robot also keeps its first joint at a fixed position (depicted by the green link), which is a secondary objective projected into the nullspace of the manipulability Jacobian. The final manipulability ellipsoids (in purple) fully overlap the desired ones (in green), showing a precise manipulability tracking. The initial manipulability ellipsoid is depicted in yellow.

5. Importance of geometry-awareness

In the previous sections we introduced a geometry-aware manipulability transfer framework composed of (1) a probabilistic model that encodes and retrieves manipulability ellipsoids, and (2) manipulability tracking controllers. In this section, we show that the geometry-awareness of our formulations is crucial for successfully learning and tracking manipulability ellipsoids in addition to providing an appropriate mathematical treatment of both problems.

5.1. Learning

We first evaluate the proposed learning formulation compared with a framework that ignores that manipulability ellipsoids belong to the SPD manifold. To do so, we encode a distribution of manipulability ellipsoids with a GMM acting in the Euclidean space and we then retrieve desired manipulability ellipsoids via the corresponding GMR. To ensure the validity of the desired manipulability ellipsoids, GMM and GMR are performed on lower triangular matrices $\mathit{L}$ obtained via Cholesky decomposition. Thus, the positive-definiteness of the desired manipulability ellipsoids computed as $\hat{\mathit{M}}=\hat{\mathit{L}}{\hat{\mathit{L}}}^{\top }$ is guaranteed, where $\hat{\mathit{L}}$ is the estimated GMR output. Note that this property is not guarantee in the case where GMM and GMR acting in the Euclidean space is applied directly to the manipulability ellipsoids $\mathit{M}$. Therefore, we do not consider this approach in the comparison as the desired matrices $\hat{\mathit{M}}$ may not be manipulability ellipsoids in some cases.

Figure 10 compares the proposed approach (Section 3) and the manipulability learning using GMM/GMR acting in Euclidean space. The demonstration consists of a time series of changing manipulability ellipsoids. For each approach, a 1-state GMM is trained and a reproduction is carried out for a longer time period than the demonstration using GMR. Both geometry-aware and Euclidean approaches obtain similar means of the GMM component (see Figure 10(a) and (b)). This is due to the fact that the Euclidean mean computed using the Cholesky decomposition is a good approximation of the mean computed on ${\mathcal{S}}_{++}^D$ if the SPD data are close enough to each other. However, the covariances of the GMM components of both approaches are not equivalent. Indeed, the covariance of our geometry-aware approach is computed using the SPD data projected in the tangent space of the mean, while that of the Euclidean GMM corresponds to the covariance of the elements of the vectorized Cholesky decomposition, which ignores the geometry of the SPD manifold.

Fig. 10.

Importance of geometry in manipulability learning formulations. (a) Demonstrated data (depicted in light gray), and mean of the GMM component for the geometry-aware and Euclidean approaches (overlapping blue and red ellipsoids, respectively). (b) Mean of the GMM component and estimated profiles in the cone of SPD matrices. The manipulability profile obtained by our approach, shown in green, follows a geodesic. The profile obtained by the Euclidean framework is depicted by the orange curve and does not follow a geodesic on the manifold. The geodesic containing the mean of the Euclidean GMM, being a geometrically valid trajectory (depicted in purple), does not correspond to the trajectory obtained with the Euclidean framework. Thus, the Euclidean approach is geometrically flawed. (c) Manipulability profiles retrieved by the geometry-aware and Euclidean GMR, shown as green and orange ellipses, respectively. The time axis is shared with (a).

The manipulability ellipsoids profiles retrieved by the geometry-aware and Euclidean GMR are similar around the mean of the GMM component, but diverge when moving away from it (see Figure 10(c)). This is because the estimated output in Euclidean space is only a valid approximation for input data lying close to the mean. In contrast, our approach is able to extrapolate the rotating behavior of the demonstrated manipulability ellipsoids as the recovered trajectory follows a geodesic on the SPD manifold (see Figure 10(b)). Note that this is the equivalent to following a straight line in Euclidean space, which is the expected result of a trajectory computed via Gaussian conditioning. This behavior is obtained by parallel transporting the GMM covariances to the tangent space of the mean of the estimated conditional distribution of GMR (30). Therefore, the Euclidean GMR does not recover a trajectory following a geodesic on the manifold, leading to inconsistent extrapolated manipulability ellipsoids.

The reported results show that our geometry-aware approach accurately reproduces the behavior of the demonstrated data, and therefore provides a mathematically sound method for learning and retrieving manipulability ellipsoids in the SPD manifold. Note that similar behaviors are observed for GMM with any number of states, the number $K=1$ was chosen here to facilitate the visualization of the results.

5.2. Tracking

5.2.1. Comparisons with Euclidean tracking

After showing the importance of geometry for learning manipulability ellipsoids, we compare the proposed tracking formulation against a controller ignoring the geometry of SPD matrices (i.e., treating the problem as Euclidean). Moreover, we evaluate our controller when the tracking of manipulability ellipsoids is assigned a secondary role. This evaluation compares our formulation against three Euclidean controllers, and the gradient-based approach in Rozo et al., (2017). For the case in which the manipulability tracking is the main objective, we consider a 4-DoF planar robot that is required to track a desired manipulability ellipsoid by minimizing the error between its current and desired manipulability ellipsoids $\mathit{M}$ and $\hat{\mathit{M}}$. We first compare the proposed approach (38) with the following Euclidean manipulability tracking controller

$${\stackrel{·}{\mathit{q}}}_t=({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}({\hat{\mathit{M}}}_t-{\mathit{M}}_t)$$ (55)

where the difference between two manipulability ellipsoids is computed in Euclidean space, i.e., ignoring that manipulability ellipsoids belong to the set of SPD matrices. Second, we compare the proposed approach to the Cholesky-based Euclidean manipulability controller

$${\stackrel{·}{\mathit{q}}}_t=({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\mathbf{\Delta }{\mathit{L}}_{\mathbf{t}}\mathbf{\Delta }{\mathit{L}}_{\mathbf{t}}^{\top })$$ (56)

where $\mathbf{\Delta }\mathit{L}=\hat{\mathit{L}}-\mathit{L}$ and matrices $\mathit{L}$ are obtained from the Cholesky decomposition such that $\mathit{M}=\mathit{L}{\mathit{L}}^{\top }$. This controller ensures that the difference between two manipulability ellipsoids is positive definite, but ignores that they belong to the SPD manifold. For completeness, we also compare our approach with the Cholesky-Jacobian-based Euclidean manipulability controller

$${\stackrel{·}{\mathit{q}}}_t=({\mathcal{J}}_{\mathrm{chol}(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\hat{\mathit{L}}-\mathit{L})$$ (57)

where ${\mathcal{J}}_{\mathrm{chol}}=\frac{\delta \mathit{L}}{\delta \mathit{q}}=\frac{\delta \mathit{L}}{\delta \mathit{M}}\mathcal{J}$ is the Cholesky-based manipulability Jacobian, so that $\stackrel{·}{\mathit{L}}={\mathcal{J}}_{\mathrm{chol}}{\times }_3{\stackrel{·}{\mathit{q}}}^{\top }$. This approach tracks a desired manipulability solely through its Cholesky decomposition with an adapted manipulability Jacobian. Similarly to (56), it ensures the positive definiteness of manipulability ellipsoids, but ignores that they belong to the SPD manifold. For all the following comparisons, the gain matrices ${\mathit{K}}_{\mathit{M}}$ are identity matrices.

Figure 11 shows the convergence rate for the proposed geometry-aware controller, the Euclidean-based approach, the Cholesky-based Euclidean and Cholesky–Jacobian-based Euclidean formulations. Two tests were carried out by varying the initial configuration of the robot and the desired manipulability ellipsoid. In the first case, the Euclidean and geometry-aware formulations converge to similar robot joint configurations with a distance between the current and desired manipulability close to zero (see Figure 11(a), left and middle, and Table 2). However, in the second test, the Euclidean formulation induces a sudden change in the joint configuration, resulting in an abrupt increase on the error measured between the current and desired manipulability ellipsoids (see Figure 11(b), left and middle). In real scenarios, such unstable robot behavior would certainly be harmful and unsafe. This erroneous tracking performance can be explained by the fact that the Euclidean path between two SPD matrices is a valid approximation of the geodesic only if these are close enough to each other, as shown in Figure 11(a)(right). When this approximation is not valid (see Figure 11(b), right), the Euclidean controller outputs inconsistent reference joint velocities that destabilize the robotic system, therefore failing to track the desired manipulability. Note that the Cholesky-based Euclidean formulation does not converge in both cases (see Table 2) and induces a sudden change in joint configuration of the robot in the second scenario, similarly to the Euclidean formulation. This can be explained by the fact that the path induced by this method is not close to geodesics on the SPD manifold as shown by Figure 11(right). As opposed to the two Euclidean formulations, the Cholesky–Jacobian-based Euclidean controller does not induce unstable robot behaviors and converges towards the desired manipulability ellipsoid for both cases. However, this method shows a poor convergence rate compared to our geometry-aware approach, as shown by Figure 11(left). This can be explained by the fact that, although this approach generates curved paths on the SPD manifold, these paths do not resemble geodesics and tend to induce detours to reach the desired manipulability ellipsoid (see Figure 11, left). This is particularly visible for the second test, where the resulting joint configuration is farther from the initial pose of the robot compared with the joint configuration obtained by the proposed geometry-aware controller (see Figure 11(b), middle and right).

Fig. 11.

Performance of different manipulability tracking formulations. The graphs on the left show the affine-invariant distance between the current and desired manipulability ellipsoids over time. The distances for the Euclidean, Cholesky-based Euclidean, Cholesky–Jacobian-based Euclidean and geometry-aware approaches are depicted in blue, yellow, lilac, and red, respectively. The graphs in the middle display the initial and final robot postures and the final manipulability ellipsoids. The initial posture is depicted in light gray, while the final posture and corresponding manipulability for the three methods are depicted in the same color as the distances. The desired manipulability is depicted in green. The middle graph in (b) also shows the sudden change in the robot posture for both Euclidean methods (55) and (56). The robot posture before and after the abrupt change is shown in blue and light blue, respectively, for (55) and in yellow and olive, respectively, for (56). The graphs on the right depict the evolution of the manipulability ellipsoids in the SPD manifold. The colors correspond to those of the previous graphs with the green dot representing the desired manipulability. The isolated light blue and olive dots in the bottom graph in (b) represent the manipulability ellipsoids after the abrupt changes in the robot joint configuration.

Table 2.

Final distances $d(\hat{\mathit{M}},{\mathit{M}}_t)$ between the current and desired manipulability ellipsoids for the performance comparison of the different manipulability tracking formulations

Approach	Euclidean	(after jump)	Cholesky	(after jump)	Cholesky–Jacobian	Geometry-aware
Figure 11(a)	$1.3\times {10}^{-4}$	—	$1.446$	—	$0.1204$	$6\times {10}^{-5}$
Figure 11(b)	$2.997$	$3.977$	$3.385$	$1.944$	$0.455$	$1.4\times {10}^{-4}$

Previously, we hypothesized that the sudden changes in joint configuration when using the Euclidean and Cholesky-based Euclidean formulations in the second scenario are due to the path induced by the methods on the SPD manifold. In order to confirm this hypothesis, we reproduced the second test with lower gain values. Figure 12 shows the convergence of the proposed geometry-aware controller, the Euclidean-based approach and the Cholesky-based Euclidean formulation for gain matrices equal to $\mathit{I}$, $0.5\mathit{I}$, $0.1\mathit{I}$, and $0.05\mathit{I}$. We observe that, even for very low gains, both Euclidean and Cholesky-based Euclidean formulations lead to a sudden change in the joint configuration, resulting in an abrupt increase on the error measured between the current and desired manipulability ellipsoids (see Figure 12, top and middle). Interestingly, the sudden changes occur at similar location along the path between the initial and desired manipulability ellipsoid independently of the gain value for both formulations (see Figure 12, bottom), therefore confirming our above statement. This can also be seen by looking at the yellow and dark blue robots of Figure 12(middle) depicting the configurations before the jump, which are almost identical in all the graphs.

Fig. 12.

Comparison of the performance of different manipulability tracking formulations for different gains ${\mathit{K}}_{\mathit{M}}$: (a) ${\mathit{K}}_{\mathit{M}}=\mathit{I}$, (b) ${\mathit{K}}_{\mathit{M}}=0.5\mathit{I}$, (c) ${\mathit{K}}_{\mathit{M}}=0.1\mathit{I}$, and (d) ${\mathit{K}}_{\mathit{M}}=0.05\mathit{I}$. The organization of the graphs and the colors are identical to Figure 11. The Cholesky–Jacobian-based Euclidean formulation is not shown.

In the case in which the manipulability tracking task becomes a secondary objective, the 4-DoF planar robot is required to keep its end-effector at a fixed Cartesian position $\hat{\mathit{x}}$ while minimizing the distance between its current and desired manipulability ellipsoids $\mathit{M}$ and $\hat{\mathit{M}}$. The four following approaches are considered for comparison with the proposed formulation (39). First, we analyze the corresponding Euclidean manipulability-tracking controller

$$\begin{array}{c}{\stackrel{·}{\mathit{q}}}_t={\mathit{J}}^{†}{\mathit{K}}_{\mathit{x}}({\hat{\mathit{x}}}_t-{\mathit{x}}_t)\hfill \\ +(\mathit{I}-{\mathit{J}}^{†}\mathit{J})({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}({\hat{\mathit{M}}}_t-{\mathit{M}}_t)\hfill \end{array}$$ (58)

where the difference between two manipulability ellipsoids is computed in Euclidean space, i.e., ignoring that manipulability ellipsoids belong to the set of SPD matrices. Second, we implement the corresponding Cholesky-based Euclidean manipulability controller

$$\begin{array}{c}{\stackrel{·}{\mathit{q}}}_t={\mathit{J}}^{†}{\mathit{K}}_{\mathit{x}}({\hat{\mathit{x}}}_t-{\mathit{x}}_t)\hfill \\ +(\mathit{I}-{\mathit{J}}^{†}\mathit{J})({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\mathbf{\Delta }{\mathit{L}}_{\mathbf{t}}\mathbf{\Delta }{\mathit{L}}_{\mathbf{t}}^{\top })\hfill \end{array}$$ (59)

which ignores that manipulability ellipsoids lie on the SPD manifold but ensure a positive-definite difference between two ellipsoids. Third, we analyze the Cholesky–Jacobian-based Euclidean manipulability controller

$$\begin{array}{c}{\stackrel{·}{\mathit{q}}}_t={\mathit{J}}^{†}{\mathit{K}}_{\mathit{x}}({\hat{\mathit{x}}}_t-{\mathit{x}}_t)\hfill \\ +(\mathit{I}-{\mathit{J}}^{†}\mathit{J})({\mathcal{J}}_{\mathrm{chol}(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\hat{\mathit{L}}-\mathit{L})\hfill \end{array}$$ (60)

which tracks manipulability ellipsoids through their Cholesky decomposition. Fourth, we evaluate the gradient-based approach of Rozo et al. (2017) that implements the controller

$${\stackrel{·}{\mathit{q}}}_t={\mathit{J}}^{†}{\mathit{K}}_{\mathit{x}}({\hat{\mathit{x}}}_t-{\mathit{x}}_t)-(\mathit{I}-{\mathit{J}}^{†}\mathit{J})\alpha \nabla g_t(\mathit{q})$$ (61)

where $\alpha$ is a scalar gain and

$$g_t(\mathit{q})=\log det\left(\frac{{\hat{\mathit{M}}}_t+{\mathit{M}}_t}{2}\right)-\frac{1}{2}\log det({\hat{\mathit{M}}}_t{\mathit{M}}_t)$$ (62)

is a cost function based on Stein divergence (a distance-like function on the SPD manifold (Sra, 2012)). The gain matrices ${\mathit{K}}_{\mathit{M}}$ are fixed as identity matrices and the scalar gain is set to 1 for the comparison.

Figure 13 shows the convergence rate for the manipulability-based redundancy resolution of the aforementioned approaches. Two tests were carried out by varying the initial configuration of the robot and the desired manipulability ellipsoid. In both cases, both geometry-aware and gradient-based approaches converge to a similar final robot configuration (see Figure 13(a) and (b), right), with similar values of the affine-invariant distance between the final and desired manipulability ellipsoids (see Figure 13(a) and (b), left and Table 3). More importantly, the proposed geometry-aware manipulability tracking approach shows a faster convergence than the gradient-based method, with a lower computational cost ($3.5$ and $4.2$ ms per time step, with non-optimized Matlab code on a laptop with 2.7 GHz CPU and 32 GB of RAM). This notable difference may be attributed to the fact that despite both methods take into account the geometry of manipulability ellipsoids, our approach is more informative about the kinematics of the robot through the use of the manipulability Jacobian $\mathcal{J}(\mathit{q})$.

Fig. 13.

Performance comparison of the different manipulability-based redundancy resolution formulations. Two cases are shown with varying initial robot configuration and desired manipulability. The graph on the left shows the convergence of the affine invariant distance between the current and the desired manipulability ellipsoid over time. The distances for the Euclidean, Cholesky-based Euclidean, Cholesky–Jacobian-based Euclidean, and geometry-aware and gradient-based approaches are depicted in blue, yellow, lilac, red, and purple, respectively. The graph on the right shows the initial and final posture of the robot along with the final manipulability ellipsoids. The initial posture of the robot is depicted in light gray. The final postures and the corresponding manipulability ellipsoids for the different methods are depicted in the same color as the distances. The desired manipulability ellipsoid is depicted in green.

Table 3.

Final distances $d(\hat{\mathit{M}},{\mathit{M}}_t)$ between the current and desired manipulability ellipsoids for the performance comparison of the different manipulability-based redundancy resolution formulations.

Approach	Euclidean	Cholesky	Cholesky–Jacobian	Geometry-aware	Gradient-based
Figure 13(a)	$0.433$	$0.808$	$1.418$	$0.416$	$0.436$
Figure 13(b)	$1.763$	$2.271$	$1.856$	$1.101$	$1.110$

Note that for some specific initial robot configurations and desired manipulability ellipsoids, the Euclidean manipulability-tracking controller (58) shows a slightly faster convergence rate than our method (see Figure 13(a)). However, this Euclidean formulation again leads to unstable behaviors in some configurations (see Figure 13(b)), where the distance between the final and desired manipulability ellipsoids remains high compared to the two geometry-aware approaches. This poor tracking performance can be attributed to the fact that the Euclidean difference between two SPD matrices is an approximation that is only valid if the matrices are close enough to each other. Thus, similarly to Euclidean controller aimed at tracking manipulability ellipsoids as first task (55), the Euclidean manipulability-based redundancy resolution is only effective if the current and desired ellipsoids are very similar. Moreover, the distance between the final and desired manipulability ellipsoids remains higher than for the geometry-aware methods and the Euclidean controller by using the Cholesky-based Euclidean manipulability-based redundancy resolution. This tendency is similar to the observations made for the tracking of manipulability ellipsoids as main objective and is due to the fact that the controller (59) induces paths on the manifold that are not close to geodesics. Furthermore, the Cholesky–Jacobian-based Euclidean controller shows a poor tracking performance for the two considered scenarios. Notably, the distance between the current and desired ellipsoids is largely increased before decreasing slowly in the first case (see Figure 13(a)). Moreover, in some configurations, the final distance remains high compared with the geometry-aware approaches as shown by Figure 13(b). These behaviors are due to the fact that the controller (60) does not follow geodesic paths on the SPD manifold.

The reported results supported our hypothesis that geometry-aware manipulability controllers result in good tracking performance while providing stable convergence regardless of the manipulability tracking error. This was observed when manipulability tracking was the main task and a secondary objective of the robot. Moreover, our manipulability-based redundancy resolution approach outperforms the gradient-based method. Furthermore, our controller permits the variability information of a task, given in the form of a fourth-order covariance tensor, to be exploited directly through the gain matrix of the controller. This allows the robot to exploit the precision required while tracking a manipulability ellipsoid either as main or secondary objective. This operation is not available in the gradient-based method used for comparison, since the corresponding controller gain is a scalar.

5.2.2. Comparisons with manipulability-based optimization

We compare our tracking approach against two state-of-the-art manipulability-based optimization methods widely used to improve robots posture for task execution. We first evaluate our geometry-aware controller against manipulability volume maximization. Then, we compare our controller to the compatibility index maximization (Chiu, 1987), where the distance from the ellipsoid center to its surface is maximized along a specified direction. To do so, we consider two 8-DoF planar robots that are required to track a desired Cartesian velocity trajectory that leads to an L-shape path in the Cartesian space. In order to achieve high dexterity in motion, the first robot is requested to track a desired manipulability ellipsoid whose main axis is elongated along the direction of motion. The second robot varies its posture in order to maximize either the manipulability volume or the compatibility index along the direction of motion.

Figure 14(a) shows the resulting joint configurations and manipulability ellipsoids of the two robots at different stages of the task where the second robot maximizes the manipulability volume as secondary objective. We observe that the main axis of the manipulability ellipsoid obtained with the volume maximization approach is often perpendicular to the direction of motion, which often occurs as this method does not consider any geometric information about the desired manipulability ellipsoid. In addition, because the resulting posture leads to ellipsoids that are not consistent with the task requirement (task velocity control directions) and degrade the robot capabilities, this becomes unstable when the gain of the velocity tracking controller is increased to achieve higher Cartesian velocities, as shown in Figure 15. Conversely, the robot tracking a desired manipulability ellipsoid successfully completes the task when higher velocities are required.

Fig. 14.

(a) Comparison of our manipulability tracking controller (in purple) with the manipulability volume maximization (in yellow). The main axis of the desired manipulability ellipsoids (in green) are aligned with the direction of motion in order to allow high velocities during the task execution. The robot colors become darker with the evolution of the movement. (b) Close-up plots of the manipulabilities represented in (a). (c) Comparison of our manipulability tracking controller (in purple) with the compatibility index maximization (in light blue).

Fig. 15.

Cartesian velocities achieved with our manipulability tracking controller (purple) and the volume maximization approach (yellow) as secondary objective for a Cartesian velocity controller. The gain of the velocity controller are equal for both approaches. The desired velocities are shown in green.

The main advantage of maximizing the compatibility index over the volume is that the directions in which the ellipsoid should be elongated are specified. However, this approach favors robot configurations that may be close to singularities as the manipulability ellipsoids corresponding to these posture are flat ellipsoids that can be largely elongated (see Figure 14(c)). This effect exacerbates when the compatibility index maximization is the main task of the robot, as this is not required to match a specific position in Cartesian space. Chiu (1988) extended the compatibility index optimization approach by defining the compatibility cost as a weighted sum, allowing the maximization or minimization of the ellipsoid along several directions. This method provides more flexibility on the resulting ellipsoid due to the weighted combination, at the cost of a laborious tuning. Moreover, the orientation and elongation of the main axes of the ellipsoid after the optimization are hard to infer from the cost weights.

In contrast to the considered manipulability-based optimization methods, the proposed geometry-aware controllers seeks to fit the full desired manipulability ellipsoid in all its directions. Singular configurations can therefore be easily avoided by defining appropriate desired manipulability ellipsoids. Moreover, our manipulability controller allows the tracking of any manipulability ellipsoid, including those providing a compromise between dexterity in motion and force exertion along any axis. This is not possible when using the compatibility index approach of Chiu (1987) as it always favors the dexterity in motion over force or vice versa. Although this compromise might be achievable using the compatibility index approach of Chiu (1988), our method does not require a laborious tuning process. Manipulability tracking is also hard to achieve through manipulability volume maximization as there is no explicit control on the resulting ellipsoid main axes.

6. Experiments

Previously, in our former work (Jaquier et al., 2018), we showed the benefits of including the manipulability redundancy resolution controller in the nullspace of a position controller for a pushing and an insertion task. In contrast to the result obtained by the position controller alone, the posture of the robot significantly varied during the execution of the tasks to be compatible with their respective force requirements as a consequence of the force manipulability tracking.

In this section, we extensively evaluate the proposed tracking formulation with different robotic platforms and different types of manipulability ellipsoids in simulation. The approach is evaluated to track a desired manipulability for grasping with an Allegro hand (four 4-DoF fingers) and to track a desired CoM manipulability with NAO and Centauro robots (25 and 39 DoFs, respectively). We then illustrate and evaluate the proposed manipulability transfer approach in a bimanual task using a Baxter robot (two 7-DoF arms) and a couple of Franka Emika Panda robots (7 DoFs).

6.1. Manipulability tracking for a robotic hand

In the context of robotic hands, manipulability ellipsoids have been used to analyze their performances in grasping tasks (Prattichizzo et al., 2012). In this experiment, we aim at modifying the posture of a robotic hand to match a desired manipulability ellipsoid while grasping an object.

For the case of multiple arm systems, the set of joint velocities of constant unit norm $\parallel {\stackrel{·}{\mathit{q}}}_a\parallel =\parallel ({\stackrel{·}{\mathit{q}}}_1^{\top },\dots ,{\stackrel{·}{\mathit{q}}}_C^{\top }{)}^{\top }\parallel =1$ is mapped to the Cartesian velocity space ${\stackrel{·}{\mathit{x}}}_a=({\stackrel{·}{\mathit{x}}}_1^{\top },\dots ,{\stackrel{·}{\mathit{x}}}_C^{\top }{)}^{\top }$ through

$$\parallel {\stackrel{·}{\mathit{q}}}_a{\parallel }^2={\stackrel{·}{\mathit{q}}}_a^{\top }{\stackrel{·}{\mathit{q}}}_a={\stackrel{·}{\mathit{x}}}_a^{\top }({\mathit{G}}_a^{†\top }{\mathit{J}}_a{\mathit{J}}_a^{\top }{\mathit{G}}_a^{†}{)}^{-1}{\stackrel{·}{\mathit{x}}}_a$$ (63)

with the Jacobian ${\mathit{J}}_a=\mathrm{diag}({\mathit{J}}_1,\dots ,{\mathit{J}}_C)$, the grasp matrix ${\mathit{G}}_a=({\mathit{G}}_1,\dots ,{\mathit{G}}_C)$, and C the number of arms. Therefore, the velocity manipulability ellipsoid of the C-arm system is given by ${\mathit{M}}^{{\stackrel{·}{\mathit{x}}}_a}={\mathit{G}}_a^{†\top }{\mathit{J}}_a{\mathit{J}}_a^{\top }{\mathit{G}}_a^{†}$ (Chiacchio et al., 1991a). Note that the system is modeled under assumptions that the arms are holding a rigid object with a tight grasp.

In this first experiment, the Allegro hand was required to track a desired manipulability, while maintaining relative positions between the different fingers. This experiment aims at emulating how humans adapt their finger configuration to the task at hand while grasping an object. In this experiment, the desired velocity manipulability ellipsoid was designed by the experimenter to be a medium-sized isotropic ellipsoid. The purpose of this design is to provide the hand with the capability to perform a displacement of the object while being resistant to external perturbations in all the directions. For example, in the case where the hand is holding a pen, it is desirable that the pen can be moved with dexterity, while the hand should resist to perturbations owing to the pen–surface contacts.

The fingers were controlled according to a leader–follower strategy (Luh and Zheng, 1987). Therefore, the thumb joints were moved to track the desired manipulability ellipsoid using the controller (38) and the other fingers were required to maintain constant relative end-effector positions with respect to the thumb end-effector, while tracking the manipulability as secondary objective with the redundancy controller (39). The center of the object was considered as the central position between the four fingers of the hand and the contact points were assumed to be at the finger tips.

Figure 16(a) and (b) show an example of adaptation of the posture of the hand to track a desired velocity manipulability ellipsoid for a grasp defined by the user. As expected, the robot modified its joint configuration in order to match, as accurately as possible, the desired velocity manipulability (see Figure 16(c)). Note that the manipulability tracking in this experiment can only be achieved partially, because the robotic hand is also required to maintain the initial grasp. Nevertheless, this tracking may be further improved if the dimensionality of the nullspace of the main task is higher (e.g., not all the finger tips are position-constrained), or using a higher-DoF robotic hand.

Fig. 16.

Manipulability tracking for grasping tasks with the Allegro hand in simulation. (a) and (b) The initial and final pose of the robot, respectively. (c) Initial, final, and desired manipulability ellipsoids depicted in yellow, dark purple, and green, respectively. The bottom-right graph shows the evolution of the distance between the current and desired manipulability ellipsoid over time (given in seconds).

6.2. Manipulability tracking for a humanoid CoM

An interesting use of manipulability ellipsoids arises when these are defined at the CoM of humanoid robots, which permits their capabilities to accelerate the CoM in locomotion to be analyzed (Azad et al., 2017; Gu et al., 2015), or how resistant they can be to external perturbations to be evaluated using the force manipulability at a specific humanoid posture. With the goal of getting some insights on the role of CoM manipulability ellipsoids in legged robots, we designed manipulability tracking experiments using two different floating-base robots in simulation, namely, the humanoid NAO and the Centauro robot (Baccelliere et al., 2017).

Specifically, we required the robots to track a desired manipulability ellipsoid defined at its CoM while maintaining balance. We assumed a strict hierarchy of tasks that gave the highest priority to the task of maintaining the CoM position over the support polygon and zero velocity at all contact points with the floor, while the manipulability tracking was considered a secondary task. Under the aforementioned assumptions, we implemented the inverse kinematics-based controller for floating-base robots proposed in Mistry et al. (2008), which we briefly introduce here. First, let us define the Jacobian for the primary task as

$${\mathit{J}}_b=\left[\begin{array}{c}{\mathit{J}}_{\mathrm{feet}}\hfill \\ \multicolumn{1}{c}{{\mathit{J}}_{\mathrm{CoM},\mathrm{xy}}}\end{array}\right]$$ (64)

where ${\mathit{J}}_{\mathrm{feet}}$ represents the Jacobians for the position/orientation of the robot feet while ${\mathit{J}}_{\mathrm{CoM},\mathrm{xy}}$ is the Jacobian for the projection of the CoM onto the $(x,y)$ plane (assuming the gravity vector is in the z direction). Next, we define the vector of primary desired velocities ${\mathit{x}}_b$ (i.e., velocities of the robot feet and CoM), noting that all the robot feet velocities must equal zero in order to maintain constraints, therefore

$${\stackrel{·}{\mathit{x}}}_b=\left[\begin{array}{c}\mathit{0}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\mathit{x}}}_{\mathrm{CoM}}}\end{array}\right]$$ (65)

where ${\stackrel{·}{\mathit{x}}}_{\mathrm{CoM}}$ is the velocity at the robot CoM so that it lies in the support polygon.

Regarding the secondary task, that is, the manipulability tracking at the robot CoM, we first compute the Jacobian at the CoM ${\mathit{J}}_{\mathrm{CoM}}$ for floating-base robots as in Mistry et al. (2008), which allows us to calculate manipulability ellipsoids of the types introduced in Section 4. Depending on which type of manipulability we require the robot to track, we can use any of the manipulability Jacobians (34), (35), or (36) to compute the desired joint velocities $\stackrel{·}{\mathit{q}}$ for the manipulability tracking task using (38). Thus, the full joint velocity controller for legged robots required to keep balance while tracking a desired manipulability ellipsoid at their CoM is defined as

$$\stackrel{·}{\mathit{q}}={\left[\begin{array}{c}{\mathit{I}}_{n\times n}\hfill \\ \multicolumn{1}{c}{{\mathit{0}}_{6\times n}}\end{array}\right]}^{\top }({\mathit{J}}_b^{†}{\stackrel{·}{\mathit{x}}}_b+{\mathit{N}}_b({\mathcal{J}}_{(3)}^{†}{)}^{\top }{\mathit{K}}_{\mathit{M}}\mathrm{vec}(\mathrm{Lo}{\mathrm{g}}_{{\mathit{M}}_t}({\hat{\mathit{M}}}_t)))$$ (66)

where the first term is included in order to account for the virtual joints of legged robots, n is the number of DoF of the robot, and ${\mathit{N}}_b$ is the nullspace of the Jacobian (64).

We ran several experiments for testing the manipulability tracking at the CoM of the Centauro (Figure 17) and NAO (Figure 18) robots using the controller (66). The tests consisted of manually setting a desired manipulability ellipsoid to be tracked at the CoM of the robot, and running a joint velocity controller given the reference provided by (66). Notably, both Centauro and NAO tracked the desired manipulability as precisely as possible without compromising the balancing task. Figures 17(c) and (c) show the distance between the desired and current CoM manipulability, which decreases over time as the robot adapts its posture to carry out a good tracking while keeping its balance. An interesting aspect about defining and tracking CoM manipulability ellipsoids is the final posture that the robots achieve. Figure 17(b) shows the final posture achieved by Centauro when tracking a CoM manipulability whose projection on the $(x_1,x_2)$ plane is a tilted ellipse, which makes the robot adopt a posture where the front legs and torso rotate on the same plane (which corresponds to the floor in the virtual environment). The final posture of NAO displayed in Figure 18(b) shows that both arms are completely extended along the humanoid frontal axis, in an attempt to align them with one of the main axes of the CoM manipulability ellipsoid. However, both the balancing task and the lower number of DoF constrain NAO to closely match the desired manipulability.

Fig. 17.

Tracking of the CoM manipulability with the Centauro robot in simulation. (a) and (b) The initial and final pose of the robot, respectively. (c) Initial, final, and desired manipulability ellipsoids depicted in yellow, dark purple, and green, respectively. The bottom-right graph shows the evolution of the distance between the current and desired manipulability ellipsoid over time (given in seconds).

Fig. 18.

CoM manipulability tracking with NAO in simulation. (a) and (b) The initial and final pose of NAO, respectively. The CoM of the robot is depicted by a red sphere. (c) Initial, final, and desired manipulability ellipsoids depicted in yellow, dark purple, and green, respectively. The bottom-right graph shows the distance between the current and desired manipulability over time (given in seconds).

6.3. Manipulability transfer between robots for a bimanual task

The performance of the proposed manipulability transfer framework was tested in a bimanual unplugging of an electric cable from a power socket. The central idea is to teach different dual-arm robots to execute a task requiring a specific manipulability profile via kinesthetic teaching provided only to one of the bimanual robots.

In the first part of the experiment, the two 7-DoF arms of a Baxter robot are kinesthetically guided to provide demonstrations (see Figure 19(a)). The posture of the arms is modified by the user so that the main axis of the dual force manipulability ellipsoid of the system ${\mathit{M}}^{{\mathit{F}}_a}=({\mathit{G}}_a^{†\top }{\mathit{J}}_a{\mathit{J}}_a^{\top }{\mathit{G}}_a^{†}{)}^{-1}$ is aligned with the direction of extraction. Then, the arms are moved in opposite directions to unplug the electric cable from the socket. We extracted both the relative position $\mathbf{\Delta }{\mathit{x}}_{\mathbf{t}}$ between the end-effectors of both arms and the force manipulability ellipsoid of the system ${\mathit{M}}_{a,t}^{\mathit{F}}$. The collected data were time-aligned and split into two datasets of time-driven trajectories, namely relative Cartesian positions and manipulability. We trained a classical GMM over the time-driven relative positions and a geometry-aware GMM over the time-driven manipulability ellipsoids. The number of components of each model ($K=4$) was selected by the experimenter.

Fig. 19.

Unplugging task: (a) demonstrations provided by the user on the Baxter robot; (b) reproduction by the Baxter robot; (c) reproduction by the two Franka Emika Panda robots. The robots pose at the beginning of the task, before, and after the extraction of the cable from the socket are shown in the left, middle, and right column, respectively.

In the second part of the experiment, the unplugging task is reproduced by both the Baxter robot and a pair of Franka Emika Panda robots (see Figure 19(b) and (c)). For both reproductions, the relative position between the end-effectors and the desired manipulability of the system were computed at each time step by a classical GMR as $\hat{\mathbf{\Delta }}{\mathit{x}}_t~p(\mathbf{\Delta }\mathit{x}|\mathbf{t})$ and a geometry-aware GMR as ${\hat{\mathit{M}}}_{a,t}^{\mathit{F}}~p({\mathit{M}}_a^{\mathit{F}}|t)$. In both cases, the left robotic arm was required to move its joints to track the desired manipulability ellipsoid (38), whereas the right arm was required to maintain the desired relative Cartesian position with respect to the left arm, while tracking the desired manipulability as secondary objective (39). Note that the actuation contribution of each robot was taken into account to compute the manipulability ellipsoids through the whole experiment.

Figure 20(a) displays the two demonstrations recorded by kinesthetically guiding the Baxter robot along with the components of the GMM encoding $\mathbf{\Delta }{\mathit{x}}_{\mathbf{t}}$ and the centers of the components of the geometry-aware GMM encoding ${\mathit{M}}_a^{\mathit{F}}$. The first and third dimensions of $\mathbf{\Delta }{\mathit{x}}_{\mathbf{t}}$ are not represented as they do not vary significantly during the experiment. Figure 20(b) shows the relative Cartesian position and manipulability ellipsoid profile to be tracked and the reproduction results when the Baxter robot executed the task. Baxter successfully tracked the desired manipulability ellipsoid while maintaining the required relative distance between its end-effectors.

Fig. 20.

(a) Demonstrations and GMM encoding the unplugging task. The top graph shows the demonstrated relative end-effectors position for the Baxter robot (in gray) and components of the 4-state GMM (in blue). Only the most representative dimension is displayed. The distance between the two arms increases when the cable is unplugged from the socket. The middle-bottom graphs show the demonstrated force manipulability profile (in gray) and centers of the 4-state GMM in the SPD manifold over time (in purple). (b) Reproduction of the unplugging task with Baxter. The desired and reproduced trajectories are represented in green and dark blue, respectively. The top graph shows the desired and reproduced relative position between the end-effectors along the second dimension. The middle-bottom graphs show the desired and reproduced (overlapping) manipulability ellipsoids. (c) Reproduction of the unplugging task with the two Panda robots. The desired and reproduced trajectories are represented in green and purple, respectively. The top graph shows the desired and reproduced relative position between the end-effectors along the second dimension. The middle-bottom graphs shows the desired and reproduced manipulability ellipsoids. The position $\mathit{x}$ and time t are given in meters and seconds.

Figure 20(c) shows the relative Cartesian position between the arms and the manipulability ellipsoid profile obtained during the reproduction of the task by the two Panda robots. These successfully achieved the required task and tracked the desired manipulability ellipsoid profile obtained from model trained with the data recorded on the Baxter robot. Note the manipulability matching is not exact in this case owing to the differences between Baxter and the Panda robots. Indeed, even if the actuation capabilities of each robot are taken into account in our manipulability transfer framework, the capabilities of the two dual-arm system differ due to other physical specificities, e.g., the relative position of the bases of the arms.

7. Discussion

Our tracking formulation enables robots to modify their posture in an exponentially stable way so that desired manipulability ellipsoids are tracked, either as a main control task or as a redundancy resolution problem where the manipulability tracking is considered a secondary objective. Compared with state-of-the-art manipulability-based optimization schemes, our tracking formulation allows the reproduction of any manipulability ellipsoid beyond the maximization of manipulability parameters. The proposed tracking approach covers different manipulability ellipsoids proposed in the literature, such as velocity, force, and dynamic manipulability ellipsoids (Doty et al., 1995). A relevant aspect about our approach is their generic structure, which means that we can track manipulability ellipsoids for a large variety of robots, as reported in the previous section, where a robotic hand, a Centauro robot, a humanoid, and two different bimanual setups were used to test our tracking approach. This shows that our approach can be used in a large variety of contexts and that many further applications can be considered.

The manipulability transfer results reported in Section 6.3 showed the effectiveness of the proposed approach for transferring manipulability ellipsoids between robots that differ in their kinematic structure, which has remained a challenge in the robot learning community. Our learning framework allows a robot to learn posture-dependent task requirements without explicitly encoding a model in the joint space of the demonstrator, which would require complex kinematic mapping algorithms and would make task analysis less interpretable at first sight. In addition, the proposed framework extends the robot learning capabilities beyond the transfer of trajectory, force, and impedance.

It is important to emphasize the fact that the manipulability tracking precision strongly depends on the number of DoFs when the task is considered a secondary objective, as the higher it is, the more capable the robot is to perform more than one task simultaneously. Note that, in the case of legged robots (which are often characterized by a high number of DoFs), the manipulability tracking may still be slightly compromised because of the set of constraints imposed by the balancing task, as observed in Section 6.2. However, if these robots are provided with the possibility of modifying their feet position while keeping balance, then the manipulability tracking may be further improved. This clearly requires more sophisticated balancing controllers, but gives robots more freedom to adapt their posture and achieve better manipulability tracking. Note that in the case of robotic hands, a similar behavior arises when the finger tips are constrained according to some grasping requirements, which might affect the manipulability tracking when projected into the nullspace of the primary task.

It is important to note that the proposed manipulability tracking approach is a local method in the sense that the solution depends on the current configuration of the robot expressed through the Jacobian. This makes the tracking convergence dependent on the current configuration of the robot, which sometimes may limit the tracking performance. However, the robot may achieve a better tracking performance if it is allowed to look for other initial postures. As an example, the robot may not track precisely the desired manipulability ellipsoids for a given initial posture, due for instance to its joint limits. However, if the robot slightly modifies its initial posture, it may find a better starting configuration to subsequently minimize the error between the desired and current manipulability ellipsoids in a larger proportion, even if the new initial posture initially increases this error.

Overall, the proposed manipulability transfer framework may be exploited in a large variety of applications, where the posture of the robot may have an impact on its performance while executing the task. In addition to varying the robot posture for task compatibility, tracking a desired manipulability profile as a secondary task may typically complement a main control task to avoid singularity, handle perturbations during task execution, optimize the execution time, or minimize the energy consumption (Kim et al., 2010). In particular, manipulability transfer may be utilized from a motion planning point of view. To do so, the robot may first track a desired manipulability as main control task in a planning phase, where the robot adapts its posture in order to anticipate the next action. Following this planning phase, the robot executes the desired action with a posture adapted to the task requirements. In this phase, the desired manipulability is tracked as a secondary task. Moreover, in the context of rehabilitation and assistance, the proposed learning and tracking formulations may be exploited in control strategies for exoskeletons. In Petric et al. (2019) the exoskeleton posture is optimized to achieve an isotropic manipulability by sensing the human muscular manipulability. In this setting, a varying exoskeleton manipulability profile may be retrieved using GMR as a function of the sensed muscular manipulability.

From a mathematical point of view, it is worth highlighting the importance of considering the structure of the data we work with. While alternative solutions to handle SPD matrices are present in literature (e.g., those using Cholesky decomposition), we showed that Euclidean manipulability-tracking controllers lead to unstable behaviors in contrast to the stable behavior displayed by our geometry-aware controller. Equally important, the manipulability ellipsoids profiles retrieved by the geometry-aware and Euclidean GMR were similar only around the mean of the GMM components, but diverged when moving away from it. This is because the estimated output in Euclidean space is only a valid approximation for input data lying close to the mean, as reported in Section 5. Therefore, geometry-awareness is crucial for successful learning and tracking of manipulability ellipsoids.

8. Conclusions and future work

This article has presented a novel framework for transferring manipulability ellipsoids to robots. The proposed approach has been built on a probabilistic learning model that allows the encoding and retrieval manipulability ellipsoids, and on the extension of the classical inverse kinematics problem to manipulability ellipsoids, by establishing a mapping between a change of manipulability ellipsoid and the robot joint velocity. We exploited tensor representation and Riemannian manifolds to build a geometry-aware learning framework and exponentially stable tracking controllers and showed the importance of geometry-awareness for manipulability transfer. We have then shown that our manipulability transfer framework allows the exploitation of task variations recovered by the learning approach to characterize the precision of the manipulability tracking problem. This approach enables the learning of posture-dependent task requirements. It provides a skill transfer strategy going beyond the imitation of trajectory, force, or impedance behaviors. Furthermore, it allows manipulability transfer between agents of different embodiments, while taking into account their individual characteristics and is adapted to complex scenarios involving any manipulability ellipsoid shape and various types of robots.

Future work will explore manipulability transfer between humans and robots. Following this research direction, we recently proposed a statistical analysis of single and dual-arm manipulability ellipsoids for human movements, accompanied by two human-to-robot manipulability transfer experiments. The corresponding results will be detailed in a forthcoming publication. We will also investigate manipulability transfer strategies where the desired manipulability would be optimized as a function of the robot. The objective would be to adapt the manipulability ellipsoid to exploit the capabilities of the learner in situations in which this learner can reach a better manipulability than the teacher for the task at hand.

Appendix A. Derivative of a matrix with respect to a vector

A.1. Left multiplication by a constant matrix (Equation (14))

We have

$$\frac{\partial \mathit{AY}}{\partial \mathit{x}}=\frac{\partial \mathit{Y}}{\partial \mathit{x}}{\times }_1\mathit{A}$$

Proof.

$${\left(\frac{\partial \mathit{AY}}{\partial \mathit{x}}\right)}_{\mathrm{ljk}}=\frac{\partial }{\partial x_k}\sum _i{a}_{\mathrm{li}}{y}_{\mathrm{ij}}=\sum _i{a}_{\mathrm{li}}\frac{\partial y_{\mathrm{ij}}}{\partial x_k}$$

  □

A.2. Right multiplication by a constant matrix (Equation (15))

We have

$$\frac{\partial \mathit{YB}}{\partial \mathit{x}}=\frac{\partial \mathit{Y}}{\partial \mathit{x}}{\times }_2{\mathit{B}}^{\top }$$

Proof.

$${\left(\frac{\partial \mathit{YB}}{\partial \mathit{x}}\right)}_{\mathrm{ilk}}=\frac{\partial }{\partial x_k}\sum _i{y}_{\mathrm{ij}}{b}_{\mathrm{jl}}=\sum _j{b}_{\mathrm{jl}}\frac{\partial y_{\mathrm{ij}}}{\partial x_k}$$

  □

A.3. Derivative of the inverse of a matrix (Equation (16))

We have

$$\frac{\partial {\mathit{Y}}^{-1}}{\partial \mathit{x}}=-{\frac{\partial \mathit{Y}}{\partial \mathit{x}}}^{\top }{\times }_1{\mathit{Y}}^{-1}{\times }_2{\mathit{Y}}^{-\top }$$

Proof. We compute the derivative of the definition of the inverse ${\mathit{Y}}^{-1}\mathit{Y}=\mathit{I}$ as

$$\frac{\partial }{\partial \mathit{x}}({\mathit{Y}}^{-1}\mathit{Y})=\frac{\partial }{\partial \mathit{x}}(\mathit{I})$$

$$\frac{\partial {\mathit{Y}}^{-1}}{\partial \mathit{x}}{\times }_2{\mathit{Y}}^{\top }+\frac{\partial \mathit{Y}}{\partial \mathit{x}}{\times }_1{\mathit{Y}}^{-1}=\mathit{0}$$

Then, by isolating $\frac{\partial {\mathit{Y}}^{-1}}{\partial \mathit{x}}$, we obtain

$$\frac{\partial {\mathit{Y}}^{-1}}{\partial \mathit{x}}=-{\frac{\partial \mathit{Y}}{\partial \mathit{x}}}^{\top }{\times }_1{\mathit{Y}}^{-1}{\times }_2{\mathit{Y}}^{-\top }$$

Appendix B. Symbolic manipulability Jacobian for a serial kinematic chain

The computation of the manipulability Jacobian involves computing the derivative of the robot Jacobian with respect to the joint angles. Those derivatives can be computed in a symbolic form as shown in Bruyninckx and De Schutter (1996). We remind here the symbolic derivative for the hybrid representation of the Jacobian $\mathit{J}\in {\mathbb{R}}^{6\times n}$ that is used in the computation of the manipulability Jacobian $\mathcal{J}$.

The ith column of the Jacobian is denoted by

$${\mathit{J}}^i=\left(\begin{array}{c}{\mathit{w}}^i\hfill \\ \multicolumn{1}{c}{{\mathit{v}}^i}\end{array}\right)$$ (67)

with ${\mathit{w}}^i\in {\mathbb{R}}^3$ and ${\mathit{v}}^i\in {\mathbb{R}}^3$ the rotational and translational components of the Jacobian.

The derivative of the Jacobian with respect to the joint angles is a third-order tensor $\frac{\partial \mathit{J}}{\partial \mathit{q}}\in {\mathbb{R}}^{6\times n\times n}$ with mode-1 fibers or columns

$${\left(\frac{\partial \mathit{J}}{\partial \mathit{q}}\right)}_{:\mathrm{ij}}=\frac{\partial {\mathit{J}}^i}{\partial q^j}=\{\begin{array}{cc}{\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j){\mathit{J}}^i\hfill & \mathrm{if}j\le i\hfill \\ \multicolumn{1}{c}{-{\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^j){\mathit{J}}^i}& \mathrm{if}j>i\hfill \end{array}$$ (68)

where

$${\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j)=\left(\begin{array}{cc}[{\mathit{w}}^j\times ]\hfill & {\mathit{0}}_{3\times 3}\hfill \\ \multicolumn{1}{c}{{\mathit{0}}_{3\times 3}}& [{\mathit{w}}^j\times ]\hfill \end{array}\right)$$ (69)

$${\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^j)=\left(\begin{array}{cc}{\mathit{0}}_{3\times 3}\hfill & {\mathit{0}}_{3\times 3}\hfill \\ \multicolumn{1}{c}{[{\mathit{v}}^j\times ]}& {\mathit{0}}_{3\times 3}\hfill \end{array}\right)$$ (70)

and × the cross product between two vectors. The notation $[{\mathit{w}}^j\times ]$ in a matrix denotes that the corresponding component of the result of the right-multiplication of the matrix by a vector is equal to the cross product between ${\mathit{w}}^j$ and the corresponding vector component, e.g., ${\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j){\mathit{J}}^i=\left(\begin{array}{c}{\mathit{w}}^j\times {\mathit{w}}^i\hfill \\ \multicolumn{1}{c}{{\mathit{w}}^j\times {\mathit{v}}^i}\end{array}\right)$.

Note that the time derivative of the Jacobian can therefore be computed as

$$\frac{d\mathit{J}}{\mathrm{dt}}=\sum _{j=1}^n\frac{\partial \mathit{J}}{\partial q^j}{\stackrel{·}{q}}_j$$ (71)

Appendix C. Symbolic dynamic manipulability Jacobian for a serial kinematic chain

The derivative of the robot inertia matrix with respect to joint angles is necessary for the computation of the dynamic manipulability Jacobian. It can be computed in closed form as follows.

The inertia matrix $\mathbf{\Lambda }(\mathit{q})\in {\mathbb{R}}^{\mathbf{n}\times \mathbf{n}}$ can be written as

$$\mathbf{\Lambda }(\mathit{q})=\sum _{\mathbf{i}=1}^{\mathbf{n}}{\mathit{J}}_{\mathbf{i}}^{\top }\left(\begin{array}{cc}{\mathbf{\Lambda }}_{\mathbf{i}}\hfill & \mathit{0}\hfill \\ \multicolumn{1}{c}{\mathit{0}}& {\mathbf{m}}_{\mathbf{i}}\mathit{I}\hfill \end{array}\right){\mathit{J}}_{\mathbf{i}}$$ (72)

where ${\mathit{J}}_i$, ${\mathbf{\Lambda }}_{\mathbf{i}}$, and $m_i$ are the Jacobian, inertia matrix, and mass of link i, respectively (Murray et al., 1994; Park, 1995).

The derivative of the inertia matrix is the third-order tensor $\frac{\partial \mathbf{\Lambda }}{\partial \mathit{q}}\in {\mathbb{R}}^{n\times n\times n}$ computed as

$$\frac{\partial \mathbf{\Lambda }}{\partial \mathit{q}}=\sum _{i=1}^n\frac{\partial {\mathit{J}}_i^{\top }}{\partial \mathit{q}}{\times }_2{\mathit{J}}_i^{\top }{\mathit{M}}_i+\frac{\partial {\mathit{J}}_i}{\partial \mathit{q}}{\times }_1{\mathit{J}}_i^{\top }{\mathit{M}}_i$$ (73)

where ${\mathit{M}}_i=\left(\begin{array}{cc}{\mathbf{\Lambda }}_{\mathbf{i}}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& m_i\mathit{I}\hfill \end{array}\right)$ and $\frac{\partial {\mathit{J}}_i}{\partial \mathit{q}}$ is computed with Equation (68).

Appendix D. Symbolic derivative of the manipulability Jacobian for a serial kinematic chain

In some cases, e.g., in the acceleration tracking controller, the time derivative of the manipulability Jacobian is required. This time derivative can be computed symbolically by exploiting the first and second derivative of the Jacobian with respect to the joint angles.

The time derivative of the velocity manipulability Jacobian ${\mathcal{J}}^{\stackrel{·}{\mathit{x}}}\in {\mathbb{R}}^{6\times n\times n}$ defined as

$${\mathcal{J}}^{\stackrel{·}{\mathit{x}}}=\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\mathit{J}+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\mathit{J}$$ (74)

is obtained by exploiting the chain rule as

$$\frac{\partial {\mathcal{J}}^{\stackrel{·}{\mathit{x}}}}{\partial t}=\frac{\partial }{\partial t}(\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\mathit{J}+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\mathit{J})$$ (75)

$$=\frac{{\partial }^2\mathit{J}}{\partial t\partial \mathit{q}}{\times }_2\mathit{J}+\frac{\partial \mathit{J}}{\partial \mathit{q}}{\times }_2\frac{\partial \mathit{J}}{\partial t}+\frac{{\partial }^2{\mathit{J}}^{\top }}{\partial t\partial \mathit{q}}{\times }_1\mathit{J}+\frac{\partial {\mathit{J}}^{\top }}{\partial \mathit{q}}{\times }_1\frac{\partial \mathit{J}}{\partial t}$$ (76)

The time derivative of the Jacobian is given by Equation (71) and the time derivative of the derivative of the Jacobian with respect to joint angles is given by

$$\frac{{\partial }^2\mathit{J}}{\partial t\partial q_j}=\sum _{k=1}^n\frac{{\partial }^2\mathit{J}}{\partial q_k\partial q_j}{\stackrel{·}{q}}_k$$ (77)

where the second derivative of the Jacobian with respect to the joint angles is a fourth-order tensor $\frac{{\partial }^2\mathit{J}}{\partial {\mathit{q}}^2}\in {\mathbb{R}}^{6\times n\times n\times n}$ with mode-1 fibers or columns

$$\begin{array}{c}{\left(\frac{{\partial }^2\mathit{J}}{\partial {\mathit{q}}^2}\right)}_{:\mathrm{ijk}}=\frac{{\partial }^2{\mathit{J}}^i}{\partial q^k\partial q^j}\hfill \\ =\{\begin{array}{cc}({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j){\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^k)){\mathit{J}}^i+{\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j)({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^k){\mathit{J}}^i)\hfill & \mathrm{if}k\le j\le i\hfill \\ \multicolumn{1}{c}{{\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j)({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^k){\mathit{J}}^i)}& \mathrm{if}j\le k\le i\hfill \\ \multicolumn{1}{c}{-{\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j)({\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^k){\mathit{J}}^i)}& \mathrm{if}j\le i<k\hfill \\ \multicolumn{1}{c}{-({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^k){\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^j)){\mathit{J}}^i-{\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^j)({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^k){\mathit{J}}^i)}& \mathrm{if}k\le i<j\hfill \\ \multicolumn{1}{c}{-({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^k){\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^j)){\mathit{J}}^i}& \mathrm{if}i<k<j\hfill \\ \multicolumn{1}{c}{-({\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j){\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^k)){\mathit{J}}^i}& \mathrm{if}i<j\le k\hfill \end{array}\hfill \end{array}$$ (78)

where ${\mathit{P}}_{\mathrm{\Delta }}({\mathit{J}}^j)$ and ${\mathit{M}}_{\mathrm{\Delta }}({\mathit{J}}^k)$ are defined as in (69) and (70), respectively. The time derivative of the force manipulability Jacobian ${\mathcal{J}}^{\mathit{F}}$ and the manipulability Jacobian ${\mathcal{J}}^{\stackrel{··}{\mathit{x}}}$ corresponding to the dynamic manipulability ellipsoid can be computed symbolically in a similar way using Equations (71) and (77). Moreover, their with respect to joint angles can be computed symbolically using the chain rules, Equations (68) and (78).