From 7b64daab52800c4f4fb907ade79e5e5b3d069c85 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 30 Jul 2024 01:07:31 -0700 Subject: [PATCH] Add demo with visualization Signed-off-by: methylDragon --- CMakeLists.txt | 2 + .../trajectory_cache/CMakeLists.txt | 6 + .../config/trajectory_cache_joint_limits.yaml | 56 + .../launch/trajectory_cache.rviz | 519 ++++++++ .../launch/trajectory_cache_demo.launch.py | 121 ++ .../trajectory_cache_move_group.launch.py | 116 ++ .../src/trajectory_cache_demo.cpp | 1047 +++++++++++++++++ .../src/trajectory_cache_tutorial_bak.cpp | 756 ++++++++++++ package.xml | 1 + 9 files changed, 2624 insertions(+) create mode 100644 doc/how_to_guides/trajectory_cache/CMakeLists.txt create mode 100644 doc/how_to_guides/trajectory_cache/config/trajectory_cache_joint_limits.yaml create mode 100644 doc/how_to_guides/trajectory_cache/launch/trajectory_cache.rviz create mode 100644 doc/how_to_guides/trajectory_cache/launch/trajectory_cache_demo.launch.py create mode 100644 doc/how_to_guides/trajectory_cache/launch/trajectory_cache_move_group.launch.py create mode 100644 doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp create mode 100644 doc/how_to_guides/trajectory_cache/src/trajectory_cache_tutorial_bak.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fae5150f0..c7349a173d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_msgs moveit_ros_planning moveit_ros_planning_interface + moveit_ros_trajectory_cache moveit_servo moveit_task_constructor_core moveit_visual_tools @@ -69,6 +70,7 @@ add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/how_to_guides/chomp_planner) add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) +add_subdirectory(doc/how_to_guides/trajectory_cache) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor) add_subdirectory(doc/tutorials/quickstart_in_rviz) diff --git a/doc/how_to_guides/trajectory_cache/CMakeLists.txt b/doc/how_to_guides/trajectory_cache/CMakeLists.txt new file mode 100644 index 0000000000..e5bbe6745c --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/CMakeLists.txt @@ -0,0 +1,6 @@ +add_executable(trajectory_cache_demo src/trajectory_cache_demo.cpp) +ament_target_dependencies(trajectory_cache_demo ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost moveit_ros_trajectory_cache) + +install(TARGETS trajectory_cache_demo DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) \ No newline at end of file diff --git a/doc/how_to_guides/trajectory_cache/config/trajectory_cache_joint_limits.yaml b/doc/how_to_guides/trajectory_cache/config/trajectory_cache_joint_limits.yaml new file mode 100644 index 0000000000..0a505a2745 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/config/trajectory_cache_joint_limits.yaml @@ -0,0 +1,56 @@ +# These are faster joint limits to allow for faster execution. +joint_limits: + panda_joint1: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_joint2: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_joint3: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_joint4: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_joint5: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_joint6: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_joint7: + has_velocity_limits: true + max_velocity: 50.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_finger_joint1: + has_velocity_limits: true + max_velocity: 25.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false + panda_finger_joint2: + has_velocity_limits: true + max_velocity: 25.0 + has_acceleration_limits: true + max_acceleration: 5.0 + has_jerk_limits: false \ No newline at end of file diff --git a/doc/how_to_guides/trajectory_cache/launch/trajectory_cache.rviz b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache.rviz new file mode 100644 index 0000000000..473746ddd6 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache.rviz @@ -0,0 +1,519 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /PlanningScene1/Scene Robot1 + - /MotionPlanning1/Scene Robot1 + Splitter Ratio: 0.5 + Tree Height: 361 + - 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: 0.10000000149011612 + 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: false + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + 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: true + 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: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/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: panda_arm + Query Goal State: false + 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: 0.10000000149011612 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Axis: true + Line: true + Path: true + Sphere: true + Text: true + home: true + Topic: + Depth: 500 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /trajectory_cache_demo + 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.3678698539733887 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.21443235874176025 + Y: 0.013669016771018505 + Z: 0.4992208182811737 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -0.06960194557905197 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.268585681915283 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 947 + Hide Left Dock: true + Hide Right Dock: true + MotionPlanning: + collapsed: true + 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: true + Width: 1920 + X: 286 + Y: 171 diff --git a/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_demo.launch.py b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_demo.launch.py new file mode 100644 index 0000000000..a0d0023b59 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_demo.launch.py @@ -0,0 +1,121 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + +moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + # OMPL required for example. + .planning_pipelines(default_planning_pipeline="ompl") + .to_moveit_configs() +) + +configurable_parameters = { + # Cache DB + 'cache_db_plugin': { + 'default': 'warehouse_ros_sqlite::DatabaseConnection', + 'description': 'Plugin to use for the trajectory cache database.' + }, + 'cache_db_host': { + 'default': '":memory:"', + 'description': 'Host for the trajectory cache database. Use ":memory:" for an in-memory database.' + }, + 'cache_db_port': { + 'default': '0', + 'description': 'Port for the trajectory cache database.' + }, + + # Reconfigurable (these can be set at runtime and will update) + 'start_tolerance': { # Trajectory cache param + 'default': '0.025', + 'description': 'Reconfigurable cache param. Tolerance for the start state of the trajectory.' + }, + 'goal_tolerance': { # Trajectory cache param + 'default': '0.001', + 'description': 'Reconfigurable cache param. Tolerance for the goal state of the trajectory.' + }, + 'delete_worse_trajectories': { # Trajectory cache param + 'default': 'false', + 'description': 'Reconfigurable cache param. Whether to delete trajectories that are worse than the current trajectory.' + }, + 'planner': { + 'default': 'RRTstar', + 'description': 'Reconfigurable. Planner to use for trajectory planning.' + }, + + # Tutorial + 'num_target_poses': { + 'default': '4', + 'description': 'Number of target poses to generate.' + }, + 'num_cartesian_target_paths_per_target_pose': { + 'default': '2', 'description': 'Number of Cartesian paths to generate for each target pose.' + }, + 'cartesian_path_distance_m': { + 'default': '0.10', + 'description': 'Length of the Cartesian path to set the goal for.' + }, + + # Trajectory Cache + 'exact_match_precision': { + # Purposely set a relatively high value to make pruning obvious. + 'default': '0.0001', + 'description': 'Precision for checking if two trajectories are exactly the same.' + }, + 'cartesian_max_step': { + 'default': '0.001', + 'description': 'Maximum step size for the Cartesian path.' + }, + 'cartesian_jump_threshold': { + 'default': '0.0', + 'description': 'Threshold for the jump distance between points in the Cartesian path.' + }, +} + + +def declare_configurable_parameters(parameters): + return [ + DeclareLaunchArgument( + param_name, + default_value=param['default'], + description=param['description'] + ) for param_name, param in parameters.items() + ] + + +def set_configurable_parameters(parameters): + return { + param_name: LaunchConfiguration(param_name) + for param_name in parameters.keys() + } + + +def generate_launch_description(): + trajectory_cache_demo = Node( + name="trajectory_cache_demo", + package="moveit2_tutorials", + executable="trajectory_cache_demo", + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + set_configurable_parameters(configurable_parameters), + {"warehouse_plugin": LaunchConfiguration("cache_db_plugin")}, + ], + ) + + return LaunchDescription( + declare_configurable_parameters(configurable_parameters) + + [ + trajectory_cache_demo + ] + ) diff --git a/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_move_group.launch.py b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_move_group.launch.py new file mode 100644 index 0000000000..a0051508e3 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_move_group.launch.py @@ -0,0 +1,116 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration, TextSubstitution + +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + +joint_limits_path = os.path.join( + get_package_share_directory("moveit2_tutorials"), + "config", + "trajectory_cache_joint_limits.yaml", +) + +moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + # OMPL required for example. + .planning_pipelines(default_planning_pipeline="ompl") + .joint_limits(file_path=joint_limits_path) + .to_moveit_configs() +) + + +def generate_launch_description(): + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + ) + + # RViz + rviz_config_file = ( + get_package_share_directory( + "moveit2_tutorials") + "/launch/trajectory_cache.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["--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=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + 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( + [ + rviz_node, + static_tf, + robot_state_publisher, + run_move_group_node, + ros2_control_node, + ] + + load_controllers + ) diff --git a/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp new file mode 100644 index 0000000000..f0ba2e29b6 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp @@ -0,0 +1,1047 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Intrinsic Innovation LLC. + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Intrinsic Innovation LLC. nor the names of + * its contributors may be used to endorse or promote products + * derived from this software without specific prior written + * permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: methylDragon */ + +/** [TUTORIAL NOTE] + * + * . . + * . |\-^-/| . + * /| } O.=.O { |\ + * /´ \ \_ ~ _/ / `\ + * /´ | \-/ ~ \-/ | `\ + * | | /\\ //\ | | + * \|\|\/-""-""-\/|/|/ + * ______/ / + * '------' + * _ _ _ ___ + * _ __ ___| |_| |_ _ _| || \ _ _ __ _ __ _ ___ _ _ + * | ' \/ -_) _| ' \ || | || |) | '_/ _` / _` / _ \ ' \ + * |_|_|_\___|\__|_||_\_, |_||___/|_| \__,_\__, \___/_||_| + * |__/ |___/ + * ------------------------------------------------------- + * github.com/methylDragon + * + * NOTE: + * Tutorial notes will be commented like this block! + * + * PRE-REQUISITES + * ^^^^^^^^^^^^^^ + * This tutorial assumes knowledge of the MoveGroupInterface. + * + * INTERACTIVITY + * ^^^^^^^^^^^^^ + * This tutorial also supports "reconfigurable" parameters!: + * + * You can adjust them with: + * `ros2 param set /trajectory_cache_demo ` + * + * Tutorial parameters: + * - planner: + * Defaults to "RRTstar". The OMPL planner used. + * It's better to use a random-sampling, non-optimal planner to see the cache working. + * + * Cache parameters: + * - start_tolerance: + * Defaults to 0.025. Determines the fuzziness of matching on planning start constraints. + * - goal_tolerance: + * Defaults to 0.001. Determines the fuzziness of matching on planning goal constraints. + * - delete_worse_trajectories: + * Defaults to true. Setting this to true will cause cached plans to be pruned. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace // Helpers. +{ + +// Consts and declarations. ======================================================================== + +namespace rvt = rviz_visual_tools; + +using TrajectoryCacheEntryPtr = warehouse_ros::MessageWithMetadata::ConstPtr; + +static const size_t N_MOTION_PLANS_PER_ITERATION = 10; +static const double MIN_EXECUTABLE_FRACTION = 0.95; + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("trajectory_cache_demo"); + +static const std::string HAIR_SPACE = " "; //   for rviz marker text display. +static const std::string PLANNING_GROUP = "panda_arm"; + +static std::random_device RD; +static std::mt19937 RAND_GEN(RD()); + +// Computation helpers. ============================================================================ + +// Generate a random 3D vector of some length. +geometry_msgs::msg::Point random3DVector(double length) +{ + std::uniform_real_distribution<> phiDist(0.0, 2.0 * 3.14159265358979323846); + std::uniform_real_distribution<> cosThetaDist(-1.0, 1.0); + + double phi = phiDist(RAND_GEN); + double cosTheta = cosThetaDist(RAND_GEN); + double sinTheta = std::sqrt(1.0 - cosTheta * cosTheta); + + geometry_msgs::msg::Point out; + out.x = sinTheta * std::sin(phi) * length; + out.y = cosTheta * std::sin(phi) * length; + out.z = std::cos(phi) * length; + + return out; +} + +// Get start of a trajectory as a pose (wrt. base frame). +std::optional +getTrajectoryStartPose(moveit_visual_tools::MoveItVisualTools& visual_tools, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::RobotTrajectory& trajectory_msg) +{ + if (trajectory_msg.joint_trajectory.points.empty()) + { + return std::nullopt; + } + + robot_trajectory::RobotTrajectoryPtr trajectory( + new robot_trajectory::RobotTrajectory(move_group.getRobotModel(), move_group.getName())); + trajectory->setRobotTrajectoryMsg(*move_group.getCurrentState(), trajectory_msg); + + const auto& tip_pose = trajectory->getWayPoint(0).getGlobalLinkTransform(move_group.getEndEffectorLink()); + if (tip_pose.translation().x() != tip_pose.translation().x()) + { + RCLCPP_ERROR(LOGGER, "NAN DETECTED AT TRAJECTORY START POINT"); + return std::nullopt; + } + + return visual_tools.convertPose(tip_pose); +} + +// Get end of a trajectory as a pose (wrt. base frame). +std::optional +getTrajectoryEndPose(moveit_visual_tools::MoveItVisualTools& visual_tools, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::RobotTrajectory& trajectory_msg) +{ + if (trajectory_msg.joint_trajectory.points.empty()) + { + return std::nullopt; + } + + robot_trajectory::RobotTrajectoryPtr trajectory( + new robot_trajectory::RobotTrajectory(move_group.getRobotModel(), move_group.getName())); + trajectory->setRobotTrajectoryMsg(*move_group.getCurrentState(), trajectory_msg); + + const auto& tip_pose = trajectory->getWayPoint(trajectory->getWayPointCount() - 1) + .getGlobalLinkTransform(move_group.getEndEffectorLink()); + if (tip_pose.translation().x() != tip_pose.translation().x()) + { + RCLCPP_ERROR(LOGGER, "NAN DETECTED AT TRAJECTORY END POINT"); + return std::nullopt; + } + + return visual_tools.convertPose(tip_pose); +} + +// Parameters. ===================================================================================== + +struct ReconfigurableParameters +{ + ReconfigurableParameters(std::shared_ptr node) : node_(node) + { + update(); + } + + void update() + { + node_->get_parameter_or("start_tolerance", start_tolerance, 0.025); + node_->get_parameter_or("goal_tolerance", goal_tolerance, 0.001); + node_->get_parameter_or("delete_worse_trajectories", delete_worse_trajectories, false); + node_->get_parameter_or("planner", planner, "RRTstar"); + } + + double start_tolerance; + double goal_tolerance; + bool delete_worse_trajectories = true; + std::string planner; + +private: + std::shared_ptr node_; +}; + +// Visualization helpers. ========================================================================== + +// Visualize all cached trajectories. +void vizCachedTrajectories(moveit_visual_tools::MoveItVisualTools& visual_tools, + const moveit::core::JointModelGroup* joint_model_group, + const std::vector& cached_trajectories, rvt::Colors color, + const std::vector& exclude_trajectories = {}) +{ + for (auto& cached_trajectory : cached_trajectories) + { + if (!cached_trajectory) + { + continue; + } + + bool found_exclude = false; + for (const auto& exclude_trajectory : exclude_trajectories) + { + if (*exclude_trajectory == *cached_trajectory) + { + found_exclude = true; + break; + } + } + if (found_exclude) + { + continue; + } + + visual_tools.publishTrajectoryLine(*cached_trajectory, joint_model_group, color); + } +} + +// Visualize all motion plan target poses. +void vizMotionPlanTargetPoses(moveit_visual_tools::MoveItVisualTools& visual_tools, + const std::vector& target_poses, + const geometry_msgs::msg::PoseStamped& home_pose) +{ + visual_tools.publishAxisLabeled(home_pose.pose, "home", rvt::Scales::XXSMALL); + for (const auto& target_pose : target_poses) + { + visual_tools.publishAxis(target_pose.pose, rvt::Scales::XXSMALL); + } +} + +// Visualize all cartesian path target poses. +void vizCartesianPathTargetPoses(moveit_visual_tools::MoveItVisualTools& visual_tools, + const std::vector& target_poses) +{ + for (const auto& target_pose : target_poses) + { + visual_tools.publishSphere(target_pose.pose, rvt::Colors::ORANGE, rvt::Scales::SMALL); + } +} + +// Visualize all cached motion plan trajectories. +void vizAllCachedMotionPlanTrajectories(moveit_visual_tools::MoveItVisualTools& visual_tools, + const moveit::core::JointModelGroup* joint_model_group, + moveit_ros::trajectory_cache::TrajectoryCache& trajectory_cache, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& motion_plan_request_msg, + rvt::Colors color, + const std::vector& exclude_trajectories = {}) +{ + // We do this in a very hacky way, by specifying a ridiculously high tolerance and removing all constraints. + for (const auto& cached_trajectory : + trajectory_cache.fetchAllMatchingTrajectories(move_group, /*cache_namespace=*/PLANNING_GROUP, + /*plan_request=*/motion_plan_request_msg, /*start_tolerance=*/9999, + /*goal_tolerance=*/9999)) + { + bool found_exclude = false; + for (const auto& exclude_trajectory : exclude_trajectories) + { + if (*exclude_trajectory == *cached_trajectory) + { + found_exclude = true; + break; + } + } + if (found_exclude) + { + continue; + } + + visual_tools.publishTrajectoryLine(*cached_trajectory, joint_model_group, color); + } +} + +// Visualize all cached cartesian plan trajectories. +void vizAllCachedCartesianPlanTrajectories( + moveit_visual_tools::MoveItVisualTools& visual_tools, const moveit::core::JointModelGroup* joint_model_group, + moveit_ros::trajectory_cache::TrajectoryCache& trajectory_cache, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& cartesian_plan_request_msg, rvt::Colors color, + const std::vector& exclude_trajectories = {}) +{ + // We do this in a very hacky way, by specifying a ridiculously high tolerance and removing all constraints. + for (const auto& cached_trajectory : trajectory_cache.fetchAllMatchingCartesianTrajectories( + move_group, /*cache_namespace=*/PLANNING_GROUP, + /*plan_request=*/cartesian_plan_request_msg, /*min_fraction=*/0.0, + /*start_tolerance=*/9999, + /*goal_tolerance=*/9999)) + { + bool found_exclude = false; + for (const auto& exclude_trajectory : exclude_trajectories) + { + if (*exclude_trajectory == *cached_trajectory) + { + found_exclude = true; + break; + } + } + if (found_exclude) + { + continue; + } + + visual_tools.publishTrajectoryLine(*cached_trajectory, joint_model_group, color); + } +} + +// Visualize diffs from trajectory. +// That is, the distance between plan start and goal poses, compared to the trajectory. +void vizTrajectoryDiffs(moveit_visual_tools::MoveItVisualTools& visual_tools, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::RobotTrajectory& trajectory_msg, + const geometry_msgs::msg::Pose& goal_pose, rvt::Colors color, rvt::Scales scale = rvt::MEDIUM) +{ + std::optional trajectory_start_pose = + getTrajectoryStartPose(visual_tools, move_group, trajectory_msg); + if (!trajectory_start_pose.has_value()) + { + return; + } + + std::optional trajectory_end_pose = + getTrajectoryEndPose(visual_tools, move_group, trajectory_msg); + if (!trajectory_end_pose.has_value()) + { + return; + } + + visual_tools.publishLine(move_group.getCurrentPose().pose.position, trajectory_start_pose->position, color, scale); + visual_tools.publishLine(goal_pose.position, trajectory_end_pose->position, color, scale); +} + +// Visualize parameter text. +void vizParamText(moveit_visual_tools::MoveItVisualTools& visual_tools, const Eigen::Isometry3d& pose, + const std::string& cache_db_host, const ReconfigurableParameters& reconfigurable_parameters) +{ + auto param_text = std::stringstream(); + param_text << "[[PARAMETERS]]\n"; + param_text << "cache_db_host:" << HAIR_SPACE << cache_db_host << "\n"; + param_text << "start_tolerance:" << HAIR_SPACE << reconfigurable_parameters.start_tolerance << "\n"; + param_text << "goal_tolerance:" << HAIR_SPACE << reconfigurable_parameters.goal_tolerance << "\n"; + param_text << "delete_worse_trajectories:" << HAIR_SPACE + << (reconfigurable_parameters.delete_worse_trajectories ? "true" : "false") << "\n"; + + visual_tools.publishText(pose, param_text.str(), rvt::WHITE, rvt::XLARGE, false); +} + +// Visualize demo phase text. +void vizDemoPhaseText(moveit_visual_tools::MoveItVisualTools& visual_tools, const Eigen::Isometry3d& pose, + size_t demo_phase) +{ + switch (demo_phase) + { + case 0: { + visual_tools.publishText(pose, "Demo_CacheNoPruning(1/4)", rvt::WHITE, rvt::XXLARGE, false); + return; + } + case 1: { + visual_tools.publishText(pose, "Demo_CacheWithPruning(2/4)", rvt::WHITE, rvt::XXLARGE, false); + return; + } + case 2: { + visual_tools.publishText(pose, "Demo_ExecuteWithCache(3/4)", rvt::WHITE, rvt::XXLARGE, false); + return; + } + case 3: + default: { + visual_tools.publishText(pose, "Demo_HighStartTolerance(4/4)", rvt::WHITE, rvt::XXLARGE, false); + return; + } + } +} + +// Visualize legend text. +void vizLegendText(moveit_visual_tools::MoveItVisualTools& visual_tools, const Eigen::Isometry3d& pose) +{ + static const std::string legend_text = []() { + std::stringstream ss; + ss << "[[LEGEND]]\n"; + ss << "TRANSLUCENT:" << HAIR_SPACE << "planner_plans" << "\n"; + ss << "GREY:" << HAIR_SPACE << "all_cached_plans" << "\n"; + ss << "WHITE:" << HAIR_SPACE << "matchable_cached_plans" << "\n"; + ss << "YELLOW:" << HAIR_SPACE << "matched_cached_plans" << "\n"; + ss << "GREEN:" << HAIR_SPACE << "best_cached_plan" << "\n"; + ss << "RED:" << HAIR_SPACE << "diff_to_trajectory" << "\n"; + return ss.str(); + }(); + + visual_tools.publishText(pose, legend_text, rvt::WHITE, rvt::XLARGE, false); +} + +// Visualize info text. +void vizInfoText(moveit_visual_tools::MoveItVisualTools& visual_tools, const Eigen::Isometry3d& pose, + moveit_ros::trajectory_cache::TrajectoryCache& trajectory_cache, TrajectoryCacheEntryPtr fetched_plan, + double fetch_time) +{ + auto info_text = std::stringstream(); + info_text << "cached-motion-plans:" << HAIR_SPACE << trajectory_cache.countTrajectories(PLANNING_GROUP) << "\n"; + info_text << "cached-cartesian-plans:" << HAIR_SPACE << trajectory_cache.countCartesianTrajectories(PLANNING_GROUP) + << "\n"; + info_text << "fetched-plan-planning-time:" << HAIR_SPACE + << (fetched_plan ? std::to_string(fetched_plan->lookupDouble("planning_time_s")) : "NO_ELIGIBLE_PLAN") + << "\n"; + info_text << "fetched-plan-fetch-time:" << HAIR_SPACE << fetch_time << "\n"; + + visual_tools.publishText(pose, info_text.str(), rvt::WHITE, rvt::XLARGE, false); +} + +} // namespace + +int main(int argc, char** argv) +{ + // =============================================================================================== + // SETUP + // =============================================================================================== + + // ROS. + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("trajectory_cache_demo", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + // Move Group. + moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); + + const moveit::core::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + + // Visualization. + moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "trajectory_cache_demo", + move_group.getRobotModel()); + visual_tools.loadRemoteControl(); + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + + Eigen::Isometry3d title_pose = Eigen::Isometry3d::Identity(); + title_pose.translation().y() = -1.5; + title_pose.translation().z() = 1.2; + + Eigen::Isometry3d legend_pose = Eigen::Isometry3d::Identity(); + legend_pose.translation().y() = -1.5; + + Eigen::Isometry3d param_pose = Eigen::Isometry3d::Identity(); + param_pose.translation().y() = -1.52; + param_pose.translation().z() = 0.5; + + Eigen::Isometry3d info_pose = Eigen::Isometry3d::Identity(); + info_pose.translation().y() = -1.4; + info_pose.translation().z() = 1.0; + + // =============================================================================================== + // CONFIGURE + // =============================================================================================== + + // Get parameters (we set these in the launch file). ============================================= + + // Set up reconfigurable parameters. + ReconfigurableParameters reconfigurable_parameters(move_group_node); + + // Tutorial params. + size_t num_target_poses; + size_t num_cartesian_target_paths_per_target_pose; + double cartesian_path_distance_m; + + move_group_node->get_parameter_or("num_target_poses", num_target_poses, 4); + move_group_node->get_parameter_or("num_cartesian_target_paths_per_target_pose", + num_cartesian_target_paths_per_target_pose, 2); + move_group_node->get_parameter_or("cartesian_path_distance_m", cartesian_path_distance_m, 0.10); + + // Cache DB init. + std::string cache_db_plugin; + std::string cache_db_host; + int64_t cache_db_port; + + move_group_node->get_parameter_or("cache_db_plugin", cache_db_plugin, + "warehouse_ros_sqlite::DatabaseConnection"); + move_group_node->get_parameter_or("cache_db_host", cache_db_host, ":memory:"); + move_group_node->get_parameter_or("cache_db_port", cache_db_port, 0); + + // Cache params. + + /** [TUTORIAL NOTE] + * + * exact_match_precision: + * Tolerance for float precision comparison for what counts as an exact match. + * + * An exact match is when: + * (candidate >= value - (exact_match_precision / 2) + * && candidate <= value + (exact_match_precision / 2)) + * + * Cache entries are matched based off their start and goal states (and other parameters), + * + * And are considered "better" if they higher priority in the sorting order specified by + * `sort_by` than exactly matching . + * + * A cache entry is "exactly matching" if its parm are close enough to another cache entry. + * The tolerance for this depends on the `exact_match_precision` arg passed in the cache + * trajectory's init() method. + * + * cartesian_max_step and cartesian_jump_threshold: + * Used for constraining cartesian planning. + */ + double exact_match_precision; + double cartesian_max_step; + double cartesian_jump_threshold; + + move_group_node->get_parameter_or("exact_match_precision", exact_match_precision, 1e-6); + move_group_node->get_parameter_or("cartesian_max_step", cartesian_max_step, 0.001); + move_group_node->get_parameter_or("cartesian_jump_threshold", cartesian_jump_threshold, 0.0); + + // Generate targets. ============================================================================= + + std::vector target_poses; + target_poses.reserve(num_target_poses); + + std::vector target_cartesian_poses; + target_cartesian_poses.reserve(num_target_poses * num_cartesian_target_paths_per_target_pose); + + move_group.rememberJointValues("home_pose"); + geometry_msgs::msg::PoseStamped home_pose = move_group.getCurrentPose(); + + for (size_t i = 0; i < num_target_poses; ++i) + { + target_poses.push_back(move_group.getRandomPose()); + } + + for (const auto& target_pose : target_poses) + { + for (size_t i = 0; i < num_cartesian_target_paths_per_target_pose; ++i) + { + target_cartesian_poses.push_back(target_pose); + + geometry_msgs::msg::Point random_cartesian_diff = random3DVector(cartesian_path_distance_m); + target_cartesian_poses.back().pose.position.x += random_cartesian_diff.x; + target_cartesian_poses.back().pose.position.y += random_cartesian_diff.y; + target_cartesian_poses.back().pose.position.z += random_cartesian_diff.z; + } + } + + // =============================================================================================== + // DEMO + // =============================================================================================== + + // Init trajectory cache. ======================================================================== + + moveit_ros::trajectory_cache::TrajectoryCache trajectory_cache(move_group_node); + + /** [TUTORIAL NOTE] + * + * The trajectory cache must be initialized, to start a connection with the DB. + * + * NOTE: + * The `warehouse_plugin` parameter must be set. This was set in the launch file. + * + * We can also use this to set the exact match precision. + * As noted above, this is the tolerance for float precision comparison for what counts as an + * exact match. + */ + + moveit_ros::trajectory_cache::TrajectoryCache::Options options; + options.db_path = cache_db_host; + options.db_port = cache_db_port; + options.exact_match_precision = exact_match_precision; + options.num_additional_trajectories_to_preserve_when_deleting_worse = 0; + + if (!trajectory_cache.init(options)) + { + RCLCPP_FATAL(LOGGER, "Could not init cache."); + return 1; + } + + // Interactivity. ================================================================================ + + std::atomic demo_phase = 0; + std::atomic run_execute = false; + + std::thread([&move_group_node, &visual_tools, &demo_phase, &run_execute]() { + // Demo_CacheNoPruning. + move_group_node->set_parameter(rclcpp::Parameter("delete_worse_trajectories", false)); + + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui to start pruning."); + + ++demo_phase; // Demo_CacheWithPruning. + move_group_node->set_parameter(rclcpp::Parameter("delete_worse_trajectories", true)); + + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui to start execution."); + + ++demo_phase; // Demo_CacheAndExecute + run_execute.store(true); + + visual_tools.prompt( + "Press 'next' in the RvizVisualToolsGui to start execution with unreasonably high start tolerance."); + + ++demo_phase; // Demo_CacheAndExecuteWithHighStartTolerance. + move_group_node->set_parameter(rclcpp::Parameter("start_tolerance", 2.0)); + + // We need the controller to also respect the start tolerance. + auto set_param_request = std::make_shared(); + set_param_request->parameters.emplace_back(); + set_param_request->parameters.back().name = "trajectory_execution.allowed_start_tolerance"; + set_param_request->parameters.back().value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; // double. + set_param_request->parameters.back().value.double_value = 2.5; // With some buffer. + move_group_node + ->create_client("/moveit_simple_controller_manager/set_parameters") + ->async_send_request(set_param_request); + }).detach(); + + // Main loop. ==================================================================================== + + moveit_msgs::msg::MotionPlanRequest to_target_motion_plan_request_msg; + moveit_msgs::srv::GetCartesianPath::Request cartesian_path_request_msg; + moveit_msgs::msg::MotionPlanRequest to_home_motion_plan_request_msg; + + /** [TUTORIAL NOTE] + * + * The loop will run a train-and-execute workflow where each iteration will do: + * + * Permuting target pose and target cartesian diff sequentially: + * 1. Motion plan N_MOTION_PLANS_PER_ITERATION times and execute to target pose. + * 2. Cartesian plan and (best-effort) execute to target cartesian diff from pose. + * 3. Motion plan N_MOTION_PLANS_PER_ITERATION times and execute to home pose. + * + * In each case, we will always plan and try to put into the cache. + * + * MULTIPLE PLANS + * ^^^^^^^^^^^^^^ + * We plan multiple times to speed up cache convergence, and also to visualize how cached plans + * are "better" than "worse", less-optimal plans. + * + * BEST PLANS + * ^^^^^^^^^^ + * The executed plan will always be the best plan from the cache that matches the planning request + * constraints, which will be from a plan in the current or prior iterations. + * + * (In this case "best" means smallest execution time.) + * + * CACHE CONVERGENCE + * ^^^^^^^^^^^^^^^^^ + * Over time execution time should improve as the cache collects more, and "better" plans. + */ + + while (rclcpp::ok()) + { + for (size_t target_pose_i = 0; target_pose_i < target_poses.size(); ++target_pose_i) + { + for (size_t target_cartesian_pose_i = 0; target_cartesian_pose_i < num_cartesian_target_paths_per_target_pose; + ++target_cartesian_pose_i) + { + auto target_pose = target_poses[target_pose_i]; + auto target_cartesian_pose = target_cartesian_poses[target_pose_i * num_cartesian_target_paths_per_target_pose + + target_cartesian_pose_i]; + + reconfigurable_parameters.update(); + move_group.setPlannerId(reconfigurable_parameters.planner); + RCLCPP_INFO(LOGGER, "Set planner to: %s", reconfigurable_parameters.planner.c_str()); + + // Plan and execute to target pose. ======================================================== + + // Plan and Cache. + move_group.setPoseTarget(target_pose.pose); + + to_target_motion_plan_request_msg = moveit_msgs::msg::MotionPlanRequest(); + move_group.constructMotionPlanRequest(to_target_motion_plan_request_msg); + + for (size_t i = 0; i < N_MOTION_PLANS_PER_ITERATION && rclcpp::ok(); ++i) + { + moveit::planning_interface::MoveGroupInterface::Plan to_target_motion_plan; + if (move_group.plan(to_target_motion_plan) == moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_INFO(LOGGER, "Got plan to target pose from planner: %s. Planning time: %f", + reconfigurable_parameters.planner.c_str(), to_target_motion_plan.planning_time); + visual_tools.publishTrajectoryLine(to_target_motion_plan.trajectory, joint_model_group, + rvt::Colors::TRANSLUCENT); + + /** [TUTORIAL NOTE] + * + * It's good to use the execution time from the plan instead of the actual time, because + * real world conditions can change (e.g. if the robot loses power), which has no true + * bearing on the optimality of the plan. + */ + double execution_time = + rclcpp::Duration(to_target_motion_plan.trajectory.joint_trajectory.points.back().time_from_start) + .seconds(); + + /** [TUTORIAL NOTE] + * + * For more information about how the cache works or the cache keying logic, see the + * associated guide instead. + * + * PUTTING MOTION PLANS + * ^^^^^^^^^^^^^^^^^^^^ + * Cache entries are only put if they are the best seen so far amongst other exactly + * matching cache entries (i.e. all properties "exactly match"). + * + * CACHE PRUNING + * ^^^^^^^^^^^^^ + * Related to this, EVEN IF A TRAJECTORY IT NOT PUT (i.e., if it is not the best seen so + * far), if `delete_worse_trajectories` is true, then if other exactly matching + * trajectories exist that are "worse" than it, then the cache will delete them. + * + * This allows for the cache memory/storage usage to be reduced, and also reduces query + * time. + */ + trajectory_cache.putTrajectory( // Returns bool. True if put. + move_group, /*cache_namespace=*/PLANNING_GROUP, to_target_motion_plan_request_msg, + to_target_motion_plan.trajectory, + /*execution_time_s=*/execution_time, + /*planning_time_s=*/to_target_motion_plan.planning_time, + /*delete_worse_trajectories=*/reconfigurable_parameters.delete_worse_trajectories); + } + else + { + RCLCPP_WARN(LOGGER, "Could not get plan to target pose from planner: %s", + reconfigurable_parameters.planner.c_str()); + } + } + + // Fetch. + /** [TUTORIAL NOTE] + * + * FETCHING PLANS + * ^^^^^^^^^^^^^^ + * The cache is keyed on the plan request message (and the robot state from the move group). + * + * You can fetch either all matching trajectories, or just the best one, sorted by some + * cache DB column. + * + * MATCH TOLERANCES + * ^^^^^^^^^^^^^^^^ + * It is recommended to have a loose start_tolerance and a strict goal_tolerance. + * + * This is because even if the manipulator is further away in configuration space from the + * first trajectory point, on execution of the trajectory, the manipulator will "snap" to + * the start any way. + * + * Whereas it is much more important that the cached trajectory's end point is close to the + * requested goal. + */ + std::vector matched_to_target_trajectories; + matched_to_target_trajectories = trajectory_cache.fetchAllMatchingTrajectories( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_target_motion_plan_request_msg, + reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, + /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + + TrajectoryCacheEntryPtr best_to_target_trajectory; + + auto best_to_target_fetch_start = move_group_node->now(); + best_to_target_trajectory = trajectory_cache.fetchBestMatchingTrajectory( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_target_motion_plan_request_msg, + reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, + /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + auto best_to_target_fetch_end = move_group_node->now(); + + if (matched_to_target_trajectories.empty() || !best_to_target_trajectory) + { + RCLCPP_FATAL(LOGGER, "No matched trajectories found."); + return 1; + } + + // Visualize. + vizMotionPlanTargetPoses(visual_tools, target_poses, home_pose); + vizCartesianPathTargetPoses(visual_tools, target_cartesian_poses); + + vizCachedTrajectories(visual_tools, joint_model_group, matched_to_target_trajectories, rvt::Colors::YELLOW, + /*exclude=*/{ best_to_target_trajectory }); + vizCachedTrajectories(visual_tools, joint_model_group, { best_to_target_trajectory }, rvt::Colors::LIME_GREEN); + + vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + to_target_motion_plan_request_msg, rvt::Colors::WHITE, + /*exclude=*/matched_to_target_trajectories); + + vizAllCachedCartesianPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + cartesian_path_request_msg, rvt::Colors::DARK_GREY, + /*exclude=*/matched_to_target_trajectories); + + vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + to_home_motion_plan_request_msg, rvt::Colors::DARK_GREY, + /*exclude=*/matched_to_target_trajectories); + + vizTrajectoryDiffs(visual_tools, move_group, *best_to_target_trajectory, target_pose.pose, rvt::Colors::RED, + rvt::Scales::LARGE); + + vizParamText(visual_tools, param_pose, cache_db_host, reconfigurable_parameters); + vizDemoPhaseText(visual_tools, title_pose, demo_phase.load()); + vizLegendText(visual_tools, legend_pose); + vizInfoText(visual_tools, info_pose, trajectory_cache, best_to_target_trajectory, + (best_to_target_fetch_end - best_to_target_fetch_start).seconds()); + + visual_tools.trigger(); + rclcpp::sleep_for(std::chrono::seconds(1)); + + // Execute. + if (run_execute.load()) + { + move_group.execute(*best_to_target_trajectory); + } + + // Cleanup. + visual_tools.deleteAllMarkers(); + reconfigurable_parameters.update(); + + // Interactivity Breakpoint. =============================================================== + + // We don't do the following steps unless we are in execute mode. + if (!run_execute.load()) + { + break; + } + + // Plan and execute to target cartesian pose. ============================================== + + // Plan and Cache. + cartesian_path_request_msg = trajectory_cache.constructGetCartesianPathRequest( + move_group, { target_pose.pose, target_cartesian_pose.pose }, cartesian_max_step, cartesian_jump_threshold); + + // Cartesian plans are one-off, so we don't need to plan multiple times. + moveit_msgs::msg::RobotTrajectory cartesian_trajectory; + + auto cartesian_plan_start = move_group_node->now(); + double fraction = move_group.computeCartesianPath(cartesian_path_request_msg.waypoints, cartesian_max_step, + cartesian_jump_threshold, cartesian_trajectory); + auto cartesian_plan_end = move_group_node->now(); + + if (fraction >= MIN_EXECUTABLE_FRACTION) + { + double execution_time = + rclcpp::Duration(cartesian_trajectory.joint_trajectory.points.back().time_from_start).seconds(); + + trajectory_cache.putCartesianTrajectory( // Returns bool. True if put. + move_group, /*cache_namespace=*/PLANNING_GROUP, cartesian_path_request_msg, cartesian_trajectory, + /*execution_time_s=*/execution_time, + /*planning_time_s=*/(cartesian_plan_end - cartesian_plan_start).seconds(), + /*fraction=*/fraction, + /*delete_worse_trajectories=*/reconfigurable_parameters.delete_worse_trajectories); + } + + // Fetch. + std::vector matched_to_cartesian_trajectories; + matched_to_cartesian_trajectories = trajectory_cache.fetchAllMatchingCartesianTrajectories( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/cartesian_path_request_msg, + /*min_fraction=*/MIN_EXECUTABLE_FRACTION, reconfigurable_parameters.start_tolerance, + reconfigurable_parameters.goal_tolerance, /*metadata_only=*/false, /*sort_by=*/"execution_time_s", + /*ascending=*/true); + + TrajectoryCacheEntryPtr best_to_cartesian_trajectory; + + auto best_to_cartesian_fetch_start = move_group_node->now(); + best_to_cartesian_trajectory = trajectory_cache.fetchBestMatchingCartesianTrajectory( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/cartesian_path_request_msg, + /*min_fraction=*/MIN_EXECUTABLE_FRACTION, reconfigurable_parameters.start_tolerance, + reconfigurable_parameters.goal_tolerance, /*metadata_only=*/false, /*sort_by=*/"execution_time_s", + /*ascending=*/true); + auto best_to_cartesian_fetch_end = move_group_node->now(); + + if (matched_to_cartesian_trajectories.empty() || !best_to_cartesian_trajectory) + { + RCLCPP_WARN(LOGGER, "No matched cartesian trajectories found."); + } + + // Visualize. + vizMotionPlanTargetPoses(visual_tools, target_poses, home_pose); + vizCartesianPathTargetPoses(visual_tools, target_cartesian_poses); + + vizCachedTrajectories(visual_tools, joint_model_group, matched_to_cartesian_trajectories, rvt::Colors::YELLOW, + /*exclude=*/{ best_to_cartesian_trajectory }); + + if (best_to_cartesian_trajectory) + { + vizCachedTrajectories(visual_tools, joint_model_group, { best_to_cartesian_trajectory }, + rvt::Colors::LIME_GREEN); + vizTrajectoryDiffs(visual_tools, move_group, *best_to_cartesian_trajectory, target_cartesian_pose.pose, + rvt::Colors::RED, rvt::Scales::LARGE); + } + + vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + to_target_motion_plan_request_msg, rvt::Colors::DARK_GREY, + /*exclude=*/matched_to_cartesian_trajectories); + + vizAllCachedCartesianPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + cartesian_path_request_msg, rvt::Colors::WHITE, + /*exclude=*/matched_to_cartesian_trajectories); + + vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + to_home_motion_plan_request_msg, rvt::Colors::DARK_GREY, + /*exclude=*/matched_to_cartesian_trajectories); + + vizParamText(visual_tools, param_pose, cache_db_host, reconfigurable_parameters); + vizDemoPhaseText(visual_tools, title_pose, demo_phase.load()); + vizLegendText(visual_tools, legend_pose); + vizInfoText(visual_tools, info_pose, trajectory_cache, best_to_cartesian_trajectory, + (best_to_cartesian_fetch_end - best_to_cartesian_fetch_start).seconds()); + + visual_tools.trigger(); + rclcpp::sleep_for(std::chrono::seconds(1)); + + // Execute. + if (run_execute.load() && fraction >= MIN_EXECUTABLE_FRACTION) + { + move_group.execute(*best_to_cartesian_trajectory); + } + + // Cleanup. + visual_tools.deleteAllMarkers(); + reconfigurable_parameters.update(); + + // Plan and execute to home pose. ========================================================== + + // Plan and Cache. + move_group.setNamedTarget("home_pose"); // Joint state target this time. + + to_home_motion_plan_request_msg = moveit_msgs::msg::MotionPlanRequest(); + move_group.constructMotionPlanRequest(to_home_motion_plan_request_msg); + + for (size_t i = 0; i < N_MOTION_PLANS_PER_ITERATION && rclcpp::ok(); ++i) + { + moveit::planning_interface::MoveGroupInterface::Plan to_home_motion_plan; + if (move_group.plan(to_home_motion_plan) == moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_INFO(LOGGER, "Got plan to home pose from planner: %s. Planning time: %f", + reconfigurable_parameters.planner.c_str(), to_home_motion_plan.planning_time); + visual_tools.publishTrajectoryLine(to_home_motion_plan.trajectory, joint_model_group, + rvt::Colors::TRANSLUCENT_LIGHT); + + double execution_time = + rclcpp::Duration(to_home_motion_plan.trajectory.joint_trajectory.points.back().time_from_start) + .seconds(); + + trajectory_cache.putTrajectory( // Returns bool. True if put. + move_group, /*cache_namespace=*/PLANNING_GROUP, to_home_motion_plan_request_msg, + to_home_motion_plan.trajectory, + /*execution_time_s=*/execution_time, + /*planning_time_s=*/to_home_motion_plan.planning_time, + /*delete_worse_trajectories=*/reconfigurable_parameters.delete_worse_trajectories); + } + else + { + RCLCPP_WARN(LOGGER, "Could not get plan to target pose from planner: %s", + reconfigurable_parameters.planner.c_str()); + } + } + + // Fetch. + std::vector matched_to_home_trajectories; + matched_to_home_trajectories = trajectory_cache.fetchAllMatchingTrajectories( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_home_motion_plan_request_msg, + reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, + /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + + TrajectoryCacheEntryPtr best_to_home_trajectory; + + auto best_to_home_fetch_start = move_group_node->now(); + best_to_home_trajectory = trajectory_cache.fetchBestMatchingTrajectory( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_home_motion_plan_request_msg, + reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, + /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + auto best_to_home_fetch_end = move_group_node->now(); + + if (matched_to_home_trajectories.empty() || !best_to_home_trajectory) + { + RCLCPP_FATAL(LOGGER, "No matched trajectories found."); + return 1; + } + + // Visualize. + vizMotionPlanTargetPoses(visual_tools, target_poses, home_pose); + vizCartesianPathTargetPoses(visual_tools, target_cartesian_poses); + + vizCachedTrajectories(visual_tools, joint_model_group, matched_to_home_trajectories, rvt::Colors::YELLOW, + /*exclude=*/{ best_to_home_trajectory }); + vizCachedTrajectories(visual_tools, joint_model_group, { best_to_home_trajectory }, rvt::Colors::LIME_GREEN); + + vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + to_target_motion_plan_request_msg, rvt::Colors::DARK_GREY, + /*exclude=*/matched_to_home_trajectories); + + vizAllCachedCartesianPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + cartesian_path_request_msg, rvt::Colors::DARK_GREY, + /*exclude=*/matched_to_home_trajectories); + + vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, + to_home_motion_plan_request_msg, rvt::Colors::WHITE, + /*exclude=*/matched_to_home_trajectories); + + vizTrajectoryDiffs(visual_tools, move_group, *best_to_home_trajectory, home_pose.pose, rvt::Colors::RED, + rvt::Scales::LARGE); + + vizParamText(visual_tools, param_pose, cache_db_host, reconfigurable_parameters); + vizDemoPhaseText(visual_tools, title_pose, demo_phase.load()); + vizLegendText(visual_tools, legend_pose); + vizInfoText(visual_tools, info_pose, trajectory_cache, best_to_home_trajectory, + (best_to_home_fetch_end - best_to_home_fetch_start).seconds()); + + visual_tools.trigger(); + rclcpp::sleep_for(std::chrono::seconds(1)); + + // Execute. + if (run_execute.load()) + { + move_group.execute(*best_to_home_trajectory); + } + + // Cleanup. + visual_tools.deleteAllMarkers(); + reconfigurable_parameters.update(); + } + } + } + + rclcpp::shutdown(); + return 0; +} diff --git a/doc/how_to_guides/trajectory_cache/src/trajectory_cache_tutorial_bak.cpp b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_tutorial_bak.cpp new file mode 100644 index 0000000000..725904cb0e --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_tutorial_bak.cpp @@ -0,0 +1,756 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Intrinsic Innovation LLC. + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Intrinsic Innovation LLC. nor the names of + * its contributors may be used to endorse or promote products + * derived from this software without specific prior written + * permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: methylDragon */ + +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +// This tutorial assumes knowledge of the MoveGroupInterface. + +// Helper functions and consts. +namespace +{ + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("trajectory_cache_demo"); + +static const std::string HAIR_SPACE = " "; //   for rviz marker text display. +static const std::string PLANNING_GROUP = "panda_arm"; + +// Append cached motion trajectory metadata to the RViz text marker. +void appendMotionPlanCacheText( + std::stringstream& stream, + warehouse_ros::MessageWithMetadata::ConstPtr trajectory) +{ + if (trajectory == nullptr) + { + stream << "cache-hit:" << HAIR_SPACE << "false" << "\n"; + } + else + { + stream << "cache-hit:" << HAIR_SPACE << "true" << "\n"; + stream << "best-cached-motion-plan-planning-time" << HAIR_SPACE << trajectory->lookupDouble("planning_time_s") + << "\n"; + stream << "best-cached-motion-plan-execution-time" << HAIR_SPACE << trajectory->lookupDouble("execution_time_s") + << "\n"; + } +}; + +// Append cached cartesian trajectory metadata to the RViz text marker. +void appendCartesianPlanCacheText( + std::stringstream& stream, + warehouse_ros::MessageWithMetadata::ConstPtr trajectory) +{ + if (trajectory == nullptr) + { + stream << "cache-hit:" << HAIR_SPACE << "false" << "\n"; + } + else + { + stream << "cache-hit:" << HAIR_SPACE << "true" << "\n"; + stream << "best-cached-cartesian-plan-fraction" << HAIR_SPACE << trajectory->lookupDouble("fraction") << "\n"; + stream << "best-cached-cartesian-plan-planning-time" << HAIR_SPACE << trajectory->lookupDouble("planning_time_s") + << "\n"; + stream << "cached-cartesian-plan-execution-time" << HAIR_SPACE << trajectory->lookupDouble("execution_time_s") + << "\n"; + } +}; + +} // namespace + +int main(int argc, char** argv) +{ + // SETUP ========================================================================================= + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("trajectory_cache_tutorial", node_options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + // Get parameters (we already set these in the launch file). + std::string cache_db_plugin = move_group_node->get_parameter("cache_db_plugin").as_string(); + std::string cache_db_host = move_group_node->get_parameter("cache_db_host").as_string(); + int64_t cache_db_port = move_group_node->get_parameter("cache_db_port").as_int(); + + // Move Group. + moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP); + + const moveit::core::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + + // Visualization. + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "panda_link0", "trajectory_cache_tutorial", + move_group.getRobotModel()); + + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + + Eigen::Isometry3d title_pose = Eigen::Isometry3d::Identity(); + title_pose.translation().z() = 1.2; + + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.0; + + Eigen::Isometry3d description_pose = Eigen::Isometry3d::Identity(); + description_pose.translation().z() = -0.10; + + // BEGIN DEMO ==================================================================================== + RCLCPP_INFO(LOGGER, "cache_db_plugin: %s", cache_db_plugin.c_str()); + RCLCPP_INFO(LOGGER, "cache_db_host: %s", cache_db_host.c_str()); + RCLCPP_INFO(LOGGER, "cache_db_port: %ld", cache_db_port); + + RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str()); + RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str()); + RCLCPP_INFO(LOGGER, "Available Planning Groups:"); + std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), + std::ostream_iterator(std::cout, ", ")); + + std::stringstream viz_text; + viz_text << "warehouse_plugin:" << HAIR_SPACE << cache_db_plugin << "\n" + << "cache_db_plugin:" << HAIR_SPACE << cache_db_plugin << "\n" + << "cache_db_host:" << HAIR_SPACE << cache_db_host << "\n" + << "cache_db_port:" << HAIR_SPACE << cache_db_port << "\n"; + + visual_tools.publishText(title_pose, "TrajectoryCache_Demo", rvt::WHITE, rvt::XXLARGE, false); + visual_tools.publishText(text_pose, viz_text.str(), rvt::WHITE, rvt::XLARGE, false); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + visual_tools.deleteAllMarkers(); + + // Construct the cache and objects needed to request or fetch cached trajectories ================ + + moveit_ros::trajectory_cache::TrajectoryCache trajectory_cache(move_group_node); + + // The trajectory cache must be initialized, to start a connection with the DB. + // + // NOTE: The `warehouse_plugin` parameter must be set. This was set in the launch file. + // + // We can also use this to set the exact match precision. + // That is, the tolerance for float precision comparison for what counts as an exact match. + if (!trajectory_cache.init(cache_db_host, cache_db_port, /*exact_match_precision=*/1e-6)) + { + RCLCPP_FATAL(LOGGER, "Could not init cache."); + return 1; + } + + // The trajectory cache returns warehouse_ros message pointers. + warehouse_ros::MessageWithMetadata::ConstPtr fetched_trajectory = nullptr; + + // And uses plan requests to key fuzzily into the cache. + moveit_msgs::msg::MotionPlanRequest motion_plan_request; + moveit_msgs::srv::GetCartesianPath::Request cartesian_plan_request; + + // Fetch motion plan from cache ================================================================== + + // Set target as if we were going to plan. + geometry_msgs::msg::Pose target_pose1; + target_pose1.orientation.w = 1.0; + target_pose1.position.x = 0.28; + target_pose1.position.y = -0.2; + target_pose1.position.z = 0.5; + move_group.setPoseTarget(target_pose1); + + // But instead of planning, we get the motion plan request to look for in the cache. + motion_plan_request = moveit_msgs::msg::MotionPlanRequest(); + move_group.constructMotionPlanRequest(motion_plan_request); + + // Trajectories are fetched from the cache, keyed fuzzily on planning requests. + // + // The fuzzy matching tolerance on the plan's start and end conditions are set separately. + // And is constrained to the namespace you give it. + // + // The matching includes fields such as: + // - Workspace name + // - Manipulator joint state + // - Goal pose, joint, and orientation constraints + // + // The robot pose will always be stored and looked-up as joint states. + // + // Recommendations: + // - It is normally ideal to set a higher start tolerance than goal tolerance. + // - A good choice of cache namespace is the planning group name. + fetched_trajectory = + trajectory_cache.fetchBestMatchingTrajectory(move_group, /*cache_namespace=*/PLANNING_GROUP, + /*plan_request=*/motion_plan_request, + /*start_tolerance=*/0.025, /*goal_tolerance=*/0.001); + + // If we did not get a cache hit, the returned trajectory will be nullptr. + // + // If cache_db_host is :memory:, the cache will be empty, so we expect nullptr here. + // Otherwise, we probably had a pre-existing cache DB we pointed to. + appendMotionPlanCacheText(viz_text, fetched_trajectory); + + visual_tools.publishAxisLabeled(target_pose1, "pose1"); + if (fetched_trajectory) + { + visual_tools.publishTrajectoryLine(*fetched_trajectory, joint_model_group, rvt::Colors::LIME_GREEN); + } + + viz_text = std::stringstream(); + viz_text << "cache_db_host:" << HAIR_SPACE << cache_db_host << "\n"; + viz_text << "cached-plans:" << HAIR_SPACE << trajectory_cache.countTrajectories(PLANNING_GROUP) << "\n"; + appendMotionPlanCacheText(viz_text, fetched_trajectory); + + visual_tools.publishText(title_pose, "Demo_FetchMotionPlan", rvt::WHITE, rvt::XXLARGE, false); + visual_tools.publishText(text_pose, viz_text.str(), rvt::WHITE, rvt::XLARGE, false); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + visual_tools.deleteAllMarkers(); + + // Plan and cache plans without pruning ========================================================== + + // We will plan using all manner of planners, though all single-query, random sampling-based. + // This is to generate a range of trajectories so it becomes obvious which are better or worse. + // + // We try to put the better planners last so we have a higher chance of generating worse plans that will then get put + // into the cache. + std::vector planners = { + "RRTstar",// "BFMT", "RRTConnect", "RRT", // , // "PDST", "FMT", "BFMT", "RRT", "LBKPIECE", "EST", "KPIECE", "BKPIECE", "STRIDE", + }; + std::vector planning_times; + std::vector execution_times; + std::vector trajectories; + + for (const auto& planner : planners) + { + move_group.setPlannerId(planner); + move_group.setPoseTarget(target_pose1); + motion_plan_request = moveit_msgs::msg::MotionPlanRequest(); + move_group.constructMotionPlanRequest(motion_plan_request); + + // Plan several times to populate the cache. + for (int i = 0; i < 20; ++i) + { + moveit::planning_interface::MoveGroupInterface::Plan plan; + if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS) + { + RCLCPP_INFO(LOGGER, "Got plan from: %s. Planning time: %f", planner.c_str(), plan.planning_time); + + // It's good to use the execution time from the plan instead of the actual time, because real world conditions + // can change. + double execution_time = + rclcpp::Duration(plan.trajectory.joint_trajectory.points.back().time_from_start).seconds(); + + execution_times.push_back(execution_time); + planning_times.push_back(plan.planning_time); + trajectories.push_back(plan.trajectory); + + // We purposely avoid pruning for demonstration purposes. + // + // NOTE: + // Even though we have 10 planners, planning 3 times each, we expect <= 10 * 3, because the cache + // only adds a plan if it is the best seen yet! + trajectory_cache.putTrajectory(move_group, /*cache_namespace=*/PLANNING_GROUP, motion_plan_request, + plan.trajectory, + /*execution_time_s=*/execution_time, + /*planning_time_s=*/plan.planning_time, /*delete_worse_trajectories=*/false); + } + else + { + RCLCPP_WARN(LOGGER, "Could not get plan from planner: %s", planner.c_str()); + } + } + } + + if (trajectories.empty()) + { + RCLCPP_FATAL(LOGGER, "Could not get any plans."); + return 1; + } + + viz_text = std::stringstream(); + viz_text << "cached-plans:" << HAIR_SPACE << trajectory_cache.countTrajectories(PLANNING_GROUP) << "\n"; + + if (!visual_tools.getSharedRobotState()) + { + RCLCPP_FATAL(LOGGER, "Could not get shared robot state."); + return 1; + } + + for (auto& trajectory : trajectories) + { + visual_tools.publishTrajectoryLine(trajectory, joint_model_group, rviz_visual_tools::Colors::TRANSLUCENT_LIGHT); + } + + visual_tools.publishAxisLabeled(target_pose1, "pose1"); + visual_tools.publishText(title_pose, "Demo_PlanAndCacheMotionPlans", rvt::WHITE, rvt::XXLARGE, false); + visual_tools.publishText(text_pose, viz_text.str(), rvt::WHITE, rvt::XLARGE, false); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + visual_tools.deleteAllMarkers(); + + // Fetch plans =================================================================================== + + RCLCPP_INFO(LOGGER, "cached-plans: %d", trajectory_cache.countTrajectories(PLANNING_GROUP)); + + move_group.setPoseTarget(target_pose1); + motion_plan_request = moveit_msgs::msg::MotionPlanRequest(); + move_group.constructMotionPlanRequest(motion_plan_request); + + // Now with the cache populated, we should get a cache hit. + rclcpp::Time cache_fetch_start = move_group_node->now(); + auto best_fetched_trajectory = trajectory_cache.fetchBestMatchingTrajectory( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/motion_plan_request, /*start_tolerance=*/0.025, + /*goal_tolerance=*/0.001, /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + rclcpp::Time cache_fetch_end = move_group_node->now(); + + if (!best_fetched_trajectory) + { + RCLCPP_FATAL(LOGGER, "Could not get best cached trajectory."); + return 1; + } + + // Since we didn't prune, you can also get a list of cache hits, sorted by some column! + auto fetched_trajectories = trajectory_cache.fetchAllMatchingTrajectories( + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/motion_plan_request, /*start_tolerance=*/0.025, + /*goal_tolerance=*/0.001, /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + if (fetched_trajectories.size() < 3) + { + RCLCPP_FATAL(LOGGER, "Could not get enough cached trajectories. Got: %ld", fetched_trajectories.size()); + return 1; + } + else + { + RCLCPP_INFO(LOGGER, "Got %ld cached trajectories.", fetched_trajectories.size()); + } + + // You should expect the first of the fetched trajectories list to be equal to the best fetched trajectory. + if (*(fetched_trajectories.at(0)) != *best_fetched_trajectory) + { + RCLCPP_FATAL(LOGGER, "Best fetched trajectories not equal."); + return 1; + } + + viz_text = std::stringstream(); + viz_text << "cached-plans:" << HAIR_SPACE << trajectory_cache.countTrajectories(PLANNING_GROUP) << "\n"; + appendMotionPlanCacheText(viz_text, best_fetched_trajectory); + viz_text << "cache-fetch-time:" << HAIR_SPACE << (cache_fetch_end - cache_fetch_start).seconds() << "\n"; + + for (auto& trajectory : trajectories) + { + visual_tools.publishTrajectoryLine(trajectory, joint_model_group, rviz_visual_tools::Colors::TRANSLUCENT_LIGHT); + } + + visual_tools.publishTrajectoryLine(*fetched_trajectories.front(), joint_model_group, + rviz_visual_tools::Colors::LIME_GREEN); + // visual_tools.publishTrajectoryLine(*fetched_trajectories.at(fetched_trajectories.size() / 2), joint_model_group, + // rviz_visual_tools::Colors::YELLOW); + // visual_tools.publishTrajectoryLine(*fetched_trajectories.back(), joint_model_group, rviz_visual_tools::Colors::RED); + + visual_tools.publishAxisLabeled(target_pose1, "pose1"); + visual_tools.publishText(title_pose, "Demo_PlanAndCacheMotionPlans", rvt::WHITE, rvt::XXLARGE, false); + visual_tools.publishText(text_pose, viz_text.str(), rvt::WHITE, rvt::XLARGE, false); + visual_tools.publishText(description_pose, "NOTE:Green_is_best_plan_fetched_from_cache", rvt::WHITE, rvt::XLARGE, + false); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + visual_tools.deleteAllMarkers(); + + // // Now, we call the planner to compute the plan and visualize it. + // // Note that we are just planning, not asking move_group + // // to actually move the robot. + // moveit::planning_interface::MoveGroupInterface::Plan my_plan; + + // bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + + // RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); + + // // Visualizing plans + // // ^^^^^^^^^^^^^^^^^ + // // We can also visualize the plan as a line with markers in RViz. + // RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); + // visual_tools.publishAxisLabeled(target_pose1, "pose1"); + // visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); + + // // Moving to a pose goal + // // ^^^^^^^^^^^^^^^^^^^^^ + // // + // // Moving to a pose goal is similar to the step above + // // except we now use the ``move()`` function. Note that + // // the pose goal we had set earlier is still active + // // and so the robot will try to move to that goal. We will + // // not use that function in this tutorial since it is + // // a blocking function and requires a controller to be active + // // and report success on execution of a trajectory. + + // /* Uncomment below line when working with a real robot */ + // /* move_group.move(); */ + + // // Planning to a joint-space goal + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // + // // Let's set a joint space goal and move towards it. This will replace the + // // pose target we set above. + // // + // // To start, we'll create an pointer that references the current robot's state. + // // RobotState is the object that contains all the current position/velocity/acceleration data. + // moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10); + // // + // // Next get the current set of joint values for the group. + // std::vector joint_group_positions; + // current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); + + // // Now, let's modify one of the joints, plan to the new joint space goal, and visualize the plan. + // joint_group_positions[0] = -1.0; // radians + // bool within_bounds = move_group.setJointValueTarget(joint_group_positions); + // if (!within_bounds) + // { + // RCLCPP_WARN(LOGGER, "Target joint position(s) were outside of limits, but we will plan and clamp to the limits + // "); + // } + + // // We lower the allowed maximum velocity and acceleration to 5% of their maximum. + // // The default values are 10% (0.1). + // // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config + // // or set explicit factors in your code if you need your robot to move faster. + // move_group.setMaxVelocityScalingFactor(0.05); + // move_group.setMaxAccelerationScalingFactor(0.05); + + // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + // RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); + + // // Visualize the plan in RViz: + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // // Planning with Path Constraints + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // + // // Path constraints can easily be specified for a link on the robot. + // // Let's specify a path constraint and a pose goal for our group. + // // First define the path constraint. + // moveit_msgs::msg::OrientationConstraint ocm; + // ocm.link_name = "panda_link7"; + // ocm.header.frame_id = "panda_link0"; + // ocm.orientation.w = 1.0; + // ocm.absolute_x_axis_tolerance = 0.1; + // ocm.absolute_y_axis_tolerance = 0.1; + // ocm.absolute_z_axis_tolerance = 0.1; + // ocm.weight = 1.0; + + // // Now, set it as the path constraint for the group. + // moveit_msgs::msg::Constraints test_constraints; + // test_constraints.orientation_constraints.push_back(ocm); + // move_group.setPathConstraints(test_constraints); + + // // Enforce Planning in Joint Space + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // + // // Depending on the planning problem MoveIt chooses between + // // ``joint space`` and ``cartesian space`` for problem representation. + // // Setting the group parameter ``enforce_joint_model_state_space:true`` in + // // the ompl_planning.yaml file enforces the use of ``joint space`` for all plans. + // // + // // By default, planning requests with orientation path constraints + // // are sampled in ``cartesian space`` so that invoking IK serves as a + // // generative sampler. + // // + // // By enforcing ``joint space``, the planning process will use rejection + // // sampling to find valid requests. Please note that this might + // // increase planning time considerably. + // // + // // We will reuse the old goal that we had and plan to it. + // // Note that this will only work if the current state already + // // satisfies the path constraints. So we need to set the start + // // state to a new pose. + // moveit::core::RobotState start_state(*move_group.getCurrentState()); + // geometry_msgs::msg::Pose start_pose2; + // start_pose2.orientation.w = 1.0; + // start_pose2.position.x = 0.55; + // start_pose2.position.y = -0.05; + // start_pose2.position.z = 0.8; + // start_state.setFromIK(joint_model_group, start_pose2); + // move_group.setStartState(start_state); + + // // Now, we will plan to the earlier pose target from the new + // // start state that we just created. + // move_group.setPoseTarget(target_pose1); + + // // Planning with constraints can be slow because every sample must call an inverse kinematics solver. + // // Let's increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed. + // move_group.setPlanningTime(10.0); + + // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + // RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); + + // // Visualize the plan in RViz: + // visual_tools.deleteAllMarkers(); + // visual_tools.publishAxisLabeled(start_pose2, "start"); + // visual_tools.publishAxisLabeled(target_pose1, "goal"); + // visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // // When done with the path constraint, be sure to clear it. + // move_group.clearPathConstraints(); + + // // Cartesian Paths + // // ^^^^^^^^^^^^^^^ + // // You can plan a Cartesian path directly by specifying a list of waypoints + // // for the end-effector to go through. Note that we are starting + // // from the new start state above. The initial pose (start state) does not + // // need to be added to the waypoint list but adding it can help with visualizations + // std::vector waypoints; + // waypoints.push_back(start_pose2); + + // geometry_msgs::msg::Pose target_pose3 = start_pose2; + + // target_pose3.position.z -= 0.2; + // waypoints.push_back(target_pose3); // down + + // target_pose3.position.y -= 0.2; + // waypoints.push_back(target_pose3); // right + + // target_pose3.position.z += 0.2; + // target_pose3.position.y += 0.2; + // target_pose3.position.x -= 0.2; + // waypoints.push_back(target_pose3); // up and left + + // // We want the Cartesian path to be interpolated at a resolution of 1 cm + // // which is why we will specify 0.01 as the max step in Cartesian + // // translation. We will specify the jump threshold as 0.0, effectively disabling it. + // // Warning - disabling the jump threshold while operating real hardware can cause + // // large unpredictable motions of redundant joints and could be a safety issue + // moveit_msgs::msg::RobotTrajectory trajectory; + // const double jump_threshold = 0.0; + // const double eef_step = 0.01; + // double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, fetched_trajectory); + // RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); + + // // Visualize the plan in RViz + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); + // for (std::size_t i = 0; i < waypoints.size(); ++i) + // visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // // Cartesian motions should often be slow, e.g. when approaching objects. The speed of Cartesian + // // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time + // // the trajectory manually, as described `here + // `_. + // // Pull requests are welcome. + // // + // // You can execute a trajectory like this. + // /* move_group.execute(trajectory); */ + + // // Adding objects to the environment + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // + // // First, let's plan to another simple goal with no objects in the way. + // move_group.setStartState(*move_group.getCurrentState()); + // geometry_msgs::msg::Pose another_pose; + // another_pose.orientation.w = 0; + // another_pose.orientation.x = -1.0; + // another_pose.position.x = 0.7; + // another_pose.position.y = 0.0; + // another_pose.position.z = 0.59; + // move_group.setPoseTarget(another_pose); + + // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + // RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); + + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.publishAxisLabeled(another_pose, "goal"); + // visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // // The result may look like this: + // // + // // .. image:: ./move_group_interface_tutorial_clear_path.gif + // // :alt: animation showing the arm moving relatively straight toward the goal + // // + // // Now, let's define a collision object ROS message for the robot to avoid. + // moveit_msgs::msg::CollisionObject collision_object; + // collision_object.header.frame_id = move_group.getPlanningFrame(); + + // // The id of the object is used to identify it. + // collision_object.id = "box1"; + + // // Define a box to add to the world. + // shape_msgs::msg::SolidPrimitive primitive; + // primitive.type = primitive.BOX; + // primitive.dimensions.resize(3); + // primitive.dimensions[primitive.BOX_X] = 0.1; + // primitive.dimensions[primitive.BOX_Y] = 1.5; + // primitive.dimensions[primitive.BOX_Z] = 0.5; + + // // Define a pose for the box (specified relative to frame_id). + // geometry_msgs::msg::Pose box_pose; + // box_pose.orientation.w = 1.0; + // box_pose.position.x = 0.48; + // box_pose.position.y = 0.0; + // box_pose.position.z = 0.25; + + // collision_object.primitives.push_back(primitive); + // collision_object.primitive_poses.push_back(box_pose); + // collision_object.operation = collision_object.ADD; + + // std::vector collision_objects; + // collision_objects.push_back(collision_object); + + // // Now, let's add the collision object into the world + // // (using a vector that could contain additional objects) RCLCPP_INFO(LOGGER, "Add an object into the world"); + + // // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message + // visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + + // // Now, when we plan a trajectory it will avoid the obstacle. + // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + // RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); + // visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + + // // The result may look like this: + // // + // // .. image:: ./move_group_interface_tutorial_avoid_path.gif + // // :alt: animation showing the arm moving avoiding the new obstacle + // // + // // Attaching objects to the robot + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // + // // You can attach an object to the robot, so that it moves with the robot geometry. + // // This simulates picking up the object for the purpose of manipulating it. + // // The motion planning should avoid collisions between objects as well. + // moveit_msgs::msg::CollisionObject object_to_attach; + // object_to_attach.id = "cylinder1"; + + // shape_msgs::msg::SolidPrimitive cylinder_primitive; + // cylinder_primitive.type = primitive.CYLINDER; + // cylinder_primitive.dimensions.resize(2); + // cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20; + // cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04; + + // // We define the frame/pose for this cylinder so that it appears in the gripper. + // object_to_attach.header.frame_id = move_group.getEndEffectorLink(); + // geometry_msgs::msg::Pose grab_pose; + // grab_pose.orientation.w = 1.0; + // grab_pose.position.z = 0.2; + + // // First, we add the object to the world (without using a vector). + // object_to_attach.primitives.push_back(cylinder_primitive); + // object_to_attach.primitive_poses.push_back(grab_pose); + // object_to_attach.operation = object_to_attach.ADD; + + // // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to. + // // We also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper. + // // You could also use applyAttachedCollisionObject to attach an object to the robot directly. + // RCLCPP_INFO(LOGGER, "Attach the object to the robot"); + // std::vector touch_links; + // touch_links.push_back("panda_rightfinger"); + // touch_links.push_back("panda_leftfinger"); + // move_group.attachObject(object_to_attach.id, "panda_hand", touch_links); + + // visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.trigger(); + + // /* Wait for MoveGroup to receive and process the attached collision object message */ + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); + + // // Replan, but now with the object in hand. + // move_group.setStartStateToCurrentState(); + // success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + // RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); + // visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); + // visual_tools.trigger(); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + + // // The result may look something like this: + // // + // // .. image:: ./move_group_interface_tutorial_attached_object.gif + // // :alt: animation showing the arm moving differently once the object is attached + // // + // // Detaching and Removing Objects + // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // + // // Now, let's detach the cylinder from the robot's gripper. + // RCLCPP_INFO(LOGGER, "Detach the object from the robot"); + // move_group.detachObject(object_to_attach.id); + + // // Show text in RViz of status + // visual_tools.deleteAllMarkers(); + // visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.trigger(); + + // /* Wait for MoveGroup to receive and process the attached collision object message */ + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the + // robot"); + + // // Now, let's remove the objects from the world. + // RCLCPP_INFO(LOGGER, "Remove the objects from the world"); + // std::vector object_ids; + // object_ids.push_back(collision_object.id); + // object_ids.push_back(object_to_attach.id); + + // // Show text in RViz of status + // visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE, false); + // visual_tools.trigger(); + + // /* Wait for MoveGroup to receive and process the attached collision object message */ + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears"); + + // // END_TUTORIAL + // visual_tools.deleteAllMarkers(); + // visual_tools.trigger(); + + rclcpp::shutdown(); + return 0; +} diff --git a/package.xml b/package.xml index bf9505634d..af96829f14 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ moveit_core moveit_ros_perception moveit_ros_planning_interface + moveit_ros_trajectory_cache moveit_servo moveit_hybrid_planning moveit_visual_tools