Distributed Cooperative Manipulation under Timed Temporal Specifications  Christos K. Verginis and Dimos V. Dimarogonas  Abstract — This paper addresses the problem of cooperative manipulation of a single object by   N   robotic agents under local   goal   specifications   given   as   Metric   Interval   Temporal Logic (MITL) formulas. In particular, we propose a distributed model-free control protocol for the trajectory tracking of the co- operatively manipulated object without necessitating feedback of the contact forces/torques or inter-agent communication. This allows us to abstract the motion of the coupled object-agents system as a finite transition system and, by employing standard automata-based   methodologies,   we   derive   a   hybrid   control algorithm for the satisfaction of a given MITL formula. In addition, we use load sharing coefficients to represent potential differences in power capabilities among the agents. Finally, simulation studies verify the validity of the proposed scheme.  I. I NTRODUCTION  Multi-agent systems have gained significant attention over the last decade, since they provide several advantages with respect to single-agent setups. In the case of object manip- ulation, complex tasks involving heavy/large payloads and difficult maneuvers necessitate the employment of more than one robot. The problem of cooperative manipulation control has been studied extensively, using centralized schemes, where a central computer handles the agents’ behavior, as well as decentralized setups, where each agent determines its actions on its own, either with partial or no communication at all [1]–[11]. In contrast to the related literature, which mainly considers the trajectory tracking of the manipulated object, we would like to define complex tasks over time, such as "never take the object to dangerous regions" or "keep moving the object from region A to B within a predefined time interval" which must be executed via the control actions of the agents. Such tasks can be expressed by temporal logic languages, which can describe complex planning objectives more efficiently than the well-studied navigation algorithms. Linear Temporal Logic (LTL) is the most common language that has been incorporated to the multi-agent motion planning problem [12]–[16], without however considering time specifications. Metric and Metric Interval Temporal Logic (MTL, MITL) [17]–[19] as well as Time Window Temporal Logic (TWTL) are languages that encode time specifications and were used for multi-agent motion planning in [20]–[22].  The authors are with the Centre for Autonomous Systems and ACCESS Linnaeus Centre, KTH Royal Institute of Technology, Stockholm 10044, Sweden. Emails: {cverginis, dimos}@kth.se. This work was supported by funding from the Knut and Alice Wallenberg Foundation, the Swedish Research Council (VR), the European Union’s Horizon 2020 Research and Innovation Programme under the Grant Agree- ment No. 644128 (AEROWORKS) and the H2020 ERC Starting Grant BUCOPHSYS.  Regarding robotic manipulation, high level planning tech- niques   have   been   proposed   in   [23]–[25]   using   common planning methods like configuration space potential fields and   A ∗   algorithms. In [25] the motion planning problem for a group of unicycles manipulating a rigid body is addressed and in [24] an abstraction methodology is introduced; LTL specifications are employed in [26], where two mobile robots transport an object in a leader-follower scheme. Additionally, temporal logic formulas are utilized in [27] for dexterous manipulation through robotic fingers and in [28] for single manipulation tasks, without, however, incorporating the dy- namics of the robotic arm in the abstracted model. In [29] a hybrid framework for cooperative manipulation is presented. For the continuous control part, impedance and/or force control is the most common methodology [2]–[5], in which the robotic arms employ sensors to obtain feedback of the contact forces/torques which, however, may result to perfor- mance decline due to sensor noise or mounting difficulties. Moreover,   most   works   in   the   related   literature   consider known dynamic models and/or parameters of the object and the agents, whose accurate knowledge, however, can be a challenging issue. In this paper, we propose a novel hybrid control scheme for the cooperative manipulation of an object under MITL specifications. In particular, we develop a distributed model- free control protocol for the trajectory tracking of the co- operative manipulated object with prescribed transient and steady state performance. The latter allows us to abstract the motion of the coupled system object-agents as a finite transition system. Then, by employing formal verification- based methodologies, we derive a path that satisfies a given MITL task. The control scheme does not use any force/torque information at the contact points or any inter-agent commu- nication and incorporates load sharing coefficients to account for differences in power capabilities among the agents. The rest of the paper is organized as follows: Section II introduces notation and preliminary background. Section III describes the problem formulation and the overall system’s model. The control strategy is presented in Section IV. Sec- tion V verifies our approach through numerical simulation results and Section VI concludes the paper. II. N OTATION AND   P RELIMINARIES  A. Notation  The set of positive integers is denoted as   N   and the real  n -coordinate space, with   n   ∈   N , as   R n ;   R n  ≥ 0   and   R n > 0   are the sets of real   n -vectors with all elements nonnegative and positive, respectively. Given a set   S ,   2 S   is the set of all  arXiv:1610.00913v4 [cs.RO] 6 Feb 2017
subsets of   S . The vector connecting the origins of coordinate frames   { A }   and   { B } expressed in frame   { C }   coordinates in   3 -D space is denoted as   p C B/A   ∈   R 3 . Given   a   ∈   R 3 ,  S ( a )   is the skew-symmetric matrix defined according to  S ( a ) b   =   a   ×   b . The rotation matrix from   { A }   to   { B }  is denoted as   R B/A   ∈   SO (3) , where   SO (3)   is the   3 -D rotation group. The angular velocity of frame   { B }   with respect to   { A }   is denoted as   ω B/A   ∈   R 3   and it holds that [30]   ̇ R B/A   =   S ( ω B/A   ) R B/A   . We further denote as   η A/B   ∈   T 3   the Euler angles representing the orientation of   { B }   with respect to   { A }   and   ω B/A   =   J B/A   ( η B/A   )  ̇ η B/A   ;   J B/A   :   T 3   →   R 3 × 3   is a smooth function representing the analytic Jacobian and   T 3  is the   3 -D torus. Moreover,   B ( c, r )   denotes the   3 -D sphere of radius   r   ≥   0   and center   c   ∈   R 3   and   d   :   R 3 × R 3   →   R   is the   3 - D Euclidean distance. We further define the set   M   =   R 3 × T 3 . For notational brevity, when a coordinate frame corresponds to an inertial frame of reference   { I } , we will omit its explicit notation (e.g.,   p B   =   p I B/I   , ω B   =   ω I B/I   , R B   =   R B/I   etc.). Finally, all vector and matrix differentiations will be with respect to an inertial frame   { I } , unless otherwise stated.  B. Metric Interval Temporal Logic (MITL)  The syntax of   MITL   over a set of atomic propositions  AP   is   defined   by   the   grammar   φ   :=   p   | ¬ φ   |   © I  φ   |   ♦ I   φ   |    I   φ   |   φ 1 U φ 2 , where   p   ∈ AP ,   © ,   ♦ ,      and   ∪  are the next, future, always and until operators, respectively.  I   is one of the following intervals:   [ i 1 , i 2 ] ,   [ i 1 , i 2 ) ,   ( i 1 , i 2 ] ,  ( i 1 , i 2 ) ,   [ i 1 ,   ∞ ) ,   ( i 1 ,   ∞ )   with   i 1 , i 2   ∈   R ≥ 0 , i 2   > i 1 . Given a set of atomic propositions   AP , an MITL formula  φ   and an infinite sequence   r   = ( r 1 , t 1 )( r 2 , t 2 )   . . .   , with   r j   ∈  2 AP   and   t j   ∈   R ≥ 0 , t j +1   > t j   ,   ∀ j   ∈   N , we define   ( r, j )   | =  φ, j   ∈   N   ( r   satisfies   φ   at   j ) in the point-wise semantics [19]:  ( r, j )   | =   p   ⇔   p   ∈   r j   ,  ( r, j )   | =   ¬ φ   ⇔   ( r, j )   6   | =   φ  ( r, j )   | =   φ 1   ∧   φ 2   ⇔   ( r, j )   | =   φ 1   and   ( r, j )   | =   φ 2  ( r, j )   | =   © I   φ   ⇔   ( r, j   + 1)   | =   φ   and   t j +1   −   t j   ∈   I  ( r, j )   | =   φ 1 U φ 2   ⇔ ∃ k, j, j   ≤   k,   s.t.   ( r, k )   | =   φ 2 , t k   −   t j   ∈   I   and   ( r, m )   | =   φ 1 ,   ∀ j   ≤   m   ≤   k.  Also,   ♦ I   φ   =   True   U φ   and    I   φ   =   ¬ ♦ I   ¬ φ . The sequence   r  satisfies   φ , denoted as   r   | =   φ   if and only if   ( r,   1)   | =   φ . More information regarding MITL can be found in [18], [19].  C. Dynamical Systems  Consider the initial value problem:   ̇ ξ   =   H ( t, ξ ) , ξ ( t 0 ) =   ξ 0   ∈   Ω ξ   ,   (1) with   H   : [ t 0 ,   + ∞ )   ×   Ω ξ   →   R n   where   Ω ξ   ⊂   R n   is a non- empty open set.  Definition 1.   [31] A solution   ξ ( t )   of the initial value problem (1) is maximal if it has no proper right extension that is also a solution of (1).  Theorem 1.   [31] Consider problem (1). Assume that   H ( t, ξ )  is: a) locally Lipschitz on   ξ   for almost all   t   ∈   [ t 0 ,   + ∞ ) , b) piecewise continuous on   t   for each fixed   ξ   ∈   Ω ξ   and c) locally integrable on   t   for each fixed   ξ   ∈   Ω ξ   . Then,  Fig. 1.   Two robotic arms rigidly grasping an object.  there exists a maximal solution   ξ ( t )   of (1) on   [ t 0 , t max )   with  t max   > t 0   such that   ξ ( t )   ∈   Ω ξ   ,   ∀ t   ∈   [ t 0 , t max ) .  Proposition 1.   [31] Assume that the hypotheses of Theorem 1 hold. For a maximal solution   ξ ( t )   on the time interval  [ t 0 , t max )   with   t max   <   ∞   and for any compact set   Ω ′  ξ   ⊂   Ω ξ  there exists a time instant   t ′   ∈   [ t 0 , t max )   such that   ξ ( t ′ )   / ∈  Ω ′  ξ   .  III. P ROBLEM   F ORMULATION  Consider a bounded workspace   W ⊂   R 3   consisting of  N   mobile manipulators rigidly grasping an object, as shown in Fig. 1. We assume that each agent   i   ∈ { 1 , . . . , N   }   has  n i   ≥   6   degrees of freedom (generalized joint coordinates), denoted as   q i   :   R ≥ 0   →   R n i   . We also denote the entire joint space as   q   = [ q T  1   ,   · · ·   , q T N   ] T   :   R ≥ 0   →   R n , with   n   =  ∑ N i =1   n i . The reference frames corresponding to the   i -th end- effector and the object’s center of mass are denoted with  { E i }   and   { O } , respectively, whereas   { I }   corresponds to an inertial reference frame. The rigidity of the grasps implies that the agents can exert any forces/torques along every direction to the object. We consider that each agent   i   knows the position and velocity only of its own joint variables   q i  as well as its own and the object’s geometric parameters. Moreover, no interaction force/torque measurements or on- line communication is required and the dynamic model of the object and the agents is considered unknown. Finally, we assume that the robotic agents and the object are away from kinematic and representation singularities [30].  A. System model 1) Kinematics:   In view of Fig. 1, we have that  p Ei   ( q )   =   p O   ( q ) +   p Ei/O   ( q ) =   p O   ( q ) +   R Ei   ( q ) p Ei Ei/O  η Ei   ( q )   =   η O   ( q ) +   α i ,   (2) where   α i   ∈   T 3   is a known angular offset. We further define  x Ei   , x O   :   R n   →   M   with   x Ei   ( q ) = [ p T  Ei   ( q ) , η T  Ei   ( q )] T   and  x O   ( q ) = [ p T  O   ( q ) , η T  O   ( q )] T   . Note that, since (2) holds for all  i   ∈ { 1 , . . . , N   } , the physical coupling between the object and the agents creates a dependence of   x O   and   x Ei   on all  q . Moreover, the grasp rigidity implies that   ω Ei   =   ω O   , i.e.,  J Ei   ( η Ei   ( q ))  ̇ η Ei   =   J O   ( η O   ( q ))  ̇ η O   . Since the agents and the object are away from representation singularities,   J − 1  O   and  J − 1  Ei   are well-defined and smooth and hence we differentiate (2) to obtain:   ̇ x Ei   =   J Oi   ( x Ei   , x O   )  ̇ x O   ,   (3)
where   J Oi   :   M   ×   M   →   R 6 × 6   is the Jacobian from the object to the   i -th agent:  J Oi   =  [   I 3 × 3   S ( p O/Ei   ) J O   ( η O   ) 0 3 × 3   J − 1  Ei   ( η Ei   ) J O   ( η O   )  ]  ,   (4) which is always invertible due to the grasp rigidity. Furthermore,   by   noticing   that   R Ei   ( q ) p Ei Ei/O   =  R O   ( η O   ) p O Ei/O   , (2) can be rewritten as  x Ei   =   f Oi   ( x O   ) ,   (5) where   f Oi   :   M   →   M   represents the coupled kinematics.  Remark   1.   Each agent   i   can compute   x Ei   ,    ̇ x Ei   via its forward and differential kinematics [30]   x Ei   =   k i ( q i )   and   ̇ x Ei   =   J i ( q i )  ̇ q i , respectively, where   k i   :   R n i   →   R 3   and  J i ( q i )   =   ∂k i ( q i ) /∂q i   is the corresponding Jacobian. In addition, since the geometric parameters   p Ei Ei/O   and   α i   are known,   x O   and    ̇ x O   can be computed by inverting eq. (2) and (3), without employing any sensory data. 2)   Object Dynamics   :   The Newton-Euler equation for the object’s second order dynamics is:  M O   ( x O   ) ̈ x O   +   C O   ( x O   ,    ̇ x O   )  ̇ x O   +   g O   ( x O   ) +   w O   ( t ) =   λ O   ,   (6)  where   M O   :   M   →   R 6 × 6   is the positive definite inertia matrix,   C O   :   M   ×   R 6   →   R 6 × 6   is the Coriolis matrix,  g O   :   M   →   R 6   is the gravity vector,   w O   :   R ≥ 0   →   R 6   is a bounded vector representing external disturbances and   λ O  is the force vector acting on the object’s center of mass. All aforementioned vector fields are continuous and unknown.  3)   Agent Dynamics   : The task-space dynamics for agent  i   ∈ { 1 , . . . , N   }   are given by [30]:  M i ( x Ei   ) ̈ x Ei   +   C i ( x Ei   ,    ̇ x Ei   )  ̇ x Ei   +   g i ( x Ei   ) + f i ( x Ei   ,    ̇ x Ei   ) +   w i ( t ) +   λ i   =   u i   (7) where   M i   :   M   →   R 6 × 6   is the task-space positive definite inertia matrix,   C i   :   M   ×   R 6   →   R 6   represents the task- space Coriolis matrix,   g i   :   M   →   R 6   is the task-space gravity   vector,   f i   :   M   ×   R 6   →   R 6   is   a   vector   field representing model uncertainties and   w i   :   R ≥ 0   →   R 6   is a bounded vector representing external disturbances. Similarly to (6), the aforementioned vector fields are continuous and completely unknown;   u i   ∈   R 6   is the task space wrench acting as the control input and   λ i   ∈   R 6   is the generalized force vector that agent   i   exerts on the object.  Remark 2.   The task-space wrench   u i   can be translated to the joint space inputs   τ i   ∈   R n i   via   τ i   =   J T i   ( q i ) u i   +( I n i × n i   −  J T i   ( q i )  ̄ J T i   ( q i )) τ i 0   , where    ̄ J i   is a generalized inverse of   J i  [30]. The term   τ i 0   concerns redundant agents ( n i   >   6 ) and does not contribute to end-effector forces. 4)   Coupled Dynamics   : The kineto-statics duality [30] along with the grasp rigidity suggest that the force   λ O   acting on the object center of mass and the generalized forces  λ i , i   ∈ { 1 , . . . , N   }   exerted by the agents at the contact points are related through  λ O   =   G T   ( x ) λ,   (8)  (a) Illustration of   ˆ L  (b) Workspace Discretization Fig. 2.   (a): An example of the agents of Fig. 1 in the configuration that derives   ˆ L . (b): The workspace partition according to the bounding box of the coupled system.  where   x   = [ x T  O   , x T  E   ] T   ∈   M N   +1 , x E   = [ x T  E 1   ,   · · ·   , x T  EN   ] T   ∈  M N   , λ   = [ λ T  1   ,   · · ·   , λ T N   ] T   ∈   R 6 N   and   G   :   M N   +1   →   R 6 N   × 6  is the grasp matrix, with   G ( x ) = [ J T  O 1   ,   · · ·   , J T  ON   ] T   . Next, we substitute (3) and its derivative in (7) and we obtain in vector form after rearranging terms:  λ   =   u   −   M   ( x E   ) G ( x ) ̈ x O   −   g ( x E   )   −   f   ( x E   )   −   w ( t )  − ( M   ( x E   )  ̇ G ( x,    ̇ x ) +   C ( x E   ,    ̇ x E   ) G ( x ))  ̇ x O   (9) where   we   have   used   the   stack   forms   M   =  diag { [ M i ] i ∈{ 1 ,...,N   } } , C   =   diag { [ C i ] i ∈{ 1 ,...,N   } } , g   = [ g T  1   ,   · · ·   , g T N   ] T   , f   =   [ f   T  1   ,   · · ·   , f   T N   ] T   , u   =   [ u T  1   ,   · · ·   , u T N   ] T  and   w   = [ w T  1   ,   · · ·   , w T N   ] T   . By substituting (9) and (6) in (8), we obtain the coupled dynamics:   ̃ M   ( x ) ̈ x O   +    ̃ C ( x,    ̇ x )  ̇ x O   +    ̃ h ( x,    ̇ x ) +    ̃ w ( x, t ) =   G T   ( x ) u,   (10) where    ̃ M   ( x ) =   M O   +   G T   M G,    ̃ C ( x,    ̇ x ) =   C O   +   G T   M    ̇ G   +  G T   CG,    ̃ h ( x,    ̇ x ) =   g O   +   G T   g   +   G T   f   , and    ̃ w ( x, t ) =   w O   +  G T   w . It is straightforward to deduce the positive definiteness of    ̃ M   as well as the continuity of all the above coupled vector fields due to the continuity of the individual terms.  B. Workspace Partition  As mentioned in Section I, we are interested in defining MITL formulas over certain properties in a discrete set of regions of the workspace. Therefore, we provide now a partition of   W   into cell regions. We first define the set   S q  that consists of all points   p s   ∈ W   that physically belong to the coupled system, i.e. they consist part of either the agents’ or the object’ volume. Note that these points depend directly on the actual value of   q . We further define the constant   ˆ L   ≥   sup   q ∈ R n  p s ∈S q  d ( p s , p O   ( q )) . Note that, although the explicit computation of   S q   may not be possible,   ˆ L   is an upper bound of the maximum distance between the object center of mass and a point in the coupled system’s volume over all possible configurations   q , and thus, it can be measured. For instance, Fig. 2(a) shows   ˆ L   for the system of Fig. 1. It is straightforward to conclude that  p s   ∈ B ( p O   ( q ) ,   ˆ L ) ,   ∀ p s   ∈ S q   , q   ∈   R n .   (11)
Next, we partition the workspace   W   into   R   equally sized rectangular regions   Π   =   { π 1 , . . . , π R } , whose geometric centers are denoted as   p c π j   ∈ W , j   ∈ { 1 , . . . , R } . The length of the region sides is set as   D   = 2 ˆ L   + 2 l 0 , where   l 0   is an arbitrary positive constant. Hence, each region   π j   can be formally defined as follows:  π j   =   { p   ∈ W   s.t.   ( p ) k   ∈   [( p c π j   ) k   −   ˆ L   −   l 0 ,   ( p c π j   ) k   + ˆ L   +   l 0 ) ,  ∀ k   ∈ { x, y, z }} ,  with   d ( p c π j +1   , p c π j   )   =   (2 ˆ L   + 2 l 0 ) ,   ∀ j   ∈   { 1 , . . . , R   −   1 }  and   ( p c π j   ) z   =   ˆ L   +   l 0 ,   ∀ j   ∈   { 1 , . . . , R } .   The   notation  ( a ) k , k   ∈   { x, y, z }   denotes the   k -th coordinate of   a   = [( a ) x ,   ( a ) y   ,   ( a ) z   ] T   ∈   R 3 . An illustration of the aforemen- tioned partition is depicted in 2(b). Note that each   π j   is a uniformly bounded, convex and well-connected set and also   π j   ∩ π j ′   =   ∅ ,   ∀ j, j ′   ∈ { 1 , . . . , R }  with   j   6   =   j ′ . We also define the neighborhood of region  π j   as the set of its adjacent regions   D ( π j   )   =   { π j ′   ∈  Π   s.t.   d ( p c π j   , p c π j ′   ) = (2 ˆ L   + 2 l 0 ) } , which is symmetric, i.e.,  π j ′   ∈ D ( π j   )   ⇔   π j   ∈ D ( π j ′   ) . To proceed we need the following definitions regarding the timed transition of the coupled system between two regions  π j   , π j ′   :  Definition 2.   The coupled system object-agents is in region  π j   at a configuration   q , denoted as   A ( q )   ∈   π j   , if and only if (i)   p s   ∈   π j   ,   ∀ p s   ∈ S q   and (ii)   d ( p O   ( q ) , p c π j   )   < l 0 .  Definition 3.   Assume that   A ( q ( t 0 ))   ∈   π j   , j   ∈ { 1 , . . . , R } , for some   t 0   ∈   R ≥ 0 . Then, there exists a transition for the coupled system object-agents from   π j   to   π j ′   , j ′   ∈ { 1 , . . . , R }  with time duration   δt j,j ′   ∈   R ≥ 0 , denoted as   π j   →   π j ′   , if and only if there exists a bounded control trajectory   u   in (10), such that   A ( q ( t 0   +   δt j,j ′   ))   ∈   π j ′   and   p s   ∈   π j   ∪   π j ′   ,   ∀ p s   ∈ S q   , t   ∈   [ t 0 , t 0   +   δt j,j ′   ] .  Note that the entire system object-agents must remain in  π j   , π j ′   during the transition and therefore the requirement  π j ′   ∈ D ( π j   )   is implicit in the definition above.  C. Specification  Given the workspace partition, we can introduce a set of atomic propositions   AP   for the object, which are expressed as boolean variables that correspond to properties of interest in the regions of the workspace (e.g., "Obstacle region", "Goal region"). Formally, the labeling function   L   : Π   →  2 AP   assigns to each region   π j   the subset of the atomic propositions   AP   that are true in   π j   .  Definition   4.   Assume   that   q ( t )   is   a   trajectory.   Then, a   timed   sequence   of   q   is   the   infinite   sequence   β   = ( q 1 ( t ) , t 1 ,   )( q 2 ( t ) , t 2 ,   )   . . .   , with   t m   ∈   R ≥ 0 , t m +1   >   t m  and   A ( q m ( t m ))   ∈   π j m   , j m   ∈   { 1 , . . . , R } ,   ∀ m   ∈   N . The timed behavior of   β   is the infinite sequence   σ β   = ( σ 1 , t 1 )( σ 2 , t 2 )   . . .   , with   σ m   ∈   2 AP   , σ m   ∈   L ( π j m   )   for  A ( q m ( t m ))   ∈   π j m   , j m   ∈ { 1 , . . . , R } ,   ∀ m   ∈   N , i.e., the set of atomic propositions that are true when   A ( q m ( t m ))   ∈   π j m   .  Definition 5.   The timed run   β   satisfies an MITL formula   φ  if and only if   σ β   | =   φ .  Fig. 3.   A transition between two adjacent regions   π j   and   π j ′   . Since  p O   ∈ B ( p d ( t ) , l 0 ) , we conclude that   p s   ∈ B ( p O   ,   ˆ L )   ∈ B ( p d ( t ) , l 0   + ˆ L )   ∈  π j   ∪   π j ′   .  We are now ready to state the problem treated in this paper.  Problem   1.   Given   N   agents rigidly grasping an object in   W   subject to the coupled dynamics described by (10), the workspace partition   Π   such that   A ( q (0))   ∈   π j   , j   ∈ { 1 , . . . , R } , a MITL formula   φ   over   AP   and the labeling function   L , develop a control strategy that achieves a timed sequence   β   which yields the satisfaction of   φ .  IV. M AIN   R ESULTS  A. Control Design  The first ingredient of the proposed solution, which con- stitutes one of the main novelties of this work, is the design of a decentralized control protocol   u   such that a transition relation between two adjacent regions according to Def. 3 is established. Assume, therefore, that   A ( q ( t 0 ))   ∈   π j   , t 0   ∈  R ≥ 0 . We wish to find a bounded   u , such that (i)   A ( q ( t 0   +  δt j,j ′   ))   ∈   π j ′   with   π j ′   ∈ D ( π j   )   and (ii)   p s   ∈   π j   ∪   π j ′   ,   ∀ p s   ∈ S q   , t   ∈   [ t 0 , t 0   +   δt j,j ′   ] , for a predefined arbitrary constant  δt j,j ′   ∈   R ≥ 0   corresponding to the transition   π j   →   π j ′   . The first step is to associate to the transition a smooth trajectory defined by the line segment that connects   p c π j   and  p c π j ′   , i.e. define   p d   : [ t 0 ,   ∞ )   →   R 3 , such that   p d ( t 0 ) =  p c π j   , p d ( t ) =   p c π j ′   ,   ∀ t   ≥   t 0   +   δt j,j ′   and  B ( p d ( t ) ,   ˆ L   +   l 0 )   ∈   π j   ∪   π j ′   ,   ∀ t   ≥   t 0 .   (12) If we guarantee that the object’s center of mass stays   l 0 - close to   p d , i.e.,   d ( p O   ( t ) , p d ( t ))   <   l 0 , then   d ( p O   ( t 0   +  δt j,j ′   ) , p c π j ′   )   < l 0   and, by invoking (11) and (12), we obtain  p s   ∈ B ( p O   ( t ) ,   ˆ L )   ∈ B ( p d ( t ) ,   ˆ L   +   l 0 )   ∈   π j   ∪   π j ′   ,   ∀ p s   ∈ S q   , t   ≥   t 0   (and therefore   t   ∈   [ t 0 , t 0   +   δt j,j ′   ] ), and thus the requirements of Def. 3 for the transition relation are met. Fig. 3 illustrates the aforementioned reasoning. Along with   p d , we also define a desired smooth trajectory   η d   : [ t 0 ,   ∞ )   →   T 3   for the object orientation and we form the desired pose trajectory  x d   : [ t 0 ,   ∞ )   →   M , with   x d ( t ) = [ x d 1   ( t ) , . . . , x d 6   ( t )] T   = [ p T d   ( t ) , η T d   ( t )] T   . We can now define the associated pose errors   e s   : [ t 0 ,   ∞ )   →   M   with  e s ( t ) = [ e s 1   ( t ) ,   · · ·   , e s 6   ( t )] T   =   x O   ( q ( t ))   −   x d ( t ) .   (13) A suitable methodology for the control design in hand is that of prescribed performance control, recently proposed in [32], which is adapted in this work in order to achieve predefined transient and steady state response bounds for the pose errors.
As stated in [32], prescribed performance characterizes the behavior where the aforementioned errors evolve strictly within a predefined region that is bounded by absolutely decaying functions of time, called performance functions. The mathematical expressions of prescribed performance are given by the inequalities:   − ρ s k   ( t )   < e s k   ( t )   < ρ s k   ( t ) ,   ∀ k   ∈ { 1 , . . . ,   6 } , where   ρ s k   :   [ t 0 ,   ∞ )   →   R > 0   with   ρ s k   ( t )   = ( ρ 0  s k   −   ρ ∞  s k   ) e − l sk   ( t − t 0   )   +   ρ ∞  s k   and   l s k   >   0 , ρ 0  s k   > ρ ∞  s k   >   0 , are designer-specified, smooth, bounded and decreasing positive functions of time with positive parameters   l s k   , ρ ∞  s k   , incor- porating the desired transient and steady state performance respectively. In particular, the decreasing rate of   ρ s k   , which is affected by the constant   l s k   , introduces a lower bound on the speed of convergence of   e s k   ( t ) . Furthermore, the constants   ρ ∞  s k   can be set arbitrarily small, achieving thus practical convergence of the pose errors to zero. Next, we propose a state feedback control protocol that does not incorporate any information on the agents’ or the object’s dynamics or the external disturbances and guarantees that   | e s k   ( t ) |   < ρ s k   ( t )   for all   [ t 0 ,   ∞ )   and hence   [ t 0 , t 0   +  δt j,j ′   ] , which, by appropriately selecting   ρ s k   ( t )   and given that   A ( q ( t 0 ))   ∈   π j   , guarantees a transition   π j   →   π j ′   with time duration of   δt j,j ′   . Thus, given the pose errors (13):  Step I-a . Select the corresponding functions   ρ s k   ( t )   = ( ρ 0  s k   −   ρ ∞  s k   ) e − l sk   )( t − t 0 )   +   ρ ∞  s k   with   ρ 0  s k   =   l 0 ,   ∀ k   ∈ { 1 ,   2 ,   3 } , ρ 0  s k   >   | e s k   ( t 0 ) | ,   ∀ k   ∈ { 4 ,   5 ,   6 }   and   ρ 0  s k   > ρ ∞  s k   >  0 , l s k   >   0 ,   ∀ k   ∈ { 1 , . . . ,   6 } .  Step I-b . Define the normalized errors   ξ s k   : [ t 0 ,   ∞ )   →   R :  ξ s k   ( t ) =   ρ − 1  s k   ( t ) e s k   ( t ) ,   ∀ k   ∈ { 1 , . . . ,   6 } ,   (14) and design the reference velocity vector as    ̇ x ∗  d   :   R 6   ×  [ t 0 ,   ∞ )   →   R 6 , with    ̇ x ∗  d   = [  ̇ x ∗  d 1   , . . . ,    ̇ x ∗  d 6   ] T   and:   ̇ x ∗  d k   ( ξ s k   , t ) =   − g s k   ε s k   ( ξ s k   ) ,   ∀ k   ∈ { 1 , . . . ,   6 } ,   (15) where   g s k   >   0   and   ε s k   :   R   →   R   is the transformed error:  ε s k   ( ξ s k   ) = ln  (   1 +   ξ s k  1   −   ξ s k  )  ,   ∀ k   ∈ { 1 , . . . ,   6 } .   (16) Before proceeding, note that (13) and (14) suggest that  x O   =   P s ( t ) ξ s   +   x d ( t ) , where   ξ s   =   [ ξ s 1   , . . . , ξ s 6   ] T   and  P s   =   diag { [ ρ s k   ] k ∈{ 1 ,..., 6 } }   . Therefore, by employing (5) we can write   x   =   [ x T  O   , f   T  O 1   ( x O   ) , . . . , f   T  ON   ( x O   )] T   =   [ ξ T s   P s ( t ) +  x T d   ( t ) , f   T  O 1   ( P s ( t ) ξ s   +   x d ( t )) , . . . , f   T  ON   ( P s ( t ) ξ s   +   x d ( t ))] T   =  f   O   ( P s ( t ) ξ s   + x d ( t )) , i.e., we can express   x   as a function of   ξ s  and   t . Therefore, in the following, the dependence on   x   will directly imply dependence on   ξ s , t . Similarly, dependence on   ̇ x   will imply dependence on   ξ s ,    ̇ ξ s   and   t .  Step   II-a .   Define   the   velocity   error   vector  e v   :   R 6   ×   [ t 0 ,   ∞ )   →   R 6   with   e v   ( ξ s , t )   = [ e v 1   ( ξ s 1   , t ) , . . . , e v 6   ( ξ s 6   , t )] T   =    ̇ x O   ( q ( t ))   −    ̇ x ∗  d ( ξ s , t )  and   select   the   corresponding   positive   performance functions   ρ v k   :   [ t 0 ,   ∞ )   →   R > 0   with   ρ v k   ( t )   =   ( ρ 0  v k   −  ρ ∞  v k   ) e − l vk   ( t − t 0 )   +   ρ ∞  v k   , such that   ρ 0  v k   >   | e v k   ( t 0 ) | , l v k   >   0  and   ρ 0  v k   > ρ ∞  v k   >   0 ,   ∀ k   ∈ { 1 , . . . ,   6 } .  Step   II-b . Define the normalized velocity errors   ξ v   : [ t 0 ,   ∞ )   →   R 6 :  ξ v   ( t ) = [ ξ v 1   ( t ) , . . . , ξ v 6   ( t ))] T   =   P   − 1  v   ( t ) e v   ( ξ s ( t ) , t ) ,   (17) where   P v   ( t )   =   diag { [ ρ v k   ( t )] k ∈{ 1 ,..., 6 } } ,   and   design   the distributed control protocol   u i   :   R 6   ×   R 6   ×   [ t 0 ,   ∞ )   →   R 6 :  u i ( ξ s , ξ v   , t ) =   − c i g v   J − T  Oi   ( x ) P   − 1  v   ( t ) R v   ( ξ v   ) ε v   ( ξ v   ) ,   (18) where   g v   >   0 , J Oi   as defined in (4) and   c i   are predefined load sharing coefficients satisfying   ∑  i ∈{ 1 ,...,N   }   c i   = 1   and  0   ≤   c i   ≤   1 ,   ∀ i   ∈ { 1 , . . . , N   } . Also,   R v   :   R 6   →   R 6 × 6 , ε v   :  R 6   →   R 6   are defined as:  R v   ( ξ v   )   =   diag  {[ 2(1   −   ξ 2  v k   ) − 1 ]  k ∈{ 1 ,..., 6 }  }  (19)  ε v   ( ξ v   )   =  [  ln  (   1 +   ξ v 1  1   −   ξ v 1  )  , . . . ,   ln  (   1 +   ξ v 6  1   −   ξ v 6  )] T  (20) The control law (18) can be written in vector form   u   = [ u T  1   , . . . , u T N   ] T   :  u ( ξ s , ξ v   , t ) =   U   j ′  j   ( t ) =   − C g   G ∗ ( x ) P   − 1  v   ( t ) R v   ( ξ v   ) ε v   ( ξ v   ) ,   (21)  where   C g   =   g v   diag { [ c i I 6 × 6 ] i ∈{ 1 ,...,N   } } ∈   R 6 N   × 6 N  ≥ 0   ,   G ∗ ( x ) = [ J − 1  O 1   , . . . , J − 1  ON   ] T   ∈   R 6 N   × 6   and the notation   U   j ′  j   stands for the transition from   π j   to   π j ′   .  Remark 3.   It can be verified by (15) and (18) that the proposed control protocol is distributed in the sense that each agent needs feedback only from the state of the ob- ject’s center of mass, which can be obtained by (2) and (3), as discussed in Section III-A. The parameters needed for the computation of   ρ s k   , ρ v k   , η d   as well as   g s k   , c i , k   ∈ { 1 , . . . ,   6 } , i   ∈ { 1 , . . . , N   }   can be transmitted off-line to the agents. Moreover, the overall control scheme does not incorporate any prior knowledge of the model nonlineari- ties/disturbances or force/torque measurements at the contact points.  Remark   4.   Note   that   internal   force   regulation   can   be also guaranteed by including in   (21)   a term of the form  ( I 6 N   −   1  N   G ∗ G T   ) ˆ f int,d , where   ˆ f int,d   is a constant vector that can be transmitted off-line to the agents. Nevertheless, the computation of   G ∗ G T   requires knowledge of all grasping points   p Ei   , which reduces to knowledge of the offsets   p O Ei/O  (since the agents can compute   R O   and   p O   ), that can be also transmitted off-line to the agents.  The next theorem summarizes the results of this section.  Theorem 2.   Consider   N   agents rigidly grasping an object with unknown coupling dynamics described by (10) and  A ( q ( t 0 ))   ∈   π j   , j   ∈ { 1 , . . . , R } . Then, the distributed control protocol (14)-(20) guarantees that   π j   →   π j ′   with time duration   δt j,j ′   and all closed loop signals being bounded, and thus establishes a transition relation between   π j   and   π j ′  for the coupled system object-agents, according to Def. 3. Proof.   Differentiating (14) and (17) with respect to time, employing (13), (10) as well as the facts that    ̇ x O   =    ̇ x ∗  d   +
P v   ( t ) ξ v   ,   ∑ N i =1   c i   = 1   and substituting (15), (21), we obtain:   ̇ ξ s k   ( ξ, t )   =   h s k   ( ξ, t ) =   − g s k   ρ − 1  s k   ( t ) ε s k   ( ξ s k   )   −   ρ − 1  s k   ( t )(  ̇ x d k   ( t ) +  ̇ ρ s k   ( t ) ξ s k   −   ρ v k   ( t ) ξ v k   ) ,   ∀ k   ∈ { 1 , . . . ,   6 }   (22)   ̇ ξ v   ( ξ, t )   =   h v   ( ξ, t ) =   − g v   P   − 1  v   ( t )    ̃ M   − 1 ( x ) P   − 1  v   ( t ) R v   ( ξ v   ) ε v   ( ξ v   )  − P   − 1  v   ( t )  [  ̃ M   − 1 ( x )(    ̃ C ( x,    ̇ x )( P v   ( t ) ξ v   +  ̇ x ∗  d ) +  ̃ h ( x,    ̇ x ) +    ̃ w ( x, t )) +  ̇ P v   ( t ) ξ v   +   ∂ ∂t    ̇ x ∗  d  ]  (23)  for all   t   ≥   t 0   with   ξ ( t ) = [ ξ T s   ( t ) , ξ T v   ( t )] T   . By defining   h   :  R 6   × R 6   × [ t 0 ,   ∞ )   →   R 12 , we can write (22), (23) in compact form:    ̇ ξ   =   h ( ξ, t ) = [ h T s   ( ξ, t ) , h T v   ( ξ, t )] T   (24) We further define the open and nonempty set   Ω ξ   = Ω ξ s   ×  Ω ξ v   ⊂   R 12   with   Ω ξ s   = Ω ξ v   = ( − 1 ,   1) 6 . Since   A ( q ( t 0 ))   ∈   π j   ,   Def.   2   implies   that  d ( p O   ( q ( t 0 )) , p c π j   )   <   l 0 .   Also,   p d ( t 0 )   =   p c π j   . Therefore,   by   choosing   ρ 0  s k   =   l 0 ,   ∀ k   ∈   { 1 ,   2 ,   3 }  as   well   as   ρ 0  s k   >   | e s k   ( t 0 ) | ,   ∀ k   ∈   { 4 ,   5 ,   6 }   we guarantee   that   ξ s ( t 0 )   ∈   Ω ξ s   .   Furthermore,   by   selecting  ρ 0  v k   >   | e v k   ( t 0 ) | ,   ∀ k   ∈   { 1 , . . . ,   6 } ,   we   guarantee   that  ξ v   ( t 0 )   ∈   Ω ξ v   .   Thus,   we   conclude   that   ξ ( t 0 )   ∈   Ω ξ   . Additionally,   h   is continuous on   t   and locally Lipschitz on  ξ   over   Ω ξ   . Therefore, according to Theorem   1 , there exists a maximal solution   ξ ( t )   of (24) on a time interval   [ t 0 , t max )  such that   ξ ( t )   ∈   Ω ξ   ,   ∀ t   ∈   [ t 0 , t max ) . Thus:  ξ m k   ( t ) =   e m k  ρ m k  ∈   ( − 1 ,   1) ,   ∀ k   ∈ { 1 , . . . ,   6 } , m   ∈ { s, v }  (25)  ∀ t   ∈   [ t 0 , t max ) , from which we obtain that   e s k   ( t )   and   e v k   ( t )  are bounded by   ρ s k   ( t )   and   ρ v k   ( t ) , respectively. Therefore, the error vectors   ε s k   ( ξ s ) ,   ∀ k   ∈ { 1 , . . . ,   6 }   and   ε v   ( ξ v   ) , as defined in (16) and (20), respectively, are well defined for all   t   ∈   [ t 0 , t max ) . Hence, consider the positive definite and radially unbounded functions   V s k   :   R   →   R ≥ 0   with  V s k   ( ε s k   ) =   ε 2  s k   ,   ∀ k   ∈ { 1 , . . . ,   6 } . By differentiating   V s k   with respect to time and substituting (22), we obtain:   ̇ V s k   =   −   4 ε s k   ρ − 1  s k   ( t ) 1   −   ξ 2  s k  ( g s k   ε s k   +  ̇ x d k   ( t ) +  ̇ ρ s k   ( t ) ξ s k   −   ρ v k   ( t ) ξ v k   )  Next, since    ̇ x d k   , ρ v k   ,    ̇ ρ s k   are bounded by construction and  ξ s k   , ξ v k   are bounded in   ( − 1 ,   1)   owing to (25),    ̇ V s k   becomes:   ̇ V s k   ≤   4 ρ − 1  s k   ( t ) 1   −   ξ 2  s k  ( B s | ε s k   | −   g s k   | ε s k   | 2 ) ,  ∀ t   ∈   [ t 0 , t max ) , where   B s   is an unknown positive constant independent of   t max   satisfying   B s   ≥ |    ̇ x d k   ( t ) +  ̇ ρ s k   ( t ) ξ s k   −  ρ v k   ( t ) ξ v k   |   . Therefore, we conclude that    ̇ V s k   is negative when  | ε s k   |   > B s g − 1  s k   and subsequently that  | ε s k   ( ξ s k   ( t )) | ≤   ε s k   = max   { | ε s k   ( ξ s k   ( t 0 )) | , B s g − 1  s k  }   ,   (26)  ∀ t   ∈   [ t 0 , t max ) , k   ∈ { 1 , . . . ,   6 } . Furthermore, from (16), taking the inverse logarithm, we obtain:  −   1   < e − ε sk   −   1  e − ε sk   + 1 =   ξ s k  ≤   ξ s k   ( t )   ≤   ξ s k   =   e ε sk   −   1  e ε sk   + 1   <   1   (27)  ∀ t   ∈   [ t 0 , t max ) , k   ∈ { 1 , . . . ,   6 } . Due to (26), the reference velocity vector    ̇ x ∗  d ( ξ s , t ) , as defined in (15), remains bounded for all   t   ∈   [ t 0 , t max ) . Moreover, invoking    ̇ x O   =  ̇ x ∗  d ( ξ s , t ) +  P v   ( t ) ξ v   and (25), we also conclude the boundedness of   ̇ x O   for all   t   ∈   [ t 0 , t max ) . Finally, differentiating    ̇ x ∗  d ( ξ s , t )  with respect to time and employing (22), (25) and (27), we conclude the boundedness of   ∂ ∂t    ̇ x ∗  d ( ξ s ,    ̇ ξ s , t ) ,   ∀ t   ∈   [ t 0 , t max ) . Applying the aforementioned line of proof, we consider the positive definite and radially unbounded function   V v   :  R 6   →   R   with   V v   ( ε v   ) =   0 . 5 ‖ ε v   ‖ 2 . By differentiating   V v  with respect to time, we substitute (23) and by employing (i) the continuity of    ̃ M ,    ̃ C,    ̃ h   and (ii) the boundedness of  w O   , w i , ξ s , ξ v   ,    ̇ P v   , ∂ ∂t    ̇ x ∗  d ,   ∀ t   ∈   [ t 0 , t max ) , we obtain:   ̇ V v   ≤ ‖ P   − 1  v   ( t ) R v   ( ξ v   ) ε v   ‖ ( B v   −   g v   λ m ‖ P   − 1  v   ( t ) R v   ( ξ v   ) ε v   ‖ )  ∀ t   ∈   [ t 0 , t max ) , where   λ m   is the minimum singular value of the positive definite matrix    ̃ M   − 1   and   B v   is a positive constant independent of   t max , satisfying  B v   ≥  ∥ ∥ ∥  ̃ M   − 1 ( x )( C ( x,    ̇ x )( P v   ( t ) ξ v   +  ̇ x ∗  d ( t )) +    ̃ h ( x,    ̇ x )+   ̃ w ( x, t ) +  ̇ P v   ξ v   +   ∂ ∂t    ̇ x ∗  d ( ξ s ,    ̇ ξ s , t ))  ∥ ∥ ∥ ∥  Therefore,    ̇ V v   is   negative   when   ‖ P   − 1  v   ( t ) R v   ( ξ v   ) ε v   ‖   > B v   ( g v   λ m ) − 1 , which, by employing the definitions of   P v  and   R v   ,   becomes   ‖ ε v   ‖   >   B v   ( g v   λ m ) − 1 ρ 0  v k   ,   with   ρ 0  v k   = max k ∈{ 1 ,..., 6 } { ρ 0  v k   } . Therefore, we conclude that  ‖ ε v   ( ξ v   ( t )) ‖ ≤   ε v   = max   { ε v   ( ξ v   ( t 0 )) , B v   ( g v   λ m ) − 1 ρ 0  v k  }   ,  ∀ t   ∈   [ t 0 , t max ) . Furthermore, from (20), taking the inverse logarithm and invoking that   | ε v k   | ≤ ‖ ε v   ‖ , we obtain:  −   1   < e − ε vk   −   1  e − ε vk   + 1 =   ξ v k  ≤   ξ v k   ( t )   ≤   ξ v k   =   e ε vk   −   1  e ε vk   + 1   <   1   (28)  ∀ t   ∈   [ t 0 , t max ) , k   ∈ { 1 , . . . ,   6 } , which also leads to the boundedness of the distributed control protocol (21). Up to this point, what remains to be shown is that   t max   can be extended to   ∞ .   In this direction, notice by (27) and (28) that   ξ ( t )   ∈   Ω ′  ξ   = Ω ′  ξ s   ×   Ω ′  ξ v   ,   ∀ t   ∈   [ t 0 , t max ) , where:  Ω ′  ξ m   = [ ξ m 1  , ξ m 1   ]   × · · · ×   [ ξ m 6  , ξ m 6   ] , m   ∈ { s, v }  are nonempty and compact subsets of   Ω ξ s   and   Ω ξ v   , respec- tively. Hence, assuming that   t max   <   ∞   and since   Ω ′  ξ   ⊂   Ω ξ   , Proposition 1 dictates the existence of a time instant   t ′   ∈  [ t 0 , t max )   such that   ξ ( t ′ )   / ∈   Ω ′  ξ   , which is a clear contradiction. Therefore,   t max   =   ∞ . Thus, all closed loop signals remain bounded and moreover   ξ ( t )   ∈   Ω ′  ξ   ,   ∀ t   ≥   t 0 . By multiplying (27) with   ρ s k   ( t ) , we obtain   | e s k   ( t ) |   < ρ s k   ( t ) ,   ∀ k   ∈   { 1 , . . . ,   6 }   and hence   | e s k   ( t ) |   <   l 0 ,   ∀ k   ∈ { 1 ,   2 ,   3 } , t   ∈   [ t 0 ,   ∞ ) , since   ρ 0  s k   =   l 0 ,   ∀ k   ∈ { 1 ,   2 ,   3 } . There- fore,   p O   ( q ( t ))   ∈ B ( p d ( t ) , l 0 ) ,   ∀ t   ≥   t 0   and, consequently,  p O   ( q ( t 0   +   δt j,j ′   ))   ∈ B ( p c π j ′   , l 0 ) , since   p d ( t 0   +   δt j,j ′   ) =  p c π j ′   . Also, since   p O   ( q ( t ))   ∈ B ( p d ( t ) , l 0 ) , we deduce that  B ( p O   ( q ( t )) ,   ˆ L )   ∈ B ( p d ( t ) , l 0   + ˆ L )   and invoking (11) and (12), we conclude that   p s   ∈   π j   ∪   π j ′   ,   ∀ t   ∈   [ t 0 , t 0   +   δt j,j ′   ]   ⊂  [ t 0 ,   ∞ ) , and therefore a transition relation with time duration  δt j,j ′   is successfully established.
Remark 5.   Instead of employing the control protocol (21) over   [ t 0 ,   ∞ ) , we can define it over a finite time interval as  u   =   U   j ′  j   ([ t 0 , t 0   +   δt j,j ′   )) . In that case, it follows by the con- tinuity of   d, p O   , p d   that   lim t → ( t 0 + δt j,j ′   ) −   d ( p O   ( t ) , p d ( t )) =  d ( p O   ( t 0   + δt j,j ′   ) , p c π j ′   )   and therefore, the transition   π j   →   π j ′  with time duration   δt j,j ′   is still achieved. Moreover, the predefined selection of   δt j,j ′   for each transition   π j   →   π j ′   is related to the control capabilities of the agents, since smaller  δtj, j ′   will produce larger, but still bounded,    ̇ x ∗  d   and   u . B. High-Level Plan Generation  The second part of our solution is the derivation of a high- level plan that satisfies the given MITL formula   φ   and can be generated using standard techniques from automata-based formal verification methodologies. Thanks to our proposed control law that allows the transition   π j   →   π j ′   for all  π j   ∈   Π   with   π j ′   ∈ D ( π j   )   in a predefined time interval  δt j,j ′   , we can abstract the motion of the coupled system object-agents as a finite weighted transition system (WTS) [33]   T   =   { Π ,   Π 0 ,   → ,   AP ,   L , γ } , where   Π   is the set of states defined in Section III-B,   Π 0   ⊂   Π   is a set of initial states,  →⊆   Π   ×   Π   is a transition relation according to Def. 3,   AP  and   L   are the atomic propositions and the labeling function, respectively, as defined in Section III-C, and   γ   : →→   R ≥ 0   is a map that assigns to each transition its time duration, i.e.,  γ ( π j   →   π j ′   ) =   δt j,j ′   . Therefore, by designing the switching control protocol   U   r j +1  r j   ( t )   from (21):  u   =   U   r j +1  r j   ( t ) ,   ∀ t   ∈   [ t j   , t j   +   δt r j   ,r j +1   ) , j   ∈   N ,   (29) with   (i)   t 1   =   0 ,   (ii)   t j +1   =   t j   +   δt r j   ,r j +1   and   (iii)  r j   ∈ { 1 , . . . , R } ,   ∀ j   ∈   N , we can define a   timed run   of the WTS as an infinite sequence   r   = ( π r 1   , t 1 )( π r 2   , t 2 )   . . .   , where   π r 1   ∈   Π 0   with   A ( q (0))   ∈   π r 1   , π r j   ∈   Π , r j   ∈ { 1 , . . . , R }   and   t j   are the corresponding time stamps such that   A ( q ( t j   ))   ∈   π r j   ,   ∀ j   ∈   N . Every timed run   r   generates a   timed word   w ( r ) = ( L ( π r 1   ) , t 1 )( L ( π r 2   ) , t 2 )   . . .   over   AP  where   L ( π r j   ) , j   ∈   N   is the subset of the atomic propositions  AP   that are true when   A ( q ( t j   ))   ∈   π r j   . The given MITL formula   φ   is translated into a   Timed Büchi Automaton   A t φ   [17] and the product   A p   =   T ⊗ A t φ   is built [33]. The projection of the accepting runs of   A p   onto   T  provides a   timed run   r φ   of   T   that satisfies   φ ;   r φ   has the form   r φ   = ( π r 1   , t 1 )( π r 2   , t 2 )   . . .   , i.e., an infinite 1   sequence of regions   π r j   to be visited at specific time instants   t j   (i.e.,  A ( q ( t j   ))   ∈   π r j   ) with   t 1   = 0   and   t j +1   =   t j   +   δt r j   ,r j +1   , r j   ∈ { 1 , . . . , R } ,   ∀ j   ∈   N . More details on the technique are beyond the scope of this paper and the reader is referred to [17], [20], [33]. The execution of   r φ   =   ( π r 1   , t 1 )( π r 2   , t 2 )   . . .   produces a trajectory   q ( t ) , t   ∈   R ≥ 0 , with timed sequence   β φ   = ( q 1 ( t ) , t 1 )( q 2 ( t ) , t 2 )   . . .   , with   A ( q j   ( t j   ))   ∈   π r j   ,   ∀ j   ∈   N . Following   Def.   4,   β φ   has   the   timed   behavior   σ β φ   = ( σ 1 , t 1 )( σ 2 , t 2 )   . . .   with   σ j   ∈   L ( π r j   ) , for   A ( q j   ( t j   ))   ∈  1 It can be proven that if such a run exists, then there also exists a run that can be always represented as a finite prefix followed by infinite repetitions of a finite suffix [33].  (a) A robotic agent   (b) Top view of the initial workspace Fig. 4.   (a): A robotic agent. (b): Top view of the initial workspace with three agents (with blue, red and green) carrying an object (with black) in  π 1 . Goal regions are marked with blue and green whereas obstacle regions with red.  π r j   ,   ∀ j   ∈   N . Since all   π r j   belong to   r φ ,   ∀ j   ∈   N , the latter implies that   σ β φ   | =   φ   and therefore that   β φ   satisfies   φ . The aforementioned discussion is summarized as follows:  Theorem 3.   The execution of   r φ   = ( π r 1   , t 1 )( π r 2   , t 2 )   . . .   of  T   that satisfies   φ   guarantees a timed behavior   σ β φ   of the coupled system object-agents that yields the satisfaction of  φ   and provides, therefore, a solution to Problem 1.  V. S IMULATION   R ESULTS  The validity of the proposed framework is verified through simulation studies. We consider three robots consisting of two joints, three links and a mobile platform that is able to move along the   x   −   y   plane (Fig. 4(a)). We denote as  q i   = [ p T c,i , η T c,i , q i 1   , q i 2   ] T   ∈   R 8   the generalized coordinates of each agent, where   p c,i , η c,i   ∈   R 3   denote the position and orientation of the   i th agent’s platform and   q i 1   , q i 2   are the joint angles,   ∀ i   ∈ { 1 ,   2 ,   3 } . The agents grasp a rigid rod of length   0 . 4 m. The initial coordinates of the system are taken as   p c, 1 (0)   =   [1 . 06 ,   2 ,   0 . 1] T   m , p c, 2 (0)   =   [2 . 14 ,   2 ,   0 . 1] T   m,  p c, 3 (0) = [1 . 6 ,   2 . 34 ,   0 . 1] T   m,   η c, 1 (0) = [0 ,   0 ,   0] T   r,   η c, 2 (0) = [0 ,   0 , π ] T   r,   η c, 3 (0) = [0 ,   0 ,   − π/ 2] T   r,   q i 1   =   q i 2   =   π  4   r,   ∀ i   ∈ { 1 ,   2 ,   3 } , and   p O   (0) = [1 . 6 ,   2 ,   0 . 44] T   m,   η O   (0) = [0 ,   0 ,   0] T   r. The workspace is partitioned into   R   = 16   regions, with  ˆ L   = 1 . 5 m , l 0   = 0 . 5 m and   p c π 1   = [2 ,   2 ,   2] T   m, from which it can be verified that   A ( q (0))   ∈   π 1 . We further choose  δt j,j ′   = 5 s,   ∀ j, j ′   ∈ { 1 , . . . ,   16 }   and we define the atomic propositions   AP   =   { “ blue ” ,   “ green ” ,   “ obs ” ,   ∅}   representing goal ( “ blue ”   and   “ green ” ) and obstacle ( “ obs ” ) regions as well as   L ( π 5 ) =   { “ blue ” } ,   L ( π 14 ) =   { “ green ” } ,   L ( π 6 ) =  L ( π 10 ) =   { “ obs ” }   and   L ( π j   ) =   ∅   for the remaining regions. Fig. 4(b) depicts the aforementioned workspace. We consider the MITL formula   φ   = (  [0 , ∞ ) ¬ “ obs ”)   ∧  ♦ [0 , 50] (“ green ”   ∧   ♦ [0 , 20] “ blue ”) , which represents the be- havior   of   (i)   always   avoiding   the   obstacle   regions   and (ii)   eventually   within   50 s   visiting   the   “ green ”   region and   after   20 s   the   “ blue ”   region.   By   following   the procedure   described   in   Section   IV-B,   we   obtain   the accepting   timed   run   r   =   ( π r 1   , t 1 )(( π r 2   , t 2 ))   · · ·   = ( π 1 ,   0)( π 2 ,   5)( π 3 ,   10)( π 4 ,   15)( π 5 ,   20)( π 12 ,   25)( π 13 ,   30) ( π 14 ,   35)( π 11 ,   40)( π 12 ,   45)( π 5 ,   50) .
Fig. 5.   The desired object trajectories (with green), the actual object trajectories (with black), the domain specified by   B ( p d ( t ) , l 0 )   (with red) and the domain specified by   B ( p O   ( t ) ,   ˆ L ) , (with purple) for   t   ∈   [0 ,   50]  sec. Since   p O   ( t )   ∈ B ( p d ( t ) , l 0 ) ,   ∀ t   ∈   [0 ,   50] s, the desired timed run is successfully executed.  Regarding each transition   π r j   →   π r j +1   , j   ∈ { 1 , . . . ,   10 } , we chose   η d   = [0 ,   0 ,   0] T   , ρ 0  s k   =   l 0   = 0 . 5 , ρ ∞  s k   = 0 . 01   and  l s k   = 1   as well as   ρ 0  v k   = 2 | e v k   ( t j   ) |   + 0 . 1 , ρ ∞  v k   = 0 . 01   and  l v k   = 1 ,   ∀ k   ∈ { 1 , . . . ,   6 } . The control gains were chosen as   g s k   =   0 . 1   and   g v   =   2 . 5 ,   ∀ k   ∈   { 1 , . . . ,   6 }   to retain the required input signals   u i   within feasible ranges that can be implemented by real actuators. Finally,   w O   , w i   and   f i  were chosen as sinusoidal functions of time and the load sharing coefficients were selected as   c 1   = 0 . 5 , c 2   = 0 . 35  and   c 3   = 0 . 15   to demonstrate a potential difference in the power capabilities of the agents. Fig. 5 depicts the transitions of the coupled system object-agents. It can be deduced from the figure that   p O   ∈ B ( p d ( t ) , l 0 ) ,   ∀ t   ∈   [0 ,   50]   and therefore  p s   ∈   π r j   ∪   π r j +1   ,   ∀ p s   ∈ S q   , j   ∈ { 1 , . . . ,   10 } , verifying thus the theoretical results. Moreover, Fig. 6 shows the control inputs of the three agents, demonstrating the effect of the load sharing coefficients. VI. C ONCLUSION AND   F UTURE   W ORK  In this work we proposed a hybrid control strategy for the cooperative manipulation of an object by   N   agents under MITL specifications. In particular, we developed a robust   decentralized   control   protocol   that   allowed   us   to abstract the motion of the coupled system object-agents as a finite transition system. Then, we employed standard formal- verification tools for the derivation of a path that satisfied the high level goal. Future efforts will be devoted towards considering non-rigid grasps and compensating for uncertain geometric characteristics of the objects. R EFERENCES [1]   Y.-H. Liu and S. Arimoto, “Decentralized adaptive and nonadaptive position/force controllers for redundant manipulators in cooperations,”  International Journal of Robotics Research , 1998.  Fig. 6.   The resulting control inputs   u i   =   [ u i (1) , . . . , u i (6)] T   , i   ∈ { 1 ,   2 ,   3 } . Top three plots:   u i (1) , u i (2) , u i (3) , for   i   = 1 ,   2 ,   3 , respectively. Bottom three plots:   u i (4) , u i (5) , u i (6) , for   i   = 1 ,   2 ,   3 , respectively. [2]   F.   Caccavale,   P.   Chiacchio,   A.   Marino,   and   L.   Villani,   “Six-dof impedance control of dual-arm cooperative manipulators,”   Transac- tions on Mechatronics , 2008. [3]   D. Heck, D. Kostic, A. Denasi, and H. Nijmeijer, “Internal and external force-based impedance control for cooperative manipulation,” in   European Control Conf. , 2013. [4]   J. Szewczyk, F. Plumet, and P. Bidaud, “Planning and controlling cooperating robots through distributed impedance,”   JRS , 2002. [5]   A. Tsiamis, C. Verginis, C. Bechlioulis, and K. Kyriakopoulos, “Co- operative manipulation exploiting only implicit communication,” in  Intern. Conf. on Intelligent Robots and Systems , 2015. [6]   A. Petitti, A. Franchi, D. D. Paola, and A. Rizzo, “Decentralized motion control for cooperative manipulation with a team of networked mobile manipulators,”   ICRA , 2016. [7]   Z. Wang and M. Schwager, “Multi-robot manipulation with no com- munication using only local measurements,” in   CDC , 2015. [8]   T. G. Sugar and V. Kumar, “Control of cooperating mobile manipula- tors,”   Transactions on Robotics and Automation , 2002. [9]   H. G. Tanner, S. G. Loizou, and K. J. Kyriakopoulos, “Nonholonomic navigation and control of cooperating mobile manipulators,”   Transac- tions on Robotics and Automation , 2003. [10]   S. Erhart and S. Hirche, “Adaptive force/velocity control for multi- robot cooperative manipulation under uncertain kinematic parameters,” in   Intern. Conf. on Intelligent Robots and Systems , 2013. [11]   J. Markdahl, Y. Karayiannidis, and X. Hu, “Cooperative object path following   control   by   means   of   mobile   manipulators:   a   switched systems approach,”   IFAC Proceedings Volumes , 2012. [12]   M. Guo, J. Tumova, and D. V. Dimarogonas, “Cooperative decen- tralized multi-agent control under local ltl tasks and connectivity constraints,” in   Conf. on Decision and Control , 2014. [13]   Y. Chen, X. C. Ding, A. Stefanescu, and C. Belta, “Formal approach to the deployment of distributed robotic teams,”   TRO , 2012. [14]   Z. Zhang and R. V. Cowlagi, “Motion-planning with global temporal logic specifications for multiple nonholonomic robotic vehicles,” in  Americal Control Conf. , 2016. [15]   I. Filippidis and R. M. Murray, “Symbolic construction of gr(1) contracts for systems with full information,” in   ACC , 2016. [16]   Y. Diaz-Mercado, A. Jones, C. Belta, and M. Egerstedt, “Correct-by- construction control synthesis for multi-robot mixing,” in   CDC , 2015. [17]   R. Alur and D. L. Dill, “A theory of timed automata,”   Theor. Comput. Sci. , 1994. [18]   R. Alur, T. Feder, and T. A. Henzinger, “The benefits of relaxing punctuality,”   Journal of the ACM , 1996. [19]   D. D’Souza and P. Prabhakar, “On the expressiveness of mtl in the pointwise and continuous semantics,”   International Journal on Software Tools for Technology Transfer , 2007. [20]   A. Nikou, J. Tumova, and D. V. Dimarogonas, “Cooperative task
planning of multi-agent systems under timed temporal specifications,”  Americal Control Conf. , 2016. [21]   S. Karaman and E. Frazzoli, “Linear temporal logic vehicle routing with applications to multi-uav mission planning,”   Intern. Journal of Robust and Nonlinear Control , 2011. [22]   D. Aksaray, C.-I. Vasile, and C. Belta, “Dynamic routing of energy- aware vehicles with temporal logic constraints,”   ICRA , 2016. [23]   A. Yamashita, T. Arai, J. Ota, and H. Asama, “Motion planning of multiple mobile robots for cooperative manipulation and transporta- tion,”   Trans. on Robotics and Automation , 2003. [24]   P. Cheng, J. Fink, and V. Kumar, “Abstractions and algorithms for cooperative multiple robot planar manipulation,”   Robotics: Science and Systems IV , 2009. [25]   G. Lionis and K. J. Kyriakopoulos, “Centralized motion planning for a group of micro agents manipulating a rigid object,” in   MED , 2005. [26]   A.   Tsiamis,   J.   Tumova,   C.   P.   Bechlioulis,   G.   C.   Karras,   D.   V. Dimarogonas, and K. J. Kyriakopoulos, “Decentralized leader-follower control under high level goals without explicit communication,” in  Intern. Conf. on Intelligent Robots and Systems , 2015. [27]   S. Chinchali, S. C. Livingston, U. Topcu, J. W. Burdick, and R. M. Murray, “Towards formal synthesis of reactive controllers for dexter- ous robotic manipulation,” in   ICRA , 2012. [28]   K. He, M. Lahijanian, L. E. Kavraki, and M. Y. Vardi, “Towards manipulation planning with temporal logic specifications,” in   Intern. Conf. on Robotics and Automation , 2015. [29]   L. Chaimowicz, M. Campos, and V. Kumar, “Hybrid systems modeling of cooperative robots,” in   ICRA , Sept 2003. [30]   B. Siciliano, L. Sciavicco, and L. Villani,   Robotics : modelling, planning and control .   Springer, 2009. [31]   E. D. Sontag,   Mathematical control theory: deterministic finite dimen- sional systems .   Springer Science & Business Media, 2013, vol. 6. [32]   C. P. Bechlioulis and G. A. Rovithakis, “A low-complexity global approximation-free control scheme with prescribed performance for unknown pure feedback systems,”   Automatica , 2014. [33]   C. Baier, J.-P. Katoen   et al. ,   Principles of model checking .   MIT press Cambridge, 2008.