A New Trajectory Deformation Algorithm Based on Affine Transformations

1 Introduction

Trajectory deformation is an important topic in robotics and computer graphics. For instance, in order to deal with unforeseen obstacles or perturbations of the target or of the robot's state, it is sometimes more advantageous to deform a previously planned trajectory rather than to re-compute entirely a new one [Lamiraux et al., 2004; Seiler et al., 2010]. In motion-capture-based applications, deforming previouslyrecorded trajectories - e.g. to adapt them to a different environment, to retarget them to a different character [Rose et al., 1996; Gleicher, 1998; Lee and Shin, 1999], or to transfer them to a humanoid robot [Yamane and Nakamura, 2003a; Ude et al., 2010] - is the only viable option, for one cannot reasonably record beforehand all the motions with the desired kinematic and dynamic properties.

∗The paper on which this extended abstract is based was the recipient of the best paper award of the 2012 Robotics: Science and Systems (RSS) conference [Pham and Nakamura, 2012a].

A fundamental requirement for trajectory deformation methods is that they should preserve the characteristic features of the original trajectory. Such features may include trajectory smoothness, periodicity, optimality,. . . or - in humancentered applications - human-characteristic properties, so that the deformed robot motions are favorably perceived by humans, or that retargeted animations retain their natural, human-like, expressions.

Existing approaches

Several approaches exist for trajectory deformation. In spline-based approaches, a deformation is made by altering the coefficients multiplying the basis splines [Ude et al., 2000] or by adding to the original trajectory a displacement map - which is a sum of splines [Gleicher, 1998; Lee and Shin, 1999]. Another approach is based on the encoding of the original trajectory by an autonomous nonlinear dynamical system [Ijspeert et al., 2002; Ude et al., 2010]. A deformation is then made by altering the coefficients multiplying the basis functions that appear in the definition of the dynamical system. These two approaches are similar in that they make use of exogenous basis functions: splines in the splinebased approach, Gaussian kernel functions in the dynamicalsystem-based approach. A first, prevalent, difficulty then consists of choosing the appropriate bases for a particular task. Secondly, adding artificial functions to a natural movement can produce undesirable behaviors, such as large spline undulations in spline-based approaches [Lee and Shin, 1999; Ude et al., 2000], lack of smoothness, etc. - which call for supplementary and often costly efforts to correct.

Affine invariance in human perception and action

As mentioned previously, introducing artificial exogenous basis functions may destroy important characteristic features of the original human trajectories. One such feature is the inverse relationship between curvature and linear velocity quantified by the two-thirds power law (cf. e.g. [Lacquaniti et al., 1983]): in planar drawing movements, the angular velocity (a) of the hand and the trajectory curvature (κ) were shown to be robustly related by: a(t) = γκ(t)2/3 , with γ being a constant.

It was subsequently observed that a motion obeying the two-thirds power law has in fact a constant equi-affine linear velocity [Pollick and Sapiro, 1997], see also section 3. This crucial observation triggered a number of important developments. First, the law of constant equi-affine velocity was tested and confirmed - to some extent - in 3D hand movements [Maoz et al., 2009; Pollick et al., 2009]. In a more theoretical avenue, this law was conceptualized within the broader framework of affine invariance: trajectories generated by humans were understood as being invariant with respect to certain groups of transformations - affine, equi-affine or Euclidean (the latter two being subgroups of the former) [Bennequin et al., 2009]. Experiments on hand and locomotor movements lent support to this view [Bennequin et al., 2009; Pham and Bennequin, 2012]. Finally, the origin of affine invariance in human movements was theorized in relation to the previously established existence of affine invariance in the human visual system (which guarantees the unity of the perception of an object observed from different viewpoints [Koenderink and Van Doorn, 1991]) and to the coupling between perception and action (as argued in e.g. [Pollick and Sapiro, 1997; Bennequin et al., 2009]). In particular, it was reported that a motion is judged to be more natural by a human observer if it obeys the two-thirds power law (hence equi-affine invariance) [Viviani and Stucchi, 1992].

Our approach

Inspired by this body of results, we propose here to deform a given trajectory by applying affine transformations on parts of it. Thus, the deformed trajectory preserves by construction the affine-invariant features of the original trajectory (such as smoothness, periodicity,. . . or more specifically, affine velocity) which, in the light of the previous discussion, may be particularly relevant in the effort to make transfered or retargeted motions look more natural. Furthermore, since the only "basis functions" are the time-series of the original trajectory coordinates, no artifacts (such as large spline undulations in spline-based approaches) is introduced. However, despite this small functions basis, the method still allows accommodating a variety of other objectives, such as optimization of geometric or dynamic costs or satisfaction of various equality and inequality constraints, by leveraging the extra redundancy offered by the affine transformations. Finally, the algorithm to calculate the relevant affine deformations is extremely efficient and easy to implement: exact, closed-form, solutions are often available, and there is no need to re-integrate even a part of the trajectory.

In section 2, we present the core algorithm which allows deforming a trajectory to reach a new target configuration while satisfying various types of constraints. In section 2.3, we show how to leverage the extra redundancy offered by the affine transformations to optimize geometric (such as closeness to the original trajectory or deformation rigidity) or dynamic costs (such as joint torques). In section 2.4, we give a characterization of trajectory redundancy by the group of admissible deformations, revisiting thereby the concept of kinematic redundancy and suggesting a new theoretical approach to motion planning. As illustration, we present, in section 3, a method for transferring human motions to a humanoid robot while preserving the equi-affine velocity and near timeoptimal trajectory planning for pick-and-place tasks. Finally, in section 4, we discuss the advantages and drawbacks of our method, as well as directions for further developments.

2 Affine trajectory deformation framework

2.1 Affine spaces and affine deformations

An affine space is a set A together with a group action of a vector space W. An element w ∈ W transforms a point θ ∈ A into another point θ′ by θ′ = θ +w, which can also be noted θ′ - θ = w or -→ θθ′ = w.

Given a point θ0 ∈ A (the origin), an affine transformation F of the affine space can be defined by a couple (w,M) where w ∈ W and M is a non-singular linear map W → W. The transformation F acts on A by

Note that, if θ0 is a fixed-point of F, then F can be written in the form

If A and W are in fact Rn , then the set of affine transformations F form a Lie group of dimension n2 + n, called the General Affine group and denoted GA(n).

We shall also consider two subgroups of GA(n):

• The special equi-affine group, of dimension n2 + n - 1 and denoted SEA(n), which consists of affine transformations whose M have determinant 1.
• The special Euclidean group, of dimension n(n + 1)/2 and denoted SE(n), which consists of affine transformations whose M are orthogonal and have determinant 1.

Consider now a given trajectory θ(t)t∈[0,T], which may represent e.g. the Cartesian coordinates of a manipulator's end-effector, the joint angles of a humanoid robot, or the position of a mobile robot in the plane. We say that a transformation F deforms θ(t)t∈[0,T] into θ′ (t)t∈[0,T] at time instant τ if

If F is affine (respectively equi-affine, Euclidean - we drop the term "special" for convenience), we say that the deformation is affine (respectively equi-affine, Euclidean).

The idea is to play with the time instant τ and the transformation F to achieve the desired trajectory corrections while respecting constraints or optimizing costs. This is discussed below.

2.2 Constraints

Smoothness constraints at the deformation instant

Assume that the trajectory θ(t)t∈[0,T] is of dimension n and is C p , that is, differentiable p times with a continuous p-th derivative. Consider an affine transformation F that deforms θ(t)t∈[0,T] into θ′ (t)t∈[0,T] at a time instant τ. We say that F is C p -preserving if the resulting θ′ (t)t∈[0,T] is also C p .

Since F is a smooth application, it is clear that C ′ (t)t∈(τ,T] - note that the interval is open at τ - is also C p . Regarding the time instant τ, the continuity (C0 ) of θ′ (t)t∈[0,T] imposes that F(θ(τ)) = θ(τ). Thus F can be written in the form of equation (1) with θ(τ) replacing θ0.

Next, for i = 1 . . . p, let us note ui = di θ dti (τ), u1 being the velocity vector at time τ , u2 being the acceleration vector, etc. Then the conditions for the continuities of the 1 . . . p-th derivatives of θ can be formulated as

Let us consider the non-degenerate case when u1, . . . , up are linearly independent. In this case, it is possible to construct an orthonormal basis B whose first p vectors form a basis of U = Span(u1, . . . , up), for instance using a Gram-Schmidt orthonormalization procedure. From equation (2), the matrix ofMin this basis is of the form

which shows that the space of C p -preserving affine transformations at τ forms a Lie subgroup of GA(n) of dimension n(n - p) = n2 - pn, parameterized by the mi,j .

Accuracy constraints on the final position, velocity, etc.

Similarly as above, one can study how k final constraints (k = 3 for instance if one wants to constraint the final position, velocity, and acceleration of the trajectory) define the space of possible affine deformations. More precisely,

• If n2 - pn ≥ kn (i.e. n ≥ k + p), then it is possible to deform θ into a θ′ that satisfies k final constraints while guaranteeing C p continuity.
• Furthermore, if n > k + p, then such a θ′ is not unique. In fact, the space of affine transformations that satisfy the above conditions constitutes a Lie subgroup of GAn of dimension n2 - (k + p)n. Within this space, one can choose the deformations that optimize certain criteria, as detailed in section 2.3.

Subgroup constraints

Restriction to a subgroup of the full affine group, e.g. to the equi-affine group or to the Euclidean group (cf. section 2.1), can also be treated as equality constraints. For instance, restricting admissible transformations to the equi-affine group can be achieved by adding an extra equation det(M) = 1 to the set of constraints. A concrete example is given in section 3.

Inequality constraints

In addition to equality constraints, many applications also require the satisfaction of inequality constraints, such as joint limits, upper-bounds on the velocities, accelerations or torques, avoidance of obstacles, etc. In some cases, such constraints can be converted into inequality constraints on the coefficients of the matrix M. Then finding optimal deformations subject to equality and inequality constraints can be formulated as a Quadratic Program, which can in turn be solved efficiently using standard algorithms, cf. [Kanoun et al., 2011].

2.3 Optimization

One important optimization objective for the deformed trajectory may consist of being the "closest" possible to the original trajectory. One way to achieve this is to minimize the distance between the transformation F and the identity transformation, which in turn can be quantified by the Frobenius distance between the matrix M and I. It can be shown that this optimization can be solved in closed form using the pseudo-inverse.

It is also possible to optimize other quantities, for instance, the "rigidity" of the deformation, which can be quantified as the distance of the deformation from the Euclidean subgroup of transformations. This is particularly useful for instance in computer graphics where the preservation the global shape of the trajectory is of special importance [Shoemake and Duff, 1992]. It can be shown that such an optimization can also be solved in closed form.

Finally, optimization criteria involving dynamics parameters (such as energy consumption, torque, etc.) can also be treated. However, there is in general no closed form solution and one has to search iteratively within the space of admissible affine transformations.

2.4 Group structure

Since the affine transformations form a group, it is possible to equip the space of admissible affine deformations - i.e., the deformations that satisfy specified constraints - of a given trajectory θ(t)t∈[0,T] with a group structure, which would include a supplementary dimension: the time instants of the deformations. We call this group A(θ). This group formulation may have several interesting practical interests. For instance, the composition property allows accelerating the computation of successive deformations. The inversion property can be particularly useful for computer graphics applications: indeed, a particular requirement for interactive motion editing systems to be user-friendly is that every editing operation should be quickly reversible [Shneiderman, 1997]. Finally, the matrix representation of the admissible deformations allows searching efficiently (using random sampling techniques, gradient search, etc.) within the space of trajectory redundancy, which we discuss next.

Indeed, based on the group structure, one can more generally revisit the notion of "trajectory redundancy". "Trajectory redundancy" means that, in general, there exist an infinite number of trajectories that can accomplish a given task (see e.g. [Pham and Hicheur, 2009]). Here, we identify part of trajectory redundancy with the group of affine deformations. This interpretation provides a quantitative view on the "degree of movement freedom" of a robot, which would then be the dimension of A(θ) as a Lie group: the larger the dimension of A(θ), the more "freedom of movement" the robot enjoys and the more easily one can plan and deform its trajectories. In this view, smoothness-related constraints (C 1 , C 2 ,. . . ) or group-related constraints (equiaffine, Euclidean,. . . ) are unified in that they both reduce the trajectory deformation group of a robot to one of its subgroups. In wheeled robots for instance, the structure of car-like robots imposes the supplementary constraint of curvature-continuity, thus reducing the deformation groups (of dimension 3 in general) of omni-directional robots to subgroups of dimension 2 [Pham and Nakamura, 2012b]. On the other hand, relaxing the constant curvature constraint of bevel needle trajectories [Duindam et al., 2010] would extend the Euclidean deformation group of dimension 2 (spanned by the time instants of the deformation and the rotations around the needle axis at a given time instant) to one of its supergroups.

3 Applications to motion transfer

As mentioned in the Introduction, equi-affine velocity is an important invariant in human hand movements both in the plane and in space. The equi-affine velocity of a 3D trajectory is given by [Maoz et al., 2009]

where r(t) is the 3D coordinate of the hand at time t and |u, v, w| denotes the scalar triple product of u, v and w in R3 . It is clear that the so-defined equi-affine velocity is invariant under any equi-affine transformation applied to any part of the trajectory r(t)t∈[0,T].

To illustrate the concept of motion transfer preserving equiaffine velocity, we first recorded a hand raising motion performed by a human subject (Fig. 1, "human" panels) using a motion capture system. We reconstructed the 3D trajectory of the hand using the wrist markers (Fig. 1, red trajectories in "robot" panels). Next, we deformed this 3D trajectory using a C 1 -preserving equi-affine transformation in order to reach a new final position (n = 3, p = 1, k = 1, see Fig. 1, green trajectories in "robot" panels). Finally, using inverse kinematics, we calculated the joint angles (shoulder pitch, roll, yaw, elbow flexion) which allow a model of the HRP4 robot to track exactly (up to numerical errors) this last trajectory. By this scheme, we have thus obtained a robot motion that has the exact same equi-affine velocity profile as in the original human motion (cf. red and green profiles in Fig. 1C), but allowing to reach a different target and adapted to the kinematic structure of the robot.

4 Discussion

We have presented a new method to deform robot trajectories motivated by recent findings in human physiology characterizing affine invariance in human perception and action. The trajectories obtained by this method preserve by construction affine-invariant properties of the original trajectories. This distinctive feature of the method may thus prove particularly relevant in character animation or human-to-robot motion transfer applications.

The proposed method is very fast and easy to implement: in general it consists of some basic matrix computations. It is nonetheless very versatile: depending on the degree of "trajectory redundancy", the deformed trajectories can be chosen so as to remain close the original trajectory (with explicit bounds), to optimize a given cost, or to satisfy inequality constraints at specific time instants. These computational advantages make the method appealing for time-critical applications.

One advantage of the method, with respect to spline- or dynamic-system-based approaches, consists in that it does not require choosing an exogenous functions basis (such as hierarchical spline basis [Lee and Shin, 1999], wavelet spline basis [Ude et al., 2000], Gaussian kernels [Ijspeert et al., 2002; Ude et al., 2010]): indeed, the only "basis functions" we use are the original joint angle trajectories. Thus, there is no need to fine-tune the basis functions or to put extra constraints on the coefficients multiplying the basis functions in order to avoid undesirable behaviors - such as spline trajectories that undulate too much [Lee and Shin, 1999] or wavelets that have too much energy [Ude et al., 2000].

Our current research focuses on developing this framework for full-scale applications in character animation [Rose et al., 1996; Lee and Shin, 1999; Yamane and Nakamura, 2003b; Yamane et al., 2004] and humanoid robot control [Yamane and Nakamura, 2003a; Ude et al., 2010].