clone.py is corrupting environment - something is toggled on every clone

[DavidB-CMU]

I have an OpenRAVE environment containing two robot objects.
I make a planning request robot.PlanToEndEffectorOffset() which throws because one of the robots is in self-collision, which it's not.

Digging deeper, the planning request is fanned-out which happens to call clone.py 3 times.
Inside clone.py I check the self-collision status of the second robot.
First time: the second robot has no self-collisions.
Second time: the second robot is in self-collision. (normally an exception would be thrown)
Third time: the second robot has no self-collisions.

[mkoval]

To be clear, are these three separate clones from the source environment? Or three nested clones? I.e.:

env1 ---> env2 ---> env3 ---> env4

or

      env2
     /
    /
env1 ---> env3
    \
     \
      env4

[DavidB-CMU]

It's the second diagram.

I dug deeper and disabled all planners except for Workspace.
If I call robot.PlanToEndEffectorOffset() the request is handled only by the Workspace planner (the other planners are disabled) and the environment gets corrupted.

However if I directly call planner.PlanToEndEffectorOffset() there everything is ok.

So maybe it's something in how the environment is cloned multiple times that causes the corruption.

[DavidB-CMU]

This self-collision error is mentioned in
PR #19 "Bugfixes for SnapPlanner"
PR #51 "Added bugfix for cloning grabbed objects"

OpenRAVE issue #335 ("CheckSelfCollision always returns false on cloned robot")

[psigen]

There are a bunch of different self-collision cloning problems we've faced in the past, so those links might or might not be relevant.

Some more things to check out:

1. Is your robot grabbing anything?
2. Can you print the DOF values, collision report, and adjacency links in each env clone (when you do and don't get self collision)?
3. Does the issue happen on other planners besides Workspace?
4. Is it the active manipulator that is affected, or some other part of the robot?

[DavidB-CMU]

I think I found the problem.
When the robot is initially loaded, OpenRAVE calculates the AdjacentLinks and the NonAdjacentLinks.
For my 2nd robot, these lists are invalid and there are some link pairs that are missing from both lists.
When the environment is cloned, OpenRAVE seems to get confused and finds a SelfCollision error between the first pair of links that are missing from either list.
Interesting the adjacency lists for this robot in the cloned environment are correct and contain all link pairs.

The lists are wrong in the first place because there's a bug for "sphere" geometry. By removing the sphere geometry from the robot xml and only using trimesh geometry, the link adjacency lists are correct, and cloning does not result in strange self collision errors.

I will create a minimum example that demonstrates the bug.

[DavidB-CMU]

I propose that we add a check somewhere:
num_adjacent_links + num_adjacent_links = expected_num_link_pairs

maybe in robot or kinbody loading,
or in our Clone() class,
or as a prpy.util function?

[DavidB-CMU]

There's some test code in this repository:
https://github.com/DavidB-CMU/Bug_reports/tree/master/prpy/255

Note: in these tests, there are 2 OpenRAVE robots: HERB and a fridge.

Bug # 1:

Link adjacency

Loading an OpenRAVE robot with spheres geometry results in an in-correct calculation of the adjacent and non-adjacent link pairs.
If the robot xml contains trimesh & spheres, or only spheres, then there is a bug.
If the robot xml only contains trimesh then it is ok.

Code: test1.py

Output:

robot.GetAdjacentLinks() = [(0, 1), (0, 2), (1, 3), (2, 4)]
robot.GetNonAdjacentLinks() = [(1, 2), (0, 3), (2, 3), (0, 4), (1, 4), (3, 4)]
Link pairs are correct

robot.GetAdjacentLinks() = [(0, 1), (0, 2), (1, 3), (2, 4)]
robot.GetNonAdjacentLinks() = [(2, 3), (1, 4)]
ERROR: some link pairs are missing <-- FROM SPHERES GEOMETRY

.

Bug # 2:

Cloning an environment containing a robot with spheres geometry AND prpy planning sequence sometimes causes a self-collision error for the cloned robot

In this example, before generating the last plan the fridge is not in self-collision.
In the cloned environment the fridge gets a self-collision error and throws an exception.
Afterwards the fridge in the original environment is not in self-collision.

I spent a long time trying to minimize this example.
The only variables I found are that the bug goes away if I do not use a prpy planning sequence and call the GreedyIKPlanner directly.
The bug also goes away if I remove the spheres geometry from the fridge.

Code: test2.py

Output:

Before planning, fridge.CheckSelfCollision() = False

PlanToEndEffectorOffset (# 2)

[INFO] [prpy.planning.base:base.py:357]:plan: Sequence - Calling planner "Sequence(SnapPlanner, VectorFieldPlanner, Trajopt)".
[INFO] [prpy.planning.base:base.py:357]:plan: Sequence - Calling planner "VectorFieldPlanner".

Failed to plan to end effector offset (# 2)

Unable to re-grab objects. Robot refrigerator was cloned while in self-collision.
Detected collision: < refrigerator, door_lower > x < refrigerator, door_upper >.

After planning, fridge.CheckSelfCollision() = False

[psigen]

Thanks! This is an excellent analysis of the bug.

[DavidB-CMU]

Regarding the bug with the lists of adjacent links and non-adjacent links, it occurs when the robot's initial configuration is such that a pair of links is already in collision.
That pair of links is not added to either list and so CheckSelfCollision() doesn't tell you that they're in collision.
If you clone the robot and the configuration changes, this can result in strange self-collision errors.
This problem is not yet addressed in the latest source from the OpenRAVE master branch.
There's a ticket here:
rdiankov/openrave#388