Skip to content

Commit e09fecd

Browse files
Abishalinirhaschke
Abishalini
authored and
rhaschke
committed
Propagate errors from planners to solution comment (moveit#525)
1 parent 0827a0d commit e09fecd

File tree

12 files changed

+161
-110
lines changed

12 files changed

+161
-110
lines changed

‎core/include/moveit/task_constructor/solvers/cartesian_path.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface
6363

6464
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6565

66-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
66+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
6767
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
6868
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
6969

70-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
70+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
7171
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
7272
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
7373
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;

‎core/include/moveit/task_constructor/solvers/joint_interpolation.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -58,14 +58,14 @@ class JointInterpolationPlanner : public PlannerInterface
5858

5959
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6060

61-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
62-
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
63-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
61+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
62+
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
63+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
6464

65-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
66-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
67-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
65+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
66+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
67+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
6969
};
7070
} // namespace solvers
7171
} // namespace task_constructor

‎core/include/moveit/task_constructor/solvers/multi_planner.h‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -63,14 +63,14 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne
6363

6464
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
6565

66-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67-
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
66+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
67+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
68+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
6969

70-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
72-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
70+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
71+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
72+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
73+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
7474
};
7575
} // namespace solvers
7676
} // namespace task_constructor

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#pragma once
4040

4141
#include <moveit/task_constructor/solvers/planner_interface.h>
42+
#include <moveit_msgs/MotionPlanRequest.h>
4243
#include <moveit/macros/class_forward.h>
4344

4445
namespace planning_pipeline {
@@ -79,16 +80,19 @@ class PipelinePlanner : public PlannerInterface
7980

8081
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
8182

82-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
83-
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
84-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
83+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
84+
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
85+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
8586

86-
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
87-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
88-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
89-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
87+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
88+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
89+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
90+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
9091

9192
protected:
93+
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::MotionPlanRequest& req,
94+
robot_trajectory::RobotTrajectoryPtr& result);
95+
9296
std::string pipeline_name_;
9397
planning_pipeline::PlanningPipelinePtr planner_;
9498
};

‎core/include/moveit/task_constructor/solvers/planner_interface.h‎

Lines changed: 17 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,14 @@ class PlannerInterface
7171
PropertyMap properties_;
7272

7373
public:
74+
struct Result
75+
{
76+
bool success;
77+
std::string message;
78+
79+
operator bool() const { return success; }
80+
};
81+
7482
PlannerInterface();
7583
virtual ~PlannerInterface() {}
7684

@@ -88,17 +96,17 @@ class PlannerInterface
8896
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
8997

9098
/// plan trajectory between to robot states
91-
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
92-
const moveit::core::JointModelGroup* jmg, double timeout,
93-
robot_trajectory::RobotTrajectoryPtr& result,
94-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
99+
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
100+
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
101+
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
102+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
95103

96104
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
97-
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
98-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
99-
const moveit::core::JointModelGroup* jmg, double timeout,
100-
robot_trajectory::RobotTrajectoryPtr& result,
101-
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
105+
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
106+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
107+
const moveit::core::JointModelGroup* jmg, double timeout,
108+
robot_trajectory::RobotTrajectoryPtr& result,
109+
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
102110
};
103111
} // namespace solvers
104112
} // namespace task_constructor

‎core/src/solvers/cartesian_path.cpp‎

Lines changed: 16 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -59,26 +59,25 @@ CartesianPath::CartesianPath() {
5959

6060
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
6161

62-
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
63-
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
64-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
65-
const moveit_msgs::Constraints& path_constraints) {
62+
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
63+
const planning_scene::PlanningSceneConstPtr& to,
64+
const moveit::core::JointModelGroup* jmg, double timeout,
65+
robot_trajectory::RobotTrajectoryPtr& result,
66+
const moveit_msgs::Constraints& path_constraints) {
6667
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
67-
if (!link) {
68-
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
69-
return false;
70-
}
68+
if (!link)
69+
return { false, "no unique tip for joint model group: " + jmg->getName() };
7170

7271
// reach pose of forward kinematics
7372
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
7473
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
7574
}
7675

77-
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
78-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
79-
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
80-
robot_trajectory::RobotTrajectoryPtr& result,
81-
const moveit_msgs::Constraints& path_constraints) {
76+
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
77+
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
78+
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
79+
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
80+
const moveit_msgs::Constraints& path_constraints) {
8281
const auto& props = properties();
8382
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
8483

@@ -109,7 +108,10 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
109108
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
110109
props.get<double>("max_acceleration_scaling_factor"));
111110

112-
return achieved_fraction >= props.get<double>("min_fraction");
111+
if (achieved_fraction < props.get<double>("min_fraction")) {
112+
return { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
113+
}
114+
return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
113115
}
114116
} // namespace solvers
115117
} // namespace task_constructor

‎core/src/solvers/joint_interpolation.cpp‎

Lines changed: 28 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -57,11 +57,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() {
5757

5858
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
5959

60-
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
61-
const planning_scene::PlanningSceneConstPtr& to,
62-
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
63-
robot_trajectory::RobotTrajectoryPtr& result,
64-
const moveit_msgs::Constraints& /*path_constraints*/) {
60+
PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
61+
const planning_scene::PlanningSceneConstPtr& to,
62+
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
63+
robot_trajectory::RobotTrajectoryPtr& result,
64+
const moveit_msgs::Constraints& /*path_constraints*/) {
6565
const auto& props = properties();
6666

6767
// Get maximum joint distance
@@ -75,23 +75,32 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
7575

7676
// add first point
7777
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
78-
if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
79-
return false;
78+
if (from->isStateColliding(from_state, jmg->getName()))
79+
return { false, "Start state is in collision!" };
80+
81+
if (!from_state.satisfiesBounds(jmg))
82+
return { false, "Start state is out of bounds!" };
8083

8184
moveit::core::RobotState waypoint(from_state);
8285
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
8386
for (double t = delta; t < 1.0; t += delta) { // NOLINT(clang-analyzer-security.FloatLoopCounter)
8487
from_state.interpolate(to_state, t, waypoint);
8588
result->addSuffixWayPoint(waypoint, t);
8689

87-
if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
88-
return false;
90+
if (from->isStateColliding(waypoint, jmg->getName()))
91+
return { false, "Waypoint is in collision!" };
92+
93+
if (!waypoint.satisfiesBounds(jmg))
94+
return { false, "Waypoint is out of bounds!" };
8995
}
9096

9197
// add goal point
9298
result->addSuffixWayPoint(to_state, 1.0);
93-
if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg))
94-
return false;
99+
if (from->isStateColliding(to_state, jmg->getName()))
100+
return { false, "Goal state is in collision!" };
101+
102+
if (!to_state.satisfiesBounds(jmg))
103+
return { false, "Goal state is out of bounds!" };
95104

96105
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
97106
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
@@ -111,14 +120,13 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
111120
}
112121
}
113122

114-
return true;
123+
return { true, "" };
115124
}
116125

117-
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
118-
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
119-
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
120-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
121-
const moveit_msgs::Constraints& path_constraints) {
126+
PlannerInterface::Result JointInterpolationPlanner::plan(
127+
const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
128+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
129+
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) {
122130
timeout = std::min(timeout, properties().get<double>("timeout"));
123131
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
124132

@@ -134,16 +142,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
134142
return to->isStateValid(*robot_state, constraints, jmg->getName());
135143
} };
136144

137-
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
138-
// TODO(v4hn): planners need a way to add feedback to failing plans
139-
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
140-
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
141-
return false;
142-
}
145+
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid))
146+
return { false, "IK failed for pose target." };
143147
to->getCurrentStateNonConst().update();
144148

145149
if (std::chrono::steady_clock::now() >= deadline)
146-
return false;
150+
return { false, "timeout" };
147151

148152
return plan(from, to, jmg, timeout, result, path_constraints);
149153
}

0 commit comments

Comments
 (0)