Robot Motion Planning Benchmarking and Optimization using Motion Planning Pipeline

Algorithms have been designed for robot motion planning with various adaptability to different problems. However, how to choose the most suitable planner in a scene has always been a problem worthy of research. This paper aims to �nd the most suitable motion planner for each query under three different scenes and six different queries. The work lies in optimization of sampling-based motion planning algorithms through Motion Planning Pipeline and Planning Request Adapter. The idea is to use the pre-processing of the planning request adapter, to run OMPL as a pre-processer for the optimized CHOMP or STOMP algorithm, and connect through the motion planning pipeline, to realize the optimization of the motion trajectory. The optimized trajectories are compared with original trajectories through benchmarking. The benchmarking determines the most suitable motion planning algorithm for different scenarios and different queries. Experimental results show that after optimization, the planning time of the algorithm is longer, but the e�ciency is signi�cantly improved. In the low-complexity scenes, STOMP optimizes the sampling algorithm very well, improves the trajectory quality greatly, and has a higher success rate. CHOMP also has a good optimization of the sampling algorithm, but it reduces the success rate of the original algorithm. However, in more complex scenes, optimization performance of the two optimization methods may not be as good as the original algorithm. In future work, we need to �nd better algorithms and better optimization algorithms to tackle with complex scenes.


Introduction
With the development and wide application of robot technology, robots re ect their importance and superiority in production and life.Robots have become an effective tool and experimental platform for studying complex intelligent behaviors and exploring human thinking patterns.The robotic arm is one of the earliest robots used in real life and social production.It is composed of many links (metal rods) and joints (electric axles).For example, a 7-DOF Panda arm is shown in Fig. 1.The robotic arm realizes any three-dimensional position and orientation within its range of motion according to the linkage and joint.The robotic arms has been assisting or streamlining operations in certain harsh or harmful environments.
In practical, the working environment of the robotic arm is very complicated, and there may be many obstacles exist, so the motion planning of the robotic arm is extremely important [1].The basic task of motion planning can be described as: the movement from the starting state to the target state [2][3][4][5][6].It must satisfy the external constraints of avoiding obstacles in the con guration space and the internal constraints of the robot's speed and acceleration in terms of machinery, sensing.In simple terms, the use of computer and software algorithm technology to determine the optimal path of the robot arm from the starting state to the target state, while ensuring that it avoids obstacles and meets its own motion performance.
For decades, as robots have become an important part of modern industry and daily life, motion planning algorithms have received a lot of attention.The earliest motion planning problem is to consider how to move a piano from one room to another room collision-free with any objects (the piano mover's problem).
In this problem, the piano has six degrees of freedom (x, y, z, roll, pitch, yaw).In order to solve this problem, a data set containing the six parameters of the piano must be set and calculated, which is time parameterized, and each parameter should be continuous for the change of time.Moving from one con guration space (starting con guration) to another con guration space (goal con guration), while meeting the internal and external constraints of obstacle avoidance.As early as 1978, Lozano Perez and Wesley introduced the conceptual construction planner of con guration space(C-Space) [1], which was an epoch-making revolution for modern motion planning.In 1979, the motion planning of the piano porter problem proved to be a PSPACE-hard problem [2].Traditional motion planning includes subdivision algorithm [3], the potential eld meth-od [4] and roadmap algorithm [5] etc.These algorithms have a common feature.When their parameters are set appropriately, these algorithms can ensure the integrity of the plan [9][10][11][12].And the upper limit of the time spent can also be guaranteed in a complex environment.
The sampling-based motion planning algorithms are proposed in recent decades and have attracted great attention.In a nutshell, they generally connect a series of randomly sampled points from an unobstructed space, trying to establish a path from the initial state to the target state.These algorithms are originated with the RPP (Randomized Potential Planner) algorithm proposed by Barraquand and Latombe [13].In 1994, the PRM (Probabilistic Road Maps) algorithm [14] and the RRT (Rapidly-exploring Random Tree) algorithm [15], setting off a new wave of research on robot motion planning.Currently, PRM and RRT belong to the Open Motion Planning Library (OMPL) [16].OMPL is a C++ open-source library based on sampling/random motion planning algorithms.It contains many prevailing algorithms for motion planning, of which the most famous are PRM and RRT.Although optimization motion planning is mentioned in OMPL, OMPL is still a sampling planning algorithm library.
Optimization-based methods recently become popular for motion planning.The most famous of these are Covariant Hamiltonian optimization for motion planning (CHOMP) [17] and Stochastic Trajectory Optimization for Motion Planning (STOMP) [18].CHOMP uses gradient-based optimization, while STOMP uses stochastic gradient-free optimization.CHOMP is a novel gradient-based trajectory optimization program and used for motion planning, which makes many daily motion planning problems simple and trainable [17].Although most high-dimensional motion planners divide trajectory generation into different planning and optimization stages, this algorithm uses covariant gradient and functional gradient methods to perform the optimization stage to design a motion planning algorithm based entirely on trajectory optimization.STOMP is a motion planner based on probability optimization [18].STOMP produces a collision-free and smooth trajectory that satis es the constraints within a certain period of time.The algorithm does not require gradients and can also produce a better trajectory.STOMP can solve motion planning queries very well and quickly when the scene is not overly complicated.
The sampling-based planning algorithm is the most common probabilistic complete algorithm and is widely used in robotic platforms with many degrees of freedom.Among such algorithms, many variant algorithms have been proposed in recent years, but there is no clear method to prove which type of problem is solved by which algorithm is the most reasonable.For different motion planners, we usually judge the performance of different motion planners through benchmarking.In this paper, we aim to optimize the sampling algorithm and benchmarking, compare the optimized trajectory metrics with the original trajectory metrics through benchmarking, in order to nd a relatively good algorithm for different scenes.Future researchers can get some help through this article.We will use the optimization-based algorithm CHOMP or STOMP to modify the initial trajectory formed by the motion planning algorithm in OMPL through the concept of Planning Adapter and Planning Pipeline.And use their different queries on different scenes to perform benchmarking respectively, and then research and analysis on the obtained data.In this way, we want to optimize some of the existing algorithms and analyze their different metrics in different problem scenes through benchmarking, so as to nd a relatively good algorithm for different planning problems.We speculate that the trajectory generated by the optimization algorithm is the shortest path, and its quality should be better than the initial trajectory, and the optimization does not affect the success rate.In different scenarios, it has a good optimization performance and stability.Our contribution is linking STOMP/CHOMP and OMPL by using the motion planning pipeline and using STOMP/CHOMP as the post-processing of OMPL through the planning request adapter.We provide relevant information for benchmarking of the implemented optimization algorithm to facilitate users to choose a speci c planner.This paper is organised as follows.Section 2 investigates the related works in motion planning and formulate the problem.Section 3 discusses the motion planning benchmarking framework and optimization procedure.In Section 4, the benchmarking and optimization results are analysed and discussed.Finally, conclusions are outlined in Section 5.

Related Works
Barraquand J and Latombe J C rst proposed the RPP method of motion planning based on sampling [13].It was this method that in uenced the subsequent random potential eld method.At the same year, another random sampling motion planning method called ZZ-method was proposed in [19].This method has had a huge impact on the Probabilistic Roadmaps (PRM) algorithm.Then the rst stochastic method in history to solve the problem of complex motion planning in high-dimensional space in a true sense, that is, the PRM method was studied [20][21][22].The PRM method has quickly become one of the most successful and popular motion planning methods based on random sampling in recent years.It has shown good performance in many high-dimensional space planning applications [23,24].J. Kuffner and Steven M. LaValle proposed Rapidly-exploring Random Trees (RRT) [25].This algorithm is another motion planning algorithm based on random sampling that has been widely developed and applied in the past ten years.The RRT algorithm is suitable for solving the path planning problems of mufti-degree-offreedom robots in complex and dynamic environments [26].In 2000, LaValle and Kuffner jointly proposed the RRT-connect algorithm [27].This bidirectional RRT technology has good search characteristics.
Compared with the original rapidly-exploring random tree algorithm, the search speed and search e ciency have been signi cantly improved.The improved algorithm of PRM, Lazy PRM, was also proposed by R. Bohlin and L. E. Kavraki in 2000.This method reduces the preprocessing process of collision detection in the sampling phase and speeds up planning [28].In 2010, Sertac and Emilio of MIT proved that in the sampling-based motion planning algorithm, as the sampling points of the RRT algorithm tend to in nity, the probability of converging to the optimal solution is 0. For this reason, they proposed the asymptotic optimality RRT* algorithm [29].In recent years, there have been some further optimization studies on RRT and PRM.Liangjun Zhang and D. Manocha proposed a new optimizationbased retraction algorithm and an enhanced version of RRT planner [30].The optimization-based retraction algorithm can improve the performance of the RRT planner by shrinking the sample, making its sample more likely to be connected to the tree.D. Kim, Y. Kwon and S. Yoon proposed an adaptive lazy collision checking named adaptive lazy PRM* in 2018 [31].The planning algorithm is based on lazy PRM, and its convergence rate is 20-250% faster than previous methods.
Regarding optimization-based methods, CHOMP and STOMP are typical representatives of optimization algorithms.CHOMP was proposed in 2009.It is a new method that uses covariant gradient techniques to improve the quality of the sampled trajectory.It is a new trajectory optimization program based on covariant gradient descent [17,32].In 2011, Zucker M et al. proposed STOMP [18].This is the rst method of motion planning using a stochastic trajectory optimization framework to explore the space around the trajectory by generating noisy trajectories.This method is different from CHOMP in that it is optimized by a speci c optimization algorithm and does not require gradient information.In recent years, CHOMP and STOMP have also received great attention and further research [33][34][35].In this article, we use the planning request adapter in Moveit to modify the trajectory created by the sampling algorithm in OMPL.The initial motion plan generated by OMPL is optimized by CHOMP/STOMP to generate a new motion planning trajectory.In this process, we set STOMP/CHOMP as the post-process of an algorithm of OMPL and use the planning pipeline which chains a motion planner (OPML) with post-processing (STOMP/CHOMP) stages and benchmark the result.
With the continuous increase of a large number of different types of planning algorithms, the evaluation of different algorithms, the tness of different scene and the comparison of different algorithms are still one of the distressing problems for scholars who study this eld.In the past two decades, a large number of motion planning algorithms have been developed.M. Elbanhawi and M. Simic conducted a general review of sampling-based planner and a comprehensive survey of the ever-growing work body in the sampling-based planner [36].Although they have considered all aspects, they did not conduct actual benchmarking and did not propose related information about benchmarks.It is di cult for novices and experts to compare which type of problem these different algorithms are suitable for.
When most motion plans are developed, there are corresponding benchmarking.In the early, Baltes J. researched a benchmark kit for mobile robots, which provided a quantitative measure of the mobile robot's ability to perform speci c tasks [37].And through benchmarking to test the path and trajectory tracking control and accuracy of the mobile robot, static path planning and dynamic path planning capabilities.The benchmarking can also be used as simple games.Including them in robotic games will allow researchers to increase their chances of evaluating their work without having to purchase expensive or specialized equipment.J. Kuffner and others also proposed a benchmark problem for humanoid robots [38].The benchmarking case used in this article is designed to experimentally evaluate the performance of path planning technology in a high-dimensional search space.At the same time, in the path planning of humanoid robots, the well-known "narrow passage" problem [39] associated with stochastic programming is considered.A benchmark system was proposed in [40] for comparing different sample-based motion planners in OMPL.The framework of this benchmark is very easy to access and use, and the benchmarks in the system can be downloaded and run for new sample-based motion planning algorithms.But it does not contain any information about the optimized planner.The benchmark system is based on the open source framework of Moveit! [41].Based on previous work, M. Moll, I. A. Sucan and L. E. Kavraki proposed a benchmark infrastructure in 2015 that is not aimed at any speci c benchmark problems, measurement standards or planning procedures [42].The infrastructure is a universal and extensible benchmarking, which can help researchers easily analyze and visualize reproducible benchmarking results.But the basic structure of the benchmarking is integrated with OMPL, and no other planners are benchmarked, especially without any optimization-based motion planning.This benchmark is also the benchmark framework used in this article, which benchmarking OMPL, CHOMP, STOMP and sample planners optimized by CHOMP/STOMP.Although the benchmarking is a broad and general software framework, other different motion planning algorithms and OMPL extensions algorithm are still to be studied.In this paper, we aim to ll this gap.

Motion Planning Benchmarking And Optimization
In this paper, we propose a framework of planning adapters to combine sampling-based planning algorithms (PRM [22], RRT, LazyPRM [28] and RRTConnect [25] in OMPL) with optimization-based algorithms (CHOMP and STOMP), and these algorithms can be used in a pipeline to produce robust motion plans.For example, the pipeline of a motion plan may be an initial plan established by OMPL, and then optimized through STOMP to generate a better path.In this section, we will introduce the methods used in this article, different motion planners and their corresponding libraries.

Sampling-based motion planning algorithm library-OPML
Among the sampling-based motion planning algorithms, the RRT algorithm, PRM algorithm, LazyPRM algorithm and RRT-Connect algorithm in OMPL are chosen.Both RRT and PRM are the most classic and basic algorithms in motion planning, so optimizing them may have obvious effects, which is bene cial to the experiment.Since it is uncertain whether the optimization algorithm is better for the basic algorithm or the improved algorithm, so we have also analyzed and researched the two algorithms of LazyPRM and RRTConnect.RRTConnect is one of the most e ciently improved algorithms.From above, it is noted that the RRTConnect algorithm compensates for the low growth e ciency of the single tree of the RRT species and the problem of tree node redundancy through the use of bidirectional-tree opposing growth strategies and Connect operations.However, some problems in RRT still exist, and RRTConnect still has these shortcomings in algorithm performance: (1) The problem of lack of directionality in tree expansion still exists; (2) As tree nodes increase, its search area is gradually reduced.The probability that the location point is selected as the sampling point also gradually decreases, that is, the guiding effect of the search area on the tree is weakened, and it is easy to cause the tree expansion to stagnate.Therefore, the RRT-Connect algorithm has much room for improvement.
Besides, RRT algorithms still have many shortcomings in the path search: (1) It is only a single tree growing from the initial point to the target point, and the algorithm e ciency is low; (2) Using uniform random sampling, although avoiding the algorithm from falling into the local optimum, the path obtained is not optimized enough due to the lack of directionality in the expansion of the random tree; (3) All points that meet the collision constraint are added to the tree as q_new, but some of the q_new may not be far apart, which is not very helpful to generate a new path, resulting in redundancy of nodes on the random number and making the tree too large and too much nodes, and cause the e ciency of the SelectNearestNeighbor function to drop signi cantly; (4) Although the algorithm can nd a path to the target node, it takes a long time for collision constraint detection and node connection caused by tree node redundancy, and the algorithm has extremely poor performance in the face of real-time planning problems.

CHOMP
CHOMP is an algorithm to iteratively improve the quality of the initial trajectory through functional gradient technology and optimize the smoothness and obstacle avoidance.It makes many motion planning problems simple and trainable.This approach can be used in high-dimensional space motion planning, while avoiding obstacles and generating smoother motion planning.It can solve motion planning queries in different scene and locally optimize feasible trajectories to improve trajectory quality, which is a simple variational strategy for achieving good trajectories.
The algorithm is mainly based on two cores: (1) gradient information is often available and can be computed inexpensively; (2) trajectory optimization should be invariant to parametrization [32].On the basis of these two principles, a motion planning algorithm is designed to generate high-quality trajectories for complex robot systems with multiple degrees of freedom, which can smoothly eliminate unnecessary motions and avoid collisions.While most high-dimensional motion planners divide trajectory generation into different planning and optimization stages, the algorithm uses the covariant gradient and functional gradient methods in the optimization stage to design a motion planning algorithm based entirely on trajectory optimization.CHOMP can quickly react to the surrounding environment and pull out the infeasible trajectory from the collision, while optimizing dynamics such as joint speed and acceleration.It can quickly converge to a smooth collision-free trajectory and can be executed e ciently on the robot.
Although CHOMP can avoid obstacles in most cases.But if the trajectory is incorrectly guessed in the initial state, the algorithm will fall into a local minimum.Due to its gradient-based nature, CHOMP usually fails to nd a solution or returns to a sub-optimal solution after falling into a local minimum, resulting in planning failure.OMPL can effectively alleviate this problem.In the experimental part, we will try to use the trajectory generated by OMPL as the initial trajectory of CHOMP and implement CHOMP optimization on this trajectory through the Motion Planning Pipeline.

STOMP
STOMP uses a stochastic trajectory optimization framework, which is an optimization-based motion planner.It will generate smooth and con ict-free motion planning trajectories in a short time.This method relies on noise trajectories to explore the space around a possible but not necessarily feasible initial trajectory, and then combines these trajectories into a lower cost updated trajectory.The cost function includes general cost, control cost, obstacle cost and smoothing cost.STOMP mainly optimizes the combination of obstacle cost and smoothing cost.In each iteration of random trajectory optimization, a series of randomly optimized trajectories with noise trajectories will be generated.The newly generated trajectory is used for simulation to determine its cost, and the candidate solution is updated by its cost.In the whole process, this method does not need to use gradient information, and it can get a certain optimization through general constraints and additional non-smooth costs.This method can directly generate high-quality trajectories or optimize the formed trajectories.
This method also uses a cost function similar to CHOMP, but compared with CHOMP, its optimization method can handle general cost functions without gradients.As mentioned above, due to gradient-based reasons, CHOMP may fall into a local minimum, and STOMP can overcome this problem through its randomness and get better optimization.OMPL is very suitable as the initial trajectory of CHOMP for optimization, STOMP and CHOMP have a similar cost function, so we reasonably speculate that OMPL as the initial trajectory of STOMP will also have a good optimization effect which also has been approved in our experiment.

Motion Planning Pipeline
In Moveit!, use the motion planner to plan the unknown environment with obstacles.In the experimental part, we will introduce in detail the detailed process of Moveit! for robot motion planning.In this part, we will talk about Motion Planning Pipeline of Moveit!.In some scenarios, some per-processing motion plan requests or post-processing motion plan requests are needed to process the motion planning.In this case, it can help us connect per-processing, motion planner, and post-processing in pipeline.The complete motion planning pipeline includes Motion Planner and Plan Request Adapters, as shown in Fig. 2.
As shown in Fig. 2, motion Planner contains the OPML, CHOMP, STOMP and other motion planning algorithms mentioned earlier.Planning request adapters (pre-processing) include: FixStartState-Bounds, FixWorkspaceBounds, FixStartStateCollision, Fix-StartStatePathConstraints, AddTimeParameter-ization, CHOMPOptimizerAdapter and STOMPSmoothing-Adapter, etc. Their functions are: FixStartStateBounds is used to repair the initial limit of joint; FixWorkspaceBounds sets the default size of the workspace; FixStartStateCollision is responsible for repairing the collision con guration le; FixStartSta-tePathConstraints is responsible for nding the posture that satis es the constraints as the initial state of the robot; Add-TimeParameterization can perform speed and acceleration constraints for the space trajectory, and add speed, acceleration, time and other parameters to each trajectory point; CHOMPOptimizerAdapter: Use CHOMP algorithm for trajectory optimization; STOMPSmoothingAdapter: Use STOMP algorithm for random trajectory optimization.The planning pipeline implements the following three functions: 1) Automatically instantiate a planner plug-in; 2) Automatically instantiate a set of planning request adapters, allowing per-processing of planning requests or post-processing of planning; 3) Combining the planner with the plan requests that the adapters are grouped together in a serialized manner.

Motion planning Benchmarking
The benchmarking package provided by Moveit! is used to perform benchmarking on motion planning algorithms and aggregate/draw statistics in combination with OMPL Planner Arena.The metrics of the current motion planning algorithm used by the robotic arm through the box plot.The benchmarking database are uploaded to OMPL Planner Arena to display interactive results.These results can help us compare different motion planners in the same environment, and also help us compare the same algorithms in different environments, so as to determine the most suitable motion planning algorithm for speci c motion planning problem.

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.

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 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 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: 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: 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 samplingbased 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: 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.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: 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.

Conclusions
In this paper, we have studied the robot motion planning benchmarking and optimization based on motion planning pipeline.Six queries in three different scenes have been explored and the most suitable algorithm has been proposed for the query.The planning algorithms studied include PRM, RRT, LazyPRM, RRTConnect in OMPL, CHOMP, STOMP, and the new algorithm obtained by OMPL with CHOMP and STOMP in pipeline, respectively.Through experimental results, it is found that in most scenes, the quality of the trajectory generated by the sampling algorithm after STOMP optimization is better than the quality of the trajectory generated after CHOMP optimization, and the success rate is higher, the time to nd the better path is short and it is easy to nd the better path.When executing queries with lower complexity in many scenes, sampling-based algorithms optimized by STOMP are the best solutions, such as RRTConnect-STOMP, PRM-STOMP, etc.But in more complex scene, STOMP algorithm has received a serious impact.This situation leads to a decrease in the success rate of its trajectory generation, and the optimization effect is greatly reduced or even worse than the original sampling algorithm.In the highcomplexity scene, only the best sampling-based algorithm RRTConnect can successfully generate a shorter motion trajectory.
The trajectory generated by the sampling algorithm is also optimized through the planning adapter and use the planning pipeline which chains one motion planner of OMPL with STOMP/CHOMP.Through experiments, we found that in general scenes, both STOMP and CHOMP are very effective in optimizing the algorithm, and STOMP has a better effect.But in more complex scenes, the optimization effect is not as good as the original algorithm.
In future work, we will consider optimizing the parameters based on the sampling planner in OMPL to study whether the optimized trajectory effect has some improvements.This experiment only uses some of the more common sampling-based optimization, we have not studied all sampling-based algorithms.Also, attempts based on sampling algorithms can be broadened, and more sampling-based planning algorithms and even improved sampling-based motion planning algorithms can be studied and benchmarking.The same is true based on optimization planning algorithms.Since only CHOMP and STOMP were tested, other optimization algorithms were not tested.Besides, we will try other prevailing optimization algorithms to optimize the sampling-based planning algorithm.CHOMP and STOMP are good optimization algorithms, they can also form better planning trajectories independently.In the future, we can also try to use optimization-based algorithms as pre-processing for other algorithms.Regarding motion planning pipeline, there is only one pre-processing or one post-processing in this paper.In the future, multiple planning adapters before and after the motion planning algorithm can be approached, using a variety of different motion planning algorithms to optimize the existing sub-trajectories.For example, after the RRT algorithm, perform STOMP optimization and CHOMP optimization.In this experiment, we only analyze and compare the basic indicators in motion planning, while different indicators can be applied in the future work.For more cluttered scenarios, the applicability and adaptability of the motion planning pipeline and optimization will need to be explored further.The query a of industrial workshop.
The query b of industrial workshop.
The query a of Library Scene.
The query b of Library Scene.
The query c of Library Scene.

Supplementary Files
This is a list of supplementary les associated with this preprint.Click to download.

Figure 1 The
Figure 1

Figure 2 Motion
Figure 2

Table 1
Tunnel Benchmarking Results

Table 5 Library_TTB
Table to bookshelf query: Benchmarking Results