From e09fecd71f2cadc0133693a798e679203ad7a3a0 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Tue, 16 Jan 2024 01:06:47 -0700 Subject: [PATCH] Propagate errors from planners to solution comment (#525) --- .../task_constructor/solvers/cartesian_path.h | 4 +- .../solvers/joint_interpolation.h | 14 ++--- .../task_constructor/solvers/multi_planner.h | 14 ++--- .../solvers/pipeline_planner.h | 18 ++++--- .../solvers/planner_interface.h | 26 ++++++---- core/src/solvers/cartesian_path.cpp | 30 ++++++----- core/src/solvers/joint_interpolation.cpp | 52 ++++++++++--------- core/src/solvers/multi_planner.cpp | 43 +++++++++------ core/src/solvers/pipeline_planner.cpp | 34 +++++++----- core/src/stages/connect.cpp | 9 ++-- core/src/stages/move_relative.cpp | 13 +++-- core/src/stages/move_to.cpp | 14 +++-- 12 files changed, 161 insertions(+), 110 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index d2c2c069..3a6c0604 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -63,11 +63,11 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index ae909120..52df4367 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -58,14 +58,14 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/multi_planner.h b/core/include/moveit/task_constructor/solvers/multi_planner.h index 2487dadf..d45c52d5 100644 --- a/core/include/moveit/task_constructor/solvers/multi_planner.h +++ b/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -63,14 +63,14 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 3e072e52..33099cd6 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include namespace planning_pipeline { @@ -79,16 +80,19 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; - bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; protected: + Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::MotionPlanRequest& req, + robot_trajectory::RobotTrajectoryPtr& result); + std::string pipeline_name_; planning_pipeline::PlanningPipelinePtr planner_; }; diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 67a876ee..ddc5a2ab 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -71,6 +71,14 @@ class PlannerInterface PropertyMap properties_; public: + struct Result + { + bool success; + std::string message; + + operator bool() const { return success; } + }; + PlannerInterface(); virtual ~PlannerInterface() {} @@ -88,17 +96,17 @@ public: virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; /// plan trajectory between to robot states - virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; + virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; /// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target - virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; + virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; }; } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 36c8bd89..f1507490 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -59,26 +59,25 @@ CartesianPath::CartesianPath() { void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {} -bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); - if (!link) { - ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName()); - return false; - } + if (!link) + return { false, "no unique tip for joint model group: " + jmg->getName() }; // reach pose of forward kinematics return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, std::min(timeout, properties().get("timeout")), result, path_constraints); } -bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, - const moveit::core::JointModelGroup* jmg, double /*timeout*/, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); planning_scene::PlanningScenePtr sandbox_scene = from->diff(); @@ -109,7 +108,10 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); - return achieved_fraction >= props.get("min_fraction"); + if (achieved_fraction < props.get("min_fraction")) { + return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) }; + } + return { true, "achieved fraction: " + std::to_string(achieved_fraction) }; } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 9f4937e8..ee0a7605 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -57,11 +57,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() { void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {} -bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double /*timeout*/, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& /*path_constraints*/) { +PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& /*path_constraints*/) { const auto& props = properties(); // Get maximum joint distance @@ -75,8 +75,11 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); - if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg)) - return false; + if (from->isStateColliding(from_state, jmg->getName())) + return { false, "Start state is in collision!" }; + + if (!from_state.satisfiesBounds(jmg)) + return { false, "Start state is out of bounds!" }; moveit::core::RobotState waypoint(from_state); double delta = d < 1e-6 ? 1.0 : props.get("max_step") / d; @@ -84,14 +87,20 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr from_state.interpolate(to_state, t, waypoint); result->addSuffixWayPoint(waypoint, t); - if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg)) - return false; + if (from->isStateColliding(waypoint, jmg->getName())) + return { false, "Waypoint is in collision!" }; + + if (!waypoint.satisfiesBounds(jmg)) + return { false, "Waypoint is out of bounds!" }; } // add goal point result->addSuffixWayPoint(to_state, 1.0); - if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg)) - return false; + if (from->isStateColliding(to_state, jmg->getName())) + return { false, "Goal state is in collision!" }; + + if (!to_state.satisfiesBounds(jmg)) + return { false, "Goal state is out of bounds!" }; auto timing = props.get("time_parameterization"); timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), @@ -111,14 +120,13 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } } - return true; + return { true, "" }; } -bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result JointInterpolationPlanner::plan( + const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { timeout = std::min(timeout, properties().get("timeout")); const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); @@ -134,16 +142,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return to->isStateValid(*robot_state, constraints, jmg->getName()); } }; - if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) { - // TODO(v4hn): planners need a way to add feedback to failing plans - // in case of an invalid solution feedback should include unwanted collisions or violated constraints - ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); - return false; - } + if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) + return { false, "IK failed for pose target." }; to->getCurrentStateNonConst().update(); if (std::chrono::steady_clock::now() >= deadline) - return false; + return { false, "timeout" }; return plan(from, to, jmg, timeout, result, path_constraints); } diff --git a/core/src/solvers/multi_planner.cpp b/core/src/solvers/multi_planner.cpp index 7603f764..70920608 100644 --- a/core/src/solvers/multi_planner.cpp +++ b/core/src/solvers/multi_planner.cpp @@ -49,49 +49,58 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { p->init(robot_model); } -bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); + std::string comment = "No planner specified"; for (const auto& p : *this) { if (remaining_time < 0) - return false; // timeout + return { false, "timeout" }; if (result) result->clear(); - if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) - return true; + auto r = p->plan(from, to, jmg, remaining_time, result, path_constraints); + if (r) + return r; + else + comment = r.message; auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; } - return false; + return { false, comment }; } -bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { double remaining_time = std::min(timeout, properties().get("timeout")); auto start_time = std::chrono::steady_clock::now(); + std::string comment = "No planner specified"; for (const auto& p : *this) { if (remaining_time < 0) - return false; // timeout + return { false, "timeout" }; if (result) result->clear(); - if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) - return true; + auto r = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints); + if (r) + return r; + else + comment = r.message; auto now = std::chrono::steady_clock::now(); remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; } - return false; + return { false, comment }; } } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 57fd495d..99c5912c 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -152,10 +152,11 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa req.workspace_parameters = p.get("workspace_parameters"); } -bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::MotionPlanRequest req; initMotionPlanRequest(req, props, jmg, timeout); @@ -165,17 +166,15 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, props.get("goal_joint_tolerance")); req.path_constraints = path_constraints; - ::planning_interface::MotionPlanResponse res; - bool success = planner_->generatePlan(from, req, res); - result = res.trajectory_; - return success; + return plan(from, req, result); } -bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { +PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::MotionPlanRequest req; initMotionPlanRequest(req, props, jmg, timeout); @@ -190,11 +189,18 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co props.get("goal_orientation_tolerance")); req.path_constraints = path_constraints; + return plan(from, req, result); +} + +PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const moveit_msgs::MotionPlanRequest& req, + robot_trajectory::RobotTrajectoryPtr& result) { ::planning_interface::MotionPlanResponse res; bool success = planner_->generatePlan(from, req, res); result = res.trajectory_; - return success; + return { success, success ? std::string() : static_cast(res.error_code_) }; } + } // namespace solvers } // namespace task_constructor } // namespace moveit diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0ab68c23..86dd119e 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -148,7 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(start); bool success = false; - std::string comment; + std::string comment = "No planners specified"; std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state @@ -161,11 +161,14 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(end); robot_trajectory::RobotTrajectoryPtr trajectory; - success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + auto result = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints); + success = bool(result); sub_trajectories.push_back(trajectory); // include failed trajectory - if (!success) + if (!success) { + comment = result.message; break; + } if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) { success = false; diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 7af3d58c..6833ac26 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -189,10 +189,14 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; + std::string comment = ""; if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + success = bool(result); + if (!success) + comment = result.message; } else { // Cartesian targets require an IK reference frame const moveit::core::LinkModel* link; @@ -278,8 +282,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // offset from link to ik_frame const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - success = + auto result = planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + success = bool(result); + if (!success) + comment = result.message; if (robot_trajectory) { // the following requires a robot_trajectory returned from planning moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); @@ -323,7 +330,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning solution.setTrajectory(robot_trajectory); if (!success) - solution.markAsFailure(); + solution.markAsFailure(comment); return true; } return false; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index c2721eed..ccc0fee8 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -197,10 +197,14 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; + std::string comment = ""; if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) { // plan to joint-space target - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + success = bool(result); + if (!success) + comment = result.message; } else { // Cartesian goal // Where to go? Eigen::Isometry3d target; @@ -232,7 +236,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); + const auto result = + planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); + success = bool(result); + if (!success) + comment = result.message; } // store result @@ -248,7 +256,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP solution.setTrajectory(robot_trajectory); if (!success) - solution.markAsFailure(); + solution.markAsFailure(comment); return true; }