From 61bb2fdc58e3d7e550c57617937a3d5286e421ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 28 Feb 2023 13:18:53 +0100 Subject: [PATCH] ComputeIK: Limit collision checking to JMG (#428) That's what MoveIt is doing as well. --- core/src/stages/compute_ik.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 1f793f77..5ce320b4 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -99,7 +99,7 @@ namespace { // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, robot_state::RobotState& robot_state, Eigen::Isometry3d pose, - const robot_model::LinkModel* link, + const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); @@ -130,6 +130,8 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce collision_detection::CollisionRequest req; collision_detection::CollisionResult result; req.contacts = (collision_result != nullptr); + if (jmg) + req.group_name = jmg->getName(); collision_detection::CollisionResult& res = collision_result ? *collision_result : result; scene->checkCollision(req, res, robot_state, acm); return res.collision; @@ -311,7 +313,7 @@ void ComputeIK::compute() { collision_detection::CollisionResult collisions; robot_state::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = - !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); + !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions); // frames at target pose and ik frame std::deque frame_markers; @@ -371,6 +373,7 @@ void ComputeIK::compute() { collision_detection::CollisionResult res; req.contacts = true; req.max_contacts = 1; + req.group_name = jmg->getName(); scene->checkCollision(req, res, *state); solution.feasible = ignore_collisions || !res.collision; if (!res.contacts.empty()) {