mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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
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:
parent
bad8e13254
commit
a0da41a4aa
@ -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;
|
||||
|
||||
@ -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) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user