Marco Faroni
email: marco.faroni@stiima.cnr.it

Manuel Beschi
email: manuel.beschi@unibs.it

Nicola Pedrocchi
email: nicola.pedrocchi@stiima.cnr.it

Inverse kinematics of redundant manipulators with dynamic bounds on joint movements

Redundant manipulators are usually required to perform tasks in the operational space, but collision-free path planning is computed in the configuration space. Limiting the deviation with respect to the collision-free configuration-space trajectory may allow the robot to avoid collisions without modifying the primary task. This paper proposes a method to guarantee that the solution of the inverse kinematic problem deviates from the nominal joint-space trajectory less than a desired threshold. The excursion limitation is ensured by means of linear constraints and the automatic regulation of the weights of secondary tasks. Numerical and experimental results prove the validness of the proposed approach.

I. INTRODUCTION

Kinematically redundant manipulators have high versatility and dexterity thanks to the additional degrees of freedom they have with respect to the task they are required to perform. Extra degrees of freedom can be exploited to endow the robot with desired secondary behaviors. To name a few examples, the redundancy can be exploited to: maximize the manipulability along selected axis [START_REF] Jin | Manipulability optimization of redundant manipulators using dynamic neural networks[END_REF], [START_REF] Huber | Efficient closed-form task space manipulability for a 7-dof serial robot[END_REF], keep the joint configuration far from the mechanical limits [START_REF] Atawnih | Kinematic control of redundant robots with guaranteed joint limit avoidance[END_REF], mimic anthropomorphic motion [START_REF] Lamperti | A redundancy resolution method for an anthropomorphic dual-arm manipulator based on a musculoskeletal criterion[END_REF], activate the least number of actuators [START_REF] Tarbouriech | Dual-arm relative tasks performance using sparse kinematic control[END_REF], avoid collisions [START_REF] Flacco | A depth space approach to human-robot collision avoidance[END_REF], [START_REF] Jin | An efficient and accurate inverse kinematics for 7-dof redundant manipulators based on a hybrid of analytical and numerical method[END_REF], or maximize the distance from obstacles [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF].

The problem of exploiting the redundancy of the robot is typically addressed when solving the Inverse Kinematic (IK) problem: a primary task is given in the operational space (usually, the Cartesian space) and, for all points, the joint coordinates are computed so that the primary task is met and the secondary behavior is performed at best [START_REF] Liegeois | Automatic supervisory control of the configuration and behavior of multibody mechanisms[END_REF]. The most widespread approach consists in solving the IK problem at differential level to exploit the linear mapping between joint and task velocities given by the task Jacobian [START_REF] Chiaverini | Redundant robots[END_REF]. Joint configurations are then obtained via forward integration and numerical drifts are recovered by means of a closedloop IK algorithm [START_REF] Chiaverini | Redundant robots[END_REF]. Up to few years ago, this was done by pseudo-inverting the Jacobian to find the minimum-norm solution to the problem and by projecting the velocity related to the secondary objective into the null space of the Jacobian [START_REF] Siciliano | A general framework for managing multiple tasks in highly redundant robotic systems[END_REF]. Recently, Quadratic Programming (QP) approaches are preferred because they allow for considering robot limitations explicitly (for example, joint configuration, velocity, and acceleration limits can be written as linear constraints in the QP problem) [START_REF] Cheng | Resolving manipulator redundancy under inequality constraints[END_REF], [START_REF] Kanoun | Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks[END_REF], [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF]. In both cases, the result is that the robot modifies its posture according to the secondary objective, as shown for example in Figure 1. On the one hand, this approach has the great advantage that the IK problem can be solved at runtime, by taking into account the current state of the system. For example, the robot can adapt its posture to the position of the operator without modifying the primary task [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF]. On the other hand, the main drawback is the lack of a non-collision guarantee. As a matter of fact, the new robot posture is usually very different from the initial one and it might be in conflict with obstacles (see Figure 1). Most works on redundancy handling take into account the collision avoidance problem by maximizing the distance from the closest obstacle (assumed to be a 3D point or a simple 3D shape) [START_REF] Flacco | A depth space approach to human-robot collision avoidance[END_REF], [START_REF] Zanchettin | Safety in human-robot collaborative manufacturing environments: Metrics and control[END_REF]. This may work in simplified laboratory examples, but it is hardly scalable to real-case scenarios. For example, industrial robots are often required to work in very cluttered environments or to approach objects with complex surfaces. In these cases, re-configuration based on the closest-obstacle criterion is prone to failure.

Most times, collision-free path planning is indeed performed in the configuration space. The most widespread methods are sampling-based (e.g., RRT and its variants [START_REF] Lavalle | Rapidly-exploring random trees: A new tool for path planning[END_REF], [START_REF] Kuffner | Rrt-connect: An efficient approach to single-query path planning[END_REF], and probabilistic roadmaps [START_REF] Kavraki | Probabilistic roadmaps for path planning in high-dimensional configuration spaces[END_REF]) and local algorithms that search in the neighborhood of an initial solution (e.g., CHOMP [START_REF] Ratliff | CHOMP: Gradient optimization techniques for efficient motion planning[END_REF] and STOMP [START_REF] Kalakrishnan | STOMP: Stochastic trajectory optimization for motion planning[END_REF]). Notice that this planning approach does not explicitly exploit redundancy. As a matter of fact, the presence of redundant degrees of freedom just results in a larger search space, but secondary objectives can not be specified. Moreover, the robot motion can not be modified at high sampling rates during the execution, which is the main advantage of online IK schemes.

Based on these considerations, [START_REF] Faroni | Energy minimization in timeconstrained robotic tasks via sequential quadratic programming[END_REF] suggested to set a collision-free joint-space trajectory computed by an online path planner as low-priority task of the IK problem. In this way, the robot should follow the joint-space reference when it is not in conflict with other tasks. In that case, however, the collision-free trajectory results in a soft constraint that is soon violated to meet higher-priority tasks (as also shown in Section IV-A of this paper). Notice that imposing hard constraints on the joint deviation with respect to the reference trajectory would result in a possible failure of the QP problem. This is a well-known issue in online IK schemes, which require additional viability constraints to ensure the feasibility of the QP problem when constant position limits are considered [START_REF] Del Prete | Joint position and velocity bounds in discrete-time acceleration/torque control of robot manipulators[END_REF], [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF]. To the best of Fig. 1: Effect of usual IK schemes. Orange robot: initial posture; dark robot: final posture that optimizes secondary tasks. Robot posture may vary significantly with respect to the initial one and collision check is no longer guaranteed. the authors' knowledge, however, no prior works address viability of time-varying position limits.

This paper proposes a method to ensure that the IK solution, computed online, has a bounded deviation with respect to a given time-varying signal. By using a configurationspace collision-free trajectory as reference signal, it is possible to avoid collisions with the surrounding environment. To do so, the paper devises a viability constraint valid for time-varying position bounds by extending the viability conditions of constant bounds [START_REF] Del Prete | Joint position and velocity bounds in discrete-time acceleration/torque control of robot manipulators[END_REF], [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF], and by using a taskscaling IK approach [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF], [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF]. Moreover, it devises a closedloop algorithm that automatically adapts the weights of the secondary tasks, in order to regulate the deviation of the joint position to a given value. Compared to soft constraintbased approaches (e.g., [START_REF] Atawnih | Kinematic control of redundant robots with guaranteed joint limit avoidance[END_REF], [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF]), the joint movement is guaranteed to be limited to the maximum range also for time-varying bounds. The paper also analytically derives a linear approximation of the new viability constraints, taking out the need of time-consuming numerical approaches (e.g., [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF] derives viable polygons for constant bounds by means of nonlinear programming). Numerical and experimental results on a 7-degree-of-freedom manipulator show that the proposed approach is able to hardly limit the excursion with respect to the nominal joint trajectory where soft-constraint approaches fail.

The paper is organized as follows. Section II deals with the formulation of IK problems as QP problems and the related feasibility issues. Proposed method is described in Section III; simulation and experimental results are in Section IV. Conclusions are drawn in Section V. Appendix A derives the polygonal viable set in closed form.

II. PRELIMINARIES

A. QP-based inverse kinematic problem

Recently, the IK problem of robot manipulators has been addressed by means of quadratic programming to cope with robot limits and/or other limitations that can be modeled as optimization constraints. As an example, consider a manipulator to which a primary task A 1 q + b 1 = 0 and a secondary task A 2 q + b 2 = 0 are assigned. The primary task is usually the desired Cartesian trajectory; that is, A 1 is the end-effector Jacobian and -b 1 is the desired twist at each time step. Closed-loop IK schemes modifies b 1 to recover from position errors [START_REF] Chiaverini | Redundant robots[END_REF]. Secondary tasks are soft constraints the robot should fulfill at best (e.g., manipulability maximization [START_REF] Huber | Efficient closed-form task space manipulability for a 7-dof serial robot[END_REF] or maximization of the human-robot distance [START_REF] Flacco | A depth space approach to human-robot collision avoidance[END_REF]).

At each sampling time t = kT (where T is the sampling period of the system), the joint velocity is chosen by solving the following hierarchical QP problem:

q = argmin u A 2 u + b 2 2 subject to A 1 u = A 1 u 1 u 1 = argmin u A 1 u + b 1 2 u ∈ S (1)
where S is a set of linear inequalities that account for the robot limits (details on the derivation of S are given in Section II-C). Problem (1) means that the secondary task (i.e., the cost function) is optimized by searching in the null space of the primary task (i.e., first and second constraints in ( 1)). This approach has been extended to any number of prioritized tasks and to inequality tasks in [START_REF] Kanoun | Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks[END_REF].

Hierarchical QP problems in the form of ( 1) can be solved by means of a nested approach, that is: i) solve the inner QP problem and find u 1 = argmin u A 1 u + b 1 2 ; ii) use u 1 to solve the outer QP problem (i.e., q = argmin u A 2 u + b 2 2 subject to A 1 u = A 1 u 1 , u ∈ S). Although useful to have an insight of QP-based IK, this approach is computationally inefficient; dedicated solvers that exploit the geometric properties of hierarchical QPs are preferred [START_REF] Escande | Hierarchical quadratic programming: Fast online humanoid-robot motion generation[END_REF], [START_REF] Liu | Generalized hierarchical control[END_REF], [START_REF] Rocchi | Opensot: a whole-body control library for the compliant humanoid robot Coman[END_REF].

Methods based on (1) are referred to as local, as they only take into account tasks and constraints at the current time instant. Look-ahead and model predictive control approaches have been also proposed to overcome the drawbacks of local methods [START_REF] Faroni | A predictive approach to redundancy resolution for robot manipulators[END_REF], [START_REF] Schuetz | Predictive online inverse kinematics for redundant manipulators[END_REF], [START_REF] Zube | Cartesian nonlinear Model Predictive Control of redundant manipulators considering obstacles[END_REF].

B. Task scaling inverse kinematics

Recent approaches exploit the path-velocity decomposition of the trajectory to preserve the geometry of the path by acting on the timing law when the nominal trajectory would exceed the robot limits [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF], [START_REF] Antonelli | Kinematic control of redundant manipulators with on-line end-effector path tracking capability under velocity and acceleration constraints[END_REF]. Problem (1) is modified by adding a scaling variable s ∈ [0, 1] that scales the primary task velocity b 1 so that the geometric direction is not changed. The resulting hierarchical QP is:

q s = argmin u,s A 2 u + b 2 2 + µ(1 -s) 2 subject to A 1 u + b 1 s = A 1 u 1 + b 1 s 1 u 1 s 1 = argmin u ,s A 1 u + b 1 s 2 u ∈ S 0 ≤ s ≤ 1 (2)
where µ >> 1. Problem ( 2) is similar to (1), but when the primary task is not feasible with respect to the robot limits, s results to be smaller than one and the trajectory automatically slows down to preserve the desired geometrical path.

A receding horizon approach to this problem was also proposed in [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF] to overcome the drawbacks of local methods.

C. Viability and feasibility of the inverse kinematic problem

Referring to (1) and ( 2), S is the set of admissible velocities that satisfies the robot limits Usually, IK schemes take into account joint configuration, velocity, and acceleration limits such that:

q min ≤ q ≤ q max -qmax ≤ q ≤ qmax -q max ≤ q ≤ qmax (3)
where q min and q max are the minimum and maximum joint configuration limits, qmax is the maximum joint velocity vector, and qmax is the maximum joint acceleration vector.

To ensure the feasibility of the QP problem for all possible states in S, an additional viability constraint has to be added as follows [START_REF] Del Prete | Joint position and velocity bounds in discrete-time acceleration/torque control of robot manipulators[END_REF]:

q + q2 2q max -q max ≤ 0 (4) q + q2 2q max -q min ≥ 0 (5)
Note: products, quotients, and inequalities between vectors are intended as component-wise.

Equations (4)(5) determine the maximal viable set for a double integral system with bounded configuration, velocity, and acceleration. The equation is quadratic in q. A viable set given by a set of linear constraint is helpful to include such conditions in the online control problem in case of optimization-based algorithms (e.g., model predictive control [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF], [START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF]). In [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF], a viable set composed of one linear constraint was derived. The result is that (4)(5) can be substituted by the following linear constraints: qmax q + qmax q -qmax q max ≤ 0 (6) qmax q + qmax q -qmax q min ≥ 0

The problem is analyzed more in details in [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF], where a numerical method to obtain the maximal polygon given the number of sides is proposed. Equations ( 4)-( 7) are valid if position constraints are static; thus, if q min or q max varies in time, they are no longer sufficient. The case of time-varying position limits is addressed by our method in the next section.

III. PROPOSED METHOD

Consider a joint-space nominal trajectory given by a curve q nom (s) and a timing law s(t). At sampling time t = kT , we aim to ensure that:

q nom (t) -∆q max ≤ q(t) ≤ q nom (t) + ∆q max ∀ t ≥ kT (8)
where ∆q max is the vector of maximum allowed joint displacement with respect to the nominal trajectory. We address this issue by adding a time-varying constraint to the optimization problems ( 1) and ( 2) and by adjusting the secondary task cost function dynamically.

A. Constraint-based limitation of the joint excursion

Equation ( 8) can be considered as a time-varying jointconfiguration constraint to be added to (1) and ( 2). Its addition requires a further viability analysis of the admissible set. Similarly to (4), the following viability constraint may be added to the QP problem (for the sake of brevity only the right side of (8) is considered):

q(k + 1) + q(k + 1) 2 2q max -q lim ≤ 0 (9)
where q lim = q nom (k + 1) + ∆q max (hereafter, dependency on k + 1 will be omitted unless necessary). Feasibility of the new problem depends on the value of q nom (k + 1). To clarify this, consider the example in Figure 2, in which joint state (q, q) is on the boundary of (4). If q nom (k+1) < q nom (k), then the viable set shifts to the left and the current state does not belong to the viable set anymore. Nonetheless, feasibility can be strongly guaranteed when a task-scaling IK scheme such as (2) is implemented. In this case, the desired configuration at time k + 1 depends on the scaling factor s, that is:

q nom (k + 1) = q nom (k) + ∆q nom (k) s (10)
from which:

q lim = q nom (k) + s ∆q nom (k) + ∆q max (11)
The decision variable s therefore can slow down the original trajectory to ensure the feasibility of the problem. Notice that in the worst case described in Figure 2, the solution s = 0 does not change the viable set from cycle k to cycle k + 1 (red and green areas coincide), keeping the state within the viable set.

1) Linear approximation of the viable set: Constraint is nonlinear. Nonetheless, the approximation of the viable set recalled in Section II-C still holds. For example, the singlesegment constraint (6) becomes: qmax q + qmax q -qmax q lim ≤ 0 [START_REF] Cheng | Resolving manipulator redundancy under inequality constraints[END_REF] which is linear in q, q, and s.

A less conservative viable set can be obtained by using more linear constraints. Given the number of constraints N , the points P j , j = 0, . . . , N , that delineate a viable polygonal set for each joint can be obtained as follows:

P j = qmax N j, q max - q2 max 2N 2 qmax j(j + 1) (13)
The derivation of ( 13) is given in Appendix A.

B. Self-regulating weighting of secondary tasks

The mere use of ( 9) (or its linear approximations) ensures the viability of the admissible set of QP problem (2). However, the IK problem is unaware of the nominal jointspace trajectory and therefore computes the joint variables by only considering the secondary task(s). As a result, the joint configurations are likely to reach the maximum excursion ∆q max and the task-scaling mechanism would slow down the robot to respect the excursion limit (even up to the stop of the task, as in the previous example). To avoid this situation, the IK problem should be aware of the desired joint-space trajectory. In [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF], the joint-space nominal trajectory was considered as the lowest-priority task in the QP problem. Here, we prefer to adopt a different approach where the priority of the joint-space nominal trajectory changes dynamically. Changing priorities between tasks on the fly is not trivial because an instantaneous swap of the priority of two tasks generates discontinuities in the joint commands and abrupt motions of the robot [START_REF] Han | Robot control near singularity and joint limit using a continuous task transition algorithm[END_REF]. This issue can be addressed by gradually swapping the tasks in a weighted fashion during a transition phase [START_REF] Liu | Generalized projector for task priority transitions during hierarchical control[END_REF], [START_REF] Kim | Continuous task transition approach for robot controller based on hierarchical quadratic programming[END_REF]. This is useful especially when the priorities of the stack of tasks are strict (e.g., humanoid robots). In industrial robotics, strict priority is usually only between the primary (i.e., the Cartesian trajectory) and the successive tasks, whereas secondary tasks are usually objective behaviors such as dexterity, humanrobot distance maximization, etc. For this reason, we use a mixed approach where the secondary task A 2 q + b 2 = 0 and the joint-space nominal trajectory are handled in a weighted fashion. The hierarchical QP problem (2) therefore becomes:

q s = argmin u,s q nom -q 2 + λ A 2 u + b 2 2 + µ(1 -s) 2 subject to A 1 u + b 1 s = A 1 u 1 + b 1 s 1 u 1 s 1 = argmin u ,s A 1 u + b 1 s 2 u ∈ S 0 ≤ s ≤ 1 ( 14
)
where λ is a non-negative variable that can be adjusted to regulate the importance of the joint-space trajectory with respect to the secondary task. The automatic choice of λ can be addressed as a regulation problem such that:

max i ∆q i ∆q ref,i → 1 (15)
where ∆q = q nom (k) -q(k) and ∆q ref is the vector of reference values for each joint excursion such that |∆q refi | ≤ ∆q max,i . Variable λ can be therefore considered as a control input to drive ∆q to a reference value ∆q ref based on the current value of ∆q. In this work, we used a discrete-time proportional-integral-derivative control law defined as:

λ PID (k) = K P y(k)+K I k j=0 y(k -j)+K D y(k)-y(k -1) (16
) where y = 1 -max i ∆qi ∆qref,i and K P , K I , K D are the proportional, integral, and derivative gains respectively. The output is saturated between 0 and λ max in such a way that:

λ(k) =      0 if λ PID (k) < 0 λ max if λ PID (k) > λ max λ PID (k) otherwise (17)
A conditioned integration anti-windup strategy have been implemented to avoid windup issues due to saturation.

IV. RESULTS

We performed simulation and experimental tests on a 7degree-of-freedom robot composed of a Universal Robots UR5 manipulator mounted on an actuated linear track. We consider conservative limits with respect to datasheet values because of the presence of cabling required by the endeffector, namely: q min = ( q track min , q robot min ) q max = ( q track max , q robot max ) qmax = ( qtrack max , qrobot max )

where q track min = 0 m, q track max = 1.55 m, qtrack max = 0.5 m/s and q robot min = (-2.5, -2.2, -2.5, -π, -π, -3.5) rad q robot max = ( 2.5, 0, 2.5, 2, π, 3.5) rad

qrobot max = (1.4, 1.4, 1.4, 1.4, 1.4, 1.4) rad/s (19)
Acceleration limits have been chosen based on empirical tests and correspond to:

qmax = ( qtrack max , qrobot max ) (20)
where qtrack max = 1.0 m/s 2 and qrobot max = (1.57, 1.57, 1.57, 1.57, 1.57, 1.57) rad/s 2 . The task-scaling IK algorithm used in the tests is the lookahead method proposed in [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF] with a predictive horizon equal to 0.2 s. Regarding the proposed method, the parameters to be chosen are ∆q max and ∆q ref , where ∆q ref < ∆q max . Their value is directly related to the maximum deviation with respect to the collision-free trajectory: the smaller are ∆q max and ∆q ref , the smaller is the deviation due to the redundancy.

A. Simulations

Simulation tests have been performed by using a constrained kinematic model of the robot in ROS (Robot Operating System). The tests consist in the execution of a pointto-point Cartesian trajectory. A joint-space path is planned by means of the sampling-based algorithm BiTRRT-Connect available in MoveIt!. This path is collision free and is taken as the nominal joint-space task q nom . The corresponding Cartesian path represents the primary task to be followed by the robot. As secondary task, the robot should keep the elbow link as far as possible from the working table positioned in front of it. This is formalized by setting A 2 equal to the second row of the robot elbow Jacobian and -b 2 equal to a velocity directed toward the direction -y of the world frame. The scenario is shown in Figure 3.

For the purpose of the present work, the actual joint trajectory q should deviate from q nom less than a given threshold ∆q max . Four different cases have been analyzed:

• Case A: typical IK scheme with constant joint bounds [q min , q max ] [22]; no deviation with respect to a jointspace reference is considered; • Case B: soft-constraint approach [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF]; the joint-space reference trajectory is set as lowest-priority task in the IK scheme; • Case C: the self-regulating mechanism described in Section III-B is used, with ∆q ref = 0.15 for all joints. • Case D: the constraint-based method described in Section III-A and the self-regulating method described in Section III-B are used, with q max = 0.2 and ∆q ref = 0.15 for all joints. In Cases C and D, the following values of the PID parameters have been used: K P = 1, K I = 10, and K D = 20.

Figure 4 shows the resulting joint trajectories of Joints 2 and 6, and the ratio max i |∆q i /∆q max,i |. Such ratio has to be less than or equal to one to satisfy the maximum deviation ∆q max . As expected, in Case A, the joint trajectory deviates from the nominal one without control. The softconstraint approach (Case B) reduces the deviation from the nominal joint-space reference (that is set as lowest-priority task) but the joint trajectories drift away from the reference as the elbow task has strict higher priority than the jointspace reference. When the adaptive weighting mechanism is activated (Case C), the deviation with respect to the nominal trajectory decreases and eventually converges to ∆q ref , but it is not possible to guarantee a maximum deviation value. As a matter of fact, although ∆q ref = 0.75∆q max , the maximum deviation value exceeds ∆q max during part of the task (at around 10.5 s). On the contrary, Case D always ensures the strict satisfaction of the maximum deviation: the joint trajectory never exits the tube q nom ± ∆q max and the ratio is always less than or equal to one. As a drawback, the scaling mechanism has to be activated to recover feasibility and this results in the slowdown of the task. As shown in the figure, the time required to complete the task is around 1.5 times larger than the time taken by Cases A, B, and C. This phenomenon is relevant especially when ∆q max is very small or when ∆q ref /∆q max is close to one, for the joints are often close to their maximum allowed excursion. It is also evident that the limitation of the joint range of motion comes at the cost of a reduced satisfaction of the secondary task; the smaller is the allowed range of motion, the smaller is the secondary task satisfaction. In this scenario, the secondary task minimizes the y-component of the elbow position. Its level of satisfaction for the different cases is shown in the following table:

Case A B C D 2 nd task satisfaction 100% 58% 39% 27%
where the percentage of the secondary task satisfaction (compared with the unbounded Case A) is computed as:

2 nd task satisfaction = 100 • y elb i -y elb 0 y elb A -y elb 0 ( 21
)
where y elb i is the average value of the elbow y-coordinate during the test, i ∈ {A, B, C, D} denotes the case and y elb 0 refers to the average value of the elbow y-coordinate corresponding to the joint-space nominal trajectory.

B. Experiments

A human-robot cooperation application has been implemented on the real 7-degree-of-freedom platform composed of a Universal Robots model UR5, version 3.2, mounted on a linear track actuated via a linear motor and an Elmo Gold Cello servo drive. The proposed IK scheme (Case D in Section IV-A) has been implemented in ROS-control framework as an intermediate controller between MoveIt! and the robot controller. The IK scheme generates the reference signal for the robot controller, which is a cascade of a joint-position and a joint-velocity control loops. The position controller is a proportional gain equal to 7, the velocity controller is the built-in control system of the robot. The IK output is the reference of the joint-position control loop, whose control action is sent to the robot velocity controller via TCP/IP with a frequency of 125 Hz (maximum allowed by the UR5 controller interface). Similarly, the linear track . Solid blue: actual joint trajectories; solid red: nominal configuration-space trajectories q nom ; rose area: allowed joint deviations q nom ± ∆q max . Cases A and B cannot limit the joint deviation; in Case C the joint deviation converges to ∆q ref but the hard constraint is violated during transients (the ratio is grater than 1 at time 10.5 s); in Case D the joint trajectories are always within the allowed tube (the ratio is always less than or equal to 1). Fig. 5: Experimental results: ratio max i |∆q i /∆q max,i | for Case D (solid blue). Ratio equal to 1 (dashed red) represents the maximum allowed deviation. Thanks to the proposed method (Case D), the ratio is always less than or equal to 1, that is, the joint trajectories are always within the allowed tube q nom ± ∆q max .

receives the control action with a frequency of 1 kHz by means of a Ethercat field-bus. The real-time position of the humans is acquired by means of two Realsense cameras, model d435. Static obstacles are removed from the point cloud signal by means of the ROS robot self-filter. The filter output is re-sampled by using 10 cm 3 voxels and sent to the IK solver with a frequency of 30 Hz. The robot has to repeat a point-to-point motion in the Cartesian space, sharing the workspace with two operators. The secondary task consists in the movement of the robot elbow in the opposite direction with respect to the position of the closest body part. Hence, A 2 coincides with the first two rows of the elbow Jacobian and b 2 is equal to the unit vector from the elbow to the human position, multiplied by a gain equal to 0.4. The maximum and reference joint excursions are ∆q max = 0.4 and ∆q ref = 0.3. To avoid collisions with the operators, the speed of the robot is reduced by a factor between 0 and 1 based on the human-robot relative distance as in [START_REF] Faroni | An MPC framework for online motion planning in human-robot collaborative tasks[END_REF]. The speed reduction factor is equal to 1 up to a distance equal to 1 m and is equal to 0 when the distance is equal to 0.3 m. Intermediate values are obtained by interpolating via a third-order polynomial.

Figure 5 shows that the ratio max i |∆q i /∆q max,i | is always less than or equal to one, which means that the joint trajectories always lay within the allowed tube q nom ± ∆q max , according to the theoretical and simulation results.

V. CONCLUSIONS

This paper studied the time-varying limitation of the joint movements in the inverse kinematic problem of redundant manipulator. The motivation of the work came from the need of ensuring a bounded deviation of the joint movement with respect to a given nominal configuration-space trajectory. The paper showed that such limitation can be strictly ensured by using a task-scaling inverse kinematics and by deriving linear constraints with respect to the joint command variables. Moreover, a self-regulating weighting strategy was implemented to keep the robot joint close to the reference signal, so that the scaling mechanism activation is reduced. The validity of the proposed approach was proved via simulations and experiments on a 7-degree-of-freedom industrial manipulator.

APPENDIX

A. Viable polygonal sets for constrained double integral systems

Imposing the viability of the admissible set is necessary to ensure the feasibility of the optimal control problem for all possible states of the system [START_REF] Blanchini | Set invariance in control[END_REF]. The equation of the maximal viable set for a double integral system is quadratic in the system states. Polygonal viable sets are useful because they can be easily implemented in optimization-based control algorithms (e.g., model predictive control [START_REF] Faroni | Predictive inverse kinematics for redundant manipulators with task scaling and kinematic constraints[END_REF], [START_REF] Faroni | Predictive joint trajectory scaling for manipulators with kinodynamic constraints[END_REF]). In [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF], a polygonal viable set with a given number of sides is obtained by setting up an optimization problem that maximizes the area of the polygon. The output of the optimization is the sequence of points that delineate the polygon. The numerical Fig. 6: Graphical representation of the polygonal viable set derivation. The first quarter of the polygon is delineated by the points {P 0 , . . . , P N } where P j = (x j , y j ).

approach is time consuming and a change in the joint limits would require the re-computation of the set. In this work we derive a viable polygonal set in closed-form.

To this purpose, consider the portion of the convex polygon in the first quarter delineated by the sequence of points {P 0 , . . , P N }, where P j (x j , y j ) (results for the third quarter of the plane can be derived straightforwardly by mirroring the solution). Starting from point P 0 (0, q max ), the maximal viable polygon can be constructed by maximizing the slope of the segments in the plane q q (see also Figure 6). The maximum slope at q = x j depends on the maximum acceleration qmax and is equal to -x j /q max . Thus, the successive points can be derived with respect to their x j coordinate, for example: P 1 = (x 1 , q max -x 2 1 /q max ) P 2 = (x 2 , q max -(x 2 -x 1 )x 2 /q max ) (

For a generic point P j it results:

P j = x j , q max - 1 qmax j i=1 (x i -x i-1 ) x i (23)
where x 0 = 0. The approach in [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF] sets up an optimization problem to minimize the area A in Figure 6. An analytical solution to the problem is doable only for small values of N (N ≤ 3). Consider the simpler problem where only the y coordinate of point P N is maximized, that is:

maximize xi q max - 1 qmax N i=1 (x i -x i-1 ) x i subject to x 0 = 0; x N = qmax . ( 24
)
Notice that this problem is a simplification of the original one, but the approximation is reasonable also in view of the fact that the maximization of y N tends to maximize the region where the robot is allowed to work at high speed. Problem [START_REF] Flacco | Control of redundant robots under hard joint constraints: Saturation in the null space[END_REF] can be solved analytically by imposing δy N /δx j = 0, that is:

- 1 qmax 2x j -x j-1 -x j+1 = 0 (25)
that can be rewritten as

x j+1 -x j = x j -x j-1 [START_REF] Liu | Generalized hierarchical control[END_REF] with x N = qmax . It follows that the points have to be equally spaced along the axis q . This can be summarized as:

x j = qmax N j with j = 1, ..., N.

Finally, the points of the polygon can be found by inserting [START_REF] Rocchi | Opensot: a whole-body control library for the compliant humanoid robot Coman[END_REF] into [START_REF] Faroni | Viability and feasibility of constrained kinematic control of manipulators[END_REF] to obtain:

P j = qmax N j, q max - q2 max 2N 2 qmax j(j + 1) (28)
It is interesting to note that:

lim N →∞
y N = q max -q2 max 2q max [START_REF] Schuetz | Predictive online inverse kinematics for redundant manipulators[END_REF] which means that the polygonal viable set tends to the maximal viable set given by ( 4), for N → ∞.

ACKNOWLEDGMENT

The research leading to these results was partially funded by the European Union H2020-ICT-2017-1 -Pickplace: Flexible, safe and dependable robotic part handling in industrial environment (grant agreement: 780488).

Fig. 2 :

 2 Fig. 2: Viability regions for time-varying joint bounds (only the first quarter is shown). Green: viability set at time k. Purple: viability set at time k + 1. At each time instant, the viability region is determined by an interval of amplitude 2∆q max centered in the current nominal joint trajectory q nom .

Fig. 3 :

 3 Fig. 3: Simulation scenario: the robot should point toward the opposite direction with respect to the working table.

Fig. 4 :

 4 Fig.4: Simulation results: joint configurations of Joints 2 and 6 (respectively, UR5's base and wrist2 joints, according to the manufacturer naming) and ratio max i |∆q i /∆q max,i | for Cases A (first row), B (second row), C (third row), and D (fourth row). Solid blue: actual joint trajectories; solid red: nominal configuration-space trajectories q nom ; rose area: allowed joint deviations q nom ± ∆q max . Cases A and B cannot limit the joint deviation; in Case C the joint deviation converges to ∆q ref but the hard constraint is violated during transients (the ratio is grater than 1 at time 10.5 s); in Case D the joint trajectories are always within the allowed tube (the ratio is always less than or equal to 1).


Marco Faroni

Manuel Beschi

Stefano Ghidini

Nicola Pedrocchi

Alessandro Umbrico

Andrea Orlandini

Amedeo Cesta

A Layered Control Approach to Human-Aware Task and Motion Planning for Human-Robot Collaboration

des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

I. INTRODUCTION

Robotized assembly and manufacturing are evolving towards a human-centered paradigm [START_REF] Monostori | Cyberphysical systems in manufacturing[END_REF] but, even if humans improve the flexibility and performance of the system, the complexity underlying task and motion planning activities increases [START_REF] Pellegrinelli | Motion planning and scheduling for human and industrial-robot collaboration[END_REF]. In Human-Robot Collaboration (HRC), the problem complexity is high even for a task composed of a few activities: a simple task can be performed through many robot trajectories and each of them could be performed simultaneously with different human tasks. Also, the robot's execution time may be different than expected, as the robot may slow down for safety reasons [START_REF] Pellegrinelli | Motion planning and scheduling for human and industrial-robot collaboration[END_REF]. Therefore, HRC makes the integration of task and motion planning difficult without relying on limiting hypothesis and applicability contexts [START_REF] Pellegrinelli | Multi-robot spot-welding cells for car-body assembly: Design and motion planning[END_REF]. To cope with such complexity, many works focus on the identification of a feasible solution and not the optimal one. Such methods are mainly structured into three groups depending on the task planning strategy: PDDL [START_REF] Srivastava | Combined task and motion planning through an extensible plannerindependent interface layer[END_REF], hierarchical task networks [START_REF] Wolfe | Combined task and motion planning for mobile manipulation[END_REF], [START_REF] Silva | The HATP hierarchical planner: Formalisation and an initial study of its usability and practicality[END_REF], and constraint satisfaction problems (CSP) [START_REF] Lozano-Pérez | A constraint-based method for solving sequential manipulation planning problems[END_REF]. These methodologies, however, may achieve low performance in industrial application, where at least a sub-optimal feasible solution is necessary. Few works have also addressed the identification of an optimal task and motion planning via, e.g., a logic-geometric programming approach [START_REF] Toussaint | Logic-geometric programming: An optimization-based approach to combined task and motion planning[END_REF]. Such approach, however, does not scale to large 1 STIIMA-CNR -Institute of Intelligent Industrial Technologies and Systems, National Research Council of Italy {name.surname}@stiima.cnr.it 2 Dipartimento di Ingegneria Meccanica e Industriale, University of Brescia {name.surname}@unibs.it 3

ISTC-CNR -Institute of Cognitive Sciences and Technologies, National

Research Council of Italy {name.surname}@istc.cnr.it number of tasks, and it cannot manage the complexity of industrial tasks. Except for [START_REF] Pellegrinelli | Motion planning and scheduling for human and industrial-robot collaboration[END_REF], [START_REF] Pellegrinelli | Multi-robot spot-welding cells for car-body assembly: Design and motion planning[END_REF], all the approaches in literature do not deal with time-variability and constraints characterizing HRC tasks. On the one hand, there are many constraints to be considered in order to safely perform robot tasks. Specifically, the execution of robot tasks must take into account simultaneous tasks the human may perform to avoid collisions and risks. On the other hand, a coordinated task plan should be generated and concurrently performed by humans and robots with the aim of increasing the production efficiency and safely supporting the human. An interesting hierarchical approach that could be extended to HRC is presented in [START_REF] Zhang | Co-optimizing task and motion planning[END_REF]. A co-optimization approach explicitly models the robot transition cost from one task to another, and splits the optimization problem over three layers: task planning, action planning and motion planning. The methodology allows the optimization of the plans to reach the robot final configuration and exploits the information coming from the low-level motion planner. However, [START_REF] Zhang | Co-optimizing task and motion planning[END_REF] does not consider temporal information and the likely concurrent execution of human and robot tasks, and the task planner uses a Travel Salesman Problem formulation that does not fit the specification of a generic industrial task. An underrated issue is the integration of the feedback coming from the field for the update of the motion and task planning and execution [START_REF] Pellegrinelli | Multi-robot spot-welding cells for car-body assembly: Design and motion planning[END_REF]. Indeed, task and motion planning architectures should also consider to dynamically change the motion plan to be compliant with human movements.

This work aims to extend [START_REF] Pellegrinelli | Multi-robot spot-welding cells for car-body assembly: Design and motion planning[END_REF] investigating a control-based approach to the optimization of the task and motion planning in HRC. Starting from the formalization of the problem as in [START_REF] Zhang | Co-optimizing task and motion planning[END_REF], we propose a layered control approach for HRC at different levels of granularity that maximizes throughput in terms of process execution time. The work is also motivated by the H2020 project ShareWork (http://www.shareworkproject.eu), which aims at developing safe and effective innovative HRC solutions in manufacturing.

The paper is organized as follow: Section II deals with the description of the methodology, Section III describes the simulation of a case study, and Section IV reports some points of discussion and the future work description.

II. HUMAN-AWARE CONTROL APPROACH

Fig. 1 shows an overview of the proposed control architecture, split in two main modules: a task planner and an action planner (which, in turn, integrates the motion planner). The Task Planner addresses human-robot coordination and task sequencing and assignment. It generates a suitable task sequence dealing with the temporal variance entailed by the human presence. The Task Planner sends the goals to a human operator, for example through a Human-Machine Interface (HMI). In addition, it receives feedback from the HMI, as operators can accept or discard the incoming tasks, and also inform the system about the outcome of the performed task (successfully finished or terminated with errors). This feedback is fundamental as the controller cannot assume the duration of human tasks but it needs to wait for a feedback. Pursuing a Sense-Plan-Act approach, the task planner is capable of dynamically (re)planning the sequence of tasks according to the feedback and the (possible) refusal of task from the operator. The robot Action Planner computes the optimal sequence of motion plans that satisfies the operational constraint. Remarkably, the actual human position is considered as feedback when the motion plans are computed. Once the action is defined, the Action Planner sends the trajectory to be executed to the robot motion controller. The combination of these modules realizes flexible robot behaviors that can also be dynamically adapted according to the observed behaviors of a human operator, limiting negative effects on the production flow, when unforeseen events occur.

Action Planner

Task Planner

Motion Controller

Task Plan Synthesis

Task Plan Execution

A. Task Planner

The Task Planner relies on Timeline-based Planning and Scheduling (P&S) [START_REF] Muscettola | HSTS: Integrating Planning and Scheduling[END_REF], a temporal planning paradigm that takes inspiration from the classical Control Theory and has been successfully applied in many real-world applications (e.g., [START_REF] Cesta | MRSPOCK: Steps in Developing an End-to-End Space Application[END_REF]). A timeline-based model is composed of a set of state variables describing the possible temporal behaviours of domain features. Each state variable specifies a set of values that represent the states or actions the related domain feature may assume or perform over time. Each value is also associated with a flexible duration and a controllability tag which specifies whether the value is controllable or not (from the "controller perspective"). A state transition function characterizes the allowed sequences of values of a state variable, i.e., the set of temporal behaviors of a domain feature that are valid with respect to the domain specification (i.e., a timeline). In order to coordinate different domain features and realize complex functionalities or achieve complex goals, a set of rules called synchronizations model such "global" constraints that must be satisfied to build valid plans. A timeline represents an envelope of valid temporal behaviors of a particular domain features. A plan is constituted by a set of timelines (one for each state variable) and represents a set of temporal behaviors (i.e., solutions) that can be adapted according to the observations/feedback received from the environment during execution. The execution of a timeline-based plan consists in selecting the particular temporal instance of a plan which best fits the observed state of the environment. The Task Planner is implemented through PLATINUm [START_REF] Umbrico | PLAT-INUm: A New Framework for Planning and Acting[END_REF], a timeline-based software framework for P&S able to deal with temporal uncertainty and already successfully deployed in real-world manufacturing scenarios [START_REF] Pellegrinelli | Motion planning and scheduling for human and industrial-robot collaboration[END_REF]. A planning synthesis module (Task Plan Synthesis in Fig. 1) synthesizes flexible behaviors of domain features (i.e., the timelines). An executive module (Task Plan Execution in Fig. 1) dispatches the tasks composing the timelines and checks the consistency of a plan with respect to the actual state of the environment. PLATINUm pursues a general solving behavior can be tailored to the particular features of a planning problem through the use of specifically designed heuristics and search strategies. The former supports the solving algorithm in selecting the flaw to solve at each iteration. This choice does not affect the completeness of algorithm but it is particularly crucial for its efficiency. The latter supports the solving algorithm in selecting which partial plan to refine at next iteration among the ones available in the search space (search space expansion). This choice may affect the completeness of the algorithm and the qualities of the synthesized plans. The design of P&S applications for HRC also entails design and verification issues [START_REF] Orlandini | Design of timeline-based planning systems for safe human-robot collaboration[END_REF]. Here, we focus on the approach feasibility and these issues are considered out of the scope of the paper.

B. Action Planning Module

The Action Planner is in charge of converting high-level actions from the task planner into actual robot movements. Its general structure is represented in Fig. 1. It is equipped with a motion planning module used to solve motion planning queries during the action planning. It encodes a set of actions such as pick from, place in, assemble, which are automatically parsed into a sequence of motion instances and basic operations (e.g., move to, grasp, release, wait). These basic operations represent the smallest level of granularity in the task decomposition hierarchy described in Fig. 2. For example, consider the case of a pick and place skill. The robot should perform the following operations: move to the picking station; approach the object; grasp it; move to the placing spot; approach the final pose; and release the object. These operations shall be executed in the aforementioned order. From the task planner point of view, it is pointless to subdivide the skill in single operations, given that their order is already known. On the contrary, gathering all the operation in a higher-level action reduces the complexity of the task planning problem because a large number of sub-tasks is removed. For this reason, all these operations are grouped in a single action (yellow nodes in Fig. 2) that corresponds to a planning instance for the Action Planner.

An action planning query finds the optimal plan to execute the sequence of basic operations (green nodes in Fig. 2. Although the order of the operations is already defined, the Action Planner has a certain level of autonomy in the way such operations are performed. Referring to the pick and place example, multiple objects of the same type might be eligible for the same pick and place task. The Action Planner runs a motion planning query from the robot current state to each eligible picking spot. In this way, it is possible to select the most suitable object. The selection criterion is the cost of the path planning query computed by means of the metric function used by the motion planning algorithm. Path length is the most common index, but indices based on the level of interference between the robot and the human's motion are also possible by using a human-occupancy costmap. Similarly, in the placing operation, if multiple placing spots are available, the Action Planner selects the most suitable with respect to the planning metrics. It is worth noticing that the path planning problems are solved online and therefore take into account the current state of the human co-worker and the environment. This means that the same motion planning query gives different solutions based on the current state of the workspace and the human behavior. Once an optimal plan to execute the action is found, the trajectories are sent to the robot controller, which is in charge of executing and monitoring the actual motion of the robot.

Other complex skills can be encoded in the motion planning module by following a similar approach. It is clear that reasoning at skill level is a great advantage for the task planner, which can now focus on high-added-value operations, while the motion planning module automatically manages complex but structured operations.

III. CASE STUDY -COLLABORATIVE MOSAIC ASSEMBLY

We analyze a case study in which an operator and a robot shall assemble the mosaic shown in Fig. 3. It reproduces the acronym SW (from the name of the project ShareWork) and is composed of 50 cubes arranged in 5 rows and 10 columns. Each slot of the mosaic is identified by its column letter and its row number (e.g., A1, A2, etc.). Letter 'S' is made up of orange cubes; letter 'W' is made up of white cubes; the background is made up of blue cubes. The mosaic shall be assembled according to the following constraints: i) orange cubes moved only by the robot; ii) white cubes moved only by the operator; iii) blue cubes moved by both the robot and the operator; iv) Row 3 shall begin after the end of Row 1; v) Row 4 shall begin after the end of Rows 1 and 2; vi) Row 5 shall begin after the end of Rows 1, 2, and 3. The following subsections describe the model considered in the proposed framework for the collaborative process and the ROS-based simulation environment developed for validating the proposed methodology.

A. Dynamic Task Planning Module Implementation

We define a timeline-based representation of the task planning problem for the case study. Fig. 2 shows an overview of the task planning model for the collaborative process modeled according to a Hierarchical Task Analysis [START_REF] Marvel | Characterizing task-based humanrobot collaboration safety in manufacturing[END_REF]. A production goal level represents the high-level goals "triggering" the execution of a collaborative production process. A production process level represents the different collaborative processes that can achieve production goals. This level models the tasks to perform and the operational constraints to satisfy in order to successfully achieve a particular production goal. Finally, a human/robot behavior layer models the low-level tasks the two actors can simultaneously perform to actually realize the production tasks.

According to [START_REF] Mayer | Planning and execution with flexible timelines: a formal account[END_REF], the dynamics of these levels are describes by means of a number of state variables. A state variable SV G = V G , T G , D G , γ G models the production goal level by representing production goals that can be performed into a production environment (e.g., the value

v i = DoM osaic ∈ V G ). A state variable SV M =
V M , T M , D M , γ M models the high-level tasks that should be performed to carry out a particular production goal of the goal level (i.e., values v i ∈ V G ). Each value represents a (collaborative) high-level task to be executed in order to successfully carry out v j ∈ V M = {DoRow 1 , ..., DoRow 5 } representing the tasks of assemblying the rows of the mosaic). Synchronization rules link production goals (e.g., v i = DoM osaic ∈ V G ) to the underlying production tasks (v j = DoRow j ∈ V M ) and specify a number of temporal relations between these values, representing desired operational constraints. Equation (1) shows an example of such a rule specifying a number of precedence relations between high-level high-level tasks v j = DoRow J ∈ V M :

a0[SV G = DoM osaic] → ∃ a1[SV M = DoRow1], ..., a5[SV M = DoRow5].C (1)
where

C = DoRow1 ≺ DoRow3 ∧ ... ∧ DoRow4 ≺ DoRow5.
Then, two additional state variables

SV H = V H , T H , D H , γ H and SV R = V R , T R , D R , γ
R characterize the behavior level and model respectively the low-level tasks the human and the robot can perform to actually carry out a collaborative process. In this specific case, the human and the robot can perform the same type of task P ickP lace x . A task P ickP lace x consists in picking a tile x of the mosaic from a box (i.e., white box, blue box and orange box) and place the tile x in the correct position (e.g., A1, B2 or G3). Values v k = P ickP lace k ∈ V H and v t = P ickP lace t ∈ V R represent the P ickP lace x low-level tasks the human and the robot can actually perform, according to specific production needs of the case study. The state variable of the robot behavior SV R would not contain P ickP lace x tasks concerning white boxes. Vice versa, the state variable of the human behavior SV H would not contain P ickP lace x tasks concerning orange boxes. For example the task P ickP lace x with x = A1 can be performed only by the robot as A1 is an "orange tile", while the task P ickP lace x with x = I3 can be performed only by the human as I3 is "white tile". Tasks concerning blue tiles instead (e.g., P ickP lace D3 ) can be performed by both and thus are contained in both state variables

(P ickP lace D3 ∈ V R ∩ V H ).
Another set of synchronization rules links high-level tasks in SV M to low-level tasks in SV H and SV R . These rules model possible allocations of low-level tasks (i.e., P ickP lace x concerning blue tiles) and thus define different ways of performing high-level tasks. Equation (2) and Equation [START_REF] Pellegrinelli | Multi-robot spot-welding cells for car-body assembly: Design and motion planning[END_REF] show two examples of synchronization rules describing two alternative ways of performing high-level task v j = DoRow 1 ∈ V M . Specifically these rules show two alternative assignments of low-level tasks P ickP lace x to the human and to the robot (Equation ( 2) assigns the low-level task P ickP lace J1 to the robot while Equation ( 3) assigns the same task to the human).

a0[SV M = DoRow1] → ∃ a1[SV R = P ickP lace A1 ], ..., a5[SV H = P ickP lace E1 ], ..., a10[SV R = P ickP lace J1 ].Cr1,a (2)
a0[SV M = DoRow1] → ∃ a1[SV R = P ickP lace A1 ], ..., a5[SV H = P ickP lace E1 ], ..., a10[SV H = P ickP lace J1 ].C r1,b (3)
In general rules with the same head (i.e., the left side of the rule) represent alternative ways of decomposing a task and are therefore treated as disjunctions during plan synthesis. In this case, they are used to model possible assignment of P ickP lace x concerning blue tiles.

Controllability information characterizes task execution with respect to the uncontrollable dynamics of the environment. The controllability tagging function of a state variable SV i specifies whether a value v j ∈ V i is controllable or not. We distinguish among controllable, partially-controllable and uncontrollable values (γ i : V i → {c, pc, u}). Controllable values are completely under the control of the system (it can decide both the start and the end of their execution). Partiallycontrollable values can only be started by the system while their end (i.e., the actual duration of their execution) can only be observed during execution. Uncontrollable values can neither be started nor ended by the system and their actual behavior can only be observed.

The behavior of the human is modeled as uncontrollable.

Values of v k ∈ V H are tagged as uncontrollable (γ H (v k ) = u, ∀ v k ∈ V H ). Robot behavior is modeled as partially controllable. Values v t ∈ V R are thus tagged as partially- controllable (γ R (v t ) = pc, ∀ v t ∈ V R ).
Values of other state variables are modeled as controllable. Controllability information is used to analyze and evaluate execution properties of plans [START_REF] Morris | Dynamic Control of Plans With Temporal Uncertainty[END_REF]. Therefore, PLATINUm synthesizes flexible plans that do not make assumptions on the actual duration of not controllable values/tasks (i.e., pseudo-controllable plans [START_REF] Morris | Dynamic Control of Plans With Temporal Uncertainty[END_REF]) assuring a good level of robustness and limiting the need for task re-planning at execution time [START_REF] Umbrico | Evaluating robustness of an acting framework over temporally uncertain domains[END_REF].

B. Action Planning Module Implementation

The general structure of the Action Planner module described in Section II-B is implemented here for the case study. The general description of Fig. 2 is therefore tailored to the case study tasks and actions. Each pick-place operation is an action planning instance (yellow nodes in Fig. 2). A pick-place is then composed of several motion instances (green nodes in Fig. 2), namely: move to X, approach object, grasp object, move to Y, approach slot, release object. X and Y are symbolic labels that identify the color (orange, white, blue) of the picking cube and the coordinates of placing spots (A1, A2, etc.) respectively.

When an action from the task planning arrives (e.g., P ickP lace J1 ), the motion planning module decodes the desired color and chooses the most suitable cube from the available ones. Then, it places the cube in the corresponding slot. To select the best action plan, a picking server and a packaging server are defined for each robot. The picking server manages multiple inbound stations, each station is a logical box containing objects of multiple types (i.e., colors). When the picking server receives the request to pick an object of a certain type, it checks how many inbound stations contain the requested object type. These stations are the goals of the path planning queries. The picking server executes the movement to the closest inbound station and, simultaneously, it runs another path planning query by setting as goals the poses corresponding to the current placing slot. After the execution of the approaching movement to the selected object, the picking server executes the grasp action, such as closing the fingers or activating the suction cup. Finally, the picking server plans and executes the removal movement. The packaging server manages the filling of the mosaic. It initially plans a path to the approaching position of the specified slot. During the execution of the movement, the algorithm plan the movement to the placing slot. After that, the release action is executed, for example opening the gripper fingers or switching off the vacuum generator. It is worth noticing that both the picking server and the packaging server can manage multiple robots acting simultaneously in the cell.

C. Software Implementation

The mock-up scenario shown in Fig. 4 is a simulation environment built in ROS. It is composed of an Universal Robot UR5 mounted on an linear axis. The mosaic should be assembled on a worktable parallel to the linear axis. The blue pieces are on a worktable (accessible to both the robot and the operator), the orange ones behind the robot, and white ones on a table near the operator. A human mannequin is integrated in the simulator 1 .

The Action Planner is implemented as a set of ROS action servers dedicated to pick or place tasks. Each action server implements a MoveIt! planner pipeline, allowing the use of all path planning algorithms available in ROS (e.g., OMPL [START_REF] S ¸ucan | The Open Motion Planning Library[END_REF] and STOMP [START_REF] Kalakrishnan | STOMP: Stochastic trajectory optimization for motion planning[END_REF]). The Task Planner is implemented through PLATINUm (https://github.com/pstlab/PLATINUm) implementing a classical plan refinement solving procedure which consists in iterative refining the timelines of a plan by solving flaws concerning the completeness and the temporal validity of a plan (i.e., P&S flaws). Also, PLATINUm integrates a pseudo-controllability check analyzing the duration of 1 The modeled movements for the mannequin are: trunk (2 translations and 3 rotations), shoulders (3 rotations each), elbow (1 rotation each), wrist (3 rotations each). partially-controllable or uncontrollable values of the timelines. The solving procedure is completed when no flaws are found and a complete and valid partially-controllable set of timelines is synthesized. In this experiment, we ran PLATINUm using a hierarchy-based heuristics for flaw selection and a HRbalancing search strategy for search space expansion. The hierarchy-based heuristics is domain independent. It infers a hierarchy among the detected flaws of a timeline-based plan by using a topological sort algorithm on an acyclic graphbased representation of their dependencies (extracted from the synchronization rules of the model). According to this hierarchy, the algorithm randomly selects one of the flaws belonging to the equivalent set at the highest level of the hierarchy (i.e., the "most independent flaws") [START_REF] Umbrico | Enriching a temporal planner with resources and a hierarchy-based heuristic[END_REF].

The HR-balancing search strategy is specifically designed for HRC planning problems. This search strategy analyzes the partial plans of the search space and selects the ones that better balance the work-load between human and robot.

The communication between the planners is schematized in Fig. 5. One ROS-node for each agent (human node and robot node in Fig. 5) converts the task Planner's functional goals into motion instances of the motion planner algorithm. One request topic and one feedback topic are defined for each agent node. The Task Planner publishes messages on the request topics to request the execution of a task and receives messages from the feedback topics to get the outcome (success/failure) of tasks. Human tasks are not controllable according to Section III-A: their duration is not ensured to be finite, as well as the time from request to beginning (such time might even be infinite if the operator refuses to perform the given task). The ROS package rosbridge suite was used to set up the communication between PLATINUm and ROS.

The feedback from the user have been simulated through the deployment of a simple random-delay trigger that sends the feedback to the Task Planner after the human task is completed. To simulate the effects of safety functions on the robot motion (i.e., robot slowdown/stop when the human enters the collaborative space), we have implemented a Speed and Separation Monitoring function, according to ISO/TS 15066 [START_REF]:2016 Robots and robotic devices -Collaborative robots[END_REF] using a simplified safety-zone approach that reduces the robot speed to: i) 50% of the nominal speed when the distance from the human to the worktable is less than 1 m; ii) 30% of the nominal speed when the distance is less than 0.5 m; iii) 0% of the nominal speed (i.e., stop of the robot) when the human is working on the worktable.

D. Use Case Simulation

Fig. 6 shows a Gantt representation of a timeline-based plan synthesized of the collaborative mosaic. A timeline-based plan encapsulates an envelope of possible temporal behaviors as valid solutions to a planning problem. The Gantt shows a single particular solution we refer to as the earliest start time solution. The task planner takes about 20 seconds for the synthesis of this plan which represents the time spent to generate a task plan of the overall collaborative process to build the mosaic. The collaborative mosaic indeed requires the execution of a total number of 50 P ickP lace tasks. If we consider an average duration of 45 seconds for task, the total execution time of the overall process would be 38 minutes in the worst case (all tasks assigned to the human or to the robot) and 20 minutes in the best case (perfect balancing between human and operator, without physical disturbance).

As can be seen in the Gantt, plans synthesized by the task planner uniformly distribute the tasks between the human and the robot. The resulting plans indeed have an average makespan of 52 time units with 26 P ickP lace tasks assigned to the human and 24 P ickP lace tasks assigned to the robot. Such plans show the efficacy of the task planner in synthesizing suitable and effective collaborations with reasoning time that complies with production latency. When the plan is ready, the communication between the task planner and the motion planning framework starts. The task planner sends the robot current task to robot node and the human current task to human node, then it waits for the feedback message of each task. Feedback messages communicate the outcome of the task. If the task was not achieved, is refused by the operator, or exceeds its maximum duration, task planner detects that the plan is not valid anymore and re-plans the remainder of the process; otherwise, the plan execution continues unaltered. Several phases of the casestudy simulation are shown in Fig. 7. The progress of the mosaic assembly is shown from 0% (Fig. 7a) to 100% (Fig. 7f). For example, in Fig. 7a, the execution has just started; while the human is placing a blue cube, the robot is still because of the safety functions that reduce the robot speed based on the proximity to the human. In Fig. 7e, the human waits for the robot to finish its task, while the robot is picking the last cube of Row 4. Fig. 7f shows the final mosaic.

IV. CONCLUSIONS AND FUTURE WORKS

The paper proposes a control-based approach based on two layers (task and action planning) where each layer reasons at a different level of abstraction. This constitutes a hierarchical framework implementing a control loop that leverages feedback to monitor the execution and dynamically (re)optimize the process plan. Moreover, operators can accept/discard commands (via HMI) and give feedback about the outcome of his/her tasks. The method is applied to an industrial case study in which a robot and a human worker cooperate to assemble a mosaic. The main current limitation is related to the need of an a-priori estimation of the sub-tasks duration. This estimation is often inaccurate (e.g., it is hard to account for robot safety holds/stops). Despite the timelinebased modelling deals with temporal variability, larger error in the time estimation weakens the robustness of the plan, and the optimality is hard to be guaranteed. Future works will focus on including a learning phase to produce a realistic estimation of the sub-task durations. Moreover, experiments in a real setting are necessary to assess user dependability, investigate scalability and extensibility of the approach and to compare its performance against industrial best practices.

Fig. 1 :

 1 Fig. 1: Task and motion planning integration overview.

Fig. 2 :

 2 Fig. 2: Hierarchical model of the case study.

Fig. 3 :

 3 Fig.3:A collaborative mosaic to be assembled. Orange tiles can be handled only by the operator, white tiles can be moved only by the robot, and blue tiles can be moved by both.

Fig. 4 :Fig. 5 :

 45 Fig.4: A 7-degree-of-freedom robot and an operator collaborate to assemble a mosaic on the worktable; cubes of different colors are placed near the agents allowed to pick them.

Fig. 6 :

 6 Fig. 6: Plan generated for the collaborative mosaic

Fig. 7 :

 7 Fig. 7: Assembly of the cooperative mosaic during the simulation.

Robot Action Level Human Action Level Idle Action x Action y Idle Action z Action t … … Production Goal Level Goal i Goal k Idle Goal j … CONTAINS CONTAINS CONTAINS Production Process Level Idle Process 1 Process 5 Process 3 … … BEFORE CONTAINS CONTAINS Robot Motion Level Human Machine Interface Move B Move A Move C

ACKNOWLEDGMENT

This work is partially supported by ShareWork project (H2020, European Commission -G.A. 820807). Alessandro Umbrico is also partially supported by ROXANNE project (H2020, ROSIN -G.A. 732287).


Manuel Beschi
email: manuel.beschi@unibs.it

Marco Faroni
email: marco.faroni@stiima.cnr.it

Cosmin Copot
email: cosmin.copot@uantwerpen.be

Nicola Pedrocchi
email: nicola.pedrocchi@stiima.cnr.it

How motion planning affects human factors in human-robot collaboration

Keywords: human-aware motion planning, human factors, human-robot interaction, ergonomics

Dependability of robot co-workers plays an important role in increasing the effectiveness of human-robot interaction in manufacturing. Our goal is to understand the role of motion planning parameters in human-robot collaboration and to provide guidelines for the selection of the most suitable motion planner. The human factors analysis provided in this paper highlights that repeatability of the motion and predictability of the robot timing affect the quality of human-robot collaboration.

INTRODUCTION

Human factors in industrial robotic applications and their consequences on the work organization have been studied in academic and industrial research for decades. Pioneering studies were proposed in the eighties [START_REF] Panny | Consequences of Industrial Robots in the Field of Work Organization[END_REF][START_REF] Nagamachi | Human factors of industrial robots and robot safety management in Japan[END_REF]. In recent years, efforts were made to improve human-robot interaction in the increasingly widespread collaborative cells [START_REF] Oliff | A Framework of Integrating Knowledge of Human Factors to Facilitate HMI and Collaboration in Intelligent Manufacturing[END_REF][START_REF] Bolmsjö | Supporting Tools for Operator in Robot Collaborative Mode[END_REF]. Many researchers addressed the implications of human factors on safety aspects in industrial scenarios [START_REF] Aaltonen | Refining levels of collaboration to support the design and evaluation of human-robot interaction in the manufacturing industry[END_REF][START_REF] Behrens | Upcoming Technologies and Fundamentals for Safeguarding All Forms of Human-Robot Collaboration[END_REF][START_REF] Robla-Gomez | Working Together: A Review on Safe Human-Robot Collaboration in Industrial Environments[END_REF][START_REF] Freedy | Measurement of trust in human-robot collaboration[END_REF], and their deployment [START_REF] Saenz | Survey of methods for design of collaborative robotics applications-Why safety is a barrier to more widespread robotics uptake[END_REF]. A roadmap for the successful implementation of human factors in industrial human-robot collaboration is proposed in [START_REF] Charalambous | Identifying the key organisational human factors for introducing human-robot collaboration in industry: an exploratory study[END_REF]. [START_REF] Hoffman | Evaluating Fluency in Human-Robot Collaboration[END_REF] proposed metrics to evaluate fluency in human-robot collaborative scenarios, measuring the effectiveness of the human and the robot to work as a team.

A few studies pointed out the influence of motion planning on human acceptance, trust, and performance. In [START_REF] Koppenborg | Effects of movement speed and predictability in human-robot collaboration[END_REF], a virtual reality experimental campaign has been performed to understand how the robot movements impact the human perception. The results indicate that higher movement speed and lower predictability leads to higher risk perception, anxiety, workload, and

The research leading to these results was partially funded by the European Union H2020-ICT-2017-1 -Pickplace: Flexible, safe and dependable robotic part handling in industrial environment (grant agreement: 780488).

a tendency toward lower task performance. In [START_REF] Lasota | Analyzing the effects of human-aware motion planning on close-proximity human-robot collaboration[END_REF], human-aware motion planners -namely motion planners that compute the path considering the knowledge of the next human task -show their effectiveness in enhancing the collaboration, reducing idle times and increasing the user's satisfaction.

These aspects are even more important in unstructured scenarios, where it is not possible to use pre-computed trajectories but the motion planner has to plan the movement on the fly. Consider for example, a collaborative scenario where the human and the robot have to pick some components from a table to assemble them and the position of the pieces is not known a priori. The robot will estimate the position of the piece just before picking it (for example, by means of a vision system) and will compute a trajectory to reach the grasping point on the fly. Similarly, the robot will compute also the trajectory to assemble the piece online. Robotic applications where online motion planning is strictly necessary are more and more widespread, especially in view of the intrinsic dynamic nature of human workers and the advances in AI reasoning that allows the robot to cope with less and less structured environments [START_REF] Karpas | Automated planning for robotics[END_REF].

In these applications, the motion planning module is key. Given a start and a goal point, the motion planning problem consists in finding a collision-free trajectory that connects the two points. The problem admits an infinite number of solutions: there are usually infinite paths that connect the two points in the space and every path can be traveled with an infinite number of velocity profiles.

Motion planning is one of the fundamental problems in robotics and a huge variety of methods have been proposed during the years. Roughly speaking, the motion planning problem can be approached through deterministic or stochastic algorithms. Under the same conditions, deterministic algorithms always find the same solution; on the contrary, stochastic planners are not repeatable. Even though stochastic planners are less predictable than deterministic ones, their effectiveness in solving the planning problem in presence of complex cells makes them the defacto standard in unstructured scenarios [START_REF] Lavalle | Planning Algorithms[END_REF][START_REF] Adiyatov | A novel RRT*based algorithm for motion planning in Dynamic environments[END_REF].

To the best authors' knowledge, there is no literature providing guidelines to select and tune motion planners to cope with human factors, concerning aspects like kinematics limits and path repeatability. The motion planner should be chosen also keeping in mind questions like: "is the human more bothered by spatial variability or temporal variability of the trajectories?"; "is the human bothered by acceleration changes and/or velocity changes?","is the synchronization of the movements of human and robot important?". This paper is a first attempt to investigate the impact of motion planning parameters on human perception, intending to help in the choice of the most suitable planner and to boost further research in human-centered motion planning. To this purpose, two experiments are carried out and evaluated by means of questionnaires and objective data (namely, variation of the human's cycle time and centroid motion due to the robot presence). Results show that motion readability strongly affects the human trust in the robot co-worker. On the contrary, differences in the velocity profile (e.g. higher or lower speed and acceleration) do not seem to bother the user as long as the robot timing is predictable. In non-cooperative tasks, it turned out that the robot cycle time influences the user behavior, as the human worker is apt to keep pace with the robot. In cooperative tasks, the human worker (especially if familiar with robots) tends to overestimate the distance from the robot, and this may pose safety hazards if the robot safety controller does not react promptly.

The paper is organized as follows: Section 2 describes the setup used in the experimental campaign. The first experiment on the execution of unsynchronized tasks in the same workspace is discussed in Section 3.1. Section 3.2 describes a scenario where the human and the robot tasks need to be synchronized. Discussion on the results and their influence on the selection and tuning of motion planning are highlighted in Section 4

MATERIALS AND METHODS

The hardware and software platform used for the experiments are described in Subsection 2.1. Two different experiments were designed to analyze different levels of collaboration. In the first experiment, the robot and the human execute independent tasks in the same area, while in the second experiment their tasks have casual relations and require synchronization. The experiments are described in Subsection 2.2. The evaluation procedure is given in Subsection 2.3. The tests involved 32 participants: 28 males and 4 females; age: 28.3±8.6 years; 24 were familiar with robots (>6 months of training), 8 were not.

Hardware and software

The human-factors campaign was carried out through the re-manufacturing pilot plant installed at CNR-STIIMA [START_REF] Brusaferri | Integrated automation system with PSO based scheduling for PCB remanufacturing plants[END_REF]. The experimental hardware, shown in Figure 1, is composed of:

• A collaborative robot, Universal Robots UR10 version 3.5, controlled via Robotic Operating System (ROS) [START_REF] Quigley | ROS: an open-source Robot Operating System[END_REF];

• A two-finger gripper, model Robotiq 2-Finger 85mm, controlled via ROS; • A time-of-flight camera, model Swiss Ranger 4000,
that publishes a point cloud on a ROS topic. The point-cloud generated by the camera was filtered by using the PCL (point cloud library) in order to find the centroid of the human.

Description of experiments

Experiment 1: unsynchronized tasks

The goals of the first test are: understanding the influence of path changes on human behavior in workspace sharing applications; understanding the influence of velocity and acceleration changes on human behavior in workspace sharing applications; understanding if the human synchronizes her/his pace with the robot one.

In the experiment, the user was asked to unscrew some screws from a metallic item. The used had to place Fig. 2. Experiment 1 layout. The blue circle is the disassembly station; red circle is the screw buffer the screw in a buffer immediately after the unscrewing operation. Figure 2 shows the working area. The path between the disassembly area and the screw buffer are inside the robot workspace but it does not intersect any robot trajectories.

The human was asked to perform the task twice. One without any robot movements, which is considered as the baseline, and one with the robot execute pick-and-place tasks. The order of the two experiments has been chosen randomly to avoid polarization.

When the robot was moving, it executed a pick-and-place task twelve times; three of them present differences with respect to the other ones. These variations are randomly selected among the following types:

• M-A: the operator unscrews the first piece without robot movements, and then it repeats the disassembly with the robot will change path without changes on the limits on velocity and acceleration; • A-M: as M-A but the first disassembly is performed when the robot is moving, while the robot is stopped in the second one; • M-B: the operator unscrews the first piece without robot movements, and then it repeats the disassembly with the robot will change on the limits on velocity and acceleration without changing the path; • B-M: as M-B but the first disassembly is performed when the robot is moving, while the robot is stopped in the second one.

Experiment 2: synchronized tasks

The second experiment deals with a task that requires a synchronization between robot and human tasks. The goals are: understanding how the operator is influenced by changes on speed, acceleration, and execution time; understanding how the operator perceives the humanrobot cooperation when he/she has to synchronize his/her activities with the robot.

The user is asked to bring to the robot a cylinder and 10 screws. The tape and the screws are located on the worktable, therefore the human has to pick one or more of them and walk to the robot, as shown in Figure 3. The robot performs a picking movement and closes the gripper finger to grasp the object, then it will place the object in the buffer. These movements can be triggered by the human position (Cases A, B, and C) or by time (Case D). The human is asked to bring the screws to the robot to place them all inside the cylinder; the user is informed that the number of screws inside the circle will be the evaluation score of his/her experiment. However, this task is introduced only to focus the user's attention on a practical task instead of the robot movement. There are four different variants of the experiments:

• A: the first five pick-and-place operations have high acceleration and low velocity; the last five have low acceleration and high velocity. The pick-and-place time is the same in all 10 operations. Each operation starts only if the human is entering the picking area. • B: the first five pick-and-place operations have low acceleration and high velocity; the last five have high acceleration and low velocity. The pick-and-place time is the same in all 10 operations. Each operation starts only if the human is entering the picking area. • C: the first five pick-and-place operations have high acceleration and low velocity; the last five have low acceleration and low velocity. The pick-and-place time is higher during the first five operations. Each operation starts only if the human is entering the picking area. • D: the first five pick-and-place operations have high acceleration and low velocity; the last five have low acceleration and high velocity. The pick-and-place time is equal in all 10 operations. The last seven operations start without considering operator position.

Evaluation procedure

The human factor evaluation is based on objective and subjective measures. Subjective measures come from a questionnaire that each subject had to answer after the experiment. The questionnaires aim to understand what are the motion parameter that affect the human reaction the most. In particular, the questionnaire of Experiment 1 investigates the level of danger and interference perceived by the subject when working with the robot. The questionnaire of Experiment 2 investigates whether the subjects noticed changes in the motion planning configuration and which configuration they felt more comfortable with. Objective measures only regarded Experiment 1. In particular, the required time for disassembling was measured, and the trajectory of subject's centroid was recorded. Based on these data, we were able to evaluate variations of the cycle time and of the human motion due to the different motion planning configurations as follows:

• Cycle time: the ratio between the time required to complete the experiment with and without the robot was computed; this should verify whether the human productivity is affected by the robot movement also during unsynchronized tasks. • Human motion: the distance between the human and the robot was computed; this should verify whether the human motion is affected by the robot movement also during unsynchronized tasks.

RESULTS

Experiment 1 : unsynchronized tasks

The questionnaire provided to the subjects is shown in Table 1. Results are summarized in Table 2 and discussed hereafter.

Q1: Did Robot perform movements that you feel dangerous? The user declares if the robot performs dangerous movements. 19% of users answer Yes, it is important to remark the presence of a strong correlation between the user's experience level and the danger perception. No one with more than 6-month experience with robot considers the robot movements dangerous. Another important consideration is the absence of correlation between the experiment type (i.e., variations on path or kinematic constraints) and the danger perception.

Q2: Has the robot hindered your activities?

The user declares if the robot hinders the user activities. 71.8% Fig. 4. Ratio between the time required to complete the experiment with and without the robot.

of the answers were negative. In this case, there is a significant correlation with the experiment type. Path changing hinders the user activities more than twice the velocity/acceleration changes.

Q3: Was the robot far from you?

The user declares if the robot was far or close in his/her opinion. As in Question Q1, the user experience is more important than the experiment type, even if in M-A and A-M experiments the robot is considered a little more closed. It is important to notice that the minimum distance during the experiments between the robot gripper and the user was around 40 cm, thus it is possible to assert that the human overestimate robot distance during tasks.

As regards objective measures, variations of the cycle time and of the human motion were analyzed. The ratio between the time required to complete the experiment with and without the robot is shown in Figure 4. As expected, the disassembly time is slower during the second disassembly due to the gained experience. However, it is worth stressing that in M-A and A-M the ratio is inversely proportional to the sequence, while in M-B and B-M the human is slower than in M-A and A-M. Considering that the robot is moving slower in experiments M-B and B-M with respect to M-A and A-M cases, it is reasonable to assert a correlation between the robot and the operator throughput also in tasks that do not require synchronization. Regarding influences on the human motion, no significant changes have been highlighted in both the experiment types. This aspect is consistent with the overestimation of the relative distance (Q3) and the absence of overlapping between human and robot paths.

Summary Experimental results highlight the path changing influences more the operator's attention with respect to speed/acceleration changes. However, human does not change significantly his/her path based on robot behavior. Conversely, Human throughput is influenced by robot throughput. Humans overestimate robot distances, especially during predictable movements. Human's path is not strongly influenced by the robot movements if the robot path does not intercept operator's trajectory.

Experiment 2 -synchronized tasks

The questionnaire provided to the subjects is shown in Table 3. Results are summarized in Table 4 and discussed hereafter.

Q1: Did the robot change speed during the test? The question allows understanding if the user perceives the speed/acceleration changes during the experiments. In experiments A, B, and D, 75% of the users did not recognize speed/acceleration changes. In these experiments, the execution time is constant while limits on acceleration and velocity are changed. In experiment C, all the users perceive the speed change.

Q2: If it changed velocity, which case did you prefer? In experiment C, all the users identify that the robot velocity was slow down during the experiment. In experiments A, B, and D, the users were able to detect the speed/acceleration changes preferred the lowacceleration/high-velocity trajectories.

Q3: Did you feel like the robot was cooperating with you? This question allows understanding if the operator thinks to cooperate with the robot even if he/she is not dictating the pace. In experiments A, B, and C the robot starts the pick-and-place movement only if the operator is in the picking area. Thus, the operator can decide his/her velocity during the movements from/to the working table. Almost all the users (91%) consider the robot to be cooperating. In experiment D, the last 7 pick-and-place movements respect a fixed time schedule therefore the operator has to accommodate his/her velocity to the robot velocity. The majority of the user (75%) consider the robot to be non-cooperating.

Summary Considering operator feelings, trajectory time is more important than speed and accelerations. Different motion profiles with the same total time are difficult to recognize when the operator focuses his/her attention on the task. Humans consider the robot more collaborative when they can decide the cooperation activity timing.

DISCUSSION

Experiments show that it is very important to compute motion paths that are predictable by the workers, moreover also the execution time should be predictable as much as possible. This aspect is very important in the selection of the motion planner. As a matter of fact, nonoptimal stochastic motion planners, like RRT [START_REF] Lavalle | Rapidly-Exploring Random Trees: A New Tool for Path Planning[END_REF][START_REF] Kuffner | RRT-connect: an efficient approach to single-query path planning[END_REF], are very effective in finding a solution in a limited amount of time, but they are characterized by a large variability of the solution path. [START_REF] Jaillet | Sampling-Based Path Planning on Configuration-Space Costmaps[END_REF][START_REF] Devaurs | Enhancing the transition-based RRT to deal with complex cost spaces[END_REF] proposed an improvements on the basic RRT algorithm using transitionbased sampling. The algorithm improves the solution quality with respect to basic RRT implementation, leading to more repeatable paths and execution times.

Optimal motion planners should provide a unique solution to the planning problem as shown in [START_REF] Luo | An empirical study of optimal motion planning[END_REF]) using a simple benchmark, but they still own a certain level of variability due to limited computational time dictated by the application. To overcome this limitation, informed sampling techniques have been proposed to speed-up the solver [START_REF] Gammell | Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic[END_REF][START_REF] Choudhury | Regionally accelerated batch informed trees (RABIT*): A framework to integrate local information into optimal path planning[END_REF].

Considering the experimental results, optimal motion planners with limited computational time should be preferred in human-robot interaction, due their ability to compute repeatable and predictable solutions. If computational time does not fulfill the application requirements, improved RRT algorithms able to improve solution quality has to be preferred.

When focused on his/her activities, the user seems to be unable to distinguish different velocity profiles (namely, different limits of velocity and acceleration) if the total cycle time is the same. From the human point of view, it is important to execute the movement in a predictable amount of time allowing the human to synchronize its activity. The choice of the velocity profile is therefore less crucial in terms of motion dependability, as long as the cycle time is predictable by the user.

It is also worth stressing that the robot throughput resulted to influence the human behavior also without any need for synchronization of the task.

In cooperative tasks, operators prefer to decide the task timing and, when focused on his/her activities, the operator overestimates the distance of the robot. These considerations imply that the motion planner has to take care of maintaining the appropriate distance to avoid the need for a significant slowdown of the robot velocity during the path.

Fig. 1 .

 1 Fig. 1. Re-manufacturing pilot plant with the collaborative robot and the human-tracking system.

Fig. 3 .

 3 Fig. 3. Experiment 2 setup. Blue circle: worktable. Red circle: Picking area

Table 1 .

 1 Questionnaire of Experiment 1

Q1: Did the robot perform movements that you felt dangerous?

Yes. No. Q2: Has the robot hindered your activities?

Yes. No. Q3: Was the robot far from you?

Yes. No.

Table 2 .

 2 Questionnaire results for the first Experiment.

Table 3 .

 3 Questionnaire of Experiment 2

	Q1: Did the robot change speed during the test?
	Yes. No.
	Q2: If it changed speed, which case did you prefer?
	lower acceleration, higher velocity.
	higher acceleration, lower velocity.
	Q3: Did you feel like the robot was cooperating with you?
	Yes. No.

Table 4 .

 4 Questionnaire results for the second experiment.

	Q1: Did the robot change speed during the test?
			yes	no
		Type A	25%	75%
		Type B	21.8% 78.2%
		Type C	81.2% 18.8%
		Type D 28.1% 71.9%
	Q2: If it changed velocity, which did you prefer?
		lower acc., higher speed higher acc., lower speed
	Type A	100%		0%
	Type B	100%		0%
	Type C	85%		15%
	Type D	100%		0%
	Q3: Did you feel like the robot was cooperating with you?
			yes	no
		Type A	100%	0%
		Type B	100%	0%
		Type C	43.7% 56.3%
		Type D	100%	0%

ACKNOWLEDGEMENTS

The research leading to these results was partially funded by the European Union H2020-ICT-2017-1 -Pickplace: Flexible, safe and dependable robotic part handling in industrial environment (grant agreement: 780488).