diff --git a/CMakeLists.txt b/CMakeLists.txt index 14a8a6e28f..5ee2731168 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 @@ -29,6 +30,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_geometry_msgs tf2_geometry_msgs tf2_ros + warehouse_ros_sqlite ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -69,6 +71,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/_static/videos/trajectory_cache/01_cache_and_execute_loop.webm b/_static/videos/trajectory_cache/01_cache_and_execute_loop.webm new file mode 100644 index 0000000000..5e2a01a822 Binary files /dev/null and b/_static/videos/trajectory_cache/01_cache_and_execute_loop.webm differ diff --git a/_static/videos/trajectory_cache/02_trajectory_cache_demo.webm b/_static/videos/trajectory_cache/02_trajectory_cache_demo.webm new file mode 100644 index 0000000000..9ff5e292fc Binary files /dev/null and b/_static/videos/trajectory_cache/02_trajectory_cache_demo.webm differ diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index 3a6969d0da..f0b992d377 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -25,6 +25,7 @@ Configuring and Using MoveIt pick_ik/pick_ik_tutorial trac_ik/trac_ik_tutorial benchmarking/benchmarking_tutorial + trajectory_cache/trajectory_cache_tutorial Developing and Documenting MoveIt --------------------------------- 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..3d3dc093b0 --- /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}) 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..1183046eda --- /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 diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_best_trajectory.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_best_trajectory.png new file mode 100644 index 0000000000..fd7db94211 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_best_trajectory.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_matchable_plans.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_matchable_plans.png new file mode 100644 index 0000000000..e6821245a3 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_matchable_plans.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_title_and_statistics.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_title_and_statistics.png new file mode 100644 index 0000000000..f94d17b057 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheAndExecute_title_and_statistics.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheNoPruning_cache_visualization.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheNoPruning_cache_visualization.png new file mode 100644 index 0000000000..c11a3fb877 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheNoPruning_cache_visualization.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheNoPruning_title_and_statistics.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheNoPruning_title_and_statistics.png new file mode 100644 index 0000000000..9902267af4 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheNoPruning_title_and_statistics.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_cache_visualization.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_cache_visualization.png new file mode 100644 index 0000000000..827575dc97 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_cache_visualization.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_delete_worse_trajectories.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_delete_worse_trajectories.png new file mode 100644 index 0000000000..6b5a132707 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_delete_worse_trajectories.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_title_and_statistics.png b/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_title_and_statistics.png new file mode 100644 index 0000000000..936f8ffa17 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_CacheWithPruning_title_and_statistics.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_diff_to_trajectory.png b/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_diff_to_trajectory.png new file mode 100644 index 0000000000..8aec11df3a Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_diff_to_trajectory.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_title_and_statistics.png b/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_title_and_statistics.png new file mode 100644 index 0000000000..22e1ccd8a2 Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_title_and_statistics.png differ diff --git a/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_tolerance.png b/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_tolerance.png new file mode 100644 index 0000000000..a536c9cf0a Binary files /dev/null and b/doc/how_to_guides/trajectory_cache/images/demo_HighStartTolerance_tolerance.png differ 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..9a2b971380 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache.rviz @@ -0,0 +1,519 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 79 + Name: Displays + Property Tree Widget: + Expanded: + - /PlanningScene1/Scene Robot1 + - /MotionPlanning1/Scene Robot1 + Splitter Ratio: 0.5 + Tree Height: 358 + - 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.2222777158021927 + Y: -0.6489211916923523 + Z: 0.525452196598053 + 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: 1440 + X: 1691 + Y: 22 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..46760cda49 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_demo.launch.py @@ -0,0 +1,115 @@ +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..0ecb16b9a7 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/launch/trajectory_cache_move_group.launch.py @@ -0,0 +1,114 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, TimerAction +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, + ros2_control_node, + TimerAction(period=2.0, actions=[run_move_group_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..efa3f4b5c4 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp @@ -0,0 +1,1213 @@ +/********************************************************************* + * 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 demo has four phases that can be advanced using the `rviz_visual_tools` dialog box. + * + * 1. Plan and cache (no pruning) + * 2. Plan and cache (with pruning) + * 3. Fetch from cache and execute (while still planning and caching with pruning) + * 4. Fetch from cache and execute, except with large start tolerances + * + * 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 is 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 +#include +#include +#include +#include +#include + +#include + +#include + +namespace // Helpers. +{ + +// Consts and declarations. ======================================================================== + +namespace rvt = rviz_visual_tools; + +using TrajectoryCacheEntryPtr = warehouse_ros::MessageWithMetadata::ConstPtr; + +using MotionPlanCacheInsertPolicy = + moveit_ros::trajectory_cache::CacheInsertPolicyInterface; + +using CartesianCacheInsertPolicy = + moveit_ros::trajectory_cache::CacheInsertPolicyInterface; + +using CartesianFeatures = moveit_ros::trajectory_cache::FeaturesInterface; + +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) + { + continue; + } + 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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultFeatures(/*start_tolerance=*/9999, + /*goal_tolerance=*/9999), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature())) + { + 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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCartesianFeatures(/*start_tolerance=*/9999, + /*goal_tolerance=*/9999, + /*min_fraction=*/0.0), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature())) + { + 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)) + * + * When using the default cache features and default cache insert policy (which this demo does): + * 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` (execution time by default) than exactly matching entries. + * + * A cache entry is "exactly matching" if its parameters 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_pruning_worse = 0; + + if (!trajectory_cache.init(options)) + { + RCLCPP_FATAL(LOGGER, "Could not init cache."); + return 1; + } + + // Interactivity. ================================================================================ + + /** [TUTORIAL NOTE] + * This demo has four phases that can be advanced using the `rviz_visual_tools` dialog box. + * + * 1. Plan and cache (no pruning) + * - Showcases basic cache functionality. + * 2. Plan and cache (with pruning) + * - Showcases pruning functionality. + * 3. Fetch from cache and execute (while still planning and caching with pruning) + * - Showcases cache fetches. + * 4. Fetch from cache and execute, except with large start tolerances + * - Showcases fuzzy matching, and the impact of start tolerance. + */ + + 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; + moveit_msgs::msg::MotionPlanRequest to_home_motion_plan_request_msg; + + std::unique_ptr default_cache_insert_policy = + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCacheInsertPolicy(); + + std::unique_ptr default_cartesian_cache_insert_policy = + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCartesianCacheInsertPolicy(); + + /** [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, by default.) + * + * 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] + * + * For more information about how the cache works or the cache keying logic, see the + * associated guide instead. + * + * INSERTING MOTION PLANS + * ^^^^^^^^^^^^^^^^^^^^^^ + * The TrajectoryCache inserts plans based off a CacheInsertPolicy that you can specify. + * + * You can specify what cache insert policy to use using the `cache_insert_policy` + * argument. + * + * The default policy inserts cache entries only if they have the best seen execution + * time so far amongst other exactly matching cache entries. + * + * You may obtain a reusable instance of the default policy to pass by calling the + * static `TrajectoryCache::getDefaultCacheInsertPolicy()` method. + * + * INSERT POLICIES AND FEATURES + * ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + * The cache insert policy you use also constrains what features you can use to fetch + * cache entries with. This is because the insert policy makes certain assumptions about + * what cache features to extract and append to the cache entry to key the entry on. + * + * Be sure to check what features the cache insert policy you use supports! + * + * This will be further discussed in a later section of this demo. + * + * Additionally, a user may decide to attach additional features using the + * `additional_features` field to append to the cache entry (provided it is also not + * covered by the cache insert policy used) for use in fetching. + * + * CACHE PRUNING + * ^^^^^^^^^^^^^ + * The cache insert policy selected also helps inform the cache for when an entry should + * be pruned. Pruning allows for the cache memory/storage usage to be reduced, and also + * reduces query time. + * + * The default policy marks "worse" (in terms of best seen execution time) exactly + * matching cache entries for deletion, which the cache will use to execute cache + * pruning. + * + * The cache's `num_additional_trajectories_to_preserve_when_pruning_worse` option can + * be used to override the pruning behavior and preserve additional cache entries. + * + * NOTE: + * Pruning still happens even if a trajectory was not inserted, as long as the + * requirements for pruning are met. + */ + trajectory_cache.insertTrajectory( // Returns bool. True if put. + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_target_motion_plan_request_msg, + /*plan=*/to_target_motion_plan, + /*cache_insert_policy=*/*default_cache_insert_policy, + /*prune_worse_trajectories=*/reconfigurable_parameters.prune_worse_trajectories, + /*additional_features=*/{}); + } + 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 TrajectoryCache fetches plans based off a vector of Features of the plan request that + * you can specify. These cache features will be used to fuzzily fetch either all matching + * trajectories, or just the "best" one, sorted by some feature. + * + * You can specify what cache features to use to fetch and sort the fetched plans with using + * the `features` and `sort_by` arguments respectively. + * + * As mentioned in the previous section, you can only fetch an inserted plan using a subset + * of the features supported by the cache insert policy used to insert it (and/or any + * `additional_features` provided at time of insert). + * + * Using any features not used during insert will cause the cache entry to fail to match, + * and hence fail to be included in the returned fetch response. + * + * The default policy keys the entry on the plan request message (and the robot state from + * the move group), including details such as: + * - Planning workspace + * - Starting joint state + * - Max velocity scaling, acceleration scaling, and cartesian speed + * - Goal constraints + * - Path constraints + * - Trajectory constraints + * + * The default sort feature is execution time. + * + * You may obtain a reusable instance of the default features and sort feature to pass for + * given tolerances by calling the static `TrajectoryCache::getDefaultFeatures()` and + * `TrajectoryCache::getDefaultSortFeature()` methods respectively. + * + * The default features uses all the features supported by the default cache insert policy, + * and they are intended to be used together. + * + * MATCH TOLERANCES + * ^^^^^^^^^^^^^^^^ + * For the default cache features, it is recommended to have a loose `start_tolerance` and a + * stricter `goal_tolerance`. + * + * This is because, in most cases, the final position requested by a plan request is more + * important. Consider: + * Even if the manipulator starts further away in configuration space than the first + * trajectory point of a fetched trajectory, on execution of the trajectory, the manipulator + * will "snap" to the start of the trajectory and follow it to the end point. + * + * As such, the start tolerance is only important to make sure that the "snapping" of the + * manipulator to the start state of the fetched trajectory will be valid, whereas the goal + * tolerance is important to make sure that the manipulator actually ends up 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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultFeatures(reconfigurable_parameters.start_tolerance, + reconfigurable_parameters.goal_tolerance), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature(), /*ascending=*/true, + /*metadata_only=*/false); + + 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, + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultFeatures(reconfigurable_parameters.start_tolerance, + reconfigurable_parameters.goal_tolerance), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature(), /*ascending=*/true, + /*metadata_only=*/false); + 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, 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 = moveit_ros::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::srv::GetCartesianPath::Response cartesian_path_response; + + auto cartesian_plan_start = move_group_node->now(); + cartesian_path_response.fraction = + move_group.computeCartesianPath(cartesian_path_request.waypoints, cartesian_max_step, + cartesian_jump_threshold, cartesian_path_response.solution); + auto cartesian_plan_end = move_group_node->now(); + + if (cartesian_path_response.fraction >= MIN_EXECUTABLE_FRACTION) + { + /** [TUTORIAL NOTE] + * + * CARTESIAN PLANS + * ^^^^^^^^^^^^^^^ + * The TrajectoryCache also supports inserting and fetching cartesian plans, with similar + * capabilities to pass user-specified cache insert policies and cache features. + * + * Just call the cartesian variants of the cache methods, and use the cartesian variants of + * the cache insert policies and cache features. + * + * For example: + * - `TrajectoryCache::getDefaultCartesianCacheInsertPolicy()` + * - `TrajectoryCache::getDefaultCartesianFeatures()` + * + * The default policy for cartesian plans keys the entry on the plan request message (and + * the robot state from the move group), including details such as: + * - Planning workspace + * - Starting joint state + * - Max velocity scaling, acceleration scaling, and cartesian speed + * - Max end effector step and jump thresholds + * - Requested waypoints + * - Path constraints + * + * ADDITIONAL FEATURES + * ^^^^^^^^^^^^^^^^^^^ + * In this example we also add a custom user-specified feature for planning time, since that + * information is not found in the plan request or response to insert. + * + * Adding this feature will allow us to fetch and sort the cache entry we are inserting + * using the "planning_time_s" feature. + */ + std::vector> additional_features; + additional_features.push_back( + std::make_unique< + moveit_ros::trajectory_cache::MetadataOnlyFeature>( + "planning_time_s", (cartesian_plan_end - cartesian_plan_start).seconds())); + + trajectory_cache.insertCartesianTrajectory( // Returns bool. True if put. + move_group, /*cache_namespace=*/PLANNING_GROUP, cartesian_path_request, cartesian_path_response, + /*cache_insert_policy=*/*default_cartesian_cache_insert_policy, + /*prune_worse_trajectories=*/reconfigurable_parameters.prune_worse_trajectories, + /*additional_features=*/additional_features); + } + + // 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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCartesianFeatures( + reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, + MIN_EXECUTABLE_FRACTION), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature(), + /*ascending=*/true, /*metadata_only=*/false); + + 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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCartesianFeatures( + reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, + MIN_EXECUTABLE_FRACTION), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature(), + /*ascending=*/true, /*metadata_only=*/false); + 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, 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() && best_to_cartesian_trajectory && + best_to_cartesian_trajectory->lookupDouble("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); + + trajectory_cache.insertTrajectory( // Returns bool. True if put. + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_home_motion_plan_request_msg, + /*plan=*/to_home_motion_plan, + /*cache_insert_policy=*/*default_cache_insert_policy, + /*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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultFeatures(reconfigurable_parameters.start_tolerance, + reconfigurable_parameters.goal_tolerance), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature(), /*ascending=*/true, + /*metadata_only=*/false); + + 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, + /*features=*/ + moveit_ros::trajectory_cache::TrajectoryCache::getDefaultFeatures(reconfigurable_parameters.start_tolerance, + reconfigurable_parameters.goal_tolerance), + /*sort_by=*/moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature(), /*ascending=*/true, + /*metadata_only=*/false); + 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, 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/trajectory_cache_tutorial.rst b/doc/how_to_guides/trajectory_cache/trajectory_cache_tutorial.rst new file mode 100644 index 0000000000..b3436dddc2 --- /dev/null +++ b/doc/how_to_guides/trajectory_cache/trajectory_cache_tutorial.rst @@ -0,0 +1,325 @@ +Fuzzy-Matching Trajectory Cache - Quickstart Guide +================================================== + +The fuzzy-matching trajectory cache allows you to insert and fetch manipulator trajectories, keyed fuzzily on ``MotionPlanRequest`` and ``GetCartesianPath`` requests and trajectories. + +The cache will work on manipulators with an arbitrary number of joints, across any number of move groups. +Furthermore, the cache supports pruning and ranking of fetched trajectories, with extension points for injecting your own feature keying, cache insert, cache prune and cache sorting logic. + +.. raw:: html + + + +For more info, please see the :moveit_codedir:`package README `. + +.. warning:: + The cache does NOT support collision detection and some other constraints (e.g. multi-DoF joints, planning scene, constraint regions, and visibility constraints)! + + Plans will be inserted into and fetched from the cache IGNORING collision and those constraints. + If your planning scene is expected to change significantly between cache lookups, ensure that you validate any fetched plans for collisions. + +Quick Example Usage +------------------- + +**PRE-REQUISITE**: The ``warehouse_plugin`` ROS parameter must be set to a `warehouse_ros `_ plugin you have installed, which determines what database backend should be used for the cache. + +:: + + auto cache = std::make_shared(node); + cache->init(/*db_host=*/":memory:", /*db_port=*/0, /*exact_match_precision=*/1e-6); + + auto default_features = TrajectoryCache::getDefaultFeatures(start_tolerance, goal_tolerance); + std::string TrajectoryCache::getDefaultSortFeature(); // Sorts by planned execution time. + + move_group.setPoseTarget(...); + moveit_msgs::msg::MotionPlanRequest motion_plan_req_msg; + move_group.constructMotionPlanRequest(motion_plan_request_msg); + + auto fetched_trajectory = // Use the cache fetch in place of planning! + cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg, + /*features=*/default_features, + /*sort_by=*/TrajectoryCache::getDefaultSortFeature(), + /*ascending=*/true); + + if (fetched_trajectory) // Great! We got a cache hit, we can execute it. + { + move_group.execute(*fetched_trajectory); + } + else // Otherwise, plan... And put it for posterity! + { + moveit::planning_interface::MoveGroupInterface::Plan plan; + if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS) + { + cache->insertTrajectory( + *interface, robot_name, std::move(plan_req_msg), std::move(plan), + /*cache_insert_policy=*/BestSeenExecutionTimePolicy(), + /*prune_worse_trajectories=*/true, /*additional_features=*/{}); + } + } + +Cartesian variants of the methods in the above example exist. + +For more information, please read the extensive API documentation for :cpp_api:`moveit_ros::trajectory_cache::TrajectoryCache`. + +Why A Cache? +------------ + +A trajectory cache helps: + +- Cut down on planning time +- Allows for consistent predictable behavior of used together with a stochastic planner + + - It effectively allows you to "freeze" a move + +To explain this, consider that planners in MoveIt generally fall under two main camps: stochastic/probabilistic, and optimization based. +The probabilistic planners are fast, but usually non-deterministic, whereas the optimization based ones are usually slow, but deterministic. + +One way to get around this is to pre-plan and manually select and label robot trajectories to "freeze" in a trajectory database to then replay by name, which avoids needing to spend time to replan, and allows you to ensure that motions are constant and repeatable. +However, this approach is not very flexible and not easily reused. + +The trajectory cache improves upon this approach by allowing a user to "freeze" and store successful plans, but **also** look up those plans in a more generalizable and natural way, using the planning request itself to key the cache, effectively allowing the cache to stand in for a planning call. + +Furthermore, the specific properties of this trajectory cache provides further unique benefits: + +1. With fuzzy matching, "frozen" plans are much easier to look up and reuse, while simultaneously increasing the chances of a cache hit. +2. The ability to rank trajectories will, if used with a stochastic planner over sufficient runs, cause the cache to eventually converge to increasingly optimal plans. + +Finally, the cache makes use of pruning to optimize fetch times, and also finds ways to "canonicalize" features of the keying request to increase chances of a cache hit. + +Working Principle +----------------- +If a plan request has features (e.g., start, goal, and constraint conditions) that are "close enough" to an entry in the cache, then the cached trajectory should be reusable for that request, allowing us to skip planning. + +The cache extracts these features from the planning request and plan response, storing them in the cache database. +When a new planning request is used to attempt to fetch a matching plan, the cache attempts to fuzzily match the request to pre-existing cache entries keyed on similar requests. +Any "close enough" matches are then returned as valid cache hits for plan reuse, with the definition of "close enough" depending on the type of feature that is being extracted. + +For more information about how the cache works and what specific features are used to key cache entries with, please see the :moveit_codedir:`package README ` and the extensive API documentation for :cpp_api:`moveit_ros::trajectory_cache::TrajectoryCache` and its related classes. + +Demo +---- + +.. raw:: html + + + +(Video is 4x speed.) + +This demo has four phases that can be advanced using the ``rviz_visual_tools`` dialog box, which combined, showcases the highlights of the cache's capabilities. + +1. Plan and cache (no pruning) +2. Plan and cache (with pruning) +3. Fetch from cache and execute (while still planning and caching with pruning) +4. Fetch from cache and execute, except with large start tolerances + +Additionally, the `demo source code itself `_ is heavily annotated with comments and tutorial notes, should you wish to inspect the code and dive deeper. + +Pre-Requisites +++++++++++++++ + +Make sure you have built the `moveit2_tutorials `_ package and sourced the workspace it is built in to ensure that you can run the demo. + +Running the Demo +++++++++++++++++ + +In one terminal, start the move group: + +:: + + ros2 launch moveit2_tutorials trajectory_cache_move_group.launch.py + +In another, launch the demo: + +:: + + ros2 launch moveit2_tutorials trajectory_cache_demo.launch.py + +You should see something similar to the video, except with a different set of randomly generated goal poses. + +Click ``next`` on the rviz window to advance the demo. + +.. note:: + Sometimes a randomly generated demo goal pose is unreachable (or the ``move_group`` fails to initialize properly). + If this happens, the demo will halt in the first phase due to a failed planning call or other issue. + + Just restart the demo, which will generate new demo goal poses, and resolve the issue. + +Configuring the Demo +++++++++++++++++++++ + +Additionally, the demo's launch file exposes launch arguments that allows you to change many aspects of the demo. +Look at the `demo's launch file `_ for the full list of configurable launch arguments. + +For example, you can specify a disk path for the cache to be saved to and loaded from, instead of memory: + +:: + + ros2 launch moveit2_tutorials trajectory_cache_demo.launch.py cache_db_host:="/trajectory_cache_demo.db + +Then if you had specified a disk path, since the demo uses SQLite by default, you can inspect the cache database file using an `SQLite database browser `_. + +1. CacheNoPruning ++++++++++++++++++ + +The first phase of the demo shows cache insertion and fetching functionality from a single starting point to multiple goal poses, with no pruning. + +The text on the top-left contains the name of the demo phase, and useful information about the state of the cache. + +.. image:: images/demo_CacheNoPruning_title_and_statistics.png + :width: 400px + +In this phase, we are running the planner multiple times per goal, and attempting a cache insert each time. +We also fetch and visualize all matching plans for a particular goal, and also the best matching plan. + +To interpret the visualization: + +- The **translucent dark grey** trajectory lines are the **planned trajectories** for the current goal +- The **white** trajectory lines are the **trajectories in the cache** +- The **yellow** trajectory lines are the **matching cached trajectories** for the start condition and planning request +- The **green** trajectory line is the **best matching cached trajectory** (in terms of execution time) for the given start condition and planning request + +.. image:: images/demo_CacheNoPruning_cache_visualization.png + :width: 450px + +.. note:: + The visualization demonstrates that it is possible to fetch "matching" trajectories for a given goal, and also to rank and determine what is "best" amongst those fetched plans. + +.. note:: + You may note how in the visualization that the green trajectory (the best trajectory) appears to be much more optimal than some of the other candidate plans. + + And furthermore, that most of the time, the fetch times are shorter than the planning times, even in this no-obstruction case, showing how the cache is saving the process planning time. + +The default cache insert policy inserts a cache plan only if it is the best seen (in terms of execution time) so far. +This is the reason why not every planned trajectory appears to be inserted in the cache. + +You should find that the number of cached plans stabilizes over time as the cache saves better and better plans over time. + +Leave the demo phase to run for a while to fill up the cache, then hit ``next`` on the ``rviz_visual_tools`` dialogue box to progress the demo to the next phase, where we will prune less optimal cache entries. + +2. CacheWithPruning ++++++++++++++++++++ + +The seconds phase of the demo shows cache insertion and fetching functionality from a single starting point to multiple goal poses, with pruning. + +Cache pruning is recommended to keep the number of trajectories in the cache low, saving on cache database storage, and making cache fetches more efficient. + +.. image:: images/demo_CacheWithPruning_title_and_statistics.png + :width: 400px + +You should also see the ``delete_worse_trajectories`` parameter get set to ``true``. + +.. image:: images/demo_CacheWithPruning_delete_worse_trajectories.png + :width: 400px + +Immediately, as plans continue to happen, you should see the cache entries get progressively pruned from the cache until eventually, the cache converges to one "best" trajectory per goal. + +.. image:: images/demo_CacheWithPruning_cache_visualization.png + :width: 700px + +The reason why it takes multiple planning runs to cause the cache to converge, is because we are only attempting to insert plans that have been obtained from the current planning run. +The cache only prunes exactly matching cache entries that are worse than (by default, in terms of execution time) the current insertion candidate. +So, in order for a plan to get pruned, the planned trajectory has to be better than it. + +.. note:: + If you wanted to immediately prune all worse plans, you could fetch the best plan, then re-insert it to trigger a prune that would achieve that. + +Hit ``next`` on the ``rviz_visual_tools`` dialogue box to progress the demo to the next phase, where we will start execution. + +3. ExecuteWithCache ++++++++++++++++++++ + +The third phase of the demo begins execution. + +We still plan and cache, now **including** cartesian plans too. +You should expect the number of cached plans to increase over time, including cached cartesian plans. + +.. image:: images/demo_CacheAndExecute_title_and_statistics.png + :width: 400px + +Observe that the executed trajectory is always locked to the best matching trajectory, as indicated by the green trajectory. + +.. image:: images/demo_CacheAndExecute_best_trajectory.png + :width: 450px + +Furthermore, notice that the set of matchable trajectories (indicated by white) is an even more constrained set from +This shows that the cache is partitionable and able to filter out unmatchable entries by nature of the constraints that key them. + +To interpret the visualization, with the same legend as before: + +- **NEW**: The **grey** trajectory lines are the **unmatchable trajectories in the cache** + +.. image:: images/demo_CacheAndExecute_matchable_plans.png + :width: 700px + +Hit ``next`` on the ``rviz_visual_tools`` dialogue box to progress the demo to the next phase, where we show the cache's fuzzy matching capability, and the impact of high start tolerance. + +4. HighStartTolerance ++++++++++++++++++++++ + +The final phase of the demo sets an inadvisably high start tolerance to showcase the cache's fuzzy matching capabilities. + +.. image:: images/demo_HighStartTolerance_title_and_statistics.png + :width: 400px + +We set a start tolerance of 2 (up from 0.025), which means that we allow trajectories up to 2 radians away per starting robot joint state and 2 units away per start constraint to match (for a still strict tolerance on the goals). + +.. image:: images/demo_HighStartTolerance_tolerance.png + :width: 400px + +To interpret the visualization, with the same legend as before: + +- **NEW**: The **red** lines are the **diff of robot state to the matched trajectory** + +.. image:: images/demo_HighStartTolerance_diff_to_trajectory.png + :width: 700px + +Notice that the robot, now for certain starting states, will use a different matching trajectory with a start state further away than its current one. +This diff is visualized as a thick red line. + +This is because that matched trajectory would have had a shorter execution time and be deemed "best", and is now matchable because of the higher start tolerance we set. +This showcases the ability for the cache to handle fuzzy matching, even though in this case it is with an inadvisably high start tolerance, since it would cause the robot to violently "snap" to the start of the trajectory. + +Best Practices +-------------- + +To end off this how-to-guide, here are some suggestions for how to best use the cache. + +**Collisions** + +Since the cache does not yet support collisions: + +- Ensure the planning scene and obstacles remain static, or, +- Validate the fetched plans separately +- BONUS: Use the ``TrajectoryCache::Options::num_additional_trajectories_to_preserve_when_deleting_worse`` option to preserve multiple trajectories after pruning to hopefully store a large enough variety of plausible trajectories that avoid obstacles, then in order from best to worst, validating each + +**Increasing Cache Hits** + +- Move the robot to static known poses (e.g., home) before fetching +- Use the cache where repetitive, non-dynamic motion is likely to occur +- Use looser start tolerance, and stricter goal tolerance + +Further Reading +--------------- + +Customizable Behavior ++++++++++++++++++++++ + +It is possible to extend the cache to key on custom user-defined features, and also to change the cache insertion, sorting, and pruning logic. +You do this by implementing the features and cache insert policy interfaces, then feeding them into the + +For example, you may decide to write your own feature extractor to key the cache, and decide when to insert or prune a cache entry on features such as: + +- Minimum jerk time +- Path length +- Any other feature not supported by this package! + +See: + +- :moveit_codedir:`package README ` +- :moveit_codedir:`package README ` \ No newline at end of file diff --git a/package.xml b/package.xml index bf9505634d..40aab80a01 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 @@ -37,6 +38,7 @@ tf2_ros moveit_task_constructor_core pick_ik + warehouse_ros_sqlite eigen geometric_shapes