"planningScene_monitor" (in MoveIt).

Later based on this information and the desired goal, the "move_group" (in MoveIt) can generate a plan for the robot to reach the desired positions while knowing the current states of obstacles in the scene. Once the plan is generated, we communicate to the UR-5 robot by using the *ur_modern_driver* [131] (this driver is currently deprecated, but given the UR-5 software version available at the lab, is the one that has to be used). With it, we can move the UR-5 robot with ROS control, and send as output the current joint states of the robot to Unity system to be used for visualization of robot in the virtual environment, if required.

### 4.2.1 MoveIt

MoveIt [27] is one of the most known and used software for robot manipulation, used on over 150 robots. It is an open-source software allowing the development of projects in industrial, commercial, and research environments for free.

It counts with a user-friendly platform for building flexible industrial, research, and commercial applications. It allows the configuration, programming, and definition of different robot models, allowing to develop and define the environment, objects and plan different tasks where path planning is required. It allows to perform and develop complex applications while being able to determine and study the interactions of the system by incorporating the latest advances in motion planning, manipulation, 3D perception, kinematics, control, and navigation.

The MoveIt architecture is based on two main nodes, the "move_group" node, and the "planningScene" (Figure4.1). The "move_group" takes care of obtaining the parameters, the setup, and the individual components of the robot model being used, so it can provide to the user ROS services and actions for the users to use on the robot.

The second element which is the "planningScene" uses the "planningScene_monitor" to handle the scene in which the robot will be included. This is what will constitute the obstacles or elements the robot has to interact with. Providing information about where they are located, and any updates that happen in the robot's surroundings. It uses as well the robot interface data to make the connection between the data being processed and the objects defined in the scene.

A detailed explanation of the components and functionality of MoveIt can be found in Appendix C

## 4.3 Implementing laboratory setup based on proposed architecture

The laboratory setup (Figure 3.1) is conformed by the UR-5 robot system and a vehicle chair, in a face-to-face configuration. Where the robot's location and height have been determined in Chapter 2 to be 80 cm above the ground. Being a position optimal enough so the robot is able to reach all the interaction points where the system has an interest in reaching. For the user (Figure 3.13), the VR helmet and trackers are attached to the body, in the humerus, and in the palms so the data can be obtained and place the user's location within the VR environment.

### 4.3.1 Requirement

In this scenario, a relation between the real world and the simulated environment in ROS has to be established using the vive trackers used by the user. Additionally we want the end-effector
<figure><img src="image_105.png" /><figcaption>Figure 4.2 – The system setup in the physical environment</figcaption></figure>

of the robot to follow the user's hand and contact it when necessary. To achieve this, we need
good movement planning. We also want no part of the robot to come into contact with the user
during movement tracking.

4.3.2 Methodology for calibration of real and virtual environments

The ICI system can be divided into two physical components shown in Figure 4.2, the robot and chair. Each component has a vive-tracker to localize between them. These components need to be calibrated to synchronize with the virtual and the real physical environment, this is achieved by using trackers. For this purpose, we consider some information, shown in Figure 4.3 we can see two trackers ($V_b$ and $V_e$) and two robot frames ($R_b$ and $R_e$) that need to be calibrated to sync the environments. The physical robot has base frame $R_b$ and tool frame $R_e$, by attaching a vive-tracker at the tool end and assuming that the frames coincide ($V_e$ and $R_e$), we calibrate the base tracker $V_b$, by assigning a translation in x (between $V_b$ and $R_b$).

To achieve the requirements of safety in the present scenario, a virtual plan is introduced that prevents the robot from crossing the interior of the car. The user and robot are located on the opposite side of the plane. This virtual plan is defined in relation to the virtual car surfaces. The user's hand is equipped with a tracker, to track and follow the hand. In Figure 4.2 we can see a tracker on the chair. Two trackers are used to depicting two frames $V_c$(vive tracker on chair) and $V_h$ (Vive-tracker on users hand). One tracker is used to define the car/chair plane(to differentiate the interior and exterior of the car). The other tracker is used to track the user's hand. Once the calibration between the physical and virtual environment is done, the tracker at the robot tool end can be removed $V_e$.

The collection of information is achieved by using OpenVR libraries. The algorithm used for collecting the tracker information can be seen in Algorithm 3

Using the OpenVR library, the position and orientation of the tracker are known with respect to a vive-world frame. Both robot and vive systems have information that needs to be linked to perform hand tracking using the vive-trackers.

To establish connection we find the transformation, assuming $V_e$ and $R_e$ are superposed, compute from respective systems $T_{rb}^{re}$ and $T_{ve}^{vb}$ and compute Eq 4.1. Now the systems are
<figure><img src="image_106.png" /><figcaption>Figure 4.3 – Calibration setup of the physical components.</figcaption></figure>

**Algorithm 3** Tracker information in ROS.

**Input:** Vive tracker frames and Names.

**Output:** New frames of vive trackers wrt ROS frames.

1. Convert all trackers to appropriate coordinate system.

2. **function** VIVE Tracker.

3. triad_openvr.triad_openvr().

4. get_serial, device_name.

5. **if** device_name prefix LHR **then**

6. rotate 180 degrees in +Z axis.

7. **else**

8. broadcast frames to /tf time stamped.

9. (x,y,z), (qx,qy,qz,qw), time, device, vive_world

10. [parent: vive_world, child:device]

11. **end if**

12. **end function**
**Algorithm 6** Motion of robot based in hand position.

**Input:** Vive tracker frames and names.

**Output:** Robot Motion.

1. move robot to desired hand position.

2. **function ROBOT_MOTION**.

3. Initialize Robot

4. Initialize move group "manipulator"

5. Set velocity Scaling Factor = 0.1

6. move in joint space from home to desired

7. **if** Plan Success **then**

8. Execute

9. **else**

10. Exit

11. **end if**

12. **while** set rate **do**

13. **function** PROJECTION($T_h$)

14. **return** desired

15. **end function**

16. **function** MOVE_ROBOT(desired)

17. set first point in waypoint (get_currentpose())

18. set waypoint (desired)

19. $F = move_group.computeCartesianPath()$

20. **if** $F \times 100 = 100$ **then**

21. Execute

22. **else**

23. Nothing

24. **end if**

25. **end function**

26. **end while**

27. **end function**
<figure><img src="image_107.png" /><figcaption>Figure 4.4 – Representation of the user’s effective workspace as a plane</figcaption></figure>

## 4.4 Computation and trajectory planning

So far we have established the flow of data between the systems and how the environment in ROS is setup. Next is to explain the planning methods used as well as the different stages in which it was developed. Starting from the setup and configuration phase, where we establish the environment and defined the planning group. Afterward, tests were run to determine which one of the multiple path planning algorithms available within the MoveIt software behaved the best on some testing experiments.

Path planning (Figure 4.5) refers to the computation or generation of a geometric path, which connects an initial point to a final one, through in-between waypoints. Waypoints are meant to be followed in order to perform a desired task or motion. This geometric computation is based on the kinematic properties of the robot along with its geometry. On the simplest case, path planning need to be performed on static and known environments. However, this problem can also be generated for robotic systems subject to kinematic constraints in a dynamic environment, which are more complex to achieve.

In the different possible scenarios, path planning can be done using a previously known map, which is defined as global planning, and it is commonly used in determining the possible paths that can be followed in order to reach the final position. This is used for the case of a known and static environment, where the position of the present obstacles is already known, and a path is generated taking this into account. This can be performed off-line, as it is based on previously known information. In the case of possible changing environments, local path planning needs to be done, which relies on sensors or any other type of data-providing interfaces in order to obtain updated information about the robot’s surroundings. This can only be done in real-time, as it depends on dynamically changing environments.

### 4.4.1 Various path planning algorithms

The configuration space can be denoted as $C_{space}$ [83], where inside this space, the sub-spaces where the robot configuration collides with and obstacle, are called $C_{obs}$, and the space where
<figure><img src="image_108.png" /><figcaption>Figure 4.5 – Work logic of global [69] and local [72] path planning.</figcaption></figure>

it does not collide is called the free configuration space $C_{free}$.

There has been multiple proposals on path planning algorithms throughout the years, from where we can provide a review of the bases and workings for the most commonly found algorithms in the robotics literature, in [46] some of these algorithms are addressed.

## Artificial Potential Fields

The Artificial Potential Fields (APF) approach [72] introduced by O. Khatib in 1985 and further developed by [135] [136]. It represents the path planning algorithm as a problem where the robot is considered as a “moving ball” or point under the influence of potential fields which are artificially generated (thus the name) by the desired goal position and the obstacles within the $C_{space}$. In this case the desired goal generates an attractive potential and the different obstacles generate repulsive potentials, where the sum of the two contributions creates a potential field which can be translated into an artificial force that will lead the “ball” to go towards the goal, while avoiding the obstacles. The succeeding configurations of the robot can be determined given the direction of the slope of the force field which the robot is subjected to, this direction represents the most optimal path towards the goal. The force field can be expressed as:

$$U_{art}(q) = U_{goal}(q) + U_{obs}(q) \quad (4.2)$$

Where for a configuration $q$, the artificial potential field $U_{art}(q)$ is formed by the attractive potential generated by the goal $U_{goal}(q)$ and the repulsive potential of the obstacles $U_{obs}(q)$.

There are two main proposals in the literature to generate the attractive potential field of the goal in order to guide the motion in the $C_{space}$ towards it. These are the *paraboloidal field* and the *conical or linear field* approach, for a $C_{space} \in R^2$ (Figure 4.6).

However this method presents conflicting situations such as: a) trap situation due to local minima; b) oscillation in the presence of obstacle; c) no passage between close spaced obstacles; d) oscillations in narrow passages. Some solutions to the local minima problem has been pro-
<figure><img src="image_109.png" /><figcaption>Figure 4.6 – Representation of an Artificial Potential Fields.</figcaption></figure>

posed, like the usage of Harmonic Potential Functions [65], virtual local target [119] and others
such as [28] [110].

Due to the way that the environment is represented, one feature of this algorithm is that it is computationally inexpensive and has good reactivity to environment changes, taking into account new obstacles that come into the *C<sub>space</sub>* allowing it to have good real-time reactions to the environment.

Probabilistic Road-maps

The Probabilistic Road-maps approach [69] consists in generating random nodes in the con-
figuration space (C<sub>space</sub>) in order to generate a grid (so called, the road-map). The algorithm
(Figure 4.7) is divided into two phases, the learning stage, where the nodes are generated, and
the query phase where the generated nodes are then connected and an optimal path is found.
This connections are only possible if there are no objects present in between nodes, which will
lead to a road-map where all the connections are feasible paths for the robot to follow. This
algorithm is considered as a Sampling-Based planner, as it does not fully reconstruct the C-
space and its boundaries, but instead, checks if each sampled robot configuration is in collision
or not, in order to consider it as a valid node or not. Once a sufficient amount of nodes has
been generated in the C<sub>space</sub>, it then performs the connections between all the neighboring nodes
where no obstacle is present in between. This will generate the road-map, where multiple paths
can be followed between q<sub>init</sub> and q<sub>goal</sub>, so another algorithm such as A* [53] or Dijkstra [33] is
used to find the shortest path within the road-map.

Afterwards, some other techniques can be used in order to do path improvement. Like trying
to connect directly two non-adjacent nodes from within the path, checking consequently if for
the following nodes a connection is also possible, this will potentially reduce the amount of
connections needed within the path, reducing its total length.

The advantages on this type of algorithms is that is fast and has probabilistic completeness, which, by not fully exploring the *C<sub>space</sub>* but instead sampling it, reduces the amount of computation needed. On the other hand it also generates a big amount of “useless nodes” as only the ones included in the final path are the ones that matters. Another problem is that it is a trial and error algorithm, to which an unsuccessful case of finding a path might just mean that there is not enough nodes in the road-map to fully connect *q<sub>init</sub>* and *q<sub>goal</sub>*, so re-tuning the
<figure><img src="image_110.png" /><figcaption>Figure 4.7 – Example of a road-map on SE(2), the gray areas are the obstacles within the $C_{space}$, the empty circles correspond to the randomly generated nodes and the bold line is the shortest path obtained by Dijkstra’s Algorithm.</figcaption></figure>

<figure><img src="image_111.png" /><figcaption>Figure 4.8 – Optimization of the obtained path by connecting non-adjacent nodes.</figcaption></figure>
<figure><img src="image_112.png" /><figcaption>Figure 4.9 – RRT: (a) Building of the tree connecting the new node to the closest node towards the random point (b) Path obtained between $q_{init}$ and $q_{goal}$.</figcaption></figure>

parameters can provide a successful outcome. This algorithm is also not suitable for dynamic environments, as the road-map has to be recomputed and rewired every time obstacles moves in the environment, having to check if the previously computed nodes still belong to $C_{free}$ or now belong to $C_{obs}$.

## Rapidly Exploring Random Trees

Rapidly Exploring Random Trees (RRTs) [80], introduced by S. LaValle in 2001 as an optimization from the classical Random Trees algorithm. It consists in a tree of nodes which is generated starting from the initial configuration until it finds a feasible path towards the desired goal configuration. This node-tree (Figure 4.9) is built by taking into consideration a random point in the $C_{space}$, and connecting the closest node within the tree to a newly generated node in that direction and at a fixed step-size, in this way, the tree is constantly growing in a random way, but always from the closest node to the random point. The newly generated nodes are always checked in order to ensure they belong to $C_{free}$, if they do not belong, the step-size is reduced until they do. Due to its always increasing behavior, this algorithm is able to explore the $C_{free}$ space in a fast and random way, and stops once it finds a feasible path between the initial and final configurations. One of the drawbacks is that, due to the randomness, the final path is not the shortest, and once it is found, the algorithm does not perform any optimization on it. Another characteristic is that the obtained path is generally not a smooth path, this last one can be slightly optimized by using spline interpolation on the obtained nodes that form the path.

Another important improvement of this algorithm is called RRT* (Figure 4.10) [68] presented by S. Karaman and E. Fazzoli in 2011, which works under the same principle as the original RRT, but with the addition of two new considerations. The first one is that in this case, each node has a cost assigned, linked to the traveled distance relative to its parent node. In this way, after the closest node of the tree to the point has been found, all the surrounding nodes of
<figure><img src="image_113.png" /><figcaption>Figure 4.10 – RRT* (a) New node connected to the cheapest neighbor parent (b) Obtained path with the shortest and smoothest path.</figcaption></figure>

the parent node are examined within a radius, and if a node with a cheaper cost is found, the cheaper node becomes the new parent and the connection to the new node is made. The second consideration is that, this algorithm allows rewiring of the tree. When a new node is added, its neighbors also check if being connected to the new node will decrease their cost, and if so, the neighbor nodes are rewired to the newly added node.

One of the biggest advantages of this method is that it is able to find a close-to-optimal path, and the obtained path can be infinitely optimized for as long as new nodes are being added, obtaining very smooth paths. It also has the characteristic that it is a forward projection algorithm, meaning that once a connection is made between the initial configuration and the desired configuration, the path is already obtained. On the other hand, a big drawback of this method is that it is computationally expensive due to the constant checking that needs to be done for each node within the algorithm (check if the node is outside an obstacle, check the cost to confirm shortest path, rewiring of the neighbors, etc).

Other proposed alternatives commonly found are:

- Informed RRT* [45]: Where the random point generation is biased in order to place the point at the desired goal configuration a percentage of times or every set amount of iterations. This makes the tree to try to grow towards the desired goal in order to decrease the computation time.

- BRRT* [67]: Starts generating a tree from the initial configuration and another tree from the desired configuration, and when both trees meet, the path is obtained.

- RT-RRT* [97]: This is an improvement of the *RRT* algorithm where the algorithm allows changes in the *q*<sub>init</sub> state and rewires the previously generated map so it can be used for dynamic environments and changing initial and desired configurations.

- BiTRRT [32]: Bi-directional transition-based *RRT* is a version of *RRT* where each new node added to the tree goes through a transition test. When the closest nodes of the trees are closer than ten times the extension step-size, and if it is possible to connect them following a downhill slope, both trees are merged.
<figure><img src="image_114.png" /><figcaption>Figure 4.11 – Exact cell decomposition method (a) Nodes generated based on vertex found (b) Connectivity graph built from adjacent nodes (c) Nodes corresponding to the path found on the connectivity graph (d) Final path generated by connecting the middle points between adjacent nodes.</figcaption></figure>

Cell Decomposition

Another approach commonly found in the literature are the Cell Decomposition algorithms (Figure 4.11) [122]. The idea of this approach is to subdivide the *C*<sub>space</sub> into smaller obstacle free regions called cells, which then are connected by building a graph of adjacency [117] in order to find a path between the initial and final configurations. The method is subdivided also into two different categories of cell decomposition approaches: the exact cell decomposition (ECD), and the approximated cell decomposition method (ACD).

The first one, uses geometrically based algorithms to explicitly determine the obstacles and build the cells. This is done by drawing parallel vertical lines from each vertex found in the *C*<sub>space</sub>, and considering the shape of the object and of the *C*<sub>space</sub>, it can find the vertex corresponding either to an obstacle or to the *C*<sub>space</sub>'s boundaries. This will allow the algorithm to build a cell-based space which represents exactly to the *C*<sub>free</sub> space (free of obstacles). The counterpart of this approach is that it's mathematically expensive, as it is difficult to obtain the exact representation of the *C*<sub>free</sub> space.

Afterwards, each obtained cell is numbered and represented as a node in the connectivity graph, built based on the adjacency relationship between the free cells. Then a path is computed from the node containing the initial configuration, to the node containing the desired one, by following the shortest path on the graph (using A* or similar). In this way, the cells of the shortest path are determined, and the real path is generated by connecting the middle point of the transitions between adjacent cells, in order to ensure that the final path is as far as possible from the *C*<sub>space</sub> boundaries and the obstacles.

The approximated cell decomposition method, also called “quadtree” decomposition or “octree” for 3D spaces (Figure 4.12), was proposed due to the high computations and geometric calculations required in the exact cell decomposition method. The algorithm works by determining fully-free, completely full or mixed obstacle cells inside the *C*<sub>space</sub>. Initially the *C*<sub>space</sub> is divided into four equal regions, the algorithm determines the mixed cells, and subdivides them
<figure><img src="image_115.png" /><figcaption>Figure 4.12 – (a) Quadtree decomposition for 2D spaces (b) Octree decomposition for 3D spaces.</figcaption></figure>

again in the same way recursively until a fixed resolution is reached or until each cell lies either
as a fully-free cell or as a completely-full cell. Once one of these criteria is met, the algorithm
stops decomposing the space. Finally, a path is obtained from the generated tree created by
the decomposition, where the initial configuration cell gets connected to the final configuration
cell through a fully-free cells path. This algorithm is resolution complete, which means that the
algorithm’s completeness depends the resolution of the grid, while the exact method has total
completeness as it exactly represents the C<sub>space</sub>.

In [3] an alternative method combines cell decomposition methods with Rapidly exploring random trees (RRT) [80], where ACD is used to initially find a path on preliminary static workspaces, and RRT is then used to validate the path and find alternatives on collisions due to changing environments. In this work, RRT and ACD algorithms are combined together in order to exploit the advantages of each of them. The RRT planner has relatively high tolerance to obstacles shapes and workspace changes. Where this feature is missing in ACD planner. In addition, the RRT is not effective in small areas or narrow passage, while ACD planner does not face this problem.

4.4.2 Comparison of various path planning algorithms

In robotics, the path planning task has proven to be one of the most challenging tasks,
specially when conditions such as expecting real-time behaviors or in dynamically changing en-
vironments. A comparison of the presented planning algorithms has been presented in Table 4.1.
<table><caption>Table 4.1 – Comparative table between path planning algorithms.</caption><thead><tr><td>Algorithms</td><td>Advantages</td><td>Disadvantages</td></tr></thead><tbody><tr><td>APF</td><td><ul><li>Easy to implement</li><li>Allows on-line planning</li><li>Commonly used method</li></ul></td><td><ul><li>Local minima and oscillation problems</li><li>Goals can be non reachable</li><li>Difficulty in narrow passages</li></ul></td></tr><tr><td>Cell Decomp</td><td><ul><li>Has completeness for exact approach</li><li>Can be used for on-line planning</li></ul></td><td><ul><li>Resolution dependant in approximated approach</li><li>C<sub>space</sub> is not described in approximated approach</li></ul></td></tr><tr><td>PRM</td><td><ul><li>Allows multi-query scenarios</li><li>Probabilistic completeness without exploring all of the C<sub>space</sub></li></ul></td><td><ul><li>Generates a lot of unused paths and nodes</li><li>Needs to rebuild road-map on dynamic environments</li></ul></td></tr><tr><td>RRTs</td><td><ul><li>Forward projection algorithm, computes and finds the path</li><li>Explores rapidly the C<sub>space</sub></li><li>Has completeness</li></ul></td><td><ul><li>Computationally demanding</li><li>Single-query</li></ul></td></tr></tbody></table>

From the algorithms presented, APF and its variations provide a good adaptation for path planning in dynamically changing environments, where any obstacle entering the $C_{space}$ generates a new repulsive field which can be taken into account in order to generate a new trajectory. But the local minima problem requires the use of alternative algorithms in order to overcome it.

The PRM case, it is well known for its ability to find a path without the need to explore the totality of the $C_{space}$, but it is also a graph based algorithm, which requires the use of shortest path searchers as $A^*$. It is proven to be highly efficient in static environments and can handle initial and final configuration changes, but if the objects in the $C_{space}$ change positions, the connections between the nodes have to be re-done. Some alternatives propose to maintain the previously generated nodes and re-check if they belong to $C_{free}$ or $C_{obs}$, and rebuild the graph based on that information and find a new path. Similarly the case for cell decomposition methods, where the graph search needs to rebuilt again. Nonetheless this methods has proven to be viable real-time options that can adapt to a dynamic environment.

Finally on RRT and RRT\* methods and alternatives, they are known to be good path planning methods, with the limitations that the generated trees are linked to the initial configuration and they have high computation demands. With the proposal of the different alternatives very optimal real-time path planners can be obtained, the limitations on these type of algorithms is that they require large memory capacity as the whole tree needs to be stored at all times, and it only works in bounded environments, so unbounded and large distance environments are still a challenge.
<figure><img src="image_116.png" /><figcaption>Figure 4.13 – Planning group for the robot system.</figcaption></figure>

### 4.4.3 Selection of planning algorithm

#### Description of MoveIt planning group

The `planning_group` is defined as the group of elements that constitute the whole robot system. This is the UR-5 robot, the 6-faced prop, and finally the robot *pedestal* (Figure 4.13). These three elements are what the path planning algorithms need to consider as the robot, in order for them to avoid any collision state that exists with any of these elements.

The pedestal was modeled in such a way that it matches the dimensions of the real-world system. As established in Chapter 2, the determined height of the pedestal is 80 cm.

For the configuration of the `plannig_group`, MoveIt has an in-built graphical interface that helps to create all the configuration files related to the kinematics, controllers, Semantic Robot Description Format (SRDF), and other files for the usage of the robot in ROS. This interface is called *MoveIt Setup Assistant*.

The MoveIt Setup Assistant creates all the mentioned files based on the robot description given to it, in this case, the UR-5 robot description files provided by [89] were taken, and modified in order to include the pedestal base (included in the URDF definition of the robot) and also the mesh file for the 6-faced end effector.

#### Different movement techniques

Different mobility alternatives that MoveIt API offers were analyzed. All the tasks related to the motion of the `planning_group` are handled by the `move_group` class. By being able to specify which planning group we want to consider, we can use all the different functions that the class offers for it, such as getting information on the current joint values, the target, configure the planning algorithm we intend to use, and perform the planning and execution of motions in the environment.

The `move_group` class has the option to perform path planning through different types of motions, these options can be chosen depending on the nature of the task. For example, we can set up as a goal a given pose in the space, or set it up as a desired joint value. Given the nature of the system, we will be working with joint value goals, as we expect to reach the different
<figure><img src="image_117.png" /><figcaption>Figure 4.14 – Planned paths using *move_group* with out re-planning. (a) Back-to-back plans. (b) Velocities for both plans.</figcaption></figure>

points in a specific configuration that provides a higher level of safety to the user (elbow up the configuration for the UR-5).

Another important feature is the option to specify whether we want to reach each one of the requested goals or not. As the implementation is going to be receiving constantly changing goals, the best implementation is to plan and go towards said goal, allowing for the system to re-plan if the goal changes, which means that we do not need to reach the initial goal.

In Figure 4.14, two different trajectories are computed from an initial configuration to a mid goal and then to a final goal. In this case, we ensure that the robot is going to fully execute each one of the trajectories, reaching both goals. This is shown in Figure 4.14(b) where the velocities come down to zero, as it is performing a stop.

In the case of Figure 4.15, we computed the same two trajectories as before, but allowing re-planning during the execution of the first plan. In this case in Figure 4.15(a) we can see both plans one after the other, wherein Figure 4.15(b) we show the representation of the segment that was not executed from the first plan, as a re-planning scenario came to place. This is where the current positions of the first plan were taken as initial positions for the second plan, which gives as a result Figure 4.15(c), showing the two plans that were executed.

A parameter to select is the planning algorithm that suits best for the task. As previously
<figure><img src="image_118.png" /><figcaption>Figure 4.15 – Re-planned paths using *move_group*</figcaption></figure>

(a) Back-to-back plans. (b) Segment not executed due to re-planning. (c) Final executed plan.
<figure><img src="image_119.png" /><figcaption>Figure 4.16 – Comparison of all planning algorithm’s times.</figcaption></figure>

<figure><img src="image_120.png" /><figcaption>Figure 4.17 – Comparison of best planning algorithm’s times</figcaption></figure>

mentioned, MoveIt has multiple built-in path planning algorithms which can be used. To determine the best option, we iterated through all of the available options, and performed a planning task for the desired goal configuration, measuring the time it took for each one of the algorithms and registering the data. This was made for a total amount of five times for each one of the 12 available planning algorithms through 9 different trajectories. We then proceeded to take the average times they took to find a solution, perform path simplification (just for the algorithms that had this feature), and computed the average total time. With this data, we were able to select which algorithms behaved the best with the shortest planning times.

After doing the computations for half of the trajectories, given the big difference in planning times for some of the algorithms, we pre-filtered them in order to exclude those ones that presented the slowest planning times, in order to only consider the suitable candidates.

Another analysis that allowed us to select the algorithm which behaved the best for the implementation, was to perform an analysis of the generated trajectories with each one of the algorithms for a fixed task. Based on the pre-filtered algorithms from the previous analysis as a starting constraint, we computed the average execution time and waypoints for a set of trajectories.

This analysis was made for the same trajectories as in the previous graphs for a total of 10
<figure><img src="image_121.png" /><figcaption>Figure 4.18 – Comparison of average execution times of the algorithms.</figcaption></figure>

<figure><img src="image_122.png" /><figcaption>Figure 4.19 – Comparison of average amount of generated waypoints.</figcaption></figure>
iterations for each algorithm, but instead of considering just the computation time (Figure 4.18), we also took into account the amount of generated waypoints (Figure 4.19). Also, the mannequin was placed in the middle of the direct trajectory for it to be avoided in the computation, to test each algorithm's capabilities in planning around it. This also allowed us to see how consistent each algorithm's behavior was.

## Analysis on different planning algorithms

Following the experiments introduced in section 4.4.3, we ran two different experiments. The first one intended in determining which one of the 12 available planning algorithms were better suited to perform the path planning within the system. As Figure 4.16 and Figure 4.17 shows. Half of the algorithms included in the MoveIt plungins have long planning times, so that led us to reduce the total amount of algorithms to consider as usable to a new total of 6 algorithms. This ones being *BiEST*, *EST*, *RRT*, *RRTConnect*, *TRRT* and *BiTRRT*. With *BiTRRT* showing the lowest average total computation times, having in 4.17 an average of 0.5799 seconds for that particular trajectory. Followed by *RRTConnect* with 0.7332 seconds. While in 4.16 both of them are also the fastest algorithms with 0.5559 and 0.5254 seconds respectively. In this last, *RRTConnect* presented a slightly faster time, this two were the ones that showed the best times throughout all the 9 tested trajectories.

Due to this, in order to deepen the study and choice of the algorithms to use, we defined the second set of experiments, which considered the execution times of the trajectories generated by the algorithms. In this case, shown in Figure 4.18 we can see that the *BiTRRT* algorithm generally reaches the goal faster for the trajectory represented in the graph with an average of 3.5727 seconds, while *RRTConnect* showed and average of 4.5399 seconds, this being 0.9674 seconds more in execution time. This behavior was also similar to the other trajectories tested in the experiment, showing that even if both algorithms take more or less the same time in finding a solution, *BiTRRT* has faster execution times.

This is also reflected in Figure 4.19, where for the *BiTRRT* algorithm the trajectories computed where more “consistent”, obtaining similar trajectories every time (both algorithms are based in RRT, which is based in random exploration). In the case of *RRTConnect*, the solutions found were less optimal, sometimes generating longer trajectories with high deviations of what was considered the “optimal path”. This is shown by the average amount of waypoints for each algorithm, where *RRTConnect* generated in average 34 waypoints, while *BiTRRT* generated 21.9 waypoints in average. This results were consistent as well in all the different trajectories tested, backing up the conclusion over the deviation from the “optimal path” for the *RRTConnect* algorithm, as an average of more waypoints and longer execution times are a reflection of longer trajectories. Leading us to pick the *BiTRRT* algorithm as our path planning algorithm for the implementation of this project.

### 4.4.4 Description of Unity's virtual environment

In parallel to the development of the project, and to further explain the developed implementation, it is important to clarify how it will merge into the project. The system will receive the desired goal configuration that is going to be the *qgoal* intended for the planning algorithm, starting from the current configuration *qinit*. This goal selection is being made in Unity by a *Point selection algorithm* which determines the point of interaction that the user intends to reach, will be dealt in detail in next Chapter 5.
<figure><img src="image_123.png" /><figcaption>Figure 4.20 – Unity VR system and representation of interaction points</figcaption></figure>

<figure><img src="image_124.png" /><figcaption>Figure 4.21 – Representation of the user’s effective workspace as a sphere</figcaption></figure>

### 4.4.5 Different mobility schemes

Based on the Unity information, two different motion or mobility schemes and scenarios have been proposed depending on the nature of the task we want to achieve at the moment. One for which no interaction with the user is required, and another one for when it is. These two scenarios have their own environment to consider, presenting in general two different behaviors.

#### Movement outside person's workspace

The first scenario is where a distinction for velocity zones is performed based on the persons reach/workspace, the region is divided with a plane that represents the user's reach. Based on the same idea, we represented the mannequin's effective workspace as a sphere surrounding the model.

The mobility scheme consists of alternating from different "safe positions", called in this way as they are interaction points outside the user's reach, meaning there's no need in constraining the robot's velocities. Due to this, the motion from one point to another just needs to take into consideration the defined sphere, as we do not want to "collide" with it.
Following the idea, we performed a computation of all the existing trajectories between the different "safe positions" and stored them in a data file (assuming here that the robot starts at a point outside to the person's workspace and want to go to another point outside the person's workspace). This allows us to perform offline path planning, and then, on run-time, based on the initial and desired goal, we can access the pre-computed trajectories to directly execute them, removing the computation time that would take otherwise by performing online planning.

The algorithm 7 shows the trajectory storage.

<caption>**Algorithm 7 Trajectory computation and storage.**</caption>

**Input:** Number of points $no_p$. A counter for start point $i$. A counter for final point $j$
1: $start_name[no_p] \leftarrow$ ▷ Store id of the points
2: $final_name[no_p] \leftarrow init_names[no_p]$ ▷ Same id's as we are iterating through all points
3: **for** $i < 0$; $i < no_p$; $i++$ **do**
4:     **for** $j < 0$; $j < no_p$; $j++$ **do**
5:         **if** $i \neq j$ **then**
6:             $\emptyset \leftarrow$ Plan_and_Exec_to(points[i]) ▷ Move to initial point of the plan
7:             $plan_array[i][j] \leftarrow$ Plan_and_Exec_to(points[j]) ▷ Move to desired point and keep the planned trajectory
8:             **end if**
9:         **end for**
10:    **end for**
11:                 ▷ Store all as a structured message
12:    **for** $i < 0$; $i < no_p$; $i++$ **do**
13:        **for** $j < 0$; $j < no_p$; $j++$ **do**
14:            **if** $i \neq j$ **then**
15:                $init_pos_id \leftarrow$ start_name[i]
16:                $goal_pos_id \leftarrow$ final_name[j]
17:                $plan \leftarrow$ plan_array[i][j]
18:            **end if**
19:        **end for**
20:    **end for**

Subsequently, the second part of the scheme consists in loading up the pre-recorded data and being able to use it on demand. Expecting as an input just the desired position to reach. This is explained in algorithm 8.

Differently, in this research topic spherical obstacle (Figure 4.21) is used to divide the two zones instead of a plane (Figure 4.4), as it allows more flexibility for the planning group to consider more configurations when computing the path between points. Also allowed to find more achievable paths for the robot to follow, as it allowed to perform easier motions for the robot.

## Movement inside person's workspace

The second mobility scheme was proposed for the scenario where we need to go inside the user's workspace, this means that the motions have to take into account the user's model to avoid colliding with it. Another thing to consider is that these motions need to have their velocity constrained, to ensure safety aspect.
<caption>**Algorithm 8** Trajectory upload and execution.</caption>

**Input:** A desired frame to go to *des_frame*. A home pose *home*. Number of elements *no*<sub>e</sub>. Initial positions *init_pos_id*. Goal positions *goal_pos_id*. Planned trajectories *plan*. A counter *i*.
1: **for** *i* < 0; *i* < *no*<sub>e</sub>; *i* + + **do**
2: &nbsp;&nbsp;&nbsp;&nbsp;▷ Extract the data from the file
3: &nbsp;&nbsp;&nbsp;&nbsp;**start_name**[*i*] ← **init_pos_id**[*i*]
4: &nbsp;&nbsp;&nbsp;&nbsp;**final_name**[*i*] ← **init_names**[*i*] &nbsp;&nbsp;&nbsp;&nbsp;▷ Same id's as we are iterating through all points
5: &nbsp;&nbsp;&nbsp;&nbsp;**plan_array**[*i*] ← **plan**[*i*]
6: **end for**
7: Ø ← **Plan_and_Exec_to**(*home*) &nbsp;&nbsp;&nbsp;&nbsp;▷ Move to home pose
8: &nbsp;&nbsp;&nbsp;&nbsp;▷ Reference to home position as current
9: **init_frame** ← "home"
10: **aux_des_frame** ← "home"
11: **while** running **do**
12: &nbsp;&nbsp;&nbsp;&nbsp;**if** *des_frame* == *init_frame* **then** &nbsp;&nbsp;&nbsp;&nbsp;▷ The robot is in position.
13: &nbsp;&nbsp;&nbsp;&nbsp;**else**
14: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;**aux_des_frame** = *des_frame* &nbsp;&nbsp;&nbsp;&nbsp;▷ Update the desired position
15: &nbsp;&nbsp;&nbsp;&nbsp;**for** *i* < 0; *i* < *no*<sub>e</sub>; *i* + + **do** ▷ Search in the list of plans the one that matches the init and final frames
16: &nbsp;&nbsp;&nbsp;&nbsp;**if** **start_name**[*i*] == *init_frame* &amp; (*final_name*[*i*] == *aux_des_frame*) **then**
17: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;execute(*plan_array*[*i*])
18: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;**init_frame** ← **aux_des_frame**
19: &nbsp;&nbsp;&nbsp;&nbsp;**end if**
20: &nbsp;&nbsp;&nbsp;&nbsp;**end for**
21: **end if**
22: **end while**

Unlike in the first scheme, in this case, the environment consists of moving obstacles, which requires a constant update of the scene and constant tracking of the objects within it. For this reason, we used the frames of the mannequin model to obtain its current positions and orientations to be able to track their movement and link it to the objects spawned in the scene.

We also need to be able to determine if a computed plan is going to enter in a collision or not, which needs to consider multiple aspects. First, based on the computed path towards the desired goal, we check if during the execution of the plan the path remains valid. This is done by checking for all the computed waypoints of the path if the respective configurations are currently in a collision with any other object present in the scene. If it is free of collisions, then we proceed with the execution. In the case of a collision present in any of the remaining states of the non-executed path, we instruct the robot to stop the execution of the computed path and re-plan again based on the updated scene information.

To test this, we performed an initial implementation of the moving obstacle principle, to which we planned to a desired joint position, and spawned an obstacle in the middle of the trajectory while mid-execution. Then, thanks to the path validity checking we can detect that an object is in collision with the planned path, so we instruct the robot to stop the current execution and re-plan towards the same goal, taking into account the refreshed planning scene.
<figure><img src="image_125.png" /><figcaption>Figure 4.22 – Representation of dynamic obstacles using the user model.</figcaption></figure>

<figure><img src="image_126.png" /><figcaption>Figure 4.23 – (a) Original planned path. (b) Re-planned path from detected collision</figcaption></figure>
This work is intended to be extrapolated to work along with the mannequin model and its kinematics as mentioned in section 3.1. So it is capable to take into account the moving mannequin in the environment as an obstacle to avoid.

## Analysis on mobility outside workspace

One of the advantages on this implementation, in section 4.4.5 is that by being capable of directly identify which trajectory we want to execute, we can access to the pre-computed plan and we perform directly the execution of said trajectory. As we are located outside the workspace of the user, there's no need to constantly plan a trajectory as once we already took into account the sphere defined in Figure 4.21 we already ensured that the trajectory will be safe. This normalizes the execution times as they are now constant.

One limitation this implementation has is that, given the nature of performing pre-recorded trajectories, the system needs to fully execute each one of the trajectories that he/she performs. Contrary to other scenario where trajectories can be re-planned as in Figure 4.23. Nonetheless, this is not a strong limitation in the system, as the prediction (will be dealt in next chapter) of the "interaction points" and "safe positions" done in Unity helps to overcome the re-planning situation.

## Analysis on mobility within workspace

This movement scheme is fundamental at the moment of interacting with the user, and thanks to having defined obstacles which have been linked to the mannequin model we can take into account this elements as the objects to avoid while performing any type of planning. Another important aspect to take into account within this scenario, is that the goals received for the system can change rapidly, so for this case we want to be able to do re-planning during any plan's execution period. Allowing the system to be more fluid in the motions as well.

On the limitations it presents is that during the development, even though that scene is being constantly updated and we can know in real time the refreshed position of the mannequin (Figure 4.22), the development of this scheme has not been fully implemented, as we are able to re-plan for dynamically appearing objects as shown in Figure 4.23, but we are not able yet to check the path validity in an iterative way, limiting the reaction and interaction with the mannequin model.

# 4.5 Conclusion

A system architecture to communicate between the multiple systems (Unity in Windows, ROS in Ubuntu and robot system) was established. A calibration setup to connect components of the physical and virtual environment in ROS was defined. A simple test was performed to verify the conversion of coordinate systems between Unity and ROS.

Four planning algorithms were studied to plan the trajectories. These algorithms were compared for a give test case and analyzed. Finally *RRT* and *RRT\** methods and alternatives, they were known to be good path planning methods, with the limitations that they have high computation demands and memory.

Another limitation, even though we are able to re-plan for dynamically appearing objects, but we are not able to check the path validity in an iterative way, which limits the reaction
and interaction with the mannequin model. To counter these drawbacks, we plan to have pre-
computed trajectories and only wish to access them based on the user intention prediction
(explained in next Chapter).

In the next chapter we compare several algorithms for detection of user intention. To have
same data as input for all prediction algorithms the analysis will be done without motion of
the robot and recording the user motion. Precomputed trajectories will be used to evaluate
the intention prediction algorithm as the planning computation time and execution time will be
constant.
# Chapter 5

# Prediction of user intention

## 5.1 Introduction

This chapter proposes a set of different movement strategies for the robot to be as fast as possible in the contact zone while guaranteeing safety. This work uses the concept of predicting the user's intention through his/her gaze direction and the position of his/her dominant hand (the one touching the object) and safe points outside the human workspace. Experiments are done and analyzed with a Pareto front with a UR-5 robot, an HTC Vive tracker system for an industrial application involving the analysis of materials in the interior of a car.

When the user uses HMD vision interfaces and has to perform haptic evaluations, he/she no longer sees the real scene, but only a virtual world. His/her physical reference points quickly disappear except for objects he/she touches such as his/her seat and the floor.

Through the user's gaze and hand movements, as well as the position of areas to be studied, it is possible to predict the tasks that the user will perform. Eye-hand coordination is a fundamental behavior that humans use to interact with the world [55, 66, 92]. The head movement facilitates subsequent gaze shifts toward the future position of the hand to guide object manipulations, thus leading to a strong correlation between head and hand movement parameters [102, 123, 124]. The purpose of this study is to ensure that the robot end-effector will be available for intermittent contact in complete safety when the human hand is close to the surface to touch. An industrial robot can perform powerful and fast movements that can be dangerous for the humans around it. Involuntary contact between the robot and humans is a threat. This is particularly important in a virtual reality context where humans equipped with an HMD will not be able to anticipate the robot's movements.

Today, more than ever, humans work closely with robots. In the case of intermittent contact interface ICI, contact is inevitable between humans and robots. Cobots are best suited to such a scenario, but in terms of human safety, accident prevention can always be improved [25]. These robots are designed to work at limited speeds during potential contacts. Moreover, it must be ensured that the desired contact with the robot during interaction will not result in a necessary restart of the robot after a safety stop [82].

The main contributions presented in this chapter can be summarized as follows:

- A set of strategies to predict the user intention.

- The modulation of the robot's speed according to its location in relation to human.

- A user-based analysis of the proposed strategies.

A motion capture system based on HTC Vive-trackers is used to know the position of the body and especially the hand used for interaction as well as the position of the chair and the
robot [129] (Figure 5.1). The prop can carry six different materials. The robot is fixed on a 80 cm high table and the user sits on a seat 60 cm above the floor. The placement of the robot in the scene has been chosen to be able to reach all the places where the user's hand will want to have haptic interaction with the robot's probe[50].

<figure><img src="image_127.png" /><figcaption>Figure 5.1 – The complete system setup for human-robot interaction.</figcaption></figure>

A virtual model under the Unity 18.4 LTS software represents the fixed objects in the envi-
ronment. The position of moving objects is known, the user position thanks to HTC trackers
located on the hands, and its seat and robot location thanks to encoders on the motors.

The outline of this chapter is as follows. Section 5.2 introduces the proposed model and methodology. Section 5.3 presents different regions of robot motion. Section 5.4 explains all the proposed strategies. Section 5.5 presents the user study carried out to evaluate the introduced strategies. Results and insights from the study are also discussed. Finally, general remarks and conclusions are commented in Section 5.6.

5.2 Human intention prediction

5.2.1 Detection of target

The robot needs to anticipate human's future actions and act accordingly while performing collaborative tasks. In most human-robot collaboration systems, the motion of robots is based on some predefined programs, which are task-based. However, most tasks are highly complex and it is difficult to redefine a complete set of instructions for such situations. In such tasks, the role of the robot should be changed from purely automated machines to autonomous companions. Previous works relied on supervised learning methods to build models of human motion, which
relied on understanding the environment, offline training or manual labeling, adapt to new people, and motion styles.

Human intention is mainly expressed through the behavior of humans and the objects they interact with. Most of the current research on human intention prediction just focuses on action classification, in which the human action is classified into several categories, such as running, walking, jumping [41] which is inadequate for accurate inference of human intention in human-robot collaboration.

We propose an HRI framework that combines hand motion with gaze direction to build models on the fly, which predict human intention in virtual reality and move the robot to the required position in a virtual space without offline training.

### 5.2.2 Proposed model

The aim of the work is for the human to make contact with different parts of the car, in a design phase where only a virtual model exists, to be able to assess the quality of the materials. The areas to be explored are limited, driver's door, passenger seat, dashboard, touchpad. Depending on where the human wants to touch, the robot must position itself so that the human can touch the appropriate material placed on the probe. The probe has a certain surface area, so a limited number of Regions of Interest (ROI) in the car have been defined that the robot will have to reach to allow contact with the human. The set of 18 ROI considered is described in Section 5.2.3. The objective is therefore to determine as soon as possible the ROI that the user wants to reach and even more so that the robot's probe is positioned as soon as possible on this ROI. If the probe arrives before the human, the human will be able to make contact without being aware that he/she is in a virtual world, otherwise the waiting time before making contact should be as short as possible.

The major elements involved in our approach are summarized in Figure 5.2. Measurements of the pose of the hand and the gaze direction via the orientation of the HMD are used to select an ROI where the human hand will touch the prop of the robot.

It should be noted that as the objective is that the robot arrives at the target as soon as possible, several strategies are possible and can be combined:

- Detect the target at the earliest,

- Move the robot as soon as possible in the right direction, even if the final target is not yet known,

- Move the robot as quickly as possible.

As we are in a cobotic context with a human locked in a virtual world that does not see the robot (the robot can also be visualized in the virtual mode but the immersion will be less), safety is a priority. A description of the methods implemented to have a fast speed of movement of the robot and ensure safety will be discussed in Section 5.3.

### 5.2.3 Scene information

From the model of the car in Unity virtual reality software, we defined the ROI the user is to interact with. Each ROI is represented as a capsule placed at the center of the surface. For each surface, the desired orientation of the probe is defined. We have defined 18 ROI to be studied in the car (Figure 5.3). They are located as follows:

- Four capsules on the door,

- Four capsules on the chair,

- Four capsules on the dashboard,
<figure><img src="image_128.png" /><figcaption>Figure 5.2 – Diagram of the inputs used to choose a robot movement strategy.</figcaption></figure>

* One capsule on steering wheel,

* One capsule on touch pad,

* Three capsules on glove compartment,

* One capsule on speedometer.

## 5.3 Safe and fast motion

When the target is defined, the robot must be moved. For this, a series of trajectories that avoid obstacles have been defined (see section 5.3.1) between the point of interest and/or safe point. While the robot is moving, a new point of interest can be defined. One could stop the robot's movement and recalculate an obstacle-free trajectory online. However, in order to avoid wasting time in this calculation, predefined trajectories have no calculation time, so the robot is let to perform its movement. And it will take into account the new target at the end of its movement.

### 5.3.1 Cobot motion

The robot will navigate between a finite number of points which are our ROI. However, the movements must ensure that collisions with humans are avoided. To do this, we will generate offline robot movements that ensure that no part of the robot enters an area encompassing the human at rest in the driver's seat. The area to be avoided is composed of a sphere and is illustrated in Figure 5.4. The dimension of the sphere covers the human head and torso and
<figure><img src="image_129.png" /><figcaption>Figure 5.3 – Location of the ROI 1 to 18 inside the car and safe-points 20 to 24.</figcaption></figure>

part of the arm, but the hands can be outside since they must be able to reach the ROI. For
this, we need to construct 18 × 17 off-line trajectories that we will assemble in line according to
the ROI detected to accomplish the task. These trajectories are close to the human, they are
realized with a maximum speed of 0.25m/s to ensure the use of the UR-5 cobot according to the
ISO standard for human-robot collaboration [125]. This condition guarantees that a possible
collision with the human will not hurt him.

5.3.2 Velocity zones

The robot must be moved closer to the target point to prepare for the interaction. The movement must be fast so that the robot has arrived before the human and thus avoid unpleasant waiting, but the maximum speed of the robot must be limited for safety reasons. Figure 5.5 shows the scene of the VR environment, it consists of the car interior and user model.
<figure><img src="image_130.png" /><figcaption>Figure 5.4 – In the definition of the robot motion to joint the ROI, the sphere that represents the user occupancy zone is avoided.</figcaption></figure>

<figure><img src="image_131.png" /><figcaption>Figure 5.5 – Car interior and user workspace in Rviz.</figcaption></figure>
<figure><img src="image_132.png" /></figure>

Figure 5.6 – The spaces defining the robot velocity. The two blue spheres described the human workspace when seated. The grey part shows the car model. The transparent sphere is the robot’s working area. The red line delimits an area where the speed of the robot can be higher because there is no risk of collision with the human.

Based on this, we distinguish three velocity zones:

* The human workspace (HW), is defined as two spheres whose radius is the size of the arm centered on the shoulders of the mannequin. This workspace will evolve according to the movements of the human. We could also consider a constant space if we limit the realistic movements of the torso. This space is represented by blue circles in Figure 5.6.

* The inside of the car (IC): this space delimits the area where we know the human must move. Even if the Unity model is complex, this zone can be approximated by a larger simple region that includes the real interior of the car. The gray rectangle, in general, represents the entire Unity model and we define a plane, depicted by the red line in Figure 5.6, that separates the region that can be reached by the user.

* The free space (FS) cannot contain points that are in the HW. We can have a certain safety margin to define this zone. In our example, this zone is simply limited by a plane represented in red in Figure 5.6.

The limit on the robot velocity is chosen according to the space:

* When the robot moves in FS, it can do so at maximum speed $V_m$ (all parts of the robot are in FS),

* When the robot moves outside of FS, it must move at reduced speed $V_r$,

The speeds are chosen such as $V_m \ge V_r \ge 0$. The different spaces are shown in Figure 5.6. The blue transparent circle is the robot’s workspace, two blue-filled circles are the workspace of the user’s hands. The grey rectangle is the complete interior model of the car and the red line is the plane that we use to differentiate the reachable and unreachable parts of the car by the user.

### 5.3.3 Velocity profiles

To ensure safety and also have better response time we defined two velocity profiles of $V_r = 0.25 \text{ m/s}$ and $V_m = 4 \text{ m/s}$ based on the zones defined above. To illustrate the idea, we
<figure><img src="image_133.png" /><figcaption>Figure 5.7 – Illustration for the comparison of motion using safe-points.</figcaption></figure>

devise the scene as shown in Figure 5.7. We define four points:

* Two points A' and B' are on the plane boundary, these are in the FS, and fast motion between these points can be produced. For the application studied in the paper, the capsule denoted 20 to 25 are in the FS.

* Two points B and A are inside the plane boundary, one point on the dashboard and another on the passenger's seat. These points play the same role as the ROI 1 to 18.

We analyze two different scenarios based on different velocity combinations.

* *The shortest way:* Knowing the target point, the robot moves towards it, and adapts its speed according to the spaces it crosses. In the studied example, it goes directly from A to B with a velocity of less than 0.25m/s.

* *Safe-points:* We use of safe-points to keep the robot's speed high. In the studied example, the points A' and B' belongs on the plane that limits FS. The robot goes from A to A' with a maximal velocity less than 0.25m/s, then from A' to B' with a maximal velocity less than 4m/s, B' to B with a maximal velocity less than 0.25m/s.

The movement of the robot inside the car should be performed at reduced speeds for safety reasons. In some cases, when the desired point is far away from the user space, it takes longer to reach it due to the low speed. In such situations, we use the via-points on plane boundary in the FS, called safe-points, between which the robot can move at high speed. The path is longer but its execution can be faster.

New trajectories are calculated off-line to connect the ROIs and the safe points with the two motion speeds.

### 5.3.4 Safe-points

As we have shown in the previous section, the interest to move the robot in FS is to speed up the robot movements. Five points on the plane boundary of FS have been defined $SP_{20}$, $SP_{21}$, $SP_{22}$, $SP_{23}$, $SP_{24}$ to be used for this purpose.

The interest of moving the robot in the FS is also to increase the safety of the operator by moving the robot away from the human. To quantify this notion of safety, we will define an average distance between the robot's end-effector and the sphere encompassing the human's torso shown in Figure 5.4. The higher this distance, the safer the human/robot interaction will be. This distance is calculated in an approximate way from the points of passage of the robot ($SP_1, ..., SP_{24}$), by making the hypothesis of a straight line displacement at constant speed between the points (defined as the distance between the points divided by the duration of the displacement).
<figure><img src="image_134.png" /><figcaption>Figure 5.8 – Comparison of motion through safe-points and without safe-points.</figcaption></figure>

$$d_s = \frac{\sum_{i=1}^{N} (\|p_i - C\| - R)}{N} \qquad (5.1)$$

Where the robot motion is sampled in N instant, $p_i$ is the coordinate vector of the end-effector of the robot for the sample i, C is the coordinates vector of the center of the sphere shown in Figure 5.4 and R is its radius.

Considering the user safety and robot velocity it is of interest to pass through points on the plane boundary in FS, when we have long robot trajectories to make. In the next part of the study we will show that this can also be useful when a movement is initiated by the human by hand and gaze but without knowing yet where the human will stop. Placing the robot on one of the safe points $SP_{20}$, $SP_{21}$, $SP_{22}$, $SP_{23}$, $SP_{24}$ will allow the robot to get closer to the goal more quickly.

### 5.3.5 Comparison of motion with or without safe-points

An example is illustrated in Figure 5.8 where $A = P_{17}$ and $B = P_5$, $A' = SP_{24}$ and $B' = SP_{21}$. The points A' and B' are positioned on the plane that divides the two different velocity zones.

We compared the time taken by the robot to move between two points, taking into account the presence of safe points and without them.

The results in Figure 5.8 show the position of points in X coordinate with respect to time. The Figure 5.8 is a representation to show the point and at what time the robot reaches that point. From the recorded trajectories of the robot to reach the points, the X-coordinates of the robot are plotted at the beginning and end of the trajectory (and connected by a straight line) against time in Figure 5.8. The direct motion is shown in blue, when the motion with intermediate safe point is shown in red. It proves that by passing through safe-points A' and B', the robot takes less time than going directly.

By traveling through safe points, the robot starts from point A and moves through $A'$, $B'$ at a higher velocity and then to the final point B. The total time taken to reach the final point was 0.41s. For the motion inside the car, the robot arrives at the final point after 0.45s.

## 5.4 Proposed strategies

We will study and compare four strategies that integrate the position of the hand, the direction of the gaze, and the use of safe-point to efficiently move the robot to one of the ROI that the human wants to reach.
### 5.4.1 Strategy A: Hand position

As the user's objective is to touch with his/her hand the ROI, the first and the simplest strategy proposed is to consider that the point to be reached is the closest to the hand position. The strategy is presented in Figure 5.9, in the example the selected point is $P_2$. The main advantage is the simplicity of the approach. For the search for the nearest point, the k-d tree structure is used to find the nearest point of the hand. An implementation of this algorithm for VR environment is done in [96]. The strategy is summarized in Algorithm 9.

<figure><img src="image_135.png" /><figcaption>Figure 5.9 – Pictorial representation of Strategy A.</figcaption></figure>

**Algorithm 9** Strategy A: Predictions with hand.

**Input:** Hand position $P_h \in R^3$

**Output:** Nearest Point P in the set of $P_i$, $i = 1...18$.

1. Build a k-d tree for all points $P_i$ in the scene.

2. **function** STA($P_h$)

3. Using hand pose as a query point $q$, return nearest point $P$ from the k-d tree.

4. **return** P

5. **end function**

The main characteristics of the approach are:

1. The point is detected only when the human has almost reached the points;

2. If two points are equidistant, a prediction fluctuation can occur with small changes in hand motion;

3. During the movement of the hand, intermediate points can be detected, which will allow the robot to start its movement before the desired end point is detected.

We used a proximity search, which is an optimization problem of finding a point in a closed set that is closest to a given point. Closeness is defined by a dissimilarity function such that the dissimilar the objects, the larger the function values.

### Problem definition

Given a set P of our interest points in a 3D space D, and a query point q which represents the user's dominant hand, we find the closest point P to q. This problem can be generalized as a k-nearest neighbour (kNN) query where we have to find the k closest points where $k \in Z^+$. Implementing the kNN is normally done by computing the distances from q to all elements in P. However, this method is computationally intensive for a large number of data points. We used a k-d tree for the nearest neighbor search proposed by [8, 44].
<figure><img src="image_136.png" /><figcaption>Figure 5.10 – k-d tree (a) k-d tree decomposition for point set and (b) The resulting k-d tree.</figcaption></figure>

Formally a k-d tree is a balanced binary tree for a set of data points $p_1, p_2, ..., p_n \in P$ where the root corresponds to all points and its two children represent almost equal-sized subsets of $P$. Every leaf corresponds to a k-dimensional point and every non-leaf node is a splitting point generating a hyperplane that splits points into subsets in a level-wise manner. Points to the left of this hyperplane are represented by the left sub-tree of the node and points to the right are represented by the right sub-tree. For a given node $p$ at level $i$, the points associated with $p$ are split into two halves by resorting to the median in dimension $i \mod k$. Such that the point inserted in the tree at each step is the one, which has the median coordinate in the direction considered [8]. However, splitting rules may vary. The recursive construction ends as soon as a node $p$ corresponds to a singleton or to a set of predefined sizes.

## Nearest neighbor search using the k-d tree

Given a point $q$, find the point $p$ in the data set $P$ that is closest to $q$. This can be done with the three following steps :

- Step 1: We defined the points representing the position and orientation of the surface for each of the ROI in the car model.

- Step 2: Build a k-d tree to store the positions for all the cubes.

- Step 3: Using the hand position as a query, we find the closest point to the hand from the k-d tree.

The algorithm 10 describes precisely these above steps.
**Algorithm 10 Nearest neighbor search.**

**Input:** k-d tree root node, query point $q$.

**Output:** Nearest node $p$.

1. Start from the root node.

2. Move down the tree recursively following the same procedure as it would be for the insertion of point $q$ in the tree.

3. Once a leaf node is reached, save the leaf as the current best point $p$.

4. Rewind the recursion tree, and

5. For each node $v$, if $d(q, p) > d(q, v)$, then the current node is $v = p$, where $d$ is a distance metric.

6. Check if there could be yet better points on the other side of the subtree by checking if neighboring boxes potentially contain points that are closer to $q$ as the current best candidate (using the median values).

7. In case a point might be closer, recurse to the sub-tree that has not yet been visited.

8. If there could be, move down again on the other side of the sub-tree. Otherwise, go up another level.

**Drawbacks of the above approach**

The above approach has the following shortfalls:

1. The accuracy decreases when the hand is close to the midpoint of any two points. In this case, the difference in the distance between each point and the hand is very small and such that a slight displacement of the hand results in the inappropriate motion of the robot to any of the nearest points.

2. Unnecessary and Involuntary hand movements. Due to human nature, the user can move their hands involuntarily without the intention to interact with any objects in the space. In this case, the algorithm would still move the robot to the best point according to the minimum distance.

To overcome the above shortfalls, we decided to include the head gaze in the model such that a predicted point is considered valid and intentional. If the user was gazing in the direction of the object the hand interacts with. This is because when humans reach to grasp an object, they look at the target first, then bring the hand to the center of gaze as the object [88, 114, 102, 123].

**Improved approach**

The improved approach involves information extraction and definition of ROI from car model, k-d tree construction for each ROI and Hand pose, nearest neighbor search, and Hand - head gaze coordination to determine the best point. A 5 step process :

- Step 1: We defined the points representing the position and orientation of the surface for each of the ROI in the car model.

- Step 2: Build a k-d tree to store the positions for all the cubes.

- Step 3: Using the hand position as a query, we find the $n_p$ points closest to the hand from the k-d tree. Contrary to section 5.4.1, we do not define directly the closest point
but we select $n_p$ candidate points. Depending on experiments $n_p$ can be two or four.

— Step 4: From the $n_p$ candidate points, we select only the $n_{pg}$ points belonging to the view frustum of the HMD.

— Step 5: Among these $n_{pg}$ points, the closest to the gaze direction is selected as predicted contact point between the user and the robot end effector.

Thesis steps are commented in the Algorithm 11.

<caption>**Algorithm 11 Predictions with head gaze.**</caption>

**Input:** Scene information, head gaze direction.

**Output:** position and orientation of desired point.

1. Cubes representing the surface and orientation of regions of interest.

2. Build a k-d tree for all points in the scene.

3. Using the hand pose as a query point $q$, return the nearest $n_p$-points from the k-d tree.

4. For each of the $n_p$ points, we select only the $n_{pg}$ points which lie within the view frustum of the HMD.

5. find the gaze direction as a unit vector from the central point of the eyes and draw a ray in the gaze direction.

6. calculate the distance of each point in $n_{pg}$ from the ray and return the position of the nearest point.

## Experimentation

We conducted experiments to analyse the motion of the robot in response to hand motion in two approaches by moving the hand between the two points, with the following objectives:

1. Midpoint test: To show the response and error in robot motion when the hand is close to the midpoint. For this, we consider two points, which are 1.3 meters apart. Placing the hand tracker at the midpoint and displacing it by 2 cm results in the robot going to extreme points

2. Involuntary and Unintended Hand Motion Test: To verify that the robot moves only when the hand and head gaze are in the same direction. A hand motion to areas outside human vision is not sufficient to move the robot.

## Results

In the first approach, we used the model without data from VR HMD, we observed that by moving the hand approximately 1.5 mm from the midpoint, the robot responds by moving to the other closest point. Figure 5.11 shows the displacement of the hand. The corresponding motion of the robot is shown in Figure 5.12. It can be seen that Robot TCP moves to the extreme points ever-time there is a small displacement in the hand tracker. To avoid this noise, motion we introduce Head Gaze.

In the second approach, by including the head gaze in the model, it is observed that the robot only moves in the direction of the object where the head is currently facing. While Figure 5.13 shows displacement of the hand tracker about the midpoint, it can be noticed that there is no significant change in the displacement of the tracker. However, for the robot TCP, it changes
<figure><img src="image_137.png" /><figcaption>Figure 5.11 – Hand Tracker Motion.</figcaption></figure>

<figure><img src="image_138.png" /><figcaption>Figure 5.12 – Robot TCP motion without head gaze.</figcaption></figure>

only when the gaze shifts. Figure 5.14 shows the motion of the robot TCP, and when comparing it with Figure 5.12, we can see a reduced noise in the displacement of the robot.

From Figures 5.14 and 5.13, it was observed the movement of the hand does not affect the motion of the robot as long as the head is not facing in the direction of the hand movement. The robot only changes the direction of motion at instances corresponding to the rotation of the head.

The fact that gaze direction is taken into account limits the inappropriate variations of the target point and allows a smoother movement of the robot. This is based on the assumption that the operator looks in the direction where he/she wants to go.

## 5.4.2 Strategy B: Hand position and gaze direction

Head gazes direction is introduced to limit the detection of points to only what the user can see. The detection of the interesting point is only possible if the point is in the field of view. The strategy is presented in Figure 5.15. The pose of the hand is used to select 2 points, the closest to the hand, in the example $P_1$ and $P_2$. From these 2 points, the closest to the gaze direction is selected, by comparison of the angle between the line connecting the point and the line of view. In the example, the closest point is $P_2$. The strategy is summarized in Algorithm 12.

<figure><img src="image_139.png" /><figcaption>Figure 5.13 – Hand Tracker Motion.</figcaption></figure>
<figure><img src="image_140.png" /><figcaption>Figure 5.14 – Robot TCP motion with head gaze.</figcaption></figure>

<figure><img src="image_141.png" /><figcaption>Figure 5.15 – Pictorial representation of Strategy B.</figcaption></figure>

**Algorithm 12** Strategy B: Predictions with head gaze.

**Input:** Hand position $P_h \in \mathbb{R}^3$, HMD position $P_s \in \mathbb{R}^3$, Head Orientation $O_s \in \mathbb{R}^4$.

**Output:** Nearest Point P.

1. Build a k-d tree for all points in the scene.

2. **function** STB($P_h, P_s, O_s$)

3. Using hand pose as a query point $q$, return nearest 2-points from the k-d tree.

4. Find the gaze direction as a unit vector from the central point of the eyes and draw a ray in the gaze direction.

5. Calculate the distance $l_1$ of each point in $n_p$ from the ray.

6. Find the angle $\lambda_i$ for each point such that $\lambda_i = l_i/L_i$ where $L_i$ is the distance from the HMD to the projection of the point on the line. (Shown in Figure 5.15).

7. The point with $\min(\lambda_i)$ is the closest point P.

8. **return** P

9. **end function**

The aim of this approach is to try to find the point of interest with a little anticipation compared to the previous method, by being able to choose a point that may be a little further from the hand but directed according to the direction of the gaze.

### 5.4.3 Strategy C: Addition of safe points

If the hand is far from the point of interest, it is probably on the way, but still far from the goal, so it may be appropriate to move the robot to a safe point to prepare for a higher speed movement. This strategy is an extension of strategy B, but with added extra safe points. This strategy is designed such that the robot will always go to the safe point if the distance between the hand and the closest point is above a threshold, here $0.2 m$. The strategy is presented in
Figure 5.16. In the example, the point $P_2$, the closest to the view line among the two closest to the hand, is at more than 0.2 *m* from the hand, thus the robot will go to the safe point which is the closest to $P_2$ among $SP_{20}, SP_{21}, SP_{22}, SP_{23}, SP_{24}$. The strategy is summarized in Algorithm 13.

<figure><img src="image_142.png" /><figcaption>Figure 5.16 – Pictorial representation of Strategy C.</figcaption></figure>

**Algorithm 13** Strategy C: Addition of safe-point.

**Input:** Hand position $P_h \in R^3$, HMD position $P_s \in R^3$, Head Orientation $O_s \in R^4$, Hand Threshold $T_h$ (Shown in Figure 5.16).

**Output:** Nearest Point P.

1. Build a k-d tree for all points in the scene.

2. **function** STC($P_h, P_s, O_s, T_h$)

3. **function** STB($P_h, P_s, O_s$)

4. **return** P

5. **end function**

6. **if** distance($P, P_h$) < $T_h$ **then**

7. **return** P

8. **else**

9. **for all** $SP_i \in SP$ **do**,

10. $d_i$ = distance($P, SP_i$).

11. $\min(d_i)$;

12. **end for**

13. **return** $SP_i$

14. **end if**

15. **end function**

The main characteristics of this strategy are as follows:

- The difference between this approach and strategy B can only be seen for long displacements (of more than 0.6 *m* between the points) for which the hand displacement can be quite far from the points $P_i$, $i = 1, \dots, 18$.

- There is a risk that the robot will move to a safe point in an inefficient way with a longer path that will not allow the robot to arrive faster

- The results obtained can vary with the choice of the threshold $T_h$, an analysis of the influence of this parameters on the prediction can be found in Section 5.5.6.
### 5.4.4 Strategy D: Head gaze and safe points

All the strategies presented so far are based on a selection or pre-selection of the point of interest-based on the hand position. Here the approach is different and the selection is based on the direction of the gaze which can greatly anticipate the movement of the hand. As in strategy C, safe points will be used if the point of interest is more than $T_h = 0.2$ m away from the hand. The strategy is presented in Figure 5.17. In the example, the point $P_2$, is the closest to the view line among the points in the frustum of the HMD. As the point $P_2$ is more than 0.2 m from the hand, thus the robot will go to the safe-point which is the closest to $P_2$ among $SP_{20}, SP_{21}, SP_{22}, SP_{23}, SP_{24}$. The strategy is summarized in Algorithm 14.

<figure><img src="image_143.png" /><figcaption>Figure 5.17 – Pictorial representation of Strategy D.</figcaption></figure>

This strategy is based on the assumption that the task will be carried out with the coordination of gaze and movement. In general, it is reasonable to think that the gaze anticipates the movement. In the context of the study, where the human is enclosed in a virtual world, it is likely that he/she will not be disturbed by external elements and that he/she will remain focused with his/her gaze directed towards the point of interest. The context should therefore be favorable to this approach.

## 5.5 Experiments and analysis

### 5.5.1 Criterion

The main research question was to find a selection strategy that maximizes user safety while minimizing robot response time. Four strategies explained in the previous section were tested against each criterion. The strategy selected needs to minimize robot time to reach the desired target pose while ensuring maximum user safety.

To evaluate the strategies, the following criteria were considered for strategy.

1. Efficacy:

* $Q_1$: If the strategy detects the final point or not. A value of 1 or 0 was assigned if final end point was detect or not.

2. Time for detection:

* $Q_2$: Time taken by the strategy to detect the desired/final point the user want to reach.

* $Q_{2norm}$: In order to be able to compare the results for several strategy, we defined a normalized criterion. It a ratio of each value of $Q_2$ for a trajectory divide by the min-
**Algorithm 14** Strategy D: Predictions with head gaze and safe-points.

**Input:** Hand position $P_h \in \mathbb{R}^3$, HMD position $P_s \in \mathbb{R}^3$, head orientation $O_s \in \mathbb{R}^4$, hand threshold $T_h$.

**Output:** Nearest Point P.

1: **function** STD($P_h, P_s, O_s, T_h$)

2: $n_i$ = points in the view frustum of HMD.

3: Find the gaze direction as a unit vector from the central point of the eyes and draw a ray in the gaze direction.

4: Calculate the distance of each point in $n_i$ from the ray.

5: Find the angle $\lambda_i$ for each point such that $\lambda_i = l_i/L_i$ where $l_i$ represents the distance between a point and its projection on the line as calculated in previous step and $L_i$ the distance from the HMD to the projection of the point on the line (Shown in Figure 5.17).

6: The point with $\min(\lambda_i)$ is the closest point P.

7: **if** distance(P, $P_h$) < $T_h$ **then**

8: **return** P

9: **else**

10: **for** all $SP_i \in SP$ **do**,

11: $d_i$ = distance(P, $SP_i$).

12: $\min(d_i)$;

13: **end for**

14: **return** $SP_i$

15: **end if**

16: **end function**
imum value of $Q_2$ for this trajectory and the four strategies analyzed. [$Q_2/\min(Q_2)$]. For the best strategy with respect to this criterion the $Q_{2norm} = 1$.

3. Time for robot:

* $Q_3$: Time taken by the robot to reach the final point (to move from start to desired point including all via points). It is the sum of the duration of all the pre-computed trajectories according to the strategy of motion of the robot according to section 5.3 and the time that the robot waited to have new point where to go.

* $Q_{3norm}$: As for the criterion $Q_2$, we define a normalized criterion, to be able to compare the strategy for several trajectories. Its a ratio of each value of $Q_3$ for a trajectory divide by the minimum value of $Q_3$ for this trajectory and the four strategies analyzed. [$Q_3/\min(Q_3)$]. For the best strategy with respect to this criterion the $Q_{3norm} = 1$.

4. Robot distance:

* $Q_4$: The distance travelled by the robot from start to end point for all via points the robot travels through.

* $Q_{4norm}$: The ratio of distance travelled by the robot (from start to end point for all via points the robot travels through) with distance between start and end point.

5. User distance:

* $Q_5$: The mean distance between the sphere centered on the driver's seat with a radius of 0.5m and all points on the trajectory. This distance is evaluated via the equation (5.1) and characterizes the safety of the user. The further the robot is from this sphere, the safer it is.

5.5.2 Experimental setup

We used the hand motion sensor as a proximity sensor. The objective is to find the closest ROI with respect to the hand. This is an optimization problem of finding a point in a closed set that is closest to a given point. Using the Head-mounted display, we find the points in the user view and if the gaze is directed towards a point $P_i$. We classify the distance of the points in the direction of the gaze as a function of $l_1/L_1$. The user will direct his/her hand towards a capsule (discrete set of $N$ points). The goal is to detect as soon as possible, which point is to be reached by the human and to move the robot to that point:

1. If the direction is known, the robot can be moved to intermediate points to facilitate the task.

2. Safe-points $SP_i$ ($i = 20 : 24$), located outside the car's interior space defined. Between these points the robot can move quickly.

3. At each moment, from the sensor data, we define the target point among the set of $N$ points $P_i$ ($i = 1 : 18$).

Seven trajectories were considered in the experimental design: two consisted of long-distance trajectories (from points 2 to 11 and 5 to 18), three medium distances (from points 5 to 11, 5 to 15, 12 to 15), and two shorter distances (from points 3 to 4 and 17 to 16). The seven trajectories have been done by three different participants.

The participant was seated in the car seat 0.6m above the ground and at a position of 0.9m in $y$ and $-0.1m$ in $x$ from the robot base frame. The sphere used for the obstacle avoidance of the user is centered at this reference point. An HTC Viva HMD was worn by the user and Vive sensors were attached to the user's dominant hand as shown in Figure 5.1. Then for each trajectory, the user was instructed to move his/her hand from a start point to a defined endpoint.
<figure><img src="image_144.png" /><figcaption>Figure 5.18 – User hand trail for motion from point 2 to 11.</figcaption></figure>

<figure><img src="image_145.png" /><figcaption>Figure 5.19 – Points of interest detected for different strategies for a hand motion from 2 to 11.</figcaption></figure>

For each trajectory performed by the user, data were recorded. It comprised of the position of the hand tracker, the head position, and orientation. The user can do the task at the speed he/she wants. During this experiment, the robot was not moved. The data recorded was used to perform the analysis in parallel with the four strategies in order to compare them on the same data set.

### 5.5.3 Analysis of one experiment

The four strategies are described and illustrated on one example, a trajectory done by one subject for moving his/her hand from $P_2$ to $P_{11}$ was recorded. A visual trail of the user hand is shown in Figure 5.18. This recorded motion was used to analyze the four proposed strategies.

#### Detection of points of interest

Figure 5.19 shows the sequence of points that are detected for the four studied strategies. It can be visualized that different strategies have different intermediate points selected except for strategies A and B that produced the same sequence of points detected.

Based on the results from the different strategies, it can be observed that strategies A, B select intermediate points which are inside the car while strategy C and D select the safe-points as some of the intermediate points. For this example, all the strategies allows to find the desired final point $P_{11}$. However the strategy D success to detect this point earlier that the strategies A, B, C that detect the final desired point at the same time (as it can be seen in Table 5.1).

The obtained sequence of points of interest is now detailed:
— Strategy A: $P_2, P_{12}, P_6, P_8, P_9, P_{10}, P_{11}$.

The points are selected based on the least distance to the hand, the points selected can be easily explained by the hand trail described in figure 5.18.

— Strategy B: $P_2, P_{12}, P_6, P_8, P_9, P_{10}, P_{11}$.

The selection of this strategy is similar to strategy A for this example because all points were all the time the point closest to the hand is also closest to the direction of gaze. Since the set of points detected is the same as for the strategy A. The robot motion will be similar and analyzed simultaneously.

— Strategy C: $P_2, SP_{20}, SP_{21}, SP_{23}, P_9, P_{10}, P_{11}$.

For strategy C, a threshold is introduced around the detected points to choose whether the robot should go to the point or to a safe-point. It is observed on Figure 5.18 that the hand passes at a distance > 0.3m from the points $P_{12}, P_6, P_8$. Consequently, the selected points will be the associated safe-points $SP_{20}, SP_{21}$ and $SP_{23}$. Then points $P_9, P_{10}, P_{11}$ were detected and found to be within the required threshold of the distance from the hand.

— Strategy D: $P_2, P_3, SP_{20}, SP_{21}, SP_{22}, SP_{23}, SP_{24}, P_{10}, SP_{24}, P_{11}$.

Strategy D uses the direction of gaze as the primary criterion and it is therefore more difficult to predict the sequence of points detected based on Figure 5.18. In this strategy $P_3$ is selected, it is done based on the user gaze and since the hand moved not far from this point, $P_3$ is selected. For the following points selected by the gaze, the hand is farther from the points so the associated safe-points were selected. Then the gaze is directed toward the point $P_{10}$, since the distance from the hand was below a threshold $T_h$, the point $P_{10}$ was selected. Then the gaze is probably oriented to point $P_{11}$ since it's distance from the hand is above the threshold, the robot has to return to the safe-point $P_{24}$ and finally when the hand is close to the final point $P_{11}$, the point is detected. This happens at a moment when the point $P_{11}$ is not yet the closest point to the hand and is therefore not yet detected by strategy A. The points detected in this example show that the gaze does not go directly to the goal but sweeps along the path accompanying the hand. The results also show a certain sensitivity of the point sequence to the value chosen for the threshold.

## The robot motion for the four strategies

For the three different selections of points (strategies A-B, strategy C, strategy D), the robot motion and the safe distance is now commented. Figures 5.20, 5.21, 5.22 illustrate, starting from the sequence of points detected represented in black as function of time, the corresponding robot motion represented in red as function of time. On the same figure, the distance between the end-effector of the robot and the sphere encompassing the human, is shown in blue as function of time and expressed in meter. This curve is directly calculated based on the robot motion and will be used for the evaluation criterion in Table 5.1.

The sequence of progression of robot motion with time from start to end point is described below:

### Strategy A and B:

The progression of the robot motion is indicated by the red line as shown in Figure 5.20. Starting from point $P_2$, the robot waits for a new point. When $P_{12}$ is detected, the robot starts moving and before it arrives, $P_6$ is detected. However the robot has to continue and complete
<figure><img src="image_146.png" /><figcaption>Figure 5.20 – Robot motion, user distance and time for detection, for strategies A and B for trajectory 2 to 11.</figcaption></figure>

the motion so then arriving at $P_{12}$ the robot now detects $P_{11}$ as new desired point, so the robot moves directly to $P_{11}$. The robot avoids all the points that are detected during the motion towards $P_{12}$. This pattern continues until the robot reaches the last point detected.

## Strategy C:

A graph of robot motion is indicated by the red line as shown in Figure 5.21. The robot starts at $P_2$ and waits for a new point. When point $SP_{20}$ is detected, it starts moving but along the way, a new point $SP_{21}$ is detected, so it has to complete the motion to $SP_{20}$ first. By the time it has reached the point $SP_{20}$, points $SP_{21}, SP_{23}, SP_{24}$ have been detected and passed by the prediction algorithm. All the points that were detected and passed during the motion of the robot have been ignored by the robot. Once a new point has been detected the goal state of the robot updates and neglects the previous points that have not been reached. After reaching $SP_{20}$ the prediction now shows point $P_{11}$ as the desired destination. So the robot moves to $P_{11}$.

<figure><img src="image_147.png" /><figcaption>Figure 5.21 – Robot motion, user distance and time for detection, for strategy C using trajectory 2 to 11.</figcaption></figure>

## Strategy D:

The motion of the robot is indicated by a red line as shown in Figure 5.22. The robot starts from $P_2$ and when $P_3$ is detected, it moves to point $P_3$. When $P_{20}$ is detected, the robot is still in motion to $P_3$, so it ignores the point. Further $SP_{21}, SP_{22}, SP_{23}$ and $SP_{24}$ are detected, but when it the robot reaches $P_3$, the prediction system still predicts $SP_{24}$ as it moves to $SP_{24}$ without waiting. Again during the motion $P_{10}$ and $SP_{24}$ are detected, but before arriving, $P_{11}$ is detected so on arrival at $SP_{24}$, it does not wait but continues to $P_{11}$, where it finally stops.
<figure><img src="image_148.png" /><figcaption>Figure 5.22 – Robot motion, user distance and time for detection, for strategy D using trajectory 2 to 11.</figcaption></figure>

### Criterion analysis for single trajectory

Table. 5.1 shows the complete data for all criteria proposed, for the single trajectory 2-11. $Q_1$ is the success of the strategy detecting the final goal state. It can been seen that all strategies are able to detect the goal point. From the table it can be seen that $Q_{2norm}$ and $Q_{3norm}$ has its best value for strategy D. These strategies have a very fast detection and robot travel time. However the distance traveled is not the best as this strategy allows the selection of safe-points which increase the robot travel distance. When considering safety, strategy D is second best. The best safety $Q_5$ is provided by strategy C.

<table><caption>Table 5.1 – Strategy analysis for the trajectory 2-11</caption><thead><tr><th colspan="8">Trajectory 2-11</th></tr><tr><th>Strategy</th><th>Efficacy</th><th colspan="2">Time for Detection</th><th colspan="2">Time for Robot</th><th colspan="2">Robot Dist.</th><th>User Dist.</th></tr><tr><th></th><th>Q<sub>1</sub></th><th>Q<sub>2</sub></th><th>Q<sub>2norm</sub></th><th>Q<sub>3</sub></th><th>Q<sub>3norm</sub></th><th>Q<sub>4</sub></th><th>Q<sub>4norm</sub></th><th>Q<sub>5</sub></th></tr></thead><tbody><tr><td>St. A</td><td>1</td><td>2.0868</td><td>1.0600</td><td>7.4413</td><td>1.5158</td><td>1.4648</td><td>1.1959</td><td>0.4370</td></tr><tr><td>St. B</td><td>1</td><td>2.0867</td><td>1.0600</td><td>7.4413</td><td>1.5158</td><td>1.4648</td><td>1.1959</td><td>0.4370</td></tr><tr><td>St. C</td><td>1</td><td>2.0867</td><td>1.0600</td><td>5.2969</td><td>1.0790</td><td>1.5690</td><td>1.2810</td><td><strong>0.4804</strong></td></tr><tr><td>St. D</td><td>1</td><td>1.9686</td><td><strong>1</strong></td><td>4.9092</td><td><strong>1</strong></td><td>2.1931</td><td>1.7903</td><td>0.4428</td></tr></tbody></table>

After this detailed analysis for one trajectory, a discussion of the criterion evaluation for seven trajectories recorded is presented in the following sections.

### 5.5.4 Analysis of all recorded experiments

The results obtained are summarized in Table. 5.2. For all the tests, the final desired position is detected, thus criterion efficacy $Q_1$ is one and the criterion is not summarized in the Table 5.2. The analysis of the results will be separated into two parts. Firstly, an observation of the results obtained for the different trials will enable us to arrive at certain conclusions about less variations of the results as function of the trajectories. Then, in a second part, we will use the average values of the different criteria, taking into account the seven trajectories, to highlight the particularities of each strategy by comparing the criteria two by two.
<table><caption>Table 5.2 – Complete analysis for seven trajectories</caption><thead><tr><th rowspan="2"></th><th rowspan="2">Criteria</th><th rowspan="2">Strategy</th><th colspan="2">Long Trajectory</th><th colspan="3">Medium Trajectory</th><th colspan="2">Short Trajectory</th><th colspan="2">Analysis</th></tr><tr><th>2-11</th><th>5-18</th><th>5-11</th><th>5-15</th><th>12-15</th><th>3-4</th><th>17-16</th><th>Mean</th><th>St. Dev</th></tr></thead><tbody><tr><td rowspan="4">Time for Detection</td><td rowspan="4">Q<sub>2</sub></td><td>A</td><td>2.0868</td><td>2.3029</td><td>2.5878</td><td>2.1767</td><td>1.9842</td><td>0.6418</td><td>1.0381</td><td>1.8312</td><td>0.6597</td></tr><tr><td>B</td><td>2.0868</td><td>2.3029</td><td>2.5878</td><td>2.1767</td><td>2.3366</td><td>0.6418</td><td>1.0381</td><td>1.8815</td><td>0.6825</td></tr><tr><td>C</td><td>2.0868</td><td>2.5370</td><td>2.5878</td><td>2.1767</td><td>2.3366</td><td>0.6418</td><td>1.0381</td><td>1.9150</td><td>0.7076</td></tr><tr><td>D</td><td>1.9687</td><td>2.5370</td><td>2.2383</td><td>2.0428</td><td>2.3366</td><td>0.3258</td><td>0.8709</td><td>1.7600</td><td>0.7687</td></tr><tr><td rowspan="4">Time for Robot</td><td rowspan="4">Q<sub>3</sub></td><td>A</td><td>1.0600</td><td>1</td><td>1.1561</td><td>1.0655</td><td>1</td><td>1.9701</td><td>1.1919</td><td>1.2062</td><td>0.3190</td></tr><tr><td>B</td><td>1.0600</td><td>1</td><td>1.1561</td><td>1.0655</td><td>1.1776</td><td>1.9701</td><td>1.1919</td><td>1.2316</td><td>0.3085</td></tr><tr><td>C</td><td>1.0600</td><td>1.1016</td><td>1.1561</td><td>1.0655</td><td>1.1776</td><td>1.9701</td><td>1.1919</td><td>1.2461</td><td>0.2995</td></tr><tr><td>D</td><td>1</td><td>1.1016</td><td>1</td><td>1</td><td>1.1776</td><td>1</td><td>1</td><td><b>1.0399</b></td><td>0.0663</td></tr><tr><td rowspan="4">Time for Robot</td><td rowspan="4">Q<sub>3</sub><sub>norm</sub></td><td>A</td><td>7.4413</td><td>7.2735</td><td>7.5526</td><td>8.0443</td><td>5.4879</td><td>2.4640</td><td>6.9719</td><td>6.4622</td><td>1.7930</td></tr><tr><td>B</td><td>7.4413</td><td>7.2735</td><td>7.3964</td><td>8.0443</td><td>5.4879</td><td>2.4640</td><td>6.9719</td><td>6.4399</td><td>1.7802</td></tr><tr><td>C</td><td>5.2969</td><td>7.2735</td><td>6.6089</td><td>8.0443</td><td>4.3095</td><td>2.4640</td><td>7.9156</td><td>5.9875</td><td>1.9159</td></tr><tr><td>D</td><td>4.9092</td><td>9.8848</td><td>4.9460</td><td>5.8196</td><td>6.0033</td><td>2.1480</td><td>5.7774</td><td>5.6412</td><td>2.1183</td></tr><tr><td rowspan="4">User Dist.</td><td rowspan="4">Q<sub>4</sub><sub>norm</sub></td><td>A</td><td>1.5158</td><td>1</td><td>1.5270</td><td>1.3823</td><td>1.2734</td><td>1.1471</td><td>1.2067</td><td>1.1605</td><td>0.1802</td></tr><tr><td>B</td><td>1.5158</td><td>1</td><td>1.4954</td><td>1.3823</td><td>1.2734</td><td>1.1471</td><td>1.2067</td><td>1.1848</td><td>0.1746</td></tr><tr><td>C</td><td>1.0790</td><td>1</td><td>1.3362</td><td>1.3823</td><td>1</td><td>1.1471</td><td>1.1701</td><td>1.2003</td><td>0.1591</td></tr><tr><td>D</td><td>1</td><td>1.3590</td><td>1</td><td>1</td><td>1.3930</td><td>1</td><td>1</td><td><b>1.0398</b></td><td>0.1701</td></tr><tr><td rowspan="4">User Dist.</td><td rowspan="4">Q<sub>5</sub></td><td>A</td><td><b>1.1959</b></td><td><b>1.4943</b></td><td><b>1.0948</b></td><td><b>1.1582</b></td><td><b>1.0225</b></td><td><b>1.0000</b></td><td><b>1.3042</b></td><td><b>1.1814</b></td><td>0.1601</td></tr><tr><td>B</td><td><b>1.1959</b></td><td><b>1.4943</b></td><td><b>1.0948</b></td><td><b>1.1582</b></td><td><b>1.0225</b></td><td><b>1.0000</b></td><td><b>1.3042</b></td><td><b>1.1814</b></td><td>0.1601</td></tr><tr><td>C</td><td>1.2810</td><td>1.6766</td><td>3.9707</td><td>1.5271</td><td>3.0161</td><td>1.0000</td><td>2.9585</td><td>2.2043</td><td>1.0273</td></tr><tr><td>D</td><td>1.7904</td><td>3.9039</td><td>7.4400</td><td>2.8315</td><td>4.2133</td><td>1.0000</td><td>2.6207</td><td>3.4000</td><td>1.9464</td></tr><tr><td rowspan="4">User Dist.</td><td rowspan="4">Q<sub>5</sub></td><td>A</td><td>0.4370</td><td>0.3301</td><td>0.4846</td><td>0.4143</td><td>0.3285</td><td>0.3535</td><td>0.4429</td><td>0.3987</td><td>0.0570</td></tr><tr><td>B</td><td>0.4370</td><td>0.3301</td><td>0.4846</td><td>0.4143</td><td>0.3285</td><td>0.3535</td><td>0.4429</td><td>0.3987</td><td>0.0570</td></tr><tr><td>C</td><td><b>0.4804</b></td><td><b>0.4134</b></td><td><b>0.4932</b></td><td>0.4149</td><td>0.3165</td><td>0.3535</td><td>0.4684</td><td><b>0.4200</b></td><td>0.0616</td></tr><tr><td>D</td><td>0.4428</td><td>0.3836</td><td>0.4199</td><td><b>0.4327</b></td><td><b>0.3510</b></td><td>0.3535</td><td><b>0.5044</b></td><td>0.4126</td><td>0.0506</td></tr></tbody></table>

## Variations of the results for the different trajectories

For almost all trajectories, the strategy B and A produce the same result, but for one test, the direction of the gaze is not well directed at the end of motion and a delay is observed with strategy B, contrary to what is expected. This delay affects the time of detection and also the time for the motion of the robot (trajectory 12-15).

It can be observed that in all the trials, the method that allowed the fastest detection of the desired point of contact was also the one that allowed the robot to reach this point as quickly as possible.

From the point of view of the efficiency of detection of the contact point, strategies A and D were the most efficient: two times for strategy A and five times for strategy D. There is no clear correlation between the efficiency of the strategy and the length of the trajectory. However for short trajectories, the strategy D is the most efficient.

For all trajectories, the distance traveled by the robot is always smaller for strategies A and B, which was expected as the robot does not move to safe-points. Consequently, in contrary strategies C and D have higher user distance.

## User distance versus time for detection

For the purpose of analyzing, $-Q_5$ has been used so as the goal would be to minimize all the selected criteria. A comparison of $-Q_5$ and $Q_{2norm}$ shows that strategies C and D belong to the Pareto front for these two criteria. Strategy D takes the least time to detect a desired point the user would like to reach and a slightly higher mean distance from the sphere as shown
in Figure 5.23. Strategy C has the largest user distance. However, it takes the longest time to detect a point than all the strategies.

<figure><img src="image_149.png" /><figcaption>Figure 5.23 – Comparison of time for detection ($Q_{2norm}$) vs user distance ($-Q_5$) for the four strategies, all trajectories.</figcaption></figure>

## User distance versus robot distance

A comparison of $-Q_5$ and $Q_{4norm}$ as presented in Figure 5.24 shows that strategies A-B and C belong to the Pareto front for these two criteria. Since strategies A and B don't use any safe points they have smallest user distance (max $-Q_5$) and always take the minimal robot distance to arrive to a desired point (min $Q_{4norm}$). Strategies C and D use safe points so, have higher user distance (min $-Q_5$) and robot distance $Q_{4norm}$. While strategy D uses head gaze as primary selection, it gives a value of $Q_5$ better than A and B but the worst robot distance.

<figure><img src="image_150.png" /><figcaption>Figure 5.24 – Comparison of robot distance ($Q_{4norm}$) vs user distance ($-Q_5$) for the four strategies, all trajectories.</figcaption></figure>

## User distance versus time for robot

A comparison of $-Q_5$ and $Q_{3norm}$ as presented in Figure 5.25 shows that strategy C and D belong to the Pareto front for both criteria. As known Strategy C gives a higher value for safety (min $-Q_5$) followed by strategy D. However strategy D far outperforms strategy C in terms of time for the robot.

## Time for robot versus time for detection

Visualization of $Q_{3norm}$ vs $Q_{2norm}$ is shown in Figure 5.26. It shows that the faster the strategy detects the point, faster the robot reaches the point. The best being strategy D. It uses the head gaze and an added use of safe-points helps it in reaching the desired point faster, as the
<figure><img src="image_151.png" /><figcaption>Figure 5.25 – Comparison of time for robot ($Q_{3norm}$) vs user distance ($-Q_{5}$) for the four strategies, all trajectories.</figcaption></figure>

robot travels at higher velocity than in strategies A and B. Strategy A has lots of intermediate point, but as the hand moves closer to the desired point the robot gradually moves closer. This is one of the reason why this strategy is the second best. Even thought strategies B and C have a head gaze, the primary selection is still based on the hand. Unless the hand is closer to the point the robot does not move to the desired point. Strategy C takes the longest time in both the axis, because the selection process used in strategy C is based primarily on hand location. For strategy D it is the user gaze that help in primary selection of points.

<figure><img src="image_152.png" /><figcaption>Figure 5.26 – Comparison of time for robot ($Q_{3norm}$) vs time for detection ($Q_{2norm}$) for the four strategies, all trajectories.</figcaption></figure>

## Robot distance versus time for robot

Visualization of $Q_{4norm}$ vs $Q_{3norm}$ is shown in Figure 5.27 strategies A and D belong to the Pareto front for these two criteria. In contrary to the assumption that longer robot distance implies longer time for robot, the results show that strategy D has minimum time for robot but has longer robot distance. This result achieved is due to combination of fast time for detection and use of safe-point as intermediate points. Strategy C uses also safe-points, but it alone does not guarantee a fast response time.

## Time for detection versus robot distance

Visualization of $Q_{2norm}$ vs $Q_{4norm}$ is shown in Figure 5.28 strategies A and D belong to the Pareto front for these two criteria. From Figure 5.28, it can be seen that Strategy A and B have least robot distance, but it detects the goal later than Strategy D. Strategy C is not ideal in both criteria.
<figure><img src="image_153.png" /></figure>

Figure 5.27 – Comparison of robot distance ($Q_{4norm}$) vs time for robot ($Q_{3norm}$) for the four strategies, all trajectories.

<figure><img src="image_154.png" /></figure>

Figure 5.28 – Comparison of time for detection ($Q_{2norm}$) vs robot distance ($Q_{4norm}$) for the four strategies, all trajectories.

### 5.5.5 Discussion

A comparative analysis of data from all the trajectories (in Table. 5.3) shows that, if the objective is to maximize safety strategy C and D would be better. Both the strategies C and D ensure safety by selecting safe-points when the hand is far away from the desired point. The safe-points are located outside the user reach, such that the robot can travel fast and does not collide with the user. The selection of the safe-points mean that the robot will have to travel a longer distance to reach the desired point. While for strategies A and B, they select intermediate points which are inside the car and no safe-points. So since the points are all inside the car, and the robot does not a travel longer distance but has reduced velocity.

<table><caption>Table 5.3 – Analysis of strategies for all trajectories.</caption><thead><tr><td>Strategy</td><td>Time for Detection</td><td>Time for Robot</td><td>Robot Dist.</td><td>User Dist.</td></tr></thead><tbody><tr><td>St. A</td><td>+</td><td>+</td><td>++</td><td>-</td></tr><tr><td>St. B</td><td>-</td><td>-</td><td>++</td><td>-</td></tr><tr><td>St. C</td><td>-</td><td>-</td><td>-</td><td>++</td></tr><tr><td>St. D</td><td>++</td><td>++</td><td>-</td><td>+</td></tr></tbody></table>

Strategy D gives second best safety and at the same time minimize the time to detect/reach a desired point. Therefore it can be seen as the best strategy. The detection time for strategy D is the smallest because we used the gaze of the user to pre-select the points. This plays a big role in giving priority to vision information over information from the hand position. Fastest detection time allows the robot to start moving to the desired point at the earliest time and
reach the desired point the fastest.

### 5.5.6 Analysis on hand threshold

In strategy C, the primary selection is based on user hand unlike in strategy D which uses the gaze direction. Since the selection in strategy C depends on the values of hand threshold $T_h$. In this subsections, Strategy C is selected to investigate the effect of each parameter on the time to detect a point, robot time, user distance and Robot distance. The same 7 trajectories recorded by same 3 users were used for this analysis.

The effect is analyzed by adjusting the threshold value $T_h$ using $T_1 = 0.10$ m, $T_2 = 0.15$ m, $T_3 = 0.2$ m, $T_4 = 0.25$ m and $T_5 = 0.3$ m.

<table><caption>Table 5.4 – Analysis for various thresholds</caption><thead><tr><th rowspan="2">$T_h$</th><th colspan="2">$Q_{2norm}$</th><th colspan="2">$Q_{3norm}$</th><th colspan="2">$Q_{4norm}$</th><th colspan="2">$Q_5$</th></tr><tr><th>Mean</th><th>St. Dev</th><th>Mean</th><th>St. Dev</th><th>Mean</th><th>St. Dev</th><th>Mean</th><th>St. Dev</th></tr></thead><tbody><tr><td>0.1</td><td>1.6084</td><td>0.4919</td><td>0.9796</td><td>0.1861</td><td>2.1135</td><td>1.0912</td><td>0.4467</td><td>0.0871</td></tr><tr><td>0.15</td><td>1.4882</td><td>0.4075</td><td>0.9197</td><td>0.2484</td><td>2.0835</td><td>1.0461</td><td>0.4271</td><td>0.0381</td></tr><tr><td>0.2</td><td>1.2461</td><td>0.2995</td><td>1.2003</td><td>0.2214</td><td>2.2043</td><td>1.0273</td><td>0.4200</td><td>0.0616</td></tr><tr><td>0.25</td><td>0.9862</td><td>0.3312</td><td>1.3204</td><td>0.2246</td><td>2.2857</td><td>0.9892</td><td>0.3975</td><td>0.2583</td></tr><tr><td>0.3</td><td>0.7660</td><td>0.3865</td><td>1.5808</td><td>0.2206</td><td>2.3175</td><td>1.0561</td><td>0.3848</td><td>0.2378</td></tr></tbody></table>

#### User distance versus time for detection

A comparison of $-Q_5$ and $Q_{2norm}$ as presented in Figure 5.29 shows that all thresholds from $T_1$ to $T_5$ belong to the Pareto front for these two criteria. $T_1$ takes the highest time to detect a desired point the user would like to reach and it gives the best mean distance from the sphere because the threshold is low so the robot is most of the time in safe points. In case of $T_5$ since the threshold is large the robot is closer to the user.

<figure><img src="image_155.png" /><figcaption>Figure 5.29 – Comparison of time for detection ($Q_{2norm}$) vs user distance ($-Q_5$) for 5 thresholds.</figcaption></figure>

#### User distance versus robot distance

A comparison of $-Q_5$ and $Q_{4norm}$ as presented in Figure 5.30 shows that $T_1$ does not belong to the Pareto front for these two criteria. During the experiments it was realized that with small threshold like $T_1$ the robot sometimes fails to stop after it has detected the point. When the
user has reached the desired point and interacting with the surface. It was noticed that a small variation in hand position causes the robot to return to the safe points. This can be seen in Figure 5.30. $T_1$ despite detecting the point earlier (concluded from Figure 5.29) fails to have the best robot distance.

<figure><img src="image_156.png" /><figcaption>Figure 5.30 – Comparison of robot distance ($Q_{4norm}$) vs user distance ($-Q_5$) for 5 thresholds.</figcaption></figure>

## User distance versus time for robot

A comparison of $-Q_5$ and $Q_{3norm}$ as presented in Figure 5.31 shows that $T_2$, $T_3$, $T_4$ and $T_5$ belong to the Pareto front for both criteria. As known $T_2$ gives a higher value for safety (min $-Q_5$) and time for robot but fails to outperform $T_3$ in detection time (concluded from Figure 5.29).

<figure><img src="image_157.png" /><figcaption>Figure 5.31 – Comparison of time for robot ($Q_{3norm}$) vs user distance ($-Q_5$) for 5 thresholds.</figcaption></figure>

## Time for robot versus time for detection

A comparison of $Q_{3norm}$ vs $Q_{2norm}$ as presented in Figure 5.32 shows that $T_1$ does not belong to the Pareto front for these two criteria. Lower thresholds give a slow response of detection but fast robot time due to use of safe points. Lower thresholds have fewer intermediate point inside the car, but as the hand moves closer to the desired point the robot gradually moves closer. This is one of the reason why lower thresholds are good in robot response time. Even though Strategy C has a head gaze, the primary selection is still based on the hand. Unless the hand is closer to the point the robot does not move to the desired point. If the threshold is big the robot continues to move inside the low velocity region and not pass through the safe-points.
<figure><img src="image_158.png" /><figcaption>Figure 5.32 – Comparison of time for robot ($Q_{3norm}$) vs time for detection ($Q_{2norm}$) for 5 thresholds.</figcaption></figure>

## Robot distance versus time for robot

A comparison of $Q_{4norm}$ vs $Q_{3norm}$ is shown in Figure 5.33. The results are consistent with the assumption that longer robot distance implies longer time for robot, the results show that lower thresholds have minimum robot distance and time for robot. The explanation for such behavior is similar to analysis seen before for $Q_{3norm}$ vs $Q_{2norm}$. Larger thresholds cause the detection of points inside the user space and cause the robot to move in low velocities.

<figure><img src="image_159.png" /><figcaption>Figure 5.33 – Comparison of robot distance ($Q_{4norm}$) vs time for robot ($Q_{3norm}$) for 5 thresholds.</figcaption></figure>

## Robot distance versus time for detection

A comparison of $Q_{2norm}$ vs $Q_{4norm}$ is shown in Figure 5.34. $T_1$ does not belong to the Pareto front for both criteria. It can be seen that $T_2$ and $T_1$ have least robot distance, but it detects the goal later than $T_3$ and $T_4$.

<figure><img src="image_160.png" /><figcaption>Figure 5.34 – Comparison of robot distance ($Q_{4norm}$) vs time for detection ($Q_{2norm}$) for 5 thresholds.</figcaption></figure>
Overall, there was no significant difference between $T_h = 0.10$ m, and 0.15 m for all the objectives $Q_{2norm}$, $Q_{3norm}$, $Q_{4norm}$ and $Q_5$. For this study, the best value to be selected is $T_h = 0.20$ m. Its based on several reasons. First, the task needs user to hold/interact with a surface whose width is 10cm. Secondly, the hand threshold is used to detect the intention of user to move his/her hand to the next point. A sufficient distance between the hand and the surface points was necessary to notice that the human had let it go after touching. Lastly, the tracker is placed on the top of hand, a distance of approximately 4cm from the palm. So the total distance from the center of the prop to the tracker is about about 15cm.

## 5.6 Conclusions

Four motion prediction strategies are used to select the areas with which the user intends to interact and to move the robot as fast as possible while ensuring user safety.

We introduce two speed profiles for the user's safety. The robot moves at a higher speed when it is outside the user's workspace. In situations where there is a large distance between two points within the workspace, we introduce via points to reduce travel time. The time needed to go through via points can be less than the time needed to go directly inside the car while being much safer.

Seven trajectories done by three users were analyzed thank to five criteria. A compromise must be made between user safety and speed for the robot to reach its target.
# Conclusions and future work

## Synopsis

Force feedback interfaces are robotic systems allowing natural motion interactions with virtual or remote environments. They are employed in several domains such as remote handling, manufacturing, entertainment, education, medicine and rehabilitation, just to mention the most popular. In virtual reality applications, the user typically holds a handle that is mechanically linked to the end-effector of the robot. This link has a non-negligible influence since the presence of the robot can be felt even in free space, decreasing the realism of the interaction.

Intermittent contact haptic interfaces are a promising technological development aiming to cope with this issue. These interfaces track and closely follow (without contact) the user movements in free space and come to his/her contact only when force feedback is required. This way ICI haptic interfaces aim to provide more realistic interactions with virtual environments.

Their design and objective evaluation are however particularly complex and there are still challenges to be met.

The focus of this doctoral thesis was on creating a robot control system for haptic-tactile sensation using such ICI systems. A system that is both safe and immersive for the user. The research is guided in 2 major directions:

- Improvement of the performances of ICI interfaces while ensuring high user safety and security.

- Improving the immersive experience of the ICI model by providing improved prediction and robot trajectory planning.

A literature review about ICIs was presented in Chapter 1. This chapter proposed a definition of the term Intermittent Contact Interface and its fundamental notions. Later, the history of haptic device was presented to showcase how the field has evolved over the years. This chapter also contributed with a classification for haptic devices according to their haptic feedback. Then, a hardware review was presented where the research works were classified according to the feedback types. The research works were reviewed according to the type of haptic feedback, interaction, and application. This chapter concluded by raising the research questions to be answered in this thesis based on the field's limitations and research opportunities.

The final and important way forward is to integrate this research with other collaborators. This research dealt with giving a trajectory planning of the robot which is safe for user and gives him independence on selection of interactive points. INRIA team had worked on designing different prop prototypes and rendering immersive interactive haptic displays. CLARTE works to integrate these contributions and also to develop better immersive VR environment.

The major contributions of this doctoral thesis are summarized as follows.
# Contributions

## 1. Improving user safety and security by defining a robot base location.

Chapter 2 presented the choice of a robot base placement to increase the safety when interacting with an ICI systems. The chapter introduced a design dedicated to increasing user safety based on goal positions (interaction points) to estimate the best robot base location. A set of tool orientations based on the TCP design were considered, and different robot workspace were created to explore the different possible base locations for avoiding collisions with the user occluded in haptic display. Later a preliminary simulation was carried with a group of interaction points to reflect the safety precautions such as position of robot far from the user, single base location to reach all the desired intersection points. Then the chapter presented the results of the evaluation made under the criteria previously proposed. Overall, results from this evaluation suggest that a single robot base location with a new table design of the robot was possible to reach all the points of interests in the car, without compromising user safety.

## 2. Improving user safety by building user model, for collision detection.

Chapter 3 introduces a user model designed in ROS based on the vive trackers. The model is designed to actively track the user motion for collision avoidance during robot motion planning. The model tracks both user hands and also his/her torso movement, the accuracy of the model was also analyzed in this chapter. This technique uses four vive trackers, two on each hands of the user. These trackers are used to estimate the position and orientation of the user hand and torso. Key features of the model were portrayed in illustrative scenarios that allowed user model to track the user for obstacle avoidance in Rviz, ROS.

## 3. Motion planning

Chapter 4 presented motion planning and velocity control strategies for the robot, considering collision avoidance based on the user model designed in Chapter 3. This approach uses the idea of restricting the max robot velocity in different regions based on the possible user location and interaction points in the VR. This approach was envisioned to optimize the robot trajectory time to increase the immersion of the user by reducing the wait time for the user to interact with the robot. This was achieved by introducing via points in the region with higher velocity. So the robot passes faster in between points to reach the goal.

## 4. Improving the security and immersion of user by prediction of intention

Chapter 5 studied the design and evaluation of intention prediction techniques for ICIs. First, a design for interaction techniques was presented along with its main features: input, movement control, and contact. Later, the chapter presented a set of interaction techniques conceived from the aforementioned designs. A use-case scenario was created to contextualize the use of the techniques. Later, the techniques were evaluated in a user study to assess user experience and their performance in helping users to accomplish the task. Results suggested that the techniques using the head-gaze from HMD and vive tracker in hand are useful for scenarios that requires detection of early user intention. The designed techniques could be used in other application scenarios and contexts where other types of applications might be required.
# Future work

The presented contributions in this thesis could be further extended as future work by focusing on key areas.

## 1. Improve user safety

In the case of addressing user safety presented in Chapter 4, the motion planning can be improved by considering the motion of the user mannequin for real-time planning scenarios. At present the strategy plans the trajectory offline and uses the appropriate one based on the user prediction. The full potential of the user mannequin has not been used in this case, during the trajectory execution additional features can be added to the planning strategy used to avoid collision with the user.

The single scalable user model can be used to create various sizes of user models and have different set of pre-computed trajectories. These trajectories can be accessed based on the dimension of the user. In the present research a fixed sphere is used to have simple occupancy of the user workspace. But the user sphere can be adjusted based on the user height/user model dimensions to have better user safety. By changing the sphere based on the user, the velocity boundary limits also changes.

Due to the complexity of the system, a complete characterization of real-time dynamic motion planning based on the user model was not possible and is still an open problem. The present approach only does a pre-computed (off-line) trajectory planning, an improved planning system where the trajectories are computed directly and re-planning is possible to have better response time.

## 2. Improve robot response

When addressing the response time of the robot, at present we have introduced velocity limits in different zones (Chapter 5). As mentioned in Chapter 4, the trajectories are computed offline and stored to be accessed later based on the prediction system. Now to increase the response time of the robot one way is to introduce more number of via points for the robot to pass. In present scenario only 23 points are used, 18 for task interaction and 5 safe points (via-points). By introducing multiple points and using the prediction system from Chapter 5, a better response can be achieved. As the robot will start its motion earlier (as points will be detected earlier). A study to analyze the effect of the number of point and relative position or distance would be an interesting direction to explore.

## 3. Improve haptic rendering for complex surfaces

The present research has only considered rendering of small flat surfaces. The research can be extended to have a continuous rendering of a surface. Further developments can be made to render complex planes (not just flat). How to virtually have the sense of infinite surface has been discussed in [88] using a different kind of prop. But the further challenge will be to plan the robot and improve the prediction and trajectory planning to achieve the same.

Another development can be made to render the stiffness of the material the user interacts. The information of different stiffness of the sample materials can be accessed and used to control the robot force impedance. This would improve the immersion and result in better tactile feedback.
Coda

This thesis introduces a technology that seems to be specific to a specific context, but such
technologies have a huge potential. This technology can also be used in entertainment industry
and improved to augmented reality. Research and development in such technology have a effect
on humanity and I hope my contribution to this has helped in developing towards the betterment
of it.
# Personal publications

## Journal publications

1. Guda, V. K., Mugisha, S. Chevallereau, C., Zoppi, M., Molfino, R., Chablat, D. (2022). Motion Strategies for a Cobot in a Context of Intermittent Haptic Interface. Journal of Mechanisms and Robotics.

2. Mugisha, S., Guda, V. K., Chevallereau, C., Zoppi, M., Molfino, R., Chablat, D. (2022). Improving Haptic Response for Contextual Human Robot Interaction. Sensors, 22(5), 2040.

## Conference publications

1. Guda, V. K., Chablat, D., Chevallereau, C. (2020, July). Safety in a human robot interactive: Application to haptic perception. In International Conference on Human-Computer Interaction (pp. 562-574). Springer, Cham.

2. Mugisha, S., Zoppi, M., Molfino, R., Guda, V., Chevallereau, C., Chablat, D. (2021, August). Safe collaboration between human and robot in a context of intermittent haptique interface. In International Design Engineering Technical Conferences and Computers and Information in Engineering Conference (Vol. 85451, p. V08BT08A007). American Society of Mechanical Engineers.

3. Gutierrez, A., Guda, V., Mugisha, S., Chevallereau, C., Chablat, D. (2022, June). Trajectory planning in Dynamics Environment: Application for Haptic Perception in Safe Human-Robot Interaction. In 24TH International Conference on Human-Computer Interaction.
<figure><img src="image_161.png" /></figure>
# Appendix A

## UR-5 kinematics

### Introduction

For the remainder of the document, we use the notation $c_i = \cos \theta_i$, $s_i = \sin \theta_i$, and for angle sums, $c_{ij} = \cos(\theta_i + \theta_j)$.

### Forward Kinematics

We first begin by giving the forward kinematics, describing the position of the end effector as a function of joint angles. The transformation matrix is defined as:

$$ ^0T_6 (\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) = \begin{bmatrix} ^0R_6 & ^0p_6 \\ 0 & 1 \end{bmatrix} \qquad (A.1) $$

$$ ^0T_6 (\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) = \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

We can split the transformation matrix is given as a chain of transformations, one for each joint:

$$ ^0T_6 (\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6) = ^0T_1 (\theta_1)^1 ^0T_2 (\theta_2)^2 ^0T_3 (\theta_3)^3 ^0T_4 (\theta_4)^4 ^0T_5 (\theta_5)^5 ^0T_6 (\theta_6) \qquad (A.2) $$

The kinematic structure of the UR-5 robot in zero position is shown in Figure A.1:

The DH-parameters shown are specified as:

- $a_i$ = distance from $z_i$ to $z_{i+1}$ measured along $x_i$

- $\alpha_i$ = angle from $z_i$ to $z_{i+1}$ measured about $x_i$

- $d_i$ = distance from $x_{i-1}$ to $x_i$ measured along $z_i$

- $\theta_i$ = angle from $x_{i-1}$ to $x_i$ measured about $z_i$

For the UR-5, the DH parameters are shown in Table A.1. The DH-parameters can be used to write the transformations for each link. The general transformation between link i-1 and i is given by:

$$ ^{i-1}T_i = \begin{bmatrix} \cos \theta_i & -\sin \theta_i & 0 & a_{i-1} \\ \sin \theta_i \cos(\alpha_{i-1}) & \cos \theta_i \cos(\alpha_{i-1}) & -\sin(\alpha_{i-1}) & -\sin(\alpha_{i-1})d_i \\ \sin \theta_i \sin(\alpha_{i-1}) & \cos \theta_i \sin(\alpha_{i-1}) & \cos(\alpha_{i-1}) & \cos(\alpha_{i-1})d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \qquad (A.3) $$
<figure><img src="image_162.png" /><figcaption>Figure A.1 – Coordinate frames for UR5 manipulator. Joints rotate around the z-axis and are pictured at $\theta_i = 0$ for $1 \le i \le 6$</figcaption></figure>

<table><caption>Table A.1 – Modified Denavit-Harteberg parameters (DH-parameters) of a UR5 robot, corresponding to the frames in Figure A.1. The parameters $\theta_i$ are variables and the remaining parameters are constants.</caption><thead><tr><th>i</th><th>a<sub>i-1</sub></th><th>α<sub>i-1</sub></th><th>d<sub>i</sub></th><th>θ<sub>i</sub></th></tr></thead><tbody><tr><td>1</td><td>0</td><td>0</td><td>d<sub>1</sub></td><td>θ<sub>1</sub></td></tr><tr><td>2</td><td>0</td><td>π/2</td><td>0</td><td>θ<sub>2</sub></td></tr><tr><td>3</td><td>a<sub>2</sub></td><td>0</td><td>0</td><td>θ<sub>3</sub></td></tr><tr><td>4</td><td>a<sub>3</sub></td><td>0</td><td>d<sub>4</sub></td><td>θ<sub>4</sub></td></tr><tr><td>5</td><td>0</td><td>π/2</td><td>d<sub>5</sub></td><td>θ<sub>5</sub></td></tr><tr><td>6</td><td>0</td><td>-π/2</td><td>d<sub>6</sub></td><td>θ<sub>6</sub></td></tr></tbody></table>
It is straightforward to write the transformation matrix for each link of the UR-5 robot by inserting the DH-parameters from Table A.1 in general transformation equation. The complete transformation from base to end-effector can then be derived by multiplication of all 6 transformation matrices. The result is analytical expressions for all 12 parameters in the transformation matrix $^0T_6$. The complete analytic equations can be found below:

$$n_x = c_6(s_1s_5 + ((c_1c_{234} - s_1s_{234})c_5)/2.0 + ((c_1c_{234} - s_1s_{234})c_5)/2.0) - (s_6((s_1c_{234} + c_1s_{234}) - (s_1c_{234} - c_1s_{234}))))/2.0$$

$$n_y = c_6(((s_1c_{234} + c_1s_{234})c_5)/2.0 - c_1s_5 + ((s_1c_{234} - c_1s_{234})c_5)/2.0) + s_6((c_1c_{234} - s_1s_{234})/2.0 - (c_1c_{234} - s_1s_{234})/2.0))$$

$$n_z = (s_{234}c_6 + c_{234}s_6)/2.0 + s_{234}c_5c_6 - (s_{234}c_6 - c_{234}s_6)/2.0$$

$$o_x = -(c_6((s_1c_{234} + c_1s_{234}) - (s_1c_{234} - c_1s_{234}))) / 2.0 - s_6(s_1s_5 + ((c_1c_{234} - s_1s_{234})c_5) / 2.0 + ((c_1c_{234} + s_1s_{234})c_5) / 2.0)$$

$$o_y = c_6((c_1c_{234} - s_1s_{234})/2.0 - (c_1c_{234} + s_1s_{234})/2.0) - s_6(((s_1c_{234} + c_1s_{234})c_5)/2.0 - c_1s_5 + ((s_1c_{234} - c_1s_{234})c_5)/2.0))$$

$$o_z = (c_{234}c_6 + s_{234}s_6)/2.0 + (c_{234}c_6 - s_{234}s_6)/2.0 - s_{234}c_5s_6$$

$$a_x = c_5s_1 - ((c_1c_{234} - s_1s_{234})s_5)/2.0 - ((c_1c_{234} + s_1s_{234})s_5)/2.0$$

$$a_y = -c_1c_5 - ((s_1c_{234} + c_1s_{234})s_5)/2.0 + ((c_1s_{234} - s_1c_{234})s_5)/2.0$$

$$a_z = (c_{234}c_5 - s_{234}s_5)/2.0 - (c_{234}c_5 + s_{234}s_5)/2.0$$

$$p_x = -(d_5(s_1c_{234} - c_1s_{234}))/2.0 + (d_5(s_1c_{234} + c_1s_{234}))/2.0 + d_4s_1 - (d_6(c_1c_{234} - s_1s_{234})s_5)/2.0 - (d_6(c_1c_{234} + s_1s_{234})s_5)/2.0 + a_2c_1c_2 + d_6c_5s_1 + a_3c_1c_2c_3 - a_3c_1s_2s_3$$

$$p_y = -(d_5(c_1c_{234} - s_1s_{234}))/2.0 + (d_5(c_1c_{234} + s_1s_{234}))/2.0 - d_4c_1 - (d_6(s_1c_{234} + c_1s_{234})s_5)/2.0 - (d_6(s_1c_{234} - c_1s_{234})s_5)/2.0 - d_6c_1c_5 + a_2c_2s_1 + a_3c_2c_3s_1 - a_3s_1s_2s_3$$

$$p_z = d_1 + (d_6(c_{234}c_5 - s_{234}s_5))/2.0 + a_3(s_2c_3 + c_2s_3) + a_2s_2 - (d_6(c_{234}c_5 + s_{234}s_5))/2.0 - d_5c_{234}$$

## Inverse kinematic solution for UR-5, 6-DoF arm

The inverse kinematic (IK) equations calculates the joint angles $\theta_{1-6}$ based on a desired position and orientation of the final frame, specified as the transformation $^0T_6$.

The analytic inverse kinematics problem is to find the set of joint configurations $Q = q_i$ where $q_i = (\theta_1^i, \theta_2^i, \theta_3^i, \theta_4^i, \theta_5^i, \theta_6^i) \in [0, 2\pi)^6$ that satisfies

$$^0T_6(\theta_1^i, \theta_2^i, \theta_3^i, \theta_4^i, \theta_5^i, \theta_6^i) = (^0T_6^d) = \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \quad (A.4)$$

where $^0T_6^d$ describes the desired position and orientation of the final link.
<figure><img src="image_163.png" /><figcaption>Figure A.2 – Illustration of the geometry of finding $\theta_1$. This is essentially an overhead view of the robot, looking down on the x-y plane.</figcaption></figure>

## Finding $\theta_1$

To find $\theta_1$, we first determine the location of frame 5 (the wrist frame) in relation to the base frame; $^0p_{05}$. As illustrated in Figure A.2, $^0p_{05}$ can be found by translating backwards from frame 6 to frame 5 along $z_6$. Remember that both $^0T_6$ and $d_6$ are known. Analyzing the transformation from frame 1 to frame 5 using equations (1) and (2), we can form the equality

$$ [^1T_5]_{\text{LHS}} = (^1T_5]_{\text{RHS}} \qquad (A.5) $$

$$ [(^0T_1)^{-1} (^0T_6^d) (^5T_6)^{-1}]_{\text{LHS}} = [(^1T_2) (^2T_3) (^3T_4) (^4T_5)]_{\text{RHS}} \qquad (A.6) $$

We can then see that the y-coordinate of the position of this frame is

$$ [(^1p_{15})_y]_{\text{LHS}} = (^0y_1)^T (^0p_{05}) \qquad (A.7) $$

$$ = [-s_1 \quad c_1 \quad 0] \begin{bmatrix} p_x - d_6 z_x \\ p_y - d_6 z_y \\ p_z - d_6 z_z \end{bmatrix} \qquad (A.8) $$

$$ = (p_x - d_6 z_x)(-s_1) + (p_y - d_6 z_y)(c_1) \qquad (A.9) $$

$$ [(^1p_{15})_y]_{\text{RHS}} = -d_4 \qquad (A.10) $$

The equation

$$ (p_x - d_6 z_x)(-s_1) + (p_y - d_6 z_y)(c_1) = -d_4 \qquad (A.11) $$

is known as a phase-shift equation whose solution can be found as

$$ \tan \alpha_1 = \frac{(^0p_{05})y}{(^0p_{05})x} \qquad (A.12) $$
$$ \cos \alpha_2 = \frac{d_4}{R} = \frac{d_4}{\sqrt{({}^0 p_{05})_x^2 + ({}^0 p_{05})_y^2}} \qquad (A.13) $$

$$ \theta_1 = \alpha_1 + \alpha_2 + \pi/2 \qquad (A.14) $$

$$ = \arctan 2(({}^0 p_{05})_y, ({}^0 p_{05})_x) \pm \cos^{-1} \frac{d_4}{R} + \pi/2 \qquad (A.15) $$

We can see that there are generally 2 solutions for $\theta_1$, which correspond to configurations where the shoulder is "left" or "right". Using the function atan2 is essential for insuring correct signs and behavior when $(^0 p_{05})x = 0$. By looking at the Figure A.2, it is easy to see that physically, no configuration is possible which makes $\sqrt{(}^0 p_{05})_x^2 + (^0 p_{05})_y^2 \le |d_4| \le 0$. Thus, both $\alpha_1$ and $\alpha_2$ always exist if an inverse solution exists.

## Finding $\theta_5$

Given a particular $\theta_1$, we can solve for $\theta_5$. Using the transformation from frame 1 to frame 6, we can form the equality

$$ [({}^0 T_1)^{-1}({}^0 T_6^d)]_{\text{LHS}} = [({}^1 T_2)({}^2 T_3)({}^3 T_4)({}^4 T_5)({}^5 T_6)]_{\text{RHS}} \qquad (A.16) $$

We can then see that the y-coordinate of the position of this frame is

$$ [({}^1 p_{16})_y]_{\text{LHS}} = ({}^0 y_1)^T ({}^0 p_{06}) \qquad (A.17) $$

$$ = \begin{bmatrix} -s_1 & c_1 & 0 \end{bmatrix} \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} \qquad (A.18) $$

$$ = p_x(-s_1) + p_y(c_1) \qquad (A.19) $$

$$ [({}^1 p_{16})_y]_{\text{RHS}} = -d_4 - c_5 d_6 \qquad (A.20) $$

$$ \theta_5 = \pm \cos^{-1} \frac{p_x s_1 - p_y c_1 - d_4}{d_6} \qquad (A.21) $$

Again, we find that there are 2 solutions for $\theta_5$, which correspond to configurations where the wrist is "in/down" or "out/up". This occurs due to the fact that the joint sum $\theta_{234}$ can allow the $^1 T_5$ to achieve orientations where $^1 y_5$ is pointing in the same direction, but that $^1 z_5$ is pointing in the opposite direction.

This flip can then be reversed very simply by the 6th joint.

## Finding $\theta_6$

This joint has a solution so long as the argument of $\cos^{-1}$ has magnitude not greater than 1, or $|({}^1 p_{16})_y - d_4| \le |d_6|$. To solve for the 6th joint, we look at the $^6 y_1$ coordinate axis:

$$ [{}^6 y_1]_{\text{LHS}} = \begin{bmatrix} -x_x s_1 + x_y c_1 \\ -y_x s_1 + y_y c_1 \\ -z_x s_1 + z_y c_1 \end{bmatrix} \qquad (A.22) $$
<figure><img src="image_164.png" /><figcaption>Figure A.3 – Illustration of the geometry of finding $\theta_5$. This is essentially an overhead view of the robot, looking down on the x-y plane.</figcaption></figure>

$$ [^6y_1]_{\text{RHS}} = \begin{bmatrix} -c_6s_5 \\ s_6s_5 \\ -c_5 \end{bmatrix} \qquad (A.23) $$

As shown in Figure A.4, this equality forms a spherical coordinate expression for the vector $-^6y_1$ where $\theta_6$ is the azimuthal angle and $\theta_5$ is the polar angle. The x and y coordinates of this vector form a system which can be easily solved as

$$ \theta_6 = \arctan\left(\frac{-y_x s_1 + y_y c_1}{s_5}, \frac{-x_x s_1 + x_y c_1}{s_5}\right) \qquad (A.24) $$

The denominators of each argument can be replaced by $\text{sign}(s_5)$. This solution is undefined in two circumstances, when both of the numerators are 0 or $s_5 = 0$. Inspection of equations (15) and (16) shows that these conditions actually imply each other. When $s_5 = 0$, we know $c_5 = \pm 1$, which indicates that the joints 2, 3, 4, and 6 are all parallel and the solution is under determined. When this occurs, a desired $\theta_6$ can be supplied to fully determine the system.

## Finding $\theta_3$

The final 3 joints can be found easily, understanding that they together form a classical 3R planar arm. Since we have the other 3 joints, we solve for the location of the base and end effector of the 3R arm, and use those equations to solve. The solution has two possible configurations, where the arm is elbow "up" or "down". No solutions exist when the distance to the 4th joint exceeds the sum $|a_2 + a_3|$ or is less than the difference $|a_2 - a_3|$. If $a_2 = a_3$, a displacement singularity exists when $\theta_3 = \pi$, making $\theta_2$ arbitrary.
<figure><img src="image_165.png" /><figcaption>Figure A.4 – Overhead view of the kinematics problem, for computing $\theta_6$.</figcaption></figure>

We can constrict ourselves to look at $^1T_4$ (frame 4 in relation to frame 1) because $^1T_0$, $^4T_5$ and $^5T_6$ at this point are known. This transformation is illustrated in the x; z-plane of frame 1 in Figure A.5. From the figure it is clear that the length of the translation $|^1p_{14xz}|$ is determined only by $\theta_3$, or similarly by $\alpha_3$. The angle $\alpha_3$ can be found by using the law of cosine:

$$ \cos \alpha_3 = \frac{(-a_2)^2 + (-a_3)^2 - |^1p_{14xz}|^2}{2(-a_2)(-a_3)} \qquad (A.25) $$

The relationship between $\cos \alpha_3$ and $\cos \theta_3$ is:

$$ \cos\theta_3 = \cos(\pi - \alpha_3) = -\cos(\alpha_3) \qquad (A.26) $$

Combining above 2 equations:

$$ \theta_3 = \pm \arccos \frac{|^1p_{14xz}|^2 - a_2^2 - a_3^2}{2a_2a_3} \qquad (A.27) $$

## Finding $\theta_2$

The angle $\theta_2$ can be found as $\alpha_1 - \alpha_2$. Each of these can be found by inspecting Figure A.5 and using atan2 and sine relations:

$$ \alpha_1 = \arctan(-^1p_{14z}, -^1p_{14x}) \qquad (A.28) $$

$$ \alpha_2 = \arcsin \frac{-a_3 \sin \alpha_3}{|^1p_{4xz}|} \qquad (A.29) $$

We can replace $\alpha_3$ with $\theta_3$ by noticing that $\sin \alpha_3 = \sin(\pi - \theta_3) = \sin\theta_3$. Combining the equations now give:

$$ \theta_2 = \alpha_1 - \alpha_2 = \arctan(-^1p_{14z}, -^1p_{14x}) - \arcsin\left(\frac{-a_3 \sin \alpha_3}{|^1p_{14xz}|}\right) \qquad (A.30) $$

## Finding $\theta_4$

The last remaining angle $\theta_4$ is defined as the angle from $x_3$ to $x_4$ measured about $z_4$. It can thus easily be derived from the last remaining transformation matrix, $^3T_4$, using its first column $^3x_4$:

$$ \theta_4 = \arctan(^3x_{4y}, ^3x_{4x}) \qquad (A.31) $$
<figure><img src="image_166.png" /><figcaption>Figure A.5 – Overhead view of the 3R planar manipulator constituted together by joints 2,3 and 4. Kinematics problem, for computing $\theta_3$, $\theta_2$ and $\theta_4$.</figcaption></figure>

Summary

To sum up, a total of 8 solutions exist in general for the general inverse kinematic problem of the UR5: $2\theta_1 \times 2\theta_5 \times \theta_6 \times 2\theta_3 \times \theta_2 \times \theta_4$.
# Appendix B

## Error analysis for user model

<figure><img src="image_167.png" /><figcaption>Figure B.1 – Error for position in shoulder sensor in right hand (Y-axis in cm).</figcaption></figure>
<figure><img src="image_168.png" /><figcaption>Figure B.2 – Error for position in shoulder sensor in left hand (Y-axis in cm).</figcaption></figure>
<figure><img src="image_169.png" /><figcaption>Figure B.3 – Error for position in wrist sensor in right hand (Y-axis in cm).</figcaption></figure>
<figure><img src="image_170.png" /><figcaption>Figure B.4 – Error for position wrist sensor in left hand (Y-axis in cm).</figcaption></figure>
<figure><img src="image_171.png" /><figcaption>Figure B.5 – Error between actual and estimated value of right shoulder frame.</figcaption></figure>
<figure><img src="image_172.png" /><figcaption>Figure B.6 – Error between actual and estimated value of left shoulder frame.</figcaption></figure>
# Appendix C

## What is MoveIt?

### Introduction

The MoveIt architecture is based on two main nodes, the *move_group* node, and the *plan-ningScene* (Figure C.1). The *move_group* takes care of obtaining the parameters, the setup, and the individual components of the robot model being used, so it can provide to the user *ROS* services and actions for the users to use on the robot. The *move_group* node can be divided into three parts (Figure C.2):

1. The user interface: This takes care of handling the inputs given by the user in order to perform actions on the robot, this can be done by:

— GUI through MoveIt RViz plugin.

— APIs for C++ or python.

Based on the actions given, the *move_group* node is able to solve the robot kinematics and is able to plan a trajectory that takes into account the defined scene, obstacles, and constraints of the system.

2. The ROS Param Server: Which obtains the robot description through *URDF*, *SRDF* and configuration files. Obtaining specifications such as joint limits, velocity limits, or other constraints for the system.

3. The Robot Interface: This provides information obtained from the robot sensors (2D or 3D) or the robot joint states provided by the controllers.

The second element which is the *planningScene* uses the *planning scene monitor* to handle the scene in which the robot will be included. This is what will constitute the obstacles or elements the robot has to interact with. Providing information about where they are located, and any updates that happen in the robot's surroundings. It uses as well the robot interface data to make the connection between the data being processed and the objects defined in the scene.

### Motion Planning

MoveIt works with motion planners through a plugin interface. This allows MoveIt to communicate with and use different motion planners from multiple libraries, making MoveIt easily extensible. The interface to the motion planners is through a *ROS Action or service* (offered by the *move_group* node). The default motion planners for *move_group* are configured using
<figure><img src="image_173.png" /><figcaption>Figure C.1 – MoveIt system architecture.</figcaption></figure>
<figure><img src="image_174.png" /><figcaption>Figure C.2 – Move group architecture.</figcaption></figure>

OMPL [126] and the MoveIt interface to OMPL by the MoveIt Setup Assistant. Other planners that are available by default are the *Pilz* industrial motion planner and *CHOMP*.

Within the planners available in the OMPL library there are:

1. PRM methods:

— PRM [69]

— PRM* [68]

— LazyPRM [12]

— LazyPRM* [12] [68]

2. RRT methods:

— RRT [80]

— RRT* [68]

— TRRT [63]

— BiTRRT [32]

— LBTRRT [115]

— RRTConnect [78]

3. Expansive Spacial Trees (EST) methods:

— EST [60]

— BiEST [60]

Collision detection

Collision checking in MoveIt is configured inside a Planning Scene using the `CollisionWorld` object. Fortunately, MoveIt is set up so that users never really have to worry about how
<figure><img src="image_175.png" /><figcaption>Figure C.3 – Planning scene architecture.</figcaption></figure>
<figure><img src="image_176.png" /></figure>
Bibliography

[1] 3D Systems (2018), *Touch haptic device*. Feb. 2022. URL: https://www.3dsystems.com/haptics-devices/touch.

[2] Iina AALTONEN, Timo SALMI, and Ilari MARSTIO. "Refining levels of collaboration to support the design and evaluation of human-robot interaction in the manufacturing industry". In: *Procedia CIRP* 72 (2018), pp. 93–98.

[3] A. ABBADI and V. PERNOSIL. "Collided Path Replanning in Dynamic Environments UsingRRT and Cell Decomposition Algorithms". In: *International Workshop on Modelling and Simulation for Autonomous Systems* (2015).

[4] Rajesh AGGARWAL and Ara DARZI. "From scalpel to simulator: a surgical journey". In: *Surgery* 145.1 (2009), pp. 1–4.

[5] Stanford Artificial Intelligence Laboratory et al. *Robotic Operating System*. Version ROS Melodic Morenia. May 23, 2018. URL: https://www.ros.org.

[6] KN AN, LJ ASKEW, and EY CHAO. "Biomechanics and functional assessment of upper extremities". In: *Trends in ergonomics/human factors III*. Vol. 1986. Elsevier Science Publishers North-Holland. 1986, pp. 573–580.

[7] Bruno ARAUJO et al. "Snake Charmer: Physically Enabling Virtual Objects". In: *Proceedings of the TEI'16: Tenth International Conference on Tangible, Embedded, and Embodied Interaction*. ACM. 2016, pp. 218–226.

[8] Jon Louis BENTLEY. "Multidimensional Binary Search Trees Used for Associative Searching". In: *Commun. ACM* 18.9 (Sept. 1975), pp. 509–517. ISSN: 0001-0782. DOI: 10.1145/361002.361007. URL: https://doi.org/10.1145/361002.361007.

[9] Massimo BERGAMASCO. "Haptic interfaces: the study of force and tactile feedback systems". In: *Proceedings 4th IEEE International Workshop on Robot and Human Communication*. IEEE. 1995, pp. 15–20.

[10] Tiziano BERNARD et al. "Haptic feedback astronaut suit for mitigating extra-vehicular activity spatial disorientation". In: *AIAA SPACE and Astronautics Forum and Exposition*. 2017, p. 5113.

[11] S James BIGGS and Mandayam A SRINIVASAN. "Haptic interfaces". In: *Handbook of virtual environments* (2002), pp. 93–116.

[12] R. BOHLIN and L.E. KAVRAKI. "Path Planning Using Lazy PRM". In: *IEEE International Conference on Robotics and Automation* (2000), pp. 521–528. DOI: 10.1109/ROBOT.2000.844107.
[13] Aude BOLOPION and Stéphane RÉGNIER. "A review of haptic feedback teleoperation systems for micromanipulation and microassembly". In: *IEEE Transactions on automation science and engineering* 10.3 (2013), pp. 496-502.

[14] Diego BORRO et al. "A large haptic device for aircraft engine maintainability". In: *IEEE Computer Graphics and Applications* 24.6 (2004), pp. 70-74.

[15] Stuart A BOWYER, Brian L DAVIES, and Ferdinando Rodriguez y BAENA. "Active constraints/virtual fixtures: A survey". In: *IEEE Transactions on Robotics* 30.1 (2013), pp. 138-157.

[16] Thurston L BROOKS. "Telerobotic response requirements". In: *1990 IEEE international conference on systems, man, and cybernetics conference proceedings*. IEEE. 1990, pp. 113-120.

[17] Frederick P BROOKS JR et al. "Project GROPEHaptic displays for scientific visualization". In: *ACM SIGGraph computer graphics* 24.4 (1990), pp. 177-185.

[18] Grigore BURDEA. "The role of haptics in physical rehabilitation". In: *Haptic Rendering* (2008), pp. 517-529.

[19] Grigore BURDEA and Philippe COIFFET. *Virtual reality technology*. John Wiley & Sons, 2003.

[20] Grigore C BURDEA. *Force and touch feedback for virtual reality*. John Wiley & Sons, Inc., 1996.

[21] Gianni CAMPION. *The Synthesis of Three Dimensional Haptic Textures: Geometry, Control, and Psychophysics*. Springer Science & Business Media, 2011.

[22] Anthony CHABRIER et al. "Design and experimental evaluation of an infrared instrumentation for haptic interfaces". In: *2017 IEEE International Instrumentation and Measurement Technology Conference (I2MTC)*. IEEE. 2017, pp. 1-6.

[23] Alan CHALMERS, David HOWARD, and Christopher MOIR. "Real virtuality: A step change from virtual reality". In: *Proceedings of the 25th Spring Conference on Computer Graphics. 2009*, pp. 9-16.

[24] Adrian David CHEOK and Kasun KARUNANAYAKA. *Virtual taste and smell technologies for multisensory internet and virtual reality*. Springer, 2018.

[25] Andrea CHERUBINI et al. "Collaborative manufacturing with physical human-robot interaction". In: *Robotics and Computer-Integrated Manufacturing* 40 (2016), pp. 1-13.

[26] Michael COHEN and Elizabeth M. WENZEL. "The Design of Multidimensional Sound Interfaces". In: *Virtual Environments and Advanced Interface Design*. USA: Oxford University Press, Inc., 1995, pp. 291-346. ISBN: 0195075552.

[27] D. COLEMAN et al. "Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study". In: *Journal of Software Engineering for Robotics* 5(1) (2014), pp. 3-16. URL: https://moveit.ros.org/.

[28] C.I. CONOLLY, J.B. BURNS, and R. WEISS. "Path Planning Using Laplace's Equation". In: *IEEE International Conference on Robotics and Automation* (1990).

[29] Damien COUROUSSÉ. *Mechanical impedance*. 2007.

[30] *CyberGlove Systems*. Jan. 2022. URL: http://www.cyberglovesystems.com/cybertouch.
[31] Desktop haptic interface. Feb. 2022. URL: http://www.moog.com/products/haptics-robotics/..

[32] D. DEVAURS, T. SIMEON, and J. CORTES. "Enhancing the Transition-based RRT to Deal with Complex Cost Spaces". In: *Proceedings - IEEE International Conference on Robotics and Automation (ICRA)* (2013), pp. 4120-4125. DOI: 10.1109/ICRA.2013.6631158.

[33] E. DIJKSTRA. "A Note on Two Problems in Connexion with Graphs Numerische Mathematik". In: *Journal of Computer and System Sciences - JCSS* (1959).

[34] Adam DREWNOWSKI. "Taste preferences and food intake". In: *Annual review of nutrition* 17.1 (1997), pp. 237-253.

[35] Mark Wm DUBIN. *How the brain works*. John Wiley & Sons, 2013.

[36] Michael EISENSTEIN. "Taste: More than meets the mouth". In: *Nature* 468.7327 (2010), S18-S19.

[37] Abdulmotaleb EL SADDIK. *Haptics rendering and applications*. BoD-Books on Demand, 2012.

[38] Abdulmotaleb EL SADDIK. "The Potential of Haptics Technologies". In: *IEEE Instrumentation Measurement Magazine* 10.1 (2007), pp. 10-17. DOI: 10.1109/MIM.2007.339540.

[39] FCL flexible collision library. June 2019. URL: https://github.com/flexible-collision-library/fcl.

[40] FeelReal. Jan. 2022. URL: https://feelreal.com.

[41] C. FEICHTENHOFER, A. PINZ, and A. ZISSERMAN. "Convolutional Two-Stream Network Fusion for Video Action Recognition". In: *2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR)*. June 2016, pp. 1933-1941. DOI: 10.1109/CVPR.2016.213.

[42] Alessandro FILIPPESCHI et al. "Encountered-type haptic interface for virtual interaction with real objects based on implicit surface haptic rendering for remote palpation". In: *2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE. 2015, pp. 5904-5909.

[43] Force Dimension (2018). sigma.7. Feb. 2022. URL: http://www.forcedimension.com/products/sigma-7/overview/..

[44] Jerome H. FRIEDMAN, Jon Louis BENTLEY, and Raphael Ari FINKEL. "An Algorithm for Finding Best Matches in Logarithmic Expected Time". In: *ACM Trans. Math. Softw.* 3.3 (Sept. 1977), pp. 209-226. ISSN: 0098-3500. DOI: 10.1145/355744.355745. URL: https://doi.org/10.1145/355744.355745.

[45] Jonathan D. GAMMELL, Siddhartha S. SRINIVASA, and Timothy D. BARFOOT. "Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic". In: *2014 IEEE/RSJ International Conference on Intelligent Robots and Systems* (2014), pp. 2997-3004. DOI: 10.1109/IROS.2014.6942976.

[46] A. GASPARETTO et al. "Path Planning and Trajectory Planning Algorithms: A General Overview". In: *Motion and Operation Planning of Robotic Systems* 29 (2015).

[47] G. GHINEA, F. ANDRES, and S. GULLIVER. *Multiple Sensorial Media Advances and Applications: New Developments in MulSeMedia*. Premier Reference Source. Information Science Reference, 2012. ISBN: 9781609608217. URL: https://books.google.fr/books?id=PjyOngEACAAJ.
[48] Franck GONZALEZ, Wael BACHTA, and Florian GOSSELIN. "Smooth transition-based control of encounter-type haptic devices". In: *2015 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE. 2015, pp. 291-297.

[49] Florian GOSSELIN et al. "Haptic systems for training sensorimotor skills: a use case in surgery". In: *Robotics and Autonomous Systems* 61.4 (2013), pp. 380-389.

[50] V.K. GUDA, D. CHABLAT, and Chevallereau C. "Safety in a Human Robot Interactive: Application to Haptic Perception". In: *International Conference on Human-Computer Interaction HCII 2000*. 2000.

[51] Blake HANNAFORD and Allison M OKAMURA. "Haptics". In: *Springer Handbook of Robotics*. Springer, 2016, pp. 1063-1084.

[52] HaptX. Jan. 2022. URL: https://haptx.com.

[53] P.E. HART, N.J. NILSON, and Raphael B. "A Formal Basis for the Heuristic Determination of Minimum Cost Paths." In: *IEEE Transactions on Systems Science and Cybernetics* 4(2) (1968), pp. 100-107.

[54] Paul HAVIG, John MCINTIRE, and Eric GEISELMAN. "Virtual reality in a cave: limitations and the need for HMDs?" In: *Head-and helmet-mounted displays XVI: Design and applications*. Vol. 8041. SPIE. 2011, pp. 58-63.

[55] Mary HAYHOE et al. "Visual short-term memory and motor planning." eng. In: *Progress in brain research* 140 (2002), pp. 349-363. ISSN: 0079-6123 (Print). DOI: 10.1016/S0079-6123(02)40062-3.

[56] Vincent HAYWARD and Karon E MACLEAN. "Do it yourself haptics: part I". In: *IEEE Robotics & Automation Magazine* 14.4 (2007), pp. 88-104.

[57] Mort HEILIG. "Enter the Experimental Revolution". In: *Proceeding of Cyberarts Conference*. 1992, pp. 292-305.

[58] Evert HELMS, Rolf Dieter SCHRAFT, and M HAGELE. "rob@ work: Robot assistant in industrial environments". In: *Proceedings. 11th IEEE International Workshop on Robot and Human Interactive Communication*. IEEE. 2002, pp. 399-404.

[59] Koichi HIROTA and Michitaka HIROSE. "Development of surface display". In: *Proceedings of IEEE Virtual Reality Annual International Symposium*. IEEE. 1993, pp. 256-262.

[60] David. HSU, Jean-Claude. LATOMBE, and Rajeev. MOTWANI. "Path planning in expansive configuration spaces". In: *International Journal of Computational Geometry & Applications* 9.4-5 (1999), pp. 495-512. DOI: 10.1142/S021819599000285.

[61] Hsin-Yu HUANG et al. "Haptic-Go-Round: A Surrounding Platform for Encounter-Type Haptics in Virtual Reality Experiences". In: *Proceedings of the 2020 CHI Conference on Human Factors in Computing Systems*. CHI '20. New York, NY, USA: Association for Computing Machinery, 2020, pp. 1-10. ISBN: 9781450367080. DOI: 10.1145/3313831.3376476. URL: https://doi.org/10.1145/3313831.3376476.

[62] TS ISO. "15066: Robots and robotic devices-Collaborative robots". In: *Geneva: ISO copyright office* (2016).

[63] L. JAILLET, J. CORTES, and T. SIMEON. "Sampling-Based Path Planning on Configuration-Space Costmaps". In: *IEEE Transactions on Robotics* 26.4 (Aug. 2010). DOI: 10.1109/ TRO.2010.2049527.
[64] Jason JERALD. *The VR book: Human-centered design for virtual reality*. Morgan & Claypool, 2015.

[65] K. JIN-OH and P.K. KHOSLA. "Real-Time Obstacle Avoidance Using Harmonic Potential Functions". In: *IEEE Transactions on Robotics and Automation* 8(3) (1992), pp. 338-349.

[66] Roland S. JOHANSSON et al. "Eye-Hand Coordination in Object Manipulation". In: *Journal of Neuroscience* 21.17 (2001), pp. 6917-6932. DOI: 10.1523/jneurosci.21-17-06917.2001. URL: https://app.dimensions.ai/details/publication/pub.1074868523andhttp://www.jneurosci.org/content/21/17/6917.full.pdf.

[67] M. JORDAN and A. PEREZ. "Optimal Bidirectional Rapidly-Exploring Random Trees". In: *MIT-CSAIL-TR-2013-021* (2013).

[68] S. KARAMAN and E. FRAZZOLI. "Sampling-based algorithms for optimal motion planning". In: *The International Journal of Robotics Research* 30.7 (2011), pp. 846-894. DOI: 10.1177/0278364911406761.

[69] L.E. KAVRAKI et al. "Probabilistic roadmaps for path planning in high-dimensional configuration spaces." In: *IEEE Transactions on Robotics and Automation* 12(4) (1996), pp. 566-80.

[70] KDL kinematics and dynamics library (kdl). June 2019. URL: http://wiki.ros.org/kdl.

[71] Wisama KHALIL and Etienne DOMBRE. *Modeling identification and control of robots*. CRC Press, 2002.

[72] O. KHATIB. "Real-time obstacle avoidance for manipulators and mobile robots". In: *IEEE International Conference on Robotics and Automation* (1985).

[73] Yaesol KIM, Hyun Jung KIM, and Young J KIM. "Encountered-type haptic display for large VR environment using per-plane reachability maps". In: *Computer Animation and Virtual Worlds* 29.3-4 (2018), e1814.

[74] Yaesol KIM et al. "Synthesizing the roughness of textured surfaces for an encountered-type haptic display using spatiotemporal encoding". In: *IEEE Transactions on Haptics* 14.1 (2020), pp. 32-43.

[75] B. KOLB and I.Q. WHISHAW. *An Introduction to Brain and Behavior, Third Edition*. Worth Publishers, 2005. ISBN: 9781429281478.

[76] Jörg KRÜGER, Terje K LIEN, and Alexander VERL. "Cooperation of human and machines in assembly lines". In: *CIRP annals* 58.2 (2009), pp. 628-646.

[77] Ernst KRUIJFF, Dieter SCHMALSTIEG, and Steffi BECKHAUS. "Using Neuromuscular Electrical Stimulation for Pseudo-Haptic Feedback". In: *Proceedings of the ACM Symposium on Virtual Reality Software and Technology*. VRST '06. New York, NY, USA: Association for Computing Machinery, 2006, pp. 316-319. ISBN: 1595933212. DOI: 10.1145/1180495.1180558. URL: https://doi.org/10.1145/1180495.1180558.

[78] James. KUFFNER and Steven.M. LAVALLE. "RRT-connect: An efficient approach to single-query path planning". In: *Proceedings of the 2000 IEEE International Conference on Robotics & Automation* (Apr. 2000), pp. 995-1001. DOI: 10.1109/ROBOT.2000.844730.

[79] G. Santhosh KUMAR and G. Sony BHAVANI. "A new dimension of immersiveness into virtual reality through haptic technology". In: *2017 IEEE International Conference on Power, Control, Signals and Instrumentation Engineering (ICPCSI)*. 2017, pp. 1651-1655. DOI: 10.1109/ICPCSI.2017.8391994.
[80] S. LAVALLE and J.J. KUFFNER. "Randomized Kinodynamic Planning". In: *The International Journal of Robotics Research* (2001).

[81] Susan J LEDERMAN and Roberta KLATZKY. "Haptic Exploration and Object". In: *Vision and action: The control of grasping* 2 (1990), p. 98.

[82] Philip LONG et al. "An industrial security system for human-robot coexistence". In: *Industrial Robot: An International Journal* 45.2 (2018), pp. 220-226.

[83] T. LOZANO-PEREZ. "Spatial planning: a configuration space approach". In: *IEEE Transactions on Computers* (1983).

[84] Anderson MACIEL et al. "Multi-Finger Haptic Rendering of Deformable Objects, In. Tenth Eurographics Symposium on Virtual Environments". In: *Eurographics. CONF*. 2004, pp. 105-111.

[85] A. MAKHAL and A. K. GOINS. "Reuleaux: Robot Base Placement by Reachability Analysis". In: *ArXiv e-prints* (Oct. 2017). arXiv: 1710.01328 [cs.RO].

[86] Dan MAYNES-AMINZADE. "Edible bits: Seamless interfaces between people, data and food". In: *Conference on Human Factors in Computing Systems (CHI'05)-Extended Abstracts*. Citeseer. 2005, pp. 2207-2210.

[87] William A MCNEELY. "Robotic graphics: a new approach to force feedback for virtual reality". In: *Proceedings of IEEE Virtual Reality Annual International Symposium*. IEEE. 1993, pp. 336-341.

[88] Víctor Rodrigo MERCADO, Maud MARCHAL, and Anatole LÉCUYER. "ENTROPiA: Towards Infinite Surface Haptic Displays in Virtual Reality Using Encountered-Type Rotating Props". In: *IEEE Transactions on Visualization and Computer Graphics* (2019).

[89] F. MESSMER et al. *ROS-Industrial-Universal-Robots*. https://github.com/ros-industrial/universalRobot. 2022.

[90] Daniel MESTRE et al. "Immersion et présence". In: *Le traité de la réalité virtuelle. Paris: Ecole des Mines de Paris* (2006), pp. 309-38.

[91] Daniel R MESTRE. "CAVE versus head-mounted displays: ongoing thoughts". In: *Electronic Imaging* 2017.3 (2017), pp. 31-35.

[92] Land MICHAEL, Mennie NEIL, and Rusted JENNIFER. "The Roles of Vision and Eye Movements in the Control of Activities of Daily Living". In: *Perception* 28.11 (1999). PMID: 10755142, pp. 1311-1328. DOI: 10.1068/p2935. URL: https://doi.org/10.1068/p2935.

[93] Matjaž MIHELJ, Domen NOVAK, and Samo BEGUŠ. "Virtual reality technology and applications". In: (2014).

[94] A. MONTAGU. "Touching, The human significance of the skin". In: *Perennial Library* (1972), pp. 98-99. URL: https://ci.nii.ac.jp/naid/10007744975/en/.

[95] MPB Technologies Inc. (2018b). *How do you Choose a Haptic Device?* Feb. 2022. URL: http://www.mpb-technologies.ca/mpbt/mpbt_web_2009/_en/resources/articles/Howdoyouchooseahapticdevice.pdf.

[96] Stanley MUGISHA et al. "Safe collaboration between human and robot in a context of intermittent haptique interface". In: *ASME International Design Engineering Technical Conferences & Computers and Information in Engineering Conference*. 2021.
[97] K. NADERI and J. RAJAMAKI. "RT-RRT*: a real-time path planning algorithm based on RRT*". In: *Conference: the 8th ACM SIGGRAPH Conference* (2015).

[98] Allison M OKAMURA. "Haptic feedback in robot-assisted minimally invasive surgery". In: *Current opinion in urology* 19.1 (2009), p. 102.

[99] Esben H OSTERGAARD. "Lightweight robot for everybody [industrial activities]". In: *IEEE robotics & automation magazine* 19.4 (2012), pp. 17–18.

[100] Ezgi ÖZCAN. "Analysis of Immersive Virtual Reality through Senses". In: (2020).

[101] Richard PALLUEL-GERMAIN et al. "A visuo-haptic device-telemaque-increases kindergarten children's handwriting acquisition". In: *Second Joint EuroHaptics Conference and Symposium on Haptic Interfaces for Virtual Environment and Teleoperator Systems (WHC'07)*. IEEE. 2007, pp. 72–77.

[102] Jeff PELZ, Mary HAYHOE, and Russ LOEBER. "The coordination of eye, head, and hand movements in a natural task". In: *Experimental Brain Research* 139.3 (2001), pp. 266–277. ISSN: 1432-1106. DOI: 10.1007/s002210100745. URL: https://doi.org/10.1007/s002210100745.

[103] Jérôme PERRET and Emmanuel VANDER POORTEN. "Touching virtual reality: a review of haptic gloves". In: *ACTUATOR 2018; 16th International Conference on New Actuators*. VDE. 2018, pp. 1–5.

[104] Javier POSSELT et al. "Toward virtual touch: investigating encounter-type haptics for perceived quality assessment in the automotive industry". In: *Proceedings of the 14th annual EuroVR conference*. Laval, France, 2017, pp. 11–13.

[105] Pinyo PUANGMALI et al. "State-of-the-art in force and tactile sensing for minimally invasive surgery". In: *IEEE Sensors Journal* 8.4 (2008), pp. 371–381.

[106] Eleanora P Westebring-van der PUTTEN et al. "Haptics in minimally invasive surgery–a review". In: *Minimally Invasive Therapy & Allied Technologies* 17.1 (2008), pp. 3–16.

[107] Quanser Inc. (2018). *High definition haptic device*. Feb. 2022. URL: https://www.quanser.com/products/hd2-high-definition-haptic-device/.

[108] Nimesha RANASINGHE and Ellen Yi-Luen Do. "Digital lollipop: Studying electrical stimulation on the human tongue to simulate taste sensations". In: *ACM Transactions on Multimedia Computing, Communications, and Applications (TOMM)* 13.1 (2016), pp. 1–22.

[109] Miriam REINER. "The role of haptics in immersive telecommunication environments". In: *IEEE Transactions on Circuits and Systems for Video Technology* 14.3 (2004), pp. 392–401.

[110] D.E. RIMON E. Koditschek. "Exact robot navigation using artificial potential functions". In: *IEEE Transactions on Robotics and Automation* 8(5) (1992), pp. 501–518.

[111] Gabriel ROBLES-DE-LA-TORRE. "The importance of the sense of touch in virtual and real environments". In: *Ieee Multimedia* 13.3 (2006), pp. 24–30.

[112] John W. RUFFNER. "Multimedia and Virtual Reality: Designing Multisensory User Interfaces by Alistair Sutcliffe". In: *Ergonomics in Design* 13.1 (2005), pp. 29–29. DOI: 10.1177/106480460501300108. eprint: https://doi.org/10.1177/106480460501300108. URL: https://doi.org/10.1177/106480460501300108.
[113] Mikel SAGARDIA et al. "VR-OOS: The DLR's virtual reality simulator for telerobotic on-orbit servicing with haptic feedback". In: *2015 IEEE Aerospace Conference*. IEEE. 2015, pp. 1-17.

[114] Steeven Villa SALAZAR et al. "Altering the Stiffness, Friction, and Shape Perception of Tangible Objects in Virtual Reality Using Wearable Haptics". In: *IEEE Transactions on Haptics (ToH)* (2020).

[115] Oren SALZMAN and Dan. HALPERIN. "Asymptotically near-optimal RRT for fast, high-quality motion planning". In: *IEEE Transactions on Robotics* 32.3 (Apr. 2016), pp. 473-483.

[116] Ganesh SANKARANARAYANAN et al. "Role of haptics in teaching structural molecular biology". In: *11th Symposium on Haptic Interfaces for Virtual Environment and Teleoperator Systems, 2003. HAPTICS 2003. Proceedings*. IEEE. 2003, pp. 363-366.

[117] M. SEDA. "Roadmap Methods vs. Cell Decomposition in Robot Motion Planning". In: *Proceedings of the 6th WSEAS International Conference on Signal Processing* (2007).

[118] JEE SHARPE. "Technical and human operational requirements for skill transfer in teleoperations". In: *Proceedings of the International Symposium on Teleoperation and Control*. 1988, pp. 175-187.

[119] J. SHENG et al. "An Improved Artificial Potential Field Algorithm for Virtual Human Path Planning". In: *Edutainment 2010, Lecture Notes in Computer Science (LNCS)* 6249 (2010), pp. 592-601.

[120] William R SHERMAN and Alan B CRAIG. *Understanding virtual reality: Interface, application, and design*. Morgan Kaufmann, 2018.

[121] Karun B SHIMOGA. "A survey of perceptual feedback issues in dexterous telemanipulation. II. Finger touch feedback". In: *Proceedings of IEEE Virtual Reality Annual International Symposium*. IEEE. 1993, pp. 271-279.

[122] N. SLEUMER and N. TSCHICHOLD-GURMAN. "Exact Cell Decomposition of Arrangements used for Path Planning in Robotics". In: *Technical Report* (2000).

[123] Jeroen B. J. SMEETS, Mary M. HAYHOE, and Dana H. BALLARD. "Goal-directed arm movements change eye-head coordination". In: *Experimental Brain Research* 109.3 (1996), pp. 434-440. ISSN: 1432-1106. DOI: 10.1007/BF00229627. URL: https://doi.org/10.1007/BF00229627.

[124] Alexander STAMENKOVIC. "Do postural constraints affect eye, head, and arm coordination?" In: *Journal of neurophysiology* 120.4 (2018), pp. 2066-2082. DOI: 10.1152/jn.00200.2018.

[125] Standard International Organization for STANDARDIZATION. "Robots and robotic devices - safety requirements for industrial robots - part I: Robots, DIN EN ISO 10218-1". en. In: (2021).

[126] Ioan A. ŞUCAN, Mark MOLL, and Lydia E. KAVRAKI. "The Open Motion Planning Library". In: *IEEE Robotics & Automation Magazine* 19.4 (Dec. 2012), pp. 72-82. DOI: 10.1109/MRA.2012.2205651. URL: https://ompl.kavrakilab.org.

[127] Susumu TACHI. "A construction method of virtual haptics space". In: *Proc. of the ICAT'94 (4th International Conference on Artificial Reality and Tele-Existence)*. 1994, pp. 131-138.
[128] Russell M TAYLOR. "Haptics for scientific visualization". In: *ACM SIGGRAPH 2005 Courses*. 2005, 174-es.

[129] Marija TOMIĆ et al. "Human to humanoid motion conversion for dual-arm manipulation tasks". In: *Robotica* 36.8 (2018), pp. 1167-1187.

[130] *Universal Robot*. https://www.universal-robots.com/fr/. Accessed: 2022-03-23.
URL: https://www.universal-robots.com.

[131] *UR modern driver*. 2022. URL: https://github.com/ros-industrial/ur_modern_driver.

[132] ÅB VALLBO et al. "Microstimulation of single tactile afferents from the human hand: Sensory attributes related to unit type and properties of receptive fields". In: *Brain* 107.3 (1984), pp. 727-749.

[133] *Vaqso*. Jan. 2022. URL: https://vaqso.com.

[134] *Virtuose 6d*. Feb. 2022. URL: https://www.haption.com/fr/products-fr/virtuose-6d-fr.html.

[135] R.A. VOLPE. "Real-time obstacle avoidance for manipulators and mobile robots". PhD thesis. 1990.

[136] R.A. VOLPE and P. KHOSLA. "Manipulator control with superquadric artificial potential functions: theory and experiments". In: *IEEE Transactions on Systems, Man, and Cybernetics* 20(6) (1990), pp. 1423-1436.

[137] Xi Vincent WANG, A SEIRA, and Lihui WANG. "Classification, personalised safety framework and strategy for human-robot collaboration". In: *Proceedings of the International Conference on Computers and Industrial Engineering.*, Auckland, New Zealand. 2018.

[138] Richard M. WARREN. *Auditory Perception: An Analysis and Synthesis*. 3rd ed. Cambridge University Press, 2008. DOI: 10.1017/CBO9780511754777.

[139] Pingjun XIA. "Haptics for product design and manufacturing simulation". In: *IEEE transactions on haptics* 9.3 (2016), pp. 358-375.

[140] Y. YOKOKOHJI, R.L. HOLLIS, and T. KANADE. "What you can see is what you can feel-development of a visual/haptic interface to virtual environment". In: *Proceedings of the IEEE 1996 Virtual Reality Annual International Symposium*. IEEE. 1996, pp. 46-53.
DOI: 10.1109/VRAIS.1996.490509.

[141] Yasuyoshi YOKOKOHJI, Ralph L HOLLIS, and Takeo KANADE. "WYSIWYF display: A visual/haptic interface to virtual environment". In: *Presence* 8.4 (1999), pp. 412-434.

[142] Yasuyoshi YOKOKOHJI, Junji KINOSHITA, and Tsuneo YOSHIKAWA. "Path planning for encountered-type haptic devices that render multiple objects in 3d space". In: *Proceedings IEEE Virtual Reality 2001*. IEEE. 2001, pp. 271-278.

[143] Yasuyoshi YOKOKOHJI et al. "Design and path planning of an encountered-type haptic display for multiple fingertip contacts based on the observation of human grasping behavior". In: *IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA'04. 2004*. Vol. 2. IEEE. 2004, pp. 1986-1991.

[144] Tsuneo YOSHIKAWA and Akihiro NAGURA. "A touch and force display system for haptic interface". In: *Proceedings of international conference on robotics and automation*. Vol. 4. IEEE. 1997, pp. 3018-3024.
**Titre** : Contributions à l'utilisation de cobots comme interfaces haptiques à contact intermittent en réalité virtuelle.

**Mots clés** : sécurité, interface à contact intermittent, prédiction de l'intention humaine, planification de trajectoires, collaboration homme-robot, réalité virtuelle.

**Résumé** : La réalité virtuelle (RV) est de plus en plus utilisée dans des simulations industrielles mais la possibilité de toucher les objets manque rapidement par exemple pour juger de la qualité perçue dans la conception de véhicule automobile. Les interfaces haptiques actuels ne permettent de restituer aisément la notion de texture, l'approche envisagée est donc une interface à contact intermittent. Un cobot vient positionner une surface mobile à l'endroit du contact avec un objet virtuel pour permettre un contact physique avec la main de l'opérateur.

Les contributions de cette thèse portent sur plusieurs aspects : le placement du robot, la modélisation de l'opérateur, la gestion du déplacement et de la vitesse du robot et la détection des intentions de l'opérateur.

Le placement du robot est choisi pour permettre d'atteindre les différentes zones de travail et pour assurer une sécurité passive en

rendant impossible au robot de heurter la tête et le buste de l'opérateur en position normale de travail, i.e. assis dans un fauteuil. Un modèle de l'utilisateur, incluant un torse et des bras, est conçu et testé pour suivre les mouvements de l'utilisateur en temps réel.

L'interaction est possible sur un ensemble de pose prédéfinies que l'utilisateur enchaine comme il le désire. Différentes stratégies sont proposées pour prédire les intentions de l'utilisateur. Les aspects clés de la prédiction sont basés sur la direction du regard et la position de la main de l'utilisateur. Une étude expérimentale ainsi que l'analyse qui en découle montrent l'apport de la prise en compte de la direction du regard. L'intérêt d'introduire des points dit « de sécurité » pour éloigner le robot de l'opérateur et permettre des déplacements rapides du robot est mis en évidence.

**Title**: Contributions to utilize a Cobot as intermittent contact haptic interfaces in virtual reality.

**Keywords**: safety, intermittent contact interface, human intention prediction, trajectory planning, human robot collaboration, virtual reality.

**Abstract**: Virtual reality (VR) is evolving and being used in industrial simulations but the possibility to touch objects is missing, for example to judge the perceived quality in the design of a car. The current haptic interfaces do not allow to easily restore the notion of texture, therefore an approach is considered "intermittent contact interface" to achieve this. A cobot positions a mobile surface at the point of contact with a virtual object to allow physical contact with the operator's hand.

The contributions of this thesis concern several aspects: the placement of the robot, the modeling of the operator, the management of the displacement and the speed of the robot and the detection of the operator's intentions.

The placement of the robot is chosen to allow reaching the different working areas and to

ensure passive safety by making it impossible for the robot to hit the head and chest of the operator in a normal working position, i.e. sitting in a chair. A model of the user, including a torso and arms, is designed and tested to follow the user's movements in real time

Interaction is possible on a set of predefined poses that the user chains together as desired. Different strategies are proposed to predict the user's intentions. The key aspects of the prediction are based on the gaze direction and the hand position of the user. An experimental study as well as the resulting analysis show the contribution of taking into account the gaze direction. The interest of introducing "safety" points to move the robot away from the operator and allow fast robot movements is highlighted.