Geometry-aware ManipulabilityLearning, Tracking and Transfer

Geometry-aware Manipulability
Learning, Tracking and Transfer

Noémie Jaquier\affilnum1    Leonel Rozo\affilnum2,3    Darwin G. Caldwell\affilnum3 and Sylvain Calinon\affilnum1 \affilnum1Idiap Research Institute, CH-1920 Martigny, Switzerland
\affilnum2Bosch Center for Artificial Intelligence, 71272 Renningen, Germany
\affilnum3Department of Advanced Robotics, Istituto Italiano di Tecnologia, 16163 Genova, Italy
noemie.jaquier@idiap.ch
Abstract

Body posture influences human and robots performance in manipulation tasks, as appropriate poses facilitate motion or force exertion 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 apply a specific force. In this context, this paper 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.

Robot learning, programming by demonstration, manipulability ellipsoids, manipulability optimization, Riemannian manifolds, differential kinematics.
\corrauth

Noémie Jaquier, Idiap Research Institute, CH-1920 Martigny, Switzerland

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 Fig. 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)].

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, since 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.

(a)
(b)
Figure 1: Illustration of pushing (a) and pulling (b) tasks for which the posture of the human significantly influences his/her ability to carry out the task.

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. [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 et al. [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.

In this paper 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 symmetric positive definite (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 sake of clarity, different aspects of the proposed learning and tracking approaches are illustrated with simple examples using simulated planar robots throughout the paper.

The proposed approach can be straightforwardly applied to different types of kineto-static 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 transfered between agents of different embodiments. The functionality of the proposed approach is evaluated in different simulated manipulability tracking tasks involving a 16-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 null-space commands. Existing approaches built on manipulability-based optimizations 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 paper 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 kineto-static manipulability measures to dynamic measures; (iii) demonstrating the asymptotic stability of the proposed manipulability tracking controllers; (iv) introducing various novel types of geometry-aware manipulability tracking schemes and introducing 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 paper. Related source codes will be available after publication.

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 -DoF robot can be found by using the kinematic relationship between task velocities and joint velocities ,

(1)

where and are the joint position and Jacobian of the robot, respectively. Moreover, consider the set of joint velocities of constant (unit) norm describing the points on the surface of a hypersphere in the joint velocity space, which is mapped into the Cartesian velocity space with111Note that an additional scaling of the joint velocities may be included to consider actuator boundaries.

(2)

by using the least-squares inverse kinematics relation . Equation (2) represents the robot manipulability in terms of motion, indicating the flexibility of the manipulator in generating velocities in Cartesian space.222Dually, the force manipulability ellipsoid can be computed from the static relationship between joint torques and Cartesian forces [Yoshikawa (1985b)].

In the literature, the velocity manipulability ellipsoid is usually defined as , 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 (see e.g. [Chiu (1987)]). For the sake of consistency, we here use an alternative definition of the velocity manipulability ellipsoid given by , which directly corresponds to the ellipsoid of end-effector velocities . So, the major axis of this manipulability ellipsoid is aligned to the eigenvector corresponding to the maximum eigenvalue of the matrix whose length equals the square root of the maximum eigenvalue. Thus, the interpretation and representation of the manipulability ellipsoid from the corresponding matrix is facilitated. Note that the major axis of the velocity manipulability ellipsoid 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 due 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 (see [Chiu (1987)] for details).

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 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 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 belongs to the set of symmetric positive definite (SPD) matrices which describe the interior of the convex cone. Consequently, our manipulability transfer formulation must consider this particular characteristic in order 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 SPD matrices is not a vector space since 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.333The original cone of SPD matrices has been changed into a regular and complete (but curved) manifold with an infinite development in each of its directions [Pennec et al. (2006)]. This metric permits to define lengths of curves on the manifold. These curves, called geodesics, 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 is a mathematical space for which each point locally resembles a Euclidean space. For each point , there exists a tangent space equipped with a positive definite inner product. In the case of the SPD manifold, the tangent space at any point is identified by the space of symmetric matrices and the inner product between two matrices , is

(3)

The space of SPD matrices can be represented as the interior of a convex cone embedded in its tangent space . To utilize these tangent spaces, we need mappings back and forth between and , which are known as exponential and logarithmic maps.

(a)
(b)
Figure 2: SPD manifold embedded in its tangent space . One point corresponds to a matrix  . Points inside the cone, such as and , belong to . (a) lies on the tangent space of such that . The shortest path between and is the geodesic represented as a purple curve, which differs from the Euclidean path depicted in yellow. (b) is the result of the parallel transport of from to .

The exponential map maps a point in the tangent space to a point on the manifold, so that it lies on the geodesic starting at in the direction and such that the distance between and is equal to the distance between and . The inverse operation is called the logarithmic map . Both operations are illustrated in Fig. 1(a).

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

(4)

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

(5)
(6)

where and are the matrix exponential and logarithm functions.

Another useful operation over manifolds is the parallel transport , which moves elements between tangent spaces such that the angle between two elements in the tangent space remains constant (see Fig. 1(b)). The parallel transport of to is given by

(7)

with (see [Sra and Hosseini (2015)] for details). This operation is exploited when it is necessary to move SPD matrices along a curve on the nonlinear manifold.

In this paper, we first exploit the Riemannian manifold framework to propose a probabilistic learning model that encodes and retrieves manipulability ellipsoids considering that these belong to . Secondly, 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 1st and 2nd-order tensors. Tensor representation permits to represent and exploit data structure of multidimensional arrays. In this paper, 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 (1st- and 2nd-order tensors, respectively), which results in a 3rd-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 . The tensor product of two tensors , is with elements

(8)

2.3.2 -mode product

The multiplication of a tensor by a matrix , known as the -mode product is defined as

(9)

where is the -mode matricization or unfolding of tensor . Element-wise, this -mode product can be written as .

2.3.3 Tensor contraction

As described in [Tyagi and Davis (2008)], we denote the element of a 4th-order tensor by with two covariant indices , and two contravariant indices , . The element (,) of a matrix is denoted by with two covariant indices , . A tensor contraction between two tensors is performed when one or more contravariant and covariant indices are identical. For example, the tensor contraction of and is written as

(10)

2.3.4 Tensor covariance

Similarly to the covariance of vectors, the th-order covariance tensor of tensors is given by

(11)

where is the total number of datapoints. 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 2nd-order symmetric matrix with mean and covariance is defined as [Basser and Pajevic (2007)]

(12)

with . 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 w.r.t a vector

In the following identities, the matrix is a function of , while and are constant matrices. The derivative of a matrix function with respect to a vector is a 3rd-order tensor such that

(13)

Note that when the matrix function is multiplied by a constant matrix, the partial derivatives of are given by:

Left multiplication by a constant matrix
(14)
Right multiplication by a constant matrix
(15)

Finally, another useful operation for our manipulability tracking formulation is the derivative of the inverse of the matrix with respect to the vector , which results in a 3rd-order tensor, namely

(16)

Note that the proposed geometry-aware manipulability tracking, introduced in the section 4, takes inspiration from the computation of the robot Jacobian, which is computed from the 1st-order time derivative of the robot forward kinematics. We use the tensor representation to similarly compute the 1st-order derivative of the function that describes the relationship between a manipulability ellipsoid and the robot joint configuration . 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 Gaussian mixture model 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 Gaussian mixture regression on the SPD manifold.

3.1 Gaussian Mixture Model on SPD manifolds

Similarly to multivariate distribution (see [Zeestraten et al. (2017); Simo-Serra et al. (2017); Dubbelman (2011)]), 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

(17)

where , is the origin in the tangent space and is the covariance tensor.

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

(18)

with being the number of components of the model, and representing the priors such that .

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

(19)
(20)

During the M-step, the mean is first updated iteratively until convergence for each component. The covariance tensor and prior are then updated using the new mean:

(21)
(22)
(23)

3.2 Gaussian Mixture Regression on SPD manifolds

GMR computes the conditional distribution of the joint distribution , where the sub-indices and denote the sets of dimensions that span the input and output variables. We use the following block decomposition of the datapoints, means and covariances:

(24)

where we represent the 4th-order tensor by separating the components of the 3rd- and 4th-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

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

(25)

where the mean is computed iteratively until convergence in its tangent space using

(26)
(27)

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

(28)

The covariance is then computed in the tangent space of the mean

(29)

where is the parallel transported covariance tensor

(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 (which has the structure of a Euclidean vector space) is what allow us to compute the conditional distribution above. Also notice that to parallel transport a 4th-order covariance tensor , the covariance is first converted to a 2nd-order tensor with , as proposed in [Basser and Pajevic (2007)]. We can then compute its eigentensors , which are used to parallel transport the covariance matrix between tangent spaces [Freifeld et al. (2014)]. Let be the -th parallel transported eigentensor with (7) and the -th eigenvalue. The parallel transported 4th-order covariance tensor is then obtained with (see [Jaquier and Calinon (2017)] for more details)

(31)

3.3 Manipulability Learning Example with 2 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 and robot manipulability ellipsoid , at each time step . 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.  (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 computed from a classical GMR as . As secondary task, the robot was also required to vary its joint configuration for matching desired manipulability ellipsoids , estimated by GMR over the SPD manifold.

Figure 2(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 2(b) displays the demonstrated ellipsoids (in gray) along with the center of the five components of the GMM encoding . 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.

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.

(a)
(b)
Figure 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 of the 5-states GMM in the SPD manifold. Position and time are given in centimeters and seconds, respectively.
(a)
(b)
Figure 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)-top Desired manipulability ellipsoids estimated by GMR. (b)-bottom Influence of GMM components on the time-driven GMR. The colors match the distributions shown in Fig. 2(b).

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

(32)

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

(33)

where is the manipulability Jacobian of an -DoF robot, representing the linear sensitivity of the changes in the robot manipulability ellipsoid to the joint velocity . 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 corresponding to the velocity manipulability ellipsoid is straightforward by using (14) and (15) 444In the remainder of the paper we drop dependencies on to simplify the notation.

(34)

Similarly, the manipulability Jacobian corresponding to the force manipulability ellipsoid is obtained using (14), (15) and (16),

(35)

In a similar fashion, the manipulability Jacobian corresponding to the dynamic manipulability ellipsoid with (as defined in [Yoshikawa (1985a)], where is the robot inertia matrix), is computed as follows

(36)

where

Details on the computation of the derivative of the Jacobian and inertia matrix w.r.t 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 to track a desired manipulability profile. More specifically, by minimizing the norm of the residuals

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

(37)

where is the vectorization of the matrix .

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 and the desired one 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

(38)

where 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 null-space velocity that similarly exploits the geometry of the SPD manifold. Thus, the corresponding controller is given by

(39)
(a)
(b)
Figure 5: (a) Manipulability tracking as 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 respectively depicted in yellow, dark purple, and green. The top rows show close-up plots corresponding to the initial and final manipulability.
Initial Final
Main task
Redundancy resolution
Table 1: Initial and final distances between the current and desired manipulability for the experiments illustrated in Fig. 5.

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

(40)

for third-order tensors and 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 coincide with the desired one , 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. Fig. 5 shows how the manipulator configuration is successfully adjusted so that 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.

Stability analysis

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

(41)

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

(42)

where is a vector field composed of the initial velocities of all geodesics departing from the origin , and is the inner product (3). As proved in [Pait and Colón (2010)], the function (42) is a Lyapunov function for a dynamical system such that if the Lie derivative is negative everywhere except at the origin . To verify this condition, we first express the velocity of the dynamical system (41) in the tangent space of using parallel transport as

(43)

The Lie derivative of the proposed Lyapunov function for the dynamical system (43) is given by

(44)

Therefore, we have

so that the function (42) is a valid Lyapunov function and the controller (38) is asymptotically stable.

Note that the Lyapunov function (42) is similar to the one usually defined to demonstrate the asymptotic stability of the classical inverse kinematic-based velocity controller . In that case, the Lyapunov function is defined as , which is equivalent to the inner product with the error . In the case of manipulability tracking, the inner product is defined in the SPD manifold and the error is computed as . 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.2 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 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

(45)

(see Appendix D for details on the computation of ). So, by minimizing the -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

(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 and the desired one , as previously specified for the velocity-based controller. Moreover, since the first-order time derivative of manipulability ellipsoids lies on the tangent space of the SPD manifold (i.e. the space of symmetric matrices ), the difference between the current manipulability velocity and the desired one is computed as a subtraction in the Euclidean space. Therefore, a reference manipulability acceleration command can be specified by

(47)

which resembles a proportional-derivative controller where and are gain matrices. Then, the reference joint acceleration 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

(48)

where 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 mapped into the Cartesian velocity space through

(49)

the velocity manipulability ellipsoid is given by , which represents the flexibility of the manipulator in generating velocities in Cartesian space considering its maximum joint velocities as illustrated in Figure 5(a). Note that the actuators contribution also has a geometrical interpretation based on the fact that the robot joint position lies on the flat -torus manifold [Park (1995)].

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

(50)

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

(51)

Figure 5(b) shows the effect of including the actuator contribution when tracking a velocity manipulability ellipsoid. Notice that the robot joint significantly moves 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 , where and . Then, the corresponding manipulability Jacobian is given by

Finally, the dynamic manipulability ellipsoid considering the maximum joint torques is with corresponding manipulability Jacobian defined as

(52)
(a)
(b)
Figure 6: Illustration of actuators contribution. (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 Fig. 4(a)) for equal maximum joint velocities (top), highest velocity limit for (middle), and lowest velocity limit for (bottom).
(a)
(b)
(c)
(d)
(e)
Figure 7: Manipulability tracking as main task with diagonal gain matrices defined from different precision tensors. Evolution of the robot posture and corresponding manipulability are respectively shown at top and bottom plots. (a): equal tracking precision for all components. (b) and (c): tracking precision is higher for and , respectively. (d): correlation between and 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 is in seconds.

4.4 Exploiting 4th-order precision matrix as controller gain

An open problem regarding the proposed tracking approach is how to specify the values of the gain matrix , which basically determines how the manipulability tracking error affects the resulting joint velocities. In this sense, we propose to define 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 (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 for a given manipulability tracking task into the controllers defined in Section 4.2. To do so, we define the gain matrix 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 along its two first dimensions, with a proportion defined by

(53)

To show how precision matrices work as controller gains in our manipulability tracking problem, we tested different forms of 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 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 , 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 it matches the whole manipulability ellipsoid. In this case, the precision ratio between the prioritized and the rest of components of the gain matrix is . 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 . Notice 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 Fig. 6(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.

(a)
(b)
Figure 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) is a scalar value, (b) 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.

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 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 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 7(a) and 7(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.

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 , aimed at fulfilling secondary objectives, can be projected into the null-space of our manipulability tracking controller (38) using the null-space operator . Therefore, the resulting redundancy resolution controller is given by

(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 , which is considered as secondary task. Thus, the null-space velocity is defined as a simple proportional controller where is the desired joint configuration and 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 . 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 null-space of the manipulability Jacobian is suitable to carry out a secondary task along with manipulability tracking as main objective.

Figure 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 (green ellipses). 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 red) fully overlap the desired ones, showing a precise manipulability tracking.

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 to 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 obtained via Cholesky decomposition. Thus, the positive-definiteness of the desired manipulability ellipsoids computed as is guaranteed, where 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 . Therefore, we do not consider this approach in the comparison as the desired matrices 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 Fig. 9(a)9(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 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.

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 Fig. 9(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 Fig. 9(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 was chosen here to facilitate the visualization of the results.

(a)
(b)
(c)
Figure 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). (c) Manipulability profiles retrieved by the geometry-aware and Euclidean GMR, shown as green and orange ellipses, 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.

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 two 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 and . We first compare the proposed approach (38) with the following Euclidean manipulability tracking controller

(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. Secondly, we compare the proposed approach to the Cholesky-based Euclidean manipulability controller

(56)

where and matrices are obtained from the Cholesky decomposition such that . This controller ensures that the difference between two manipulability ellipsoids is positive definite, but ignores that they belong to the SPD manifold. For all the following comparisons, the gain matrices are identity matrices.

Figure 11 shows the convergence rate for the proposed geometry-aware controller, the Euclidean-based approach and the Cholesky-based Euclidean formulation. 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 Fig. 10(a)-top, 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 Fig. 10(b)-top, 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 Fig. 10(a)-bottom. When this approximation is not valid (see Fig.