From 6851f8ad7675186065b520014c41faf410318a8e Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Sun, 13 Oct 2024 20:02:33 +0200 Subject: [PATCH] Add constrained planning tutorial (#52) * Add contrained planning tutorial * Make it work * Update limit compution * Re-enable jerk bounds * Remove unused variables * Updates and small bug fix * Fix issues by making kinematics limits finite and adding padding to constraints --------- Co-authored-by: Sebastian Castro --- demo/CMakeLists.txt | 10 +- demo/config/constrained_planning.rviz | 314 ++++++++++++++++++ .../constrained_planning_demo.launch.py | 158 +++++++++ demo/src/constrained_planning_demo.cpp | 278 ++++++++++++++++ .../ktopt_planning_context.hpp | 2 +- include/moveit/drake/conversions.hpp | 24 ++ res/ktopt_moveit_parameters.yaml | 56 +++- src/conversions.cpp | 78 ++++- src/ktopt_planning_context.cpp | 189 ++++------- 9 files changed, 972 insertions(+), 137 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..33ef046 --- /dev/null +++ b/demo/launch/constrained_planning_demo.launch.py @@ -0,0 +1,158 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +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", + ], + } + } + + # Add drake_ktopt_planning_pipeline_config to moveit_config + moveit_config.planning_pipelines["planning_pipelines"] = ["ompl", "drake_ktopt"] + moveit_config.planning_pipelines.update(drake_ktopt_planning_pipeline_config) + + # 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, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, + ], + ) + + # 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..9eeeb32 --- /dev/null +++ b/demo/src/constrained_planning_demo.cpp @@ -0,0 +1,278 @@ +#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); + // Use KTOpt + move_group_interface.setPlanningPipelineId("drake_ktopt"); + + // 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); + + 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; +} diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index 637a63b..47abcf4 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -83,7 +83,7 @@ class KTOptPlanningContext : public planning_interface::PlanningContext void setRobotDescription(std::string robot_description); void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene); void addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant& plant, - Context& plant_context); + Context& plant_context, const double padding); private: const ktopt_interface::Params params_; diff --git a/include/moveit/drake/conversions.hpp b/include/moveit/drake/conversions.hpp index 4d38431..78547a3 100644 --- a/include/moveit/drake/conversions.hpp +++ b/include/moveit/drake/conversions.hpp @@ -73,6 +73,18 @@ namespace moveit::drake const std::string& group_name, const ::drake::multibody::MultibodyPlant& plant); +/** + * @brief Copy position bounds from joint model group to Eigen vectors + * + * @param joint_model_group Joint model group to get position bounds from + * @param plant Drake MultiBody Plant, used to get model information + * @param lower_position_bounds Lower position bounds populated by this function + * @param upper_position_bounds Upper position bounds populated by this function + */ +void getPositionBounds(const moveit::core::JointModelGroup* joint_model_group, + const ::drake::multibody::MultibodyPlant& plant, Eigen::VectorXd& lower_position_bounds, + Eigen::VectorXd& upper_position_bounds); + /** * @brief Copy velocity bounds from joint model group to Eigen vectors * @@ -97,6 +109,18 @@ void getAccelerationBounds(const moveit::core::JointModelGroup* joint_model_grou const ::drake::multibody::MultibodyPlant& plant, Eigen::VectorXd& lower_acceleration_bounds, Eigen::VectorXd& upper_acceleration_bounds); +/** + * @brief Get the Jerk Bounds object + * + * @param joint_model_group Joint model group to get jerk bounds from + * @param plant Drake model plant, used to get model information + * @param lower_jerk_bounds Lower jerk bounds populated by this function + * @param upper_jerk_bounds Upper jerk bounds populated by this function + */ +void getJerkBounds(const moveit::core::JointModelGroup* joint_model_group, + const ::drake::multibody::MultibodyPlant& plant, Eigen::VectorXd& lower_jerk_bounds, + Eigen::VectorXd& upper_jerk_bounds); + /** * @brief Create a Piecewise Polynomial from a moveit trajectory (see * https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_polynomial.html) diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index 972d740..2d0740e 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -1,7 +1,7 @@ ktopt_interface: drake_robot_description: { type: string, - description: "Robot description to be loaded by the internal Drake MultibodyPlant", + description: "Robot description to be loaded by the internal Drake MultibodyPlant.", default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf", } num_iterations: { @@ -28,9 +28,41 @@ ktopt_interface: gt<>: [0.0] } } + min_trajectory_time: { + type: double, + description: "The minimum allowable trajectory time, in seconds.", + default_value: 0.5, + validation: { + gt<>: [0.0] + } + } + max_trajectory_time: { + type: double, + description: "The maximum allowable trajectory time, in seconds.", + default_value: 10.0, + validation: { + gt<>: [0.0] + } + } + duration_cost_weight: { + type: double, + description: "The weight on trajectory duration cost.", + default_value: 1.0, + validation: { + gt_eq<>: [0.0] + } + } + path_length_cost_weight: { + type: double, + description: "The weight on path length cost.", + default_value: 1.0, + validation: { + gt_eq<>: [0.0] + } + } num_collision_check_points: { type: int, - description: "Number of collision checks to perform along the trajectory.", + description: "Number of points on the path on which to perform collision checks.", default_value: 25, validation: { gt_eq<>: [2] @@ -46,24 +78,32 @@ ktopt_interface: } num_position_constraint_points: { type: int, - description: "Number of points on the path that moveit's bounding box constraint needs to be imposed", + description: "Number of points on the path where MoveIt's bounding box constraint needs to be imposed.", default_value: 10, + validation: { + gt_eq<>: [2] + } + } + position_constraint_padding: { + type: double, + description: "Padding distance for position constraints, in meters, to make sure optimizing at sampled points still globally meets constraints.", + default_value: 0.005, validation: { gt_eq<>: [0.0] } } num_orientation_constraint_points: { type: int, - description: "Number of points on the path where moveit's orientation constraint needs to be imposed", + description: "Number of points on the path where MoveIt's orientation constraint needs to be imposed.", default_value: 10, validation: { - gt_eq<>: [0.0] + gt_eq<>: [2] } } - joint_jerk_bound: { + orientation_constraint_padding: { type: double, - description: "Maximum jerk that is allowed for generated trajectory", - default_value: 1.0, + description: "Padding distance for orientation constraints, in radians, to make sure optimizing at sampled points still globally meets constraints.", + default_value: 0.005, validation: { gt_eq<>: [0.0] } diff --git a/src/conversions.cpp b/src/conversions.cpp index 16b6f5f..9000098 100644 --- a/src/conversions.cpp +++ b/src/conversions.cpp @@ -36,6 +36,16 @@ */ #include + +namespace +{ +// These define large default limits for velocity, acceleration, and jerk values. +// If we use floating-point limits, it makes the optimization fail. +constexpr double kMaxVelocity = 100.0; +constexpr double kMaxAcceleration = 100.0; +constexpr double kMaxJerk = 100.0; +} // namespace + namespace moveit::drake { using ::drake::geometry::SceneGraph; @@ -75,12 +85,42 @@ using ::drake::systems::DiagramBuilder; return joint_velocities; } +void getPositionBounds(const moveit::core::JointModelGroup* joint_model_group, const MultibodyPlant& plant, + Eigen::VectorXd& lower_position_bounds, Eigen::VectorXd& upper_position_bounds) +{ + assert(plant.num_positions() >= joint_model_group->getActiveJointModels().size()); + lower_position_bounds.resize(plant.num_positions()); + upper_position_bounds.resize(plant.num_positions()); + for (int i = 0; i < plant.num_positions(); ++i) + { + lower_position_bounds(i) = std::numeric_limits::lowest(); + upper_position_bounds(i) = std::numeric_limits::max(); + } + for (const auto& joint_model : joint_model_group->getActiveJointModels()) + { + const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0]; // Assume single DoF joints + const auto& joint_name = joint_model->getName(); + const auto& joint_index = plant.GetJointByName(joint_name).ordinal(); + + if (bounds.position_bounded_) + { + lower_position_bounds(joint_index) = bounds.min_position_; + upper_position_bounds(joint_index) = bounds.max_position_; + } + } +} + void getVelocityBounds(const moveit::core::JointModelGroup* joint_model_group, const MultibodyPlant& plant, Eigen::VectorXd& lower_velocity_bounds, Eigen::VectorXd& upper_velocity_bounds) { assert(plant.num_velocities() >= joint_model_group->getActiveJointModels().size()); lower_velocity_bounds.resize(plant.num_velocities()); upper_velocity_bounds.resize(plant.num_velocities()); + for (int i = 0; i < plant.num_velocities(); ++i) + { + lower_velocity_bounds(i) = -kMaxVelocity; + upper_velocity_bounds(i) = kMaxVelocity; + } for (const auto& joint_model : joint_model_group->getActiveJointModels()) { const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0]; // Assume single DoF joints @@ -92,11 +132,6 @@ void getVelocityBounds(const moveit::core::JointModelGroup* joint_model_group, c lower_velocity_bounds(joint_index) = bounds.min_velocity_; upper_velocity_bounds(joint_index) = bounds.max_velocity_; } - else - { - lower_velocity_bounds(joint_index) = -std::numeric_limits::max(); - upper_velocity_bounds(joint_index) = std::numeric_limits::max(); - } } } @@ -106,21 +141,46 @@ void getAccelerationBounds(const moveit::core::JointModelGroup* joint_model_grou assert(plant.num_accelerations() >= joint_model_group->getActiveJointModels().size()); lower_acceleration_bounds.resize(plant.num_velocities()); upper_acceleration_bounds.resize(plant.num_velocities()); + for (int i = 0; i < plant.num_velocities(); ++i) + { + lower_acceleration_bounds(i) = -kMaxAcceleration; + upper_acceleration_bounds(i) = kMaxAcceleration; + } for (const auto& joint_model : joint_model_group->getActiveJointModels()) { const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0]; // Assume single DoF joints const auto& joint_name = joint_model->getName(); const auto& joint_index = plant.GetJointByName(joint_name).ordinal(); - if (bounds.velocity_bounded_) + if (bounds.acceleration_bounded_) { lower_acceleration_bounds(joint_index) = bounds.min_acceleration_; upper_acceleration_bounds(joint_index) = bounds.max_acceleration_; } - else + } +} + +void getJerkBounds(const moveit::core::JointModelGroup* joint_model_group, const MultibodyPlant& plant, + Eigen::VectorXd& lower_jerk_bounds, Eigen::VectorXd& upper_jerk_bounds) +{ + assert(plant.num_velocities() >= joint_model_group->getActiveJointModels().size()); + lower_jerk_bounds.resize(plant.num_velocities()); + upper_jerk_bounds.resize(plant.num_velocities()); + for (int i = 0; i < plant.num_velocities(); ++i) + { + lower_jerk_bounds(i) = -kMaxJerk; + upper_jerk_bounds(i) = kMaxJerk; + } + for (const auto& joint_model : joint_model_group->getActiveJointModels()) + { + const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0]; // Assume single DoF joints + const auto& joint_name = joint_model->getName(); + const auto& joint_index = plant.GetJointByName(joint_name).ordinal(); + + if (bounds.jerk_bounded_) { - lower_acceleration_bounds(joint_index) = -std::numeric_limits::max(); - upper_acceleration_bounds(joint_index) = std::numeric_limits::max(); + lower_jerk_bounds(joint_index) = bounds.min_jerk_; + upper_jerk_bounds(joint_index) = bounds.max_jerk_; } } } diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index c5f1fb9..3115f5d 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -50,76 +50,21 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); - const int num_positions = plant.num_positions(); - const int num_velocities = plant.num_velocities(); - // const auto link_ee = params_.link_ee; - // const auto& link_ee_frame = plant.GetFrameByName(link_ee); - - // extract position and velocity bounds - std::vector lower_position_bounds; - std::vector upper_position_bounds; - std::vector lower_velocity_bounds; - std::vector upper_velocity_bounds; - std::vector lower_acceleration_bounds; - std::vector upper_acceleration_bounds; - std::vector lower_jerk_bounds; - std::vector upper_jerk_bounds; - lower_position_bounds.reserve(num_positions); - upper_position_bounds.reserve(num_positions); - lower_velocity_bounds.reserve(num_positions); - upper_velocity_bounds.reserve(num_positions); - lower_acceleration_bounds.reserve(num_positions); - upper_acceleration_bounds.reserve(num_positions); - lower_jerk_bounds.reserve(num_positions); - upper_jerk_bounds.reserve(num_positions); - - const auto& all_joint_models = joint_model_group->getJointModels(); - for (const auto& joint_model : all_joint_models) - { - const auto& joint_name = joint_model->getName(); - const bool is_active = joint_model_group->hasJointModel(joint_name); - - if (is_active) - { - // Set bounds for active joints - const auto& bounds = joint_model->getVariableBounds()[0]; - lower_position_bounds.push_back(bounds.min_position_); - upper_position_bounds.push_back(bounds.max_position_); - lower_velocity_bounds.push_back(-bounds.max_velocity_); - upper_velocity_bounds.push_back(bounds.max_velocity_); - lower_acceleration_bounds.push_back(-bounds.max_acceleration_); - upper_acceleration_bounds.push_back(bounds.max_acceleration_); - lower_jerk_bounds.push_back(-params_.joint_jerk_bound); - upper_jerk_bounds.push_back(params_.joint_jerk_bound); - } - else - { - // Set default values for inactive joints - lower_position_bounds.push_back(-kDefaultJointMaxPosition); - upper_position_bounds.push_back(kDefaultJointMaxPosition); - lower_velocity_bounds.push_back(0.0); - upper_velocity_bounds.push_back(0.0); - lower_acceleration_bounds.push_back(0.0); - upper_acceleration_bounds.push_back(0.0); - lower_jerk_bounds.push_back(0.0); - upper_jerk_bounds.push_back(0.0); - } - } - Eigen::Map lower_position_bounds_eigen(lower_position_bounds.data(), - lower_position_bounds.size()); - Eigen::Map upper_position_bounds_eigen(upper_position_bounds.data(), - upper_position_bounds.size()); - Eigen::Map lower_velocity_bounds_eigen(lower_velocity_bounds.data(), - lower_velocity_bounds.size()); - Eigen::Map upper_velocity_bounds_eigen(upper_velocity_bounds.data(), - upper_velocity_bounds.size()); - Eigen::Map lower_acceleration_bounds_eigen(lower_acceleration_bounds.data(), - lower_acceleration_bounds.size()); - Eigen::Map upper_acceleration_bounds_eigen(upper_acceleration_bounds.data(), - upper_acceleration_bounds.size()); - Eigen::Map lower_jerk_bounds_eigen(lower_jerk_bounds.data(), lower_jerk_bounds.size()); - Eigen::Map upper_jerk_bounds_eigen(upper_jerk_bounds.data(), upper_jerk_bounds.size()); + // Get velocity and acceleration bounds + Eigen::VectorXd lower_position_bounds; + Eigen::VectorXd upper_position_bounds; + Eigen::VectorXd lower_velocity_bounds; + Eigen::VectorXd upper_velocity_bounds; + Eigen::VectorXd lower_acceleration_bounds; + Eigen::VectorXd upper_acceleration_bounds; + Eigen::VectorXd lower_jerk_bounds; + Eigen::VectorXd upper_jerk_bounds; + + moveit::drake::getPositionBounds(joint_model_group, plant, lower_position_bounds, upper_position_bounds); + moveit::drake::getVelocityBounds(joint_model_group, plant, lower_velocity_bounds, upper_velocity_bounds); + moveit::drake::getAccelerationBounds(joint_model_group, plant, lower_acceleration_bounds, upper_acceleration_bounds); + moveit::drake::getJerkBounds(joint_model_group, plant, lower_jerk_bounds, upper_jerk_bounds); // q represents the complete state (joint positions and velocities) VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); @@ -144,18 +89,12 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) auto trajopt = KinematicTrajectoryOptimization(plant.num_positions(), params_.num_control_points); auto& prog = trajopt.get_mutable_prog(); - // Costs - // TODO: These should be parameters - trajopt.AddDurationCost(1.0); - trajopt.AddPathLengthCost(1.0); - + // Add costs + trajopt.AddDurationCost(params_.duration_cost_weight); + trajopt.AddPathLengthCost(params_.path_length_cost_weight); // TODO: Adds quadratic cost // This acts as a secondary cost to push the solution towards joint centers - // prog.AddQuadraticErrorCost( - // MatrixXd::Identity(joints.size(), joints.size()), - // nominal_q_; - // ); - // prog.AddQuadraticErrorCost(); + // prog.AddQuadraticErrorCost(MatrixXd::Identity(plant.num_positions(), plant.num_positions()), nominal_q_, prog.control_points()); // Constraints // Add constraints on start joint configuration and velocity @@ -169,18 +108,23 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const auto& goal_velocity = moveit::drake::getJointVelocityVector(goal_state, getGroupName(), plant); trajopt.AddPathVelocityConstraint(goal_velocity, goal_velocity, 1.0); - // TODO: Add constraints on joint position/velocity/acceleration - trajopt.AddPositionBounds(lower_position_bounds_eigen, upper_position_bounds_eigen); - trajopt.AddVelocityBounds(lower_velocity_bounds_eigen, upper_velocity_bounds_eigen); - trajopt.AddAccelerationBounds(lower_acceleration_bounds_eigen, upper_acceleration_bounds_eigen); - trajopt.AddJerkBounds(lower_jerk_bounds_eigen, upper_jerk_bounds_eigen); + // Add constraints on joint kinematic limits. + trajopt.AddPositionBounds(lower_position_bounds, upper_position_bounds); + trajopt.AddVelocityBounds(lower_velocity_bounds, upper_velocity_bounds); + trajopt.AddAccelerationBounds(lower_acceleration_bounds, upper_acceleration_bounds); + trajopt.AddJerkBounds(lower_jerk_bounds, upper_jerk_bounds); // Add constraints on duration - // TODO: These should be parameters - trajopt.AddDurationConstraint(0.5, 5); + if (params_.min_trajectory_time > params_.max_trajectory_time) + { + RCLCPP_ERROR(getLogger(), "Minimum trajectory time cannot be greater than maximum trajectory time."); + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + return; + } + trajopt.AddDurationConstraint(params_.min_trajectory_time, params_.max_trajectory_time); // process path_constraints - addPathPositionConstraints(trajopt, plant, plant_context); + addPathPositionConstraints(trajopt, plant, plant_context, params_.position_constraint_padding); // solve the program auto result = Solve(prog); @@ -215,29 +159,29 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) moveit::drake::getRobotTrajectory(traj, params_.trajectory_time_step, plant, res.trajectory); // Visualize the trajectory with Meshcat - visualizer_->StartRecording(); - const auto num_pts = static_cast(std::ceil(traj.end_time() / params_.trajectory_time_step) + 1); - for (unsigned int i = 0; i < num_pts; ++i) - { - const auto t_scale = static_cast(i) / static_cast(num_pts - 1); - const auto t = std::min(t_scale, 1.0) * traj.end_time(); - plant.SetPositions(&plant_context, traj.value(t)); - auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_); - visualizer_->ForcedPublish(vis_context); - // Without these sleeps, the visualizer won't give you time to load your - // browser - // TODO: This should not hold up planning time - std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(params_.trajectory_time_step * 10000.0))); - } - visualizer_->StopRecording(); - visualizer_->PublishRecording(); + // visualizer_->StartRecording(); + // const auto num_pts = static_cast(std::ceil(traj.end_time() / params_.trajectory_time_step) + 1); + // for (unsigned int i = 0; i < num_pts; ++i) + // { + // const auto t_scale = static_cast(i) / static_cast(num_pts - 1); + // const auto t = std::min(t_scale, 1.0) * traj.end_time(); + // plant.SetPositions(&plant_context, traj.value(t)); + // auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_); + // visualizer_->ForcedPublish(vis_context); + // // Without these sleeps, the visualizer won't give you time to load your + // // browser + // // TODO: This should not hold up planning time + // std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(params_.trajectory_time_step * 10000.0))); + // } + // visualizer_->StopRecording(); + // visualizer_->PublishRecording(); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return; } void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant& plant, - Context& plant_context) + Context& plant_context, const double padding) { // retrieve the motion planning request const auto& req = getMotionPlanRequest(); @@ -254,14 +198,28 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz // Extract dimensions of the bounding box from // constraint_region.primitives Assuming it is a box // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z] - const auto link_ee = position_constraint.link_name; - if (!plant.HasBodyNamed(link_ee)) + const auto link_ee_name = position_constraint.link_name; + if (!plant.HasBodyNamed(link_ee_name)) + { + RCLCPP_ERROR(getLogger(), + "The link specified in the PositionConstraint message, %s, does not exist in the Drake " + "plant.", + link_ee_name.c_str()); + continue; + } + const auto& link_ee_frame = plant.GetFrameByName(link_ee_name); + + const auto base_frame_name = position_constraint.header.frame_id; + if (!plant.HasBodyNamed(base_frame_name)) { - RCLCPP_ERROR(getLogger(), "The link specified in the PositionConstraint message does not exist in the Drake " - "plant, please check your URDF"); + RCLCPP_ERROR(getLogger(), + "The link specified in the PositionConstraint message, %s, does not exist in the Drake " + "plant.", + base_frame_name.c_str()); continue; } - const auto& link_ee_frame = plant.GetFrameByName(link_ee); + const auto& base_frame = plant.GetFrameByName(base_frame_name); + const auto& primitive = position_constraint.constraint_region.primitives[0]; if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX) { @@ -269,20 +227,19 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz continue; } - double x_dim = primitive.dimensions[0] / 2.0; - double y_dim = primitive.dimensions[1] / 2.0; - double z_dim = primitive.dimensions[2] / 2.0; - // Calculate the lower and upper bounds based on the box dimensions // around the center - Eigen::Vector3d lower_bound = box_center - Eigen::Vector3d(x_dim, y_dim, z_dim); - Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim); + const auto offset_vec = Eigen::Vector3d(std::max(0.0, primitive.dimensions[0] / 2.0 - padding), + std::max(0.0, primitive.dimensions[1] / 2.0 - padding), + std::max(0.0, primitive.dimensions[2] / 2.0 - padding)); + const auto lower_bound = box_center - offset_vec; + const auto upper_bound = box_center + offset_vec; // Add position constraint to each knot point of the trajectory for (int i = 0; i < params_.num_position_constraint_points; ++i) { trajopt.AddPathPositionConstraint( - std::make_shared(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame, + std::make_shared(&plant, base_frame, lower_bound, upper_bound, link_ee_frame, Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context), static_cast(i) / (params_.num_position_constraint_points - 1)); }