Temporally Coupled Dynamical Movement Primitives in Cartesian Space Martin Karlsson* Anders Robertsson Rolf Johansson Abstract— Control of robot orientation in Cartesian space implicates some difficulties, because the rotation group SO(3) is not contractible, and only globally contractible state spaces support continuous and globally asymptotically stable feedback control systems. In this paper, unit quaternions are used to rep- resent orientations, and it is first shown that the unit quaternion set minus one single point is contractible. This is used to design a control system for temporally coupled dynamical movement primitives (DMPs) in Cartesian space. The functionality of the control system is verified experimentally on an industrial robot. I. INTRODUCTION Industrial robots typically work well for tasks where accurate position control is sufficient, and where work spaces and robot programs have been carefully prepared, so that Fig. 1: The ABB YuMi robot [16] used in the experiments. hardware configurations can be foreseen a priori by robot programmers in each step of the tasks. Such preparation is very time consuming, and introduces high costs in terms of made practically realizable in [13], and proven exponentially engineering work. Further, the arrangements are sensitive to stable in [14]. However, these previous results are applicable variations, e.g., uncertainties in work object positions, small only if the robot state space is Euclidean, which is not true differences between individual work objects, etc. This has for orientation in Cartesian space. Higher levels of robot prohibited the automation of a range of tasks, including control typically operate in Cartesian space, for instance to seemingly repetitive ones such as assembly tasks and short- control the pose of a robot end-effector or an unmanned series production. aerial vehicle. It would therefore be beneficial if the capabilities of In this paper, we therefore address the question of whether robots to adapt to their surroundings could be improved. the control algorithm in [13] could be extended also to The framework of dynamical movement primitives (DMPs), incorporate orientations. Because a contractible state space used to model robot movement, has an emphasis on such is necessary for design and analysis of a continuous globally adaptability [1]. For instance, the time scale and goal position asymptotically stable control law (see Sec. II), we first of a movement can be adjusted through one parameter each. investigate the contractibility properties of the quaternion The fundamentals of DMPs have been described in [1], and set used to represent orientations. A space is contractible earlier versions have been introduced in [2], [3], [4]. DMPs if and only if it is homotopy equivalent to a one-point space have been used to modify robot movement based on moving [15], which intuitively means that the space can be deformed arXiv:1905.11176v1 [cs.RO] 27 May 2019 targets in the context of object handover [5], and based continuously to a single point; see, e.g., [15] for a definition on demonstrations by humans [6], [7], [8], [9]. In most of of homotopy equivalence. the previous research, it has been assumed that the robot configuration space is a real coordinate space, such as joint space or Cartesian position space; see, e.g., [5], [6], [8], [10], A. Contribution [11]. However, in [12] DMPs were formulated for orientation This paper provides a control algorithm for DMPs with in Cartesian space. temporal coupling in Cartesian space. It extends our previous Temporal coupling for DMPs enables robots to recover research in [13], [14] by including orientation in Cartesian from unforeseen events, such as disturbances or detours space. Equivalently, it extends [12] by including temporal based on sensor data. This concept was introduced in [1], was coupling. Furthermore, it is shown that the quaternion set * The authors work at the Department of Automatic Control, Lund minus one single point is contractible, which is a necessary University, PO Box 118, SE-221 00 Lund, Sweden. property for design of a continuous and globally asymptoti- [email protected] cally stable control algorithm. Finally, the theoretical results The research leading to these results has received funding from the Vinnova project Kirurgens Perspektiv. The authors are members of the LCCC are verified experimentally on an ABB YuMi robot; see Linnaeus Center and the ELLIIT Excellence Center at Lund University. Fig. 1 and [16]. TABLE I: Notation used in this paper. All quaternions a restriction of f. Since a restriction of a homeomorphism represent orientations and are therefore of unit length. For is also a homeomorphism [23], f2 is a homeomorphism, and such quaternions, the inverse is the same as the conjugate. hence X n p =∼ Y n f(p). Notation Description We will also use that homeomorphism preserves con- H Unit quaternion set tractibility. n n+1 S 2 R Unit sphere of dimension n ∼ 3 Lemma 3: If X = Y , and X is contractible, then Y is ya 2 R Actual robot position 3 g 2 R Goal position also contractible. 3 yc 2 R Coupled robot position Proof: Since X =∼ Y , they are homotopy equivalent qa 2 H Actual robot orientation according to Lemma 1. In turn, X is contractible and qg 2 H Goal orientation qc 2 H Coupled robot orientation therefore homotopy equivalent to a one-point space. Hence 3 !c 2 R Coupled angular velocity Y is also homotopy equivalent to a one-point space, and q0 2 H Initial robot orientation therefore contractible. h Quaternion difference space dcg 2 h Difference between qc and qg 3 B. The quaternion set minus one point is contractible z; !z 2 R DMP states + αz; βz; kv; kp 2 R Constant control coefficients n + First, it will be shown that the unit sphere (see τ 2 R Nominal DMP time constant S + τa 2 R Adaptive time parameter Definition 1) minus a point is contractible. This will then + 3 x 2 R Phase variable be applied to H, which is homeomorphic to S [24]. + αx; αe; kc 2 R Positive constants 6 Definition 1: Let n be a non-negative integer. The unit f(x) 2 R Learnable virtual forcing term 3 sphere with dimension n is defined as fp(x); fo(x) 2 R Position and orientation components + Nb 2 Z Number of basis functions n n+1 6 = p 2 j kpk = 1 Ψj (x) 2 R The j:th basis function vector S R 2 (1) 6 n n wj 2 R The j:th weight vector Theorem 1: The unit sphere minus a point p 2 , 3 S S e 2 R × h Low-pass filtered pose error denoted n n p, is contractible. e 2 3 Position component of e S p R Proof: n ≥ 1 eo 2 h Orientation component of e Consider first the case . There exists a 3 n n y¨r; !_ r 2 R Reference robot acceleration mapping from S n p to R called stereographic projection 22 3 ξ 2 R × h DMP state vector from p, which is a homeomorphism. Thus, Sn np =∼ Rn [25], q¯ 2 H Inverse of quaternion q n ' Homotopy equivalence [18]. See Fig. 2 for a visualization of these spaces. Since R =∼ Homeomorphic relation is a Euclidean space it is contractible, and it follows from Lemma 3 that Sn n p is also contractible. Consider now the case n = 0. The sphere S0 consists of the pair of points {−1; 1g according to Definition 1. Thus II. A CONTRACTIBLE SUBSET S0 n p consists of one point only, and homotopy equivalence OF THE UNIT QUATERNION SET with a one-point space is trivial. Hence S0 np is contractible. The fundamentals of mathematical topology and set theory are described in, e.g., [15], [17], [18]. As noted in [19], the Remark 1: Albeit we consider unit spheres in this paper, rotation group SO(3) is not contractible, and therefore it is it is not necessary to assume radius 1 in Theorem 1. Further, not possible for any continuous state-feedback control law it is arbitrary which point p 2 Sn to remove. to yield a globally asymptotically stable equilibrium point in Theorem 2: The set of unit quaternions H minus a point SO(3) [20], [21]. Contractibility is also necessary to apply the q~ 2 H, denoted H n q~, is contractible. contraction theory from [22], as done in [14]. In this paper, Proof: The set H is homeomorphic to S3 [24]. There- unit quaternions are used to parameterize SO(3). Similarly to fore H n q~ =∼ S3 n p for some point p 2 S3, according SO(3), the unit quaternion set, H, is not contractible. In this to Lemma 2. Theorem 1 with n = 3 yields that S3 n p section however, is is shown that it is sufficient to remove is contractible, and because of the homeomorphic relation, one point from H to yield a contractible space. Table I lists Lemma 3 yields that H n q~ is also contractible. some of the notation used in this paper. It is noteworthy that the contractible subset H n q~ is the largest possible subset of , because one point is the smallest A. Preliminary topology H possible subset to remove. Hence, it is guaranteed that no un- We will use that homeomorphism (defined in, e.g., [17]) necessary restriction is made in Theorem 2, though there are is a stronger relation than homotopy equivalence. other, more limited, subsets of H that are also contractible. Lemma 1: If two spaces X and Y are homeomorphic, Sometimes only half of H, for instance the upper half of then they are homotopy equivalent. the quaternion hypersphere, is used to represent orientations.