From 0a318ca9ec64b748365396cc65a62a86e8a77cee Mon Sep 17 00:00:00 2001 From: Shobuj Paul Date: Wed, 21 Feb 2024 23:47:33 +0530 Subject: [PATCH 1/3] Implemented stomp::solve MotionPlanDetailedResponse similar to chomp --- .../src/stomp_moveit_planning_context.cpp | 72 +++++++++++++++++-- 1 file changed, 67 insertions(+), 5 deletions(-) diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index f5895ed0d3..d61fcbcf64 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -277,12 +277,74 @@ void StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) res.planning_time = elapsed_seconds.count(); } -void StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& /*res*/) +void StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { - // TODO(#2168): implement this function - RCLCPP_ERROR(getLogger(), - "StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&) is not implemented!"); - return; + // Start time + auto time_start = std::chrono::steady_clock::now(); + + res.planner_id = std::string("stomp"); + // Default to happy path + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + + // Extract start and goal states + const auto& req = getMotionPlanRequest(); + const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); + moveit::core::RobotState goal_state(start_state); + constraint_samplers::ConstraintSamplerManager sampler_manager; + auto goal_sampler = sampler_manager.selectSampler(getPlanningScene(), getGroupName(), req.goal_constraints.at(0)); + if (!goal_sampler || !goal_sampler->sample(goal_state)) + { + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return; // Can't plan without valid goal state + } + + // STOMP config, task, planner instance + const auto group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); + auto config = getStompConfig(params_, group->getActiveJointModels().size() /* num_dimensions */); + robot_trajectory::RobotTrajectoryPtr input_trajectory; + if (extractSeedTrajectory(request_, getPlanningScene()->getRobotModel(), input_trajectory)) + { + config.num_timesteps = input_trajectory->size(); + } + const auto task = createStompTask(config, *this); + stomp_ = std::make_shared(config, task); + + std::condition_variable cv; + std::mutex cv_mutex; + bool finished = false; + auto timeout_future = std::async(std::launch::async, [&, stomp = stomp_]() { + std::unique_lock lock(cv_mutex); + cv.wait_for(lock, std::chrono::duration(req.allowed_planning_time), [&finished] { return finished; }); + if (!finished) + { + stomp->cancel(); + } + }); + + // Solve + res.trajectory.resize(1); + if (!solveWithStomp(stomp_, start_state, goal_state, group, input_trajectory, res.trajectory[0])) + { + // We timed out if the timeout task has completed so that the timeout future is valid and ready + bool timed_out = + timeout_future.valid() && timeout_future.wait_for(std::chrono::nanoseconds(1)) == std::future_status::ready; + res.error_code.val = + timed_out ? moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT : moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + } + stomp_.reset(); + { + std::unique_lock lock(cv_mutex); + finished = true; + cv.notify_all(); + } + // Description + res.description.resize(1); + res.description[0] = "plan"; + + // Stop time + res.processing_time.resize(1); + std::chrono::duration elapsed_seconds = std::chrono::steady_clock::now() - time_start; + res.processing_time[0] = elapsed_seconds.count(); } bool StompPlanningContext::terminate() From af3d462f697053c19b2bff5e09a7d4d1dab45804 Mon Sep 17 00:00:00 2001 From: Shobuj Paul Date: Fri, 23 Feb 2024 17:26:12 +0530 Subject: [PATCH 2/3] Updated MIGRATION.md --- MIGRATION.md | 1 + 1 file changed, 1 insertion(+) diff --git a/MIGRATION.md b/MIGRATION.md index 9af12e14c4..5772d599b3 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,7 @@ API changes in MoveIt releases ## ROS Rolling +- [2/2024] `StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&)` has been implemented which gives a detailed response with multiple trajectories and their corresponding planning times. - [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse. The constructors have been replaced by builder methods so that errors can be communicated. Paths and trajectories now need to be created with `Path::Create()` and `Trajectory::Create()`. These methods now return an `std::optional` that needs to be checked for a valid value. `Trajectory` no longer has the `isValid()` method. If it's invalid, `Trajectory::Create()` will return `std::nullopt`. Finally, `Path` now takes the list of input waypoints as `std::vector`, instead of `std::list`. - [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK From 8aedb64206a07badb4560f79b35beb23ec2f1b41 Mon Sep 17 00:00:00 2001 From: Shobuj Paul Date: Sat, 2 Mar 2024 22:52:19 +0530 Subject: [PATCH 3/3] Stored trajectories for multiple iterations in res.trajectories vector --- .../src/stomp_moveit_planning_context.cpp | 125 ++++++++++-------- 1 file changed, 68 insertions(+), 57 deletions(-) diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index d61fcbcf64..bcb7ab7205 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -279,72 +279,83 @@ void StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) void StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { - // Start time - auto time_start = std::chrono::steady_clock::now(); - - res.planner_id = std::string("stomp"); - // Default to happy path - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - - // Extract start and goal states - const auto& req = getMotionPlanRequest(); - const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); - moveit::core::RobotState goal_state(start_state); - constraint_samplers::ConstraintSamplerManager sampler_manager; - auto goal_sampler = sampler_manager.selectSampler(getPlanningScene(), getGroupName(), req.goal_constraints.at(0)); - if (!goal_sampler || !goal_sampler->sample(goal_state)) - { - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return; // Can't plan without valid goal state - } - // STOMP config, task, planner instance const auto group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); auto config = getStompConfig(params_, group->getActiveJointModels().size() /* num_dimensions */); - robot_trajectory::RobotTrajectoryPtr input_trajectory; - if (extractSeedTrajectory(request_, getPlanningScene()->getRobotModel(), input_trajectory)) + + // Reset num_iterations in config to go through and store each trajectory at each iteration + int num_iterations = config.num_iterations; + config.num_iterations = 0; + res.trajectory.resize(num_iterations); + res.description.resize(num_iterations); + res.processing_time.resize(num_iterations); + + for (int i = 0; i < num_iterations; i++) { - config.num_timesteps = input_trajectory->size(); - } - const auto task = createStompTask(config, *this); - stomp_ = std::make_shared(config, task); + // Increase number of iterations for each successive trajectory + config.num_iterations++; + + // Start time + auto time_start = std::chrono::steady_clock::now(); + + res.planner_id = std::string("stomp"); + // Default to happy path + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + + // Extract start and goal states + const auto& req = getMotionPlanRequest(); + const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); + moveit::core::RobotState goal_state(start_state); + constraint_samplers::ConstraintSamplerManager sampler_manager; + auto goal_sampler = sampler_manager.selectSampler(getPlanningScene(), getGroupName(), req.goal_constraints.at(0)); + if (!goal_sampler || !goal_sampler->sample(goal_state)) + { + res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; + return; // Can't plan without valid goal state + } - std::condition_variable cv; - std::mutex cv_mutex; - bool finished = false; - auto timeout_future = std::async(std::launch::async, [&, stomp = stomp_]() { - std::unique_lock lock(cv_mutex); - cv.wait_for(lock, std::chrono::duration(req.allowed_planning_time), [&finished] { return finished; }); - if (!finished) + robot_trajectory::RobotTrajectoryPtr input_trajectory; + if (extractSeedTrajectory(request_, getPlanningScene()->getRobotModel(), input_trajectory)) { - stomp->cancel(); + config.num_timesteps = input_trajectory->size(); } - }); + const auto task = createStompTask(config, *this); + stomp_ = std::make_shared(config, task); + + std::condition_variable cv; + std::mutex cv_mutex; + bool finished = false; + auto timeout_future = std::async(std::launch::async, [&, stomp = stomp_]() { + std::unique_lock lock(cv_mutex); + cv.wait_for(lock, std::chrono::duration(req.allowed_planning_time), [&finished] { return finished; }); + if (!finished) + { + stomp->cancel(); + } + }); - // Solve - res.trajectory.resize(1); - if (!solveWithStomp(stomp_, start_state, goal_state, group, input_trajectory, res.trajectory[0])) - { - // We timed out if the timeout task has completed so that the timeout future is valid and ready - bool timed_out = - timeout_future.valid() && timeout_future.wait_for(std::chrono::nanoseconds(1)) == std::future_status::ready; - res.error_code.val = - timed_out ? moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT : moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; - } - stomp_.reset(); - { - std::unique_lock lock(cv_mutex); - finished = true; - cv.notify_all(); - } - // Description - res.description.resize(1); - res.description[0] = "plan"; + // Solve + if (!solveWithStomp(stomp_, start_state, goal_state, group, input_trajectory, res.trajectory[i])) + { + // We timed out if the timeout task has completed so that the timeout future is valid and ready + bool timed_out = + timeout_future.valid() && timeout_future.wait_for(std::chrono::nanoseconds(1)) == std::future_status::ready; + res.error_code.val = timed_out ? moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT : + moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + } + stomp_.reset(); + { + std::unique_lock lock(cv_mutex); + finished = true; + cv.notify_all(); + } + // Description + res.description[i] = "plan_" + std::to_string(i); - // Stop time - res.processing_time.resize(1); - std::chrono::duration elapsed_seconds = std::chrono::steady_clock::now() - time_start; - res.processing_time[0] = elapsed_seconds.count(); + // Stop time + std::chrono::duration elapsed_seconds = std::chrono::steady_clock::now() - time_start; + res.processing_time[i] = elapsed_seconds.count(); + } } bool StompPlanningContext::terminate()