From 66b4381cc09ec02bc7459880b65ef87726c2aec8 Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Sun, 5 Jan 2020 18:03:44 +0100
Subject: [PATCH] Only check end effector collisions for target pose in
ComputeIK
---
core/src/stages/compute_ik.cpp | 15 +++++++++++++--
1 file changed, 13 insertions(+), 2 deletions(-)
diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp
index 32f75b9e5..2de62e47f 100644
--- a/core/src/stages/compute_ik.cpp
+++ b/core/src/stages/compute_ik.cpp
@@ -96,6 +96,7 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen:
// consider all rigidly connected parent links as well
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
+ const auto& target_links = parent->getParentJointModel()->getDescendantLinkModels();
if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
@@ -103,8 +104,8 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen:
robot_state.updateStateWithLinkAt(parent, pose);
robot_state.updateCollisionBodyTransforms();
- // disable collision checking for parent links (except links fixed to root)
- auto& acm = scene->getAllowedCollisionMatrixNonConst();
+ // disable collision checking for parent links in the kinematic chain (except links fixed to root)
+ collision_detection::AllowedCollisionMatrix acm(scene->getAllowedCollisionMatrix());
std::vector<const std::string*> pending_links; // parent link names that might be rigidly connected to root
while (parent) {
pending_links.push_back(&parent->getName());
@@ -119,6 +120,16 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen:
}
}
+ // disable collision checking between collision objects and links not part of the rigid target group
+ const auto& world_object_ids = scene->getWorld()->getObjectIds();
+ for (const auto& robot_link : scene->getRobotModel()->getLinkModelsWithCollisionGeometry()) {
+ if (std::find(target_links.begin(), target_links.end(), robot_link) == target_links.end()) {
+ for (const auto& object_id : world_object_ids) {
+ acm.setEntry(object_id, robot_link->getName(), true);
+ }
+ }
+ }
+
// check collision with the world using the padded version
collision_detection::CollisionRequest req;
collision_detection::CollisionResult result;