Experimental Environment
All the experiments in this paper are on VMware virtual machines with 16G RAM and quad-core Intel i7-9750H CPU@ 2.60GHz. This virtual machine has 6G RAM, 40G storage space and the system Ubuntu 16.04. Because ROS and Moveit! involved in this experiment need to be used based on Ubuntu, and currently on Ubuntu 16.04, ROS and Moveit! support more complete versions.
Motion Planning Benchmarking
Robot for Benchmarking
In this experiment, the robot used is Franka Panda. It is a collaborative robot arm, developed by FRANKA EMIKA. Panda has 7 DOF with torque sensors in all seven axes, the arm can delicately manipulate objects, and accomplish complex tasks. And each joint of Panda is equipped with strain gauges, which can detect minor collisions to avoid injury to people, and is suitable for high-precision 3C industries, such as assembly and inspection.
Benchmarking Scenarios
In this experiment, we mainly designed 3 different benchmarking scenarios:
Narrow Tunnel Scene: The first experimental environment is a narrow tunnel as shown in Fig. 3. It is a classic scene in the motion planning benchmarking. Due to the narrow passage and limited space, many classic motion planning algorithms are not necessarily applicable. In this experiment, we implemented pipelines on the traditional sampling-based motion planning algorithm and the optimization-based algorithm. The narrow tunnel is a very good obstacle avoidance environment for testing the optimized algorithm. This scene has only one query, that is, the start state (green) and goal state (orange) of the robotic arm are at both ends of the channel. There are two ways for the robot arm to complete the motion planning, one is to reach the other side of the board through a narrow tunnel; Second, it doesn't pass the tunnel, it takes a long path time for the arm to bypass the board. In this experimental scene, it is possible to judge whether the algorithm used passes through a narrow tunnel and completes the motion planning by running time, thereby judging the motion planning algorithm whether found the shortest way.
Industrial Workshop Scenes: The second experimental scene is an industrial workshop as shown in Fig. 4 and Fig. 5. In industrial workshops, workers need to assemble many different parts every day. They pick the parts they need from the box containing the parts and place them on the workbench for assembly. If the process of selecting and searching can be
completed by a robotic arm, it will save time and the human operator can concentrate on assembling, reducing the workload and improving work efficiency. In this scene, two different queries are mainly involved: (1) the robotic arm pick up parts from the box on the second floor of the parts table and brings them to the workbench. The complexity of the benchmarking scene is low. As long as the robot arm does not touch the obstacles near the workbench, the motion planning can successfully make the robot arm from the second layer of the parts table to the workbench. (2) The robotic arm selects parts from the first layer of the parts table and brings the parts to the workbench. Although the query looks very similar to the first query, it is a little complex than the first, because it is necessary to avoid not only obstacles on the workbench but also obstacles on the parts table.
Library Scene: The last scene is the library environment as shown in Figs. 6-8. The workload of the library is very large, and the staff needs to organize the books every day. This includes sorting out the books returned by readers, sorting them, and putting them back on the shelves. This is a heavy workload, especially in libraries with a lot of people. If we use robotic arms to classify and place different types of books. Then it will reduce people's workload to a large extent and improve work efficiency. In this scene, three different queries are set: (1) The robotic arm moves the arm from under the table to the top of the table. This query is mainly designed for when the book is accidentally dropped on the ground, and how putting the books back on the desktop after they dropped on the ground when they are collected. The motion planning must avoid collisions between the robotic arm and the desk and the bookshelf. The scene is relatively low in complexity, with fewer obstacles and not dense. (2) The robotic arm moves from the table to the bookshelf. This query is mainly for collecting and sorting the books and putting them back to the corresponding position on the bookshelf. The scene is highly complex, and it is necessary to avoid collisions with bookshelf spacers and tables. (3) The robotic arm moves from the third level of the bookshelf to the second level of the bookshelf. This query is mainly reflected in the operations needed to organize the bookshelf. The scene is highly complex and needs to move from two narrow spaces. The above three experimental scenes are good for a reasonable evaluation of the motion planning algorithm.
Multiple sets of data against different planning metrics are obtained. We need to analyze these data and find a better motion planning algorithm in the scenario. Besides, the above three scenarios only include the motion planning of the robot arm without the gripper, so the pick and place of the items are not implemented. In future work, it can be integrated.
Benchmarking Metrics
This experiment measures the performance, reliability and quality of the paths generated by all planners via the following metrics: (1) Solve (%): it indicates the percentage of motion planners finding solutions to the current query in the current scene. The higher the value, the higher the probability that the planner finds a solution, that is, the better the performance of the motion planner. (2) Path plan time (s): It is the path running time planned by the motion planner. This value is another way to test the length of the path. The shorter the running time, the shorter the path planned by the motion planner, and the better the quality of the path generated by the motion planner. (3) Path smoothness: It represents whether the trajectory generated by the motion planner is smooth. If the trajectory is smooth, the smaller the metric, the better the performance and reliability of the motion planner. This value is calculated from three consecutive path points and the angle formed between them. The closer the value is to 0, the more the trajectory tends to be a straight line. (4) Path clearance (m): It represents the average value from the path point on the trajectory generated by the motion planner to the nearest obstacle or invalid state value. If the value is larger, it proves that the motion planning path is far away from obstacles and invalid states, and it also indicates that the quality and reliability of the path generated by the motion planner are higher. (5) Total time(s): It refers to the time it takes for the entire motion planning algorithm to find a feasible solution. It includes plan time, interpolation time, simplify time and process time. The smaller the metric, the better the performance of the motion planner.
The Process of Benchmarking
The experiment contains scene design, motion planner selection, planning adapter setting and benchmarking. In each scene, benchmarking the different queries are required, and each query is described and analyzed. The benchmarking performed 50 times planning for each motion planning algorithm and collected its data. If the single running time exceeds 10.0s, the motion planning will be stopped, and the planning failure will be marked. First, we configure the motion planner parameters and benchmarking parameters, and then run the benchmarking to benchmark the currently configured scene and motion planner. Get the corresponding database. Then we uploaded the database through the OMPL Planner Arena, the visual data results shown by box plots are obtained.
Results
There may be outliers in the obtained data set, so we have no way to evaluate this set of data in the form of an average. The median better interprets the average level of the data set with outliers, so the median is used in this experiment to better describe the data information of each data set. All the original results of the experiment are submitted in supplementary files.
Narrow Space (Tunnel)
Table 1 Tunnel Benchmarking Results
Planners
|
Solve
(%)
|
Path time
(s)
|
Path smoothness
|
Path clearance
|
Total time(s)
|
PRM
|
100
|
0.0600
|
0.228
|
0.0412
|
0.061
|
RRT
|
100
|
0.0438
|
0.198
|
0.0351
|
0.045
|
LazyPRM
|
100
|
0.0465
|
0.201
|
0.0412
|
0.048
|
RRTConnect
|
100
|
0.0372
|
0.208
|
0.0362
|
0.041
|
CHOMP
|
0
|
-
|
-
|
-
|
-
|
STOMP
|
100
|
0.2063
|
0.013
|
0.0343
|
0.211
|
PRM-CHOMP
|
84
|
0.1462
|
0.075
|
0.0365
|
0.380
|
RRT-CHOMP
|
74
|
0.1300
|
0.055
|
0.0360
|
0.389
|
LazyPRM-CHOMP
|
78
|
0.1320
|
0.083
|
0.0395
|
0.377
|
RRTConnect-CHOMP
|
78
|
0.1310
|
0.048
|
0.0340
|
0.372
|
PRM-STOMP
|
100
|
0.1410
|
0.036
|
0.0410
|
0.168
|
RRT-STOMP
|
100
|
0.1210
|
0.028
|
0.0371
|
0.144
|
LazyPRM-STOMP
|
100
|
0.1250
|
0.030
|
0.0374
|
0.150
|
RRTConnect-STOMP
|
100
|
0.1120
|
0.037
|
0.0388
|
0.140
|
Table I shows that, in this scenario, the resolution rate of the original algorithm is almost 100%, and only the CHOMP algorithm fails. In the original algorithm, the path time of RRT, LazyPRM and RRTConnect is relatively short. After optimizing through the CHOMP algorithm, the success rate of the algorithm is reduced. Our reasonable guess may be that the CHOMP algorithm is trapped in a local minimum so the motion planning fails. Trajectory time can generally represent the length of the planned path. It can be seen from the trajectory time that motion planning algorithms based on sampling can find a shorter path through the tunnel, while the STOMP algorithm cannot find a shorter path through the tunnel, so the trajectory time is longer. Secondly, it can be seen that the time of the trajectory generated based on the optimized sampling algorithm is greater than the original time.
Regarding the smoothness, the optimized sampling algorithm has been significantly improved. Although there is a gap with the smoothness of STOMP, it has provided a fairly good motion planning trajectory. Since this scene is tested for the narrow space, the clearance of this set of data is basically unchanged. In terms of total time, the sampling algorithm based on optimization takes longer than the original sampling algorithm, but it is worth noting that the sampling algorithm based on optimization takes less time than the STOMP algorithm.
It can be seen from the above that in a narrow space scene, after CHOMP optimization, the success rate of the algorithm will decrease, but the trajectory quality is better than the original trajectory quality. The sampling-based algorithm optimized by STOMP has a high success rate, and the quality of the trajectory is greatly improved compared with the trajectory generated by the initial algorithm, and it also improves that the STOMP algorithm cannot find a shorter planned path through the tunnel. In the tunnel scene, we recommend to use any sampling algorithm and optimize the STOMP trajectory.
Industrial
a. The first floor parts box to table query:
Table 2 Industrial_FTT Benchmarking Results
Planner
|
Solve
(%)
|
Path time(s)
|
Path smoothness
|
Path clearance
|
Total time(s)
|
PRM
|
100
|
0.87
|
0.29
|
0.046
|
0.89
|
RRT
|
26
|
0.98
|
0.32
|
0.051
|
10.0
|
LazyPRM
|
46
|
0.60
|
0.30
|
0.070
|
10.0
|
RRTConnect
|
100
|
0.064
|
0.34
|
0.044
|
0.068
|
CHOMP
|
0
|
-
|
-
|
-
|
-
|
STOMP
|
94
|
0.17
|
0.051
|
0.163
|
0.19
|
PRM-CHOMP
|
64
|
0.99
|
0.050
|
0.046
|
1.70
|
RRT-CHOMP
|
16
|
0.26
|
0.070
|
0.053
|
10.0
|
LazyPRM-CHOMP
|
32
|
0.54
|
0.050
|
0.049
|
10.0
|
RRTConnect-CHOMP
|
64
|
0.175
|
0.070
|
0.045
|
0.47
|
PRM-STOMP
|
92
|
0.74
|
0.083
|
0.158
|
0.83
|
RRT-STOMP
|
28
|
0.70
|
0.077
|
0.128
|
10.0
|
LazyPRM-STOMP
|
42
|
0.98
|
0.072
|
0.140
|
10.0
|
RRTConnect-STOMP
|
96
|
0.12
|
0.075
|
0.160
|
0.16
|
In this query, the original algorithm PRM, RRTConnect and STOMP all have a good problem solving rate. The RRTConnect algorithm takes extremely short time and can generate the shortest trajectory, but its trajectory quality is not very good. After the optimization of the CHOMP algorithm, the success rate of the sampling algorithm is greatly reduced, and the smoothness of the trajectory is greatly improved. After the basic sampling algorithm is optimized by the STOMP algorithm, the success rate is almost unchanged, but the quality of the generated trajectory has been significantly improved. Under the query of this scenario, the optimal solution algorithm is the RRTConnect-STOMP algorithm, which can generate a shorter trajectory with better quality in a relatively short time.
b. The second floor parts box to table query:
Table 3 Industrial_STT Benchmarking Results
Planner
|
Solve
(%)
|
Path time(s)
|
Path smoothness
|
Path clearance
|
Total time(s)
|
PRM
|
100
|
0.48
|
0.28
|
0.067
|
0.49
|
RRT
|
18
|
1.48
|
0.29
|
0.080
|
10.0
|
LazyPRM
|
68
|
0.11
|
0.27
|
0.072
|
1.0
|
RRTConnect
|
100
|
0.041
|
0.26
|
0.062
|
0.046
|
CHOMP
|
0
|
-
|
-
|
-
|
-
|
STOMP
|
98
|
0.06
|
0.032
|
0.182
|
0.067
|
PRM-CHOMP
|
44
|
0.36
|
0.076
|
0.080
|
1.15
|
RRT-CHOMP
|
16
|
0.21
|
0.122
|
0.076
|
10.0
|
LazyPRM-CHOMP
|
36
|
0.17
|
0.148
|
0.079
|
1.14
|
RRTConnect-CHOMP
|
58
|
0.146
|
0.080
|
0.077
|
0.42
|
PRM-STOMP
|
90
|
0.45
|
0.080
|
0.164
|
0.63
|
RRT-STOMP
|
36
|
0.95
|
0.071
|
0.153
|
10.0
|
LazyPRM-STOMP
|
64
|
0.25
|
0.076
|
0.162
|
0.62
|
RRTConnect-STOMP
|
96
|
0.11
|
0.066
|
0.161
|
0.14
|
In this query, the problem solving rate of RRT, PRM and STOMP in the original algorithm is still the highest. The success rate of the algorithm obtained after CHOMP optimization has been reduced a lot, resulting in unstable algorithm. However, after the optimization of the STOMP algorithm, although its success rate is slightly reduced, it is always within an acceptable range. The trajectory generated by the STOMP algorithm is the best trajectory among all benchmarking algorithms. Not only is the generated trajectory the shortest path, but it also has a fairly good quality. Although the sampling-based algorithm also has a significant improvement after optimization, the STOMP algorithm solution is more excellent under the query of this scene, so STOMP is the best solution algorithm under the query of this scene.
Library
a. Under table to on table query:
Table 4 Library_DTU Benchmarking Results
Planner
|
Solve
(%)
|
Path time (s)
|
Path smoothness
|
Path clearance
|
Total time(s)
|
PRM
|
100
|
0.043
|
0.48
|
0.093
|
0.046
|
RRT
|
100
|
0.029
|
0.44
|
0.064
|
0.032
|
LazyPRM
|
100
|
0.026
|
0.36
|
0.071
|
0.028
|
RRTConnect
|
100
|
0.019
|
0.41
|
0.081
|
0.022
|
CHOMP
|
0
|
-
|
-
|
-
|
-
|
STOMP
|
100
|
0.115
|
0.05
|
0.186
|
0.119
|
PRM-CHOMP
|
80
|
0.129
|
0.18
|
0.088
|
0.45
|
RRT-CHOMP
|
62
|
0.125
|
0.12
|
0.080
|
0.47
|
LazyPRM-CHOMP
|
70
|
0.121
|
0.14
|
0.088
|
0.42
|
RRTConnect-CHOMP
|
80
|
0.115
|
0.16
|
0.075
|
0.41
|
PRM-STOMP
|
100
|
0.078
|
0.058
|
0.143
|
0.082
|
RRT-STOMP
|
100
|
0.065
|
0.063
|
0.153
|
0.07
|
LazyPRM-STOMP
|
100
|
0.069
|
0.052
|
0.139
|
0.075
|
RRTConnect-STOMP
|
100
|
0.053
|
0.072
|
0.149
|
0.058
|
Under the query in the library scene, we can see from the table that all the original algorithms except CHOMP can complete the task satisfactorily. This is because the complexity of the query is relatively low. The quality of the trajectory produced based on the sampling algorithm is not as good as the STOMP algorithm. However, the length of the trajectory generated by the STOMP algorithm is longer, which is much longer than the trajectory generated by the sampling algorithm. After the optimization of the CHOMP algorithm based on the sampling algorithm, its success rate has decreased, requiring a long path time, and the generated trajectory need a long process time, that is not the shortest path, but its trajectory quality has been improved. After the sampling algorithm is optimized by the STOMP algorithm, its success rate is still 100%. The path time of the generated path is shorter than that of the original STOMP algorithm, indicating that the path is shorter. The path time of the generated path is shorter than that of the original STOMP algorithm, indicating that the path is shorter, and the quality of the path has also been greatly improved. Therefore, in this query, we recommend using a sampling algorithm optimized by the STOMP algorithm.
b. Table to bookshelf query:
Table 5 Library_TTB Benchmarking Results
Planner
|
Solve
(%)
|
Path time(s)
|
Path smoothness
|
Path clearance
|
Total time(s)
|
PRM
|
100
|
0.71
|
0.098
|
0.094
|
0.85
|
RRT
|
26
|
0.32
|
0.076
|
0.118
|
10.0
|
LazyPRM
|
58
|
0.50
|
0.098
|
0.101
|
2.51
|
RRTConnect
|
100
|
0.044
|
0.158
|
0.073
|
0.049
|
CHOMP
|
0
|
-
|
-
|
-
|
-
|
STOMP
|
54
|
0.25
|
0.043
|
0.171
|
0.71
|
PRM-CHOMP
|
70
|
0.78
|
0.047
|
0.098
|
1.25
|
RRT-CHOMP
|
10
|
0.29
|
0.049
|
0.127
|
10.0
|
LazyPRM-CHOMP
|
44
|
0.48
|
0.046
|
0.104
|
0.65
|
RRTConnect-CHOMP
|
68
|
0.14
|
0.082
|
0.071
|
0.45
|
PRM-STOMP
|
78
|
1.25
|
0.037
|
0.149
|
1.67
|
RRT-STOMP
|
16
|
0.28
|
0.021
|
0.134
|
10.0
|
LazyPRM-STOMP
|
50
|
0.32
|
0.028
|
0.140
|
3.0
|
RRTConnect-STOMP
|
68
|
0.33
|
0.050
|
0.130
|
0.65
|
In this query, RRTConnect has the best success rate, the shortest trajectory, and the most time-saving among the basic algorithms, but its trajectory quality is very poor and is not recommended. If the shortest path is considered, it is recommended to use the original RRTConnect algorithm, and the trajectory produced is the shortest path. If the path quality is considered, PRM-STOMP is most recommended in this query. Its success rate is only 78% and the trajectory quality is high, but its long trajectory is not the shortest path. If considering the overall situation, it is recommended to use the RRT-STOMP algorithm, all metrics of which are normal and acceptable values, and the resulting trajectory is also excellent. In the data of the original algorithm, it can be seen that the usually stable STOMP algorithm has also been affected by the complexity of the scene, and the success rate is only 54%. This also leads to a decrease in the optimization performance of the STOMP algorithm, which reduces the success rate of the sampling algorithm after STOMP optimization.
c. Move book from Third floor to Second floor on the bookshelf query:
Table 6 Library_ B3TB2 Benchmarking Results
Planner
|
Solve
(%)
|
Path time(s)
|
Path smoothness
|
Path clearance
|
Total time(s)
|
PRM
|
92
|
1.2
|
0.32
|
0.38
|
1.24
|
RRT
|
66
|
0.1
|
0.30
|
0.36
|
1.3
|
LazyPRM
|
60
|
0.75
|
0.40
|
0.41
|
5.0
|
RRTConnect
|
100
|
0.038
|
0.31
|
0.47
|
0.041
|
CHOMP
|
0
|
-
|
-
|
-
|
-
|
STOMP
|
32
|
0.68
|
0.11
|
0.178
|
1.6
|
PRM-CHOMP
|
50
|
0.77
|
0.19
|
0.042
|
1.35
|
RRT-CHOMP
|
48
|
0.61
|
0.135
|
0.036
|
1.35
|
LazyPRM-CHOMP
|
16
|
0.2
|
0.275
|
0.132
|
10.0
|
RRTConnect-CHOMP
|
46
|
0.132
|
0.125
|
0.037
|
0.98
|
PRM-STOMP
|
40
|
2.0
|
0.07
|
0.168
|
3.80
|
RRT-STOMP
|
30
|
1.4
|
0.08
|
0.145
|
3.81
|
LazyPRM-STOMP
|
30
|
1.2
|
0.07
|
0.151
|
10.0
|
RRTConnect-STOMP
|
38
|
0.75
|
0.07
|
0.137
|
2.60
|
The complexity of the query is very high, so that the original algorithm has the problem of planning failure. Only the PRM and RRTConnect algorithms have a higher success rate. STOMP has also been greatly affected, and its success rate is only 32%, which also leads to a greatly reduced success rate of the optimized algorithm. Through the comparison of the data in the table, the most suitable motion planning algorithm used in this scenario is the RRTConnect.