Fix clamping of joint constraints (#665)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci TARGET_CMAKE_ARGS:-DC… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.5 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled

Do not enforce position bounds (by clamping to valid positions), but let the stage fail if joints are outside the limits.
This commit is contained in:
Captain Yoshi 2025-02-25 07:12:53 -05:00 committed by GitHub
parent bad8e13254
commit a0da41a4aa
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 25 additions and 1 deletions

View File

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

View File

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