From b8103d8616d4295e3aa3d2a681faa5f3da8b9856 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 10 Oct 2024 09:12:50 +0200 Subject: [PATCH] Add contrained planning tutorial --- demo/CMakeLists.txt | 10 +- demo/config/constrained_planning.rviz | 314 ++++++++++++++++++ .../constrained_planning_demo.launch.py | 152 +++++++++ demo/src/constrained_planning_demo.cpp | 279 ++++++++++++++++ 4 files changed, 752 insertions(+), 3 deletions(-) create mode 100644 demo/config/constrained_planning.rviz create mode 100644 demo/launch/constrained_planning_demo.launch.py create mode 100644 demo/src/constrained_planning_demo.cpp diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index b1f432c..4efc9b1 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,8 +1,12 @@ add_executable(pipeline_testbench_example src/pipeline_testbench_main.cpp) -target_include_directories(pipeline_testbench_example PRIVATE include) ament_target_dependencies(pipeline_testbench_example - ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) + ${THIS_PACKAGE_INCLUDE_DEPENDS}) -install(TARGETS pipeline_testbench_example DESTINATION lib/${PROJECT_NAME}) +add_executable(constrained_planning_demo src/constrained_planning_demo.cpp) +ament_target_dependencies(constrained_planning_demo + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install(TARGETS pipeline_testbench_example constrained_planning_demo + DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) diff --git a/demo/config/constrained_planning.rviz b/demo/config/constrained_planning.rviz new file mode 100644 index 0000000..5c6f381 --- /dev/null +++ b/demo/config/constrained_planning.rviz @@ -0,0 +1,314 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PlanningScene1 + - /Trajectory1 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 555 + - 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: true + Move Group Namespace: "" + Name: PlanningScene + 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.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 + 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 + - 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: + Cuboid: true + Sphere: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz_visual_tools + Value: true + 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: 2.306405544281006 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6553979516029358 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7403981685638428 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1024 + Hide Left Dock: false + Hide Right Dock: 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: 1860 + X: 2620 + Y: 27 diff --git a/demo/launch/constrained_planning_demo.launch.py b/demo/launch/constrained_planning_demo.launch.py new file mode 100644 index 0000000..1254f96 --- /dev/null +++ b/demo/launch/constrained_planning_demo.launch.py @@ -0,0 +1,152 @@ +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") + .planning_pipelines(pipelines=["ompl"]) + .to_moveit_configs() + ) + + # Set planning pipeline parameters + moveit_config.planning_pipelines["ompl"]["panda_arm"][ + "enforce_constrained_state_space" + ] = True + moveit_config.planning_pipelines["ompl"]["panda_arm"][ + "projection_evaluator" + ] = "joints(panda_joint1,panda_joint2)" + moveit_config.planning_pipelines["ompl"]["panda_arm_hand"][ + "enforce_constrained_state_space" + ] = True + moveit_config.planning_pipelines["ompl"]["panda_arm_hand"][ + "projection_evaluator" + ] = "joints(panda_joint1,panda_joint2)" + + drake_ktopt_planning_pipeline_config = { + "drake_ktopt": { + "planning_plugins": [ + "ktopt_interface/KTOptPlanner", + ], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], + } + } + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict(), drake_ktopt_planning_pipeline_config], + ) + + # Demo OMPL constrained planning node + demo_node = Node( + package="moveit_drake", + executable="constrained_planning_demo", + output="both", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_drake") + + "/config/constrained_planning.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=["--frame-id", "world", "--child-frame-id", "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="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--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"], + ) + + return LaunchDescription( + [ + static_tf, + robot_state_publisher, + rviz_node, + run_move_group_node, + demo_node, + ros2_control_node, + joint_state_broadcaster_spawner, + arm_controller_spawner, + hand_controller_spawner, + ] + ) diff --git a/demo/src/constrained_planning_demo.cpp b/demo/src/constrained_planning_demo.cpp new file mode 100644 index 0000000..da53417 --- /dev/null +++ b/demo/src/constrained_planning_demo.cpp @@ -0,0 +1,279 @@ +#include + +#include +#include +#include +#include +#include + +static const auto LOGGER = rclcpp::get_logger("constrained_planning_demo"); +int main(int argc, char** argv) +{ + using namespace std::chrono_literals; + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("constrained_planning_demo_node", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto spinner = std::thread([&executor]() { executor.spin(); }); + + moveit::planning_interface::MoveGroupInterface move_group_interface(node, "panda_arm"); + auto moveit_visual_tools = + moveit_visual_tools::MoveItVisualTools{ node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, + move_group_interface.getRobotModel() }; + + rclcpp::sleep_for(1s); + + // Create some helpful lambdas + auto current_pose = move_group_interface.getCurrentPose(); + + // Creates a pose at a given positional offset from the current pose + auto get_relative_pose = [current_pose, &moveit_visual_tools](double x, double y, double z) { + auto target_pose = current_pose; + target_pose.pose.position.x += x; + target_pose.pose.position.y += y; + target_pose.pose.position.z += z; + moveit_visual_tools.publishSphere(current_pose.pose, rviz_visual_tools::RED, 0.05); + moveit_visual_tools.publishSphere(target_pose.pose, rviz_visual_tools::GREEN, 0.05); + moveit_visual_tools.trigger(); + return target_pose; + }; + + // Resets the demo by cleaning up any constraints and markers + auto reset_demo = [&move_group_interface, &moveit_visual_tools]() { + move_group_interface.clearPathConstraints(); + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.trigger(); + }; + + reset_demo(); + moveit_visual_tools.loadRemoteControl(); + moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to start with the box constraint example"); + + // Create the first planning problem + auto target_pose = get_relative_pose(0.0, 0.3, -0.3); + + // Let's try the simple box constraints first! + moveit_msgs::msg::PositionConstraint box_constraint; + box_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + box_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive box; + box.type = shape_msgs::msg::SolidPrimitive::BOX; + box.dimensions = { 0.1, 0.4, 0.4 }; + box_constraint.constraint_region.primitives.emplace_back(box); + + geometry_msgs::msg::Pose box_pose; + box_pose.position.x = current_pose.pose.position.x; + box_pose.position.y = 0.15; + box_pose.position.z = 0.45; + box_pose.orientation.w = 1.0; + box_constraint.constraint_region.primitive_poses.emplace_back(box_pose); + box_constraint.weight = 1.0; + + // Visualize the box constraint + Eigen::Vector3d box_point_1(box_pose.position.x - box.dimensions[0] / 2, box_pose.position.y - box.dimensions[1] / 2, + box_pose.position.z - box.dimensions[2] / 2); + Eigen::Vector3d box_point_2(box_pose.position.x + box.dimensions[0] / 2, box_pose.position.y + box.dimensions[1] / 2, + box_pose.position.z + box.dimensions[2] / 2); + moveit_visual_tools.publishCuboid(box_point_1, box_point_2, rviz_visual_tools::TRANSLUCENT_DARK); + moveit_visual_tools.trigger(); + + // We need to wrap the constraints in a generic `Constraints` message. + moveit_msgs::msg::Constraints box_constraints; + box_constraints.position_constraints.emplace_back(box_constraint); + + // Don't forget the path constraints! That's the whole point of this tutorial. + move_group_interface.setPathConstraints(box_constraints); + + // Now we have everything we need to configure and solve a planning problem - plan to the target pose + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + + // And let the planner find a solution. + // The move_group node should automatically visualize the solution in Rviz if a path is found. + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with box constraint %s", success ? "" : "FAILED"); + + // Now wait for the user to press Next before trying the planar constraints. + moveit_visual_tools.prompt( + "Press 'Next' in the RvizVisualToolsGui window to continue to the planar constraint example"); + + // Clear the path constraints and markers for the next example + reset_demo(); + + // In the second problem we plan with the end-effector constrained to a plane. + // We need to create a pose goal that lies in this plane. + // The plane is tilted by 45 degrees, so moving an equal amount in the y and z direction should be ok. + // Any goal or start state should also satisfy the path constraints. + target_pose = get_relative_pose(0.0, 0.3, -0.3); + + // We create a plane perpendicular to the y-axis and tilt it by 45 degrees + + // Solving the problem using equality constraints is a bit more complicated. (Or should I say, hacky?) + // We need to tell the planner explicitly that we want to use equality constraints for the small dimensions. + // This is achieved by setting the name of the constraint to :code:`"use_equality_constraints"`. + // In addition, any dimension of the box below a threshold of :code:`0.001` will be considered an equality constraint. + // However, if we make it too small, the box will be thinner that the tolerance used by OMPL to evaluate constraints + // (:code:`1e-4` by default). MoveIt will use the stricter tolerance (the box width) to check the constraints, and + // many states will appear invalid. That's where the magic number :code:`0.0005` comes from, it is between + // :code:`0.00001` and :code:`0.001`. + moveit_msgs::msg::PositionConstraint plane_constraint; + plane_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + plane_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive plane; + plane.type = shape_msgs::msg::SolidPrimitive::BOX; + plane.dimensions = { 1.0, 0.0005, 1.0 }; + plane_constraint.constraint_region.primitives.emplace_back(plane); + + geometry_msgs::msg::Pose plane_pose; + plane_pose.position.x = current_pose.pose.position.x; + plane_pose.position.y = current_pose.pose.position.y; + plane_pose.position.z = current_pose.pose.position.z; + plane_pose.orientation.x = sin(M_PI_4 / 2); + plane_pose.orientation.y = 0.0; + plane_pose.orientation.z = 0.0; + plane_pose.orientation.w = cos(M_PI_4 / 2); + plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose); + plane_constraint.weight = 1.0; + + // Visualize the constraint + 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.trigger(); + + moveit_msgs::msg::Constraints plane_constraints; + plane_constraints.position_constraints.emplace_back(plane_constraint); + plane_constraints.name = "use_equality_constraints"; + move_group_interface.setPathConstraints(plane_constraints); + + // And again, configure and solve the planning problem + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(10.0); + + // Use KTOpt + move_group_interface.setPlanningPipelineId("drake_ktopt"); + + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with plane constraint %s", success ? "" : "FAILED"); + + moveit_visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui window to continue to the linear constraint example"); + + reset_demo(); + + // We can also plan along a line. We can use the same pose as last time. + target_pose = get_relative_pose(0.0, 0.3, -0.3); + + // Building on the previous constraint, we can make it a line, by also reducing the dimension of the box in the x-direction. + moveit_msgs::msg::PositionConstraint line_constraint; + line_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + line_constraint.link_name = move_group_interface.getEndEffectorLink(); + shape_msgs::msg::SolidPrimitive line; + line.type = shape_msgs::msg::SolidPrimitive::BOX; + line.dimensions = { 0.0005, 0.0005, 1.0 }; + line_constraint.constraint_region.primitives.emplace_back(line); + + geometry_msgs::msg::Pose line_pose; + line_pose.position.x = current_pose.pose.position.x; + line_pose.position.y = current_pose.pose.position.y; + line_pose.position.z = current_pose.pose.position.z; + line_pose.orientation.x = sin(M_PI_4 / 2); + line_pose.orientation.y = 0.0; + line_pose.orientation.z = 0.0; + line_pose.orientation.w = cos(M_PI_4 / 2); + line_constraint.constraint_region.primitive_poses.emplace_back(line_pose); + line_constraint.weight = 1.0; + + // Visualize the constraint + moveit_visual_tools.publishLine(current_pose.pose.position, target_pose.pose.position, + rviz_visual_tools::TRANSLUCENT_DARK); + moveit_visual_tools.trigger(); + + moveit_msgs::msg::Constraints line_constraints; + line_constraints.position_constraints.emplace_back(line_constraint); + line_constraints.name = "use_equality_constraints"; + move_group_interface.setPathConstraints(line_constraints); + move_group_interface.setPoseTarget(target_pose); + + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with line constraint %s", success ? "" : "FAILED"); + + moveit_visual_tools.prompt( + "Press 'Next' in the RvizVisualToolsGui window to continue to the orientation constraint example"); + reset_demo(); + + // Finally, we can place constraints on orientation. + // Set the target pose to be the other side of the robot + target_pose = get_relative_pose(-0.6, 0.1, 0); + + // Create an orientation constraint + moveit_msgs::msg::OrientationConstraint orientation_constraint; + orientation_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame(); + orientation_constraint.link_name = move_group_interface.getEndEffectorLink(); + + orientation_constraint.orientation = current_pose.pose.orientation; + orientation_constraint.absolute_x_axis_tolerance = 0.4; + orientation_constraint.absolute_y_axis_tolerance = 0.4; + orientation_constraint.absolute_z_axis_tolerance = 0.4; + orientation_constraint.weight = 1.0; + + moveit_msgs::msg::Constraints orientation_constraints; + orientation_constraints.orientation_constraints.emplace_back(orientation_constraint); + move_group_interface.setPathConstraints(orientation_constraints); + move_group_interface.setPoseTarget(target_pose); + + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with orientation constraint %s", success ? "" : "FAILED"); + + moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to try mixed_constraints"); + reset_demo(); + + // Finally, we can place constraints on orientation. + // Use the target pose from the previous example + target_pose = get_relative_pose(-0.6, 0.1, 0); + + // Reuse the orientation constraint, and make a new box constraint + box.dimensions = { 1.0, 0.6, 1.0 }; + box_constraint.constraint_region.primitives[0] = box; + + box_pose.position.x = 0; + box_pose.position.y = -0.1; + box_pose.position.z = current_pose.pose.position.z; + box_constraint.constraint_region.primitive_poses[0] = box_pose; + box_constraint.weight = 1.0; + + // Visualize the box constraint + Eigen::Vector3d new_box_point_1(box_pose.position.x - box.dimensions[0] / 2, + box_pose.position.y - box.dimensions[1] / 2, + box_pose.position.z - box.dimensions[2] / 2); + Eigen::Vector3d new_box_point_2(box_pose.position.x + box.dimensions[0] / 2, + box_pose.position.y + box.dimensions[1] / 2, + box_pose.position.z + box.dimensions[2] / 2); + moveit_msgs::msg::Constraints mixed_constraints; + mixed_constraints.position_constraints.emplace_back(box_constraint); + mixed_constraints.orientation_constraints.emplace_back(orientation_constraint); + moveit_visual_tools.publishCuboid(new_box_point_1, new_box_point_2, rviz_visual_tools::TRANSLUCENT_DARK); + moveit_visual_tools.trigger(); + + move_group_interface.setPathConstraints(mixed_constraints); + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setPlanningTime(20.0); + + success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Plan with mixed constraint %s", success ? "" : "FAILED"); + + // Done! + moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to clear the markers"); + moveit_visual_tools.deleteAllMarkers(); + moveit_visual_tools.trigger(); + move_group_interface.clearPathConstraints(); + + rclcpp::shutdown(); + spinner.join(); + return 0; +}