From 5fa6344fe72a2f135a8d1922c8b6e116341f3838 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 5 Jul 2022 11:07:13 +0200 Subject: [PATCH 01/15] First demo and tests --- CMakeLists.txt | 1 + .../create_better_paths/CMakeLists.txt | 13 ++ .../launch/better_paths_demo.launch.py | 101 +++++++++++ .../create_better_paths/path_quality.md | 2 + .../src/better_paths_main.cpp | 167 ++++++++++++++++++ 5 files changed, 284 insertions(+) create mode 100644 doc/how_to_guides/create_better_paths/CMakeLists.txt create mode 100644 doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py create mode 100644 doc/how_to_guides/create_better_paths/path_quality.md create mode 100644 doc/how_to_guides/create_better_paths/src/better_paths_main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f779981de..03a668bfa9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ add_subdirectory(doc/examples/planning_scene_ros_api) add_subdirectory(doc/examples/planning_scene) add_subdirectory(doc/examples/realtime_servo) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) +add_subdirectory(doc/how_to_guides/create_better_paths) add_subdirectory(doc/examples/robot_model_and_robot_state) 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/create_better_paths/CMakeLists.txt b/doc/how_to_guides/create_better_paths/CMakeLists.txt new file mode 100644 index 0000000000..d78acedbca --- /dev/null +++ b/doc/how_to_guides/create_better_paths/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(better_paths_demo src/better_paths_main.cpp) +target_include_directories(better_paths_demo PUBLIC include) +ament_target_dependencies(better_paths_demo ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + +install(TARGETS better_paths_demo + 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/create_better_paths/launch/better_paths_demo.launch.py b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py new file mode 100644 index 0000000000..067d995e80 --- /dev/null +++ b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py @@ -0,0 +1,101 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .moveit_cpp( + file_path=get_package_share_directory("moveit2_tutorials") + + "/config/moveit_cpp.yaml" + ) + .to_moveit_configs() + ) + # MoveItCpp demo executable + moveit_cpp_node = Node( + name="moveit_cpp_tutorial", + package="moveit2_tutorials", + executable="better_paths_demo", + output="screen", + parameters=[moveit_config.to_dict()], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit2_tutorials") + + "/launch/moveit_cpp_tutorial.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, + ], + ) + + # 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", + ) + + # Load controllers + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="screen", + ) + ] + + return LaunchDescription( + [ + static_tf, + robot_state_publisher, + rviz_node, + moveit_cpp_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/doc/how_to_guides/create_better_paths/path_quality.md b/doc/how_to_guides/create_better_paths/path_quality.md new file mode 100644 index 0000000000..dcfa4b605a --- /dev/null +++ b/doc/how_to_guides/create_better_paths/path_quality.md @@ -0,0 +1,2 @@ +# How to improve MoveIt2 path quality? +## What is path quality? diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp new file mode 100644 index 0000000000..78d298729f --- /dev/null +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -0,0 +1,167 @@ +#include +#include +// MoveitCpp +#include +#include + +#include + +#include + +namespace rvt = rviz_visual_tools; + +// All source files that use ROS logging should define a file-specific +// static const rclcpp::Logger named LOGGER, located at the top of the file +// and inside the namespace with the narrowest scope (if there is one) +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_tutorial"); + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + RCLCPP_INFO(LOGGER, "Initialize node"); + + // This enables loading undeclared parameters + // best practice would be to declare parameters in the corresponding classes + // and provide descriptions about expected use + node_options.automatically_declare_parameters_from_overrides(true); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options); + + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + static const std::string PLANNING_GROUP = "panda_arm"; + static const std::string LOGNAME = "better_paths_tutorial"; + + /* Otherwise robot with zeros joint_states */ + rclcpp::sleep_for(std::chrono::seconds(1)); + + RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials..."); + + auto moveit_cpp_ptr = std::make_shared(node); + moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); + + auto planning_components = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + auto robot_model_ptr = moveit_cpp_ptr->getRobotModel(); + auto robot_start_state = planning_components->getStartState(); + auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + // Visualization + // ^^^^^^^^^^^^^ + // + // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, + // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script + moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "moveit_cpp_tutorial", + moveit_cpp_ptr->getPlanningSceneMonitor()); + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.75; + visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + + planning_components->setStartStateToCurrentState(); + + +// Some helper functions +auto printAndPlan = [&](auto num_iterations, auto execute = false){ + for(auto i = 0; i < num_iterations; i++) + { + planning_components->setStartStateToCurrentState(); + + auto plan_solution2 = planning_components->plan(); + + // Check if PlanningComponents succeeded in finding the plan + if (plan_solution2) + { + // Visualize the trajectory in Rviz + visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); + visual_tools.trigger(); + } + // Start the next plan + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + if(execute) + { + planning_components->execute();} + }}; + +auto 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_components->getStartState(); + robot_goal_state->setJointPositions("panda_joint1", &panda_joint1); + robot_goal_state->setJointPositions("panda_joint2", &panda_joint2); + 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); + robot_goal_state->setJointPositions("panda_joint3", &panda_joint3); + + planning_components->setGoal(*robot_goal_state); +}; + +auto setCartGoal = [&](double const x, double const y, double const z, double const qx, double const qy, + double const qz, double const qw){ + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = "panda_link0"; + target_pose.pose.orientation.x = qx; + target_pose.pose.orientation.y = qy; + target_pose.pose.orientation.z = qz; + target_pose.pose.orientation.w = qw; + target_pose.pose.position.x = x; + target_pose.pose.position.y = y; + target_pose.pose.position.z = z; + planning_components->setGoal(target_pose, "panda_link8"); +}; + + // Experiment 0 - Long motion - Joint goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); +setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); + +printAndPlan(3, false); + + // Experiment 0 - Long motion - Cartesian goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); +setCartGoal(-0.013696,0.005568,-0.38056,0.92464,-0.42889,0.26552,0.6666); +printAndPlan(3, false); + + + // Experiment 1 - Short motion - Cartesian goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Short motion - Cartesian goal ##############"); +setCartGoal(0.28,-0.2, 0.5, 0.0, 0.0, 0.0, 1.0); +printAndPlan(3, false); + + // Experiment 1 - Short motion - Joint goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Short motion - Joint goal ##############"); +setJointGoal(2.8886513579712823, 0.3875018855212286, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212, 2.720914148047324); +printAndPlan(3, true); + + + + // Experiment 2 - Same pose - Cartesian goal + // ^^^^^ +RCLCPP_INFO(LOGGER, "################ Same pose - Cartesian goal ##############"); +setJointGoal(2.8886513579712823, 0.3875018855212286, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212, 2.720914148047324); +printAndPlan(3, true); + +RCLCPP_INFO(LOGGER, "################ Same pose - Joint goal ##############"); +setJointGoal(2.8886513579712823, 0.3875018855212286, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212, 2.720914148047324); +printAndPlan(3, true); + + + RCLCPP_INFO(LOGGER, "Shutting down."); + rclcpp::shutdown(); + return 0; +} From 45362b371152696f448c109158ba1ff3419e7a77 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 5 Jul 2022 13:18:05 +0200 Subject: [PATCH 02/15] Add path length evaluation function --- .../src/better_paths_main.cpp | 71 ++++++++++++++----- 1 file changed, 55 insertions(+), 16 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index 78d298729f..bac8d4ff23 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -69,29 +69,67 @@ int main(int argc, char** argv) planning_components->setStartStateToCurrentState(); +// Benchmark function +// Compute and print average path length, path similarity +auto analyzeResult = [&](std::vector solutions){ + + // Adapted from https://github.com/ros-planning/moveit2/blob/main/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L872 + // Analyzing the trajectories geometrically + auto average_traj_len = 0.0; + auto average_path_simularity = 0.0; + + // Calculate average path length + for(auto solution : solutions){ + // Only process successful solutions + if(solution.error_code == moveit::core::MoveItErrorCode::SUCCESS){ + // Compute path length + for (std::size_t index = 1; index < solution.trajectory->getWayPointCount(); ++index) + average_traj_len += solution.trajectory->getWayPoint(index - 1).distance(solution.trajectory->getWayPoint(index)); + } + } + + if(solutions.size() > 0){ + average_traj_len /= solutions.size(); + } + +// Path similarity + + RCLCPP_INFO_STREAM(LOGGER, "Average path length (sum of average L1 norm): '" << average_traj_len << " rad'"); + RCLCPP_INFO_STREAM(LOGGER, "Average path similarity: '" << average_traj_len << "'"); +}; + // Some helper functions auto printAndPlan = [&](auto num_iterations, auto execute = false){ + std::vector solutions; + solutions.reserve(num_iterations); for(auto i = 0; i < num_iterations; i++) { - planning_components->setStartStateToCurrentState(); + planning_components->setStartStateToCurrentState(); - auto plan_solution2 = planning_components->plan(); + auto plan_solution = planning_components->plan(); // Check if PlanningComponents succeeded in finding the plan - if (plan_solution2) + if (plan_solution) { // Visualize the trajectory in Rviz - visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); + visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); visual_tools.trigger(); + solutions.push_back(plan_solution); } // Start the next plan visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); visual_tools.deleteAllMarkers(); visual_tools.trigger(); - if(execute) - { - planning_components->execute();} - }}; + if(execute) + { + planning_components->execute(); + } + + } + + // Print result + analyzeResult(solutions); + }; auto setJointGoal = [&](double const panda_joint1, double const panda_joint2, double const panda_joint3, double const panda_joint4, double const panda_joint5, @@ -115,7 +153,7 @@ auto setCartGoal = [&](double const x, double const y, double const z, double co target_pose.pose.orientation.x = qx; target_pose.pose.orientation.y = qy; target_pose.pose.orientation.z = qz; - target_pose.pose.orientation.w = qw; + target_pose.pose.orientation.w = qx; target_pose.pose.position.x = x; target_pose.pose.position.y = y; target_pose.pose.position.z = z; @@ -132,7 +170,7 @@ printAndPlan(3, false); // Experiment 0 - Long motion - Cartesian goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); -setCartGoal(-0.013696,0.005568,-0.38056,0.92464,-0.42889,0.26552,0.6666); +setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); printAndPlan(3, false); @@ -145,20 +183,21 @@ printAndPlan(3, false); // Experiment 1 - Short motion - Joint goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Short motion - Joint goal ##############"); -setJointGoal(2.8886513579712823, 0.3875018855212286, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212, 2.720914148047324); -printAndPlan(3, true); +setJointGoal(2.8886513579712823, 0.3875018855212286, 2.720914148047324, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212); +printAndPlan(3, false); // Experiment 2 - Same pose - Cartesian goal // ^^^^^ RCLCPP_INFO(LOGGER, "################ Same pose - Cartesian goal ##############"); -setJointGoal(2.8886513579712823, 0.3875018855212286, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212, 2.720914148047324); -printAndPlan(3, true); +setCartGoal(0.30702, -5.2214e-12, 0.59027, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); +printAndPlan(3, false); RCLCPP_INFO(LOGGER, "################ Same pose - Joint goal ##############"); -setJointGoal(2.8886513579712823, 0.3875018855212286, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212, 2.720914148047324); -printAndPlan(3, true); +setJointGoal(0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785); + +printAndPlan(3, false); RCLCPP_INFO(LOGGER, "Shutting down."); From ddd24a837904e1209b13beab4ee2ad3494303833 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 5 Jul 2022 14:43:33 +0200 Subject: [PATCH 03/15] Add difficult collision object --- .../src/better_paths_main.cpp | 68 ++++++++++++++++++- 1 file changed, 65 insertions(+), 3 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index bac8d4ff23..6d408721a2 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv) Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); text_pose.translation().z() = 1.75; - visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Better Paths Tutorial", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); @@ -94,8 +94,8 @@ auto analyzeResult = [&](std::vectorsetGoal(target_pose, "panda_link8"); }; +auto add_collision_objects = [&](){ + moveit_msgs::msg::CollisionObject collision_object_1; + moveit_msgs::msg::CollisionObject collision_object_2; + moveit_msgs::msg::CollisionObject collision_object_3; + + collision_object_1.header.frame_id = "panda_link0"; + collision_object_1.id = "box1"; + + collision_object_2.header.frame_id = "panda_link0"; + collision_object_2.id = "box2"; + + collision_object_3.header.frame_id = "panda_link0"; + collision_object_3.id = "box3"; + + shape_msgs::msg::SolidPrimitive box_1; + box_1.type = box_1.BOX; + box_1.dimensions = { 0.01, 0.12, 1.0 }; + + shape_msgs::msg::SolidPrimitive box_2; + box_2.type = box_2.BOX; + box_2.dimensions = { 0.01, 1.0, 0.2 }; + + // Add new collision objects + geometry_msgs::msg::Pose box_pose_1; + box_pose_1.position.x = 0.2; + box_pose_1.position.y = 0.2; + box_pose_1.position.z = 0.7; + + collision_object_1.primitives.push_back(box_1); + collision_object_1.primitive_poses.push_back(box_pose_1); + collision_object_1.operation = collision_object_1.ADD; + + geometry_msgs::msg::Pose box_pose_2; + box_pose_2.position.x = 0.2; + box_pose_2.position.y = -0.2; + box_pose_2.position.z = 0.7; + + collision_object_2.primitives.push_back(box_1); + collision_object_2.primitive_poses.push_back(box_pose_2); + collision_object_2.operation = collision_object_2.ADD; + + geometry_msgs::msg::Pose box_pose_3; + box_pose_3.position.x = -0.1; + box_pose_3.position.y = 0.0; + box_pose_3.position.z = 0.85; + + collision_object_3.primitives.push_back(box_2); + collision_object_3.primitive_poses.push_back(box_pose_3); + collision_object_3.operation = collision_object_3.ADD; + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor()); + scene->processCollisionObjectMsg(collision_object_1); + scene->processCollisionObjectMsg(collision_object_2); + scene->processCollisionObjectMsg(collision_object_3); + } // Unlock PlanningScene +}; + // Experiment 0 - Long motion - Joint goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); +add_collision_objects(); printAndPlan(3, false); // Experiment 0 - Long motion - Cartesian goal @@ -200,6 +260,8 @@ setJointGoal(0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785); printAndPlan(3, false); + + RCLCPP_INFO(LOGGER, "Shutting down."); rclcpp::shutdown(); return 0; From 62797f0c5cff159659b737aad2482cc479167680 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 5 Jul 2022 14:57:58 +0200 Subject: [PATCH 04/15] Better rviz config --- .../create_better_paths/launch/better_paths_demo.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py index 067d995e80..5225e1bb45 100644 --- a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py +++ b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): # RViz rviz_config_file = ( get_package_share_directory("moveit2_tutorials") - + "/launch/moveit_cpp_tutorial.rviz" + + "/config/better_paths_config.rviz" ) rviz_node = Node( package="rviz2", From 712418ae299e1034e5e80dbe142f16da967d7951 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 5 Jul 2022 14:58:14 +0200 Subject: [PATCH 05/15] Reorder experiments --- .../src/better_paths_main.cpp | 41 ++++++++++++------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index 6d408721a2..fe7e2d8137 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -219,21 +219,6 @@ auto add_collision_objects = [&](){ } // Unlock PlanningScene }; - // Experiment 0 - Long motion - Joint goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); -setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); - -add_collision_objects(); -printAndPlan(3, false); - - // Experiment 0 - Long motion - Cartesian goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); -setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); -printAndPlan(3, false); - - // Experiment 1 - Short motion - Cartesian goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Short motion - Cartesian goal ##############"); @@ -259,6 +244,32 @@ setJointGoal(0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785); printAndPlan(3, false); + // Experiment 3 - Long motion freespace - Joint goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); +setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); + +printAndPlan(3, false); + + // Experiment 3 - Long motion freespace - Cartesian goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); +setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); +printAndPlan(3, false); + + // Experiment 4 - Long motion with collisions - Joint goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); +add_collision_objects(); +setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); +printAndPlan(3, false); + + // Experiment 4 - Long motion with collisions - Cartesian goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); +setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); +printAndPlan(3, false); + From 5444521eb08a2f91b3a08ce619cc51e86da6c0d0 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 6 Jul 2022 13:42:06 +0200 Subject: [PATCH 06/15] Fix cart. orientation and add visiual text before each experiment --- .../src/better_paths_main.cpp | 62 ++++++++++++------- .../ompl_constrained_planning_tutorial.cpp | 2 +- 2 files changed, 41 insertions(+), 23 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index fe7e2d8137..edcf8be71c 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -153,7 +153,7 @@ auto setCartGoal = [&](double const x, double const y, double const z, double co target_pose.pose.orientation.x = qx; target_pose.pose.orientation.y = qy; target_pose.pose.orientation.z = qz; - target_pose.pose.orientation.w = qx; + target_pose.pose.orientation.w = qw; target_pose.pose.position.x = x; target_pose.pose.position.y = y; target_pose.pose.position.z = z; @@ -219,56 +219,74 @@ auto add_collision_objects = [&](){ } // Unlock PlanningScene }; - // Experiment 1 - Short motion - Cartesian goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +// Experiment 1 - Short motion - Cartesian goal +// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Short motion - Cartesian goal ##############"); -setCartGoal(0.28,-0.2, 0.5, 0.0, 0.0, 0.0, 1.0); +visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); +setCartGoal(0.28,-0.2, 0.5, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); printAndPlan(3, false); - // Experiment 1 - Short motion - Joint goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +// Experiment 1 - Short motion - Joint goal +// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Short motion - Joint goal ##############"); +visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); setJointGoal(2.8886513579712823, 0.3875018855212286, 2.720914148047324, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212); printAndPlan(3, false); - // Experiment 2 - Same pose - Cartesian goal - // ^^^^^ +// Experiment 2 - Same pose - Cartesian goal +// ^^^^^ RCLCPP_INFO(LOGGER, "################ Same pose - Cartesian goal ##############"); +visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); setCartGoal(0.30702, -5.2214e-12, 0.59027, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); -printAndPlan(3, false); +printAndPlan(1, true); +printAndPlan(15, false); +// Experiment 2 - Same pose - Joint goal +// ^^^^^ RCLCPP_INFO(LOGGER, "################ Same pose - Joint goal ##############"); +visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); setJointGoal(0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785); - printAndPlan(3, false); - // Experiment 3 - Long motion freespace - Joint goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +// Experiment 3 - Long motion freespace - Joint goal +// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); +visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); - printAndPlan(3, false); - // Experiment 3 - Long motion freespace - Cartesian goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +// Experiment 3 - Long motion freespace - Cartesian goal +// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); -setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); +visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); +setCartGoal(-0.42889,0.26552,0.6666, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); printAndPlan(3, false); - // Experiment 4 - Long motion with collisions - Joint goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +// Experiment 4 - Long motion with collisions - Joint goal +// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); add_collision_objects(); +visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); +visual_tools.trigger(); setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); -printAndPlan(3, false); +printAndPlan(5, false); // Experiment 4 - Long motion with collisions - Cartesian goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); -setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); -printAndPlan(3, false); + RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); + visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); + printAndPlan(5, false); diff --git a/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp b/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp index 6541f7d0ad..9351f3d377 100644 --- a/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp +++ b/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp @@ -143,7 +143,7 @@ int main(int argc, char** argv) auto d = sqrt(pow(target_pose.pose.position.y, 2) + pow(target_pose.pose.position.z, 2)); Eigen::Vector3d normal(0, 1, 1); - moveit_visual_tools.publishNormalAndDistancePlane(normal, d, rviz_visual_tools::TRANSLUCENT_DARK); + //moveit_visual_tools.publishNormalAndDistancePlane(normal, d, rviz_visual_tools::TRANSLUCENT_DARK); moveit_visual_tools.trigger(); moveit_msgs::msg::Constraints plane_constraints; From 029caa5cb3defa28bcb495df622f079467c594af Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 13 Jul 2022 20:11:51 +0200 Subject: [PATCH 07/15] Add config for multi-pipeline planning --- .../config/moveit_cpp_better_paths.yaml | 51 +++++++++++++++++++ .../launch/better_paths_demo.launch.py | 3 +- .../src/better_paths_main.cpp | 6 ++- 3 files changed, 58 insertions(+), 2 deletions(-) create mode 100644 doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml diff --git a/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml b/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml new file mode 100644 index 0000000000..ddf2340dca --- /dev/null +++ b/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml @@ -0,0 +1,51 @@ +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 + +planning_pipelines: + #namespace: "moveit_cpp" # optional, default is ~ + pipeline_names: ["pilz_industrial_motion_planner"] #["ompl", "pilz_industrial_motion_planner"] + +# PILZ planner config + pilz_industrial_motion_planner: + name: pilz_industrial_motion_planner + planning_plugin: "pilz_industrial_motion_planner/CommandPlanner" + planner_id: "LIN" + request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + 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 + + +one: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +two: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "PRMstarkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +three: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "LIN" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 \ No newline at end of file diff --git a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py index 5225e1bb45..dfa09bd67b 100644 --- a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py +++ b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py @@ -13,8 +13,9 @@ def generate_launch_description(): .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") .moveit_cpp( file_path=get_package_share_directory("moveit2_tutorials") - + "/config/moveit_cpp.yaml" + + "/config/moveit_cpp_better_paths.yaml" ) + .cartesian_limits("config/cartesian_limits.yaml") .to_moveit_configs() ) # MoveItCpp demo executable diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index edcf8be71c..400b388b3d 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -106,7 +106,11 @@ auto printAndPlan = [&](auto num_iterations, auto execute = false){ { planning_components->setStartStateToCurrentState(); - auto plan_solution = planning_components->plan(); + moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request; + multi_pipeline_plan_request.load(node, {"one", "two", "three"}); + + + auto plan_solution = planning_components->plan(multi_pipeline_plan_request); // Check if PlanningComponents succeeded in finding the plan if (plan_solution) From b108b0e8ad22f2190dfaf8f393c2d196b47b2945 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 15 Jul 2022 18:11:27 +0200 Subject: [PATCH 08/15] Add tutorial description and experiments --- .../config/moveit_cpp_better_paths.yaml | 10 +- .../create_better_paths/path_quality.md | 66 ++- .../src/better_paths_main.cpp | 391 +++++++++--------- 3 files changed, 263 insertions(+), 204 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml b/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml index ddf2340dca..299d633ba3 100644 --- a/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml +++ b/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml @@ -9,7 +9,7 @@ planning_scene_monitor_options: planning_pipelines: #namespace: "moveit_cpp" # optional, default is ~ - pipeline_names: ["pilz_industrial_motion_planner"] #["ompl", "pilz_industrial_motion_planner"] + pipeline_names: ["ompl", "pilz_industrial_motion_planner"] #["ompl", "pilz_industrial_motion_planner"] # PILZ planner config pilz_industrial_motion_planner: @@ -38,14 +38,14 @@ two: plan_request_params: planning_attempts: 1 planning_pipeline: ompl - planner_id: "PRMstarkConfigDefault" + planner_id: "RRTConnectkConfigDefault" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 three: plan_request_params: planning_attempts: 1 - planning_pipeline: pilz_industrial_motion_planner - planner_id: "LIN" + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 \ No newline at end of file + max_acceleration_scaling_factor: 1.0 diff --git a/doc/how_to_guides/create_better_paths/path_quality.md b/doc/how_to_guides/create_better_paths/path_quality.md index dcfa4b605a..6066bf2d42 100644 --- a/doc/how_to_guides/create_better_paths/path_quality.md +++ b/doc/how_to_guides/create_better_paths/path_quality.md @@ -1,2 +1,64 @@ -# How to improve MoveIt2 path quality? -## What is path quality? +How to customize path planning with MoveItCpp? +============================================== + +This guide should help you to get the paths with MoveItCpp you want! + +Learning Objectives +------------------- +- What is path (planning) quality? +- Composition of the MoveItCpp planning pipeline +- Practical tipps to customize the path quality + +What is path (planning) quality? +-------------------------------- +Internals of the MoveItCpp planning pipeline +-------------------------------------------- +Use a distance aware IK solver +------------------------------ + +.. code-block:: yaml + + kinematics_solver: bio_ik/BioIKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 1.0 + # optional bio_ik configuration parameters + center_joints_weight: 1.0 + minimal_displacement_weight: 1.0 + avoid_joint_limits_weight: 1.0 + mode: "gd_c" + +Plan with multiple planning pipelines in parallel and select the most suitable solution +--------------------------------------------------------------------------------------- + +.. code-block:: yaml + + one: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + + two: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + + three: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +Run the demo: + +.. code-block:: bash + + ros2 launch moveit2_tutorials better_paths_demo.launch.py diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index 400b388b3d..d0b3369889 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -68,76 +68,76 @@ int main(int argc, char** argv) planning_components->setStartStateToCurrentState(); - -// Benchmark function -// Compute and print average path length, path similarity -auto analyzeResult = [&](std::vector solutions){ - - // Adapted from https://github.com/ros-planning/moveit2/blob/main/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L872 - // Analyzing the trajectories geometrically - auto average_traj_len = 0.0; - auto average_path_simularity = 0.0; - - // Calculate average path length - for(auto solution : solutions){ - // Only process successful solutions - if(solution.error_code == moveit::core::MoveItErrorCode::SUCCESS){ - // Compute path length - for (std::size_t index = 1; index < solution.trajectory->getWayPointCount(); ++index) - average_traj_len += solution.trajectory->getWayPoint(index - 1).distance(solution.trajectory->getWayPoint(index)); - } + // Benchmark function + // Compute and print average path length, path similarity + auto analyzeResult = [&](std::vector solutions) { + // Adapted from https://github.com/ros-planning/moveit2/blob/main/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L872 + // Analyzing the trajectories geometrically + auto average_traj_len = 0.0; + auto average_path_simularity = 0.0; + + // Calculate average path length + for (auto solution : solutions) + { + // Only process successful solutions + if (solution.error_code == moveit::core::MoveItErrorCode::SUCCESS) + { + // Compute path length + for (std::size_t index = 1; index < solution.trajectory->getWayPointCount(); ++index) + average_traj_len += + solution.trajectory->getWayPoint(index - 1).distance(solution.trajectory->getWayPoint(index)); + } } - if(solutions.size() > 0){ - average_traj_len /= solutions.size(); - } - -// Path similarity + if (solutions.size() > 0) + { + average_traj_len /= solutions.size(); + } - RCLCPP_INFO_STREAM(LOGGER, "Average path length (sum of L1 norms): '" << average_traj_len << " rad'"); - RCLCPP_INFO_STREAM(LOGGER, "Average path similarity: '" << average_path_simularity << "'"); -}; + // Path similarity -// Some helper functions -auto printAndPlan = [&](auto num_iterations, auto execute = false){ - std::vector solutions; - solutions.reserve(num_iterations); - for(auto i = 0; i < num_iterations; i++) - { - planning_components->setStartStateToCurrentState(); + RCLCPP_INFO_STREAM(LOGGER, "Average path length (sum of L1 norms): '" << average_traj_len << " rad'"); + RCLCPP_INFO_STREAM(LOGGER, "Average path similarity: '" << average_path_simularity << "'"); + }; - moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request; - multi_pipeline_plan_request.load(node, {"one", "two", "three"}); + // Some helper functions + auto printAndPlan = [&](auto num_iterations, auto execute = false) { + std::vector solutions; + solutions.reserve(num_iterations); + for (auto i = 0; i < num_iterations; i++) + { + planning_components->setStartStateToCurrentState(); + moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request; + multi_pipeline_plan_request.load(node, { "one", "two", "three" }); - auto plan_solution = planning_components->plan(multi_pipeline_plan_request); + auto plan_solution = planning_components->plan(multi_pipeline_plan_request); - // Check if PlanningComponents succeeded in finding the plan - if (plan_solution) - { - // Visualize the trajectory in Rviz - visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); + // Check if PlanningComponents succeeded in finding the plan + if (plan_solution) + { + // Visualize the trajectory in Rviz + visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); + visual_tools.trigger(); + solutions.push_back(plan_solution); + } + // Start the next plan + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + visual_tools.deleteAllMarkers(); visual_tools.trigger(); - solutions.push_back(plan_solution); - } - // Start the next plan - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); - if(execute) + if (execute) { planning_components->execute(); } - } // Print result analyzeResult(solutions); - }; + }; -auto 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 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_components->getStartState(); robot_goal_state->setJointPositions("panda_joint1", &panda_joint1); robot_goal_state->setJointPositions("panda_joint2", &panda_joint2); @@ -148,152 +148,149 @@ auto setJointGoal = [&](double const panda_joint1, double const panda_joint2, robot_goal_state->setJointPositions("panda_joint3", &panda_joint3); planning_components->setGoal(*robot_goal_state); -}; - -auto setCartGoal = [&](double const x, double const y, double const z, double const qx, double const qy, - double const qz, double const qw){ - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = "panda_link0"; - target_pose.pose.orientation.x = qx; - target_pose.pose.orientation.y = qy; - target_pose.pose.orientation.z = qz; - target_pose.pose.orientation.w = qw; - target_pose.pose.position.x = x; - target_pose.pose.position.y = y; - target_pose.pose.position.z = z; - planning_components->setGoal(target_pose, "panda_link8"); -}; - -auto add_collision_objects = [&](){ - moveit_msgs::msg::CollisionObject collision_object_1; - moveit_msgs::msg::CollisionObject collision_object_2; - moveit_msgs::msg::CollisionObject collision_object_3; - - collision_object_1.header.frame_id = "panda_link0"; - collision_object_1.id = "box1"; - - collision_object_2.header.frame_id = "panda_link0"; - collision_object_2.id = "box2"; - - collision_object_3.header.frame_id = "panda_link0"; - collision_object_3.id = "box3"; - - shape_msgs::msg::SolidPrimitive box_1; - box_1.type = box_1.BOX; - box_1.dimensions = { 0.01, 0.12, 1.0 }; - - shape_msgs::msg::SolidPrimitive box_2; - box_2.type = box_2.BOX; - box_2.dimensions = { 0.01, 1.0, 0.2 }; - - // Add new collision objects - geometry_msgs::msg::Pose box_pose_1; - box_pose_1.position.x = 0.2; - box_pose_1.position.y = 0.2; - box_pose_1.position.z = 0.7; - - collision_object_1.primitives.push_back(box_1); - collision_object_1.primitive_poses.push_back(box_pose_1); - collision_object_1.operation = collision_object_1.ADD; - - geometry_msgs::msg::Pose box_pose_2; - box_pose_2.position.x = 0.2; - box_pose_2.position.y = -0.2; - box_pose_2.position.z = 0.7; - - collision_object_2.primitives.push_back(box_1); - collision_object_2.primitive_poses.push_back(box_pose_2); - collision_object_2.operation = collision_object_2.ADD; - - geometry_msgs::msg::Pose box_pose_3; - box_pose_3.position.x = -0.1; - box_pose_3.position.y = 0.0; - box_pose_3.position.z = 0.85; - - collision_object_3.primitives.push_back(box_2); - collision_object_3.primitive_poses.push_back(box_pose_3); - collision_object_3.operation = collision_object_3.ADD; - - // Add object to planning scene - { // Lock PlanningScene - planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor()); - scene->processCollisionObjectMsg(collision_object_1); - scene->processCollisionObjectMsg(collision_object_2); - scene->processCollisionObjectMsg(collision_object_3); - } // Unlock PlanningScene -}; - -// Experiment 1 - Short motion - Cartesian goal -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Short motion - Cartesian goal ##############"); -visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setCartGoal(0.28,-0.2, 0.5, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); -printAndPlan(3, false); - -// Experiment 1 - Short motion - Joint goal -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Short motion - Joint goal ##############"); -visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setJointGoal(2.8886513579712823, 0.3875018855212286, 2.720914148047324, -2.876892569361917, 2.886120885454169, 0.6383841437770176, 0.4312752220542212); -printAndPlan(3, false); - - - -// Experiment 2 - Same pose - Cartesian goal -// ^^^^^ -RCLCPP_INFO(LOGGER, "################ Same pose - Cartesian goal ##############"); -visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setCartGoal(0.30702, -5.2214e-12, 0.59027, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); -printAndPlan(1, true); -printAndPlan(15, false); - -// Experiment 2 - Same pose - Joint goal -// ^^^^^ -RCLCPP_INFO(LOGGER, "################ Same pose - Joint goal ##############"); -visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setJointGoal(0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785); -printAndPlan(3, false); - -// Experiment 3 - Long motion freespace - Joint goal -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); -visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); -printAndPlan(3, false); - -// Experiment 3 - Long motion freespace - Cartesian goal -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); -visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setCartGoal(-0.42889,0.26552,0.6666, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); -printAndPlan(3, false); - - -// Experiment 4 - Long motion with collisions - Joint goal -// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); -add_collision_objects(); -visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); -visual_tools.trigger(); -setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, 1.1244922991023616, 2.708936891424673); -printAndPlan(5, false); - - // Experiment 4 - Long motion with collisions - Cartesian goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); - visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - setCartGoal(-0.42889,0.26552,0.6666,-0.013696,0.005568,-0.38056,0.92464); - printAndPlan(5, false); - + }; + + auto setCartGoal = [&](double const x, double const y, double const z, double const qx, double const qy, + double const qz, double const qw) { + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = "panda_link0"; + target_pose.pose.orientation.x = qx; + target_pose.pose.orientation.y = qy; + target_pose.pose.orientation.z = qz; + target_pose.pose.orientation.w = qw; + target_pose.pose.position.x = x; + target_pose.pose.position.y = y; + target_pose.pose.position.z = z; + planning_components->setGoal(target_pose, "panda_link8"); + }; + + auto add_collision_objects = [&]() { + moveit_msgs::msg::CollisionObject collision_object_1; + moveit_msgs::msg::CollisionObject collision_object_2; + moveit_msgs::msg::CollisionObject collision_object_3; + + collision_object_1.header.frame_id = "panda_link0"; + collision_object_1.id = "box1"; + + collision_object_2.header.frame_id = "panda_link0"; + collision_object_2.id = "box2"; + + collision_object_3.header.frame_id = "panda_link0"; + collision_object_3.id = "box3"; + + shape_msgs::msg::SolidPrimitive box_1; + box_1.type = box_1.BOX; + box_1.dimensions = { 0.01, 0.12, 1.0 }; + + shape_msgs::msg::SolidPrimitive box_2; + box_2.type = box_2.BOX; + box_2.dimensions = { 0.01, 1.0, 0.2 }; + + // Add new collision objects + geometry_msgs::msg::Pose box_pose_1; + box_pose_1.position.x = 0.2; + box_pose_1.position.y = 0.2; + box_pose_1.position.z = 0.7; + + collision_object_1.primitives.push_back(box_1); + collision_object_1.primitive_poses.push_back(box_pose_1); + collision_object_1.operation = collision_object_1.ADD; + + geometry_msgs::msg::Pose box_pose_2; + box_pose_2.position.x = 0.2; + box_pose_2.position.y = -0.2; + box_pose_2.position.z = 0.7; + + collision_object_2.primitives.push_back(box_1); + collision_object_2.primitive_poses.push_back(box_pose_2); + collision_object_2.operation = collision_object_2.ADD; + + geometry_msgs::msg::Pose box_pose_3; + box_pose_3.position.x = -0.1; + box_pose_3.position.y = 0.0; + box_pose_3.position.z = 0.85; + + collision_object_3.primitives.push_back(box_2); + collision_object_3.primitive_poses.push_back(box_pose_3); + collision_object_3.operation = collision_object_3.ADD; + + // Add object to planning scene + { // Lock PlanningScene + planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor()); + scene->processCollisionObjectMsg(collision_object_1); + scene->processCollisionObjectMsg(collision_object_2); + scene->processCollisionObjectMsg(collision_object_3); + } // Unlock PlanningScene + }; + + // Experiment 1 - Short motion - Cartesian goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + RCLCPP_INFO(LOGGER, "################ Short motion - Cartesian goal ##############"); + visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setCartGoal(0.28, -0.2, 0.5, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); + printAndPlan(3, false); + // // Experiment 1 - Short motion - Joint goal + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + RCLCPP_INFO(LOGGER, "################ Short motion - Joint goal ##############"); + visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setJointGoal(2.8886513579712823, 0.3875018855212286, 2.720914148047324, -2.876892569361917, 2.886120885454169, + 0.6383841437770176, 0.4312752220542212); + printAndPlan(3, false); + + // // Experiment 2 - Same pose - Cartesian goal + // // ^^^^^ + RCLCPP_INFO(LOGGER, "################ Same pose - Cartesian goal ##############"); + visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setCartGoal(0.30702, -5.2214e-12, 0.59027, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); + printAndPlan(1, true); + printAndPlan(2, false); + + //// Experiment 2 - Same pose - Joint goal + //// ^^^^^ + RCLCPP_INFO(LOGGER, "################ Same pose - Joint goal ##############"); + visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setJointGoal(0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785); + printAndPlan(3, false); + // // Experiment 3 - Long motion freespace - Joint goal + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); + visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, + 1.1244922991023616, 2.708936891424673); + printAndPlan(3, false); + + // // Experiment 3 - Long motion freespace - Cartesian goal + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); + visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setCartGoal(-0.42889, 0.26552, 0.6666, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); + printAndPlan(3, false); + + // Experiment 4 - Long motion with collisions - Joint goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + RCLCPP_INFO(LOGGER, "################ Long motion - Joint goal ##############"); + add_collision_objects(); + visual_tools.publishText(text_pose, "JointGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setJointGoal(-2.583184932292678, -0.290335965663780, -1.030661387231159, -2.171781392507914, 2.897232510573447, + 1.1244922991023616, 2.708936891424673); + printAndPlan(3, false); + + // // Experiment 4 - Long motion with collisions - Cartesian goal + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + RCLCPP_INFO(LOGGER, "################ Long motion - Cartesian goal ##############"); + visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + setCartGoal(-0.42889, 0.26552, 0.6666, -0.013696, 0.005568, -0.38056, 0.92464); + printAndPlan(3, false); RCLCPP_INFO(LOGGER, "Shutting down."); rclcpp::shutdown(); From c9003a93ef752f04be0f4c077019c274a97bb8d1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 15 Aug 2022 10:03:04 -0600 Subject: [PATCH 09/15] Debug print + disable execution + clear up launch config --- .../launch/better_paths_demo.launch.py | 1 - .../create_better_paths/src/better_paths_main.cpp | 9 +++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py index dfa09bd67b..23c871ba22 100644 --- a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py +++ b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py @@ -15,7 +15,6 @@ def generate_launch_description(): file_path=get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp_better_paths.yaml" ) - .cartesian_limits("config/cartesian_limits.yaml") .to_moveit_configs() ) # MoveItCpp demo executable diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index d0b3369889..a76aa07af0 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -64,6 +64,12 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Better Paths Tutorial", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); + const auto planning_pipeline_names = moveit_cpp_ptr->getPlanningPipelineNames(); + + for (auto it = planning_pipeline_names.begin(); it!=planning_pipeline_names.end();it++){ + RCLCPP_INFO_STREAM(LOGGER, "Average path similarity: '" << *it << "'"); + } + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); planning_components->setStartStateToCurrentState(); @@ -246,8 +252,7 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "CartesianGoal", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); setCartGoal(0.30702, -5.2214e-12, 0.59027, 0.92396, -0.3825, 1.3251e-12, 3.2002e-12); - printAndPlan(1, true); - printAndPlan(2, false); + printAndPlan(3, false); //// Experiment 2 - Same pose - Joint goal //// ^^^^^ From 31a64b1c1db764dafd06e83c57f4af1723414e50 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 15 Aug 2022 15:32:04 -0600 Subject: [PATCH 10/15] Cleanup config --- .../config/moveit_cpp_better_paths.yaml | 24 ++++--------------- .../launch/better_paths_demo.launch.py | 3 ++- .../src/better_paths_main.cpp | 4 ++-- 3 files changed, 8 insertions(+), 23 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml b/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml index 299d633ba3..e81f860868 100644 --- a/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml +++ b/doc/how_to_guides/create_better_paths/config/moveit_cpp_better_paths.yaml @@ -9,22 +9,7 @@ planning_scene_monitor_options: planning_pipelines: #namespace: "moveit_cpp" # optional, default is ~ - pipeline_names: ["ompl", "pilz_industrial_motion_planner"] #["ompl", "pilz_industrial_motion_planner"] - -# PILZ planner config - pilz_industrial_motion_planner: - name: pilz_industrial_motion_planner - planning_plugin: "pilz_industrial_motion_planner/CommandPlanner" - planner_id: "LIN" - request_adapters: >- - default_planner_request_adapters/AddTimeOptimalParameterization - default_planner_request_adapters/ResolveConstraintFrames - 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 - + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"] one: plan_request_params: @@ -37,15 +22,14 @@ one: two: plan_request_params: planning_attempts: 1 - planning_pipeline: ompl - planner_id: "RRTConnectkConfigDefault" + planning_pipeline: pilz_industrial_motion_planner + planner_id: "LIN" max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 three: plan_request_params: planning_attempts: 1 - planning_pipeline: ompl - planner_id: "RRTConnectkConfigDefault" + planning_pipeline: chomp max_velocity_scaling_factor: 1.0 max_acceleration_scaling_factor: 1.0 diff --git a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py index 23c871ba22..1d80932348 100644 --- a/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py +++ b/doc/how_to_guides/create_better_paths/launch/better_paths_demo.launch.py @@ -10,7 +10,8 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines("ompl", ["ompl", "chomp", "pilz_industrial_motion_planner"]) + .trajectory_execution(file_path="config/moveit_controllers.yaml") .moveit_cpp( file_path=get_package_share_directory("moveit2_tutorials") + "/config/moveit_cpp_better_paths.yaml" diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index a76aa07af0..0afa5c5649 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -64,10 +64,10 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Better Paths Tutorial", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - const auto planning_pipeline_names = moveit_cpp_ptr->getPlanningPipelineNames(); + const auto planning_pipeline_names = moveit_cpp_ptr->getPlanningPipelineNames(PLANNING_GROUP); for (auto it = planning_pipeline_names.begin(); it!=planning_pipeline_names.end();it++){ - RCLCPP_INFO_STREAM(LOGGER, "Average path similarity: '" << *it << "'"); + RCLCPP_INFO_STREAM(LOGGER, "Pipeline names: '" << *it << "'"); } visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); From 2ec278d93b1023cd9a93d31845fd5719ed199d3c Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 17 Aug 2022 13:55:12 -0600 Subject: [PATCH 11/15] Cleanup + RVIZ config --- .../config/better_paths_config.rviz | 457 ++++++++++++++++++ .../src/better_paths_main.cpp | 23 +- 2 files changed, 462 insertions(+), 18 deletions(-) create mode 100644 doc/how_to_guides/create_better_paths/config/better_paths_config.rviz diff --git a/doc/how_to_guides/create_better_paths/config/better_paths_config.rviz b/doc/how_to_guides/create_better_paths/config/better_paths_config.rviz new file mode 100644 index 0000000000..d4272d8b7a --- /dev/null +++ b/doc/how_to_guides/create_better_paths/config/better_paths_config.rviz @@ -0,0 +1,457 @@ +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: 679 + - 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: + Text: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /better_paths_tutorial + 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: 5.1277079582214355 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.2651444971561432 + Y: 0.5764509439468384 + Z: 0.707146406173706 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.27039825916290283 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.033580303192139 + 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/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index 0afa5c5649..b435c14794 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -10,10 +10,7 @@ namespace rvt = rviz_visual_tools; -// All source files that use ROS logging should define a file-specific -// static const rclcpp::Logger named LOGGER, located at the top of the file -// and inside the namespace with the narrowest scope (if there is one) -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_tutorial"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("better_paths_tutorial"); int main(int argc, char** argv) { @@ -21,15 +18,10 @@ int main(int argc, char** argv) rclcpp::NodeOptions node_options; RCLCPP_INFO(LOGGER, "Initialize node"); - // This enables loading undeclared parameters - // best practice would be to declare parameters in the corresponding classes - // and provide descriptions about expected use node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options); - // We spin up a SingleThreadedExecutor for the current state monitor to get information - // about the robot's state. - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); std::thread([&executor]() { executor.spin(); }).detach(); @@ -49,12 +41,7 @@ int main(int argc, char** argv) auto robot_start_state = planning_components->getStartState(); auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); - // Visualization - // ^^^^^^^^^^^^^ - // - // The package MoveItVisualTools provides many capabilities for visualizing objects, robots, - // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script - moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "moveit_cpp_tutorial", + moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "better_paths_tutorial", moveit_cpp_ptr->getPlanningSceneMonitor()); visual_tools.deleteAllMarkers(); visual_tools.loadRemoteControl(); @@ -66,7 +53,8 @@ int main(int argc, char** argv) const auto planning_pipeline_names = moveit_cpp_ptr->getPlanningPipelineNames(PLANNING_GROUP); - for (auto it = planning_pipeline_names.begin(); it!=planning_pipeline_names.end();it++){ + for (auto it = planning_pipeline_names.begin(); it != planning_pipeline_names.end(); it++) + { RCLCPP_INFO_STREAM(LOGGER, "Pipeline names: '" << *it << "'"); } @@ -101,7 +89,6 @@ int main(int argc, char** argv) } // Path similarity - RCLCPP_INFO_STREAM(LOGGER, "Average path length (sum of L1 norms): '" << average_traj_len << " rad'"); RCLCPP_INFO_STREAM(LOGGER, "Average path similarity: '" << average_path_simularity << "'"); }; From a5d38679cfa82275933b5826ac94aeb09d728452 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 31 Aug 2022 13:57:02 +0200 Subject: [PATCH 12/15] Change upstream dependency so branch builds --- moveit2_tutorials.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit2_tutorials.repos b/moveit2_tutorials.repos index 5b2e5a680b..e69d391437 100644 --- a/moveit2_tutorials.repos +++ b/moveit2_tutorials.repos @@ -17,8 +17,8 @@ repositories: version: ros2 moveit2: type: git - url: https://github.com/ros-planning/moveit2 - version: main + url: https://github.com/sjahr/moveit2 # TODO(sjahr) Switch back to upstream once pr-parallel_planning_pipelines got merged + version: pr-parallel_planning_pipelines moveit_resources: type: git url: https://github.com/ros-planning/moveit_resources From bf39c54dc549bd953fe5dfac7d84f18d805498a5 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 29 Sep 2022 21:38:26 +0200 Subject: [PATCH 13/15] Adjust to moveit_cpp changes --- .../create_better_paths/src/better_paths_main.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index b435c14794..409bec777c 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -64,7 +64,7 @@ int main(int argc, char** argv) // Benchmark function // Compute and print average path length, path similarity - auto analyzeResult = [&](std::vector solutions) { + auto analyzeResult = [&](std::vector solutions) { // Adapted from https://github.com/ros-planning/moveit2/blob/main/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L872 // Analyzing the trajectories geometrically auto average_traj_len = 0.0; @@ -74,12 +74,12 @@ int main(int argc, char** argv) for (auto solution : solutions) { // Only process successful solutions - if (solution.error_code == moveit::core::MoveItErrorCode::SUCCESS) + if (solution.error_code_ == moveit::core::MoveItErrorCode::SUCCESS) { // Compute path length - for (std::size_t index = 1; index < solution.trajectory->getWayPointCount(); ++index) + for (std::size_t index = 1; index < solution.trajectory_->getWayPointCount(); ++index) average_traj_len += - solution.trajectory->getWayPoint(index - 1).distance(solution.trajectory->getWayPoint(index)); + solution.trajectory_->getWayPoint(index - 1).distance(solution.trajectory_->getWayPoint(index)); } } @@ -95,14 +95,13 @@ int main(int argc, char** argv) // Some helper functions auto printAndPlan = [&](auto num_iterations, auto execute = false) { - std::vector solutions; + std::vector solutions; solutions.reserve(num_iterations); for (auto i = 0; i < num_iterations; i++) { planning_components->setStartStateToCurrentState(); - moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request; - multi_pipeline_plan_request.load(node, { "one", "two", "three" }); + moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request {node, { "one", "two", "three" }}; auto plan_solution = planning_components->plan(multi_pipeline_plan_request); @@ -110,7 +109,7 @@ int main(int argc, char** argv) if (plan_solution) { // Visualize the trajectory in Rviz - visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr); + visual_tools.publishTrajectoryLine(plan_solution.trajectory_, joint_model_group_ptr); visual_tools.trigger(); solutions.push_back(plan_solution); } From 1b2b00b9e95fafa39afd801ad47f85906b2e32e6 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 8 Nov 2022 11:26:51 +0100 Subject: [PATCH 14/15] Switch back to moveit2 main --- moveit2_tutorials.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit2_tutorials.repos b/moveit2_tutorials.repos index e69d391437..5b2e5a680b 100644 --- a/moveit2_tutorials.repos +++ b/moveit2_tutorials.repos @@ -17,8 +17,8 @@ repositories: version: ros2 moveit2: type: git - url: https://github.com/sjahr/moveit2 # TODO(sjahr) Switch back to upstream once pr-parallel_planning_pipelines got merged - version: pr-parallel_planning_pipelines + url: https://github.com/ros-planning/moveit2 + version: main moveit_resources: type: git url: https://github.com/ros-planning/moveit_resources From 88b6660cb6f11c6cd0a7066fa15c86e75c51ed86 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 22 Nov 2022 12:08:24 +0100 Subject: [PATCH 15/15] Make compile --- .../create_better_paths/src/better_paths_main.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp index 409bec777c..c1b1b5552b 100644 --- a/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp +++ b/doc/how_to_guides/create_better_paths/src/better_paths_main.cpp @@ -50,14 +50,6 @@ int main(int argc, char** argv) text_pose.translation().z() = 1.75; visual_tools.publishText(text_pose, "Better Paths Tutorial", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - - const auto planning_pipeline_names = moveit_cpp_ptr->getPlanningPipelineNames(PLANNING_GROUP); - - for (auto it = planning_pipeline_names.begin(); it != planning_pipeline_names.end(); it++) - { - RCLCPP_INFO_STREAM(LOGGER, "Pipeline names: '" << *it << "'"); - } - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); planning_components->setStartStateToCurrentState(); @@ -101,7 +93,9 @@ int main(int argc, char** argv) { planning_components->setStartStateToCurrentState(); - moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request {node, { "one", "two", "three" }}; + moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request{ + node, { "one", "two", "three" } + }; auto plan_solution = planning_components->plan(multi_pipeline_plan_request);