Coordinated multi-arm motion planning: Reaching for moving objects in the face of uncertainty

Transcription

1 Robotics: Science and Systems 216 Ann Arbor, MI, USA, June 18-22, 216 Coordinated multi-arm motion lanning: Reaching for moving objects in the face of uncertainty Seyed Sina Mirrazavi Salehian, Nadia Figueroa and Aude Billard LASA Laboratory, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland {sina.mirrazavi, nadia.figueroafernandez, Abstract Coordinated control strategies for multi-robot systems are necessary for tasks that cannot be executed by a single robot. This encomasses tasks where the worksace of the robot is too small or where the load is too heavy for one robot to handle. Using multile robots makes the task feasible by extending the worksace and/or increase the ayload of the overall robotic system. In this aer, we consider two instances of such tasks: a co-worker scenario in which a human hands over a large object to a robot; interceting a large flying object. The roblem is made difficult as the ick-u/intercet motions must take lace while the object is in motion and because the object s motion is not deterministic. The challenge is then to adat the motion of the robotic arms in coordination with one another and with the object. Determining the ick-u/intercet oint is done by taking into account the worksace of the multi-arm system. The oint is continuously recomuted to adat to change in the object s trajectory. We roose a virtual object based dynamical systems (DS) control law to generate autonomous and synchronized motions for a multi-arm robot system. We show theoretically that the multi-arm + virtual object system converges asymtotically to the moving object. We validate our aroach on a dual-arm robotic system and demonstrate that it can resynchronize and adat the motion of each arm in a fraction of a second, even when the motion of the object is fast and not accurately redictable. I. INTRDUCTIN Many daily activities involve tasks such as lifting, carrying and reaching for large and heavy objects. To accomlish these tasks, humans rely heavily on bi-manual reaching and, more generally, on coordination between the hands [6]. Performing these tasks with one arm is often infeasible, mainly because a single arm has a limited worksace. Moreover, the dexterity and flexibility required for such task is beyond a single arm s caabilities. This holds for robotic systems as well. A single robot arm is simly incaable of meeting the requirements of such comlex tasks. A dual or multi-arm robotic system, on the other hand, extends the worksace of a single robot arm. It allows for highly comlex maniulation of heavy or large objects that would otherwise be infeasible for singlearm systems. Most effort in the field of multi-arm control has focused rimarily on devising strategies for coordinated and stable maniulation of static objects that are already artially or fully grased by the multi-arm system [1, 23, 5, 21, 33, 7]. Seldom work has focused on develoing reaching strategies that a multi-arm system can use to reach and grab moving objects. ne can envision a lethora of alications in factories, airorts, or storage facilities that would benefit from such Fig. 1: Illustration of a ossible alication where a multi-arm system has to reach for large moving objects carried to them by humans. The arms move in synchrony towards the couled feasible reaching oints of the object (e.g. squares or ellises). As the object aroaches, the arms end-effectors align their trajectories with that of the object. strategies. Examles include, grabbing, catching, carrying, lifting or re-orienting ackages or large-sized arts traveling on a cart or a running conveyor belt (Fig. 1), carried by humans or even flying towards the multi-arm robot system. We osit that, until now, these alications have not yet been exlored, due to the unsuitability of the state-of-the-art to tackle the challenges of coordinating the motion of multile arms while reaching and adating to the motion of a dynamic object in a comutationally efficient way. According to studies in motor and cognitive develoment, reaching for an object in a smooth and efficient manner requires ones to deal simultaneously with different issues [27]. Each hand has to adjust to the orientation, shae and size of the object while reaching for it. Moreover, the action of grasing the object (i.e. closing the fingers of the hands) must be timed rior to rather than as a reaction to interceting the object. Hence, bi-manual, and by extension, multi-manual reaching require us to solve simultaneously several satial and temoral coordination constraints to move toward the object in coordination and to intercet the object [27]. We roose an aroach that generates coordinated trajectories for a multi-arm robot system that ensures that the arms will reach the moving object simultaneously. Most imortantly, the aroach udates the arms motion continuously and in synchrony to adat to changes in the target object s trajectory. To validate the aroach, we consider a scenario in which

2 an object with arbitrary shae and mass is moving towards a multi-arm system. We do not assume a known model of the dynamics of the object. To simulate an arbitrary object s motion, not easily redictable, our exeriments have a human carry the object toward the robot while being blindfolded. The sole knowledge about the object is its couled feasible reaching oints, which are the referred reaching ositions and orientations on the object, secified by the user (see Fig. 1). A multi-arm reach is thus deemed successful if and only if all robot arms simultaneously intercet the object on its feasible reaching oints. To achieve this, we must solve for the aforementioned constraints that constitute a multi-arm reach. From a robotics oint of view, these constraints can be translated to object level constraints (i.e. task constraints) and arm level constraints (i.e. coordination constraints). Task constraints imose osition and velocity constraints at the object s intercetion. Position constraints ensure that the lanned motion of each robot s end-effector is coordinated with the feasible reaching ositions of the object. Whereas the velocity constraints allow readjustments of the hand, alm and fingers osture while there are uncertainties in the hand orientation and osition. n the other hand, coordination constraints imose that the robots move in coordination with each other. This is necessary not only to ensure that the arms simultaneously intercet the object, but also to avoid collisions between their end-effectors while they adat to the moving object s motion. Handling multile constraints simultaneously is a roblem that could be addressed by using standard otimal control aroaches. These, however, are time consuming and will not converge within a few milliseconds, which is the exected reaction time necessary for all arms to raidly and simultaneously reach and adat to the moving object. In this aer, we use autonomous dynamical systems (DSs) to instantaneously re-lan the coordinated motion for each K r -th hand-arm robot in a multi-arm system [2, 11, 18]. The roosed dynamical system coules both task and coordination constraints by modeling the motion of a virtual object, which is exressed as a Linear Parameter Varying (LPV) system subject to stability constraints to ensure that the object is simultaneously interceted by all the arms at the desired time, osition and velocity. A theoretical analysis of the stability and convergence of this roosed DS based control law is resented and emirically validated on a realworld dual-arm exeriment with a KUKA IIWA and a KUKA LWR 4+, both 7 degree of freedom robot arms mounted with a 16 DF (Degree of Freedom) Allegro hand and a 4 DF Barret hand, resectively. II. RELATED WRK Although coordinating multile robotic arms for object maniulation has been extensively studied in robotics literature, lanning the reach-to-gras motion for a moving object with a multi-arm robotic system, while keeing coordination constraints, is a new field of research. Recently, Vahrenkam et al. [25, 24] roosed an RRT-based algorithm to generate collision free motions to gras an object at rest with a bi-manual latform. Given its search-based strategy, this aroach can guarantee feasible grass by both arms and self collision avoidance. However, due to its comutational comlexity and the fact that it cannot guarantee simultaneous intercetion of the object by all arms, it becomes inadequate when trying to reach for a moving object. To the best of the author s knowledge, there is no related work in the field of reaching for large moving object with multi-arm systems. Extensive surveys on coordination strategies for dual/multiarm systems are resented in [21, 31]. Early aroaches addressed the coordination roblem with master-slave strategies. In the work of [16], the motion of the master robot is lanned and always known, whereas the slave robot must follow the master s motion while conforming with closedchain geometrical constraints. n the same track, Gams et al. [9] roosed a control architecture that addresses the task of lifting an unknown object with a dual-arm system. It uses feedback errors constructed from osition and velocity comonents to synchronize the motion of one arm to a redefined motion of the other arm. Even though this strategy is comutationally efficient, erformance is adversely affected when communication delays arise. Moreover, when dealing with moving objects, a difficulty arises when the master and slave roles have to be assigned to the arms. Centralized controller strategies can address some of these issues [1, 22, 28]. These strategies consider the robots and the maniulated object as a closed kinematic chain. In this line, Wimböck and tt [32] roosed an imedance control architecture for dual-arm maniulation, where the two endeffectors and a virtual frame, which is a function of the endeffectors oses, are couled via satial srings. Zhu [34] roosed a motion synchronization controller to coordinate the end-effectors of two robots when they are rigidly or flexibly holding an object without ayload. Likar et al. [14] roosed a velocity level motion synchronization algorithm for controlling a cooerative dual-arm system. They introduced an augmented kinematic chain which is a reresentation of two arms and the object. The corresonding Jacobian is calculated to control the augmented kinematic chain by solving the inverse kinematics roblem at the velocity level. n the other hand, some researchers have roosed decentralized control architectures, where the robots are controlled searately by their own local controllers [15]. Caccavale et al. [4] roosed a combination of a centralized and decentralized imedance control strategies to achieve a desired imedance at the object and the end-effector level, resectively. All reviously mentioned works assume that the object is firmly attached to the robots and modeled via a virtual object frame or by closing the kinematic chain. In this work, we adot the idea of the virtual object to coordinate the motion of the robots with each other and further, use it to coordinate the robots to the real object, albeit it is not attached to the arms until they reach it. The term virtual object is mostly used in robotics literature to reresent the internal forces of a grasing task [29, 3]. In this aer, however, this term is used in the framework of motion coordination. The motion of the virtual

3 TABLE I: Notation Fig. 2: Block diagram for coordinated multi-arm motion lanning for reaching a large moving object. Where K r reresents the total number of robot arms. In this aer, we assume that the low-level controller of the robot is a erfect tracking controller. object is coordinated and aligned to the motion of the real object to satisfy the task constraints. Concurrently, it is used to coordinate the motion of the robots through a centralized controller, which is governed by the virtual object s motion and robust to erturbations from any of th K r -th robot arms. III. MULTI-ARM REACHING FR MVING BJECTS In order to achieve stable and coordinated reaching motions for multile arms when the target object is in motion, several roblems need to be solved simultaneously: (i) rediction of the object s trajectory; (ii) comuting intercet oints for each arm and (iii) lanning coordinated motion of the robot arms towards their corresonding intercet oints. An overview of the roosed framework is illustrated in Fig. 2. As seen on the illustration, sub-comonent (A) comutes the intercet oint and lans the trajectory to the intercet oint. It uses an estimate of the reachable worksace of the multi-arm system (learned off-line rior to exeriment) and an on-line ste in which it continuously measures the object s ose from a visual tracking system. Sub-comonent (B) uses the intercet oints redicted from (A) and the current end-effector oses to generate the desired end-effector oses of the K r -robot multiarm system. It is based on a centralized controller that exloits a forward model of the virtual object s motion. A detailed descrition of each of these sub-comonents is resented in the following subsections. For simlicity and racticality, we summarize the most relevant notation used throughout the aer in Table I and illustrate them in Fig.3. A. bject Trajectory and Intercet Point Prediction As stated reviously, the roosed control architecture coordinates and aligns the motion of a virtual object attached to the end-effectors of the K r robots to the motion of a real object (Fig.3). The real object is reresented by a set of feasible reaching oints (ξ 1,ξ 2 R3 ), defined by the user. Their resultant vector (ξ (t) R 3 ) corresonds to the osition of the object. Its orientation (ξ o (t) S(3)) is estimated from the ositions of the reaching oints. The virtual object is a dulicate of these set of oints, with the end-effectors of each robot attached to the virtual reaching oints (ξ V 1,ξV 2 R3 ) by zero length srings and damers (Fig.3). nce the real object starts moving towards the robots, carried or thrown by an oerator, the virtual object follows its motion. A forward Variable Definition ε A small ositive number. K A large constant ositive number. K r Number of robot arms. ξ j R Pose of the j th end-effector for j 1,...,K r ξ V j j th reaching oint (ose) on the virtual object. ξ V, ξo V Position and orientation of the virtual object ξ,ξo Position/orientation of the real object. ω Angular velocity of the virtual object ξ j j th feasible reaching oint on the real object. j ξ ξ in the reference frame of j th robot base. θ W Set of K r worksace models θ W = {θi W i 1,...,K r } θi W GMM arameters for each worksace θi W = {π l i, µ l i,σ l i } π l i, µ l i,σ l i Priors, means and covariance matrices for GMM with l = 1...Ki w Gaussian functions. x V (t),x V o (t) States of the virtual object s dynamical system. x R j States of j th end-effector. θ A,θ A Scheduling arameters for os./orient. dynamics. A i (θ Ai ) Affine deendent state-sace matrices for i {,o}. u (t),u o (t) Control inut for the os./orient. virtual object dynamics. γ (t),γ o (t) Coordination arameters for the virtual object dynamics. model of the virtual object comutes its rogress ahead of time and determines a oint along this trajectory where the object will become reachable by all robot arms. We name this oint the feasible intercet oint. Using the lanned osition and orientation of the virtual object at intercet oint (ξ V (t) R 3, ξ V o (t) S(3)), we can then generate the desired reaching motions for each arm. If the motion of the object is redictable, one can use a model-based rediction aroach [13] and find feasible intercet ostures by extending a single-robot arm feasible osture extraction algorithm (such as [12]) to K r -robot arms. In this case, a simle oint-to-oint motion can be devised to intercet the object. As we assume that the motion of the object is not accurately redictable (e.g when carried by a blind-folded human oerator as in our exeriments); using a model-based aroach would be imractical and limiting. Instead, we use a simle ballistic motion algorithm to redict the motion of the real object and roose a controller (Sec. III-B) that is comliant and robust to rediction inaccuracies. To model the motion of the virtual object, we must first make sure that the object travels through the worksace of the robots; i.e. there is at least one feasible intercet oint. In order to find the feasible intercet oint, the kinematic feasibility of the redicted reaching oints must be evaluated. The reachable worksace of each robot is modeled via a robabilistic classification model j ( j ξ j ;θw j ) j {1,...,K r }, namely a Gaussian Mixture Model as follows: j ( j ξ j;θ W j ) = K w j l=1 π l j N ( j ξ j µ l j,σ l j ) (1) where π l j, µ l j,σ l j corresond to the rior, mean and covariance matrix of the l = {1...K w j } Gaussian functions, resectively, estimated by using the Exectation-Maximization algorithm [3]. In order to generate the training dataset, all ossible ostures of each robot are simulated by testing all ossible dislacements of their joints. δ j is the minimum likelihood threshold and it is determined such that the likelihood

4 (3b) x Vo (t) = Ao (θao (xvo (t)))xvo (t) + uo (t) h it Where xv (t) = ξ V (t) ξ V (t) and xvo (t) = ξov (t) are the states of the dynamical system.2 θai RKi 1 i {, o} are vectors of scheduling arameters3 ; h it θai = θai1... θaiki i {, o} (4) Ai (.) : RKi 1 R j j (i, j) {(, 6), (o, D)} are the affine deendence of state-sace matrices on the scheduling arameter and the state vectors: K A (θa ) = θa k A k A k R6 6 θa k R1 1 D D 1 1 k=1 Ko Ao (θao ) = (5) Aok R θaok Aok θaok R k=1 Fig. 3: An illustration of the variables defined in the aer is resented. The reachable areas are feasible areas for grasing the object. Excet for 2 ξ 2 and 1 ξ 1, the variables are exressed in the reference frame located on the T desired intercet oint; i.e. ξi (T ) =... i {, o}. Note that ω is the angular velocity of the virtual object, which is different from the numerical differentiation of the virtual object orientation; i.e. ω 6= ξ ov. of 99% of the training oints is higher than the threshold δ j. If j ( j ξ j ; θ jw ) exceeds it, j ξ j is classified as a feasible configuration. As the reachable worksace of each robot is statistically indeendent from the others, we can calculate the joint distribution of all the models by comuting the roduct of all distributions: (1 ξ 1,..., ξ K ; θ W ) = i ( j ξ j ; θ jw ) r ne oular choice to aroximate the arameters of LPV systems is regression models; i.e. olynomial, eriodic functions [2] or Gaussian Mixture Regression (GMR) [19]. In this aer, an aroach similar to that offered in [19] is taken, which inherently results in a normalized scheduling i of arameters; i.e. < θai j 1, Kk=1 θaik = 1 (i, j) (, 1),..., (, K ), (o, 1),..., (o, Ko ). As discussed in Section III-A, the virtual object and consequently the arms must simultaneously intercet the object at the feasible reachable oints. To achieve this, we define the following control inuts (u, uo ) for the osition and orientation of the virtual object: 1 x γ (t) A (θa )xγ (t) + (U j ω? R j ) u (t) = + 1 (2) Similarly the minimum joint likelihood threshold is δ = (T ),..., ξ (T ); θ W ) exceeds it, δ j and if (1 ξ 1 (ξ (T ) = 1 j ξ j (T )) is classified as the feasible in- tercet oint. As the motion of the object is not redicted accurately, there is no guarantee that the object will ass through the redicted oint at T, yet we can ensure that the object is moving toward the robots. B. Centralized Robot Coordinated Motion Generator To model the osition and the orientation of the virtual object, we consider the following class of fully observable continuous-time LPV based DSs [26]. LPV systems can be considered as a non-linear combination of linear systems which are valid at articular oerating oints. Formulating the motion of the virtual object as a LPV system allows modeling comlex and nonlinear behaviors and the use of many tools from the linear systems theory for analysis and control [8].1 x V (t) = A (θa (xv (t)))xv (t) + u (t) (3a) 1 As the states of the virtual object are fully observable, y (t) = xv (t) and yo (t) = xvo (t) where yi (t) i {, o} are the oututs. A (θa )xv (t) + 1 (6a) uo (t) = γo (t)x o (t) + γ o (t)xo (t) γo (t)ao (θao )xo (t) (6b) h it (t) = Where xγ γ (t)ξ (t) γ (t)ξ (t) + γ (t)ξ (t) 4 and xo (t) = ξo (t). The origin ist located on the desired intercet oint ξi (T ) =... i {, o}. x o (t) is the numeri T cal differentiation of xo (t). ω? R j = ω r j ω (ω r j ), where r j = ξ V j (t) ξ V (t). < γi (t) < 1 i {, o} are the coordination arameters and are of class C1. U j is the interaction effect of the tracking controller of the jth endeffector on the virtual object: U j = x Rj (t) + ARj (xv j (t) xrj (t)) (7) 2 The dynamics of the osition and orientation of the virtual object are resented in the acceleration and velocity level due to imlementation constraints. However, the framework is not theoretically constrained to this. 3 The scheduling arameters can be a function of time t, the states of the system xvi (t) i {, o} or external signals d(t), i.e. θa1 (t, ξ (t), d(t)). In the rest of the aer, we assume that it is only function of the states of the system xvi (t) i {, o} and the arguments are droed for simlicity. 4 x (t) is the derivative of x (t) with resect to time; x (t) = γ γ h it γ γ (t)ξ (t) + γ (t)ξ (t) γ (t)ξ (t) + 2γ (t)ξ (t) + γ (t)ξ (t)

5 Where A R j R6 6 is a constant matrix. The desired motion of the j th end-effector (x R j (t)) is calculated based on the tracking error between the osition of j th oint on the virtual object (x V j (t)) and the end-effector: ẋ R j (t) = ẋ V j (t) A R j (x V j(t) x R j (t)) (8) By substituting, (6a) and (7) into (3a) and (6b) into (3b), we have: ẋ V (t) = 1 ( ẋγ + A (θ A )(x V (t) x K r + 1 γ(t)) K r ) (9a) + (ẋ R j (t) + A R i (x V j (t) x R j (t)) ω R j ) ẋ V o (t) = γ o (t)ẋ o + γ o (t)x o (t)+a o (θ Ao )(x V o (t) γ(t)x o ) (9b) Theorem 1: The dynamical systems given [ by (9a) and (9b) asymtotically converge to γi (t)ξi (t) γ i (t) ξ (t) + γ i (t)ξi (t) ] T i {, o} and the j th end-effector asymtotically converges to the j th reaching area on the virtual object i.e. lim t xr j (t) x V j(t) = (1) lim t ξv i (t) γ i (t)ξi (t) = (11) lim ξ V t i (t) (γ i (t) ξ i (t) + γ i (t)ξi (t)) = (12) if there are Pi V, Pj R, QV i, QR j such that: Pi V Pj R Q V i Q R j Pi V A ik + A T ik Pi V Q V i k {1,...,K i } Pj R A R j + A R T j P R j Q R j j {1,...,K r } θ Aik 1, i {,o} (13) where and refer to ositive and negative definiteness of a matrix, resectively. Proof: see Aendix A. 5 If we set the coordination arameters γ i (t) = γ i (t) = i {, o}, (9) generates asymtotically stable motions towards the redicted intercet oint: i.e. the coordination between the robots is reserved, but the coordination between the robots and the object is lost. n the other hand, if γ i (t) = 1 and γ i (t) = i {,o}, (9) will generate asymtotically stable motions towards the real object even though its motion is not accurately redicted: i.e. erfect coordination with the object. 6 However, in this case, there is no guarantee that the virtual object converges to the real object by following a ath which is kinematically feasible for all arms, i.e. due to the robots constraints, the coordination between the robots is lost. Thus, one can vary the values of the coordination arameters 5 Due to sace limitation, only the roof for the case i = is resented. Stability and convergence roof for i = o is similar to i =. 6 We assume that the dynamical system (9) is fast enough to converge to an accetable neighborhood around the desired trajectory γ [ ξi (t) ξ i (t) ] T before T ; i.e. ξi V (T ) γξi (T ) ε i {,o} between zero and one such that they are one at the vicinity of the redicted intercet time; e.g.: 1 γ γ = ξ (t) ξ (T ) + ε = 1 γ ξ (14a) (t) + ε 1 γ o = (14b) 1 + e K((1 ξ1 (t),..., ξ (t);θ W )) δ). (14a) imroves the robustness of the multi-arm reaching motion in face of inaccuracies in the object s motion rediction as it ensures that when the object is close enough to the feasible reaching ositions, the virtual object converges to the real object and erfectly tracks it; i.e. γ i (T ) = 1 i {,o}. Hence, the robots can simultaneously track the desired reaching oints on the object in coordination. (14b) states that the orientation of the virtual object tracks the orientation of the object when the reaching oints of the object are in the worksace of the corresonding robot. (14b) is advantageous in two ways. First, when the object is far from the robots, the virtual object does not rotate around itself hence ω. This significantly simlifies (9a). Second, if there are more than one couled reaching oints, (14b) is advantageous in that each robot only converges to the object by interceting it from the corresonding feasible reaching oint, avoiding jums in the end-effector from one reaching oint to another. A. Exerimental Setu IV. EMPIRICAL VALIDATIN The roosed framework is imlemented on a real dualarm latform, consisting of two 7 DF robotic arms, namely a KUKA LWR 4+ and a KUKA IIWA mounted with a 4 DF Barrett hand and a 16 DF Allegro hand. The distance between the base of the two robots is [ ] T m. The robot imlementation involves converting the outut of the dynamical system (8) (e.g. the desired motion of each K r - th end-effector) into a 7-DF joint state (for each arm, see Fig. 2) using a velocity based control method without joint velocity integration [17]. In order to avoid high torques, the resulting joint angles are filtered by a critically damed filter. The robot is controlled at a rate of 5 Hz. The fingers are controlled with joint osition controllers. All the hardware involved (e.g. arms and hands) are connected to and controlled by one 3.4- GHz i7 PC. The osition of the feasible reaching oints of the objects are catured by an titrack motion cature system from Natural oint at 24 Hz. Since the control loo is faster than the motion cature system, the redicted osition of the object is used as the object osition in (6), when the current osition of the object is not available. ur emirical validation is divided into three arts that demonstrate the controller s ability: (i) coordinate the multiarm systems; (ii) adat the two arms motions in coordination so as to reach to grab a large moving object, when introducing unredictability in the object s motion (by having the object be carried by a blindfolded human) and (iii) raidly adat bimanual coordination to intercet a flying object, without using a re-defined model of the object s dynamics. A corresonding video is available on-line: htts://youtu.be/ufucwrga7k8

6 Y [m] Y [m] Time [s] Y [m] 4 Time [s] Time [s] Fig. 6: Different examles of the osition of the end-effectors generated by the dynamical system (9a). Due to the sace limit, only the trajectories along y axis is resented. The illustrated object trajectory is the redicted trajectory of the uncaught object. The rediction of the box s trajectory requires some data to be initialized and uses almost all of the first.2 meter of the object in x. The initial value of γp is. and (14) is used to change it with resect to the distance of the object to the robots. As exected, the oututs of (9a) first converges to desired intercet osition, since γ is a small value, then it softly intercets the object s trajectory and follow the object s motion. The robots are stoed if the object is not moving or the fingers are closed. (a) (b) (c) Fig. 4: Snashots of the video illustrating coordination of the arms in free sace. The real object is outside the worksace of the robots; hence, the coordination arameter γi is close to and arm-to-arm coordination is favored. The human oerator erturbs one of the arms, which leads the other arm to move in synchrony following the motion of the virtual object attached to the two end-effectors. (a) (b) Fig. 7: Snashots of the robots motion when reaching for a moving object, carried by a blindfolded oerator. (a) onset of object trajectory s rediction. (c) arms have interceted the object and the fingers have closed on the object. (a).39s (a) (b) (c) (b).11s (c) Fig. 5: Snashots of the coordination caabilities between the arms and a moving/rotating object. The real object is inside the worksace of the robots; hence, the coordination arameter γi is close to 1 and the arms-to-object coordination is favored. The bottom figures show the real-time visualization of the robots, and the virtual (green) and real object (blue). B. Coordination Caabilities The first scenario is designed to illustrate the coordination caabilities of the arms with each other and with the object. We initially show arm-to-arm coordination caabilities by keeing the real object outside the worksace of the robots, this will force the coordination arameter γi to be, favoring arm-toarm coordination. As the human oerator erturbs one of the robot arms, the virtual object is erturbed as well, resulting in a stable synchronous motion of the other unerturbed arm (Fig. 4). Since we offer a centralized controller based on the virtual object s motion, there is no master/slave arm; thus, when any of the robots are erturbed, the others will synchronize their motions accordingly. We then resent the coordination of the arms with the object by moving it inside the worksace of the robots. The object used is a large box (6 6 4cm) held by a human oerator. The edges of the (c).s Fig. 8: Snashots of the arms reaching for a fast moving object. In order to not damage the robot s hands, the robot hands do not close on the object when the hands intercet the object. The robots follow the unerturbed trajectory of the object redicted by the object trajectory rediction; see Section III-A. box are secified as the feasible reaching oints. When the box is inside the joint worksace of the robots, the oerator changes the orientation and the osition of the box to show the coordination caabilities between the robots and the object (Fig. 5). Due to (14), successful tracking and coordination with the object and with each other is achieved. C. Reaching to Grab a Large Moving bject In this second scenario, we use the same object as before. Yet, now the oerator holds the box while walking towards the robots. nce the end-effectors are less than 2 cm away from

7 the feasible reaching oints, finger closures of the hands are triggered and the box is successfully grabbed from the human. As can be seen in Fig.7, we blindfolded the oerator to achieve unredictable trajectories and avoid the natural reactions of the humans to hel the robots. When the human oerator carrying the box is aroaching the robots, the virtual object converges to the box and follows it until the desired intercetion oints are reached. The fingers close and the box is grabbed from the human. The initial values of γ and γ o in (14) are set to. An examle of the desired robot trajectory and the box trajectory are shown in Fig.6. As exected, the end-effectors converge to the box and continue to track its motion. As initially γ i = i {, o}, the virtual object asymtotically converges to the desired reaching osition. While the box is aroaching the robots, γ i i {,o} starts increasing and finally reaches to γ i = 1 when the object is in the worksace of the robots. Hence, (6) generates asymtotically stable motions towards the real object instead of the intercet oint. Consequently, the rediction of the intercet oint does not lay a vital role in grabbing the box. D. Reaching for Fast Flying bjects The third scenario is designed to show the coordination between the robots and a fast moving object, where a rod (15 1cm) is thrown to the robots from 2.5m away, resulting in arox..56s flying time. The distance between the base of the two robots is reduced to [ ] T m. The first.4m of the object trajectory initializes the trajectory rediction introduced in Section III-A. Due to inaccurate rediction of the object trajectory, the feasible intercet oints need to be udated and redefined during the reach. The new feasible intercet oint is chosen in the vicinity of the revious one to minimize the convergence time. As the motion of the object is fast and the redicted reaching oints are not accurate, the initial values of γ and γ o in (14) are set to.5 and., resectively. This decreases the convergence duration of the robots to the real object. Snashots of the real robot exeriments are shown in Fig. 8. Visual insection of the data and video confirmed that the robots coordinately follow the motion of the object and intercet it at the vicinity of the redicted feasible intercet oint. E. Systematic Assessment The success rates of our exeriments are measured by defining a Boolean metric; i.e. success or failure. A trial is classified as a success if the robot intercets the object at the desired oint within less than 2cm error. The success rate is 85% in the box scenario and 37.5% in the rod scenario. Failures are due to either inaccuracies in the measurement of the object s state or the IK solver. To track the object, all the markers must be visible to the cameras. In the box scenario, the object s tracking was obscured artly when the object was covered by the robotic arms or the oerator. In the rod scenario, the vision system looses track of the markers arox. 55% of the time. This haens, for examle, when the rod rotates raidly. To systematically assess the robustness of the algorithm to unmeasured object ositions, a set of simulations The intercetion error [m] Without Noise [Fast Moving Ball] With Noise [Fast Moving Ball] With Noise [Slow Moving Ball] The ercentage of unmeasured object oints [%] Fig. 9: The intercetion error is the average of the minimum distance between the ball and the end-effectors. The throwing ositions are randomly chosen within the range of [ 3.5 ±.1 1. ±.1. ±.1 ] T m. The initial ball seeds for fast and slow motions are randomly chosen within range of 8.94±.173 m s and 1.63±.173 m s, resectively. We only consider trials when the ball asses through the robots worksaces. The measurement noise is simulated with seudo-random values within the range of ±.2m. The cutoff success/failure assessment is illustrated by the back dashed line. was designed to reach for a moving ball. The ball diameter is 1.2m. The simulation is reeated 13 times in total for two different object velocities; i.e. fast and slow motions. To assess the effects of the unmeasured object ositions on the intercetion error, the desired and the real end-effectors states are assumed equal. Results from this evaluation indicate that the intercetion error is directly correlated to the ercentage of unmeasured object oints and consequently, the velocity of the moving object (see Fig. 9). Thus, the faster the object, the more sensitive the system is to tracking inaccuracies. Failures caused by the IK solver are mostly observed in the flying rod scenario, where the inability to accurately calculate joint-level motions corresonding to the desired end-effectors trajectories results in errors in the robots motions. In over 4 trials, the tracking error between the desired and the actual end-effector osition at the intercet oint is aroximately 1.42 ± 1.92cm. The large variance in the error indicates the imlemented IK Solvers sensitivity to the robot s kinematic configuration. V. SUMMARY AND DISCUSSIN In this aer, we roosed a dynamical system based framework to coordinate multile robotic arms to reach a moving object. If rovided with robotic arms that can travel fast enough, our algorithm ensures that the object is interceted by the robots in coordination and with the desired velocity aligned to the object. To imrove the robustness of the framework, we define a arameter (i.e. the coordination arameter) and modulate it, such that the robots converge and track the real object when it is close enough. The stability and convergence of the roosed dynamical system deends on ensuring conditions (13). As there are no constraint on the magnitude of the eigenvalues ( λ A i ) of A i j j (i, j) {(r,1),...,(r,k r ),(, 1),...,(, K ),(,o1),...,(,o K )}, there is no analytical roof that (6) is fast enough to converge to γξ in time. To address this challenge, a otential direction would be to estimate the arameters of (6) with resect to

8 the stability and the convergence rate constraints. ne way to calculate the convergence rate of a dynamical system is to rove that it is exonentially stable [1]. Throughout the roofs, we assume that the intercet oint is a fixed attractor. However, due to the imerfect rediction of the object trajectory, the feasible intercet ostures need to be iteratively udated. Nevertheless, this does not affect the convergence of the system for two main reasons. First, when γ < 1 the new feasible intercet oint is chosen in the vicinity of the revious ones; i.e. the convergence rate is much faster than the rate of udate. Second, when the object is reachable, γ = 1, the virtual object converges to the real object and the osition of the intercet oint does not affect the convergence. In this aer, we control the motion of the arm from initial condition (alm oen, robots far from the object) to the oint when the arms reach the object and the fingers are about to close on the object. Hence, there are no interaction forces (which would arise once in contact with the object). nce the fingers close on the object, the robots-object system become a closed kinematic chain. In this case, devising an aroriate force controller is necessary to coordinate the robots. Future work in multi-arm maniulation will be directed to address the challenges of devising an aroriate force controller and the transition between the osition and force controller. Since the control architecture is designed in terms of the virtual object, the coordinated motion of the robots is ensured during motion execution; furthermore, it can be guaranteed that the end-effectors will not collide. However, the roosed algorithm does not guarantee that the rest of the arms will not collide with each other. ne ossible direction for avoiding collision across the robots links is to define an IK (Inverse Kinematics) null-sace, which maximizes the arms distance, and solve the IK roblems with resect to this null-sace. In the imlementation with the box and the flying bar, since the min. distance between the base of the arms is 1.29m, there is a max. of 3 cm intersection between the worksaces. Yet, the distance between the couled reaching area on the bar is 1.2m. Hence, the arms are ensured to not collide. ne of the main advantages of the roosed framework is the comutational cost. The imlementation shows that the overall comutation is raid, thus enabling us to not only comute the feasible intercet oint, the reaching motion and solving IK roblems, but also to control a 34 ( ) DF system with one 3.4-GHz i7 PC. Even though the robot control rate is 5Hz, the algorithm is caable of running at a rate of u to 33Hz. Finally, we are currently working on imroving the erformance of (6), by learning its arameters via a convex otimization roblem with resect to the worksace constraint of the robots. With this aroach, we could ensure that the erformance of the dynamical system is otimal and the generated motion is not infeasible for the robots to follow. APPENDIX A PRF F THEREM 1 The necessary condition for stability of (9) and asymtotically stability of the tracking error (8); i.e. lim t x V j (t) xr j (t) = j {1,...,K r}. (8) is asymtotically stable if and only if there are P R j and Q R j such that 7 P R j A R j + A R j T P R j Q R j j {1,...,K r } (15) If (8) is asymtotically stable, by substantiating (8) in (9a), we have: ẋ V (t) = 1 ( ẋ K r + 1 γ(t) + A (θ A )(x V (t) xγ(t)) + K r ) (ẋ V j (t) ω R j ) (16) The first and the second derivative of ξ V j = r j + ξ V with resect to time are ξ V j = ω r j + ξ V and ξ V j = ω (ω r j) + ξ V, resectively. Hence, ẋ V j = ω R j + ẋ V and (16) can be written as: ẋ V (t) = 1 ( ẋ K r + 1 γ(t) + A (θ A )(x V (t) xγ(t)) K r ) + ẋ V (t) = 1 ( ) ẋ K r + 1 γ(t) + A (θ A )(x V (t) xγ(t)) + K r ẋ V (t) ẋ V (t) = ẋ γ(t) + A (θ A )(x V (t) x γ(t)) We roose a Lyaunov function as follows: (17) V = 1 2 (xv (t) x γ(t)) T P v (x V (t) x γ(t)) (18) V is ositive definite, radially unbounded, continuous and continuously differentiable. The derivative of V with resect to time is as follows V = dv = 1 ( (x V (t) x dt 2 γ(t)) T P(ẋ v V (t) ẋγ(t)) ) (19) + (ẋ V (t) ẋγ(t)) T P(x v V (t) xγ(t)) Substituting (5) and (17) into (19), we have: V = dv = 1 ( (x V (t) x dt 2 γ(t)) T PA v (θ A )(x V (t) xγ(t)) ) + (x V (t) xγ(t)) T A (θ A ) T P(x v V (t) xγ(t)) = 1 ( K (x V (t) x 2 γ(t)) T P( v θ Ak A k )(x V (t) xγ(t))) k=1 K ) + (x V (t) xγ(t)) T ( θ Ak A T k )P(x v V (t) xγ(t))) k=1 K = θ Ak (x V (t) xγ(t)) T (PA v 2 k k=1}{{} + A2 T k P v )(x V (t) xγ(t)) }{{} > Q v k }{{} < < (2) Therefore, dynamical system (17) and (9a) are globally stable; i.e. x V and ẋ V are bounded as x γ(t) and ẋ γ(t) are bounded. Since V is finite, Barbalat s lemma [1] indicates that the attractor is globally asymtotically stable; i.e:, c.q.f.d. lim t xv xγ(t) = lim ξ V t (γ ξ + γ ξ ) =, lim ξ v t γ ξ = ACKNWLEDGMENTS (21) This work was suorted by EU rojects Cogimon H22 ICT and EU FP7 Project RoboHow (Grant Agreement Number ). The authors kindly thank Nicolas Sommer and Felix Duvallet for rearing the exerimental setu with Allegro hand and Seyed Mohammad Khansari-Zadeh and Klas onander for roviding IIWA connectivity ackage. 7 The roof is omitted due to lack of sace. Asymtotic stability of (8) can be roven using the non-negative Lyaunov function: V = 1 2 (x V j (t) xr j (t))t P R j (xv j (t) xr j (t)).

9 REFERENCES [1] F. Aghili. Adative control of maniulators forming closed kinematic chain with inaccurate kinematic model. IEEE/ASME Transactions on Mechatronics,, 18(5): , ct 213. [2] Bassam Bamieh and Laura Giarre. Identification of linear arameter varying models. International journal of robust and nonlinear control, 12(9): , 22. [3] M. C. Bisho. Pattern Recognition and Machine Learning. Sringer, 27. ISBN [4] F. Caccavale, P. Chiacchio, A. Marino, and L. Villani. Six-dof imedance control of dual-arm cooerative maniulators. IEEE/ASME Transactions on Mechatronics, 13(5): , ct 28. [5] Fabrizio Caccavale and Masaru Uchiyama. Cooerative maniulators. In Bruno Siciliano and ussama Khatib, editors, Sringer Handbook of Robotics, ages Sringer Berlin Heidelberg, 28. [6] Rachel. Coats and John P. Wann. Reaching a better understanding of the control of bimanual movements in older adults. PLoS NE, 7(1):e47222, [7] Benjamin Cohen, Mike Phillis, and Maxim Likhachev. Planning single-arm maniulations with n-arm robots. In Proceedings of Robotics: Science and Systems, Berkeley, USA, July 214. [8] Zlatko Emedi and Alireza Karimi. Fixed-structure LPV Discrete-time Controller Design with Induced l2-norm and H2 Performance. International Journal of Control, 215. [9] A. Gams, A. Ude, and J. Morimoto. Accelerating synchronization of movement rimitives: Dual-arm discreteeriodic motion of a humanoid robot. In Intelligent Robots and Systems (IRS), 215 IEEE/RSJ International Conference on, ages , Set 215. doi: 1.119/IRS [1] Hassan Khalil. Nonlinear Systems. Prentice Hall, 22. ISBN [11] S.M. Khansari-Zadeh and A. Billard. Learning stable nonlinear dynamical systems with gaussian mixture models. IEEE Transactions on Robotics, 27(5): , 211. [12] S. Kim, A. Shukla, and A. Billard. Catching objects in flight. IEEE Transactions on Robotics, 3(5): , 214. [13] Seungsu Kim and Aude Billard. Estimating the nonlinear dynamics of free-flying objects. Robotics and Autonomous Systems, 6(9): , 212. ISSN [14] Nejc Likar, Bojan Nemec, and Leon lajah. Virtual mechanism aroach for dual-arm maniulation. Robotica, FirstView:1 16, ISSN [15] Y.-H. Liu and S. Arimoto. Decentralized adative and nonadative osition/force controllers for redundant maniulators in cooerations. International Journal of Robotics Research, 17(3): , [16] J. Y. S. Luh and Y. F. Zheng. Constrained relations between two coordinated industrial robots for motion control. International Journal of Robotics Research, 6 (3):6 7, Setember [17] J. Nakanishi, R. Cory, M. Mistry, J. Peters, and S. Schaal. Comarative exeriments on task sace control with redundancy resolution. In IEEE/RSJ International Conference on Intelligent Robots and Systems, ages , 25. [18] Klaus Neumann and Jochen J. Steil. Learning robot motions with stable dynamical systems under diffeomorhic transformations. Robotics and Autonomous Systems, 7: 1 15, 215. ISSN [19] S. S. M. Salehian, M. Khoramshahi, and A. Billard. A dynamical system aroach for softly catching a flying object: Theory and exeriment. IEEE Transactions on Robotics, 32(2): , Aril 216. ISSN doi: 1.119/TR [2] S. Schaal, S. Kotosaka, and D. Sternad. Nonlinear dynamical systems as movement rimitives. In Humanoids2, First IEEE-RAS International Conference on Humanoid Robots, 2. [21] C. Smith, Y. Karayiannidis, L. Nalantidis, X. Gratal, P. Qi, D. V. Dimarogonas, and D. agic. Dual arm maniulationa survey. Robotics and Autonomous Systems, 6(1): , 212. [22] R. Suda, K. Kosuge, and H. Kakuya. bject-imedancebased cooerative handling of object by mobile robot heler and human using visual and force information. In Advanced Intelligent Mechatronics, 23. AIM 23. Proceedings. 23 IEEE/ASME International Conference on, volume 1, ages vol.1, July 23. doi: /AIM [23] Dong Sun and James K. Mills. Maniulating rigid ayloads with multile robots using comliant griers. IEEE/ASME Transactions on Mechatronics, 7(1):23 34, Mar 22. [24] N. Vahrenkam, M. Do, T. Asfour, and R. Dillmann. Integrated gras and motion lanning. In IEEE International Conference on Robotics and Automation (ICRA), ages , 21. [25] N. Vahrenkam, T. Asfour, and R. Dillmann. Simultaneous gras and motion lanning: Humanoid robot armariii. IEEE Robotics Automation Magazine, 19(2):43 57, 212. [26] Jan-Willem van Wingerden and Michel Verhaegen. Subsace identification of bilinear and LPV systems for oen- and closed-loo data. Automatica, 45(2): , 29. [27] David Vernon, Claes von Hofsten, and Luciano Fadiga. A Roadma for Cognitive Develoment in Humanoid Robots, volume 11 of Cognitive Systems Monograhs. Sringer Berlin Heidelberg, 211. [28] Meiling Wang, Minzhou Luo, Tao Li, and Marco Ceccarelli. A unified dynamic control method for a redundant dual arm robot. Journal of Bionic Engineering, 12(3):

10 , 215. [29] D. Williams and. Khatib. The virtual linkage: a model for internal forces in multi-gras maniulation. In IEEE International Conference on Robotics and Automation, ages vol.1, [3] T. Wimbock, C. tt, and G. Hirzinger. Analysis and exerimental evaluation of the intrinsically assive controller (ic) for multifingered hands. In IEEE International Conference on Robotics and Automation, ages , 28. [31] T. Wimböck, C. tt, A. Albu-Schäffer, and G. Hirzinger. Comarison of object-level gras controllers for dynamic dexterous maniulation. International Journal of Robotics Research, 31(1):3 23, 212. [32] Thomas Wimböck and Christian tt. Dual-arm maniulation. In Towards Service Robots for Everyday Environments, volume 76 of Sringer Tracks in Advanced Robotics, ages Sringer Berlin Heidelberg, 212. [33] Thomas Wimböck, Christian tt, Alin Albu-Schäffer, and Gerd Hirzinger. Comarison of object-level gras controllers for dynamic dexterous maniulation. The International Journal of Robotics Research, 31(1):3 23, 212. [34] Wen-Hong Zhu. n adative synchronization control of coordinated multirobots with flexible/rigid constraints. IEEE Transactions on Robotics, 21(3):52 525, June 25. ISSN