Trajectory planning in Dynamics Environment: Application for Haptic Perception in Safe Human-Robot Interaction

The architecture of MoveIt is based on two main nodes, the node move_group and the node planning_scene, which is part of the first one. The move_group node is responsible for obtaining the parameters, configuration and individual components of the robot model being used, in order to provide the user with services and actions to use on the robot.

Within the planners available in the OMPL library there are:

• •

  PRM methods (PRM [7], PRM* [14], LazyPRM [15], LazyPRM* [15] [14]),

• •

  RRT methods (RRT [13], RRT* [16], TRRT [17]), BiTRRT [18], LBTRRT [19], RRTConnect [20],

• •

  Expansive Spacial Trees (EST) methods (EST [21], BiEST [21]).

Collision checking in MoveIt is configured within a planning scene using the CollisionWorld object. Collision checking in MoveIt is performed using the Flexible Collision Library (FCL) package - MoveIt’s main collision checking library.

MoveIt uses a plugin infrastructure, specifically designed to allow users to write their own inverse kinematics algorithms. Direct kinematics and Jacobian search are built into the RobotState class itself. The default inverse kinematics plugin for MoveIt is configured using the KDL numerical solver [22] based on Jacobians. This plugin is automatically configured by the MoveIt configuration wizard.

ROS-Industrial is an open-source project that extends the advanced capabilities of ROS software to industrial hardware and applications. For this project, we used the ROS-Industrial-Universal-Robots metapackage [23], which provides and facilitates the main configuration files for the use of Universal Robots cobots in the ROS environment, providing the different descriptions of the robot, configuration files such as joint boundaries, UR kinematics, etc.. This package also facilitates the use of the robot in MoveIt, providing the setup for its use in simulation or in real implementations.

The HTC Vive is a motion tracking system that allows users to be immersed in a VR system [24]. It consists of trackers, which can attach to any rigid object, and work with the VR headset. The tracker creates a wireless connection between the object and the headset and then allows the user to represent the objects movements in a virtual world.

The laboratory setup consists of a UR5 robotic system and a car chair in a face-to-face configuration (Figure 3). The location and height of the robot was determined by [4] to be 75 cm above the floor. This position is optimal enough for the robot to reach all the interaction points that the system is interested in reaching. For the user, the VR headset and trackers are attached to the body (the humerus and palms), in order to obtain data and locate the user’s location in the VR environment (Figure 3).

The move_group class has the ability to perform path planning through different types of movements. These options can be chosen according to the nature of the task. For example, we can define a given pose in workspace or a desired joint value as the goal. Given the nature of the system, we will work with joint value goals, as we hope to achieve the different points in a specific configuration that provides a higher level of safety to the user (elbow up configuration for the UR5).

Another important feature is the ability to specify whether one wants to achieve each of the requested objectives or not. As the implementation will receive constantly changing goals, the best implementation is to plan and move towards said goal by allowing the system to replan if the goal changes, meaning that we do not need to reach the initial goal. To do this, the move_group class relies on the $move\_group.execute(my\_plan)$ function to strictly reach the goal and on the $move\_group.asyncExecute(my\_plan)$ function to execute the planned path with the possibility of re-planning during this execution.

In Figure 5, two trajectories are calculated from an initial configuration, to an intermediate goal, and then to a final goal. In this case, by using the function $move\_group.execute(my\_plan)$, we ensure that the robot will completely execute each of the trajectories and achieve both goals. This is illustrated in Figure 5, where the speeds drop to zero as the robot comes to a stop.

In the case of figure 6, we have calculated the same two trajectories as before, but using the function $move\_group.asyncExecute(my\_plane)$, which allows replanning during the execution of the first plane. In this case, in figure 6(a), we can see the two plans one after the other, while in figure 6(b), we show the representation of the segment that was not executed from the first plan, because a replanning scenario was set up. In this case, the current positions of the first plan were taken as the initial positions for the second plan, resulting in Figure  6(c), showing the two plans that were executed.

Another parameter to select was the planning algorithm that best suited the task. As mentioned earlier, MoveIt has several built-in path planning algorithms that can be used. In order to determine the best option, we went through all the available options and performed a planning task to a desired target configuration, measuring the time required for each algorithm and recording the data. We ran each of the 12 available planning algorithms five times through nine different paths. We then took the average time it took them to find a solution, to simplify the trajectory (only for the algorithms that had this feature) and calculated the total average time. Using this data, we were able to select the algorithms that performed best with the shortest planning times (Figure 7).

After performing these calculations, given the large difference in planning times for some of the algorithms, we select the six best algorithms to compare them on 12 trajectories (Figure 8).

Another analysis that allowed us to select the algorithm which behaved the best for the implementation, was to perform an analysis on the generated trajectories with each one of the algorithms for a fixed task. Based on the six best algorithms from the previous analysis as a starting constraint, we computed the average execution time and via-points number for a set of trajectories. The BiTRRT algorithm wins for both comparisons.

This analysis was performed for the same trajectories as in the previous graphs for a total of ten iterations for each algorithm, but instead of considering only the computation time (Figure 10), we also took into account the amount of via-points generated (Figure 10). Between each via-point, a linear interpolation is performed in the joint space. For theses trajectories, the mannequin was placed in its seat so that it was avoided in the calculation, in order to test each algorithm’s ability to plan around it. This also allowed us to see how consistent the behaviour of each algorithm was.

In parallel to the development of the project, and to better explain the developed implementation, it is important to specify how it will fit into the project. The system will receive a desired goal configuration which will be the $q_{goal}$ for the planning algorithm, from the current $q_{init}$ configuration. This goal selection is done in Unity by a Point selection algorithm which determines the interaction point the user intends to reach [26] (Figure 12).

The first scenario is based on [26] where a distinction for velocity zones is made and where a plane divides the environment (space with the user and space where the user cannot go).Based on the same idea, we represented the effective working space of the mannequin as a sphere surrounding the model (Figure 12).

The mobility scheme consists of alternating between different “Safe positions”. These positions are so called because they are points out of reach of the user, which means that there is no need to constrain the robot’s speeds. Therefore, the movement from one point to another just has to take into account the defined sphere, as we do not want to “collide” with it. Following this idea, we have performed a calculation of all existing trajectories between the different “Safe positions” and stored them in a data file. This allows us to perform offline path planning, and then at runtime, depending on the initial and desired goal, we can access the pre-calculated paths to execute them directly, eliminating the computational time that would otherwise be required by performing online planning. The algorithm 1 allows the storage of the trajectory.

Then, the second part of the device consists of loading the pre-registered data and being able to use them on demand (Algorithm 2). We wait until we know the position we want to reach. Unlike [26], we have used a spherical surface here to divide the two areas of the space instead of a plane, as this allows greater flexibility for the planning group to consider more configurations when calculating the path between points. It also allows for more feasible trajectories for the robot.

The second mobility scheme has been proposed for the scenario where the robot end effector has to go inside the user’s workspace, which means that the movements have to take into account the user’s model in order to avoid any collision with him. We also have to take into account that the speed of these movements must be limited, in order to ensure safety.

Unlike the first scheme, in this case the environment consists of moving obstacles, which requires constant updating of the scene and constant tracking of the objects in it (Figure 13). For this reason, we used the images of the mannequin model to obtain its current positions and orientations in order to track their movement and link it to the objects created in the scene.

We also need to be able to determine whether a computed plan will collide or not, which requires taking several aspects into account. First, based on the calculated path to the desired goal, we check whether the path remains valid during the execution of the plan. To do so, we check for all calculated via-points of the path, whether the respective configurations are currently colliding with any other object present in the scene. If there are no collisions, we continue the execution. In the case of a collision present in any of the remaining states of the path, we instruct the robot to stop the execution of the computed path and replan it based on the updated scene information.

To test our framework, we performed an initial trajectory planning. Then, during the execution, we created an obstacle. Then, by checking the validity of the trajectory, we are able to detect that an object is in collision with the planned trajectory. We then instruct the robot to stop the current execution and replan towards the same goal, taking into account the updated planning scene. This work is intended to be extrapolated to work according to the size of the mannequin. Thus, we can take into account the user moving in the environment as an obstacle to be avoided (Figure 14).