Disabled collisions for `PyBulletClient` not working

[romanarust]

Describe the bug

If passing disabled_collisions from RobotSemantics to the PyBulletClient, it still raises CollisionError for disabled collisions. But maybe I am not passing this information correctly ??... If not, it would be good to document this somewhere.

To Reproduce

import compas_fab
from compas.robots import Configuration
from compas_fab.backends import PyBulletClient
from compas_fab.robots import RobotSemantics

with PyBulletClient() as client:
    urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
    srdf_filename = compas_fab.get('universal_robot/ur5_moveit_config/config/ur5.srdf')

    robot = client.load_robot(urdf_filename)
    client.disabled_collisions = RobotSemantics.from_srdf_file(srdf_filename, robot.model).disabled_collisions
    configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
    frame_WCF = robot.forward_kinematics(configuration, options={"check_collision":True})

Raises

[WARNING] Collision between 'shoulder_link' and 'upper_arm_link'
Traceback (most recent call last):
  File "c:\Users\rustr\Desktop\temp.py", line 15, in <module>
    frame_WCF = robot.forward_kinematics(configuration, options={"check_collision":True})
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\robots\robot.py", line 1286, in forward_kinematics
    frame_WCF = self.client.forward_kinematics(self,
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\interfaces\client.py", line 31, in forward_kinematics
    return self.planner.forward_kinematics(*args, **kwargs)
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\pybullet\planner.py", line 43, in forward_kinematics
    return PyBulletForwardKinematics(self.client)(*args, **kwargs)
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\interfaces\backend_features.py", line 16, in __call__
    return self.forward_kinematics(robot, configuration, group, options)
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\pybullet\backend_features\pybullet_forward_kinematics.py", line 48, in forward_kinematics
    self.client.check_collisions(robot, configuration)
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\pybullet\client.py", line 430, in check_collisions
    self.check_robot_self_collision(robot)
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\pybullet\client.py", line 474, in check_robot_self_collision
    self._check_collision(body_id, link_1_name, body_id, link_2_name, link_1_id, link_2_id)
  File "c:\users\rustr\workspace\compas_fab\src\compas_fab\backends\pybullet\client.py", line 502, in _check_collision
    raise CollisionError(body_1_name, body_2_name)
compas_fab.backends.pybullet.exceptions.CollisionError: Collision between 'shoulder_link' and 'upper_arm_link'

Although 'shoulder_link' and 'upper_arm_link' are in the disabled collisions.

[beverlylytle]

RIght, so disabled_collisions in RobotSemantics is a set of tuples while disabled_collisions in PyBulletClient is a set of sets. Because order doesn't matter in these pairs, I chose this in the PyBullet backend implementation because it makes the code a bit cleaner. I guess I could switch it to tuples and enforce that the tuple is in dictionary order so that it is more in line with the semantics, but I would then also want that the semantic disabled collision pairs were also in dictionary order...

[romanarust]

Thank you for the clarification! Makes sense to have sets instead of tuples.

Could we change this in RobotSemantics directly without breaking change? Still, it would be good to have a function in the PyBulletClient that retrieves information from semantics more transparently. Something like:

• client.load_semantics(srdf_filename) or
• client.set_semantics(semantics) (they could also do the conversion from tuples to sets)

[beverlylytle]

Yeah, I'll see what I can do.