From 5720b83dce00b68a3e4e2d4c835c3643afd47cf1 Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Sun, 25 Feb 2024 10:58:56 -0500
Subject: [PATCH] Connect: ensure end-state matches goal state (#532)

---
 .../moveit/task_constructor/stages/connect.h  |  1 +
 core/src/stages/connect.cpp                   | 11 ++++++-
 core/test/test_move_to.cpp                    | 30 ++++++++++++++++++-
 3 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h
index bb64dbb74..bac878fd9 100644
--- a/core/include/moveit/task_constructor/stages/connect.h
+++ b/core/include/moveit/task_constructor/stages/connect.h
@@ -76,6 +76,7 @@ class Connect : public Connecting
 	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));
 	}
diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp
index 5f71defb9..0ab68c23d 100644
--- a/core/src/stages/connect.cpp
+++ b/core/src/stages/connect.cpp
@@ -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);
 }
 
diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp
index 8e7ac023f..aaaf313eb 100644
--- a/core/test/test_move_to.cpp
+++ b/core/test/test_move_to.cpp
@@ -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();