Geometry-aware Dynamic Movement Primitives

03/12/2020 ∙ by Fares J. Abu-Dakka, et al. ∙ aalto 0

In many robot control problems, factors such as stiffness and damping matrices and manipulability ellipsoids are naturally represented as symmetric positive definite (SPD) matrices, which capture the specific geometric characteristics of those factors. Typical learned skill models such as dynamic movement primitives (DMPs) can not, however, be directly employed with quantities expressed as SPD matrices as they are limited to data in Euclidean space. In this paper, we propose a novel and mathematically principled framework that uses Riemannian metrics to reformulate DMPs such that the resulting formulation can operate with SPD data in the SPD manifold. Evaluation of the approach demonstrates that beneficial properties of DMPs such as change of the goal during operation apply also to the proposed formulation.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Day by day realistic robotic applications are bringing robots into human environments such as houses, hospitals, and museums where they are expected to assist us in our daily life tasks. Such human-inhabited environments are highly unstructured, dynamic and uncertain, making hard-coding the environments and related skills infeasible.

In this context, human expertise can be exploited to teach robots how to perform such tasks by transferring human skills to robots [17]. Learning-from-human-demonstrations (LfD) has been widely studied as a convenient way to transfer human skills to robots. This learning approach is aimed at extracting relevant motion patterns from human demonstrations and subsequently applying these patterns to different situations. In the past decades, several LfD based approaches have been developed such as: dynamic movement primitives (DMP) [9, 2], probabilistic movement primitives (ProMP) [13]

, Gaussian mixture models (GMM) along with Gaussian mixture regression (GMR)

[4], and more recently, kernelized movement primitives (KMP) [8, 7]. DMPs have several beneficial properties such as robustness against perturbations and ability to adapt to new requirements such as a new goal.

However, many tasks in those environments require variable impedance [10, 19, 1] or high manipulability [6, 21, 12, 15]), the parameters of which are encapsulated in symmetric positive definite (SPD) matrices. Because of the structure of the manifold of SPD matrices, standard LfD approaches such as DMPs can not be directly used as they rely on Euclidean parametrization of the space.

In this paper, we a novel formulation for DMPs using Riemannian metrics such that the resulting formulation can operate with SPD data. This allows variable SPD quantities to be modeled while retaining the useful properties of standard DMPs. The main contributions are

  1. A novel and mathematically principled framework for reformulating DMPs using Riemanian metrics, in order to learn and reproduce SPD-matrices-based robot skills.

  2. Reformulating standard DMP goal switching to be able to handle SPD-matrix-based robot skills.

This extension of DMPs to Riemannian manifolds allows the generation of smooth trajectories for data that do not belong to the Euclidean space.

The work is inspired by quaternion and rotation matrix based formulations of DMPs [2, 20] which target specifically the problem of parametrizing the space of orientations

. Compared to the tensor-based formulation of GMM and GMR on Riemannian manifold of SPD matrices

[11] (demonstrated for manipulability transfer in [15]), the proposed approach allows adapting the SPD profile on-line to a new goal, similar to the difference between standard GMM/GMR and DMPs for Euclidean quantities.

This paper is organized as follows: We begin by providing background about standard DMPs (Section II-A) and Riemannian manifold of SPD matrices (Section II-B). Afterwards, we exploit Riemannian manifold to derive the new formulation of DMPs (Section III-A) followed by goal switching formulation (Section III-B). Subsequently, we evaluate our approach through several examples (Section IV). The work is concluded in Section V.

Ii Background

In this scope we introduce a brief introduction to standard DMPs and Riemannian manifold of SPD matrices.

Ii-a Dynamic Movement Primitives

The basic idea of DMPs is to model movements by a system of differential equations that ensure some desired behavior, e.g. convergence to the specified attractor point [16, 9, 2]

. A DMP for a single degree of freedom trajectory

is defined by the following set of nonlinear differential equations [9]

(1)
(2)
(3)

where is the scaled velocity, is the phase variable to avoid explicit time dependency and , and define the behavior of the 2nd–order system, is the goal of the movement, and is a nonlinear forcing term that provides a modeling of complex trajectories. Choosing a time constant along with and will make the linear part of (1) and (2) critically damped, which insures the convergence of and to a unique attractor point at and [9]. is defined as a linear combination of

nonlinear radial basis functions, which enables the robot to follow any smooth trajectory from the initial position

to the final configuration

(4)
(5)

where

are the centers of Gaussians distributed along the phase of the movement and

their widths. For a given and setting equal to the total duration of the desired movement, we can define , and where . For each DoF, the weights should be adjusted from the measured data so that the desired behavior is achieved. For controlling a robotic system with more than one DoF, we represent the movement of every DoF with its own equation system (1)–(2), but with the common phase (3) to synchronize them.

Ii-B Riemannian manifold of SPD matrices

Let us define as the set of

SPD matrices which cannot be considered as a vector space since it is not closed under addition and scalar product

[14], and thus the use of classical Euclidean space methods for treating and analyzing these matrices is inadequate. A compelling solution is to incorporate these matrices with a Riemannian metric, which allows the set of SPD matrices to form a Riemannian manifold [14]. Such a metric defines the geodesics, that is, the minimum length curves between two points on the manifold.

A Riemannian manifold is a topological space, each point of which locally resembles a Euclidean space. For each point , there exists a tangent space which corresponds to the space of symmetric matrices for the case of the SPD manifold. The metric in the tangent space is flat, which allows the use of classical arithmetic tools. Note that the space of can be represented as the interior of a convex cone embedded in its tangent space of symmetric matrices .

To operate on the tangent spaces, a mapping system is required to switch between and . The two mapping operators are known as exponential and logarithmic maps:

The logarithmic map is a function that maps a point in the manifold to a point in the tangent space.

(6)

The exponential map is a function that maps a point to a point , so that it lies on the geodesic starting from in the direction of .

(7)

where and are the matrix logarithm and exponential functions.

Moving elements between different tangent spaces is performed by the parallel transport operator [18, 22]. The parallel transport is a function that transports to over the geodesic from to is given by

(8)

where . Equation (8) has been proved to be computationally efficient [22]. This transporter is exploited whenever it is required to transport SPD matrices along geodesics in a nonlinear manifold.

In this paper, we exploit the Riemannian manifold to reformulate DMPs to be capable of encoding and reproducing SPD-matrices-based robot skills.

Iii Proposed Approach

In this section, we provide a complete formulation for DMPs in order to learn and reproduce SPD-matrices-based robot skills. For the sake of simplicity let us first recall the re-interpretation of basic standard operations in a Riemannian manifold (Table I). Define and .

Euclidean space Riemannian manifold
Subtraction
Addition
Distance
Interpolation
TABLE I: Re-interpretation of basic standard operations in a Riemannian manifold [14].

Iii-a Geometry-aware DMPs Formulation

Define a variable as an arbitrary SPD matrix and as the set of SPD matrices in one demonstration. In order to prepare the demonstration data for DMP, its 1st- and 2nd-time derivatives are needed. The 1st-time derivative is computed as follows

(9)

where each belongs to the corresponding tangent space . Afterwards, we use (8) to move all to a common/shared arbitrary tangent space, e.g. the tangent space of the first SPD data . The tangent space corresponds to , which allows the use of classical arithmetic tools as mentioned in section II-B. To avoid replicating information due to symmetry, we propose to reduce the space dimensionality of the data in the tangent space to using Mandel’s representation.

(10)

where is a function that transforms a symmetric matrix into a vector using Mandel’s notation. E.g. a vectorization of a symmetric matrix is

(11)

Now, the 2nd-derivatives can be computed straight forward using standard Euclidean tools and its vectorization is denoted as . Having all necessary data , we transform the standard DMP system (1)–(2) into a geometry-aware form as follows

(12)
(13)

where is the vectorization of . represents the goal SPD matrix. is the vectorization of the transported symmetric matrix over the geodesic from to . The forcing term can be recalculated as

(14)

where the phase . Using (14), the weights

can be estimated by encoding any sampled SPD-matrices-based robot skills.

In the reproduction, equation (13) is integrated as follows

(15)

where the function is the inverse of and denotes to the matricization using Mandel’s notation. represents the new SPD-matrices-based robot skills.

Iii-B Goal switching

In the standard DMP formulation, in case of sudden goal switching (e.g. based on external sensory information) during the execution, Ijspeert et al. [9] proposed to add an additional equation to the dynamic system (1)–(2) in order to smoothly change the goal in (1) to a new goal as

(16)

where is a constant. This equation transforms from being a constant to a continuous variable. Analogously, SPD-based DMP can switch the goal using

(17)

so now is updated continually.

Iv Experiments

We evaluated the proposed imitation learning framework using simulated data. All algorithms have been implemented in MATLAB

® using a workstation running Ubuntu 16.04 LTS with Intel Core i9–8950HK CPU @ 2.90GHz 12, 16 GB of RAM.

Four different experiments were carried out to evaluate the proposed framework:

  • Learning and reproducing full stiffness matrix profiles with a 2-DoF virtual-mass spring-damper system (MSD).

  • Goal switching applied to full stiffness matrix profiles.

  • Comparison of the resulting SPD profile between the proposed DMP and GMM/GMR proposed in [11].

Iv-a Learning Variable Impedance Skills

Fig. 1: Learning and reproduction of variable stiffness profile for MSD system. Top: The reproduced stiffness ellipsoids in different time steps (in green) coinciding the demonstration (in gray) over the Cartesian trajectory of the MSD. Bottom: The reproduced stiffness ellipsoids in different time steps (in green) coinciding the ground truth (in gray) over time.

In this scope, we propose to use a simulation of MSD to evaluate our geometry-aware DMPs for learning and reproducing variable impedance111Here we refer to variable impedance as variable stiffness profiles. Nevertheless, the proposed approach can also be used to learn variable damping controllers as well as any SPD-matrix-based robot skills. skills. Note that stiffness matrices belong to the space of . The MSD system starts from an initial, horizontally-aligned, stiffness ellipsoid at rest position. Afterwards, external forces are applied to stimulate the MSD system. During the stimulation, is rotating through ( is a rotation matrix) until it ends up with a vertically-aligned ellipsoid as shown in Fig. 1 in gray. This demonstration then is encoded using (12)–(13) to reproduce the ellipsoids in green . From the figure, we can see the match between the results of the SPD-based DMPs and the demonstration.

Figure 2 tests the accuracy of the proposed SPD-based DMP by calculating the distance between the resulting SPD profile and the demonstration one. Here we used two distance metric systems: (i) Log-Euclidean distance [3]. (i) Jensen-Bregman Log-Determinant distance [5].

Fig. 2: The distance between the resulting stiffness profile from the proposed DMPs and the ground truth using different metrics.

Iv-B Goal Switching

Fig. 3: Response of the proposed DMPs to the switching of stiffness goal during the movement over Cartesian trajectory (Top) and over Time (Bottom). The stiffness goal was changed at the middle of the movement (blue ellipsoid). The new goal (in red) is the original ellipsoid but rotated by 90 degrees.

In this simulation we used the same MSD setup introduced in section IV-A. However, here we are about to test the response of the proposed SPD-based DMP to sudden goal changing during the execution. At the middle of DMP execution we changed the goal by rotating it 90 degrees to be horizontally-aligned (red ellipsoid in Fig. 3) instead of being vertically-aligned (in gray).

Figure 3 shows the smoothness of the adaptation of the stiffness profile (in green) to the new goal (in red). The blue stiffness ellipsoid marks the instant of goal switching. It is clear from the figure that the resulting profile was following the demonstrated one until the blue ellipsoid, then started to adapt to the new goal. More clearification regarding the accuracy of the approach can be seen in Fig. 4. The blue part of the figure shows the distance before the occurrence of goal switching. However, the red part shows the distances between the SPD-based DMP results and the new goal. The figure illustrates that the system converges to the new goal.

Fig. 4: Represents the distance between the demonstrated stiffness profile and the resulting DMP profile just before the goal switching occurs. is the distance between the resulting stiffness from the DMPs and the new goal. shows how the DMP is adapting and approaching to the new goal.

Iv-C Comparison with GMM/GMR

In this scope we compare the proposed SPD-based DMP with SPD-based GMM/GMR proposed by [11]. For fair comparison, as DMP is trained using one demonstration, we used also this same one demonstration to train GMM. As number of Gaussian components influence the accuracy of GMM/GMR, we trained 1-, 4-, 7-, and 10-states GMMs.

For each GMM model, we calculated the distance error between the SPD profile obtained by GMR and the demonstration. Moreover, the distance error also has been calculated in the case of the proposed SPD-based DMP. Figure 5 shows the resulting distance error in all cases. From the figure, it is clear that the accuracy of GMM/GMR increases when the number of Gaussian components increases. However, increasing Gaussian components leads to a significant increase in the computation time as shown in Table II, while the proposed SPD-based DMP is significantly faster. Moreover, the GMM/GMR approach would not allow e.g. goal switching.

Fig. 5: The log-Euclidean distance between the resulting SPD profile from the proposed DMPs and the ground truth using different metrics.
Time in seconds
Learning Reproduction
1-state GMM/GMR 2.8354 1.0788
4-states GMM/GMR 9.9459 3.9176
7-states GMM/GMR 16.3209 6.1992
10-states GMM/GMR 22.2517 8.3852
Proposed SPD-based DMP 0.0941 0.4214
TABLE II: Comparison of the computational cost between the proposed DMP and GMM/GMR [11]

V Conclusions

In this paper we successfully exploited the Riemannian manifold of to derive a new formulation of DMPs capable of direct learning and reproduction of SPD-matrix-based robot skills. This formulation avoids any prior reparametrization of such skills. Moreover, we integrated a new formulation for the goal switching that can deal directly with SPD-matrix-based robot skills.

The algorithm has been extensively validated through multiple simulation examples. Moreover, a comparison with GMM/GMR demonstrates that the proposed approach provides at least similar accuracy with a significantly lower computation cost.

In the future we propose to integrate our approach with other algorithms, e.g. iterative learning control, in order to not just reproduce SPD-matrix-based skills, but also to adapt to different situations and perform more complex tasks (e.g. forced-based variable impedance control). Moreover, we will work on exploration-based learning methods, which will prove to be crucial when a robot needs to significantly adapt to a new situation, e.g. adapt its stiffness, in order to perform successfully in a large diversity of task situations.

Acknowledgment

This work is supported by CHIST-ERA project IPALM (Academy of Finland decision 326304).

References

  • [1] F. J. Abu-Dakka, L. Rozo, and D. G. Caldwell (2018) Force-based variable impedance learning for robotic manipulation. Robotics and Autonomous Systems 109, pp. 156–167. Cited by: §I.
  • [2] F. J. Abu-Dakka, B. Nemec, J. A. Jørgensen, T. R. Savarimuthu, N. Krüger, and A. Ude (2015) Adaptation of manipulation skills in physical contact with the environment to reference force profiles. Autonomous Robots 39 (2), pp. 199–217. Cited by: §I, §I, §II-A.
  • [3] V. Arsigny, P. Fillard, X. Pennec, and N. Ayache (2006) Log-euclidean metrics for fast and simple calculus on diffusion tensors. Magnetic Resonance in Medicine: An Official Journal of the International Society for Magnetic Resonance in Medicine 56 (2), pp. 411–421. Cited by: §IV-A.
  • [4] S. Calinon, D. Bruno, and D. G. Caldwell (2014) A task-parameterized probabilistic model with minimal intervention control. In IEEE International Conference on Robotics and Automation, pp. 3339–3344. Cited by: §I.
  • [5] A. Cherian, S. Sra, A. Banerjee, and N. Papanikolopoulos (2011) Efficient similarity search for covariance matrices via the jensen-bregman logdet divergence. In

    2011 International Conference on Computer Vision

    ,
    pp. 2399–2406. Cited by: §IV-A.
  • [6] L. Guilamo, J. Kuffner, K. Nishiwaki, and S. Kagami (2006) Manipulability optimization for trajectory generation. In IEEE International Conference on Robotics and Automation, pp. 2017–2022. Cited by: §I.
  • [7] Y. Huang, F. J. Abu-Dakka, J. Silvério, and D. G. Caldwell (2019) Generalized orientation learning in robot task space. In IEEE International Conference on Robotics and Automation, pp. 2531–2537. Cited by: §I.
  • [8] Y. Huang, L. Rozo, J. Silvério, and D. G. Caldwell (2019) Kernelized movement primitives. The International Journal of Robotics Research 38 (7), pp. 833–852. Cited by: §I.
  • [9] A. J. Ijspeert, J. Nakanishi, H. Hoffmann, P. Pastor, and S. Schaal (2013) Dynamical movement primitives: learning attractor models for motor behaviors. Neural Computation 25 (2), pp. 328–373. Cited by: §I, §II-A, §III-B.
  • [10] R. Ikeura and H. Inooka (1995) Variable impedance control of a robot for cooperation with a human. In IEEE International Conference on Robotics and Automation, Vol. 3, Nagoya, Japan, pp. 3097–3102. Cited by: §I.
  • [11] N. Jaquier and S. Calinon (2017) Gaussian mixture regression on symmetric positive definite matrices manifolds: application to wrist motion estimation with semg. In IEEE/RSJ International Conference on Intelligent Robots and Systems, Vancouver, Canada, pp. 59–64. Cited by: §I, 3rd item, §IV-C, TABLE II.
  • [12] I. Lee and J. Oh (2016) Humanoid posture selection for reaching motion and a cooperative balancing controller. Journal of Intelligent & Robotic Systems 81 (3-4), pp. 301–316. Cited by: §I.
  • [13] A. Paraschos, C. Daniel, J. R. Peters, and G. Neumann (2013) Probabilistic movement primitives. In Advances in Neural Information Processing Systems, pp. 2616–2624. Cited by: §I.
  • [14] X. Pennec, P. Fillard, and N. Ayache (2006) A riemannian framework for tensor computing. International Journal of Computer Vision 66 (1), pp. 41–66. Cited by: §II-B, TABLE I.
  • [15] L. Rozo, N. Jaquier, S. Calinon, and D. G. Caldwell (2017) Learning manipulability ellipsoids for task compatibility in robot manipulation. In IEEE/RSJ International Conference on Intelligent Robots and Systems, Vancouver, Canada, pp. 3183–3189. Cited by: §I, §I.
  • [16] S. Schaal, P. Mohajerian, and A. Ijspeert (2007) Dynamics systems vs. optimal control—a unifying view. Progress in brain research 165, pp. 425–445. Cited by: §II-A.
  • [17] S. Schaal (1999) Is imitation learning the route to humanoid robots?. Trends in Cognitive Sciences 3 (6), pp. 233–242. Cited by: §I.
  • [18] S. Sra and R. Hosseini (2015) Conic geometric optimization on the manifold of positive definite matrices. SIAM Journal on Optimization 25 (1), pp. 713–739. Cited by: §II-B.
  • [19] T. Tsumugiwa, R. Yokogawa, and K. Hara (2002) Variable impedance control based on estimation of human arm stiffness for human-robot cooperative calligraphic task. In IEEE International Conference on Robotics and Automation, Vol. 1, Washington, DC, USA, pp. 644–650. Cited by: §I.
  • [20] A. Ude, B. Nemec, T. Petrić, and J. Morimoto (2014) Orientation in cartesian space dynamic movement primitives. In 2014 IEEE International Conference on Robotics and Automation, pp. 2997–3004. Cited by: §I.
  • [21] N. Vahrenkamp, T. Asfour, G. Metta, G. Sandini, and R. Dillmann (2012) Manipulability analysis. In 12th IEEE/RAS international conference on humanoid robots (humanoids), pp. 568–573. Cited by: §I.
  • [22] O. Yair, M. Ben-Chen, and R. Talmon (2019) Parallel transport on the cone manifold of spd matrices for domain adaptation. IEEE Transactions on Signal Processing 67 (7), pp. 1797–1811. Cited by: §II-B.