Gennaro Raiola

Xavier Lamy

Freek Stulp

Co-manipulation with Multiple Probabilistic Virtual Guides

In co-manipulation, humans and robots solve manipulation tasks together. Virtual guides are important tools for co-manipulation, as they constrain the movement of the robot to avoid undesirable effects, such as collisions with the environment. Defining virtual guides is often a laborious task requiring expert knowledge. This restricts the usefulness of virtual guides in environments where new tasks may need to be solved, or where multiple tasks need to be solved sequentially, but in an unknown order.

To this end, we propose a framework for multiple probabilistic virtual guides, and demonstrate a concrete implementation of such guides using kinesthetic teaching and Gaussian mixture models. Our approach enables non-expert users to design virtual guides through demonstration. Also, they may demonstrate novel guides, even if already known guides are active. Finally, users are able to intuitively select the appropriate guide from a set of guides through physical interaction with the robot. We evaluate our approach in a pick-and-place task, where users are to place objects at one of several positions in a cupboard.

I. INTRODUCTION

Industrial robots have had a crucial role in achieving very efficient assembly of products with a consistent, high quality. However, due to limitations of robots in terms of their adaptivity and flexibility, and difficulties in tracking and manipulating non-rigid objects, many assembly lines still require human workers. To exploit the best of both worlds, co-manipulation enables robots and humans to solve manipulation/assembly tasks together. One successful approach to enabling joint human-robot tasks has been the "virtual guiding fixture" approach [START_REF] Lin | Portability and applicability of virtual fixtures across medical and manufacturing tasks[END_REF], which constrains the motion of the robot along only certain task-relevant trajectories. An example of a "guiding fixture" from everyday life is the ruler, which allows us to draw very straight lines by constraining the movement of the pen tip along a 1-D trajectory on the 2-D paper.

Virtual guiding fixtures are especially useful in contexts where human decision making is still required to perform the overall task, but where constraints on the accuracy or required forces of the motion preclude humans from efficiently or effectively performing such tasks without robot assistance. Examples include industrial tasks [START_REF] Li | Virtual reality for fixture design and assembly[END_REF], where heavy parts must be transported, and surgical tasks [START_REF] Park | Virtual fixtures for robotic cardiac surgery[END_REF], [START_REF] Becker | Vision-based control of a handheld surgical micromanipulator with virtual fixtures[END_REF], where accuracy and stability of the end-effector are essential. In this paper, we consider the task of placing objects in a cupboard, where guides assist the human in avoiding collisions with the shelves, as illustrated in Fig. 1. Fig. 1. Left: Using a virtual guiding fixture to facilitate the placement of objects in a cupboard with shelves. Right: Schematic illustration of modelling a virtual guiding fixture with a virtual mechanism [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF]. Here, the virtual mechanism is implemented as a virtual cart on a rail. It is connected to the robot end-effector with a spring-damper system.

One issue in using virtual guides is addressing contexts in which multiple tasks are to be solved, or where new tasks may arise during operation. How can users intuitively demonstrate guides for novel tasks? And if a movements are constrained to a guide, how can users 'escape' the guide to demonstrate new trajectories? And if multiple tasks are to be solved, how can the user indicate which guide is relevant for the current task? And how should the robot detect this user choice? We address these questions by making the following contributions: 1) Enabling users to demonstrate virtual guides through kinesthetic teaching, based on [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF]. 2) Introducing probabilistic virtual guides, which enable users to 'escape' a virtual guide, and thus to demonstrate novel guides for novel tasks. 3) Using multiple probabilistic virtual guides for multiple tasks; the robot estimates the probability of each guide to interpret user intentions, and to choose the correct guide for the current task. 4) Evaluating these contributions with several users on a 7-DOF robotic arm, as illustrated in Fig. 1.

The rest of this paper is structured as follows. In the next section, we discuss related work. In III, we introduce a possible way to generate virtual guides by using virtual mechanisms as proposed in [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF], which forms the basis of our work. We describe the general framework of multiple probabilistic virtual guides in Section IV, and a concrete implementation using Gaussian mixture models in Section V. We evaluate our approach with a humanoid robot in Section VI, and conclude with Section VII.

II. RELATED WORK

Virtual fixtures [START_REF] Rosenberg | Virtual fixtures: perceptual tools for telerobotic manipulation[END_REF], [START_REF] Abbott | Virtual fixture architectures for telemanipulation[END_REF] enforce virtual constraints on the movements of robots. In teleoperation for instance, virtual fixtures have been used to avoid robots falling into pits (with "forbidden regions virtual fixtures" [START_REF] Abbott | Virtual fixture architectures for telemanipulation[END_REF]), or to constrain the movement of a robot along a preferred direction or trajectory with "virtual guiding fixtures" [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF], which we sometimes refer to as 'virtual guides' or simply 'guides' in this paper.

Virtual guides are especially useful in contexts where human decision making is still required to perform the overall task, but where constraints on the accuracy or required forces of the motion preclude humans from performing such tasks without robot assistance. Virtual guides have been used for instance in industrial [START_REF] Li | Virtual reality for fixture design and assembly[END_REF] and surgical [START_REF] Park | Virtual fixtures for robotic cardiac surgery[END_REF] tasks. Colgate et al. provide an early overview of the use of intelligent assistive devices and virtual guides in industrial applications [10]. Yoon et al. [START_REF] Yoon | Modeling user's drivingcharacteristics in a steering task to customize a virtual fixture based on task-performance[END_REF] use inverse optimal control (IOC) to accomplish a steering task with a mobile robot, while Ryden et al. [START_REF] Ryden | Advanced telerobotic underwater manipulation using virtual fixtures and haptic rendering[END_REF] use virtual guides to teleoperate an underwater robot.

The particular implementation of virtual guides we use is based on the work by Joly [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF], where a passive virtual mechanism is connected to the robot tooltip by a springdamper system in a teleoperation context. Moreover, the passivity of the system is proven when using one virtual guide. We extended this proof to multiple probabilistic virtual guides in [START_REF] Raiola | Parallel guiding virtual fixtures: Control and stability[END_REF]. Virtual mechanisms have also been used by Pezzementi et al. [START_REF] Pezzementi | Dynamic guidance with pseudoadmittance virtual fixtures[END_REF], where they are called "proxies". Virtual guides may also be implemented by using anisotropic admittances to attenuate the non-preferred user force component [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF], [START_REF] Bettini | Vision assisted control for manipulation using virtual fixtures[END_REF]. These methods require the sensing of external inputs by the user, such as the force or the velocity applied by the user on the robot tooltip. This is not required with our control scheme.

Virtual guides have often been limited to pre-defined geometric shapes [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF] or combinations of shapes [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], [START_REF] Kuang | Assembling virtual fixtures for guidance in training environments[END_REF]. Generating guides from demonstrations has also been explored by Aarno et al. [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], who model demonstrations in a segmented sequence of straight lines. In our work, we train Gaussian mixture models with user demonstrations, as in [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF], which ensures smooth movements, and explicitly models the variance in user demonstrations.

One novel aspect of our work is to define probabilistic virtual guides. A first advantage of the probabilistic approach is that it enables a guide to be activated/deactivated based on the probability of belonging to it, which leads to smooth transitions. This is preferable to switching the guide on/off as in [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], [START_REF] Yu | Telemanipulation assistance based on motion intention recognition[END_REF], and does not require the manual design of distance thresholds for activation, as in [START_REF] Nolin | Activation cues and force scaling methods for virtual fixtures[END_REF]. A second advantage is that the probabilistic approach allows us to simultaneously activate and recognize several guides, by assigning probabilities to each guide based on user behavior. Thus, our method enables the use of a library of guides, with one guide for each distinct tasks. Multiple guides have been previously used, but these (sub)guides are activated sequentially for one unique task, rather than in parallel for several tasks. For instance, Kuang et al. [START_REF] Kuang | Assembling virtual fixtures for guidance in training environments[END_REF] combine different shape primitives to facilitate maze navigation. Aarno et al. [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF] use a Hidden Markov Model to probabilistically choose a guide in a sequence of linear guides to accomplish a pick and place task. Finally, unlike many of previous works, we use virtual guides in a co-manipulation framework instead of teleoperation; in this aspect our work can be compared to [START_REF] Ben Amor | Interaction primitives for human-robot cooperation tasks[END_REF] where the user and the robot have to execute a learned task together.

III. VIRTUAL MECHANISMS AS VIRTUAL GUIDES

We model the virtual guiding fixture using a virtual mechanism (VM) [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF]. In the context of this paper, the VM can best be thought as a cart moving along a rail. The robot end-effector and virtual "cart" mechanism are coupled by a spring-damper system. If the robot end-effector moves, it pulls the cart along the rail. On the other hand, the cart also pulls the robot towards the rail, because the connection pulls in both directions. The overall effect is that the robot endeffector can be moved easily along the virtual rail, but not away from the rail, which constrains the movement of the robot such that tasks are more easily achieved.

The position of the cart on the rail in Cartesian space is described by x vm . The distance it has travelled along the rail is function of the phase s vm , with s vm = 0 at the beginning and s vm = 1 at the end of the rail, as illustrated in Fig. 2. The kinematics of the virtual mechanism is described by:

x vm = f (s vm ) (1) ẋvm = J vm (s vm ) ṡvm (2)
Fig. 2. The main variables and equations of the virtual mechanism.

A. Force on the virtual mechanism

The virtual mechanism is connected to the robot endeffector with a virtual spring-damper system. The force applied to the virtual mechanism by the robot is:

F r = K(x r -x vm ) + B( ẋr -ẋvm ). (3)
The virtual mechanism is ideal, so the efforts applied on it are null

J vm F r = 0, (4)
which leads leads to

J vm (K(x r -x vm ) + B( ẋr -J vm ṡvm )) = 0. (5)
By solving [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF] with respect to ṡvm , we obtain a first order dynamical system that expresses the evolution of the virtual cart along the virtual rail:

ṡvm = (J vm BJ vm ) -1 J vm (K(x r -x vm ) + B ẋr ). (6)
Moving the robot end-effector away from the virtual cart (x r = x vm ) will thus make it slide along the rail, with a velocity described by [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF].

B. Force on the robot end-effector

Because the virtual mechanism and the robot end-effector are connected to each other, the virtual mechanism also applies a force on the robot end-effector, i.e.

F vm = -F r = K(x vm -x r ) + B( ẋvm -ẋr ). (7)
This virtual force can be transformed into actual control commands for the robot, for instance with a compliance controller. In our implementation, we used the Jacobian of the robot J r to convert the forces into torque references for the motor controllers.

IV. MULTIPLE PROBABILISTIC VIRTUAL GUIDES

Consider a scenario in which multiple guides are active, i.e. the robot end-effector is connected to multiple virtual mechanisms, as illustrated in Fig. 3. This scenario applies when there are multiple tasks, i.e. transporting an object to one of multiple possible positions. In this section, we propose three modes of interaction when multiple guides are active, based on probabilistic virtual mechanisms. Fig. 3. Multiple virtual mechanisms -one for each task -simultaneously connected to the robot end-effector.

A. Probabilistic Virtual Mechanisms

We define a "probabilistic virtual mechanism" to be a VM for which the position of the cart is modeled by a multivariate normal distribution

x vm ∼ N (µ vm , Σ vm ) (8) µ vm = f (s vm ) cf. (1) (9) Σ vm = v(s vm ) (10)
In Section V-B, we describe how the functions ( 9) and ( 10) are learned from demonstrated trajectories. This distribution has the probability density function

g(x; µ vm , Σ vm ) = e (-1 2 (x-µ vm ) Σvm -1 (x-µ vm )) (2π) k |Σ vm | . (11)
Because µ vm and Σ vm are completely specified by the phase through the functions in ( 9) and (10), we also define the abbreviation g(x; s vm ) = g(x; f (s vm ), v(s vm )).

(12)

B. Three interaction modes

When using multiple virtual guides, an assignment problem arises: which guide is currently responsible for guiding the human? Probabilistic virtual guides enable us to address this question by weighting the contribution of each guide with the probability that this is the guide chosen by the human. We define three weighting schemes for three different modes of interaction:

1) Hard virtual guides. This weighting scheme enforces that the user is constrained to the guides, and cannot escape them. This is useful in scenarios where no novel tasks arise, and the user chooses to exploit only the current guides. 2) Soft virtual guides. With this weighting scheme, the user can 'escape' the virtual guides, for example to train novel guides. When the end-effector is not close to any of the guides, none of the guides is active. The robot then operates in zero-gravity mode. 3) Zero virtual guides. This turns off all of the virtual guides, and is equivalent to operating the robot in zerogravity mode (a.k.a. gravity compensation mode). This mode may also be used to train novel guides. Table I summarizes the most relevant differences between the three interaction modes, from the user's perspective.

p(n; x r , s n vm ) = g(x r ; µ n vm , Σ n vm ) N i=1 g(x r ; µ i vm , Σ i vm ) = g(x r ; s n vm ) N i=1 g(x r ; s i vm ) , (13)
where the means and covariance matrices of the cart position are determined from the cart phase s vm with ( 9) and (10) respectively.

Each of the N virtual mechanisms applies a force F n vm to the end-effector. The relative influence of each VM is scaled with the probability p(n; x r , s n vm ), so that the resultant force on the end-effector is:

F res = N n=1 p(n; x r , s n vm ) • F n vm . (14)
Fig. 4. Scaling the forces of multiple virtual guides with probabilistic virtual mechanisms, using the first interaction mode (hard virtual guides).

Each graph represents the value of one equation, depicted above/below the graphs. These illustrations are based on synthetic data. Since they represent a static snapshot, damping terms are omitted for simplicity.

The relevant probabilities and forces in this control mode are visualized in Fig. 4. The stability of hard virtual guides is analyzed in [START_REF] Raiola | Parallel guiding virtual fixtures: Control and stability[END_REF].

2) Soft virtual guides: The underlying assumption in using the hard virtual guides is that x r must belong to one of the VMs. Another approach is to assume that if x r is too far from the VMs, it does not belong to any of the VMs. To do so, we use a Gaussian function h(x r ; µ vm , Σ vm ) with mode 1, i.e. a probability density function as in [START_REF] Yoon | Modeling user's drivingcharacteristics in a steering task to customize a virtual fixture based on task-performance[END_REF], but without the normalization factor (2π) k |Σ vm |. By using these weights (to determine if an individual virtual guide is active in the first place), as well as the probability p(n; x r , s n vm ) (to determine the relative weighting between all the guides), the resultant force becomes

F res = N n=1 h(x r ; s n vm ) • p(n; x r , s n vm ) • F n vm . (15)
The main difference between these methods arises when x r is not close to any x n vm , as illustrated in Fig. 5. With the hard virtual guides, x r is always pulled towards the closest x n vm ; the further you are, the stronger the force (standard PD control). With the soft virtual guides, x r does not belong to any x n vm , and F k vm = 0 for all n. The resultant force is therefore also zero.

3) Zero virtual guides: Finally, a second training mode in which only gravity-compensation is active is provided. Within our probabilistic framework, this training mode is interpreted as all virtual guides (if any) having a probability 0 of being responsible, i.e. ∀n, p(n; s n vm ) = 0 V. IMPLEMENTATION To enable the use of probabilistic virtual guides, the functions µ vm = f (s vm ) (9) and Σ vm = v(s vm ) (10) must be implemented. In this section, we describe one possible implementation, based on kinesthetic teaching of demonstration trajectories, trajectory clustering, and the modelling of trajectory clusters with Gaussian mixture models [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF]. Fig. 5. Scaling the forces of multiple virtual guides with probabilistic virtual mechanisms, using the second interaction mode (soft virtual guides).

A. Training the Gaussian Mixture Model

Each task is performed multiple times by the user through kinesthetic teaching, i.e. the user holds the end-effector of the robot, and demonstrated by guiding it along the desired trajectory. The demonstrations are gathered without a specific order, in this way the number of tasks is not known a priori. Then dynamic time warping [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF] is used to compute distances between trajectories that are independent of time. These distances are used to perform hierarchical clustering of the trajectories. An example of clustering on data from the placement tasks (cf. Fig. 1) is shown in Fig. 6. Fig. 6.

Left: Clusters representing two different movements for placing objects in shelves. Right: Gaussian mixture models for the clustered data. Training trajectories are light gray, the mean of the GMM is black. For visualization purposes, the GMM is projected on the xz-plane.

Each sample [x(t i ), y(t i ), z(t i )] i=1:N in a trajectory is associated with a phase value s(t i ) = (t i -t 1 )/(t N -t 1 ), i.e. s(t 1 ) = 0 at the beginning of the trajectory, and s(t N ) = 1 at the end. The last step is to fit a Gaussian mixture model (GMM) to all the 4-D data points (3D position and phase) in all the trajectories in each of the clusters. In a GMM, data is represented by a set of E multivariate Gaussians

p(r) = E e=1 π e N (r; µ e , Σ e ), with E e=1 π e = 1. (16)
The Expectation-Maximization algorithm adjusts the priors π e and the parameters µ e and Σ e of the Gaussian functions that define this model [START_REF] Ghahramani | Supervised learning from incomplete data via an EM approach[END_REF]. Fig. 6 depicts the result of fitting a GMM to the clusters, where the number of Gaussians is manually set to 5 per cluster. In future work, we will consider methods to set this parameter automatically [START_REF] Yoon | An efficient model selection for gaussian mixture model in a bayesian framework[END_REF].

B. GMMs as Probabilistic Virtual Mechanisms

Probabilistic virtual mechanisms require implementations of the functions x vm = f (s vm ) (1), ẋvm = J vm (s vm ) (2), and Σ vm = v(s vm ) (10). We implement these functions through Gaussian mixture regression (GMR) [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF], based on the Gaussian mixture model (GMM) trained in the previous section.

In the context of a virtual mechanism, the input space is S, and the output space is X, corresponding to the phase s vm and virtual mechanism position x vm respectively. Given this partition, the mean and covariance matrix1 are decomposed as

µ e = [µ e,S , µ e,X ] and Σ e = Σ e,S Σ e,SX Σ e,XS Σ e,X , (17)
The implementation of x vm = f (s vm ) in (1) corresponds to computing x vm = E(x vm |s vm ), i.e. the expectation of x vm given the input s vm :

x vm = E e=1 β e (s vm )(µ e,X + Σ e,XS Σ -1 e,S (s vm -µ e,S )), (18) with
The function J vm (s vm ) in ( 2) is implemented with the analytical derivative of [START_REF] Nolin | Activation cues and force scaling methods for virtual fixtures[END_REF]. Finally, Σ vm in (2) is implemented by computing the variance in the estimate var(x vm ) with

Σ vm = E e=1 β e (s vm ) 2 Σ e,X -Σ e,XS Σ -1 e,S Σ e,XS . (20)

VI. PILOT STUDY

Our application task, illustrated in Fig. 1, is to place objects on different shelves in a cupboard. During one episode, users take the robot at the wrist, and guide it to one of two positions in the cupboard. At the desired position, the robot releases the object for placement on the shelf (when the phase of the movement > 0.9). Although using heavy objects would be better demonstrate the advantages of using virtual guides, we cannot do so due to the limited payload of our robot.

In this pilot study, we first illustrate two advantages of probabilistic virtual guides over standard guides. We then show a typical use case for multiple probabilistic guides. With four human subjects, we finally compare single and multiple virtual guides with respect to 3 evaluation criteria.

A. Advantage: Switching between multiple guides

In Fig. 7, we illustrate switching between multiple virtual guides by using the first interaction mode (hard virtual guides) to scale the forces, as described in Section IV-B.1. In the left graph, we see that at the beginning of the trajectory, when the end-effector is close to both of the guides, the probabilities of both guides ≈ 0.5. Because the guides are close to each other, it is so easy to switch between them that it is difficult to feel the transition.

In the middle plot, a switch is attempted half-way through the movement, when the probability that is the upper trajectory is 1. But with enough force, the user can still change to the other guide. The sensation is that the robot "gives way", and then locks into the other guide. At this point, we recommend viewing the video supplement. In it, we show a user reproducing the cases explained in Fig. 7 and also those in Fig. 8. Although sequential switching between multiple virtual (sub)guides for one task has been demonstrated [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], [START_REF] Kuang | Assembling virtual fixtures for guidance in training environments[END_REF], the switching between multiple virtual guides (that are active in parallel) for multiple tasks is a novel feature of our approach.

Even further along the movements however (right plot), the distance between the guides is too large. Pushing the end-effector downwards hardly influences the probabilities, and the end-effector is locked to the upper guide. Fig. 7. Illustration of switching between multiple probabilistic virtual guides. Top: Side view of virtual guides and end-effector movement. Bottom: probabilities of the two virtual guides. The dark red line represents the user movement the thin short lines represent the direction and magnitude of the resultant force by the virtual mechanisms.

B. Advantage: Escaping an active guide

In Fig. 8, we illustrate how probabilistic virtual guides enable a user to escape the guide in a transparent way by using the second interaction mode (Soft virtual guides), as described in Section IV-B.2. Here, only the virtual guide for positioning the object on the lower shelf is active. With a standard virtual guide (left graph), the user cannot escape the virtual guide, as the spring will exert high forces to pull the end-effector back to the guide. When using soft virtual guides (as in Fig. 5), forces decrease if the to the guide large. Once the the user has 'escaped' the virtual guide, demonstrations for novel tasks can be given, as illustrated in the video supplement. The advantage of using probabilistic virtual guides is that it leads to smooth transitions (rather than on/off switching as in [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], [START_REF] Yu | Telemanipulation assistance based on motion intention recognition[END_REF]), and does not require the manual design of distance thresholds (as in [START_REF] Nolin | Activation cues and force scaling methods for virtual fixtures[END_REF]).

C. Typical Use Case Illustration

We now illustrate how escaping active guides and switching between multiple guides combine in a typical use case where novel tasks arise during operation. Fig. 9 shows the force scaling of virtual guides during 30 task executions with the same subject. The use case is described in detail in the caption of Fig. 9 to allow easy switching between the graph and its description.

D. Comparing Safety and Efficiency.

We perform a pilot study with four users, and the setup in Fig. 1. We compare three assistance modes: gravity compensation only (no virtual guides), a single virtual guide (for the current task), multiple virtual guides (by using hard guides). With these 3 modes, user executes 10 episodes for each of the 2 tasks (object goal positions), i.e. a total of 60 = 3 × 10 × 2 episodes per user. Each user thus tries all modes and positions, but were presented in a randomized order to the users to avoid training effects.

The measures that we are interested in are: • execution time, to measure efficiency. • accuracy of tracking • actually observed collisions, to measure safety. These results are summarized in Fig. 10 Fig. 10. Comparison of the three assistance modes (gravity compensation only, only one virtual guide, multiple virtual guides), for both guides (for the upper and lower shelf), and three measures (execution time, position error, number of collisions)

From these results, we draw the following conclusions: A) Tracking errors substantially (from 3.0cm to 2.0/2.0 for Task 1 and from 5.2 to 2.5/2.7 for Task 2) and significantly decrease when using virtual guides. B) This enables virtual guides reduce the percentages of trials in which a collision occurs to 0. C) Execution times also decrease (from 2.7s to 1.7/2.3 for Task 1 and from 3.3 to 2.3/2.1 for Task 2). D) Using multiple guides in parallel (instead of activating only the appropriate guide for the task at hand) does not lead to a significant deterioration in performance.

Some of the spontaneous user comments about the virtual guides, although anecdotal, are nevertheless interesting to report. One user reported "I feel more confident in moving the robot"; this confidence is verified by the fact that users are able to execute tasks more quickly, because they are more confident that the robot will not collide with the cupboard. Another comment includes "It feels like there are virtual shelves". Thus the user interprets the robot as avoiding the shelves, rather than following a guide, which corresponds to what the demonstrator considers when demonstrating the trajectories.

VII. CONCLUSION

We propose probabilistic virtual guides for comanipulation. These guides enable smooth deactivation of guides without manual thresholds (experiment VI-B). Furthermore, they enable the recognition of user intentions for multiple tasks (experiment VI-C), and transparent switching between multiple guides that are active in parallel during movement execution (experiment VI-A). Finally, we show that probabilistic guides improve the safety and efficiency of task completion, and that multiple guides do not deteriorate this performance (experiment VI-D). Furthermore, we define different interaction modes for multiple guides, based on using hard, soft, or zero virtual guides. Our implementation of probabilistic virtual guides is based on proven methods for kinesthetic teaching, clustering and probabilistic modelling with Gaussian mixture models [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF].

Adding more and more virtual guides for an abundance of tasks has diminishing returns. For example, with 100 Fig. 9. Use case illustrating how escaping virtual guides enables on-the-fly generation of novel guides. In block A.1 the user executes task 1 (place the object on the lower shelf) using the previously trained guide 1. In block A.2 the user is able to execute task 2 -for which there is not yet a guide -by escaping guide 1. The trajectories from block A.2 are not considered to belong to guide 1 (see the low weights in block A.2), and are stored to train a new guide (guide 2), using the clustering and training methods from Section V. From then onwards, there are two guides. In block B.1 and B.2 (where task 1 and 2 are executed, respectively), we see that the correct guide is recognized during the movement, and used to guide the human. Escaping the guides would still be possible, but not necessary since no novel task has to be solved. Finally, in block C.1 and C.2, it is assumed that no further tasks will arise, and the interaction mode is (manually) switched to provide hard virtual guides, which leads to earlier and even more accurate recognition of the appropriate guide for the task. different object positions on the shelves and 100 guides, would be so many guides active at the same time that the resultant forces would be zero, corresponding to zero-gravity mode. Increasing the overall stiffness of the robot could alleviate this effect and allow more guides to be introduced, but may make switching between guides more difficult. Our future work is aimed at investigating these diminishing returns, and there relation to the task domain and stiffness of the robot.

  : β e (s vm ) = π e g(x; µ e,S , Σ e,S ) E l=1 π l g(x; µ l,S , Σ l,S ) = π e g(x; s e vm ) E l=1 π l g(x; s l vm ).

Fig. 8 .

 8 Fig.8. Illustration of escaping a virtual guide. Top: Side view of virtual guide and end-effector movement. Bottom: Probabilities of the virtual guides.

  If we have N virtual mechanisms, there are N cart positions x n=1:N vm , and N probabilities. The probability that the n th cart is responsible for guiding the end-effector at position x r is

		Type of guide
		Zero Soft Hard
	User can demonstrate new guides?	Yes	Yes	No
	Guides enabled when close to guide?	No	Yes	Yes
	Guides enabled when far from guide?	No	No	Yes
	TABLE I
	MAIN FUNCTIONALITIES FOR THE THREE INTERACTION MODES.

1) Hard virtual guides:

Robotics and Computer Vision, Unité d'Informatique et d'Ingénierie des Systèmes, ENSTA ParisTech, Université Paris-Saclay, 828 bd des Maréchaux, 91762 Palaiseau cedex France

FLOWERS Research Team, INRIA Bordeaux Sud-Ouest,

Talence, France3 CEA-List, Gif-sur-Yvette, France

The covariance matrix Σ e,S is actually a scalar, because the phase is always 1-dimensional. For consistency, we nevertheless use the bold symbol Σ rather than σ

.

Acknowledgements

This project has received funding from DIGITEO (www.digiteo.fr).


Gennaro Raiola

Pedro Rodriguez-Ayerbe

Xavier Lamy

Sami Tliba

Freek Stulp

Parallel Guiding Virtual Fixtures: Control and Stability

Guiding virtual fixtures have been proposed as a method for human-robot co-manipulation. They constrain the motion of the robot to task-relevant trajectories, which enables the human to execute the task more efficiently, accurately and/or ergonomically. When sequences of different tasks must be solved, multiple guiding virtual fixtures are required, and the appropriate guide for the current task must be detected automatically. To this end, we propose a novel control scheme for multiple guiding virtual fixtures that are active in parallel. Furthermore, we determine under which conditions using multiple fixtures is stable. Finally, we perform a pilot study for a real-world application with a humanoid robot.

I. INTRODUCTION

A guiding virtual fixture constrains robot motion along task-relevant trajectories [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF], [START_REF] Lin | Portability and applicability of virtual fixtures across medical and manufacturing tasks[END_REF]. Such fixtures make it easy to move the robot end-effector along a trajectory, but not in other directions away from the fixture. They are especially useful in contexts where human decision making is still required to perform the overall task, but where constraints on the accuracy or required forces of the motion preclude humans from efficiently or effectively performing such tasks without robot assistance. Examples include industrial tasks [START_REF] Li | Virtual reality for fixture design and assembly[END_REF], where heavy parts must be transported, and surgical tasks [START_REF] Park | Virtual fixtures for robotic cardiac surgery[END_REF], [START_REF] Becker | Vision-based control of a handheld surgical micromanipulator with virtual fixtures[END_REF], where accuracy and stability of the end-effector are essential.

Joly [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF] proposed to implement guiding virtual fixtures as virtual mechanisms, where a passive virtual mechanism is connected to the robot tooltip by a spring-damper system. This mechanism can be, for instance, another virtual robot, or a cart moving along a rail, as illustrated in Fig. 1. The robot end-effector and virtual "cart" mechanism are coupled by a spring-damper system.

The overall scheme is reminiscent of Victorian waterway transport, when horses pulled boats along canals with ropes. The horse (the robot end-effector) can pull the boat (the virtual mechanism) along the canal (the guide) with the rope (the spring-damper system). And the boat and rope constrain the horse so that it cannot walk away from the canal.

One issue in using virtual guides is addressing contexts in which multiple tasks are to be solved. For multiple tasks, multiple guides are required. And with multiple guides, it is necessary to have methods for recognizing and selecting the appropriate guide for a particular task. For safety reasons, it Fig. 1. Left: Schematic illustration of modelling a guiding virtual fixture with a virtual mechanism [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF]. Here, the virtual mechanism is implemented as a virtual cart on a rail. It is connected to the robot end-effector with a springdamper system. Main variables and equations of the virtual mechanism are included. Right: Multiple virtual mechanisms -one for each tasksimultaneously connected to the robot end-effector. must be shown that using multiple guides in parallel does not lead to instabilities. The contributions of this paper are:

• to propose a control framework for multiple virtual guides, based on virtual mechanisms. • to prove the stability of multiple virtual mechanisms that are active in parallel1 . To illustrate the practical relevance of these theoretical contributions to real-world applications, we include in this paper a pilot study with a humanoid robot, which has previously been reported in [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF].

The rest of this paper is structured as follows. In the next section, we discuss related work. In Section III, we then present how guiding virtual fixtures are implemented as virtual mechanisms. The control framework and stability proof for multiple guides are presented in Section IV and Section V respectively. A pilot study is presented in Section VI, after which we conclude with Section VII II. RELATED WORK The "virtual fixture" method [START_REF] Rosenberg | Virtual fixtures: perceptual tools for telerobotic manipulation[END_REF], [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF], is a successful approach to joint human-robot tasks. This method has been used especially in the context of teleoperation, where virtual constraints are used to improve the precision of a teleoperated robot. Virtual fixtures can be also used to avoid sending wrong commands to a teleoperated robot, an example is a mobile robot that does not accept teleoperation commands that would make it fall into a pit. Such virtual fixtures are known also as "forbidden regions virtual fixtures" [START_REF] Abbott | Virtual fixture architectures for telemanipulation[END_REF].

Guiding virtual fixtures on the other hand allow the operator to control the motion of the robot along a preferred direction (e.g. along a pre-specified trajectory) whilst constraining it in the non-preferred directions [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF] (e.g. not allowing the robot to move too far away from the pre-specified trajectory). In guiding virtual fixtures, the compliance along the trajectory is high, whereas it is low in directions perpendicular to the trajectory. This direction guidance thus pulls the user towards the trajectory, which is what makes it easier to move along the trajectory. Pezzementi et al. [START_REF] Pezzementi | Dynamic guidance with pseudoadmittance virtual fixtures[END_REF] add speed guidance too, to achieve desired tool velocities during the movement.

Colgate et al. [START_REF] Colgate | Intelligent assist devices in industrial applications: a review[END_REF] provide an early overview of the use of intelligent assistive devices and virtual guides in industrial applications.

Apart from the work of Joly [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF] discussed in the introduction, virtual mechanisms have also been used by Pezzementi et al. [START_REF] Pezzementi | Dynamic guidance with pseudoadmittance virtual fixtures[END_REF], where they are called "proxies". Virtual guides may also be implemented by using anisotropic admittances to attenuate the non-preferred user force component [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF], [START_REF] Bettini | Vision assisted control for manipulation using virtual fixtures[END_REF] These methods require the sensing of external inputs by the user, such as the force or the velocity applied by the user on the robot tooltip. This is not required with our control scheme.

Virtual guides have often been limited to pre-defined geometric shapes [START_REF] Marayong | Spatial motion constraints: theory and demonstrations for robot guidance using virtual fixtures[END_REF] or combinations of shapes [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], [START_REF] Kuang | Assembling virtual fixtures for guidance in training environments[END_REF]. Generating guides from demonstrations has also been explored by Aarno et al. [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], who model demonstrations in a segmented sequence of straight lines.

One novel aspect of our work is to provide a control scheme in which the output of individual virtual guides are weighted within a probabilistic framework. A first advantage of the probabilistic approach is that it enables a guide to be activated/deactivated based on the probability of belonging to it, which leads to smooth transitions. This is preferable to switching the guide on/off as in [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF], [START_REF] Li | Recognition of operator motions for realtime assistance using virtual fixtures[END_REF], [START_REF] Yu | Telemanipulation assistance based on motion intention recognition[END_REF], and does not require the manual design of distance thresholds for activation, as in [START_REF] Nolin | Activation cues and force scaling methods for virtual fixtures[END_REF].

A second advantage is that the probabilistic approach allows us to simultaneously activate and recognize several guides, by assigning probabilities to each guide based on user behavior. Thus, our method enables the use of a library of guides, with one guide for each distinct task. Multiple guides have been previously used, but these (sub)guides are activated sequentially for one unique task, rather than in parallel for several tasks. For instance, Kuang et al. [START_REF] Kuang | Assembling virtual fixtures for guidance in training environments[END_REF] combine different shape primitives to facilitate maze navigation. Aarno et al. [START_REF] Aarno | Adaptive virtual fixtures for machine-assisted teleoperation tasks[END_REF] use a Hidden Markov Model to probabilistically choose a guide in a sequence of linear guides to accomplish a pick and place task.

Another important aspect of our work is to prove the stability of the controller. In [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF] the passivity for a single virtual mechanism is proven by studying its dissipated energy. Intuitively a virtual mechanism is passive because it is realized with passive mechanical components such as springs and dampers. In our case, due the non-linearity of the controller we use the Lyapunov theory for the stability analysis as explained in [START_REF] Siciliano | Robotics: Modelling, Planning and Control, ser. Advanced Textbooks in Control and Signal Processing[END_REF].

III. VIRTUAL MECHANISMS AS VIRTUAL GUIDES

We model the guiding virtual fixture using a virtual mechanism (VM) [START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF]. In the context of this paper, the VM can best be thought as a cart moving along a rail. The position of the cart on the rail in Euclidean space is described by x vm . The distance it has travelled along the rail is function of the phase s vm , with s vm = 0 at the beginning and s vm = 1 at the end of the rail, as illustrated in Fig. 1. The kinematics of the virtual mechanism is described by:

x vm = f (s vm ) (1) ẋvm = J vm (s vm ) ṡvm (2)

A. Force on the virtual mechanism

The virtual mechanism is connected to the robot endeffector with a virtual spring-damper system. The force applied to the virtual mechanism by the robot is:

F r = k(x r -x vm ) + b( ẋr -ẋvm ). (3)
Where k, b > 0 are respectively the stiffness and the damping gains and x r represents the robot position in the workspace. The virtual mechanism is ideal, so the force exerted on the virtual mechanism tip is always orthogonal to its velocity:

J vm F r = 0, (4)
meaning that no power can be transmitted to it nor taken from it, this leads to:

J vm (k(x r -x vm ) + b( ẋr -J vm ṡvm )) = 0. (5)
By solving [START_REF] Becker | Vision-based control of a handheld surgical micromanipulator with virtual fixtures[END_REF] with respect to ṡvm , we obtain a first order dynamical system that expresses the evolution of the virtual cart along the virtual rail:

ṡvm = (J vm bJ vm ) -1 J vm (k(x r -x vm ) + b ẋr ). (6)
Moving the robot end-effector away from the virtual cart (x r = x vm ) will thus make it slide along the rail, with a velocity described by [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF].

To avoid singularities in [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF] we add an adaptive damping term to the dynamical system:

ṡvm = (J vm bJ vm + b vm ) -1 J vm (k(x r -x vm ) + b ẋr ). (7)
Where b vm is computed based on the proximity to the singularity:

b vm = B vmmax exp - 4 γ det(J vm J vm ) , (8)
B vmmax define the maximum damping value obtained when the determinant of J vm J vm is zero (the virtual mechanism is in a singular configuration). γ defines the value of det(J vm J vm ) above of what the damping is practically zero (the virtual mechanism is away from a singular configuration).

B. Force on the robot end-effector

Because the virtual mechanism and the robot end-effector are connected to each other, the virtual mechanism also applies a force on the robot end-effector, i.e.

F vm = -F r = k(x vm -x r ) + b( ẋvm -ẋr ). (9)
This virtual force can be transformed into actual control commands for the robot, for instance with a compliance controller. In our implementation, we used the Jacobian of the robot J r to convert the forces into torque references for the motor controllers.

IV. MULTIPLE WEIGHTED VIRTUAL GUIDES

We now consider a control structure in which N guides are active in parallel, i.e. the robot end-effector is connected to multiple virtual mechanisms, as illustrated in Fig. 1. This scenario applies when there are multiple tasks (e.g. transporting an object to one of multiple possible positions), and the robot does not know initially which task will be executed.

Each of the N virtual mechanisms applies a force F i vm to the end-effector. The relative influence of each VM is scaled with a weight p i , so that the resultant force on the end-effector is:

F res = N i=1 p i • F i vm . (10)
Thus, the final force applied to the end-effector is a weighted sum of the forces from each guide, where the weights are determined by their probability, as shown in the control diagram in Fig. The main question now is under which conditions on p i this system is stable. This is studied in the next section. As we shall see, p i must behave as a probability, that is N i=1 p i = 1 and ∀i, p i ≥ 0.

V. STABILITY ANALYSIS Our aim is now to prove the stability of using multiple guides in parallel. We do so with the Lyapunov direct method. This method has extensively used in robotics applications due the intrinsic nonlinearities of a high-dofs robots [START_REF] Siciliano | Robotics: Modelling, Planning and Control, ser. Advanced Textbooks in Control and Signal Processing[END_REF].

The robot non linear dynamic model in joint space can be written as: M (q r )q r + C(q r , qr ) qr + F qr + g(q r ) = u, [START_REF] Colgate | Intelligent assist devices in industrial applications: a review[END_REF] where u represents the command torques, M the inertia matrix, C the Coriolis matrix, F is the viscous attrition, and g is the gravity term. We consider the Lyapunov function [START_REF] Siciliano | Robotics: Modelling, Planning and Control, ser. Advanced Textbooks in Control and Signal Processing[END_REF] V = 1 2 q r M (q r ) qr + 1 2 k

N i=1 i p i i > 0, ∀ qr , = 0, ( 12
)
where i is the error i = x vmi -x r , and p i is the weight in [START_REF] Pezzementi | Dynamic guidance with pseudoadmittance virtual fixtures[END_REF]. By differentiating [START_REF] Bettini | Vision assisted control for manipulation using virtual fixtures[END_REF] with respect to time we obtain:

V = q r M (q r )q r + 1 2 q r Ṁ (q r ) qr + k N i=1 ˙ i p i i + D, ( 13
)
with D = 1 2 k N i=1 i ṗi i . (14)
By substituting M (q r )q r with the robot model, omitting the dependencies on q r , qr and rearranging the terms we acquire

V = 1 2 q r Ṁ -2C qr -q r F qr + q r [u -g] + k N i=1 ˙ i p i i + D. (15)
Due to the skew-symmetry property of the matrix Ṁ -2C the term q r Ṁ -2C qr is null, and we simplify to

V = -q r F qr + q r [u -g] + k N i=1 ˙ i p i i + D. (16)
A. Virtual mechanisms with fixed positions.

We first study the case where all virtual mechanisms have a fixed position in the robot workspace, i.e. ∀i, ẋvm,i = 0. This allows the simplification ˙ i = -ẋr = -J r qr , which leads to:

V = -q r F qr + q r [u -g] -k N i=1 q r J r p i i + D. (17)
We choose a control input u that compensates the gravity term and introduces a proportional-derivative control: u = g + J r N i=1 p i (k i -b ẋr ), where each term in the controller is weighted with its own probability p i . By substituting the chosen control in [START_REF] Nolin | Activation cues and force scaling methods for virtual fixtures[END_REF] we obtain:

V = -q r (F + b) qr + D (18)
The term -q r (F + b) qr is negative. Thus, the system is stable if D = 1 2 k N i=1 i ṗi i is also negative. We now study which conditions on p i ensure this is the case.

B. Constraints on the weights p i .

First of all, we assume that p i behaves as a probability, that is N i=1 p i = 1, N i=1 ṗi = 0 and ∀i, p i 0. To simplify the study we consider the case in which only two virtual mechanisms are active, and thus ṗ2 = -ṗ1 . By simplifying [START_REF] Siciliano | Robotics: Modelling, Planning and Control, ser. Advanced Textbooks in Control and Signal Processing[END_REF] we get:

V = -q r (F + b) qr + 1 2 k ṗ1 [ 1 1 -2 2 ] . (19)
Because k > 0, the system is asymptotically stable iff ṗ1 [ 1 1 -2 2 ] ≤ 0. This leads to the construction of a probability function that satisfy the following conditions:

ṗ1 > 0 when 1 < 2 , ṗ1 < 0 when 2 < 1 . ( 20
)
and vice-versa ṗ2 .

The conditions above show that the probability function p i has to be dependent on the errors i , in particular, the inequalities provide the intuition that if the robot is closer to one guide the probability of belonging to that guide should increase over time, and consequently its error should decrease. For instance, we can define a probability function that increases over time when the square error is decreasing:

ṗi > 0 ⇐⇒ d( i i) dt = 2 ˙ i i < 0.
We first model this function as a probability over all the guides, given an activation weight over individual guides. The probability p i that the i th cart is responsible for guiding the end-effector at position x becomes:

p i = p(i; x r , s i vm ) = g(x r ; s i vm ) N j=1 g(x r ; s j vm ) . ( 21
)
We implement the activation weight g(x r ; s i vm ) as a Gaussian function:

g(x r ; x vm , Σ vm ) = e - 1 2 (xvm-xr) Σvm -1 (xvm-xr) . ( 22
)
Defined the probability function (21) we can compute the derivatives for the two guides 2 :

ṗ1 = p 1 p 2 ( ˙ 2 Σ vm,2 -1 2 -˙ 1 Σ vm,1 - 1
1 ), ṗ2 = p 1 p 2 ( ˙ 1 Σ vm,1 -1 1 -˙ 2 Σ vm,2 -1 2 ). ( 23
)
By considering the virtual mechanisms to have fixed positions i.e. ˙ i = -ẋr and by changing system reference in order to have x r = 0, the expressions above can be simplified to:

ṗ1 = p 1 p 2 ẋ r (Σ vm,1 -1 x vm,1 -Σ vm,2 -1 x vm,2 ), ṗ2 = p 1 p 2 ẋ r (Σ vm,2 -1 x vm,2 -Σ vm,1 -1 x vm,1 ). ( 24
)
2 To compute these derivatives we consider the covariance matrices to be constant in respect of time, this is practically true if we consider that the covariance changes slowly due the fact that is extracted by human demonstrations, and by considering the virtual mechanisms with fixed position.

We are interested in studying the inequality ṗi > 0, to do so we can simplify (24) in the following:

ṗ1 ∼ ẋ r (x vm,1 -x vm,2 ) > 0, ṗ2 ∼ ẋ r (x vm,2 -x vm,1 ) > 0.
(25)

Note that this simplification is possible because p 1 , p 2 > 0 and the covariance matrices are definite positive. By construction these scalar products are positive if the robot moves toward one of the point x vm,i , so for example ṗ1 > 0 if the robot moves toward the point x vm,1 . The inequality (25) combined with (20) shows that the robot is attracted by the closest guide, that the probabilities of that guide thus increase, and the system is thus stable.

C. Virtual mechanisms with moving positions.

We have shown under which conditions on p i the system is stable for guides that do not move. We now consider the more general case where they do move, i.e. ẋvm,i = 0, i = 1..N .

In this situation we can add to the control input the tracking of the velocities generated by the virtual mechanisms:

u = g + J r N i=1 p i (k i + b ˙ i ).
By substituting in ( 16) we obtain:

V = -q r F qr + E + D. (26) with E
= N i=1 ( q r J r p i k i + q r J r p i b ˙ i + ˙ i p i k i ) (27)
The first term in (26) is always negative. We now focus on E. By substituting the error derivate ˙ i = J vm,i ṡvm,i -J r qr :

E = N i=1 (-q r J r p i bJ r qr + ṡ vm,i J vm,i p i k i + q r J r p i bJ vm,i ṡvm,i ). (28) = N i=1 (-q r J r p i bJ r qr E.1 + p i ( i k + q r J r b)J vm,i ṡvm,i E.2 ). (29)
The latter simplification can be made because ṡ vm,i J vm,i p i k i is a scalar, we can transpose it.

To study (E.2) in (29) we can refer to the virtual mechanism equation in [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF]. By substituting [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF] in (E.2) we have:

p i (k i + bJ r qr ) J vm,i (J vm,i bJ vm,i ) -1 J vm,i (-k i + bJ r qr ). (30)
By defining A i = J vm,i (J vm,i bJ vm,i ) -1 J vm,i , (E.2) becomes:

-p i k 2 i A i i + p i q r J r b 2 A i J r qr . (31)
We can use (31) in the expression (29), therefore:

E = N i=1 (-q r J r p i bJ r qr -p i k 2 i A i i + p q r J r b 2 i A i J r qr ), = N i=1 (-p i k 2 i A i i -p i b q r J r (I -bA i )J r qr ). (32)
To have E definite negative we have to proof that the matrix (I -bA i ) is semi-definite positive. Since J vm,i is a column vector, the following inequality holds:

(I -J vm,i (J vm,i J vm,i ) -1 J vm,i ) (I -Iλ max (J vm,i (J T vm,i J vm,i ) -1 J T vm,i )) = 0, (33)
as the maximal eigenvalue of matrix J vm,i (J vm,i J vm,i ) -1 J vm,i is 1 by construction i.e.

λ max (J vm,i (J vm,i J vm,i ) -1 J vm,i ) = 1. ( 34
)
Due the presence of numeric issues, the scalar inverse (J vm,i J vm,i ) -1 , is calculated as (J vm,i J vm,i +b vm ) -1 to avoid infinite value for certain configurations. In this case the inequality holds as:

λ max (J vm,i (J vm,i J vm,i + b vm )) -1 J vm,i ) < λ max (J vm,i (J vm,i J vm,i ) -1 J vm,i ). (35)
Finally we proved that E is semi-definite negative.

To proof that the Lyapunov function ( 26) is definite negative, we have to study D. Before we studied this term when the virtual mechanisms don't move, now we consider ẋvm,i = 0, therefore we can write the derivative of the probability (23) for the first virtual mechanism as:

ṗ1 = p 1 p 2 ẋ r Σ vm,1 -1 x vm,1 -Σ vm,2 -1 x vm,2 + ẋ vm,2 Σ vm,2 -1 x vm,2 -ẋ vm,1 Σ vm,1 -1 x vm,1 . (36)
Considering (4) we have that the velocities ẋvm,i are orthogonal to the forces applied on the virtual mechanisms, so, by plugging (3) into (4) and choosing k >> b we obtain the following simplification

J vm [k(-x vm ) + b( ẋr -ẋvm )] ≈ J vm x vm ≈ 0, (37)
meaning that the velocities of the virtual mechanism are orthogonal to its position vector. By substituting the virtual mechanism equation ( 6) in (36) and considering the simplification done before (37), we get

ṗ1 = p 1 p 2 ẋ r Σ vm,1 -1 x vm,1 -Σ vm,2 -1 x vm,2 + kx vm,1 A 1 Σ vm,1 -1 x vm,1 -kx vm,2 A 2 Σ vm,2 -1 x vm,2 . (38)
The terms with A i in the equation above can be proven to be null using (37):

k(J vm,i bJ vm,i ) -1 x vmi J vm,i ≈0 J vm,i Σ vm,i -1 x vmi ≈ 0, (39)
so equation ( 38) can be written as done before for the case with fixed virtual mechanisms (24), concluding with the same results. Finally the system with the chosen control converges asymptotically to the equilibrium point. The domain of attraction of the equilibrium point for each guide can be computed using (20).

D. Summary

We investigated the stability of the control system in Fig. 2. Using the direct Lyapunov method, we derived [START_REF] Siciliano | Robotics: Modelling, Planning and Control, ser. Advanced Textbooks in Control and Signal Processing[END_REF]. By assuming that p i behaves as a probability, we could derive the conditions (20). We showed that these conditions are met, and the system is thus stable, when a Gaussian function is used to relate probabilities p i to errors i . We then generalized to the case when the virtual mechanism is moving. We perform a pilot study with four users, and the setup in Fig. 3, where the task is to place objects on different shelves in a cupboard. During one episode, users take the robot at the wrist, and guide it to one of two positions in the cupboard. At the final position, the robot automatically releases the object for placement on the shelf. Details of the experimental set-up are in [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF].

VI. PILOT STUDY

A. Implementation of the Guides

Implementing the weighting of multiple guides that are active in parallel with [START_REF] Pezzementi | Dynamic guidance with pseudoadmittance virtual fixtures[END_REF] depends on knowing the probabilities in (21), which itself depends on knowing the activation weight g(x r ; s n vm ) related to the Mahalanobis distance of x r to the guide n with current phase s n vm . This function is acquired through Gaussian mixture regression (GMR) [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF], based on a trained Gaussian mixture model (GMM).

The training procedure is taken from [START_REF] Calinon | On learning, representing and generalizing a task in a humanoid robot[END_REF]. First, each task is performed multiple times by the user through kinesthetic teaching, i.e. the user holds the end-effector of the robot and guides it in zero-gravity mode. The end-effector trajectories are then clustered with hierarchical clustering, based on the distances acquired through dynamic time warping. The Gaussian Mixture Model (GMM) is then trained on the data in the clusters through Expectation-Maximization; hence the GMM contains the relevant informations about the virtual guides such as position and covariance matrices. On-line when the guide is active, GMR is used to compute x n vm , ẋn vm and Σ n vm , by conditioning on the phase s n vm for each guide [START_REF] Raiola | Co-manipulation with multiple probabilistic virtual guides[END_REF]. This allows us to compute the kinematics of the virtual mechanism (1), ( 2) and the activation weights g(x r ; µ n vm , Σ n vm ) as a proxy for g(x r ; s n vm ). These weights are then used to compute the probabilities p(n; x r , s n vm ) in ( 21), which are used to scale the controls in [START_REF] Pezzementi | Dynamic guidance with pseudoadmittance virtual fixtures[END_REF].

B. Comparing Safety and Efficiency.

In the pilot study, each of four users executes in a randomized order 10 episodes for each of the 2 tasks (object goal positions), with the controller gains setted experimentally at the values k = 700 and b = 50. The measures that we are interested in are:

• execution time, to measure efficiency.

• accuracy of tracking.

• actually observed collisions, to measure safety. These results of these measurements over all users are summarized in Fig. 4 Fig. 4. Comparison of the three assistance modes (gravity compensation only, only one virtual guide, multiple virtual guides), for both guides (for the upper and lower shelf), and three measures (execution time, position error, number of collisions)

From these results, we draw the following conclusions:

• When using virtual guides, the tracking errors decrease substantially (from 3.0cm to 2.0/2.0 for Task 1 and from 5.2 to 2.5/2.7 for Task 2). • This enables virtual guides reduce the percentages of trials in which a collision occurs to 0. • Execution times also decrease from 2.7s to 1.7/2.3 for Task 1 and from 3.3 to 2.3/2.1 for Task 2. • Using multiple guides in parallel (instead of activating only the appropriate guide for the task at hand) does not lead to a significant deterioration in performance.

VII. CONCLUSION

In applications of co-manipulation where users must be able to sequentially switch between multiple tasks, it is necessary to have multiple guides i.e. one for each task. We propose a control framework where multiple virtual guides are active in parallel, and the appropriate guide is recognized on-line during the execution of the movement. By weighting the controls of the individual guides with their probabilities, we are able to show that stability of the overall control system is ensured. Our pilot study demonstrates the practical use and advantages of multiple virtual guides. In our future work, we will investigate the exact expression of the equilibrium points and their basin of attraction.

2

 2

Fig. 2 .

 2 Fig. 2. Control structure for multiple virtual fixtures.

Fig. 3 .

 3 Fig. 3. Pilot study task.

The passivity of single virtual mechanisms was shown in[START_REF] Joly | Imposing motion constraints to a force reflecting telerobot through real-time simulation of a virtual mechanism[END_REF].

Acknowledgements

This project has received funding from DIGITEO (www.digiteo.fr).


Freek Stulp

Jonathan Grizou

Baptiste Busch

Manuel Lopes

Facilitating Intention Prediction for Humans by Optimizing Robot Motions

Members of a team are able to coordinate their actions by anticipating the intentions of others. Achieving such implicit coordination between humans and robots requires humans to be able to quickly and robustly predict the robot's intentions, i.e. the robot should demonstrate a behavior that is legible. Whereas previous work has sought to explicitly optimize the legibility of behavior, we investigate legibility as a property that arises automatically from general requirements on the efficiency and robustness of joint human-robot task completion. We do so by optimizing fast and successful completion of joint human-robot tasks through policy improvement with stochastic optimization. Two experiments with human subjects show that robots are able to adapt their behavior so that humans become better at predicting the robot's intentions early on, which leads to faster and more robust overall task completion.

I. INTRODUCTION

We humans are very good at anticipating the intentions of others from their actions, and at adapting our own actions accordingly. Many studies have shown that the capability of anticipatory action improves team work and is a natural expectation of humans [START_REF] Erlhagen | the development intention understanding for joint action tasks[END_REF]. If you move an open bottle in my direction, I will place my glass in an appropriate position for you to pour. If you reach for the screwdriver, I will lift the shelf to hold it in place for you. By monitoring the actions of others and inferring their intentions, a human can predict and preemptively initiate the appropriate complementary actions without the need for verbal communication [START_REF] Sebanz | Joint action: bodies and minds moving together[END_REF], [START_REF] Sartori | Cues to intention: the role of movement information[END_REF], [START_REF] Becchio | Grasping intentions: from thought experiments to empirical evidence[END_REF]. Furthermore, it has been shown that humans unconsciously change their behavior, for instance the speed of task execution, to improve coordination [START_REF] Vesper | Making oneself predictable: reduced temporal variability facilitates joint action coordination[END_REF].

Achieving such implicit, non-verbal coordination between robots and humans will greatly improve the efficiency of human-robot collaboration. The seamless integration of the complementary skills of humans (dexterity, task knowledge, flexibility) and robots (speed, strength, precision, repeatability) will have a huge impact on applications ranging from industrial manufacturing to assistance and medical robots [START_REF] Bauer | Human-robot collaboration: a survey[END_REF]. Enabling robots to automatically recognize human intentions is an important aspect of this vision, and an active field of research [START_REF] Aggarwal | Human activity analysis: A review[END_REF]. But, on the other hand, how can robots make their own behavior easier to interpret for humans? How can robots improve the legibility of their behavior? Illustration of the button pressing experiment, where the robot reaches for and presses a button. The human subject predicts which button the robot will push, and is instructed to quickly press a button of the same color when sufficiently confident about this prediction. By rewarding the robot for fast and successful joint completion of the task -which indirectly rewards how quickly the human recognizes the robot's intention and thus how quickly the human can start the complementary action -the robot learns to perform more legible motion. The three example trajectories above illustrate the concept of legible behavior: it enables correct prediction of the intention early on in the trajectory.

One option is to tailor robot controllers to be legible in specific task contexts, for instance to facilitate handing over an object [START_REF] Cakmak | Using spatial and temporal contrast for fluent robot-human hand-overs[END_REF], [START_REF] Lee | Predictability or adaptivity?: designing robot handoffs modeled from trained dogs and people[END_REF], [START_REF] Strabala | Learning the communication of intent prior to physical collaboration[END_REF], [START_REF] Strabala | Towards seamless human-robot handovers[END_REF]. This involves understanding and modelling how humans interpret actions, and implementing controllers based on this knowledge. Explicit task-specific encoding of intention prediction has also been used to coordinate robot soccer players [START_REF] Stulp | Implicit coordination in robotic teams using learned prediction models[END_REF], [START_REF] Pagello | Cooperative behaviors in multi-robot systems through implicit communication[END_REF].

Dragan et al. [START_REF] Dragan | Legibility and predictability of robot motion[END_REF] take a different approach, by providing a general-purpose definition of legibility: how probable is a goal, given a partially observed trajectory? Higher legibility implies earlier divergence of probabilities for different goals. They also clearly formalize the difference to predictability: what is the most probable trajectory, given knowledge of the goal? Although legibility and predictability are general measures, they are based on cost functions which are tailored to specific task contexts. It is a non-trivial task to adapt this cost function to novel task contexts, and especially to different (classes of) users. Robots are able to generate legible behavior by optimizing the legibility measure off-line through functional gradient optimization [START_REF] Dragan | Generating legible motion[END_REF].

Rather than defining legibility as an explicit property to be optimized, we investigate legibility as an emergent adaptive property of robots who are rewarded for efficiently cooperating with humans. Our approach is based on modelfree reinforcement learning, where the robot iteratively improves its legibility through trial-and-error interaction with a human. This approach has the advantage that no assumptions about the task or the human must be made, and the robot automatically adapts its legibility to the user preferences during the interaction. We evaluate our approach both in simulation and in two user studies with a humanoid robot, one of which is depicted in Fig. 1.

An important component of legibility is that human motions are highly stereotypical. For instance, Glasauer et al. [START_REF] Glasauer | Interacting in time and space: Investigating human-human and human-robot joint action[END_REF] showed that the trapezoidal joint velocity profiles used in industrial robotics are more difficult to predict than the minimum-jerk profiles that are typical for humans. Our work, as that of Dragan et al. [START_REF] Dragan | Legibility and predictability of robot motion[END_REF], [START_REF] Dragan | Generating legible motion[END_REF] rather focuses on the question: "How can I deviate from the stereotypical behavior such that it contains information that enables an observer to quickly infer my intentions?"

The rest of this paper is structured as follows. In the next section, we describe the generic reinforcement learning algorithm used to optimize behavior. In Section III and IV we then present our simulation experiment and user study, including the relevant cost functions and results. We conclude with Section V

II. MODEL-FREE REINFORCEMENT LEARNING

The key idea in this paper is that legibility of robot behavior need not be defined and optimized explicitly, but that it arises automatically if joint task execution is penalized for not being efficient. In particular, this behavior should arise from direct interaction with users, so as to tailor the legibility to specific user preferences.

As we need to optimize a cost function, but do not have models of individual users, we formulate the problem as a model-free reinforcement learning problem. We describe the generic reinforcement learning algorithm and policy representation used in this paper.

A. Policy Improvement through Black-Box optimization

Policy improvement is a form of model-free reinforcement learning, where the parameters θ of a parameterized policy π θ are optimized through trial-and-error interaction with the environment. The optimization algorithm we use is PI BB , short for "Policy Improvement through Black-Box optimization" [START_REF] Stulp | Simultaneous on-line discovery and improvement of robotic skill options[END_REF]. It optimizes the parameters θ with a twostep iterative procedure. The first step is to locally explore the policy parameter space by sampling K parameter vectors θ k from the Gaussian distribution N (θ, Σ), to execute the policy with each θ k , and to determine the cost J k of each execution. This exploration step is visualized in Fig. 2, where N (θ, Σ) is represented as the large (blue) circle, and the samples J k=1...10 are small (blue) dots.

The second step is to update the policy parameters θ. Here, the costs J k are converted into weights P k with Fig. 2. Illustration of the PI BB algorithm on a simple cost function J(θ) = θ (without policies). Left: iterative updating of the parameters, where the exploratory samples for the first iteration are shown. Right: mapping the costs J k to weights P k for the first iteration. The algorithmic parameters are K = 10, λ = 0.7.

P k = exp -h(J k -min(J))
max(J)-min(J) , where low-cost samples thus have higher weights. For the samples in Fig. 2, this mapping is visualized to the right. The weights are also represented in the left figure as filled (green) circles, where a larger circle implies a higher weights. The parameters θ are then updated with reward-weighted averaging θ ←

K k=1 P k θ k .
Exploration is decreased after each iteration Σ ← λΣ with a decay factor 0 < λ ≤ 1. The updated policy and exploration parameters (red circle in Fig. 2) are then used for the next exploration/update step in the iteration. Despite its simplicity, PI BB is able to learn robot skills efficiently and robustly [START_REF] Stulp | Simultaneous on-line discovery and improvement of robotic skill options[END_REF]. Alternatively, algorithms such as PI 2 , PoWER, NES, PGPE, or CMA-ES could be used, see [START_REF] Stulp | Policy improvement methods: Between blackbox optimization and episodic reinforcement learning[END_REF], [START_REF] Kober | Policy search for motor primitives in robotics[END_REF] for an overview and comparisons.

B. Policy Representation

The policy π θ itself is implemented as a dynamical movement primitive (DMP) [START_REF] Ijspeert | Dynamical Movement Primitives: Learning attractor models for motor behaviors[END_REF]. DMPs combine a closed loop controller (a spring-damper system with rest point g) with an open loop controller (a function approximator) to generate goal-directed movements. DMPs are convenient for our experiments, as they ensure convergence towards a goal g, whilst allowing the trajectory towards this goal to be adapted by changing the parameters θ of the function approximator used inside the DMP. The function approximator we use is a radial basis function network with 3 basis functions. The policy parameters θ thus correspond to the weights of the basis functions. The output of the DMP is the angular position of the joints of the robot. Our approach does not hinge on the use of DMPs as a policy representation, and we refer to [START_REF] Ijspeert | Dynamical Movement Primitives: Learning attractor models for motor behaviors[END_REF] for details.

III. SIMULATED EXPERIMENTS

Before turning to the user studies, we first verify that trajectory legibility can emerge by optimizing time efficiency in joint tasks in simple simulated environments. We do so with two experiments: 1) We simulate an 'intention recognizer'based on the legibility measure defined by Dragan et al. [START_REF] Dragan | Legibility and predictability of robot motion[END_REF] -and give higher rewards (lower penalties) to the robot if the intention recognizer recognizes the intention earlier.

2) We use another intention recognizer, but it does not use the legibility measure.

We are not interested in quantitative differences between the solutions the methods generate, but rather the fact that they are able to generate the same qualitative results in the first place. These experiments are a proof-of-concept that legible behavior may emerge even if the legibility measure is not explicitly encoded in the cost function. Thus, our approach can also -as we show in the user studies -be used by robots to (implicitly) optimize their legibility for humans, who are 'intention recognizers' whose cost function is not objectively accessible.

Experimental Design. The design is the same for all experiments. The start position of the movement is always the same, and there are two targets, cf. Fig. 3. We run two optimizations, in which one target is the goal and the other the distractor, and vice versa. The 2D trajectories are generated by a 2D DMP with 6 basis functions, and the initial weights are all set to zero, leading to a straight line towards the goal. The algorithmic parameters of PI BB are Σ = 5I K = 10 and λ = 0.95.

A. Experiment 1: Optimizing Recognition Time (Based on Legibility)

In the work of Dragan et al. [START_REF] Dragan | Generating legible motion[END_REF], a functional gradient algorithm is used to directly optimize the legibility measure along the trajectory. We have reproduced one of their example results (Fig. 1 from [START_REF] Dragan | Generating legible motion[END_REF]) in Fig. 3. In Experiment 1, we provide a less informative measure: the time at which this probability exceeds a certain threshold. Why use a less informative measure? Because we can objectively measure it in human subjects, whereas explicitly modeling legibility measures for humans is difficult, and needs to be adapted to each individual.

Cost Function. We compute a predictor using the Eq. 4 from [START_REF] Dragan | Generating legible motion[END_REF] which estimates the probability of a goal given part of a trajectory. To evaluate the decision time, we compute the evolution of the probability of each goal through time.

p(G h |ξ 0...t ) ∝ p(ξ 0...t |G h )p(G h ) (1)
with p(ξ 0...t |G h ) from Eq. 8 of [START_REF] Dragan | Legibility and predictability of robot motion[END_REF] and ξ 0...t being the trajectory up to time t.

We then normalize the probabilities to 1 across all goal hypotheses. A goal is predicted as being the targeted one when its probability exceeds a threshold β. The response time of the human is the first time t that p t (G h ) exceeds β.

The cost function is defined as the response time of the user. The earlier the reply, the better the legibility. If an incorrect goal is predicted, the cost is set to a high value, here 20. We also add a term that penalizes high jerk trajectory.

Results. The resulting trajectories (Fig. 3) confirm that optimizing a proxy cost function -the time for an 'intention recognizer' to estimate the intended goal -can lead to similar trajectories as when optimizing directly for legibility. 1 in [START_REF] Dragan | Generating legible motion[END_REF]. The initial trajectories are the straight dashed lines between the start and the two goals, the thick black lines is the result after optimization. Intermediate policies during optimization are plotted in gray.

B. Experiment 2: Optimizing Recognition Time (Not Based on Legibility)

Although in Experiment 1 the cost only considered the time at which the goal intention was recognized confidently, it still used the measure of legibility to perform this recognition. In this experiment, we use an ad-hoc cost function that does not use the legibility measure from [START_REF] Dragan | Generating legible motion[END_REF]. Our aim is to show that if ad-hoc cost functions (which mimic the prediction time of a human) lead to similar trajectories than previous works considering an explicit definition of legibility (e.g. [START_REF] Dragan | Generating legible motion[END_REF]), then our approach is applicable to human subjects.

Cost function. As in Experiment 1, this cost function observes the trajectory time step by time step. A (partial) trajectory is modeled as a Gaussian process between the x and y coordinate [START_REF] Williams | Gaussian processes for machine learning[END_REF]. The Gaussian process uses a Gaussian kernel with maximum variance σ 2 =0.25 and length l=1.0. Using Gaussian process regression, we estimate the mean µ and variance σ of x for the y-coordinates of the goal and the distractor. Given these means and variances, we compute the conditional probability that the trajectory will pass through the goal. If this probability > 0.9, the intention is correctly recognized. The time at which this recognition occurs is the cost. The main aim of this implementation is to demonstrate that an 'ad hoc' intention recognizer, not based on the formal concept of legibility, enables legible behavior to be generated. Although this recognizer models and approximates human intent recognition, the optimization algorithm does not have access to this model.

Results. The same general trend of exaggerated movements away from the distractor are visible. Thus, legible behavior can also be generated with cost functions involving only time, without any underlying concept of legibility.

C. Discussion

These results demonstrate that A) legible behavior may arise if only recognition time is used as a cost function. B) model-free stochastic optimization is able to optimize this cost function, without requiring a model of the 'intention recognizer'. This makes our approach applicable to human subjects, as we demonstrate in the next section.

IV. ROBOT EXPERIMENTS WITH HUMAN SUBJECTS

We perform two experiments with human subjects. In Experiment A, the task consists in pressing two corresponding buttons. First the robot decides on a button to press and then the subject needs to press the corresponding button as soon as possible; subjects are to press a button of the same color. In Experiment B, the task consists in selecting a button based on the location a robot will deposit a bottle. First the robot decides on a target location to put a bottle after grasping it, the subject needs to press the button corresponding to the location of the target location before the robot grasps the bottle.

In both experiments the time taken by the user to press the button is used as the main cost of the collaborative task. In practice if the motion of the robot is more informative then the subject will be able to predict sooner the target of the robot.

Both experiments follow the same protocol. The subject is instructed to press the corresponding button as soon as he understand the intention of the robot. All experiments start with an habituation phase of 32 trials where the robot performs always the same trajectory for the same task. This phase allows the subject to get used to the robotic motions, and practice the prediction and button pressing. Preliminary results indicate that 32 trials are sufficient for habituation. The recognition time at the end of this phase is a baseline on which we will evaluate the improvement that the robotic optimization gave for the shared task.

After habituation, we start the optimization phase of 96 trials with the PI BB algorithm as presented in Sec. II, during which the robot executes variations of the movement by exploring policy parameter space. The two DMPs are optimized in two separate optimization processes. The parameters of PI BB are K = 8 (trials per update), Σ = 5I (initial exploration magnitude) and λ = 0.9 (exploration decay).

We recruited 20 volunteers among INRIA staff, PhD students in computer science, and under-grad students of cognitive science. Among them 13 participated to Experiment A (9 starting from initial trajectories and 4 starting from previously optimized trajectories) and 7 to Experiment B.

A. Experiment A: Joint button pressing

In this task, the robot reaches for and presses one of two buttons. The subject is instructed to press a button of the same color as early as possible, whilst avoiding mistakes, i.e. pressing another button than the robot intended to. We used the set-up presented in Fig. 1, where the subject sits in front of the robot.

The two initial policies, i.e. one for each button, have been recorded through programming by demonstration, as described in [START_REF] Ijspeert | Dynamical Movement Primitives: Learning attractor models for motor behaviors[END_REF]. The starting position is the same and the path to each button is straight. Thus trajectories are hard to differentiate in the first part of the movement.

Cost function. The cost function consists of three components: Efficiency: The time taken by the human and the robot to press their respective buttons from the onset of the robot's movements, T robot + T subject . Robustness: Whether the subject pressed the same button as the robot (δ buttons =0) or not (δ buttons =1). γ is an arbitrary high cost, it was set to 20 in this experiment. Energy: The sum over the jerk (third derivative of the joint position, α ... q ti ) at each time step i in the trajectory. α is a scaling factor chosen such that the cost of the jerk is about 1/20 of the total cost in the initial trajectories.

J = T robot + T subject efficiency + γδ buttons robustness + α| ... q 1...N,1...T | energy (2)
Note that this cost function does not contain a direct measure of legibility. Our hypothesis is that legibility can arise by penalizing efficiency and robustness only, which are only indirect measures of legibility.

Results. For illustration purposes, the top graph of Fig. 4 shows a learning session for one of the subjects. The xaxis corresponds to the number of trials, and the y-axis to the time between the onset of the robot's movements and the pushing of the button by the human, which we denote "prediction time". Individual trials are depicted as circles or crosses corresponding to successful predictions of the robot's intentions or "prediction errors" respectively. For this subject, we see that during the initial habituation phase, prediction times drop from ≈ 3s to ≈ 2.5s. After the optimization phase the prediction time is reduced further. This reduction is not due to the subject quickly pushing random buttons, because the error rate does not increase (crosses indicate prediction errors, i.e. pressing the wrong button).

The center graph in Fig. 4 shows the prediction times averaged over all 9 subjects. To allow comparison between subjects without introducing variance due to the natural overall differences in their prediction time, we normalized their results by their intrinsic prediction times, computed as the average of their last 8 prediction times in the habituation phase. Finally, the bottom graph in Fig. 4 shows the number of prediction errors per block of 8, averaged over all subjects. During optimization the execution time drops by 20%, without leading to significantly more prediction errors, i.e. execution time does not decrease because humans are simply guessing.

However, the number of errors slightly increases at the beginning of the optimization phase. This is probably due to the sudden variability in the robot trajectories. Indeed, after 32 trials, the robot starts exploring the parameter space. As a result, two successive trajectories might look different while aiming at the same target and the error rate is thus likely to increase. Soon enough, the co-adaptation between the human and the robot takes place and the error rate decreases.

The box-plots at the top of Fig. 5 highlight the statistics of the normalized prediction times at important transitions during learning: the start (trial 1 to 8), the last trials of the habituation phase (25-32), the first trials of the optimization phase (33-40), and the final trials (121-128). We observe a substantial (20%) and significant (p = 5.3e -8 , Wilcoxon signed-rank test) drop in prediction time between the end of the habituation phase (25-32) and the end of the optimization (121-128). The small decrease during the habituation phase is also significant (p = 0.001), indicating that initial trajectories are different. Yet, the prediction time is further improved by 20% after the optimization showing that initial trajectories are not differentiable as early as optimized ones.

Do subjects learn quicker when starting with policies that have been optimized previously with another subject? To analyze this, we ran the same experiment as above with another 4 more subjects, but started the habituation phase with policies resulting from optimization with other subjects. The results are plotted in the lower graph in Fig. 5. Please note that prediction times are again normalized with respect to trials 25-32, i.e. the second box plot in each row. In contrast to the previous experiment, the decrease in prediction time is still significant during habituation (p = 0.0003), but not during optimization (p = 0.427). This suggesting the trajectories optimized with another subject are sufficiently separable to be differentiate by a new human in only a few iterations, and cannot be optimized further.

The (unnormalized) prediction times at the end of the habituation phase is significantly lower (p < 0.001, Mann-Whitney U-test) when starting with pre-optimized trajectories than using the initial straight approach. Interestingly, at the end of the optimization phase the prediction times are not significantly different (p = 0.93). Finally, we must ensure that the global task is executed more efficiently, and that the robot is not reducing human prediction time by, for instance, dramatically increasing energy consumption. Fig. 6 shows the evolution of each component of the cost function for Exp. A and for both initial conditions. It confirms that the global cost is effectively decreased and that the human prediction time has the most impact during the optimization process. Interestingly, the robot time also decreases slightly. The jerk component slightly increases over time, indicating that trajectories with more pronounced features are generated. Discussion. The algorithm presented improves humanrobot collaboration by producing motions that subjects find easier to predict. By penalizing errors and the joint execution time, the robot learns policies that enable the human to distinguish the robot's intentions earlier without more errors. New subjects are able to quickly read intentions from robot behavior that has been optimized for another subject. Optimized behavior is thus transferable to other subjects.

B. Experiment B: Pick-and-place

In the second task, the robot reaches for and grasps a single object. After grasping the object, the robot has to place it inside one of two boxes located on each side (see Fig. 7). The 7 subjects are instructed to press the button corresponding to the aimed box as early as possible while avoiding mistakes. To improve the task, the subject must predict which side the object will be moved to and press the corresponding button. This experiment differs from the joint button task because the motions to approach the object are initially identical for both outcomes, guessing before that point results in 50% chances of success. As the robot aims at eliciting an early response from the subject, differentiating the approach motion is necessary to improve joint coordination. Thus, our hypothesis is that at the end of the optimization phase, the subject should be able to predict the robot's intention before it even grasps the object.

Cost function. We use the same cost function as in the previous experiment, with γ again set to 20, and α chosen as in previous experiment.

Results. As in the previous experiment, Fig. 8 summarizes the results by showing the results for one example subject (top), the average prediction times over all 7 subjects (middle), and the number of errors (bottom). The results for the example subject show that during the habituation phase, this subject waits for the robot to actually start moving the object (approx. 14s) towards the box to predict the ultimate goal. Because the initial trajectories for each box are identical during habituation, apparent guessing before that point results in an error rate of 50%. When the optimization starts however, the two trajectories start distinguishing themselves and there is co-adaptation between the robot and the human on the intent of each trajectory. After some trial and error, the prediction time of the human drops to a consistent 1.5s, which implies that the trajectories for the left or right box already differ early on. Despite such early decisions, this subject has almost no prediction errors (only one in the last 70 trials). In comparison to the previous task, the improvements due to optimizing are more pronounced.

The prediction times have a bimodal distribution. Subjects either wait until the robot starts transporting the object, or make a prediction early on during the reaching phase. Rather than averaging over this bimodal distribution data, we compute the ratio of early/late prediction times, averaged over blocks of 8 trials and all 7 subjects. The threshold is the average over all the prediction times for one subject. In the middle graph, we see that the late prediction ratio decreases from [0.8-1.0] during the habituation phase to <0.05 at the end of the optimization phase. Furthermore, this early prediction is not accompanied by an increase in the number of errors, as the bottom graph shows. Thus, the robot learned behaviors that enabled subjects to predict the correct box before the grasp was even performed.

Because the robot's reaching behavior is the same for both boxes during the habituation phase, subjects can expect a 50% error rate when pressing the button when the robot is still reaching. The increase in the ratio of late predictions and the decrease of errors during the habituation phase may indicate that subjects learn that early guesses lead to errors, and should thus be avoided.

Finally, Fig. 9 confirms that the global cost is effectively reduced and that the human prediction time is dramatically reduced by the optimization process. We note that the robot only grasps the object once the trajectory is finished, hence the constant cost (DMPs are of fixed duration). The jerk component slightly increases over time, indicating more pronounced trajectories are performed. Discussion. During habituation, subjects must wait for the robot to transport the object before being able to robustly predict its intention. After optimization however, all subjects are able to predict the intention of the robot very early on during the reaching phase, when the robot has not yet grasped the object. Experiment B thus confirms the observations in Experiment A, that earlier intention recognition is achieved, but without an increase in errors.

V. CONCLUSION

The main contribution of this paper is to demonstrate that robots are able to autonomously adapt their behavior so that humans recognize the robot's intentions early and robustly, and that robots are able to do so without a model of how humans predict motion intentions, or knowledge of the concept of legibility. Achieving this with our approach hinges on two steps. The first is to define a cost function that penalizes efficiency (joint execution time), robustness (task errors), and energy (jerk); these are generic measures, as illustrated by the fact that we used the exact same cost function for the two very different tasks in Section IV. The second is to use a model-free optimization algorithm, such as PI BB , to efficiently optimize this cost function through trial-and-error interaction of the robot with the human. We thus provide a generic approach for improving human-robot collaboration, where legible behavior is a side-effect of generic, easy to specify cost measures.

A limitation of our work, as for all model-free optimization approaches, is that novel tasks or substantial changes to a task require re-optimization of the policies.

An open question, also raised by Dragan et al. [START_REF] Dragan | Generating legible motion[END_REF], [START_REF] Dragan | Legibility and predictability of robot motion[END_REF], is: which aspects of legibility are universal, and which are specific to individual humans? Although we have shown that optimized policies are sufficiently different to elicit earlier responses from subjects which never interacted with the robot before (Fig. 5), we clearly cannot claim that the behavior generated by the robot are universally legible. Also, to which extent can legibility be separated from coadaptation? To which extent did the robot's behavior truly become generally legible, and to which extent did the user learn to recognize idiosyncratic robot behavior that arose from exploration? In our future work, we will investigate these questions, for instance by having different subjects alternatively interacting with the robot during the same optimization process. Nevertheless the importance of coadaptation cannot be dismissed in any complex learning robot that interacts with

Fig. 1 .

 1 Fig. 1.Illustration of the button pressing experiment, where the robot reaches for and presses a button. The human subject predicts which button the robot will push, and is instructed to quickly press a button of the same color when sufficiently confident about this prediction. By rewarding the robot for fast and successful joint completion of the task -which indirectly rewards how quickly the human recognizes the robot's intention and thus how quickly the human can start the complementary action -the robot learns to perform more legible motion. The three example trajectories above illustrate the concept of legible behavior: it enables correct prediction of the intention early on in the trajectory.

Fig. 3 .

 3 Fig. 3. Results of the simulation experiments. The left figure is adapted from Fig. 1 in[START_REF] Dragan | Generating legible motion[END_REF]. The initial trajectories are the straight dashed lines between the start and the two goals, the thick black lines is the result after optimization. Intermediate policies during optimization are plotted in gray.

Fig. 4 .

 4 Fig. 4. Results for Experiment A. Top) Prediction times of an example subject during learning. Middle) Average and standard deviation (µ ± σ) over the normalized prediction times of the 9 subjects. Bottom) Number times the incorrect button was pushed, averaged over 9 subjects. The start of the optimization phase is indicated by the dashed line.

Fig.

  Fig. Box plots for the normalized prediction times, averaged over all subjects, and blocks of 8 trials. Top) Experiment with 9 subjects, with initial trajectories, as in Fig. 4. Bottom) Experiment with 4 subjects, when using previously optimized trajectories from the beginning.

Fig. 6 .

 6 Fig. 6. Components of the cost function for the two conditions of Exp. A averaged across all subjects.

Fig. 7 .

 7 Fig. 7. Illustration of the pick-and-place experiment

Fig. 8 .

 8 Fig. 8. Results for Experiment B. Top) Prediction times of an example subject during learning. Middle) Ratio of late prediction times (see threshold in top plot), averaged over the 7 subjects. Bottom) Number times the incorrect button was pushed, averaged over 7 subjects. The start of the optimization phase is indicated by the vertical dashed line.

Fig. 9 .

 9 Fig. 9. Components of the cost function for Exp. B averaged across all subjects.

Work partially supported by INRIA, Conseil Régional d'Aquitaine, the ERC grant EXPLORERS 24007, the 3rd Hand Project (funded under 7th FWP), and the European Union's Horizon 2020 research and innovation programme under grant agreement No 640891.