Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Propagate errors from planners. #525

Merged
merged 7 commits into from
Jan 25, 2024
Merged

Propagate errors from planners. #525

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2ffacf8
Use tl expected
Abishalini Jan 16, 2024
823899f
Propagate errors from planners
Abishalini Jan 18, 2024
c5bf96f
Fix bug in move relative
Abishalini Jan 18, 2024
c8a75cb
tl::expected -> MoveItErrorCode
Abishalini Jan 23, 2024
32cd2b8
Remove tl::expected
Abishalini Jan 24, 2024
0d7b86e
Modify return type in comments
Abishalini Jan 24, 2024
5a2607e
Remove TODO
Abishalini Jan 24, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions core/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ find_package(moveit_task_constructor_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rviz_marker_tools REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tl_expected REQUIRED)
find_package(visualization_msgs REQUIRED)

set(CMAKE_CXX_STANDARD 17)
Expand Down Expand Up @@ -44,5 +45,6 @@ ament_export_dependencies(moveit_task_constructor_msgs)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rviz_marker_tools)
ament_export_dependencies(tf2_eigen)
ament_export_dependencies(tl_expected)
ament_export_dependencies(visualization_msgs)
ament_package()
4 changes: 2 additions & 2 deletions core/include/moveit/task_constructor/solvers/cartesian_path.h
View file
Open in desktop
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,
tl::expected<bool, std::string> 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,
tl::expected<bool, std::string> 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
Open in desktop
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;
tl::expected<bool, std::string>
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;
tl::expected<bool, std::string>
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
Open in desktop
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;
tl::expected<bool, std::string>
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;
tl::expected<bool, std::string>
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
6 changes: 3 additions & 3 deletions core/include/moveit/task_constructor/solvers/pipeline_planner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ class PipelinePlanner : public PlannerInterface
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
tl::expected<bool, std::string> 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 @@ -142,7 +142,7 @@ class PipelinePlanner : public PlannerInterface
* \param [in] path_constraints Path contraints for the planning problem
* \return true If the solver found a successful solution for the given planning problem
*/
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
tl::expected<bool, std::string> 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,
tl::expected<bool, std::string> 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
19 changes: 10 additions & 9 deletions core/include/moveit/task_constructor/solvers/planner_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit/task_constructor/properties.h>
#include <Eigen/Geometry>
#include <tl_expected/expected.hpp>

namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
Expand Down Expand Up @@ -88,17 +89,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 tl::expected<bool, std::string>
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 tl::expected<bool, std::string>
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
1 change: 1 addition & 0 deletions core/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
<depend>rclcpp</depend>
<depend>rviz_marker_tools</depend>
<depend>tf2_eigen</depend>
<depend>tl_expected</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_cmake_gmock</test_depend>
Expand Down
27 changes: 16 additions & 11 deletions core/src/solvers/cartesian_path.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,26 +63,28 @@ 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) {
tl::expected<bool, std::string> 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 tl::make_unexpected("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) {
tl::expected<bool, std::string> 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 +116,10 @@ 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 tl::make_unexpected("Min fraction not met. Achieved fraction : " + std::to_string(achieved_fraction));
}
return true;
}
} // namespace solvers
} // namespace task_constructor
Expand Down
30 changes: 15 additions & 15 deletions core/src/solvers/joint_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,11 +59,10 @@ 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*/) {
tl::expected<bool, std::string> 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 +77,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 tl::make_unexpected("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 +86,13 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
result->addSuffixWayPoint(waypoint, t);

if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
return false;
return tl::make_unexpected("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 tl::make_unexpected("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 @@ -116,11 +115,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
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::msg::Constraints& path_constraints) {
tl::expected<bool, std::string>
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 @@ -140,12 +140,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
// 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 tl::make_unexpected("IK failed for pose target");
}
to->getCurrentStateNonConst().update();

if (std::chrono::steady_clock::now() >= deadline)
return false;
return tl::make_unexpected("Timed out");

return plan(from, to, jmg, timeout, result, path_constraints);
}
Expand Down
20 changes: 11 additions & 9 deletions core/src/solvers/multi_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,11 @@ 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::msg::Constraints& path_constraints) {
tl::expected<bool, std::string> 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::msg::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();

Expand All @@ -71,11 +72,12 @@ bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
return false;
}

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::msg::Constraints& path_constraints) {
tl::expected<bool, std::string> 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::msg::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();

Expand Down
Loading
Loading