From c7cfd22fcf3ed57715a1e08874a248cf18a96fae Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 6 Aug 2024 18:26:08 -0700 Subject: [PATCH] Refactor tutorial to support TrajectoryCache refactor Signed-off-by: methylDragon --- .../trajectory_cache_move_group.launch.py | 7 +- .../src/trajectory_cache_demo.cpp | 158 +++++++++++------- 2 files changed, 100 insertions(+), 65 deletions(-) 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 index a0051508e3..cdbd449a95 100644 --- 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 @@ -3,10 +3,9 @@ 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.actions import ExecuteProcess, TimerAction from launch_ros.actions import Node + from moveit_configs_utils import MoveItConfigsBuilder joint_limits_path = os.path.join( @@ -109,8 +108,8 @@ def generate_launch_description(): rviz_node, static_tf, robot_state_publisher, - run_move_group_node, 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 index d178b8c020..6284f27d1e 100644 --- a/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp +++ b/doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp @@ -88,12 +88,19 @@ #include #include +#include +#include +#include #include +#include #include #include #include #include +#include +#include +#include #include @@ -234,6 +241,10 @@ void vizCachedTrajectories(moveit_visual_tools::MoveItVisualTools& visual_tools, bool found_exclude = false; for (const auto& exclude_trajectory : exclude_trajectories) { + if (!exclude_trajectory) + { + continue; + } if (*exclude_trajectory == *cached_trajectory) { found_exclude = true; @@ -281,10 +292,13 @@ void vizAllCachedMotionPlanTrajectories(moveit_visual_tools::MoveItVisualTools& 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)) + 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) @@ -316,9 +330,12 @@ void vizAllCachedCartesianPlanTrajectories(moveit_visual_tools::MoveItVisualTool // 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)) + /*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) @@ -649,9 +666,20 @@ int main(int argc, char** argv) // Main loop. ==================================================================================== moveit_msgs::msg::MotionPlanRequest to_target_motion_plan_request_msg; - moveit_msgs::srv::GetCartesianPath::Request cartesian_path_request_msg; + moveit_msgs::srv::GetCartesianPath::Request cartesian_path_request; moveit_msgs::msg::MotionPlanRequest to_home_motion_plan_request_msg; + std::unique_ptr> + cache_insert_policy = moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCacheInsertPolicy(); + + std::unique_ptr> + 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: @@ -713,16 +741,6 @@ int main(int argc, char** argv) 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 @@ -743,10 +761,9 @@ int main(int argc, char** argv) * 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, + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_target_motion_plan_request_msg, + /*plan=*/to_target_motion_plan, + /*cache_insert_policy=*/*cache_insert_policy, /*prune_worse_trajectories=*/reconfigurable_parameters.prune_worse_trajectories); } else @@ -780,16 +797,21 @@ int main(int argc, char** argv) 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); + /*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, - reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, - /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + 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) @@ -811,7 +833,7 @@ int main(int argc, char** argv) /*exclude=*/matched_to_target_trajectories); vizAllCachedCartesianPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, - cartesian_path_request_msg, rvt::Colors::DARK_GREY, + cartesian_path_request, rvt::Colors::DARK_GREY, /*exclude=*/matched_to_target_trajectories); vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, @@ -851,46 +873,57 @@ int main(int argc, char** argv) // Plan and execute to target cartesian pose. ============================================== // Plan and Cache. - cartesian_path_request_msg = trajectory_cache.constructGetCartesianPathRequest( + 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::msg::RobotTrajectory cartesian_trajectory; + moveit_msgs::srv::GetCartesianPath::Response cartesian_path_response; 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); + 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 (fraction >= MIN_EXECUTABLE_FRACTION) + if (cartesian_path_response.fraction >= MIN_EXECUTABLE_FRACTION) { - double execution_time = - rclcpp::Duration(cartesian_trajectory.joint_trajectory.points.back().time_from_start).seconds(); + 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_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); + move_group, /*cache_namespace=*/PLANNING_GROUP, cartesian_path_request, cartesian_path_response, + /*cache_insert_policy=*/*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_msg, - /*min_fraction=*/MIN_EXECUTABLE_FRACTION, reconfigurable_parameters.start_tolerance, - reconfigurable_parameters.goal_tolerance, /*metadata_only=*/false, /*sort_by=*/"execution_time_s", - /*ascending=*/true); + 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_msg, - /*min_fraction=*/MIN_EXECUTABLE_FRACTION, reconfigurable_parameters.start_tolerance, - reconfigurable_parameters.goal_tolerance, /*metadata_only=*/false, /*sort_by=*/"execution_time_s", - /*ascending=*/true); + 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) @@ -918,7 +951,7 @@ int main(int argc, char** argv) /*exclude=*/matched_to_cartesian_trajectories); vizAllCachedCartesianPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, - cartesian_path_request_msg, rvt::Colors::WHITE, + cartesian_path_request, rvt::Colors::WHITE, /*exclude=*/matched_to_cartesian_trajectories); vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, @@ -935,7 +968,8 @@ int main(int argc, char** argv) rclcpp::sleep_for(std::chrono::seconds(1)); // Execute. - if (run_execute.load() && fraction >= MIN_EXECUTABLE_FRACTION) + if (run_execute.load() && best_to_cartesian_trajectory && + best_to_cartesian_trajectory->lookupDouble("fraction") >= MIN_EXECUTABLE_FRACTION) { move_group.execute(*best_to_cartesian_trajectory); } @@ -962,14 +996,10 @@ int main(int argc, char** argv) 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, + move_group, /*cache_namespace=*/PLANNING_GROUP, /*plan_request=*/to_home_motion_plan_request_msg, + /*plan=*/to_home_motion_plan, + /*cache_insert_policy=*/*cache_insert_policy, /*prune_worse_trajectories=*/reconfigurable_parameters.prune_worse_trajectories); } else @@ -983,16 +1013,22 @@ int main(int argc, char** argv) 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); + /*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, - reconfigurable_parameters.start_tolerance, reconfigurable_parameters.goal_tolerance, - /*metadata_only=*/false, /*sort_by=*/"execution_time_s", /*ascending=*/true); + /*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) @@ -1014,7 +1050,7 @@ int main(int argc, char** argv) /*exclude=*/matched_to_home_trajectories); vizAllCachedCartesianPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group, - cartesian_path_request_msg, rvt::Colors::DARK_GREY, + cartesian_path_request, rvt::Colors::DARK_GREY, /*exclude=*/matched_to_home_trajectories); vizAllCachedMotionPlanTrajectories(visual_tools, joint_model_group, trajectory_cache, move_group,