From a0da41a4aa1f97a6da82b9cae55a024ac247ca3b Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Tue, 25 Feb 2025 07:12:53 -0500
Subject: [PATCH] Fix clamping of joint constraints (#665)

Do not enforce position bounds (by clamping to valid positions), but let the stage fail if joints are outside the limits.
---
 core/src/stages/move_relative.cpp |  1 -
 core/test/test_move_relative.cpp  | 25 +++++++++++++++++++++++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp
index 7bf065f39..bf31a2f30 100644
--- a/core/src/stages/move_relative.cpp
+++ b/core/src/stages/move_relative.cpp
@@ -94,7 +94,6 @@ static bool getJointStateFromOffset(const boost::any& direction, const Interface
 				throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
 			auto index = jm->getFirstVariableIndex();
 			robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second);
-			robot_state.enforceBounds(jm);
 		}
 		robot_state.update();
 		return true;
diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp
index 9c91ae93e..63c5d5ebb 100644
--- a/core/test/test_move_relative.cpp
+++ b/core/test/test_move_relative.cpp
@@ -4,6 +4,7 @@
 #include <moveit/task_constructor/stages/move_relative.h>
 #include <moveit/task_constructor/stages/fixed_state.h>
 #include <moveit/task_constructor/solvers/cartesian_path.h>
+#include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 
 #include <moveit/planning_scene/planning_scene.h>
@@ -32,6 +33,10 @@ solvers::CartesianPathPtr create<solvers::CartesianPath>() {
 	return std::make_shared<solvers::CartesianPath>();
 }
 template <>
+solvers::JointInterpolationPlannerPtr create<solvers::JointInterpolationPlanner>() {
+	return std::make_shared<solvers::JointInterpolationPlanner>();
+}
+template <>
 solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>() {
 	auto p = std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner");
 	p->setPlannerId("LIN");
@@ -68,6 +73,7 @@ struct PandaMoveRelative : public testing::Test
 	}
 };
 using PandaMoveRelativeCartesian = PandaMoveRelative<solvers::CartesianPath>;
+using PandaMoveRelativeJoint = PandaMoveRelative<solvers::JointInterpolationPlanner>;
 
 moveit_msgs::CollisionObject createObject(const std::string& id, const geometry_msgs::Pose& pose) {
 	moveit_msgs::CollisionObject co;
@@ -163,6 +169,25 @@ TEST_F(PandaMoveRelativeCartesian, cartesianRotateAttachedIKFrame) {
 	EXPECT_CONST_POSITION(move->solutions().front(), attached_object);
 }
 
+// https://github.com/moveit/moveit_task_constructor/issues/664
+TEST_F(PandaMoveRelativeJoint, jointOutsideBound) {
+	// move joint inside limit
+	auto initial_jpos = scene->getCurrentState().getJointPositions("panda_joint7");
+	move->setDirection([initial_jpos] {
+		return std::map<std::string, double>{ { "panda_joint7", 2.0 - *initial_jpos } };
+	}());
+	EXPECT_TRUE(this->t.plan()) << "Plan should succeed, joint inside limit";
+
+	this->t.reset();
+
+	// move joint outside limit: 2.8973
+	move->setDirection([initial_jpos] {
+		return std::map<std::string, double>{ { "panda_joint7", 3.0 - *initial_jpos } };
+	}());
+
+	EXPECT_FALSE(this->t.plan()) << "Plan should fail, joint outside limit";
+}
+
 using PlannerTypes = ::testing::Types<solvers::CartesianPath, solvers::PipelinePlanner>;
 TYPED_TEST_SUITE(PandaMoveRelative, PlannerTypes);
 TYPED_TEST(PandaMoveRelative, cartesianCollisionMinMaxDistance) {