filtering out incorrect collisions in pydrake

Question

In our robotics project, we use pydrake query object to check wether we have collisions in our iiwa arm (this is necessary for our RRT and useful for double-checking that the output of our inverse kinematics module is good).

However, with standard settings, we detect some non-existant collisions that limit our robot's motion. In particular, when using the following sdf for an iiwa arm (the scenario contains only this iiwa arm and nothing else):

- add_model:
    name: iiwa
    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf

And setting its angles with

example_joint_angles = np.array([0., 0., 0., 0., 0., 1.95, 0.])
environment.model.robot_arm.set_joint_angles(example_joint_angles)
print(environment.model.has_collisions())

we are told that there is a collision, but setting the 6th angle to be 1.9 says no collisions (for reference, the implementation of our methods are included below).

I think that these angles should not result in a collision for two reasons. First, just looking at the meshcat, I see that the arm is still far from folding on itself. Second, we did not expect this kind of collision in first place, because we set the angle limits on our inverse kinematics according to the joint limits listed in the sdf of the iiwa arm (which allow for an angle of up to 2.0944 on this joint).

So I guess the first question is: is there a better way of checking for collisions with an iiwa arm (e.g., using a different sdf that has slightly more refined collisions)?

If not, is there some way of telling the query object / scene_graph / etc. to skip collisions between these two objects?

class IIWAArm:
    #...
    def set_joint_angles(self, q: np.ndarray, visualize: bool = True):
    '''
    Instantaneously moves the arm to the given angles (for visualization / motion planning).

    Args:
        q (np.ndarray): Angles to move to.
    '''
    self.iiwa = self.plant.GetModelInstanceByName("iiwa")
    self.plant.SetPositions(self.plant_context, self.iiwa, q)
    if visualize:
        self.diagram.ForcedPublish(self.context)


class SimulationModel:
    robot_arm: IIWAArm
    #...
    def has_collisions(self) -> bool:
        scene_graph = self.station.GetSubsystemByName("scene_graph")
        context = self.context
        scene_graph_context = scene_graph.GetMyMutableContextFromRoot(context)
        query_object = scene_graph.GetOutputPort("query").Eval(scene_graph_context)
        return query_object.HasCollisions()

    def get_collisions(self) -> List[PenetrationAsPointPair]:
        scene_graph = self.station.GetSubsystemByName("scene_graph")
        context = self.context
        scene_graph_context = scene_graph.GetMyMutableContextFromRoot(context)
        query_object = scene_graph.GetOutputPort("query").Eval(scene_graph_context)
        res = query_object.ComputePointPairPenetration()
        return res

I also tried using the function get_collisions listed above, which lets me extract the GeometryId's of the objects in a collision. So, in theory, I think that I should be able to use get_collisions and then skip the collisions that don't make sense (e.g. between joints 6 and 7 of the iiwa). I got stuck with that strategy, because I couldn't find a function that lets me extract "human readable" information about what object a given GeometryId corresponds to (e.g., there is a scene_graph.RenameGeometry that allows me to set the name corresponding to a GeometryId, but I couldn't find a similar "getter function").

My current (extremely hacky) approach is to change the const'r of SimulationModel to purposefully cause this false collision on purpose to find its corresponding GeometryId's and make has_collisions skip them. I.e., something like:

class SimulationModel:
    def __init__(self, ...):
        # ...
        self.robot_arm.set_joint_angles(BAD_ANGLES)
        self.fake_collisions = self.get_collisions()
        self.robot_arm.set_joint_angles(DEFAULT_ANGLES)

    def new_has_collisions(self) -> bool:
        suspected_collisions = self.get_collisions()
        filtered_collisions = filter_out(suspected_collisions, self.fake_collisions)
        return len(filtered_collisions) != 0

Answer 1

The problem you're having is simply one of proximity representation. As the name suggests, the iiwa you're using represents each link as a box for proximity queries (aka distance and contact queries).

Boxes are simple but exceptionally coarse geometric approximations (especially for something like the iiwa arm's links). If you are pushing near the edge of the real robot's configuration space, you'll need proximity geometry that better approximates the actual robot geometry. Capsules would be the next logical thing to try; they'll fit the cylindrical links better but still offer reasonable performance in the queries. At the limit, you can use the meshes themselves (they'll be replaced with inefficient representations of the meshes' contact hulls). If you converge on meshes, you'd be better off making low-resolution, convex approximations of the mesh by hand (Drake can't do that for you as of this data). If you look at the model package://drake/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf, you'll see it has a hand-crafted polytope for the end link.

Below, I've attached a number of steps that you can use to introspect and better understand the model.

If you run the following:

bazel run //tool:model_visualizer -- package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf

You can exactly see the robot (the screen grab shows the expansion of the control panel):

If we turn on the "proximity" geometry, you'll see the boxes.

Finally, if I set the pose you suggest (all zero except for iiwa_joint_6), it exhibits contact between link 7 and link 5.