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.

Author: captain-yoshi

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;

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) {