Connect: ensure end-state matches goal state (#532)

This commit is contained in:
Captain Yoshi 2024-02-25 10:58:56 -05:00 committed by GitHub
parent 7638e5ff8b
commit 5720b83dce
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 40 additions and 2 deletions

View File

@ -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));
}

View File

@ -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);
}

View File

@ -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();