TMP

moveit · Jul 31, 2023 · f61dfd4 · f61dfd4
1 parent 52be8f2
commit f61dfd4
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 18 deletions.
diff --git a/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml b/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: ["ompl_stomp"]
 
 plan_request_params:
   planning_attempts: 1
-  planning_pipeline: stomp
+  planning_pipeline: ompl_stomp
   planner_id: "stomp"
   max_velocity_scaling_factor: 1.0
   max_acceleration_scaling_factor: 1.0
-  planning_time: 5.0
+  planning_time: 30.0
diff --git a/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py b/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py
@@ -42,7 +42,6 @@ def launch_setup(context, *args, **kwargs):
         .planning_scene_monitor(
             publish_robot_description=True, publish_robot_description_semantic=True
         )
-        .planning_pipelines("ompl", ["ompl", "stomp"])
         .trajectory_execution(file_path="config/moveit_controllers.yaml")
         .moveit_cpp(
             os.path.join(
@@ -71,13 +70,39 @@ 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()}
+                }
+            },
+        }
+    }
+
     # MoveItCpp demo executable
     moveit_cpp_node = Node(
         name="planner_cost_functions_example",
         package="moveit2_tutorials",
         executable="planner_cost_functions_example",
         output="screen",
-        parameters=[moveit_config.to_dict(), warehouse_ros_config],
+        parameters=[
+            moveit_config.to_dict(),
+            ompl_stomp_planning_pipeline_config,
+            warehouse_ros_config,
+        ],
     )
 
     # RViz

diff --git a/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp b/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp
@@ -297,6 +297,17 @@ 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)
@@ -310,18 +321,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();
   }
 
@@ -367,7 +378,7 @@ int main(int argc, char** argv)
   demo.setQueryGoal();
 
   demo.planAndVisualize(
-      { { "ompl", "RRTConnectkConfigDefault" }, { "ompl", "RRTstarkConfigDefault" }, { "stomp", "stomp" } });
+      { { "ompl_stomp", "RRTConnectkConfigDefault" }, /*{ "ompl", "RRTstarkConfigDefault" }, { "stomp", "stomp" }*/ });
 
   demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");
   rclcpp::shutdown();