Skip to content

Commit

Permalink
Merge pull request #525 from Abishalini/pr-get-error-msg-from-planners
Browse files Browse the repository at this point in the history 
Propagate errors from planners.
  • Loading branch information
@Abishalini
Abishalini authored Jan 25, 2024
2 parents 57e8490 + 5a2607e commit 8a89a97
Show file tree
Hide file tree
Showing 12 changed files with 160 additions and 101 deletions.
4 changes: 2 additions & 2 deletions core/include/moveit/task_constructor/solvers/cartesian_path.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,11 +63,11 @@ class CartesianPath : public PlannerInterface

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

bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
MoveItErrorCode 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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
MoveItErrorCode 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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
Expand Down
16 changes: 9 additions & 7 deletions core/include/moveit/task_constructor/solvers/joint_interpolation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,14 +58,16 @@ class JointInterpolationPlanner : public PlannerInterface

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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;

std::string getPlannerId() const override { return std::string("JointInterpolationPlanner"); }
};
Expand Down
16 changes: 9 additions & 7 deletions core/include/moveit/task_constructor/solvers/multi_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,14 +63,16 @@ class MultiPlanner : public PlannerInterface, public std::vector<solvers::Planne

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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor
Expand Down
10 changes: 5 additions & 5 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,9 +124,9 @@ class PipelinePlanner : public PlannerInterface
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
* \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, return an appropriate error type and message explaining why it failed.
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
Expand All @@ -140,9 +140,9 @@ class PipelinePlanner : public PlannerInterface
* \param [in] timeout Maximum planning timeout for an individual stage that is using the pipeline planner in seconds
* \param [in] result Reference to the location where the created trajectory is stored if planning is successful
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
* \return MoveItErrorCode - Return MoveItErrorCodes::SUCCESS if solver found a successful solution. Otherwise, return an appropriate error type and message explaining why it failed.
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* joint_model_group, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
Expand Down Expand Up @@ -170,7 +170,7 @@ class PipelinePlanner : public PlannerInterface
* problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
MoveItErrorCode plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::JointModelGroup* joint_model_group,
const moveit_msgs::msg::Constraints& goal_constraints, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
Expand Down
22 changes: 13 additions & 9 deletions core/include/moveit/task_constructor/solvers/planner_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit/task_constructor/properties.h>
#include <moveit/utils/moveit_error_code.h>
#include <Eigen/Geometry>

namespace planning_scene {
Expand All @@ -64,6 +65,9 @@ namespace moveit {
namespace task_constructor {
namespace solvers {

using MoveItErrorCode = moveit::core::MoveItErrorCode;
using MoveItErrorCodes = moveit_msgs::msg::MoveItErrorCodes;

MOVEIT_CLASS_FORWARD(PlannerInterface);
class PlannerInterface
{
Expand All @@ -88,17 +92,17 @@ class PlannerInterface
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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
virtual MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
virtual MoveItErrorCode
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::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;

// get name of the planner
virtual std::string getPlannerId() const = 0;
Expand Down
27 changes: 16 additions & 11 deletions core/src/solvers/cartesian_path.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,26 +63,27 @@ 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::msg::Constraints& path_constraints) {
MoveItErrorCode 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::msg::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) {
RCLCPP_WARN_STREAM(LOGGER, "no unique tip for joint model group: " << jmg->getName());
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "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<double>("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::msg::Constraints& path_constraints) {
MoveItErrorCode 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::msg::Constraints& path_constraints) {
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff();

Expand Down Expand Up @@ -114,7 +115,11 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));

return achieved_fraction >= props.get<double>("min_fraction");
if (achieved_fraction < props.get<double>("min_fraction")) {
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
"Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
}
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
}
} // namespace solvers
} // namespace task_constructor
Expand Down
39 changes: 20 additions & 19 deletions core/src/solvers/joint_interpolation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,11 +59,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::msg::Constraints& /*path_constraints*/) {
MoveItErrorCode 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::msg::Constraints& /*path_constraints*/) {
const auto& props = properties();

// Get maximum joint distance
Expand All @@ -78,7 +78,7 @@ 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;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Start state in collision or not within bounds");

moveit::core::RobotState waypoint(from_state);
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
Expand All @@ -87,13 +87,14 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
result->addSuffixWayPoint(waypoint, t);

if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE,
"One of the waypoint's state is in collision or not within bounds");
}

// add goal point
result->addSuffixWayPoint(to_state, 1.0);
if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg))
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Goal state in collision or not within bounds");

auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
Expand All @@ -113,14 +114,15 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
}
}

return true;
return MoveItErrorCode(MoveItErrorCodes::SUCCESS);
}

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::msg::Constraints& path_constraints) {
MoveItErrorCode 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::msg::Constraints& path_constraints) {
timeout = std::min(timeout, properties().get<double>("timeout"));
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);

Expand All @@ -137,15 +139,14 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
} };

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
RCLCPP_WARN(LOGGER, "IK failed for pose target");
return false;
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "IK failed for pose target.");
}
to->getCurrentStateNonConst().update();

if (std::chrono::steady_clock::now() >= deadline)
return false;
if (std::chrono::steady_clock::now() >= deadline) {
return MoveItErrorCode(MoveItErrorCodes::FAILURE, "Timed out.");
}

return plan(from, to, jmg, timeout, result, path_constraints);
}
Expand Down
Loading

0 comments on commit 8a89a97

Please sign in to comment.