mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Propagate errors from planners to solution comment (#525)
This commit is contained in:
parent
0827a0d260
commit
e09fecd71f
@ -63,11 +63,11 @@ public:
|
|||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
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::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
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,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||||
|
|||||||
@ -58,14 +58,14 @@ public:
|
|||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
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 core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
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,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||||
};
|
};
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -63,14 +63,14 @@ public:
|
|||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
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::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
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,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||||
};
|
};
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -39,6 +39,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||||
|
#include <moveit_msgs/MotionPlanRequest.h>
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
namespace planning_pipeline {
|
namespace planning_pipeline {
|
||||||
@ -79,16 +80,19 @@ public:
|
|||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
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 core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
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,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::MotionPlanRequest& req,
|
||||||
|
robot_trajectory::RobotTrajectoryPtr& result);
|
||||||
|
|
||||||
std::string pipeline_name_;
|
std::string pipeline_name_;
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -71,6 +71,14 @@ class PlannerInterface
|
|||||||
PropertyMap properties_;
|
PropertyMap properties_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
struct Result
|
||||||
|
{
|
||||||
|
bool success;
|
||||||
|
std::string message;
|
||||||
|
|
||||||
|
operator bool() const { return success; }
|
||||||
|
};
|
||||||
|
|
||||||
PlannerInterface();
|
PlannerInterface();
|
||||||
virtual ~PlannerInterface() {}
|
virtual ~PlannerInterface() {}
|
||||||
|
|
||||||
@ -88,17 +96,17 @@ public:
|
|||||||
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
|
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
|
||||||
|
|
||||||
/// plan trajectory between to robot states
|
/// plan trajectory between to robot states
|
||||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
|
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
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
|
/// 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,
|
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||||
};
|
};
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -59,26 +59,25 @@ CartesianPath::CartesianPath() {
|
|||||||
|
|
||||||
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
||||||
|
|
||||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
|
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints) {
|
||||||
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
|
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
|
||||||
if (!link) {
|
if (!link)
|
||||||
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
|
return { false, "no unique tip for joint model group: " + jmg->getName() };
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// reach pose of forward kinematics
|
// reach pose of forward kinematics
|
||||||
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
|
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
|
||||||
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
|
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
||||||
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
const moveit_msgs::Constraints& path_constraints) {
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
|
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<double>("max_velocity_scaling_factor"),
|
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||||
props.get<double>("max_acceleration_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 { false, "min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
|
||||||
|
}
|
||||||
|
return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
|
||||||
}
|
}
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -57,11 +57,11 @@ JointInterpolationPlanner::JointInterpolationPlanner() {
|
|||||||
|
|
||||||
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
||||||
|
|
||||||
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& /*path_constraints*/) {
|
const moveit_msgs::Constraints& /*path_constraints*/) {
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
|
|
||||||
// Get maximum joint distance
|
// Get maximum joint distance
|
||||||
@ -75,8 +75,11 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
|||||||
|
|
||||||
// add first point
|
// add first point
|
||||||
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
||||||
if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg))
|
if (from->isStateColliding(from_state, jmg->getName()))
|
||||||
return false;
|
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);
|
moveit::core::RobotState waypoint(from_state);
|
||||||
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
|
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
|
||||||
@ -84,14 +87,20 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
|||||||
from_state.interpolate(to_state, t, waypoint);
|
from_state.interpolate(to_state, t, waypoint);
|
||||||
result->addSuffixWayPoint(waypoint, t);
|
result->addSuffixWayPoint(waypoint, t);
|
||||||
|
|
||||||
if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
|
if (from->isStateColliding(waypoint, jmg->getName()))
|
||||||
return false;
|
return { false, "Waypoint is in collision!" };
|
||||||
|
|
||||||
|
if (!waypoint.satisfiesBounds(jmg))
|
||||||
|
return { false, "Waypoint is out of bounds!" };
|
||||||
}
|
}
|
||||||
|
|
||||||
// add goal point
|
// add goal point
|
||||||
result->addSuffixWayPoint(to_state, 1.0);
|
result->addSuffixWayPoint(to_state, 1.0);
|
||||||
if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg))
|
if (from->isStateColliding(to_state, jmg->getName()))
|
||||||
return false;
|
return { false, "Goal state is in collision!" };
|
||||||
|
|
||||||
|
if (!to_state.satisfiesBounds(jmg))
|
||||||
|
return { false, "Goal state is out of bounds!" };
|
||||||
|
|
||||||
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
|
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
|
||||||
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
timing->computeTimeStamps(*result, props.get<double>("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,
|
PlannerInterface::Result JointInterpolationPlanner::plan(
|
||||||
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) {
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
|
||||||
timeout = std::min(timeout, properties().get<double>("timeout"));
|
timeout = std::min(timeout, properties().get<double>("timeout"));
|
||||||
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
|
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
|
||||||
|
|
||||||
@ -134,16 +142,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
|||||||
return to->isStateValid(*robot_state, constraints, jmg->getName());
|
return to->isStateValid(*robot_state, constraints, jmg->getName());
|
||||||
} };
|
} };
|
||||||
|
|
||||||
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
|
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
|
return { false, "IK failed for pose target." };
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
to->getCurrentStateNonConst().update();
|
to->getCurrentStateNonConst().update();
|
||||||
|
|
||||||
if (std::chrono::steady_clock::now() >= deadline)
|
if (std::chrono::steady_clock::now() >= deadline)
|
||||||
return false;
|
return { false, "timeout" };
|
||||||
|
|
||||||
return plan(from, to, jmg, timeout, result, path_constraints);
|
return plan(from, to, jmg, timeout, result, path_constraints);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -49,49 +49,58 @@ void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
|
|||||||
p->init(robot_model);
|
p->init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints) {
|
||||||
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
|
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
|
||||||
auto start_time = std::chrono::steady_clock::now();
|
auto start_time = std::chrono::steady_clock::now();
|
||||||
|
|
||||||
|
std::string comment = "No planner specified";
|
||||||
for (const auto& p : *this) {
|
for (const auto& p : *this) {
|
||||||
if (remaining_time < 0)
|
if (remaining_time < 0)
|
||||||
return false; // timeout
|
return { false, "timeout" };
|
||||||
if (result)
|
if (result)
|
||||||
result->clear();
|
result->clear();
|
||||||
if (p->plan(from, to, jmg, remaining_time, result, path_constraints))
|
auto r = p->plan(from, to, jmg, remaining_time, result, path_constraints);
|
||||||
return true;
|
if (r)
|
||||||
|
return r;
|
||||||
|
else
|
||||||
|
comment = r.message;
|
||||||
|
|
||||||
auto now = std::chrono::steady_clock::now();
|
auto now = std::chrono::steady_clock::now();
|
||||||
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
||||||
start_time = now;
|
start_time = now;
|
||||||
}
|
}
|
||||||
return false;
|
return { false, comment };
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
||||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
const moveit_msgs::Constraints& path_constraints) {
|
||||||
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
|
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
|
||||||
auto start_time = std::chrono::steady_clock::now();
|
auto start_time = std::chrono::steady_clock::now();
|
||||||
|
|
||||||
|
std::string comment = "No planner specified";
|
||||||
for (const auto& p : *this) {
|
for (const auto& p : *this) {
|
||||||
if (remaining_time < 0)
|
if (remaining_time < 0)
|
||||||
return false; // timeout
|
return { false, "timeout" };
|
||||||
if (result)
|
if (result)
|
||||||
result->clear();
|
result->clear();
|
||||||
if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints))
|
auto r = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints);
|
||||||
return true;
|
if (r)
|
||||||
|
return r;
|
||||||
|
else
|
||||||
|
comment = r.message;
|
||||||
|
|
||||||
auto now = std::chrono::steady_clock::now();
|
auto now = std::chrono::steady_clock::now();
|
||||||
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
||||||
start_time = now;
|
start_time = now;
|
||||||
}
|
}
|
||||||
return false;
|
return { false, comment };
|
||||||
}
|
}
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
|
|||||||
@ -152,10 +152,11 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
|
|||||||
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
|
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
|
const planning_scene::PlanningSceneConstPtr& to,
|
||||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints) {
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
moveit_msgs::MotionPlanRequest req;
|
moveit_msgs::MotionPlanRequest req;
|
||||||
initMotionPlanRequest(req, props, jmg, timeout);
|
initMotionPlanRequest(req, props, jmg, timeout);
|
||||||
@ -165,17 +166,15 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
|||||||
props.get<double>("goal_joint_tolerance"));
|
props.get<double>("goal_joint_tolerance"));
|
||||||
req.path_constraints = path_constraints;
|
req.path_constraints = path_constraints;
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
return plan(from, req, result);
|
||||||
bool success = planner_->generatePlan(from, req, res);
|
|
||||||
result = res.trajectory_;
|
|
||||||
return success;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
|
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
||||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
const Eigen::Isometry3d& target_eigen,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result,
|
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||||
const moveit_msgs::Constraints& path_constraints) {
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints) {
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
moveit_msgs::MotionPlanRequest req;
|
moveit_msgs::MotionPlanRequest req;
|
||||||
initMotionPlanRequest(req, props, jmg, timeout);
|
initMotionPlanRequest(req, props, jmg, timeout);
|
||||||
@ -190,11 +189,18 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
|
|||||||
props.get<double>("goal_orientation_tolerance"));
|
props.get<double>("goal_orientation_tolerance"));
|
||||||
req.path_constraints = path_constraints;
|
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;
|
::planning_interface::MotionPlanResponse res;
|
||||||
bool success = planner_->generatePlan(from, req, res);
|
bool success = planner_->generatePlan(from, req, res);
|
||||||
result = res.trajectory_;
|
result = res.trajectory_;
|
||||||
return success;
|
return { success, success ? std::string() : static_cast<std::string>(res.error_code_) };
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|||||||
@ -148,7 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
|||||||
intermediate_scenes.push_back(start);
|
intermediate_scenes.push_back(start);
|
||||||
|
|
||||||
bool success = false;
|
bool success = false;
|
||||||
std::string comment;
|
std::string comment = "No planners specified";
|
||||||
std::vector<double> positions;
|
std::vector<double> positions;
|
||||||
for (const GroupPlannerVector::value_type& pair : planner_) {
|
for (const GroupPlannerVector::value_type& pair : planner_) {
|
||||||
// set intermediate goal state
|
// set intermediate goal state
|
||||||
@ -161,11 +161,14 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
|||||||
intermediate_scenes.push_back(end);
|
intermediate_scenes.push_back(end);
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
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
|
sub_trajectories.push_back(trajectory); // include failed trajectory
|
||||||
|
|
||||||
if (!success)
|
if (!success) {
|
||||||
|
comment = result.message;
|
||||||
break;
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) {
|
if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) {
|
||||||
success = false;
|
success = false;
|
||||||
|
|||||||
@ -189,10 +189,14 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
|
std::string comment = "";
|
||||||
|
|
||||||
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
||||||
// plan to joint-space target
|
// 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 {
|
} else {
|
||||||
// Cartesian targets require an IK reference frame
|
// Cartesian targets require an IK reference frame
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
@ -278,8 +282,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
// offset from link to ik_frame
|
// offset from link to ik_frame
|
||||||
const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
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);
|
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
|
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
|
||||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
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);
|
solution.setTrajectory(robot_trajectory);
|
||||||
|
|
||||||
if (!success)
|
if (!success)
|
||||||
solution.markAsFailure();
|
solution.markAsFailure(comment);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
|
|||||||
@ -197,10 +197,14 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
|||||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
|
std::string comment = "";
|
||||||
|
|
||||||
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
||||||
// plan to joint-space target
|
// 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
|
} else { // Cartesian goal
|
||||||
// Where to go?
|
// Where to go?
|
||||||
Eigen::Isometry3d target;
|
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;
|
Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||||
|
|
||||||
// plan to Cartesian target
|
// 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
|
// store result
|
||||||
@ -248,7 +256,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
|||||||
solution.setTrajectory(robot_trajectory);
|
solution.setTrajectory(robot_trajectory);
|
||||||
|
|
||||||
if (!success)
|
if (!success)
|
||||||
solution.markAsFailure();
|
solution.markAsFailure(comment);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user