Journal Title Geometry-aware Manipulability Learning ...scalinon/papers/Jaquier-IJRR2020.pdf ·...

Geometry-aware ManipulabilityLearning, Tracking and Transfer

Journal TitleXX(X):1–24c©The Author(s) 2019

Reprints and permission:sagepub.co.uk/journalsPermissions.navDOI: 10.1177/ToBeAssignedwww.sagepub.com/

Noemie Jaquier1, Leonel Rozo2,3, Darwin G. Caldwell3 and Sylvain Calinon1

AbstractBody posture influences human and robots performance in manipulation tasks, as appropriate poses facilitate motionor 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 bedesigned according to different task requirements, such as tracking a desired position or apply a specific force. Inthis context, this article presents a novel manipulability transfer framework, a method that allows robots to learn andreproduce 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 manifoldof symmetric positive definite matrices. Learning is coupled with a geometry-aware tracking controller allowing robotsto 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 feasibilityof the approach.

KeywordsRobot learning, programming by demonstrations, manipulability ellipsoids, manipulability optimization, Riemannianmanifolds, differential kinematics.

1 Introduction

When we perform a manipulation task, we naturally placeour arms (and body) in a posture that is best suited tocarry out the task at hand (see Fig. 1). Such posturevariation is a means through which the motion and strengthcharacteristics of the arms are made compatible with thetask requirements. For example, human arm kinematics playsa central role when humans plan point-to-point reachingmovements, where joint trajectory patterns arise as a functionof the visual target (Morasso 1981), indicating that thetask requirements influence the human arm posture. Thisinsight was also identified in more complex situations,where not only kinematic but also other biomechanicfactors affect the task planning (Cos et al. 2011). Forexample, the human central nervous system plans armmovements considering its directional sensitivity, whichis directly related to the arm posture (Sabes and Jordan1997). This allows humans to be mechanically resistant topotential perturbations coming from obstacles occupying theworkspace. Interestingly, directional preferences of humanarm movements are characterized by a tendency to exploitinteraction torques for movement production at the shoulderor elbow, indicating that the preferred directions are largelydetermined by biomechanical factors (Dounskaia et al.2014).

The robotics community has also been aware of the impactof robot posture on reaching movements and manipulationtasks (e.g., pushing, pulling, reaching). It is well known thatby varying the posture of a robot, we can change the optimaldirections for generating motion or applying specific forces.This has direct implications in hybrid control, since the

controller capability can be fully realized when the optimaldirections for controlling velocity and force coincide withthose dictated by the task (Chiu 1987). In this context, the so-called manipulability ellipsoid (Yoshikawa 1985b) serves asa geometric descriptor that indicates the ability to arbitrarilyperform motion and exert a force along the different taskdirections in a given joint configuration.

Manipulability ellipsoids have been used to measurethe compatibility of robot postures with respect to fineand coarse manipulation (Chiu 1987), and to improveminimum-time trajectory planning using a manipulability-aware inverse kinematics algorithm (Chiacchio 1990).Vahrenkamp et al. (2012) proposed a grasp selectionprocess that favored high manipulability in the robotworkspace. Other works have focused on maximizing themanipulability ellipsoid volume in trajectory generationalgorithms (Guilamo et al. 2006), and task-level robotprogramming frameworks (Somani et al. 2016), to obtainsingularity-free joint trajectories and high task-spacedexterity. Nevertheless, as stated in (Lee 1989), solelymaximizing the ellipsoid volume to achieve high dexterity inmotion may cause a reverse effect on the flexibility in force.

1Idiap Research Institute, CH-1920 Martigny, Switzerland2Bosch Center for Artificial Intelligence, 71272 Renningen, Germany3Department of Advanced Robotics, Istituto Italiano di Tecnologia, 16163Genova, Italy

Corresponding author:Noemie Jaquier, Idiap Research Institute, CH-1920 Martigny, Switzer-landEmail: [email protected]

Prepared using sagej.cls [Version: 2015/06/09 v1.01]

2 Journal Title XX(X)

(a) (b)

Figure 1. Illustration of pushing (a) and pulling (b) tasks forwhich the posture of the human significantly influences his/herability to carry out the task.

The aforementioned approaches do not specify a desiredrobot manipulability for the task. In contrast, Lee and Oh(2016) proposed an optimization method to find reachingpostures for a humanoid robot that achieved desired(manually-specified) manipulability volumes. Similarly, aseries of desired manipulability ellipsoids was predefinedaccording to Cartesian velocity and force requirements indual-arm manipulation tasks (Lee 1989). Note that both (Lee1989) and (Lee and Oh 2016) predetermined the task-dependent robot manipulability, which required a meticulousand demanding analysis about the motion that the robotneeded to perform, which becomes impractical when therobot is required to carry out a large set of different tasks.Furthermore, these approaches overlooked an importantcharacteristic of manipulability ellipsoids, namely, the factthat they lie on the manifold of symmetric positive definite(SPD) matrices. This may influence the optimal robot jointconfiguration for the task at hand.

Other geometric descriptors have been proposed in theliterature to evaluate the velocity or force performanceof robots at a given joint configuration. In contrast tomanipulability ellipsoids that do not fully account forboundary limits in the space of joint velocities or torques,manipulability polytopes provide a linear estimate of theexact joint constraints in task space (Chiacchio et al. 1997;Lee 1997). Moreover, Ajoudani et al. (2015) introduced theconcept of stiffness feasibility region (SFR) to representthe non-polytopic boundary where the realization of adesired Cartesian stiffness matrix is feasible. While thepolytope approaches provide a more accurate estimate ofthe velocity or force generation capabilities of the robotcompared to manipulability ellipsoids, their calculation iscomputationally expensive. SFR is a particular Cartesianstiffness descriptor and therefore does not generalize to otherrobot control settings. Manipulability ellipsoids are easy tocompute, while representing an intuitive estimate of the robotability to perform velocities, accelerations or exert forcesalong the different task directions.

In this article we introduce the novel idea thatmanipulability-based posture variation for task compatibilitycan be addressed from a robot learning from demonstrationperspective. Specifically, we cast this problem as amanipulability transfer between a teacher and a learner.The former demonstrates how to perform a task with

a desired time-varying manipulability profile, while thelatter reproduces the task by exploiting its own redundantkinematic structure so that its manipulability ellipsoidmatches the demonstration. Unlike classical learningframeworks that encode reference position, velocity andforce trajectories, our approach offers the possibilityof transferring posture-dependent task requirements suchas preferred directions for motion and force exertionin operational space, which are encapsulated in thedemonstrated manipulability ellipsoids.

This idea opens two main challenges, namely, (i) how toencode and retrieve a sequence of manipulability ellipsoids,and (ii) how to track a desired time-varying manipulabilityeither as the main task of the robot or as a secondaryobjective. To address the former problem, we proposea tensor-based formulation of Gaussian mixture model(GMM) and Gaussian mixture regression (GMR) thattakes into account that manipulability ellipsoids lie on themanifold of symmetric positive definite (SPD) matrices(see Section 3 for a full description of the model). Thelatter challenge is solved through a manipulability trackingformulation inspired by the classical inverse kinematicsproblem in robotics, where a first-order differentialrelationship between the robot manipulability ellipsoidand the robot joints is established, as explained inSection 4. This relationship also demands to consider thatmanipulability ellipsoids lie on the SPD manifold, whichis here tackled by exploiting tensor-based representationsand differential geometry (see Section 2). The geometry-awareness of our formulations is crucial for achievingsuccessful manipulability transfer, as shown in Section 5.Note that Riemannian geometry has also been successfullyexploited in robot motion optimization (Ratliff et al. 2015)and manipulability analysis of closed chains (Park and Kim1998). For sake of clarity, different aspects of the proposedlearning and tracking approaches are illustrated with simpleexamples using simulated planar robots throughout thearticle.

The proposed approach can be straightforwardly appliedto different types of kinetostatic and dynamic manipulabilitymeasures. This opens the door to manipulability transferscenarios with various types of robots where different taskrequirements at kinematic and dynamic levels can be learnedand successfully transfered between agents of differentembodiments. The functionality of the proposed approach isevaluated in different simulated manipulability tracking tasksinvolving a 16-DoF robotic hand and two legged robots. Thefull manipulability transfer is showcased in a bimanual setupwhere an unplugging task is kinesthetically demonstrated toa 14-DoF dual-arm robot, which then transfers the learnedmodel to a different dual-arm system that reproduces theunplugging task successfully, as described in Section 6.

Early contributions on our learning and tracking frame-works were presented in (Rozo et al. 2017) and (Jaquieret al. 2018), respectively. In (Rozo et al. 2017), the learningapproach provided a sequence of desired manipulabilityellipsoids that a learner robot reproduced using gradient-based nullspace commands. Existing approaches built on theoptimization of manipulability-based indices are not suitableas they do not allow the tracking of specific manipulabilityellipsoids. In (Jaquier et al. 2018), the tracking framework

Prepared using sagej.cls

Geometry-aware Manipulability Transfer 3

used manually-specified robot manipulability ellipsoids forthe task. As mentioned previously, this may be tedious andcumbersome when the robot needs to carry out different andcomplex tasks. Therefore, the integration of the proposedlearning and tracking approaches solves the aforementionedproblems and offers a complete geometry-aware manipu-lability transfer framework where manipulability ellipsoidprofiles are learned from demonstrations and reproducedaccurately. This opens the possibility to transfer posture-dependent task requirements between agents of dissimilarkinematic structures. In particular, this framework also per-mits to transfer other velocity, force or impedance specifica-tions with any priority order with respect to the manipulabil-ity tracking controller.

Beyond the combination of our early contributions onmanipulability learning and tracking, the other contribu-tions of this article are: (i) analyzing the role of the pro-posed differential geometry formulation of the geometry-aware tensor-based GMM/GMR adapted to manipulabilityellipsoids; (ii) extending the geometry-aware manipulabilitytracking control scheme initially designed for kinetostaticmanipulability measures to dynamic measures; (iii) demon-strating the exponential stability of the proposed manipu-lability tracking controllers; (iv) introducing various noveltypes of geometry-aware manipulability tracking schemesand introducing methodologies to consider the robot actua-tors contribution and variability-based tracking precision; (v)analyzing the importance of the geometry-awareness of themanipulability tracking controllers by comparison againststate-of-the-art manipulability-based optimization methods.

A summary video, as well as videos of the illustrativeplanar examples and simulated and real experimentsaccompany 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 ellipsoidsVelocity and force manipulability ellipsoids introducedin (Yoshikawa 1985b) are kinetostatic performance measuresof robotic platforms. They indicate the preferred directions inwhich force or velocity control commands may be performedat a given joint configuration. More specifically, the velocitymanipulability ellipsoid describes the characteristics offeasible motion in Cartesian space corresponding to allthe unit norm joint velocities. The velocity manipulabilityof an n-DoF robot can be found by using the kinematicrelationship between task velocities x and joint velocities q,

x = J(q)q, (1)

where q∈Rn and J ∈R6×n are the joint position andJacobian of the robot, respectively. Moreover, consider theset of joint velocities of constant (unit) norm ‖q‖2 =1describing the points on the surface of a hypersphere inthe joint velocity space, which is mapped into the Cartesianvelocity space R6 with∗

‖q‖2 = qTq = xT(JJT)−1x, (2)

by using the least-squares inverse kinematics relationq=J†x=JT(JJT)−1x. Equation (2) represents the robotmanipulability in terms of motion, indicating the flexibilityof the manipulator in generating velocities in Cartesianspace.†

In the literature, the velocity manipulability ellipsoid isusually defined as (JJT)−1, where the principal axes of theellipsoid coincide with the eigenvectors and their length isequal to the inverse of the square root of the correspondingeigenvalues, i.e., 1√

λi(see e.g. (Chiu 1987)). For the sake

of consistency, we here use an alternative definition of thevelocity manipulability ellipsoid given by M x = JJT. Inthis case, the major axis of the manipulability ellipsoid isaligned to the eigenvector associated with the maximumeigenvalue λmax of M x, whose length equals the squareroot of λmax. Thus, the interpretation and representation ofthe manipulability ellipsoid from the corresponding matrixare facilitated. Note that the major axis of the velocitymanipulability ellipsoid M x = JJT indicates the directionin which the greater velocity can be generated, which isalso the direction in which the robot is more sensitive toperturbations. This occurs due to the principal axes of theforce manipulability being aligned with those of the velocitymanipulability, with reciprocal lengths (eigenvalues) causedby the duality of velocity and force (see (Chiu 1987) fordetails).

Other forms of manipulability ellipsoids exist, suchas the dynamic manipulability (Yoshikawa 1985a), whichgives a measure of the ability of performing end-effectoraccelerations along each task-space direction for a givenset of joint torques. This has shown to be useful whenthe robot dynamics cannot be neglected in highly dynamicmanipulation tasks (Chiacchio et al. 1991b). Recent workshave extended this measure to analyze the robot capacity toaccelerate its center of mass for locomotion stability (Azadet al. 2017; Gu et al. 2015), showing the applicability of theaforementioned tools beyond robotic manipulation.

As mentioned previously, any manipulability ellipsoid Mbelongs to the set of symmetric positive definite (SPD)matrices SD++ which describe the interior of the convex cone.Consequently, our manipulability transfer formulation mustconsider this particular characteristic in order to properlyencode, reproduce and track manipulability ellipsoids. To doso, we here propose geometry-aware formulations of bothlearning and tracking problems by exploiting Riemannianmanifolds and tensor representations, which are introducednext.

2.2 Riemannian manifold of SPD matricesThe set of D×D SPD matrices SD++ is not a vectorspace since it is not closed under addition and scalarproduct (Pennec et al. 2006), and thus the use of classicalEuclidean space methods for treating and analyzing thesematrices is inadequate. A compelling solution is to endowthese matrices with a Riemannian metric so that these form

∗Note that an additional scaling of the joint velocities may be included toconsider actuator boundaries.†Dually, the force manipulability ellipsoid can be computed from the staticrelationship between joint torques and Cartesian forces (Yoshikawa 1985b).


https://sites.google.com/view/manipulability

https://sites.google.com/view/manipulability

https://github.com/NoemieJaquier/Manipulability

https://github.com/NoemieJaquier/Manipulability


(a) (b)

Figure 2. SPD manifold S2++ embedded in its tangent space

Sym2. One point corresponds to a matrix(T11 T12

T12 T22

)∈ Sym2.

Points inside the cone, such as Σ and Λ, belong to S2++. (a) L

lies on the tangent space of Σ such that L = LogΣ(Λ). Theshortest path between Σ and Λ is the geodesic representedas a red curve, which differs from the Euclidean path depictedin yellow. (b) T ∈ TΛM is the result of the parallel transportof T ∈ TΣM from TΣM to TΛM. The coordinate axes of thetangent spaces TΣM and TΛM are represented in dark gray.

a Riemannian manifold.‡ This metric permits to definegeodesics, which are the generalization of straight linesto Riemannian manifolds. Similarly to straight lines inEuclidean space, geodesics are the minimum-length curvesbetween two points on the manifold.

Intuitively, a Riemannian manifold M is a mathematicalspace for which each point locally resembles a Euclideanspace. For each point Σ∈M, there exists a tangent spaceTΣM equipped with a positive definite inner product. In thecase of the SPD manifold, the tangent space at any pointΣ ∈ SD++ is identified by the space of symmetric matricesSymD and the inner product between two matrices T1, T2 ∈TΣM is

〈T1,T2〉Σ = tr(Σ−12T1Σ

−1T2Σ− 1

2 ). (3)

The space of SPD matrices can be represented as theinterior of a convex cone embedded in its tangent spaceSymD. To utilize these tangent spaces, we need mappingsback and forth between TΣM andM, which are known asexponential and logarithmic maps.

The exponential map ExpΣ : TΣM→Mmaps a point Lin the tangent space to a point Λ on the manifold, so that itlies on the geodesic starting at Σ in the direction L and suchthat the distance between Σ and Λ is equal to the norm ofL in the tangent space. The inverse operation is called thelogarithmic map LogΣ :M→ TΣM. Both operations areillustrated in Fig. 2a.

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

d(Λ,Σ) = ‖ log(Σ−12 ΛΣ−

12 )‖F, (4)

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

Λ = ExpΣ(L) = Σ12 exp(Σ−

12LΣ−

12 )Σ

12 , (5)

L = LogΣ(Λ) = Σ12 log(Σ−

12 ΛΣ−

12 )Σ

12 , (6)

where exp(·) and log(·) are the matrix exponential andlogarithm functions.

Another useful operation over manifolds is the paralleltransport ΓΣ→Λ : TΣM→ TΛM, which moves elements

between tangent spaces such that the angle between twoelements in the tangent space remains constant (see Fig. 2b).The parallel transport of T ∈ TΣSD++ to TΛSD++ is given by

T = ΓΣ→Λ(T ) = AΣ→Λ T ATΣ→Λ, (7)

with AΣ→Λ = Λ12 Σ−

12 (see (Sra and Hosseini 2015) for

details). This operation is exploited when it is necessary tomove SPD matrices along a curve on the nonlinear manifold.Finally, for a complete introduction to differential geometryand Riemannian manifolds, we refer the interested reader toe.g., (do Carmo 1992; Lee 2012).

In this article, we first exploit the Riemannian manifoldframework to propose a probabilistic learning model thatencodes and retrieves manipulability ellipsoids consideringthat these belong to SD++. Secondly, we take advantageof the Riemannian geometry to compute the differencebetween manipulability ellipsoids in the tracking problem,and consequently propose novel velocity- and acceleration-based controllers. This geometry-aware approach proves tobe crucial for learning and tracking manipulability ellipsoidsin terms of accuracy, stability and convergence, beyondproviding an appropriate mathematical treatment of bothproblems.

2.3 Tensor representationTensors are generalization of matrices to arrays of higherdimensions (Kolda and Bader 2009), where vectors andmatrices may respectively be seen as 1st and 2nd-ordertensors. Tensor representation permits to represent andexploit data structure of multidimensional arrays. In thisarticle, such representation is first used in the learningprocess to encode a distribution of manipulability ellipsoids(as explained in Section 3). Then, tensor representationis also exploited in the proposed manipulability trackingformulation to find the first-order differential relationshipbetween the robot joints and the robot manipulabilityellipsoid (1st- and 2nd-order tensors, respectively), whichresults in a 3rd-order tensor (see Section 4). To do so, we firstintroduce the tensor operations needed for our mathematicaltreatment.

2.3.1 Tensor product The tensor product is a multilineargeneralization of the outer product of two vectors x⊗ y =xyT. The tensor product of two tensors X ∈ RI1×...×IM ,Y ∈ RJ1×...×JN is X ⊗Y ∈ RI1×...×IM×J1×...×JN withelements

(X ⊗Y)i1,...,iM ,j1,...,jN = xi1,...,iM yj1,...,jN . (8)

2.3.2 n-mode product The multiplication of a tensorX ∈RI1×...×In×...×IN by a matrix A∈RJ×In , known as then-mode product is defined as

Y = X ×n A ⇐⇒ Y(n) = AX(n), (9)

where X(n)∈RIn×I1I2...IN is the n-modematricization or unfolding of tensor X . Element-wise, this n-mode product can be written as(X ×n A)i1...in−1jnin+1...iN =

∑in

ajninxi1...in−1inin+1...iN .

‡The original cone of SPD matrices has been changed into a regular andcomplete (but curved) manifold with an infinite development in each of itsD(D + 1)/2 directions (Pennec et al. 2006).



2.3.3 Tensor contraction As described in (Tyagi andDavis 2008), we denote the element (i, j, k, l) of a 4th-ordertensor S by Sklij with two covariant indices i, j and twocontravariant indices k, l. The element (k,l) of a matrix Xis denoted by Xkl with two covariant indices k, l. A tensorcontraction between two tensors is performed when one ormore contravariant and covariant indices are identical. Forexample, the tensor contraction of S ∈ RD×D×D×D andX ∈ RD×D is written as

SX =

D∑k=1

D∑l=1

SklijXkl. (10)

2.3.4 Tensor covariance Similarly to the covarianceof vectors, the 2M th-order covariance tensor S ∈RI1×...×IM×I1×...×IM of centered tensors Xn ∈ RI1×...×IMis given by

S =1

N − 1

N∑n=1

Xn ⊗Xn, (11)

where N is the total number of datapoints. This definitionis used in the formulation of tensor-variate normaldistributions.

2.3.5 Normal distribution of symmetric matrices Thetensor-variate normal distribution of a random 2nd-ordersymmetric matrix X ∈ SymD with mean Ξ ∈ SymD andcovariance S ∈ RD×D×D×D is defined as (Basser andPajevic 2007)

N (X|Ξ,S) =1√

(2π)D|S|e−

12 (X−Ξ)S−1(X−Ξ), (12)

with D = D +D(D − 1)/2. This formulation is used inSection 3 to formulate a normal distribution of SPD matricesnecessary to adapt the formulations of GMM and GMR toencode and retrieve manipulability ellipsoids.

2.3.6 Derivative of a matrix w.r.t a vector In the followingidentities, the matrix Y ∈RI×J is a function of x∈RK ,while A∈RL×I and B∈RJ×L are constant matrices. Thederivative of a matrix function Y with respect to a vector xis a 3rd-order tensor ∂Y∂x ∈R

I×J×K such that(∂Y

∂x

)ijk

=∂yij∂xk

. (13)

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

Left multiplication by a constant matrix

∂AY

∂x=∂Y

∂x×1 A (14)

Right multiplication by a constant matrix

∂Y B

∂x=∂Y

∂x×2 B

T (15)

Finally, another useful operation for our manipulabilitytracking formulation is the derivative of the inverse of thematrix Y with respect to the vector x, which results in a 3rd-order tensor, namely

∂Y −1

∂x= −∂Y

∂x

T

×1 Y−1 ×2 Y

−T (16)

Note that the proposed geometry-aware manipulabilitytracking, introduced in the section 4, takes inspirationfrom the computation of the robot Jacobian, which iscomputed from the 1st-order time derivative of the robotforward kinematics. We use the tensor representation tosimilarly compute the 1st-order derivative of the function thatdescribes the relationship between a manipulability ellipsoidM and the robot joint configuration q. Mathematical proofsfor (14), (15) and (16) are given in Appendix A.

3 Learning Manipulability EllipsoidsThe first open problem in manipulability transfer is to appro-priately encode sequences of demonstrated manipulabilityellipsoids and subsequently retrieve a desired manipulabilityprofile that encapsulates the patterns observed during thedemonstrations. In order to describe how we tackle thisproblem, we first introduce the mathematical formulationof a Gaussian mixture model that encodes a set of demon-strated manipulability ellipsoids over the manifold of SPDmatrices. This probabilistic formulation models the trend ofthe demonstrated manipulability sequences along with theirvariability, reflecting their dispersion through the differentdemonstrations. After, we describe how a distribution ofthe desired manipulability ellipsoids can be retrieved viaGaussian mixture regression on the SPD manifold.

3.1 Gaussian Mixture Model on SPDmanifolds

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

NM(X|Ξ,S) =1√

(2π)D|S|e−

12 LogΞ(X)S−1 LogΞ(X),

(17)where X ∈M, Ξ ∈M is the origin in the tangent spaceand S ∈ TΞM is the covariance tensor.

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

p(X) =

K∑k=1

πkNM(X|Ξk,Sk), (18)

with K being the number of components of the model, andπk representing the priors such that

∑k πk = 1.

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

p(k|Xi) =πk NM(Xi|Ξk,Sk)∑Kj=1 πj NM(Xi|Ξj ,Sj)

, (19)

Nk =

N∑i=1

p(k|Xi). (20)

During the M-step, the mean Ξk is first updated iterativelyuntil convergence for each component. The covariance tensor



Sk and prior πk are then updated using the new mean:

Ξk ←1

NkExpΞk

(N∑i=1

p(k|Xi) LogΞk(Xi)

), (21)

Sk ←1

Nk

N∑i=1

p(k|Xi) LogΞk(Xi)⊗ LogΞk

(Xi), (22)

πk ←NkN. (23)

3.2 Gaussian Mixture Regression on SPDmanifolds

GMR computes the conditional distribution p(XOO|XII) ofthe joint distribution p(X), where the sub-indices I and Odenote the sets of dimensions that span the input and outputvariables. We use the following block decomposition of thedatapoints, means and covariances:

X =

(XII 0

0 XOO

),Ξ =

(ΞII 00 ΞOO

),

S =

SIIII 0 0 00 SOOII 0 00 0 SIIOO 00 0 0 SOOOO

, (24)

where we represent the 4th-order tensor by separating thecomponents of the 3rd- and 4th-modes with horizontaland vertical bars, respectively. With this decomposition,manifold functions can be applied individually on input andoutput parts, for example the exponential map would be

ExpΞk(X) =

(ExpΞII

(XII) 00 ExpΞOO

(XOO)

).

Similarly to GMR in Euclidean space (Rozo et al.2016) and in manifolds where data are represented byvectors (Zeestraten et al. 2017), GMR on SPD manifoldapproximates the conditional distribution by a singleGaussian

p(XOO|XII) ∼ N (ΞOO, SOO

OO), (25)

where the mean ΞOO is computed iteratively untilconvergence in its tangent space using

∆k = LogΞOO(ΞOO,k)− SIIOO,k SII−1

II,k LogXII(ΞII,k),(26)

ΞOO ← ExpΞOO

(∑k

hk∆k

), (27)

with hk describing the responsibilities of the GMMcomponents in the regression, namely

hk =πk N (XII|ΞII,k,SIIII,k)∑Kj=1 πj N (XII|ΞII,j ,SIIII,j)

. (28)

The covariance SOO

OO is then computed in the tangent spaceof the mean

SOO

OO =∑k

hk

(SOOOO,k − SIIOO,kS

II−1

II,k SOOII,k + ∆k ⊗∆k

)− ΞOO ⊗ ΞOO, (29)

where S is the parallel transported covariance tensor

S = ΓΞ→X(S) with X =

(XII 0

0 ΞOO

). (30)

This covariance has been typically used to define thecontroller gains of robotic systems for trajectory trackingproblems (see also Section 4.4). Note that the definitionof the tangent space TΞM (which has the structure of aEuclidean vector space) is what allow us to compute theconditional distribution above. Also notice that to paralleltransport a 4th-order covariance tensor S ∈ RD×D×D×D,the covariance is first converted to a 2nd-order tensor Σ ∈RD×D with D = D +D(D − 1)/2, as proposed in (Basserand Pajevic 2007). We can then compute its eigentensorsVk, which are used to parallel transport the covariancematrix between tangent spaces (Freifeld et al. 2014).Let Vk = ΓΞ→X(Vk) be the k-th parallel transportedeigentensor with (7) and λk the k-th eigenvalue. The paralleltransported 4th-order covariance tensor is then obtained with(see (Jaquier and Calinon 2017) for more details)

ΓΞ→X(S) =∑k

λkVk ⊗ Vk. (31)

3.3 Manipulability Learning Example with 2Planar Robots

In order to illustrate the functionality of the proposedlearning approach, we carried out an experiment usinga couple of simulated planar robots with dissimilarembodiments and a different number of joints. The centralidea is to teach a redundant robot to track a referencetrajectory in Cartesian space with a desired time-varyingmanipulability 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 positionxt and robot manipulability ellipsoid Mt(q), at each timestep t. The collected time-aligned data were split intotwo training datasets of time-driven trajectories, namelyCartesian position and manipulability. We trained a classicalGMM over the time-driven Cartesian trajectories and ageometry-aware GMM over the time-driven manipulabilityellipsoids, using models with five components, i.e. K=5(the number was selected by the experimenter).

During the reproduction phase, a 5-DoF student robotexecuted the time-driven task by following a desiredCartesian trajectory xt computed from a classical GMRas xt ∼ P(x | t). As secondary task, the robot was alsorequired to vary its joint configuration for matchingdesired manipulability ellipsoids Mt ∼ P(M |t), estimatedby GMR over the SPD manifold.

Figure 3a shows the four demonstrations carried outby the 3-DoF robot, where both the Cartesian trajectoryand manipulability ellipsoids are displayed. Note that therecorded manipulability ellipsoids slightly change acrossdemonstrations as a side effect of the variation observedin both the initial end-effector position and the generatedtrajectory. Figure 3b displays the demonstrated ellipsoids(in gray) along with the center Ξk of the five componentsof the GMM encoding XM . These are centered at theend-effector position recovered by the classical GMR for



(a) Demonstrations (b) Learned model

Figure 3. (a) Four demonstrations of a 3-DoF planar robottracking a C-shape trajectory. The end-effector path (light graysolid lines) and the manipulability ellipsoids at different timesteps are shown for all the demonstrations. (b) Demonstratedmanipulability ellipsoids (in gray) and centers Ξk of the 5-statesGMM in the SPD manifold. Position x and time t are given incentimeters and seconds, respectively.

(a) Desired execution (b) Time profile

Figure 4. (a) Desired execution of a C-shape tracking task.The desired Cartesian trajectory and manipulability profile aredepicted as a black curve and green ellipsoids. (b)-top Desiredmanipulability ellipsoids estimated by GMR. (b)-bottom Influenceof GMM components on the time-driven GMR. The colors matchthe distributions shown in Fig. 3b.

the corresponding time steps represented in the geometry-aware GMM. Figure 4 shows the desired Cartesian trajectoryand manipulability ellipsoid profile respectively estimatedby classical GMR and GMR in the SPD manifold. Bothmanipulability and Cartesian path are references to betracked by the student robot.

These results validate that the proposed learning frame-work permits to learn and plan the reproduction of referencetrajectories, while fulfilling additional task requirementsencapsulated in a profile of desired manipulability ellipsoids.In Section 4, we develop a manipulability tracking formula-tion that will then be used by the 5-DoF student robot to trackthe desired manipulability profile obtained in the learningphase.

4 Tracking Manipulability EllipsoidsSeveral robotic manipulation tasks may demand the robot totrack a desired trajectory with certain velocity specifications,or apply forces along different task-related axes. Theserequirements are more easily achieved if the robot adoptsa posture that suits velocity or force control commands. Inother 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 desiredmanipulability ellipsoids that are compatible with the taskrequirements. In this section, we introduce an approachthat addresses this problem by exploiting the mathematicalconcepts presented in Section 2.

4.1 Manipulability JacobianGiven a desired profile of manipulability ellipsoids, the goalof the robot is to adapt its posture to match the desiredmanipulability, either as its main task or as a secondaryobjective. We here propose a formulation inspired by theclassical inverse kinematics problem in robotics, whichpermits to compute the joint angle commands to track adesired manipulability ellipsoid.

First, the manipulability ellipsoid is expressed as afunction of time

M(t) = f(J(q(t)

)), (32)

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

∂M(t)

∂t=∂f(J(q))

∂q×3

∂q(t)

∂t

T

= J (q)×3 qT, (33)

where J ∈ R6×6×n is the manipulability Jacobian ofan n-DoF robot, representing the linear sensitivity ofthe changes in the robot manipulability ellipsoid M =∂M(t)∂t to the joint velocity q = ∂q(t)

∂t . Note that thecomputation of the manipulability Jacobian depends on thetype of manipulability ellipsoid that is used. We develophere the expressions for the force, velocity and dynamicmanipulability ellipsoids.

The derivation of the manipulability Jacobian J x

corresponding to the velocity manipulability ellipsoidM x = JJT is straightforward by using (14) and (15) §

J x =∂J

∂q×2 J +

∂JT

∂q×1 J . (34)

Similarly, the manipulability Jacobian J F correspondingto the force manipulability ellipsoid MF = (JJT)−1 isobtained using (14), (15) and (16),

J F = −(∂J

∂q×2 J +

∂JT

∂q×1 J

)×1 M

F ×2 MF .

(35)In a similar fashion, the manipulability Jacobian J x cor-

responding to the dynamic manipulability ellipsoid M x =ΥΥT with Υ = JΛ(q)−1 (as defined in (Yoshikawa1985a), where Λ(q) is the robot inertia matrix), is computedas follows

J x =∂Υ

∂q×2 Υ +

∂ΥT

∂q×1 Υ, (36)

§In the remainder of the article we drop dependencies on q to simplify thenotation.



where

∂Υ

∂q=∂J

∂q×2 Λ−T +

∂Λ−1

∂q×1 J

=∂J

∂q×2 Λ−T − ∂Λ

∂q×1 Υ×2 Λ−T.

Details on the computation of the derivative of theJacobian and inertia matrix w.r.t the joint angles are givenin Appendices B and C.

4.2 Geometry-aware manipulability trackingformulation

4.2.1 Velocity-based controller A solution to control arobot so that it tracks a desired end-effector trajectory isto compute the desired joint velocities using the inversekinematics formulation derived from (1). We use herea similar approach to compute the joint velocities q totrack a desired manipulability profile. More specifically, byminimizing the `2 norm of the residuals

minq‖M −J ×3 q

T‖ = minq‖vec(M)−J T

(3)q‖,

we can compute the required joint velocities of the robot totrack a profile of desired manipulability ellipsoids as its maintask with

q = (J †(3))Tvec(M), (37)

where vec(M) is the vectorization of the matrix M .Note that (37) allows us to define a controller to track

a reference manipulability ellipsoid as main task, similarlyas the classical velocity-based control that tracks a desiredtask-space velocity. To do so, we propose to use a geometry-aware similarity measure to compute the joint velocitiesnecessary to move the robot towards a posture where thematch between the current manipulability ellipsoid Mt andthe desired one Mt is maximum. Specifically, the differencebetween manipulability ellipsoids is computed using thelogarithmic map (6) on the SPD manifold. Therefore, thecorresponding controller is given by

qt = (J †(3))T KM vec

(LogMt

(Mt)), (38)

where KM is a gain matrix.Alternatively, for the case in which the main task of

the robot is to track reference trajectories in the form ofCartesian positions or force profiles, the tracking of a profileof manipulability ellipsoids is assigned a secondary role.Thus, the robot task objectives are to track the referencetrajectories while exploiting the kinematic redundancyto minimize the difference between current and desiredmanipulability ellipsoids. In this situation, a manipulability-based redundancy resolution is carried out by computing anullspace velocity that similarly exploits the geometry of theSPD manifold. Thus, the corresponding controller is givenby

qt = J†Kx (xt − xt)

+ (I − J†J) (J †(3))T KM vec

(LogMt

(Mt)). (39)

(a) (b)

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

Table 1. Initial and final distances d(M ,Mt) between thecurrent and desired manipulability for the experiments illustratedin Fig. 5.

Initial FinalMain task 1.342 0.199

Redundancy resolution 2.194 0.955

Note that matricization and vectorization operationscan be defined using Mandel notation to alleviatethe computational cost of the controllers using tensorrepresentations, such that

X(3) =

vec (X :,:,1)T

...vec (X :,:,K)

T

and vec

((α ββ γ

))=

αγ√2β

,

(40)for 2×2×K third-order tensors and 2×2 matrices.

In order to show the functionality of the proposedapproach where the goal of the robot is to reproduce agiven manipulability ellipsoid either as its main task oras a secondary objective, we carried out experiments witha simulated 4-DoF planar robot. In the first case, therobot is required to vary its joint configuration to make itsmanipulability ellipsoid Mt coincide with the desired oneM , 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 jointsto match the desired manipulability ellipsoid. Fig. 5 showshow the manipulator configuration is successfully adjustedso that Mt ' M when the manipulability ellipsoid trackingis considered as the main task or as a secondary objective(see Table 1). These results show that our geometry-awarecontrollers inspired by the inverse kinematics formulation



are suitable to solve the manipulability ellipsoid trackingproblem.

Stability analysis We here analyze the stability propertiesof the proposed manipulability tracking controller giventhe geometry of the underlying manifold. First of all, notethat the dynamical system operated by the controller (38)corresponds to

M = kM LogM (M), (41)

where the controller gain is assumed to be a positive scalarvalue for sake of simplicity. Then, we select the Lyapunovfunction V as

V (M) = 〈F ,F 〉M , (42)

where F = LogM (M) is a vector field composed of theinitial velocities of all geodesics departing from the originM , and 〈·, ·〉M is the inner product (3). As proved in (Paitand Colon 2010), the function (42) is a Lyapunov functionfor a dynamical system M = h(M) such that h(M) =0 if the Lie derivative LhV (M) = 2〈h,F 〉M is negativeeverywhere except at the origin M . To verify this condition,we first express the velocity of the dynamical system (41) inthe tangent space of M using parallel transport as

ΓM→M (M) = −kM LogM (M). (43)

The Lie derivative LhV of the proposed Lyapunov functionfor the dynamical system (43) is given by

LhV (M) = 2〈−kM LogM (M),LogM (M)〉M= −2kM 〈LogM (M),LogM (M)〉M= −2kMV. (44)

Therefore, we have

V (M) > 0, LhV (M) < 0 ∀ M 6= M ,

V (M) = LhV (M) = 0 ⇐⇒ M = M ,

so that the function (42) is a valid Lyapunov function.Moreover, by observing that V (M) = d2(M ,M) withd(·, ·) the affine-invariant distance (4), we have

c1d2(M ,M) ≤ V (M) ≤ c2d2(M ,M),

LhV (M) ≤ −c3d2(M ,M),

where 0 < c1 ≤ 1, c2 ≥ 1, c3 = 2kM > 0. This implies thatthe controller (38) is exponentially stable (see e.g., (Wu2020)). It can be easily shown that this result holds withc3 = 2λmin(KM ) for a positive-definite controller gainmatrix KM , where λmin(·) returns the minimum eigenvalueof the matrix.

Note that the Lyapunov function (42) is similar to theone usually defined to demonstrate the exponential stabilityof the classical inverse kinematic-based velocity controllerqt = J†Kx (xt − xt). In that case, the Lyapunov functionis defined as V (x)=(x− x)T(x− x), which is equivalentto the inner product 〈e, e〉 with the error e = x− x. In thecase of manipulability tracking, the inner product 〈·, ·〉 isdefined in the SPD manifold and the error e is computedas LogM (M). Finally, it is worth highlighting that whenthe manipulability tracking is assigned a secondary role, thecontroller (39) does not influence the stability of the maintask of the robot as the manipulability-based redundancyresolution is carried out in the corresponding nullspace.

4.2.2 Acceleration-based controller Similarly to thevelocity-based controller, we propose a geometry-awareacceleration-based controller that allows the computationof the joint accelerations q required to track a desiredmanipulability trajectory (i.e. desired manipulability andmanipulability velocity profiles). The approach is inspiredby the inverse kinematics formulation and its differentialrelationships used to compute the joint accelerationsnecessary to track desired end-effector positions andvelocities.

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

∂2M(t)

∂t2= J (q)×3 q

T + J (q)×3 qT, (45)

(see Appendix D for details on the computation of J (q)).So, by minimizing the `2-norm of the residuals, we cancompute the required joint accelerations of the robot to tracka desired trajectory of manipulability ellipsoids as its maintask with

q = (J †(3))T(

vec(M)− JT

(3)q). (46)

Similarly as in the classical acceleration-based controllerthat tracks a desired end-effector trajectory, we can definea controller to track a reference manipulability ellipsoidtrajectory based on (46). To do so, we exploit the geometryof the SPD manifold to compute the difference betweenthe current manipulability ellipsoid Mt and the desiredone Mt, as previously specified for the velocity-basedcontroller. Moreover, since the first-order time derivative ofmanipulability ellipsoids lies on the tangent space of the SPDmanifold (i.e. the space of symmetric matrices SymD), thedifference between the current manipulability velocity Mt

and the desired one M t is computed as a subtraction inthe Euclidean space. Therefore, a reference manipulabilityacceleration command can be specified by

vec(Mt) = Kpvec(

LogMt(Mt)

)+ Kdvec

(Mt − Mt

),

(47)which resembles a proportional-derivative controller whereKp and Kd are gain matrices. Then, the reference jointacceleration q can be computed using (46) and (47). Notethat this reference joint acceleration can correspond to a maintask of the robot or to a secondary tracking objective. In thelatter case, a manipulability-based redundancy resolution canalso be implemented in a similar way as (39).

4.3 Actuators contributionIn many practical applications, the joint velocities of therobot are limited. The definition of manipulability ellipsoidcan then be extended to include these actuation constraints,as shown in (Lee 1997). We here provide the definition ofthe force, velocity and dynamic manipulability ellipsoids andthe corresponding manipulability Jacobians considering jointactuation constraints.

To include the joint velocity constraints of the robot in thedefinition of the velocity manipulability ellipsoid, we use the



following weighted forward kinematics formulation

x = (JW q)︸︷︷︸J

(W q−1q)︸︷︷︸˜q

, (48)

where W q = diag(q1,max, . . . , qn,max) is a diagonal matrixwhose elements correspond to the maximum joint velocitiesof the robot. Then, considering the set of joint velocitiesof constant unit norm ‖˜q‖ = 1 mapped into the Cartesianvelocity space through

‖˜q‖2 = ˜qT˜q = xT(J JT)−1x, (49)

the velocity manipulability ellipsoid is given by M x =J JT = JW qW qTJT, which represents the flexibility ofthe manipulator in generating velocities in Cartesian spaceconsidering its maximum joint velocities as illustrated inFigure 6a. Note that the actuators contribution W qW qT alsohas a geometrical interpretation based on the fact that therobot joint position q lies on the flat n-torus manifold (Park1995).

By following the methodology of Section 4.1, the changein the robot manipulability ellipsoid is related to the jointvelocity via

∂M(t)

∂t= J (q)×3 q

T. (50)

Therefore, the velocity manipulability Jacobian includingjoint velocity limits is given by

Jx

=∂J

∂q×2 JW

qW qT +∂JT

∂q×1 JW

qW qT. (51)

Figure 6b shows the effect of including the actuatorcontribution when tracking a velocity manipulabilityellipsoid. Notice that the robot joint q1 significantly moveswhen given the highest velocity limit. In contrast, itsinfluence on the manipulability tracking task is minimalwhen given the lowest velocity limit. This demonstrates theimportance of considering the robot actuator specificationswhen tracking manipulability ellipsoids in real platforms.

In a similar way, the force manipulability ellip-soid considering the maximum joint torques is definedas MF =(JΩτJT)−1, where Ωτ =(W τW τT)−1 andW τ = diag(τ1,max, . . . , τn,max). Then, the correspondingmanipulability Jacobian is given by

JF

=−(∂J

∂q×2 JΩτ +

∂JT

∂q×1 JΩτ

)×1 M

F ×2 MF .

Finally, the dynamic manipulability ellipsoid consideringthe maximum joint torques is M x = ΥΩτ−1ΥT withcorresponding manipulability Jacobian defined as

Jx

=∂Υ

∂q×2 ΥΩτ−1 +

∂ΥT

∂q×1 ΥΩτ−1. (52)

4.4 Exploiting 4th-order precision matrix ascontroller gain

An open problem regarding the proposed tracking approachis how to specify the values of the gain matrix KM , whichbasically determines how the manipulability tracking error

(a) (b)

Figure 6. Illustration of actuators contribution. (a) Velocitymanipulability ellipsoids obtained when setting a maximum jointvelocity, for each joint, five times higher than the rest. Themanipulability corresponding to equal maximum joint velocity isshown in gray. (b) Joint trajectories obtained with manipulabilitytracking (as in Fig. 5a) for equal maximum joint velocities (top),highest velocity limit for q1 (middle), and lowest velocity limit forq1 (bottom).

affects the resulting joint velocities. In this sense, we proposeto define KM as a precision matrix, which describes howaccurately the robot should track a desired manipulabilityellipsoid. In learning from demonstration applications, suchgain matrix would typically be set as proportional to theinverse of the observed covariance S (see Section 3.2). Thisencapsulates variability information of the task to be learned.Our goal here is to exploit this information to demand therobot a high precision tracking for directions in which lowvariability is observed, and vice-versa.

We therefore introduce the required precision S−1 fora given manipulability tracking task into the controllersdefined in Section 4.2. To do so, we define the gain matrixKM as a function of the precision tensor. Specifically, wedefine the controller gain matrix as a full SPD matrix, whichis computed from the matricization of the precision tensorS−1 along its two first dimensions, with a proportion definedby

KM ∝ S−1(1,2). (53)

To show how precision matrices work as controller gainsin our manipulability tracking problem, we tested differentforms of KM aimed at reproducing a given manipulabilityellipsoid as a main task with a simulated 4-DoF planar robot.The robot is required to move its joints to track a desiredmanipulability ellipsoid, where the controller gain matrixKM is a diagonal matrix with the diagonal elements of (53)to take into account the variation of each component of themanipulability ellipsoid. We tested four different precisiontensors. First, equal variability for all components of themanipulability ellipsoid matrix is given. Then, the variabilityalong the first or the second main axis of the manipulabilityellipsoid, corresponding to the first and second diagonalelements of the gain matrix KM , is reduced. This meansthat the robot needs to prioritize the tracking of one of theellipsoid main axes over the other. In the fourth test, thevariability of the correlation between the two main axesof the manipulability ellipsoid is lowered. In this last case,



(a) Baseline (b) Precision along x1 (c) Precision along x2 (d) Diagonal precision (e) Trajectories

Figure 7. Manipulability tracking as main task with diagonal gain matrices defined from different precision tensors. The top plotsdepict the end-effector trajectory (solid colored line) and the posture of the robot along with the corresponding manipulability at timet = 0, 0.25 and 1s. The evolution of the manipulability along time is shown in the bottom plots. (a): equal tracking precision for allcomponents. (b) and (c): tracking precision is 10:1 higher for x1 and x2, respectively. (d): correlation between x1 and x2 axes isassigned a high tracking accuracy. (e) Evolution of the robot manipulability and end-effector trajectory for the gain matrices usedin (a)-(d). The colors match those of the previous graphs. Initial and desired manipulability ellipsoids are depicted in dark blue andgreen on all graphs. Time t is in seconds.

the manipulability controller prioritizes the tracking of theellipsoid orientation over the shape.

Figure 7 shows how the manipulator posture is adapted totrack the desired manipulability ellipsoid with a priority onthe component with the lowest variability. Note that whenhigh tracking precision is required for one of the main axesof the ellipsoid, the robot initially seeks to fit the shapeof the ellipsoid along that specific axis, and subsequentlyit matches the whole manipulability ellipsoid. In this case,the precision ratio between the prioritized and the rest ofcomponents of the gain matrix is 10:1. When high trackingprecision is assigned to the correlation of the ellipsoid axes,the robot first tries to align its manipulability with theorientation of the desired ellipsoid, and afterwards the wholemanipulability is matched. In this case, the precision ratiobetween the prioritized correlation and the other componentsof the gain matrix is 3:1. Notice that the precision tensornaturally affects the computed joint velocities required totrack a given ellipsoid, which consequently influences theresulting motion of the end-effector as a function of theprecision constraints, as shown in Fig. 7e. After convergence,the desired manipulability ellipsoid is successfully matchedfor all experiments. These results show that our geometry-aware tracking permits to take into account the variabilityinformation of a task to define the manipulability trackingprecision.

Therefore, our manipulability tracking approach maybe readily combined with the manipulability learningframework introduced in Section 3. In order to illustratethis, we show the reproduction phase of the experimentcarried out in Section 3.2. The 5-DoF student robot wasrequested to track a desired Cartesian trajectory as maintask, while varying its joint configuration for matchingdesired manipulability ellipsoids as secondary task. Thestudent robot used the geometry-aware controller definedby (39), where KM was defined either as a scalar valueor as a diagonal matrix with the diagonal elements of (53)with the precision tensor being equal to the inverse of thecovariance tensor S

OO

OO retrieved by GMR (29). Our goalhere was to exploit the learned variability information ofthe task to demand the robot a high precision tracking

(a) KM as a scalar (b) KM from GMR

Figure 8. Reproductions of a learned C-shape tracking taskwith desired manipulability ellipsoids. The end-effector trajectoryis shown in black solid line, while the desired and reproducedmanipulabilities are depicted in green and dark purple,respectively. (a) KM is a scalar value, (b) KM is the diagonalof the precision tensor retrieved by GMR. The required trackingprecision is higher at the start and end of the task as aconsequence of the low observed variability.

where low variability was observed in the demonstrations,and vice-versa. Successful reproductions of the demonstratedtask using our manipulability-based redundancy resolutioncontroller with scalar and variability-based matrix gains areshown in Figures 8a and 8b, respectively. Note that thevariability-based matrix gain changes the required trackingprecision, where higher precision is enforced only at thebeginning and the end of the task, which results in lowercontrol efforts in between. These results validate that theproposed approach allows the robot to reproduce referenceprofiles of desired manipulability ellipsoids while adaptingthe tracking precision according to the demonstratedrequirements of the task.

4.5 Nullspace of the manipulability JacobianAs traditionally done when designing redundancy resolutioncontrollers, the nullspace of the manipulability Jacobiancan also be exploited to fulfill secondary objectives whenmanipulability tracking is the main task. More specifically,a joint velocity qN , aimed at fulfilling secondary objectives,



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

can be projected into the nullspace of our manipulabilitytracking controller (38) using the nullspace operator(I − (J †(3))

TJ T(3)

). Therefore, the resulting redundancy

resolution controller is given by

qt=(J †(3))T KM vec

(LogMt

(Mt))

+(I − (J †(3))

TJ T(3)

)qN . (54)

In order to show the functionality of this nullspaceoperator, we carried out experiments with a simulated 6-DoF planar robot. The main task of the robot is to tracka desired manipulability ellipsoid while keeping a desiredpose for its first joint q0, which is considered as secondarytask. Thus, the nullspace velocity is defined as a simpleproportional controller qN = KPq (q − qt) where q is thedesired joint configuration and KPq is a matrix gain definedso that only joint position errors in the first joint arecompensated. Figure 9 shows that the black manipulatorconfiguration is adjusted to track the desired manipulabilityellipsoid and keep, as accurately as possible, the desired jointposition for q0. Note that the black robot is able to find analternative joint configuration that permits not only to closelytrack the desired manipulability, but also fulfill secondaryobjectives projected into its nullspace, in contrast to theblue robot which exclusively implements a manipulabilitytracking task. These results show that the nullspace of themanipulability Jacobian is suitable to carry out a secondarytask along with manipulability tracking as main objective.

5 Importance of geometry-awareness

In the previous sections we introduced a geometry-awaremanipulability transfer framework composed of (1) aprobabilistic model that encodes and retrieves manipulabilityellipsoids, and (2) manipulability tracking controllers. Inthis section, we show that the geometry-awareness ofour formulations is crucial for successfully learning andtracking manipulability ellipsoids in addition to providing anappropriate mathematical treatment of both problems.

5.1 LearningWe first evaluate the proposed learning formulationcompared to a framework that ignores that manipulabilityellipsoids belong to the SPD manifold. To do so, weencode a distribution of manipulability ellipsoids with aGMM acting in the Euclidean space and we then retrievedesired manipulability ellipsoids via the correspondingGMR. To ensure the validity of the desired manipulabilityellipsoids, GMM and GMR are performed on lowertriangular matrices L obtained via Cholesky decomposition.Thus, the positive-definiteness of the desired manipulabilityellipsoids computed as M = LLT is guaranteed, where Lis the estimated GMR output. Note that this property isnot guarantee in the case where GMM and GMR acting inthe Euclidean space is applied directly to the manipulabilityellipsoids M . Therefore, we do not consider this approachin the comparison as the desired matrices M may not bemanipulability ellipsoids in some cases.

Figure 10 compares the proposed approach (Section 3)and the manipulability learning using GMM/GMR acting inEuclidean space. The demonstration consists of a time seriesof changing manipulability ellipsoids. For each approach, a1-state GMM is trained and a reproduction is carried out for alonger time period than the demonstration using GMR. Bothgeometry-aware and Euclidean approaches obtain similarmeans of the GMM component (see Fig. 10a, 10b). Thisis due to the fact that the Euclidean mean computed usingthe Cholesky decomposition is a good approximation ofthe mean computed on SD++ if the SPD data are closeenough to each other. However, the covariances of the GMMcomponents of both approaches are not equivalent. Indeed,the covariance of our geometry-aware approach is computedusing the SPD data projected in the tangent space of themean, while that of the Euclidean GMM corresponds tothe covariance of the elements of the vectorized Choleskydecomposition, which ignores the geometry of the SPDmanifold.

The manipulability ellipsoids profiles retrieved by thegeometry-aware and Euclidean GMR are similar around themean of the GMM component, but diverge when movingaway from it (see Fig. 10c). This is because the estimatedoutput in Euclidean space is only a valid approximationfor input data lying close to the mean. In contrast, ourapproach is able to extrapolate the rotating behavior ofthe demonstrated manipulability ellipsoids as the recoveredtrajectory follows a geodesic on the SPD manifold (seeFig. 10b). Note that this is the equivalent to followinga straight line in Euclidean space, which is the expectedresult of a trajectory computed via Gaussian conditioning.This behavior is obtained by parallel transporting theGMM covariances to the tangent space of the meanof the estimated conditional distribution of GMR (30).Therefore, the Euclidean GMR does not recover a trajectoryfollowing a geodesic on the manifold, leading to inconsistentextrapolated manipulability ellipsoids.

The reported results show that our geometry-awareapproach accurately reproduces the behavior of thedemonstrated data, and therefore provides a mathematicallysound method for learning and retrieving manipulabilityellipsoids in the SPD manifold. Note that similar behaviorsare observed for GMM with any number of states, the



(a) Data and GMM (b) GMR profiles in S2++

(c) GMM and retrieved GMR profiles

Figure 10. Importance of geometry in manipulability learningformulations. (a) Demonstrated data (depicted in light gray),and mean of the GMM component for the geometry-aware and Euclidean approaches (overlapping blue and redellipsoids, respectively). (c) Manipulability profiles retrieved bythe geometry-aware and Euclidean GMR, shown as green andorange ellipses, respectively. The time axis is shared with (a).(b) Mean of the GMM component and estimated profiles inthe cone of SPD matrices. The manipulability profile obtainedby our approach, shown in green, follows a geodesic. Theprofile obtained by the Euclidean framework is depicted by theorange 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), doesnot correspond to the trajectory obtained with the Euclideanframework. Thus, the Euclidean approach is geometricallyflawed.

numberK = 1 was chosen here to facilitate the visualizationof the results.

5.2 Tracking5.2.1 Comparisons with Euclidean tracking After show-ing the importance of geometry for learning manipulabilityellipsoids, we compare the proposed tracking formulationagainst a controller ignoring the geometry of SPD matrices(i.e., treating the problem as Euclidean). Moreover, we eval-uate our controller when the tracking of manipulability ellip-soids is assigned a secondary role. This evaluation comparesour formulation against three Euclidean controllers, and thegradient-based approach in (Rozo et al. 2017). For the casein which the manipulability tracking is the main objective,we consider a 4-DoF planar robot that is required to tracka desired manipulability ellipsoid by minimizing the errorbetween its current and desired manipulability ellipsoids Mand M . We first compare the proposed approach (38) withthe following Euclidean manipulability tracking controller

qt = (J †(3))TKMvec(Mt −Mt), (55)

where the difference between two manipulability ellipsoidsis computed in Euclidean space, i.e., ignoring thatmanipulability ellipsoids belong to the set of SPD matrices.Secondly, we compare the proposed approach to the

Cholesky-based Euclidean manipulability controller

qt = (J †(3))TKMvec(∆Lt∆LT

t ), (56)

where ∆L = L−L and matrices L are obtained from theCholesky decomposition such that M = LLT. This con-troller ensures that the difference between two manipulabil-ity ellipsoids is positive definite, but ignores that they belongto the SPD manifold. For completeness, we also compareour approach with the Cholesky-Jacobian-based Euclideanmanipulability controller

qt = (J †chol(3))TKM vec(L−L), (57)

where Jchol = δLδq = δL

δMJ is the Cholesky-based manipu-

lability Jacobian, so that L = Jchol ×3 qT. This approach

tracks a desired manipulability solely through its Choleskydecomposition with an adapted manipulability Jacobian.Similarly to (56), it ensures the positive-definiteness ofmanipulability ellipsoids, but ignores that they belong to theSPD manifold. For all the following comparisons, the gainmatrices KM are identity matrices.

Figure 11 shows the convergence rate for the proposedgeometry-aware controller, the Euclidean-based approach,the Cholesky-based Euclidean and Cholesky-Jacobian-basedEuclidean formulations. Two tests were carried out byvarying the initial configuration of the robot and the desiredmanipulability ellipsoid. In the first case, the Euclideanand geometry-aware formulations converge to similar robotjoint configurations with a distance between the currentand desired manipulability close to zero (see Fig. 11a-left, middle and Table 2). However, in the second test, theEuclidean formulation induces a sudden change in the jointconfiguration, resulting in an abrupt increase on the errormeasured between the current and desired manipulabilityellipsoids (see Fig. 11b-left, middle). In real scenarios,such unstable robot behavior would certainly be harmfuland unsafe. This erroneous tracking performance can beexplained by the fact that the Euclidean path between twoSPD matrices is a valid approximation of the geodesiconly if these are close enough to each other, as shown inFig. 11a-right. When this approximation is not valid (seeFig. 11b-right), the Euclidean controller outputs inconsistentreference joint velocities that destabilize the robotic system,therefore failing to track the desired manipulability. Notethat the Cholesky-based Euclidean formulation does notconverge in both cases (see Table 2) and induces a suddenchange in joint configuration of the robot in the secondscenario, similarly to the Euclidean formulation. This can beexplained by the fact that the path induced by this methodis not close to geodesics on the SPD manifold as shown byFig. 11-right. As opposed to the two Euclidean formulations,the Cholesky-Jacobian-based Euclidean controller does notinduce unstable robot behaviors and converges towards thedesired manipulability ellipsoid for both cases. However,this method shows a poor convergence rate compared to ourgeometry-aware approach, as shown by Fig. 11-left. Thiscan be explained by the fact that, although this approachgenerates curved paths on the SPD manifold, these paths donot resemble geodesics and tend to induce detours to reachthe desired manipulability ellipsoid (see Fig. 11-left). This



(a)

(b)

Figure 11. Performance of different manipulability tracking formulations. The left graphs show the affine-invariant distance betweenthe current and desired manipulability ellipsoids over time. The distances for the Euclidean, Cholesky-based Euclidean, Cholesky-Jacobian-based Euclidean and geometry-aware approaches are respectively depicted in blue, yellow, lila and red. The middle graphsdisplay the initial and final robot postures and the final manipulability ellipsoids. The initial posture is depicted in light gray, while thefinal posture and corresponding manipulability for the three methods are depicted in the same color as the distances. The desiredmanipulability is depicted in green. Middle-(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 yellowand olive, respectively for (56). The right graphs depict the evolution of the manipulability ellipsoids in the SPD manifold. The colorscorrespond to those of the previous graphs with the green dot representing the desired manipulability. The isolated light blue andolive dots in the bottom-(b) graph represent the manipulability ellipsoids after the abrupt changes in the robot joint configuration.

Table 2. Final distances d(M ,Mt) between the current and desired manipulability ellipsoids for the performance comparison ofthe different manipulability tracking formulations.

Approach Euclidean (after jump) Cholesky (after jump) Cholesky Jacobian Geometry-aware

Fig. 11a 1.3e−4 - 1.446 - 0.1204 6e−5

Fig. 11b 2.997 3.977 3.385 1.944 0.455 1.4e−4

is particularly visible for the second test, where the resultingjoint configuration is farther from the initial pose of the robotcompared to the joint configuration obtained by the proposedgeometry-aware controller (see Fig. 11b-middle, right).

Previously, we hypothesized that the sudden changes injoint configuration when using the Euclidean and Cholesky-based Euclidean formulations in the second scenario are dueto the path induced by the methods on the SPD manifold.In order to confirm this hypothesis, we reproduced thesecond test with lower gain values. Figure 12 shows theconvergence of the proposed geometry-aware controller,the Euclidean-based approach and the Cholesky-basedEuclidean formulation for gain matrices equal to I , 0.5I ,

0.1I and 0.05I . We observe that, even for very low gains,both Euclidean and Cholesky-based Euclidean formulationslead to a sudden change in the joint configuration, resultingin an abrupt increase on the error measured between thecurrent and desired manipulability ellipsoids (see Fig. 12-top, middle). Interestingly, the sudden changes occur atsimilar location along the path between the initial and desiredmanipulability ellipsoid independently of the gain value forboth formulations (see Fig. 12-bottom), therefore confirmingour above statement. This can also be seen by looking at theyellow and dark blue robots of Fig. 12-middle depicting theconfigurations before the jump, which are almost identical inall the graphs.



(a) KM = I (b) KM = 0.5I (c) KM = 0.1I (d) KM = 0.05I

Figure 12. Comparison of the performance of different manipulability tracking formulations for different gains KM . The organizationof the graphs and the colors are identical to Fig. 11. The Cholesky-Jacobian-based Euclidean formulation is not shown.

In the case in which the manipulability tracking taskbecomes a secondary objective, the 4-DoF planar robotis required to keep its end-effector at a fixed Cartesianposition x while minimizing the distance between its currentand desired manipulability ellipsoids M and M . The fourfollowing approaches are considered for comparison withthe proposed formulation (39). Firstly, we analyze thecorresponding Euclidean manipulability-tracking controller

qt = J†Kx(xt − xt)

+ (I − J†J)(J †(3))TKMvec(Mt −Mt), (58)

where the difference between two manipulability ellipsoidsis computed in Euclidean space, i.e., ignoring thatmanipulability ellipsoids belong to the set of SPD matrices.Secondly, we implement the corresponding Cholesky-basedEuclidean manipulability controller


+ (I − J†J)(J †(3))TKMvec(∆Lt∆LT

t ), (59)

which ignores that manipulability ellipsoids lie on the SPDmanifold but ensure a positive definite difference betweentwo ellipsoids. Thirdly, we analyze the Cholesky-Jacobian-based Euclidean manipulability controller


+ (I − J†J)(J †chol(3))TKMvec(L−L), (60)

Table 3. Final distances d(M ,Mt) between the current anddesired manipulability ellipsoids for the performance comparisonof the different manipulability-based redundancy resolutionformulations.

Approach Euclidean Cholesky Cholesky Jac. Geometry-aware Gradient-based

Fig. 13a 0.433 0.808 1.418 0.416 0.436

Fig. 13b 1.763 2.271 1.856 1.101 1.110

which tracks manipulability ellipsoids through theirCholesky decomposition. Fourthly, we evaluate the gradient-based approach of (Rozo et al. 2017) that implements thecontroller

qt = J†Kx(xt − xt)− (I − J†J)α∇gt(q), (61)

where α is a scalar gain and

gt(q) = log det

(Mt + Mt

2

)− 1

2log det

(MtMt

)(62)

is a cost function based on Stein divergence (a distance-likefunction on the SPD manifold (Sra 2012)). The gain matricesKM are fixed as identity matrices and the scalar gain is setto 1 for the comparison.

Figure 13 shows the convergence rate for themanipulability-based redundancy resolution of theaforementioned approaches. Two tests were carried outby varying the initial configuration of the robot andthe desired manipulability ellipsoid. In both cases, both



(a)

(b)

Figure 13. Performance comparison of the differentmanipulability-based redundancy resolution formulations.Two cases are shown with varying initial robot configuration anddesired manipulability. The left graph shows the convergenceof the affine invariant distance between the current and thedesired manipulability ellipsoid over time. The distancesfor the Euclidean, Cholesky-based Euclidean, Cholesky-Jacobian-based Euclidean, geometry-aware and gradient-basedapproaches are respectively depicted in blue, yellow, lila, red,and purple. The right graph shows the initial and final posture ofthe robot along with the final manipulability ellipsoids. The initialposture of the robot is depicted in light gray. The final posturesand the corresponding manipulability ellipsoids for the differentmethods are depicted in the same color as the distances. Thedesired manipulability ellipsoid is depicted in green.

geometry-aware and gradient-based approaches convergeto a similar final robot configuration (see Fig. 13a, 13b-right), with similar values of the affine-invariant distancebetween the final and desired manipulability ellipsoids(see Fig. 13a, 13b-left and Table 3). More importantly, theproposed geometry-aware manipulability tracking approachshows a faster convergence than the gradient-based method,with a lower computational cost (3.5 ms and 4.2 ms pertime step, with non-optimized Matlab code on a laptop with2.7GHz CPU and 32 GB of RAM). This notable differencemay be attributed to the fact that despite both methods takeinto account the geometry of manipulability ellipsoids, ourapproach is more informative about the kinematics of therobot through the use of the manipulability Jacobian J (q).

Note that for some specific initial robot configura-tions and desired manipulability ellipsoids, the Euclideanmanipulability-tracking controller (58) shows a slightlyfaster convergence rate than our method (see Fig. 13a).However, this Euclidean formulation again leads to unstablebehaviors in some configurations (see Fig. 13b), wherethe distance between the final and desired manipulabilityellipsoids remains high compared to the two geometry-aware approaches. This poor tracking performance can be

attributed to the fact that the Euclidean difference betweentwo SPD matrices is an approximation that is only valid ifthe matrices are close enough to each other. Thus, similarlyto Euclidean controller aimed at tracking manipulabilityellipsoids as first task (55), the Euclidean manipulability-based redundancy resolution is only effective if the currentand desired ellipsoids are very similar. Moreover, the dis-tance between the final and desired manipulability ellipsoidsremains higher than for the geometry-aware methods and theEuclidean controller by using the Cholesky-based Euclideanmanipulability-based redundancy resolution. This tendencyis similar to the observations made for the tracking ofmanipulability ellipsoids as main objective and is due to thefact that the controller (59) induces paths on the manifoldthat are not close to geodesics. Furthermore, the Cholesky-Jacobian-based Euclidean controller shows a poor trackingperformance for the two considered scenarios. Notably,the distance between the current and desired ellipsoids islargely increased before decreasing slowly in the first case(see Fig. 13a). Moreover, in some configurations, the finaldistance remains high compared to the geometry-awareapproaches as shown by Fig. 13b. These behaviors are dueto the fact that the controller (60) does not follow geodesicpaths on the SPD manifold.

The reported results supported our hypothesis thatgeometry-aware manipulability controllers result in goodtracking performance while providing stable convergenceregardless of the manipulability tracking error. This wasobserved when manipulability tracking was the maintask and a secondary objective of the robot. Moreover,our manipulability-based redundancy resolution approachoutperforms the gradient-based method. Furthermore, ourcontroller permits to directly exploit the variabilityinformation of a task, given in the form of a 4th-ordercovariance tensor, through the gain matrix of the controller.This allows the robot to exploit the precision requiredwhile tracking a manipulability ellipsoid either as mainor secondary objective. This operation is not available inthe gradient-based method used for comparison, since thecorresponding controller gain is a scalar.

5.2.2 Comparisons with manipulability-based optimiza-tion We compare our tracking approach against two state-of-the-art manipulability-based optimization methods widelyused to improve robots posture for task execution. We firstevaluate our geometry-aware controller against manipulabil-ity volume maximization. Then, we compare our controllerto the compatibility index maximization (Chiu 1987), wherethe distance from the ellipsoid center to its surface is max-imized along a specified direction. To do so, we considertwo 8-DoF planar robots that are required to track a desiredCartesian velocity trajectory that leads to an L-shape pathin the Cartesian space. In order to achieve high dexterityin motion, the first robot is requested to track a desiredmanipulability ellipsoid whose main axis is elongated alongthe direction of motion. The second robot varies its posturein order to maximize either the manipulability volume or thecompatibility index along the direction of motion.

Fig. 14a shows the resulting joint configurations andmanipulability ellipsoids of the two robots at differentstages of the task where the second robot maximizes



the manipulability volume as secondary objective. Weobserve that the main axis of the manipulability ellipsoidobtained with the volume maximization approach is oftenperpendicular to the direction of motion, which often occursas this method does not consider any geometric informationabout the desired manipulability ellipsoid. Also, since theresulting posture leads to ellipsoids that are not consistentwith the task requirement (task velocity control directions)and degrade the robot capabilities, this becomes unstablewhen the gain of the velocity tracking controller is increasedto achieve higher Cartesian velocities, as shown in Fig. 15.Conversely, the robot tracking a desired manipulabilityellipsoid successfully completes the task when highervelocities are required.

The main advantage of maximizing the compatibilityindex over the volume is that the directions in which theellipsoid should be elongated are specified. However, thisapproach favors robot configurations that may be close tosingularities as the manipulability ellipsoids correspondingto these posture are flat ellipsoids that can be largelyelongated (see Fig. 14c). This effect exacerbates when thecompatibility index maximization is the main task of therobot, as this is not required to match a specific positionin Cartesian space. Chiu (1988) extended the compatibilityindex optimization approach by defining the compatibilitycost as a weighted sum, allowing the maximization orminimization of the ellipsoid along several directions. Thismethod provides more flexibility on the resulting ellipsoiddue to the weighted combination, at the cost of a laborioustuning. Moreover, the orientation and elongation of the mainaxes of the ellipsoid after the optimization are hard to inferfrom the cost weights.

In contrast to the considered manipulability-based opti-mization methods, the proposed geometry-aware controllersseeks to fit the full desired manipulability ellipsoid in allits directions. Singular configurations can therefore be eas-ily avoided by defining appropriate desired manipulabilityellipsoids. Moreover, our manipulability controller allowsthe tracking of any manipulability ellipsoid, including thoseproviding a compromise between dexterity in motion andforce exertion along any axis. This is not possible whenusing the compatibility index approach of (Chiu 1987) asit always favors the dexterity in motion over force or vice-versa. Although this compromise might be achievable usingthe compatibility index approach of (Chiu 1988), our methoddoes not require a laborious tuning process. Manipulabil-ity tracking is also hard to achieve through manipulabilityvolume maximization as there is no explicit control on theresulting ellipsoid main axes.

6 ExperimentsPreviously, in our former work (Jaquier et al. 2018),we showed the benefits of including the manipulabilityredundancy resolution controller in the nullspace of aposition controller for a pushing and an insertion task.Contrary to the result obtained by the position controlleralone, the posture of the robot significantly varied duringthe execution of the tasks to be compatible with theirrespective force requirements as a consequence of the forcemanipulability tracking.

(a) (b) (c)

Figure 14. (a) Comparison of our manipulability trackingcontroller (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 orderto allow high velocities during the task execution. The robotcolors 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).

Figure 15. Cartesian velocities achieved with our manipulabilitytracking controller (purple) and the volume maximizationapproach (yellow) as secondary objective for a Cartesian velocitycontroller. The gain of the velocity controller are equal for bothapproaches. The desired velocities are shown in green.

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

6.1 Manipulability tracking for a robotic handIn the context of robotic hands, manipulability ellipsoidshave been used to analyze their performances in graspingtasks (Prattichizzo et al. 2012). In this experiment, we aimat modifying the posture of a robotic hand to match a desiredmanipulability ellipsoid while grasping an object.

For the case of multiple arm systems, the set of jointvelocities of constant unit norm ‖qa‖ = ‖(qT

1 , . . . , qTC)T‖ =

1 is mapped to the Cartesian velocity space xa =(xT

1 , . . . , xTC)T through

‖qa‖2 = qTa qa = xT

a (G†Ta JaJTaG†a)−1xa, (63)

with the Jacobian Ja = diag(J1, . . . ,JC), the grasp matrixGa = (G1, . . . ,GC) and C the number of arms. Therefore,the velocity manipulability ellipsoid of the C-arms system



(a) Initial (b) Final (c)

Figure 16. Manipulability tracking for grasping tasks with theAllegro hand in simulation. (a) and (b) show the initial andfinal pose of the robot, respectively. (c) Initial, final and desiredmanipulability ellipsoids respectively depicted in yellow, darkpurple and green. The bottom-right graph shows the evolutionof the distance between the current and desired manipulabilityellipsoid over time (in seconds).

is given by M xa = G†Ta JaJTaG†a (Chiacchio et al. 1991a).

Note that the system is modeled under assumptions that thearms are holding a rigid object with a tight grasp.

In this first experiment, the Allegro hand was requiredto track a desired manipulability, while maintaining relativepositions between the different fingers. This experiment aimsat emulating how humans adapt their finger configuration tothe task at hand while grasping an object. In this experiment,the desired velocity manipulability ellipsoid was designedby the experimenter to be a medium-size isotropic ellipsoid.The purpose of this design is to provide the hand with thecapability to perform a displacement of the object whilebeing resistant to external perturbations in all the directions.For example, in the case where the hand is holding a pen, itis desirable that the pen can be moved with dexterity, whilethe hand should resist to perturbations due to the pen-surfacecontacts.

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

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

6.2 Manipulability tracking for a humanoidcenter of mass

An interesting use of manipulability ellipsoids arises whenthese are defined at the center of mass (CoM) of humanoidrobots, which permits to analyze their capabilities toaccelerate the CoM in locomotion (Azad et al. 2017; Gu et al.2015), or to evaluate how resistant they can be to externalperturbations using the force manipulability at a specifichumanoid posture. With the goal of getting some insights onthe role of CoM manipulability ellipsoids in legged robots,we designed manipulability tracking experiments using twodifferent floating-base robots in simulation, namely, thehumanoid NAO and the Centauro robot (Baccelliere et al.2017).

Specifically, we required the robots to track a desiredmanipulability ellipsoid defined at its CoM while keepingbalance. We assumed a strict hierarchy of tasks that gave thehighest priority to the task of maintaining the CoM positionover the support polygon and zero velocity at all contactpoints with the floor, while the manipulability tracking wasconsidered a secondary task. Under the aforementionedassumptions, we implemented the inverse kinematics-basedcontroller for floating-base robots proposed in (Mistry et al.2008), which we briefly introduce here. First, let us definethe Jacobian for the primary task as

Jb =

[Jfeet

JCoM,xy

], (64)

where Jfeet represents the Jacobians for theposition/orientation of the robot feet while JCoM,xy isthe 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 xb(i.e. velocities of the robot feet and CoM), noting that allthe robot feet velocities must equal zero in order to maintainconstraints, therefore

xb =

[0

xCoM

], (65)

where xCoM is the velocity at the robot CoM so that it lies inthe support polygon.

Regarding the secondary task, that is, the manipulabilitytracking at the robot CoM, we first compute the Jacobian atthe CoM JCoM for floating-base robots as in (Mistry et al.2008), which allows us to calculate manipulability ellipsoidsof the types introduced in Section 4. Depending on whichtype of manipulability we require the robot to track, we canuse any of the manipulability Jacobians (34), (35) or (36) tocompute the desired joint velocities q for the manipulabilitytracking task using (38). So, the full joint velocity controllerfor legged robots required to keep balance while tracking adesired manipulability ellipsoid at their CoM is defined as

q =

[In×n

06×n

]T (J†b xb +Nb (J †(3))

T KM vec(

LogMt(Mt)

)),

(66)where the first term is included in order to account for thevirtual joints of legged robots, n is the number of DoF of therobot, and Nb is the nullspace of the Jacobian (64).

We ran several experiments for testing the manipulabilitytracking at the CoM of the Centauro (Fig. 17) and NAO(Fig. 18) robots using the controller (66). The tests consisted




Figure 17. Tracking of the COM manipulability with theCentauro robot in simulation. (a) and (b) show the initial andfinal pose of the robot, respectively. (c) Initial, final and desiredmanipulability ellipsoids respectively depicted in yellow, darkpurple and green. The bottom-right graph shows the evolutionof the distance between the current and desired manipulabilityellipsoid over time, given in seconds.


Figure 18. COM manipulability tracking with NAO in simulation.(a) and (b) show 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 respectively depictedin yellow, dark purple and green. The bottom-right graph showsthe distance between the current and desired manipulability overtime (given in seconds).

of manually setting a desired manipulability ellipsoid to betracked at the CoM of the robot, and running a joint velocitycontroller given the reference provided by (66). Notably,both Centauro and NAO tracked the desired manipulabilityas precisely as possible without compromising the balancingtask. Figures 17c and 18c show the distance between thedesired and current CoM manipulability, which decreasesover time as the robot adapts its posture to carry out a goodtracking while keeping its balance. An interesting aspectabout defining and tracking CoM manipulability ellipsoidsis the final posture that the robots achieve. Figure 17b showsthe final posture achieved by Centauro when tracking a CoMmanipulability whose projection on the (x1, x2) plane is atilted ellipse, which makes the robot adopt a posture wherethe front legs and torso rotate on the same plane (whichcorresponds to the floor in the virtual environment). The finalposture of NAO displayed in Fig. 18b shows that both armsare completely extended along the humanoid frontal axis, inan attempt to align them with one of the main axis of theCoM manipulability ellipsoid. However, both the balancingtask and the lower number of DoF constrain NAO to closelymatch the desired manipulability.

6.3 Manipulability transfer between robots fora bimanual task

The performance of the proposed manipulability transferframework was tested in a bimanual unplugging of anelectric cable from a power socket. The central idea is toteach different dual-arm robots to execute a task requiringa specific manipulability profile via kinesthetic teachingprovided only to one of the bimanual robots.

In the first part of the experiment, the two 7-DoF armsof a Baxter robot are kinesthetically guided to providedemonstrations (see Fig. 19a). The posture of the armsis modified by the user so that the main axis of thedual force manipulability ellipsoid of the system MFa =(G†Ta JaJ

TaG†a)−1 is aligned with the direction of extraction.

Then, the arms are moved in opposite directions to unplugthe electric cable from the socket. We extracted both therelative position ∆xt between the end-effectors of botharms and the force manipulability ellipsoid of the systemMF

a,t. The collected data were time-aligned and split in twodatasets of time-driven trajectories, namely relative Cartesianpositions and manipulability. We trained a classical GMMover the time-driven relative positions and a geometry-awareGMM over the time-driven manipulability ellipsoids. Thenumber of components of each model (K = 4) was selectedby the experimenter.

In the second part of the experiment, the unpluggingtask is reproduced by both the Baxter robot and a pair ofFranka Emika Panda robots (see Fig. 19b, 19c). For bothreproductions, the relative position between the end-effectorsand the desired manipulability of the system were computedat each time step by a classical GMR as ∆xt ∼ p(∆x|t)and a geometry-aware GMR as MF

a,t ∼ P (MFa |t). In both

cases, the left robotic arm was required to move its jointsto track the desired manipulability ellipsoid (38), whilethe right arm was required to maintain the desired relativeCartesian position with respect to the left arm, while trackingthe desired manipulability as secondary objective (39). Notethat the actuation contribution of each robot was taken intoaccount to compute the manipulability ellipsoids through thewhole experiment.

Figure 20a displays the two demonstrations recorded bykinesthetically guiding the Baxter robot along with thecomponents of the GMM encoding ∆xt and the centersof the components of the geometry-aware GMM encodingMF

a . The first and third dimensions of ∆xt are notrepresented as they do not vary significantly during theexperiment. Figure 20b shows the relative Cartesian positionand manipulability ellipsoid profile to be tracked and thereproduction results when the Baxter robot executed thetask. Baxter successfully tracked the desired manipulabilityellipsoid while maintaining the required relative distancebetween its end-effectors.

Figure 20c shows the relative Cartesian position betweenthe arms and the manipulability ellipsoid profile obtainedduring the reproduction of the task by the two Panda robots.These successfully achieved the required task and tracked thedesired manipulability ellipsoid profile obtained from modeltrained with the data recorded on the Baxter robot. Note themanipulability matching is not exact in this case due to thedifferences between Baxter and the Panda robots. Indeed,



(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

Figure 19. Unplugging task. The robots pose at the beginningof the task, before and after the extraction of the cable fromthe socket are respectively shown in the left, middle and rightcolumn.

even if the actuation capabilities of each robot are takeninto account in our manipulability transfer framework, thecapabilities of the two dual-arm system differ due to otherphysical specificities, e.g. the relative position of the basesof the arms.

7 Discussion

Our tracking formulation enables robots to modify theirposture in an exponentially stable way so that desiredmanipulability ellipsoids are tracked, either as a maincontrol task or as a redundancy resolution problem wherethe manipulability tracking is considered a secondaryobjective. Compared to state-of-the-art manipulability-basedoptimization schemes, our tracking formulation allows thereproduction of any manipulability ellipsoid beyond themaximization of manipulability parameters. The proposedtracking approach covers different manipulability ellipsoidsproposed in the literature, such as velocity, force anddynamic manipulability ellipsoids (Doty et al. 1995). Arelevant aspect about our approach is their generic structure,which means that we can track manipulability ellipsoids fora large variety of robots, as reported in the previous section,where a robotic hand, a Centauro robot, a humanoid andtwo different bimanual setups were used to test our trackingapproach. This shows that our approach can be used in a largevariety of contexts and that many further applications can beconsidered.

The manipulability transfer results reported in Section6.3 showed the effectiveness of the proposed approach fortransferring manipulability ellipsoids between robots thatdiffer in their kinematic structure, which has remained achallenge in the robot learning community. Our learningframework allows a robot to learn posture-dependent taskrequirements without explicitly encoding a model in the jointspace of the demonstrator, which would require complexkinematic mapping algorithms and would make task analysisless interpretable at first sight. In addition, the proposed

framework extends the robot learning capabilities beyond thetransfer of trajectory, force and impedance.

It is important to emphasize the fact that the manipulabilitytracking precision strongly depends on the number of DoFswhen the task is considered a secondary objective, as thehigher it is, the more capable the robot is to performmore than one task simultaneously. Note that, in the caseof legged robots (which are often characterized by a highnumber of DoFs), the manipulability tracking may stillbe slightly compromised because of the set of constraintsimposed by the balancing task, as observed in Section 6.2.However, if these robots are provided with the possibilityof modifying their feet position while keeping balance, thenthe manipulability tracking may be further improved. Thisclearly requires more sophisticated balancing controllers,but gives robots more freedom to adapt their posture andachieve better manipulability tracking. Notice that in thecase of robotic hands, a similar behavior arises when thefinger tips are constrained according to some graspingrequirements, which might affect the manipulability trackingwhen projected into the nullspace of the primary task.

It is important to notice that the proposed manipulabilitytracking approach is a local method in the sense thatthe solution depends on the current configuration of therobot expressed through the Jacobian. This makes thetracking convergence dependent on the current configurationof the robot, which sometimes may limit the trackingperformance. However, the robot may achieve a bettertracking performance if it is allowed to look for other initialpostures. As an example, the robot may not track preciselythe desired manipulability ellipsoids for a given initialposture, due for instance to its joint limits. However, if therobot slightly modifies its initial posture, it may find a betterstarting configuration to subsequently minimize the errorbetween the desired and current manipulability ellipsoids ina larger proportion, even if the new initial posture initiallyincreases this error.

Overall, the proposed manipulability transfer frameworkmay be exploited in a large variety of applications, wherethe posture of the robot may have an impact on itsperformance while executing the task. In addition to varyingthe robot posture for task compatibility, tracking a desiredmanipulability profile as a secondary task may typicallycomplement a main control task to avoid singularity,handle perturbations during task execution, optimize theexecution time or minimize the energy consumption (Kimet al. 2010). In particular, manipulability transfer may beutilized from a motion planning point of view. To doso, the robot may first track a desired manipulability asmain control task in a planning phase, where the robotadapts its posture in order to anticipate the next action.Following this planning phase, the robot executes the desiredaction with a posture adapted to the task requirements.In this phase, the desired manipulability is tracked as asecondary task. Moreover, in the context of rehabilitation andassistance, the proposed learning and tracking formulationsmay be exploited in control strategies for exoskeletons.In (Petric et al. 2019), the exoskeleton posture is optimizedto achieve an isotropic manipulability by sensing thehuman muscular manipulability. In this setting, a varying



(a) Demonstrations and GMM (b) Reproduction with Baxter (c) Reproduction with the Panda robots

Figure 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-states GMM (in blue). Only the most representativedimension 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-states GMM in the SPD manifold overtime (in purple). (b) Reproduction of the unplugging task with Baxter. The desired and reproduced trajectories are representedin green and dark blue respectively. The top graph shows the desired and reproduced relative position between the end-effectorsalong 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 representedin green and purple respectively. The top graph shows the desired and reproduced relative position between the end-effectors alongthe second dimension. The middle-bottom graphs shows the desired and reproduced manipulability ellipsoids. The position x andtime t are given in meters and seconds.

exoskeleton manipulability profile may be retrieved usingGMR as a function of the sensed muscular manipulability.

From a mathematical point of view, it is worth highlightingthe importance of considering the structure of the datawe work with. While alternative solutions to handle SPDmatrices are present in literature (e.g. those using Choleskydecomposition), we showed that Euclidean manipulability-tracking controllers lead to unstable behaviors in contrastto the stable behavior displayed by our geometry-awarecontroller. Equally important, the manipulability ellipsoidsprofiles retrieved by the geometry-aware and EuclideanGMR were similar only around the mean of the GMMcomponents, but diverged when moving away from it. Thisis because the estimated output in Euclidean space is only avalid approximation for input data lying close to the mean,as reported in Section 5. Therefore, geometry-awareness iscrucial for successful learning and tracking of manipulabilityellipsoids.

8 Conclusions and Future Work

This article presented a novel framework for transferringmanipulability ellipsoids to robots. The proposed approachis built on a probabilistic learning model that allows theencoding and retrieval manipulability ellipsoids, and on theextension of the classical inverse kinematics problem tomanipulability ellipsoids, by establishing a mapping betweena change of manipulability ellipsoid and the robot jointvelocity. We exploited tensor representation and Riemannianmanifolds to build a geometry-aware learning frameworkand exponentially stable tracking controllers and showedthe importance of geometry-awareness for manipulabilitytransfer. We then showed that our manipulability transferframework allows the exploitation of task variationsrecovered by the learning approach to characterize theprecision of the manipulability tracking problem. Thisapproach enables the learning of posture-dependent task

requirements. It provides a skill transfer strategy goingbeyond the imitation of trajectory, force or impedancebehaviors. Furthermore, it allows manipulability transferbetween agents of different embodiments, while taking intoaccount their individual characteristics and is adapted tocomplex scenarios involving any manipulability ellipsoidshape and various types of robots.

Future work will explore manipulability transfer betweenhumans and robots. Following this research direction, werecently proposed a statistical analysis of single and dual-arm manipulability ellipsoids for human movements, accom-panied by two human-to-robot manipulability transfer exper-iments. The corresponding results will be detailed in a forth-coming publication. We will also investigate manipulabilitytransfer strategies where the desired manipulability would beoptimized in function of the robot. The objective would be toadapt the manipulability ellipsoid to exploit the capabilitiesof the learner in situations in which this learner can reach abetter manipulability than the teacher for the task at hand.

Acknowledgements

This work was supported by the Swiss National Science Foundation(SNSF/DFG project TACT-HAND).

References

Ajoudani A, Tsagarakis NG and Bicchi A (2015) On the role ofrobot configuration in cartesian stiffness control. In: IEEE Intl.Conf. on Robotics and Automation (ICRA). pp. 1010–1016.

Azad M, Babic J and Mistry M (2017) Dynamic manipulabilityof the center of mass: A tool to study, analyse and measurephysical ability of robots. In: IEEE Intl. Conf. on Robotics andAutomation (ICRA). pp. 3484–3490.

Baccelliere L, Kashiri N, Muratore L, Laurenzi A, Kamedula M,Margan A, Cordasco S, Malzahn J and Tsagarakis NG (2017)Development of a human size and strength compliant bi-manual platform for realistic heavy manipulation tasks. In:



IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS).Vancouver, Canada, pp. 5594–5601.

Basser P and Pajevic S (2007) Spectral decomposition of a 4th-order covariance tensor: Applications to diffusion tensor MRI.Signal Processing 87(2): 220–236.

Bruyninckx H and De Schutter J (1996) Symbolic differentiation ofthe velocity mapping for a serial kinematic chain. Mechanismand Machine Theory 31(2): 135–148.

Chiacchio P (1990) Exploiting redundancy in minimum-time pathfollowing robot control. In: American Control Conference. pp.2313–2318.

Chiacchio P, BouffardVercelli Y and Pierrot F (1997) Forcepolytope and force ellipsoid for redundant manipulators.Journal of Robotic Systems 14(8): 613–620.

Chiacchio P, Chiaverini S, Sciavicco L and Siciliano B (1991a)Global Task Space Manipulability Ellipsoids for Multiple-ArmSystems. IEEE Trans. on Robotics and Automation 7(5): 678–685.

Chiacchio P, Chiaverini S, Sciavicco L and Siciliano B (1991b)Reformulation of dynamic manipulability ellipsoid for roboticmanipulators. In: IEEE Intl. Conf. on Robotics and Automation(ICRA). pp. 2192–2197.

Chiu SL (1987) Control of redundant manipulators for taskcompatibility. In: IEEE Intl. Conf. on Robotics and Automation(ICRA). pp. 1718–1724.

Chiu SL (1988) Task compatibility of manipulator postures. Intl.Journal of Robotics Research 7(5): 13–21.

Cos I, Belanger N and Cisek P (2011) The influence ofpredicted arm biomechanics on decision making. Journal ofNeurophysiology 105: 3022–3033.

do Carmo M (1992) Riemannian Geometry. Birkhauser Basel.Doty K, Schwartz E, Melchiorri C and Bonivento C (1995)

Robot manipulability. IEEE Transactions on Robotics andAutomation 11(3): 462–468.

Dounskaia N, Wang W, Sainburg RL and Przybyla A (2014)Preferred directions of arm movements are independent ofvisual perception of spatial directions. Experimental BrainResearch 232(2): 575–586.

Dubbelman G (2011) Intrinsic Statistical Techniques for RobustPose Estimation. PhD thesis, University of Amsterdam,Netherlands.

Freifeld O, Hauberg S and Black MJ (2014) Model transport:Towards scalable transfer learning on manifolds. In: Conf. onComputer Vision and Pattern Recognition (CVPR). Columbus,OH, USA, pp. 1378–1385.

Gu Y, Lee G and Yao B (2015) Feasible center of mass dynamicmanipulability of humanoid robots. In: IEEE Intl. Conf. onRobotics and Automation (ICRA). pp. 5082–5087.

Guilamo L, Kuffner J, Nishiwaki K and Kagami S (2006)Manipulability optimization for trajectory generation. In: IEEEIntl. Conf. on Robotics and Automation (ICRA). pp. 2017–2022.

Jaquier N and Calinon S (2017) Gaussian mixture regression onsymmetric positive definite matrices manifolds: Application towrist motion estimation with sEMG. In: IEEE/RSJ Intl. Conf.on Intelligent Robots and Systems (IROS). Vancouver, Canada,pp. 59–64.

Jaquier N, Rozo L, Caldwell DG and Calinon S (2018) Geometry-aware tracking of manipulability ellipsoids. In: Robotics:

Science and Systems (R:SS). Pittsburgh, USA.Kim W, Lee S, Kang M, Han J and Han C (2010) Energy-efficient

gait pattern generation of the powered robotic exoskeletonusing DME. In: IEEE/RSJ Intl. Conf. on Intelligent Robots andSystems (IROS). pp. 2475–2480.

Kolda T and Bader B (2009) Tensor decompositions andapplications. SIAM Review 51(3): 455–500.

Lee I and Oh J (2016) Humanoid posture selection for reachingmotion and a cooperative balancing controller. Journal ofIntelligent and Robotics Systems 8(3-4): 301–316.

Lee J (1997) A Study on the Manipulability Measures for Robotmanipulators. In: IEEE/RSJ Intl. Conf. on Intelligent Robotsand Systems (IROS). pp. 1458–1465.

Lee J (2012) Introduction to Smooth Manifolds, Graduate Texts inMathematics, volume 218. Springer, New York.

Lee S (1989) Dual redundant arm configuration optimization withtask-oriented dual arm manipulability. IEEE Transactions onRobotics and Automation 5(1): 78–97.

Luh JYS and Zheng YF (1987) Constrained Relations BetweenTwo Coordinated Industrial Robots for Motion Control. Intl.Journal of Robotics Research 6(3): 60–70.

Mistry M, Nakanishi J, Cheng G and Schaal S (2008) Inversekinematics with floating base and constraints for full bodyhumanoid robot control. In: IEEE/RAS Intl. Conf. onHumanoid Robots (Humanoids). Daejeon, South Korea, pp.22–27.

Morasso P (1981) Spatial control of arm movements. ExperimentalBrain Research 42: 223–227.

Murray RM, Li Z and Shankar Sastry S (1994) A MathematicalIntroduction to Robotic Manipulation. CRC Press, Inc.

Pait F and Colon D (2010) Some properties of the Riemanniandistance function and the position vector X, with applications tothe construction of Lyapunov functions. In: IEEE Conferenceon Decision and Control (CDC). Atlanta, USA, pp. 6277–6280.

Park FC (1995) Optimal Robot Design and Differential Geometry.Journal of Mechanical Design 117(B): 87–92.

Park FC and Kim JW (1998) Manipulability of closed kinematicchains. ASME Journal of Mechanical Design 120(4): 542–548.

Pennec X, Fillard P and Ayache N (2006) A Riemannian frameworkfor tensor computing. Intl. Journal on Computer Vision 66(1):41–66.

Petric T, Peternel L, Morimoto J and Babic J (2019) Assistive arm-exoskeleton control based on human muscular manipulability.Frontiers in Neurorobotics 13.

Prattichizzo D, Malvezzi M, Gabiccini M and Bicchi A (2012) Onthe Manipulability Ellipsoids of Underactuated Robotic Handswith Compliance. Robotics and Autonomous Systems 60(3):337–346.

Ratliff N, Toussaint M and Schaal S (2015) Understanding thegeometry of workspace obstacles in motion optimization. In:IEEE Intl. Conf. on Robotics and Automation (ICRA). pp.4202–4209.

Rozo L, Calinon S, Caldwell DG, Jimenez P and Torras C (2016)Learning physical collaborative robot behaviors from humandemonstrations. IEEE Trans. on Robotics 32(3): 513–527.

Rozo L, Jaquier N, Calinon S and Caldwell DG (2017) Learningmanipulability ellipsoids for task compatibility in robotmanipulation. In: IEEE/RSJ Intl. Conf. on Intelligent Robotsand Systems (IROS). pp. 3183–3189.


23

Sabes P and Jordan M (1997) Obstacle avoidance and a perturbationsensitivity model for motor planning. Journal of Neuroscience17: 7119–7128.

Simo-Serra E, Torras C and Moreno-Noguer F (2017) 3D humanpose tracking priors using geodesic mixture models. Intl.Journal on Computer Vision 122(2): 388–408.

Somani N, Rickert M, Gaschler A, Cai C, Perzylo A and Knoll A(2016) Task level robot programming using prioritized non-linear inequality constraints. In: IEEE/RSJ Intl. Conf. onIntelligent Robots and Systems (IROS). pp. 430–437.

Sra S (2012) A new metric on the manifold of kernel matrices withapplication to matrix geometric means. Neural InformationProcessing Systems : 144–152.

Sra S and Hosseini R (2015) Conic geometric optimization onthe manifold of positive definite matrices. SIAM Journal onOptimization 25(1): 713–739.

Tyagi A and Davis JW (2008) A recursive filter for linear systemson Riemannian manifolds. In: Conf. on Computer Vision andPattern Recognition (CVPR). Anchorage, Alaska, pp. 1–8.

Vahrenkamp N, Asfour T, Metta G, Sandini G and Dillmann R(2012) Manipulability analysis. In: IEEE/RAS Intl. Conf. onHumanoid Robots (Humanoids). pp. 568–573.

Wu D (2020) Intrinsic construction of Lyapunov functions onRiemannian manifold. arXiv preprint 2002.11384 .

Yoshikawa T (1985a) Dynamic manipulability of robotic manipu-lators. Journal of Robotic Systems 2: 113–124.

Yoshikawa T (1985b) Manipulability of robotic mechanisms. Intl.Journal of Robotics Research 4(2): 3–9.

Zeestraten MJA, Havoutis I, Silverio J, Calinon S and CaldwellDG (2017) An approach for imitation learning on Riemannianmanifolds. IEEE Robotics and Automation Letters 2(3): 1240–1247.

AppendicesA Derivative of a matrix w.r.t. a vector

Left multiplication by a constant matrix (Eq. (14))

∂AY

∂x=∂Y

∂x×1 A

Proof.(∂AY

∂x

)ljk

=∂

∂xk

∑i

aliyij =∑i

ali∂yij∂xk

Right multiplication by a constant matrix (Eq. (15))

∂Y B

∂x=∂Y

∂x×2 B

T

Proof.(∂Y B

∂x

)ilk

=∂

∂xk

∑i

yijbjl =∑j

bjl∂yij∂xk

Derivative of the inverse of a matrix (Eq. (16))

∂Y −1

∂x= −∂Y

∂x

T

×1 Y−1 ×2 Y

−T

Proof. We compute the derivative of the definition of theinverse Y −1Y = I as

∂

∂x(Y −1Y ) =

∂

∂x(I),

∂Y −1

∂x×2 Y

T +∂Y

∂x×1 Y

−1 = 0.

Then, by isolating ∂Y −1

∂x , we obtain

∂Y −1

∂x= −∂Y

∂x

T

×1 Y−1 ×2 Y

−T.

B Symbolic manipulability Jacobian for aserial kinematic chain

The computation of the manipulability Jacobian involvescomputing the derivative of the robot Jacobian w.r.t. the jointangles. Those derivatives can be computed in a symbolicform as shown in (Bruyninckx and De Schutter 1996).We remind here the symbolic derivative for the hybridrepresentation of the Jacobian J ∈ R6×n that is used in thecomputation of the manipulability Jacobian J .

The i-th column of the Jacobian is denoted by

J i =

(wi

vi

), (67)

with wi ∈ R3 and vi ∈ R3 the rotational and translationalcomponents of the Jacobian.

The derivative of the Jacobian w.r.t. the joint angles isa third order tensor ∂J

∂q ∈ R6×n×n with mode-1 fibers orcolumns(

∂J

∂q

):ij

=∂J i

∂qj=

P∆(J j)J i if j ≤ i−M∆(J j)J i if j > i

,

(68)where

P∆(J j) =

([wj×] 03×3

03×3 [wj×]

), (69)

M∆(J j) =

(03×3 03×3

[vj×] 03×3

), (70)

and × the cross product between two vectors. The notation[wj×] in a matrix denotes that the corresponding componentof the result of the right-multiplication of the matrix bya vector is equal to the cross product between wj andthe corresponding vector component, e.g. P∆(J j)J i =(wj ×wi

wj × vi

).

Note that the time derivative of the Jacobian can thereforebe computed as

dJ

dt=

n∑j=1

∂J

∂qjqj . (71)

C Symbolic dynamic manipulabilityJacobian for a serial kinematic chain

The derivative of the robot inertia matrix w.r.t. joint angles isnecessary for the computation of the dynamic manipulabilityJacobian. It can be computed in closed form as follows.



The inertia matrix Λ(q) ∈ Rn×n can be written as

Λ(q) =

n∑i=1

JTi

(Λi 00 miI

)Ji, (72)

where Ji, Λi and mi are the Jacobian, inertia matrix andmass of link i, respectively (Park 1995; Murray et al. 1994).

The derivative of the inertia matrix is the third order tensor∂Λ∂q ∈ Rn×n×n computed as

∂Λ

∂q=

n∑i=1

∂JTi

∂q×2 J

Ti Mi +

∂Ji∂q×1 J

Ti Mi, (73)

where Mi =

(Λi 00 miI

)and ∂Ji

∂q is computed with

Eq. (68).

D Symbolic derivative of the manipulabilityJacobian for a serial kinematic chain

In some cases, e.g. in the acceleration tracking controller, thetime derivative of the manipulability Jacobian is required.This time derivative can be computed symbolically byexploiting the first and second derivative of the Jacobianw.r.t. the joint angles.

The time derivative of the velocity manipulabilityJacobian J x ∈ R6×n×n defined as

J x =∂J

∂q×2 J +

∂JT

∂q×1 J , (74)

is obtained by exploiting the chain rule as

∂J x

∂t=

∂

∂t

(∂J

∂q×2 J +

∂JT

∂q×1 J

)(75)

=∂2J

∂t∂q×2 J +

∂J

∂q×2

∂J

∂t+∂2JT

∂t∂q×1 J +

∂JT

∂q×1

∂J

∂t.

(76)

The time derivative of the Jacobian is given by Eq. (71)and the time derivative of the derivative of the Jacobian w.r.t.joint angles is given by

∂2J

∂t∂qj=

n∑k=1

∂2J

∂qk∂qjqk, (77)

where the second derivative of the Jacobian w.r.t. the jointangles is a fourth order tensor ∂

2J∂q2 ∈ R6×n×n×n with mode-

1 fibers or columns(∂2J

∂q2

):ijk

=∂2J i

∂qk∂qj=

(P∆(Jj)P∆(Jk)

)J i + P∆(Jj)

(P∆(Jk)J i

)if k≤j≤ i

P∆(Jj)(P∆(Jk)J i

)if j≤k≤ i

−P∆(Jj)(M∆(Jk)J i

)if j≤ i<k

−(P∆(Jk)M∆(Jj)

)J i −M∆(Jj)

(P∆(Jk)J i

)if k≤ i<j

−(P∆(Jk)M∆(Jj)

)J i if i<k<j

−(P∆(Jj)M∆(Jk)

)J i if i<j≤k

(78)

where P∆(J j) and M∆(Jk) are defined as in (69)and (70), respectively. The time derivative of the force

manipulability Jacobian J F and the manipulability JacobianJ x corresponding to the dynamic manipulability ellipsoidcan be computed symbolically in a similar way usingEqs. (71) and (77). Moreover, their w.r.t. joint angles canbe computed symbolically using the chain rules, Eqs. (68)and (78).


Date post:	23-Aug-2020
Category:	Documents
Upload:	others
View:	2 times
Download:	0 times

Journal Title Geometry-aware Manipulability Learning ...scalinon/papers/Jaquier-IJRR2020.pdf ·...

Documents