From 2757b3d6d2a15e6e38fd08436d252beb282cbc35 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 30 Jul 2024 23:24:30 -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 | 1051 +++++++++++++++++ package.xml | 1 + 8 files changed, 1872 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 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..bdd42776e5 --- /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.' + }, + 'prune_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..d178b8c020 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp @@ -0,0 +1,1051 @@ +/********************************************************************* + * 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. + * - prune_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("prune_worse_trajectories", prune_worse_trajectories, false); + node_->get_parameter_or("planner", planner, "RRTstar"); + } + + double start_tolerance; + double goal_tolerance; + bool prune_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 << "prune_worse_trajectories:" << HAIR_SPACE + << (reconfigurable_parameters.prune_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("prune_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("prune_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 `prune_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.insertTrajectory( // 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, + /*prune_worse_trajectories=*/reconfigurable_parameters.prune_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.insertCartesianTrajectory( // 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, + /*prune_worse_trajectories=*/reconfigurable_parameters.prune_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.insertTrajectory( // 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, + /*prune_worse_trajectories=*/reconfigurable_parameters.prune_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/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