Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Only check end effector collisions for target pose in ComputeIK #117

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+13 −2
Open

Only check end effector collisions for target pose in ComputeIK #117

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 13 additions & 2 deletions core/src/stages/compute_ik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,15 +96,16 @@ 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();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe move this line down to the code block that actually uses this variable?

if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);

// place link at given pose
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());
Expand All @@ -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);
}
}
}
Comment on lines +123 to +131
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As far as I understand, this PR wants to consider collisions of end-effector links with collision objects only, for the pre-check in isTargetPoseColliding(). I agree that unrelated group links (e.g. another arm) should be ignored because they will be ignored later as well.
However, collisions of links rigidly connected to the root link of the current group should be still considered, shouldn't they? That's what the code in lines 107-121 was doing. With your PR this code actually would become superfluous.


// check collision with the world using the padded version
collision_detection::CollisionRequest req;
collision_detection::CollisionResult result;
Expand Down