From 81aa9321ca6310dde661bb244a1378da4add24fb Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 8 May 2023 15:03:24 +0200 Subject: [PATCH 01/16] Initial commit for planner cost function tutorial --- CMakeLists.txt | 1 + .../planner_cost_functions/CMakeLists.txt | 13 + .../config/planner_cost_moveit_cpp.yaml | 21 ++ .../launch/planner_cost_functions.launch.py | 171 ++++++++++++ .../planner_cost_functions_tutorial.rst | 4 + .../src/planner_cost_functions_main.cpp | 254 ++++++++++++++++++ 6 files changed, 464 insertions(+) create mode 100644 doc/how_to_guides/planner_cost_functions/CMakeLists.txt create mode 100644 doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml create mode 100644 doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py create mode 100644 doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst create mode 100644 doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ef0b667e..21b0ed67ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/how_to_guides/chomp_planner) add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) +add_subdirectory(doc/how_to_guides/planner_cost_functions) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor) add_subdirectory(doc/tutorials/quickstart_in_rviz) diff --git a/doc/how_to_guides/planner_cost_functions/CMakeLists.txt b/doc/how_to_guides/planner_cost_functions/CMakeLists.txt new file mode 100644 index 0000000000..aa39114b5c --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(planner_cost_functions_example src/planner_cost_functions_main.cpp) +target_include_directories(planner_cost_functions_example PRIVATE include) +ament_target_dependencies(planner_cost_functions_example ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS planner_cost_functions_example + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) 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 new file mode 100644 index 0000000000..90b9e9e2d3 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/config/planner_cost_moveit_cpp.yaml @@ -0,0 +1,21 @@ +# Default configurations. More information can be found in the moveit_cpp and planning scene monitor tutorials +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +# 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"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl # Different OMPL pipeline name! + planner_id: "RRTstarkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 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 new file mode 100644 index 0000000000..d1494c2dfc --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/launch/planner_cost_functions.launch.py @@ -0,0 +1,171 @@ +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config", + default_value="panda_moveit_config_demo.rviz", + description="RViz configuration file", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) + + +def launch_setup(context, *args, **kwargs): + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines("ompl", ["ompl"]) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .moveit_cpp( + os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "planner_cost_moveit_cpp.yaml", + ) + ) + .to_moveit_configs() + ) + + # Warehouse config + sqlite_database = os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "panda_test_db.sqlite", + ) + + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse": { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "host": sqlite_database, + "port": 33828, + "scene_name": "kitchen_panda_scene_sensed1", + }, + } + + # 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], + ) + + # RViz + rviz_config_file = os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "parallel_planning_config.rviz", + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="both", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + nodes_to_start = [ + static_tf, + robot_state_publisher, + rviz_node, + moveit_cpp_node, + ros2_control_node, + joint_state_broadcaster_spawner, + arm_controller_spawner, + hand_controller_spawner, + ] + + return nodes_to_start diff --git a/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst b/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst new file mode 100644 index 0000000000..a97f863ec8 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst @@ -0,0 +1,4 @@ +Using cost functions in the MoveIt Planning Pipeline +==================================================== + +TODO \ No newline at end of file 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 new file mode 100644 index 0000000000..c8a996b8b0 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/src/planner_cost_functions_main.cpp @@ -0,0 +1,254 @@ +#include +#include +// MoveitCpp +#include +#include + +#include + +#include + +// Warehouse +#include +#include +#include +#include +#include +#include + +namespace rvt = rviz_visual_tools; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("plannner_cost_fn_example"); +const std::string PLANNING_GROUP = "panda_arm"; +static const std::vector CONTROLLERS(1, "panda_arm_controller"); +} // namespace +namespace plannner_cost_fn_example +{ + +/// \brief Utility class to create and interact with the parallel planning demo +class Demo +{ +public: + Demo(rclcpp::Node::SharedPtr node) + : node_{ node } + , moveit_cpp_{ std::make_shared(node) } + , planning_component_{ std::make_shared(PLANNING_GROUP, moveit_cpp_) } + , visual_tools_(node, "panda_link0", "plannner_cost_fn_example", moveit_cpp_->getPlanningSceneMonitorNonConst()) + { + moveit_cpp_->getPlanningSceneMonitorNonConst()->providePlanningSceneService(); + + visual_tools_.deleteAllMarkers(); + visual_tools_.loadRemoteControl(); + + 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_.trigger(); + } + + bool loadPlanningSceneAndQuery() + { + std::string hostname = ""; + int port = 0.0; + std::string scene_name = ""; + + node_->get_parameter_or(std::string("warehouse.host"), hostname, std::string("127.0.0.1")); + node_->get_parameter_or(std::string("warehouse.port"), port, 33829); + + if (!node_->get_parameter("warehouse.scene_name", scene_name)) + { + RCLCPP_WARN(LOGGER, "Warehouse scene_name NOT specified"); + } + + moveit_warehouse::PlanningSceneStorage* planning_scene_storage = nullptr; + + // Initialize database connection + try + { + warehouse_ros::DatabaseLoader db_loader(node_); + warehouse_ros::DatabaseConnection::Ptr warehouse_connection = db_loader.loadDatabase(); + warehouse_connection->setParams(hostname, port, 20); + if (warehouse_connection->connect()) + { + planning_scene_storage = new moveit_warehouse::PlanningSceneStorage(warehouse_connection); + RCLCPP_INFO(LOGGER, "Connected to database: '%s'", hostname.c_str()); + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to connect to database"); + return false; + } + } + catch (std::exception& e) + { + RCLCPP_ERROR(LOGGER, "Failed to initialize planning scene storage: '%s'", e.what()); + return false; + } + + // Load planning scene + moveit_msgs::msg::PlanningScene scene_msg; + try + { + if (!planning_scene_storage) + { + RCLCPP_ERROR(LOGGER, "No planning scene storage"); + return false; + } + + if (planning_scene_storage->hasPlanningScene(scene_name)) // Just the world (no robot) + { + moveit_msgs::msg::PlanningSceneWorld world_meta_data; + if (!planning_scene_storage->getPlanningSceneWorld(world_meta_data, scene_name)) + { + RCLCPP_ERROR(LOGGER, "Failed to load planning scene world '%s'", scene_name.c_str()); + return false; + } + scene_msg.world = world_meta_data; + scene_msg.robot_model_name = "No robot information. Using only world geometry."; + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to find planning scene '%s'", scene_name.c_str()); + return false; + } + } + catch (std::exception& ex) + { + RCLCPP_ERROR(LOGGER, "Error loading planning scene: %s", ex.what()); + return false; + } + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_->getPlanningSceneMonitorNonConst()); + scene->processPlanningSceneWorldMsg(scene_msg.world); + } // Unlock PlanningScene + + RCLCPP_INFO(LOGGER, "Loaded planning scene successfully"); + + // Get planning scene query + moveit_warehouse::MotionPlanRequestWithMetadata planning_query; + std::string query_name = scene_name + "_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(*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() + { + // Set goal state + if (!planning_query_request_.goal_constraints.empty()) + { + planning_component_->setGoal(planning_query_request_.goal_constraints); + } + else + RCLCPP_ERROR(LOGGER, "Planning query request does not contain any goal constraints"); + } + + /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. + void planAndPrint() + { + // Set start state as current state + planning_component_->setStartStateToCurrentState(); + + auto plan_solution = planning_component_->plan(); + + // Check if PlanningComponents succeeded in finding the plan + if (plan_solution) + { + // Visualize the trajectory in Rviz + auto robot_model_ptr = moveit_cpp_->getRobotModel(); + auto robot_start_state = planning_component_->getStartState(); + auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); + visual_tools_.trigger(); + } + + // Execute the trajectory and block until it's finished + moveit_cpp_->execute(plan_solution.trajectory, true /* blocking*/, CONTROLLERS); + + // Start the next plan + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + visual_tools_.deleteAllMarkers(); + visual_tools_.trigger(); + } + +private: + std::shared_ptr node_; + std::shared_ptr moveit_cpp_; + std::shared_ptr planning_component_; + moveit_visual_tools::MoveItVisualTools visual_tools_; + moveit_msgs::msg::MotionPlanRequest planning_query_request_; +}; +} // namespace plannner_cost_fn_example + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + RCLCPP_INFO(LOGGER, "Initialize node"); + + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("plannner_cost_fn_example", "", node_options); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + plannner_cost_fn_example::Demo demo(node); + + if (!demo.loadPlanningSceneAndQuery()) + { + rclcpp::shutdown(); + return 0; + } + + RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); + + // Experiment 1 - Short free-space motion, Pilz is expected to create the fastest and shortest solution + RCLCPP_INFO(LOGGER, "Experiment 1 - Short free-space motion"); + + demo.setJointGoal(0.0, -0.8144019900299497, 0.0, -2.6488387075338133, 0.0, 1.8344367175038623, 0.7849999829891612); + demo.planAndPrint(); + + // Experiment 2 - Long motion with collisions, CHOMP and Pilz are likely to fail here due to the difficulty of the planning problem + RCLCPP_INFO(LOGGER, "Experiment 2 - Long motion with collisions"); + demo.setQueryGoal(); + demo.planAndPrint(); + + rclcpp::shutdown(); + return 0; +} From 846501b3194c9cb78a3b3d227c9bb3b06b392b28 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 8 May 2023 17:27:14 +0200 Subject: [PATCH 02/16] Format and add cost_function --- .../planner_cost_functions_tutorial.rst | 2 +- .../src/planner_cost_functions_main.cpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst b/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst index a97f863ec8..9ea29a45c7 100644 --- a/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst +++ b/doc/how_to_guides/planner_cost_functions/planner_cost_functions_tutorial.rst @@ -1,4 +1,4 @@ Using cost functions in the MoveIt Planning Pipeline ==================================================== -TODO \ No newline at end of file +TODO 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 index c8a996b8b0..df3043bc41 100644 --- 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 @@ -8,6 +8,8 @@ #include +// Cost functions +#include // Warehouse #include #include @@ -183,6 +185,9 @@ class Demo // Set start state as current state planning_component_->setStartStateToCurrentState(); + // Set cost function + planning_component_->setStateCostFunction(moveit::cost_functions::getClearanceCostFn()); + auto plan_solution = planning_component_->plan(); // Check if PlanningComponents succeeded in finding the plan From fddff903f6b6f89479c9e154e09da1c5fb824bc2 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 10 May 2023 16:54:25 +0200 Subject: [PATCH 03/16] Format and visualize --- .../src/planner_cost_functions_main.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) 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 index df3043bc41..741bb00b29 100644 --- 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 @@ -186,7 +186,17 @@ class Demo planning_component_->setStartStateToCurrentState(); // Set cost function - planning_component_->setStateCostFunction(moveit::cost_functions::getClearanceCostFn()); + planning_component_->setStateCostFunction( + [this, LOGGER](const moveit::core::RobotState& robot_state, const planning_interface::MotionPlanRequest& request, + const planning_scene::PlanningSceneConstPtr& planning_scene) 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(); + return clearance_cost_fn(robot_state, request, planning_scene); + }); auto plan_solution = planning_component_->plan(); @@ -211,6 +221,11 @@ class Demo visual_tools_.trigger(); } + moveit_visual_tools::MoveItVisualTools& getVisualTools() + { + return visual_tools_; + } + private: std::shared_ptr node_; std::shared_ptr moveit_cpp_; From fa3c3964bb64e79e68a5eeaabae90ac02e763e32 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 17 May 2023 17:22:49 +0200 Subject: [PATCH 04/16] Update to new cost function interface --- .../src/planner_cost_functions_main.cpp | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) 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 index 741bb00b29..828a2b6c90 100644 --- 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 @@ -176,7 +176,9 @@ class Demo planning_component_->setGoal(planning_query_request_.goal_constraints); } else + { RCLCPP_ERROR(LOGGER, "Planning query request does not contain any goal constraints"); + } } /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. @@ -185,17 +187,28 @@ class Demo // Set start state as current state planning_component_->setStartStateToCurrentState(); + // Get start state + auto robot_start_state = planning_component_->getStartState(); + + // Get planning scene + auto planning_scene_monitor = moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); + auto planning_scene = [planning_scene_monitor] { + planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor); + return planning_scene::PlanningScene::clone(ls); + }(); + + auto group_name = planning_query_request_.group_name; // Set cost function planning_component_->setStateCostFunction( - [this, LOGGER](const moveit::core::RobotState& robot_state, const planning_interface::MotionPlanRequest& request, - const planning_scene::PlanningSceneConstPtr& planning_scene) mutable { + [robot_start_state, group_name, planning_scene](const std::vector& 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(); - return clearance_cost_fn(robot_state, request, planning_scene); + auto clearance_cost_fn = + moveit::cost_functions::getClearanceCostFn(*robot_start_state, group_name, planning_scene); + return clearance_cost_fn(state_vector); }); auto plan_solution = planning_component_->plan(); @@ -205,7 +218,6 @@ class Demo { // Visualize the trajectory in Rviz auto robot_model_ptr = moveit_cpp_->getRobotModel(); - auto robot_start_state = planning_component_->getStartState(); auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); From 768b1904dc2f7084fca65831ad2c5dcc5398e063 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 18 May 2023 16:44:53 +0200 Subject: [PATCH 05/16] Add stomp as selectable planner --- .../config/planner_cost_moveit_cpp.yaml | 8 ++++---- .../launch/planner_cost_functions.launch.py | 2 +- 2 files changed, 5 insertions(+), 5 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 index 90b9e9e2d3..87141bd7fb 100644 --- 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"] + pipeline_names: ["ompl", "stomp"] plan_request_params: planning_attempts: 1 - planning_pipeline: ompl # Different OMPL pipeline name! - planner_id: "RRTstarkConfigDefault" + planning_pipeline: stomp + planner_id: "stomp" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 - planning_time: 1.5 + planning_time: 10.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 index d1494c2dfc..7862c03d0d 100644 --- 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,7 @@ def launch_setup(context, *args, **kwargs): .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True ) - .planning_pipelines("ompl", ["ompl"]) + .planning_pipelines("ompl", ["ompl", "stomp"]) .trajectory_execution(file_path="config/moveit_controllers.yaml") .moveit_cpp( os.path.join( From 548d1f8c09899f70f12ff60b0f713a6fa6735e65 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 23 May 2023 14:27:11 +0200 Subject: [PATCH 06/16] Use stomp + cleanup --- doc/examples/moveit_cpp/config/moveit_cpp.yaml | 5 +++-- .../moveit_cpp/launch/moveit_cpp_tutorial.launch.py | 1 + .../src/planner_cost_functions_main.cpp | 10 ++-------- 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/doc/examples/moveit_cpp/config/moveit_cpp.yaml b/doc/examples/moveit_cpp/config/moveit_cpp.yaml index ffa333f8a4..8d6915fc56 100644 --- a/doc/examples/moveit_cpp/config/moveit_cpp.yaml +++ b/doc/examples/moveit_cpp/config/moveit_cpp.yaml @@ -9,10 +9,11 @@ planning_scene_monitor_options: planning_pipelines: #namespace: "moveit_cpp" # optional, default is ~ - pipeline_names: ["ompl"] + pipeline_names: ["stomp"] plan_request_params: planning_attempts: 1 - planning_pipeline: ompl + planning_pipeline: stomp + planner_id: "stomp" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 diff --git a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py index b1ea4678f2..63c2d1d737 100644 --- a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py +++ b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines("stomp", ["stomp"]) .moveit_cpp( file_path=get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp.yaml" 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 index 828a2b6c90..b01443167d 100644 --- 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 @@ -270,14 +270,8 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); - // Experiment 1 - Short free-space motion, Pilz is expected to create the fastest and shortest solution - RCLCPP_INFO(LOGGER, "Experiment 1 - Short free-space motion"); - - demo.setJointGoal(0.0, -0.8144019900299497, 0.0, -2.6488387075338133, 0.0, 1.8344367175038623, 0.7849999829891612); - demo.planAndPrint(); - - // Experiment 2 - Long motion with collisions, CHOMP and Pilz are likely to fail here due to the difficulty of the planning problem - RCLCPP_INFO(LOGGER, "Experiment 2 - Long motion with collisions"); + // 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.planAndPrint(); From 2723a987ec4209864d281331b5d19098f71a2436 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 24 May 2023 11:26:38 +0200 Subject: [PATCH 07/16] Add TrajectoryLineVisualization --- .../config/cost_fn_config.rviz | 459 ++++++++++++++++++ .../config/planner_cost_moveit_cpp.yaml | 2 +- .../launch/planner_cost_functions.launch.py | 2 +- .../src/planner_cost_functions_main.cpp | 129 ++++- 4 files changed, 573 insertions(+), 19 deletions(-) create mode 100644 doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz diff --git a/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz new file mode 100644 index 0000000000..980b3ccdf7 --- /dev/null +++ b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz @@ -0,0 +1,459 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 671 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: false + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /moveit_cpp/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Path: true + Sphere: true + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plannner_cost_fn_example + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: hand + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.8018128871917725 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.4451259672641754 + Y: -0.10787936300039291 + Z: 0.5332368612289429 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.16039825975894928 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.0435791015625 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1058 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 22 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 index 87141bd7fb..508936676b 100644 --- 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 @@ -18,4 +18,4 @@ plan_request_params: planner_id: "stomp" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 - planning_time: 10.0 + planning_time: 5.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 index 7862c03d0d..b2f1f8fc4d 100644 --- 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 @@ -84,7 +84,7 @@ def launch_setup(context, *args, **kwargs): rviz_config_file = os.path.join( get_package_share_directory("moveit2_tutorials"), "config", - "parallel_planning_config.rviz", + "cost_fn_config.rviz", ) rviz_node = Node( 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 index b01443167d..62fc7f1d13 100644 --- 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 @@ -29,6 +29,63 @@ static const std::vector CONTROLLERS(1, "panda_arm_controller"); namespace plannner_cost_fn_example { +// Color to string +[[nodiscard]] std::string colorToString(rviz_visual_tools::Colors rviz_color) +{ + std::string color_str = "unknown"; + switch (rviz_color) + { + case rviz_visual_tools::Colors::BLACK: + color_str = "black"; + break; + case rviz_visual_tools::Colors::BROWN: + color_str = "brown"; + break; + case rviz_visual_tools::Colors::BLUE: + color_str = "blue"; + break; + case rviz_visual_tools::Colors::CYAN: + color_str = "cyan"; + break; + case rviz_visual_tools::Colors::GREY: + color_str = "grey"; + break; + case rviz_visual_tools::Colors::DARK_GREY: + color_str = "dark grey"; + break; + case rviz_visual_tools::Colors::GREEN: + color_str = "green"; + break; + case rviz_visual_tools::Colors::LIME_GREEN: + color_str = "lime green"; + break; + case rviz_visual_tools::Colors::MAGENTA: + color_str = "magenta"; + break; + case rviz_visual_tools::Colors::ORANGE: + color_str = "orange"; + break; + case rviz_visual_tools::Colors::PURPLE: + color_str = "purple"; + break; + case rviz_visual_tools::Colors::RED: + color_str = "red"; + break; + case rviz_visual_tools::Colors::PINK: + color_str = "pink"; + break; + case rviz_visual_tools::Colors::WHITE: + color_str = "white"; + break; + case rviz_visual_tools::Colors::YELLOW: + color_str = "yellow"; + break; + default: + break; + } + return color_str; +} + /// \brief Utility class to create and interact with the parallel planning demo class Demo { @@ -173,6 +230,21 @@ class Demo // Set goal state if (!planning_query_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); } else @@ -182,8 +254,11 @@ class Demo } /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. - void planAndPrint() + void planAndVisualize(std::vector> pipeline_planner_pairs) { + visual_tools_.deleteAllMarkers(); + visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // Set start state as current state planning_component_->setStartStateToCurrentState(); @@ -211,25 +286,42 @@ class Demo return clearance_cost_fn(state_vector); }); - auto plan_solution = planning_component_->plan(); + moveit_cpp::PlanningComponent::PlanRequestParameters plan_request_parameters; + plan_request_parameters.load(node_); + RCLCPP_DEBUG_STREAM( + LOGGER, "Default plan request parameters loaded with --" + << " planning_pipeline: " << plan_request_parameters.planning_pipeline << ',' + << " planner_id: " << plan_request_parameters.planner_id << ',' + << " planning_time: " << plan_request_parameters.planning_time << ',' + << " planning_attempts: " << plan_request_parameters.planning_attempts << ',' + << " max_velocity_scaling_factor: " << plan_request_parameters.max_velocity_scaling_factor << ',' + << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); + + std::vector solutions; + solutions.reserve(pipeline_planner_pairs.size()); + for (auto const& pipeline_planner_pair : pipeline_planner_pairs) + { + plan_request_parameters.planning_pipeline = pipeline_planner_pair.first; + plan_request_parameters.planner_id = pipeline_planner_pair.second; + solutions.push_back(planning_component_->plan(plan_request_parameters, planning_scene)); + } + int color_index = 1; + 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 - if (plan_solution) + for (auto const& plan_solution : solutions) { - // Visualize the trajectory in Rviz - auto robot_model_ptr = moveit_cpp_->getRobotModel(); - auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); - - visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); - visual_tools_.trigger(); + 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++; + } } - - // Execute the trajectory and block until it's finished - moveit_cpp_->execute(plan_solution.trajectory, true /* blocking*/, CONTROLLERS); - - // Start the next plan - visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools_.deleteAllMarkers(); visual_tools_.trigger(); } @@ -273,8 +365,11 @@ int main(int argc, char** argv) // 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.planAndPrint(); + demo.planAndVisualize( + { { "ompl", "RRTConnectkConfigDefault" }, { "ompl", "RRTstarkConfigDefault" }, { "stomp", "stomp" } }); + + demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); rclcpp::shutdown(); return 0; } From 916bc0f52124d7526ba75d9dd0732e28d9ce2f95 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 26 May 2023 12:45:37 +0200 Subject: [PATCH 08/16] Update function signature to Eigen --- .../planner_cost_functions/src/planner_cost_functions_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 62fc7f1d13..a60b78a0f3 100644 --- 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 @@ -276,7 +276,7 @@ class Demo auto group_name = planning_query_request_.group_name; // Set cost function planning_component_->setStateCostFunction( - [robot_start_state, group_name, planning_scene](const std::vector& state_vector) mutable { + [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, From faeb028887699778c28b078f47d992b0e9f132cd Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 14 Jun 2023 18:32:08 +0200 Subject: [PATCH 09/16] Add dedicated STOMP publisher marker --- .../config/cost_fn_config.rviz | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz index 980b3ccdf7..d38f02ae42 100644 --- a/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz +++ b/doc/how_to_guides/planner_cost_functions/config/cost_fn_config.rviz @@ -153,6 +153,20 @@ Visualization Manager: Trajectory Topic: /display_planned_path Use Sim Time: false Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Path: true + Sphere: true + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep All + Reliability Policy: Reliable + Value: /stomp_visualization + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray From d0a27386c61f9aec21c2d25a9adf4032813393c4 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 19 Jul 2023 08:40:08 +0200 Subject: [PATCH 10/16] TMP --- .../config/planner_cost_moveit_cpp.yaml | 6 +-- .../launch/planner_cost_functions.launch.py | 29 ++++++++++++++- .../src/planner_cost_functions_main.cpp | 37 ++++++++++++------- 3 files changed, 54 insertions(+), 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 index 508936676b..eefa0e2b0c 100644 --- 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 index b2f1f8fc4d..b8990467c8 100644 --- 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 index a60b78a0f3..1478fcaec6 100644 --- 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& 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 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(); From 2cc0454bf616562ddd0a966a013b6c586266e542 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 31 Jul 2023 12:06:36 +0200 Subject: [PATCH 11/16] Iterate through more benchmark queries --- .../config/planner_cost_moveit_cpp.yaml | 6 +- .../launch/planner_cost_functions.launch.py | 53 ++++--- .../src/planner_cost_functions_main.cpp | 146 +++++++----------- 3 files changed, 89 insertions(+), 116 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 index eefa0e2b0c..a540a4510f 100644 --- 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: ["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 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 index b8990467c8..ff2f117ab0 100644 --- 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 @@ -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,7 +156,7 @@ 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( @@ -163,8 +164,6 @@ def launch_setup(context, *args, **kwargs): executable="spawner", arguments=[ "joint_state_broadcaster", - "--controller-manager-timeout", - "300", "--controller-manager", "/controller_manager", ], 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 index 1478fcaec6..854a6d4b76 100644 --- 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 @@ -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(*planning_query); + motion_plan_requests.push_back(static_cast(*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> pipeline_planner_pairs) + void planAndVisualize(std::vector> 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& 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 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 getMotionPlanRequests() + { + return motion_plan_requests; + } + private: std::shared_ptr node_; std::shared_ptr moveit_cpp_; std::shared_ptr planning_component_; moveit_visual_tools::MoveItVisualTools visual_tools_; - moveit_msgs::msg::MotionPlanRequest planning_query_request_; + std::vector 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(); From 7f985f5301388b506dbfac5818d8010ea917cd68 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 7 Aug 2023 10:52:36 +0200 Subject: [PATCH 12/16] Make custom cost function optional --- .../config/planner_cost_moveit_cpp.yaml | 2 +- .../src/planner_cost_functions_main.cpp | 77 ++++++++----------- 2 files changed, 35 insertions(+), 44 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 index a540a4510f..2e4175d8c4 100644 --- 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 @@ -18,4 +18,4 @@ plan_request_params: planner_id: "stomp" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 - planning_time: 10.0 + planning_time: 4.0 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 index 854a6d4b76..7fb05b92c1 100644 --- 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 @@ -208,42 +208,22 @@ class Demo return true; } - /// \brief Set goal state for next planning attempt based on query loaded from the database - void setQueryGoal(moveit_msgs::msg::MotionPlanRequest const& motion_plan_request) + struct PipelineConfig { - // Set goal state - if (!motion_plan_request.goal_constraints.empty()) - { - // 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 - { - RCLCPP_ERROR(LOGGER, "Planning query request does not contain any goal constraints"); - } - } + std::string planning_pipeline; + std::string planner_id; + bool use_cost_function; + }; /// \brief Request a motion plan based on the assumption that a goal is set and print debug information. - void planAndVisualize(std::vector> pipeline_planner_pairs, + void planAndVisualize(std::vector pipeline_configs, moveit_msgs::msg::MotionPlanRequest const& motion_plan_request) { visual_tools_.deleteAllMarkers(); visual_tools_.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + // Set goal state + planning_component_->setGoal(motion_plan_request.goal_constraints); // Set start state as current state planning_component_->setStartStateToCurrentState(); @@ -259,14 +239,6 @@ class Demo }(); 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 { - // 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); - }); moveit_cpp::PlanningComponent::PlanRequestParameters plan_request_parameters; plan_request_parameters.load(node_); @@ -280,11 +252,28 @@ class Demo << " max_acceleration_scaling_factor: " << plan_request_parameters.max_acceleration_scaling_factor); std::vector solutions; - solutions.reserve(pipeline_planner_pairs.size()); - for (auto const& pipeline_planner_pair : pipeline_planner_pairs) + solutions.reserve(pipeline_configs.size()); + for (auto const& pipeline_config : pipeline_configs) { - plan_request_parameters.planning_pipeline = pipeline_planner_pair.first; - plan_request_parameters.planner_id = pipeline_planner_pair.second; + plan_request_parameters.planning_pipeline = pipeline_config.planning_pipeline; + plan_request_parameters.planner_id = pipeline_config.planner_id; + + // Set cost function + if (pipeline_config.use_cost_function) + { + planning_component_->setStateCostFunction( + [robot_start_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable { + // 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); + }); + } + else + { + planning_component_->setStateCostFunction(nullptr); + } + solutions.push_back(planning_component_->plan(plan_request_parameters, planning_scene)); } @@ -297,7 +286,8 @@ class Demo if (plan_solution.trajectory) { RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str() - << ": " << colorToString(rviz_visual_tools::Colors(color_index))); + << ", " << std::to_string(plan_solution.use_cost_function) << ": " + << 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)); @@ -350,8 +340,9 @@ int main(int argc, char** argv) 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.planAndVisualize( + { { "ompl", "RRTConnectkConfigDefault", false }, { "stomp", "stomp", false }, { "stomp", "stomp", true } }, + motion_plan_req); } demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); From 8b763997a38e30142482ba5d1f7645bc11557881 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 10 Aug 2023 09:19:29 +0200 Subject: [PATCH 13/16] Cleanup and switch to getMinJointDisplacementCostFn --- .../src/planner_cost_functions_main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 index 7fb05b92c1..981a6085c9 100644 --- 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 @@ -263,9 +263,8 @@ class Demo { planning_component_->setStateCostFunction( [robot_start_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable { - // rviz_visual_tools::MEDIUM); this->getVisualTools().trigger(); auto clearance_cost_fn = - moveit::cost_functions::getClearanceCostFn(*robot_start_state, group_name, planning_scene); + moveit::cost_functions::getMinJointDisplacementCostFn(*robot_start_state, group_name, planning_scene); return clearance_cost_fn(state_vector); }); } @@ -286,8 +285,7 @@ class Demo if (plan_solution.trajectory) { RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str() - << ", " << std::to_string(plan_solution.use_cost_function) << ": " - << colorToString(rviz_visual_tools::Colors(color_index))); + << ": " << 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)); From 37ba0319f602b3e08d9fa5d7a5246e6ab9190135 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 14 Sep 2023 16:57:22 +0200 Subject: [PATCH 14/16] Format! --- .../src/planner_cost_functions_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) 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 index 981a6085c9..f6a9b54619 100644 --- 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 @@ -272,8 +272,13 @@ class Demo { planning_component_->setStateCostFunction(nullptr); } + auto solution = planning_component_->plan(plan_request_parameters, planning_scene); + if (pipeline_config.use_cost_function) + { + solution.planner_id += std::string("_with_cost_term"); + } - solutions.push_back(planning_component_->plan(plan_request_parameters, planning_scene)); + solutions.push_back(solution); } int color_index = 1; @@ -285,7 +290,8 @@ class Demo if (plan_solution.trajectory) { RCLCPP_INFO_STREAM(LOGGER, plan_solution.planner_id.c_str() - << ": " << colorToString(rviz_visual_tools::Colors(color_index))); + << ": " << colorToString(rviz_visual_tools::Colors(color_index)) + << ", Path length: " << robot_trajectory::path_length(*plan_solution.trajectory)); // Visualize the trajectory in Rviz visual_tools_.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr, rviz_visual_tools::Colors(color_index)); From 9b726a1a99c315f551e4ce3fbcc682630d526c8f Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 26 Oct 2023 13:31:06 +0200 Subject: [PATCH 15/16] Update function name --- .../planner_cost_functions/src/planner_cost_functions_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index f6a9b54619..c690be291a 100644 --- 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 @@ -264,7 +264,7 @@ class Demo planning_component_->setStateCostFunction( [robot_start_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable { auto clearance_cost_fn = - moveit::cost_functions::getMinJointDisplacementCostFn(*robot_start_state, group_name, planning_scene); + moveit::cost_functions::createMinJointDisplacementCostFn(*robot_start_state, group_name, planning_scene); return clearance_cost_fn(state_vector); }); } From 863a9a520eace08f991fd524ae5660d46959d2e5 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 26 Oct 2023 13:40:31 +0200 Subject: [PATCH 16/16] Revert moveit_cpp changes and add small fix --- doc/examples/moveit_cpp/config/moveit_cpp.yaml | 6 +++--- .../moveit_cpp/launch/moveit_cpp_tutorial.launch.py | 2 +- .../src/planner_cost_functions_main.cpp | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/doc/examples/moveit_cpp/config/moveit_cpp.yaml b/doc/examples/moveit_cpp/config/moveit_cpp.yaml index 8d6915fc56..0dbd6f88ee 100644 --- a/doc/examples/moveit_cpp/config/moveit_cpp.yaml +++ b/doc/examples/moveit_cpp/config/moveit_cpp.yaml @@ -9,11 +9,11 @@ planning_scene_monitor_options: planning_pipelines: #namespace: "moveit_cpp" # optional, default is ~ - pipeline_names: ["stomp"] + pipeline_names: ["ompl"] plan_request_params: planning_attempts: 1 - planning_pipeline: stomp - planner_id: "stomp" + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 diff --git a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py index 63c2d1d737..4e1a9143c2 100644 --- a/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py +++ b/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py @@ -11,7 +11,7 @@ def generate_launch_description(): MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") - .planning_pipelines("stomp", ["stomp"]) + .planning_pipelines("ompl", ["ompl"]) .moveit_cpp( file_path=get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp.yaml" 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 index c690be291a..690f9d1472 100644 --- 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 @@ -261,12 +261,12 @@ class Demo // Set cost function if (pipeline_config.use_cost_function) { - planning_component_->setStateCostFunction( - [robot_start_state, group_name, planning_scene](const Eigen::VectorXd& state_vector) mutable { - auto clearance_cost_fn = - moveit::cost_functions::createMinJointDisplacementCostFn(*robot_start_state, group_name, planning_scene); - return clearance_cost_fn(state_vector); - }); + planning_component_->setStateCostFunction([robot_start_state, group_name, + planning_scene](const Eigen::VectorXd& state_vector) mutable { + auto clearance_cost_fn = + moveit::cost_functions::createMinJointDisplacementCostFn(*robot_start_state, group_name, planning_scene); + return clearance_cost_fn(state_vector); + }); } else {