Iterate through more benchmark queries

Author: sjahr

doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml

@@ -10,12 +10,12 @@ planning_scene_monitor_options:
 
 # Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning
 planning_pipelines:
-  pipeline_names: ["ompl_stomp"]
+  pipeline_names: ["stomp", "ompl"]
 
 plan_request_params:
   planning_attempts: 1
-  planning_pipeline: ompl_stomp
+  planning_pipeline: stomp
   planner_id: "stomp"
   max_velocity_scaling_factor: 1.0
   max_acceleration_scaling_factor: 1.0
-  planning_time: 30.0
+  planning_time: 10.0

doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py

@@ -39,6 +39,7 @@ def launch_setup(context, *args, **kwargs):
     moveit_config = (
         MoveItConfigsBuilder("moveit_resources_panda")
         .robot_description(file_path="config/panda.urdf.xacro")
+        .planning_pipelines(pipelines=["ompl", "stomp"])
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
@@ -55,9 +56,9 @@ def launch_setup(context, *args, **kwargs):
 
     # Warehouse config
     sqlite_database = os.path.join(
-        get_package_share_directory("moveit2_tutorials"),
-        "config",
-        "panda_test_db.sqlite",
+        get_package_share_directory("moveit_resources_benchmarking"),
+        "databases",
+        "panda_kitchen_test_db.sqlite",
     )
 
     warehouse_ros_config = {
@@ -71,26 +72,26 @@ def launch_setup(context, *args, **kwargs):
     }
 
     # Load additional OMPL pipeline
-    ompl_stomp_planning_pipeline_config = {
-        "ompl_stomp": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            "request_adapters": """\
-                stomp_moveit/StompSmoothingAdapter \
-                default_planner_request_adapters/AddTimeOptimalParameterization \
-                default_planner_request_adapters/FixWorkspaceBounds \
-                default_planner_request_adapters/FixStartStateBounds \
-                default_planner_request_adapters/FixStartStateCollision \
-                default_planner_request_adapters/FixStartStatePathConstraints \
-              """,
-            "start_state_max_bounds_error": 0.1,
-            "planner_configs": {
-                "RRTConnectkConfigDefault": {
-                    "type": "geometric::RRTConnect",
-                    "range": 0.0,  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()}
-                }
-            },
-        }
-    }
+    # ompl_stomp_planning_pipeline_config = {
+    #    "ompl_stomp": {
+    #        "planning_plugin": "ompl_interface/OMPLPlanner",
+    #        "request_adapters": """\
+    #            stomp_moveit/StompSmoothingAdapter \
+    #            default_planner_request_adapters/AddTimeOptimalParameterization \
+    #            default_planner_request_adapters/FixWorkspaceBounds \
+    #            default_planner_request_adapters/FixStartStateBounds \
+    #            default_planner_request_adapters/FixStartStateCollision \
+    #            default_planner_request_adapters/FixStartStatePathConstraints \
+    #          """,
+    #        "start_state_max_bounds_error": 0.1,
+    #        "planner_configs": {
+    #            "RRTConnectkConfigDefault": {
+    #                "type": "geometric::RRTConnect",
+    #                "range": 0.0,  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()}
+    #            }
+    #        },
+    #    }
+    # }
 
     # MoveItCpp demo executable
     moveit_cpp_node = Node(
@@ -100,7 +101,7 @@ def launch_setup(context, *args, **kwargs):
         output="screen",
         parameters=[
             moveit_config.to_dict(),
-            ompl_stomp_planning_pipeline_config,
+            # ompl_stomp_planning_pipeline_config,
             warehouse_ros_config,
         ],
     )
@@ -155,16 +156,14 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
-        output="both",
+        output="screen",
     )
 
     joint_state_broadcaster_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=[
             "joint_state_broadcaster",
-            "--controller-manager-timeout",
-            "300",
             "--controller-manager",
             "/controller_manager",
         ],

doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp

@@ -103,7 +103,7 @@ class Demo
 
     Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
     text_pose.translation().z() = 1.75;
-    visual_tools_.publishText(text_pose, "Parallel Planning Tutorial", rvt::WHITE, rvt::XLARGE);
+    visual_tools_.publishText(text_pose, "Cost Function Tutorial", rvt::WHITE, rvt::XLARGE);
     visual_tools_.trigger();
   }
 
@@ -188,64 +188,48 @@ class Demo
     RCLCPP_INFO(LOGGER, "Loaded planning scene successfully");
 
     // Get planning scene query
-    moveit_warehouse::MotionPlanRequestWithMetadata planning_query;
-    std::string query_name = scene_name + "_query";
-    try
+    for (int index = 0; index < 10; index++)
     {
-      planning_scene_storage->getPlanningQuery(planning_query, scene_name, query_name);
-    }
-    catch (std::exception& ex)
-    {
-      RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what());
-    }
+      moveit_warehouse::MotionPlanRequestWithMetadata planning_query;
+      std::string query_name = "kitchen_panda_scene_sensed" + std::to_string(index) + "_query";
+      try
+      {
+        planning_scene_storage->getPlanningQuery(planning_query, scene_name, query_name);
+      }
+      catch (std::exception& ex)
+      {
+        RCLCPP_ERROR(LOGGER, "Error loading motion planning query '%s': %s", query_name.c_str(), ex.what());
+      }
 
-    planning_query_request_ = static_cast<moveit_msgs::msg::MotionPlanRequest>(*planning_query);
+      motion_plan_requests.push_back(static_cast<moveit_msgs::msg::MotionPlanRequest>(*planning_query));
+    }
     visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
     visual_tools_.trigger();
     return true;
   }
 
-  /// \brief Set joint goal state for next planning attempt
-  /// \param [in] panda_jointN Goal value for the Nth joint [rad]
-  void setJointGoal(double const panda_joint1, double const panda_joint2, double const panda_joint3,
-                    double const panda_joint4, double const panda_joint5, double const panda_joint6,
-                    double const panda_joint7)
-  {
-    auto robot_goal_state = planning_component_->getStartState();
-    robot_goal_state->setJointPositions("panda_joint1", &panda_joint1);
-    robot_goal_state->setJointPositions("panda_joint2", &panda_joint2);
-    robot_goal_state->setJointPositions("panda_joint3", &panda_joint3);
-    robot_goal_state->setJointPositions("panda_joint4", &panda_joint4);
-    robot_goal_state->setJointPositions("panda_joint5", &panda_joint5);
-    robot_goal_state->setJointPositions("panda_joint6", &panda_joint6);
-    robot_goal_state->setJointPositions("panda_joint7", &panda_joint7);
-
-    // Set goal state
-    planning_component_->setGoal(*robot_goal_state);
-  }
-
   /// \brief Set goal state for next planning attempt based on query loaded from the database
-  void setQueryGoal()
+  void setQueryGoal(moveit_msgs::msg::MotionPlanRequest const& motion_plan_request)
   {
     // Set goal state
-    if (!planning_query_request_.goal_constraints.empty())
+    if (!motion_plan_request.goal_constraints.empty())
     {
-      for (auto constraint : planning_query_request_.goal_constraints)
-      {
-        for (auto joint_constraint : constraint.joint_constraints)
-        {
-          RCLCPP_INFO_STREAM(LOGGER, "position: " << joint_constraint.position << "\n"
-                                                  << "tolerance_above: " << joint_constraint.tolerance_above << "\n"
-                                                  << "tolerance_below: " << joint_constraint.tolerance_below << "\n"
-                                                  << "weight: " << joint_constraint.weight << "\n");
-        }
-        for (auto position_constraint : constraint.position_constraints)
-        {
-          RCLCPP_WARN(LOGGER, "Goal constraints contain position constrains, please use joint constraints only in this "
-                              "example.");
-        }
-      }
-      planning_component_->setGoal(planning_query_request_.goal_constraints);
+      // for (auto constraint : motion_plan_request.goal_constraints)
+      // {
+      //   for (auto joint_constraint : constraint.joint_constraints)
+      //   {
+      //     RCLCPP_INFO_STREAM(LOGGER, "position: " << joint_constraint.position << "\n"
+      //                                             << "tolerance_above: " << joint_constraint.tolerance_above << "\n"
+      //                                             << "tolerance_below: " << joint_constraint.tolerance_below << "\n"
+      //                                             << "weight: " << joint_constraint.weight << "\n");
+      //   }
+      //   for (auto position_constraint : constraint.position_constraints)
+      //   {
+      //     RCLCPP_WARN(LOGGER, "Goal constraints contain position constrains, please use joint constraints only in this "
+      //                         "example.");
+      //   }
+      // }
+      planning_component_->setGoal(motion_plan_request.goal_constraints);
     }
     else
     {
@@ -254,7 +238,8 @@ class Demo
   }
 
   /// \brief Request a motion plan based on the assumption that a goal is set and print debug information.
-  void planAndVisualize(std::vector<std::pair<std::string, std::string>> pipeline_planner_pairs)
+  void planAndVisualize(std::vector<std::pair<std::string, std::string>> pipeline_planner_pairs,
+                        moveit_msgs::msg::MotionPlanRequest const& motion_plan_request)
   {
     visual_tools_.deleteAllMarkers();
     visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
@@ -273,13 +258,10 @@ class Demo
       return planning_scene::PlanningScene::clone(ls);
     }();
 
-    auto group_name = planning_query_request_.group_name;
+    auto group_name = motion_plan_request.group_name;
     // Set cost function
     planning_component_->setStateCostFunction(
         [robot_start_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable {
-          // Publish robot state
-          // auto const ee_tip = robot_state.getJointModelGroup(PLANNING_GROUP)->getOnlyOneEndEffectorTip();
-          // this->getVisualTools().publishSphere(robot_state.getGlobalLinkTransform(ee_tip), rviz_visual_tools::GREEN,
           // rviz_visual_tools::MEDIUM); this->getVisualTools().trigger();
           auto clearance_cost_fn =
               moveit::cost_functions::getClearanceCostFn(*robot_start_state, group_name, planning_scene);
@@ -288,7 +270,7 @@ class Demo
 
     moveit_cpp::PlanningComponent::PlanRequestParameters plan_request_parameters;
     plan_request_parameters.load(node_);
-    RCLCPP_DEBUG_STREAM(
+    RCLCPP_INFO_STREAM(
         LOGGER, "Default plan request parameters loaded with --"
                     << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ','
                     << " planner_id: " << plan_request_parameters.planner_id << ','
@@ -297,17 +279,6 @@ class Demo
                     << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ','
                     << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor);
 
-    // planning_component_->setStateCostFunction(
-    //     [robot_start_state, group_name, planning_scene](const std::vector<double>& state_vector) mutable {
-    //       // Publish robot state
-    //       // auto const ee_tip = robot_state.getJointModelGroup(PLANNING_GROUP)->getOnlyOneEndEffectorTip();
-    //       // this->getVisualTools().publishSphere(robot_state.getGlobalLinkTransform(ee_tip), rviz_visual_tools::GREEN,
-    //       // rviz_visual_tools::MEDIUM); this->getVisualTools().trigger();
-    //       auto clearance_cost_fn =
-    //           moveit::cost_functions::getClearanceCostFn(*robot_start_state, group_name, planning_scene);
-    //       return clearance_cost_fn(state_vector);
-    //     });
-
     std::vector<planning_interface::MotionPlanResponse> solutions;
     solutions.reserve(pipeline_planner_pairs.size());
     for (auto const& pipeline_planner_pair : pipeline_planner_pairs)
@@ -321,18 +292,18 @@ class Demo
     auto robot_model_ptr = moveit_cpp_->getRobotModel();
     auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
     // Check if PlanningComponents succeeded in finding the plan
-    // for (auto const& plan_solution : solutions)
-    //{
-    //  if (plan_solution.trajectory)
-    //  {
-    //    RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str()
-    //                                   << ": " << colorToString(rviz_visual_tools::Colors(color_index)));
-    //    // Visualize the trajectory in Rviz
-    //    visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr,
-    //                                        rviz_visual_tools::Colors(color_index));
-    //    color_index++;
-    //  }
-    //}
+    for (auto const& plan_solution : solutions)
+    {
+      if (plan_solution.trajectory)
+      {
+        RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str()
+                                       << ": " << colorToString(rviz_visual_tools::Colors(color_index)));
+        // Visualize the trajectory in Rviz
+        visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr,
+                                            rviz_visual_tools::Colors(color_index));
+        color_index++;
+      }
+    }
     visual_tools_.trigger();
   }
 
@@ -341,12 +312,17 @@ class Demo
     return visual_tools_;
   }
 
+  std::vector<moveit_msgs::msg::MotionPlanRequest> getMotionPlanRequests()
+  {
+    return motion_plan_requests;
+  }
+
 private:
   std::shared_ptr<rclcpp::Node> node_;
   std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_;
   std::shared_ptr<moveit_cpp::PlanningComponent> planning_component_;
   moveit_visual_tools::MoveItVisualTools visual_tools_;
-  moveit_msgs::msg::MotionPlanRequest planning_query_request_;
+  std::vector<moveit_msgs::msg::MotionPlanRequest> motion_plan_requests;
 };
 }  // namespace plannner_cost_fn_example
 
@@ -371,14 +347,12 @@ int main(int argc, char** argv)
     return 0;
   }
 
-  RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");
-
-  // Experiment - Long motion with collisions, CHOMP and Pilz are likely to fail here due to the difficulty of the planning problem
-  RCLCPP_INFO(LOGGER, "Experiment - Long motion with collisions");
-  demo.setQueryGoal();
-
-  demo.planAndVisualize(
-      { { "ompl_stomp", "RRTConnectkConfigDefault" }, /*{ "ompl", "RRTstarkConfigDefault" }, { "stomp", "stomp" }*/ });
+  RCLCPP_INFO(LOGGER, "Starting Cost Function Tutorial...");
+  for (auto const& motion_plan_req : demo.getMotionPlanRequests())
+  {
+    demo.setQueryGoal(motion_plan_req);
+    demo.planAndVisualize({ { "ompl", "RRTConnectkConfigDefault" }, { "stomp", "stomp" } }, motion_plan_req);
+  }
 
   demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
   rclcpp::shutdown();