Benchmark description

The benchmark problems we define here require the specification of these components:

• Robot model: The particular robot used for the experiments.
• Scenes: The environment to be used for motion planning.
• Queries: A set of initial and goal robot states associated with the scenes.

Robot Model

For our benchmarks, we used the Universal Robots UR10 robot arm. We attached a box as end-effector to simulate the collision issues we will have if a robot hand was attached. The MoveIt! configuration used can be found here.

The robot can be launched using the following command:

roslaunch sr_moveit_planner_benchmarking robot.launch

You can add the following arguments:

• initial_z: to move the robot up, such as when it needs to be on the table
• visualization: set this to true to launch rviz, or false to not start rviz
• robot_description: you can change the urdf used in case you want to change your robot or change some urdf values.

Scenes

The scene file format (.scene) is used to define a scene where the robot will be performing the motions. It can be imported, modified and exported using the Motion Planning RViz plugin.

The scenes defined for our benchmarks can be found here and are shown in the following pictures:

Queries

A query is defined as the start and goal state for a robot in a specific scene. We have used Rviz to create a set of queries for each scene with different complexity levels. The queries for each scene can be found here.

Benckmark configuration

Each benchmark has a configuration file in yaml format, containing the following information:

• Details of the warehouse
• Number of runs for each planning algorithm on each request
• The name of the group to plan
• The maximum time for a single run
• The directory to write the output to
• The stored start states in the warehouse to try
• The motion plan queries in the warehouse to try
• The bounds of the workspace the robot plans in.
• A list of planners to benchmark the queries in.

This is an example of the format of a benchmark configurarion yaml file:

benchmark_config:
    warehouse:
        host: 127.0.0.1
        port: 33829
        scene_name: scene_ground
    parameters:
        name: scene_ground
        runs: 10
        group: right_arm
        timeout: 10.0
        output_directory: /tmp/moveit_benchmarks/
        queries: .*
    planners:
        - plugin: ompl_interface/OMPLPlanner
          planners:
            - SBLkConfigDefault
            - ESTkConfigDefault
            - LBKPIECEkConfigDefault
            - BKPIECEkConfigDefault
            - KPIECEkConfigDefault
            - RRTkConfigDefault
            - RRTConnectkConfigDefault
            - RRTstarkConfigDefault
            - TRRTkConfigDefault
            - PRMkConfigDefault
            - PRMstarkConfigDefault

The configuration for our benchmarks can be found here.

Tools to work with the queries and scenes files

To load the .scene and .queries files into the warehouse, you can use the following methods:

Our high level methods

• To load all our scene and queries to the warehouse:

roslaunch sr_moveit_planner_benchmarking load_all_scenes_and_queries_to_db.launch

• To load one scene and queries files:

roslaunch sr_moveit_planner_benchmarking load_scenes_and_queries_to_db.launch queries_file:=<path_to_file> scene_file:=<path_to_file>

• To load just a scene:

roslaunch sr_moveit_planner_benchmarking load_scenes_to_db.launch scene_file:=<path_to_file>

• To load just a query:

roslaunch sr_moveit_planner_benchmarking load_queries_to_db.launch queries_file:=<path_to_file>

• To export a scene that is in the warehouse to a .scene text file:

roslaunch sr_moveit_planner_benchmarking export_scenes_to_text.launch output_directory:=<path_to_folder_to_save_file>

• To export the queries that is in the warehouse to a .queries text file:

roslaunch sr_moveit_planner_benchmarking export_queries_to_text.launch output_directory:=<path_to_folder_to_save_file> (group_prefix:=ra) (cartesian:=false)

If you want to export the queries in cartesian space (position and orientation) instead of joint space, you can set the cartesian argument to true.

Methods from MoveIt!

The previous methods, internally call a set of MoveIt! methods. You can call the MoveIt! methods directly.

Export to text

An executable is available to allow to export the scene and queries that are available in the Moveit! warehouse to text files:

rosrun moveit_ros_warehouse moveit_warehouse_save_as_text

It has different parameters that can be specified:

• help: Show help message.
• host: Host for the DB. Default 127.0.0.1
• port: Port for the DB. Default 33829
• output_directory: Directory to save the generated files.
• scene: Saves the scenes available in the warehouse. It generates .scene files. If it is not specified the queries are saved.
• cartesian: Save queries in cartesian space (start and end pose of eef). If not specified they will be saved in joint space by default.
• eef: Specify the end effector (Only needed when the cartesian option is set). Default: last link.
• group_prefix: Specify the group prefix you’d like to plan with. This is useful if you want to save the queries only for the arm and it has a prefix, so only those joints will be considered (e.g. “ra”).

To be able to use this command, the robot should be launched in another terminal first.

Here are a few examples of how to use them:

• Example to export the scenes:

  rosrun moveit_ros_warehouse moveit_warehouse_save_as_text --scene --output_directory /tmp/scene
  

  Example of the exported scene (empty space with ground): example_scene.scene

  example_scene
  * ground__link
  1
  box
  5 5 0.01
  -0.2 0.6 0
  0 0 0 1
  0 0 0 0
  .
  

• Example to export the queries in joint space:

  rosrun moveit_ros_warehouse moveit_warehouse_save_as_text --output_directory /tmp/queries --group_prefix ra
  

  Example of exporting a query: example_scene.queries

  example_scene
  example_query
  START
  ra_shoulder_pan_joint = -0.000150601
  ra_shoulder_lift_joint = -0.992381
  ra_elbow_joint = 1.73153
  ra_wrist_1_joint = 2.46467
  ra_wrist_2_joint = -0.000163041
  ra_wrist_3_joint = 3.07979
  .
  GOAL
  joint_constraint
  ra_shoulder_pan_joint = 2.20941
  ra_shoulder_lift_joint = -1.89185
  ra_elbow_joint = 1.81825
  ra_wrist_1_joint = 0.14342
  ra_wrist_2_joint = 2.20853
  ra_wrist_3_joint = 0.0420439
  .
  

• Example to export the queries in cartesian space:

  rosrun moveit_ros_warehouse moveit_warehouse_save_as_text --output_directory /tmp/queries --group_prefix ra --cartesian --eef ra_wrist_3_link
  

  Example of exporting a query: cartesian_example_scene.queries

  example_scene
  cartesian_query
  START
  ra_shoulder_pan_joint = 2.64932
  ra_shoulder_lift_joint = -1.99619
  ra_elbow_joint = -1.29736
  ra_wrist_1_joint = 1.87004
  ra_wrist_2_joint = -2.11698
  ra_wrist_3_joint = 2.63777
  .
  GOAL
  position_constraint
  End_effector = ra_wrist_3_link
  Position = -0.317122 -0.130014 0.548275
  Orientation = 0.0291105 0.158759 0.383943 0.90914
  

Import from text

An executable is available to allow to import .scenes and .queries files to the Moveit! warehouse:

rosrun moveit_ros_warehouse moveit_warehouse_import_from_text

It has different parameters that can be specified:

• help: Show help message
• host: Host for the DB. Default 127.0.0.1
• port: Port for the DB. Default 33829
• queries: Name of file containing motion planning queries.
• scene: Name of file containing motion planning scene.

To be able to use this command, the robot should be launched in another terminal first.

Here are a few examples of how to use them:

• Example to import a scene file:

  rosrun moveit_ros_warehouse moveit_warehouse_import_from_text --scene example_scene.scene
  

• Example to import a query file:

  rosrun moveit_ros_warehouse moveit_warehouse_import_from_text --queries example_scene.queries
  

Generate random valid queries

An executable is available to allow to the generation of a specified number of valid (collision free) random queries and save them into the Moveit! warehouse:

rosrun moveit_ros_warehouse moveit_warehouse_generate_random_queries <name_of_scene> <number_of_random_queries>

It has different parameters that can be specified:

• help: Show help message.
• host: Host for the DB. Default 127.0.0.1
• port: Port for the DB. Default 33829
• limited_joints: Limit joints from -pi to pi to avoid a lot of impossible queries.
• group_prefix: Specify the group prefix you’d like to plan with. This is useful if you want to save the queries only for the arm and it has a prefix, so only those joints will be considered (e.g. “ra”).
• cartesian: Generate the cartesian space query equivalent to the one generated in joint space.
• eef: Specify the end effector (Only needed when the cartesian option is set). Default: last link.
• clear: Clears all the random queries for a given scene.

After generating the queries, they are saved in the warehouse. You can use the commands above to export them to text.

Here are a few examples of how to use them:

• Example of generating 10 random queries in joint space in the example_scene:

  rosrun moveit_ros_warehouse moveit_warehouse_generate_random_queries example_scene 10 --group_prefix ra
  

• Example of generating 10 random queries in joint space in the example_scene limiting the joints:

  rosrun moveit_ros_warehouse moveit_warehouse_generate_random_queries example_scene 10 --group_prefix ra --limited_joints
  

• Example of generating 10 random queries in cartesian space in the example_scene:

  rosrun moveit_ros_warehouse moveit_warehouse_generate_random_queries example_scene 10 --group_prefix ra --cartesian --eef ra_wrist_3_link
  

• Example of clearing the random queries in example_scene:

  rosrun moveit_ros_warehouse moveit_warehouse_generate_random_queries example_scene --clear
  

Benchmark metrics

We use several metrics to perform the benchmarks of the different planners.

Default MoveIt! metrics

These are the metrics defined by default in MoveIt!:

• Total time (s):
  Time taken by the whole process. It is calculated with the following formula:
  T = plan_time + interpolation_time + simplify_time + process_time
  The lower the value, the better performance of the planner.
• Solved (%):
  Percentage of queries that the planner was able to find a solution for.
  The higher the value, the better performance of the planner.
• Correct (%):
  Percentage of solved plans generated by the planner that are correct. Correctness means that path trajectory avoids collisions and all waypoints satisfy given bounds.
  The higher the value, the better performace of the planner.
• Length (rad):
  Calculated by a sum of angles traveled by each of the joints. Formula:
  L = sum_i=0^n-1{abs(x_i- x_i0)}, where:
  n - number of robot’s joints,
  x - joint’s goal position,
  x₀ - joint’s initial position.
  The lower the value, the better the plan.
• Smoothness:
  Looks at three consecutive waypoints and the angle formed between them. Value is calculated as a square of sum of all the angles calculated that way. Formula:
  S = sum_i=2^n-1{(2 _* (pi - arccos((d_i-2,i-1²+d_i-1,i²-d_i-2,i²)/(2 _* d_{i-2,i-1 *} d_i-1,i))))²}, where:
  n - number of waypoints on the path,
  d_x,y - distance between waypoints with index x and y.
  Aligned points result in S = 0.
  The lower the value, the smoother plan.
• Clearance (m):
  Calculated by average distance to nearest invalid state (obstacle) throughout the planned path. Formula:
  C = (1/n) _* sum_i=0^n-1{cl(s_i)}, where:
  n - number of states on path,
  s_i - ith state,
  cl() - distance to the first invalid state.
  The higher the value, the better the plan.

New proposed metrics

We have added two new metrics:

• Plan quality 1:
  Calculated by a weighted sum of angles traveled by each of the joints, giving higher weights to the joints closer to the base of the robot, thus penalizing them as small movements of these joints will result in bigger movements of the end effector. Formula:
  PQ₁ = sum_i=0^n-1{w_{i *} abs(x_i - x_i0)}, where:
  n - number of robot’s joints,
  w - weight specified for each joint,
  x - joint’s goal position,
  x₀ - joint’s initial position.
  The lower the value, the better the plan.
• Plan quality 2:
  Measures how different is the total distance traveled by the arm end effector to the most direct path between the start and goal position. Formula:
  PQ₂ = (d_t/d_l + r_t/r_l)/2, where:
  d_t - total distance travelled by the end effector,
  d_l - distance travelled by the end effector in a straight line,
  r_t - total rotation done by the end effector,
  r_l - relative rotation of end effector between start and goal pose.
  The lower the value, the better the plan with 1 being the best possible score.