Only check end effector collisions for target pose in ComputeIK #117
Conversation
Codecov Report
@@ Coverage Diff @@
## master #117 +/- ##
==========================================
- Coverage 26.87% 26.84% -0.03%
==========================================
Files 90 90
Lines 5752 5758 +6
==========================================
Hits 1546 1546
- Misses 4206 4212 +6
Continue to review full report at Codecov.
|
There was a problem hiding this comment.
Could you please elaborate which issue you want to tackle with this PR?
At first sight, it doesn't make sense to me to restrict collision checking to the end-effector.
If there are collisions with the arm, the IK solution is not feasible, is it?
Also, there is a much better approach to restrict collision planning to a subset of links:
https://github.com/ros-planning/moveit/blob/844a0212865d66541a3d6dc6ac6abef23b0da526/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h#L275-L276
This is just the pre-check for seeing if the target pose itself is in collision (not the IK solution). The actual collision check inside the IK validation callback is not affected and would still detect collisions in the arm, therefore all IK solutions are feasible. The solved issue is simply that ComputeIK detects collisions in static links and secondary groups (i.e. other groups than the ones it should run IK for) only because of added padding. IMO ComputeIK should only "care" for collisions in the groups it was initiated with. The method you linked is a distance request. I wasn't aware that you could run this without computing the distances. |
I missed this fact. Sorry.
Are those unrelated links considered without padding in all remaining checks? But, your implementation is still sub optimal: Instead of first checking all possible collision pairs and subsequently filtering out unrelated ones, you should restrict collision checking to the relevant pairs in first place. |
|
@henningkayser, @rhaschke's suggestion of setting up the ACM correctly before the collision check seems reasonable to me. Can you make this change? |
henningkayser
force-pushed
the
pr-compute_ik_target_col_check
branch
from
35a7aa3
to
366b7fa
Compare
|
@rhaschke @mamoll I updated this PR to only modify the ACM instead of checking the colliding links afterwards. Collisions between rigidly connected root links and scene objects are now allowed since these don't affect the IK solution as already discussed above. Also, the modifications are applied to a copied ACM so that IK is run using only the original entries. |
henningkayser
force-pushed
the
pr-compute_ik_target_col_check
branch
from
366b7fa
to
8123dd0
Compare
|
I just saw that this still doesn't cover all cases. If the colliding links are outside of the kinematic chain then the pre-check will still collide with other collision objects. My suggestion now is to really only check for collisions in the eef links in the pre-check and disable all others. |
henningkayser
force-pushed
the
pr-compute_ik_target_col_check
branch
from
8123dd0
to
66b4381
Compare
|
@rhaschke ping |
| @@ -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(); | |||
There was a problem hiding this comment.
Maybe move this line down to the code block that actually uses this variable?
| // 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); | ||
| } | ||
| } | ||
| } |
There was a problem hiding this comment.
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.
|
@rhaschke @henningkayser My end-effector is randomly colliding with some remote environment link. This is not happening with a MoveTo stage. I am guessing the error comes from the issue explained here. Any plan to merge this? Or was it solve already elsewhere? |
Fixes false positive collisions of links/objects unrelated to the (rigidly extended) end effector. This includes collisions between objects or secondary groups and root links caused by the added padding. Common examples for this are collision objects positioned on a table link or another planning group having a self collision.