Skip to content

Commit

Permalink
Refactor tutorial to support TrajectoryCache refactor
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
@methylDragon
methylDragon committed Aug 8, 2024
1 parent 935b7a9 commit c7cfd22
Show file tree
Hide file tree
Showing 2 changed files with 100 additions and 65 deletions.
7 changes: 3 additions & 4 deletions doc/how_to_guides/trajectory_cache/launch/trajectory_cache_move_group.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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
)
158 changes: 97 additions & 61 deletions doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,12 +88,19 @@
#include <thread>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp>
#include <moveit/trajectory_cache/features/constant_features.hpp>
#include <moveit/trajectory_cache/features/features_interface.hpp>
#include <moveit/trajectory_cache/trajectory_cache.hpp>
#include <moveit/trajectory_cache/utils/utils.hpp>

#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/motion_plan_request.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/srv/get_cartesian_path.hpp>

#include <moveit_visual_tools/moveit_visual_tools.h>

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -281,10 +292,13 @@ void vizAllCachedMotionPlanTrajectories(moveit_visual_tools::MoveItVisualTools&
const std::vector<TrajectoryCacheEntryPtr>& 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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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<moveit_ros::trajectory_cache::CacheInsertPolicyInterface<
moveit_msgs::msg::MotionPlanRequest, moveit::planning_interface::MoveGroupInterface::Plan,
moveit_msgs::msg::RobotTrajectory>>
cache_insert_policy = moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCacheInsertPolicy();

std::unique_ptr<moveit_ros::trajectory_cache::CacheInsertPolicyInterface<moveit_msgs::srv::GetCartesianPath::Request,
moveit_msgs::srv::GetCartesianPath::Response,
moveit_msgs::msg::RobotTrajectory>>
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:
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -780,16 +797,21 @@ int main(int argc, char** argv)
std::vector<TrajectoryCacheEntryPtr> 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)
Expand All @@ -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,
Expand Down Expand Up @@ -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<std::unique_ptr<
moveit_ros::trajectory_cache::FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>
additional_features;
additional_features.push_back(
std::make_unique<
moveit_ros::trajectory_cache::MetadataOnlyFeature<double, moveit_msgs::srv::GetCartesianPath::Request>>(
"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<TrajectoryCacheEntryPtr> 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)
Expand Down Expand Up @@ -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,
Expand All @@ -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);
}
Expand All @@ -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
Expand All @@ -983,16 +1013,22 @@ int main(int argc, char** argv)
std::vector<TrajectoryCacheEntryPtr> 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)
Expand All @@ -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,
Expand Down

0 comments on commit c7cfd22

Please sign in to comment.