mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Connect: ensure end-state matches goal state (#532)
This commit is contained in:
parent
7638e5ff8b
commit
5720b83dce
@ -76,6 +76,7 @@ public:
|
||||
using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
|
||||
Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
|
||||
|
||||
void setMaxDistance(double max_distance) { setProperty("max_distance", max_distance); }
|
||||
void setPathConstraints(moveit_msgs::Constraints path_constraints) {
|
||||
setProperty("path_constraints", std::move(path_constraints));
|
||||
}
|
||||
|
||||
@ -55,6 +55,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
|
||||
|
||||
auto& p = properties();
|
||||
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
|
||||
p.declare<double>("max_distance", 1e-4, "maximally accepted distance between end and goal sate");
|
||||
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
||||
"constraints to maintain during trajectory");
|
||||
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
|
||||
@ -136,6 +137,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
const auto& props = properties();
|
||||
double timeout = this->timeout();
|
||||
MergeMode mode = props.get<MergeMode>("merge_mode");
|
||||
double max_distance = props.get<double>("max_distance");
|
||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||
|
||||
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
|
||||
@ -146,6 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
intermediate_scenes.push_back(start);
|
||||
|
||||
bool success = false;
|
||||
std::string comment;
|
||||
std::vector<double> positions;
|
||||
for (const GroupPlannerVector::value_type& pair : planner_) {
|
||||
// set intermediate goal state
|
||||
@ -164,6 +167,12 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
if (!success)
|
||||
break;
|
||||
|
||||
if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) {
|
||||
success = false;
|
||||
comment = "Trajectory end-point deviates too much from goal state";
|
||||
break;
|
||||
}
|
||||
|
||||
// continue from reached state
|
||||
start = end;
|
||||
}
|
||||
@ -174,7 +183,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
if (!solution) // success == false or merging failed: store sequentially
|
||||
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
|
||||
if (!success) // error during sequential planning
|
||||
solution->markAsFailure();
|
||||
solution->markAsFailure(comment);
|
||||
connect(from, to, solution);
|
||||
}
|
||||
|
||||
|
||||
@ -3,8 +3,10 @@
|
||||
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -149,7 +151,33 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
// This test require a running rosmaster
|
||||
// Using a Cartesian interpolation planner targeting a joint-space goal, which is
|
||||
// transformed into a Cartesian goal by FK, should fail if the two poses are on different
|
||||
// IK solution branches. In this case, the end-state, although reaching the Cartesian goal,
|
||||
// will strongly deviate from the joint-space goal.
|
||||
TEST(Panda, connectCartesianBranchesFails) {
|
||||
Task t;
|
||||
t.setRobotModel(loadModel());
|
||||
auto scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||
scene->getCurrentStateNonConst().setToDefaultValues();
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
|
||||
t.add(std::make_unique<stages::FixedState>("start", scene));
|
||||
|
||||
stages::Connect::GroupPlannerVector planner = { { "panda_arm", std::make_shared<solvers::CartesianPath>() } };
|
||||
t.add(std::make_unique<stages::Connect>("connect", planner));
|
||||
|
||||
// target an elbow-left instead of an elbow-right solution (different solution branch)
|
||||
scene = scene->diff();
|
||||
scene->getCurrentStateNonConst().setJointGroupPositions(
|
||||
"panda_arm", std::vector<double>({ 2.72, 0.78, -2.63, -2.35, 0.36, 1.57, 0.48 }));
|
||||
|
||||
t.add(std::make_unique<stages::FixedState>("end", scene));
|
||||
EXPECT_FALSE(t.plan());
|
||||
EXPECT_STREQ(t.findChild("connect")->failures().front()->comment().c_str(),
|
||||
"Trajectory end-point deviates too much from goal state");
|
||||
}
|
||||
|
||||
// This test requires a running rosmaster
|
||||
TEST(Task, taskMoveConstructor) {
|
||||
auto create_task = [] {
|
||||
moveit::core::RobotModelConstPtr robot_model = getModel();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user