Fixed a bunch of test errors including the nasty {} in RobotCellState…

….__init__()

src/compas_fab/backends/exceptions.py

@@ -7,6 +7,7 @@
     "BackendFeatureNotSupportedError",
     "BackendTargetNotSupportedError",
     "TargetModeMismatchError",
+    "PlanningGroupNotExistsError",
     "InverseKinematicsError",
     "KinematicsError",
     "CollisionCheckError",
@@ -52,6 +53,15 @@ class TargetModeMismatchError(Exception):
     pass
 
 
+class PlanningGroupNotExistsError(Exception):
+    """Indicates that the selected planning group does not exist in the robot model.
+
+    For example, if the robot model does not have a planning group with the name provided.
+    """
+
+    pass
+
+
 # -------------------------
 # Kinematics related errors
 # -------------------------

src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py

@@ -3,6 +3,7 @@
 from __future__ import print_function
 
 from compas_fab.backends.interfaces import ForwardKinematics
+from compas_fab.backends.exceptions import PlanningGroupNotExistsError
 from compas_fab.robots import TargetMode
 import compas
 
@@ -78,6 +79,10 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, scale=No
         # Check if the target mode is valid for the robot cell state
         planner.ensure_robot_cell_state_supports_target_mode(robot_cell_state, target_mode, group)
 
+        # Check if the planning group is supported by the planner
+        if group not in robot.group_names:
+            raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group))
+
         # Setting the entire robot cell state, including the robot configuration
         planner.set_robot_cell_state(robot_cell_state)
 
@@ -130,6 +135,10 @@ def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=Non
         robot = client.robot  # type: Robot
         group = group or robot.main_group_name
 
+        # Check if the planning group is supported by the planner
+        if group not in robot.group_names:
+            raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group))
+
         # Setting the entire robot cell state, including the robot configuration
         planner.set_robot_cell_state(robot_cell_state)
 

src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py

@@ -36,6 +36,7 @@
 from compas.geometry import axis_angle_from_quaternion
 from compas_fab.backends.exceptions import InverseKinematicsError
 from compas_fab.backends.exceptions import CollisionCheckError
+from compas_fab.backends.exceptions import PlanningGroupNotExistsError
 from compas_fab.backends.interfaces import InverseKinematics
 from compas_fab.backends.pybullet.conversions import pose_from_frame
 from compas_fab.backends.pybullet.exceptions import PlanningGroupNotSupported
@@ -177,6 +178,10 @@ def iter_inverse_kinematics(self, target, robot_cell_state, group=None, options=
         # Check if the robot cell state supports the target mode
         planner.ensure_robot_cell_state_supports_target_mode(robot_cell_state, target.target_mode, group)
 
+        # Check if the planning group is supported by the planner
+        if group not in robot.group_names:
+            raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group))
+
         # ===================================================================================
         # End of common lines
         # ===================================================================================

src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py

@@ -48,8 +48,10 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None):
 
         for tool_id in list(client.tools_puids.keys()):
             client.remove_tool(tool_id)
+        # client.tools_puids = {}
         for rigid_body_id in list(client.rigid_bodies_puids.keys()):
             client.remove_rigid_body(rigid_body_id)
+        # client.rigid_bodies_puids = {}
 
         # Add the robot cell to the PyBullet world
         for name, tool_model in robot_cell.tool_models.items():

src/compas_fab/backends/pybullet/client.py

@@ -16,7 +16,9 @@
 from compas_robots.files import URDF
 from compas_robots.model import MeshDescriptor
 from compas_robots import ToolModel
-from compas_robots.model import Joint
+from compas_robots.model import Inertial
+from compas_robots.model import Inertia
+from compas_robots.model import Mass
 from compas_robots.model import Material
 
 from compas_fab.backends import CollisionCheckError
@@ -359,8 +361,8 @@ def remove_tool(self, name):
         name : :obj:`str`
             The name of the tool.
         """
-        if name not in self.tools_puids:
-            return
+        assert name in self.tools_puids
+
         pybullet.removeBody(self.tools_puids[name], physicsClientId=self.client_id)
         del self.tools_puids[name]
 
@@ -519,6 +521,9 @@ def robot_model_to_urdf(self, robot_model, concavity=False):
                     fp = self._handle_concavity(fp, self._cache_dir.name, concavity, 1, str(joined_mesh.guid))
                     shape.filename = fp
 
+            if link.inertial is None:
+                link.inertial = self.pybullet_empty_inertial()
+
         # create urdf with new mesh locations
         urdf = URDF.from_robot(robot_model)
 
@@ -533,6 +538,13 @@ def robot_model_to_urdf(self, robot_model, concavity=False):
 
         return urdf_filepath
 
+    def pybullet_empty_inertial(self):
+        origin = Frame.worldXY()
+        mass = Mass(0.0)
+        inertia = Inertia(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        inertial = Inertial(origin, mass, inertia)
+        return inertial
+
     # --------------------------------
     # Functions for collision checking
     # --------------------------------

src/compas_fab/robots/robot_cell.py

@@ -330,13 +330,13 @@ class RobotCellState(Data):
 
     """
 
-    def __init__(self, robot_flange_frame, robot_configuration=None, tool_states={}, rigid_body_states={}):
+    def __init__(self, robot_flange_frame, robot_configuration=None, tool_states=None, rigid_body_states=None):
         # type: (Frame, Optional[Configuration], Dict[str, ToolState], Dict[str, RigidBodyState]) -> None
         super(RobotCellState, self).__init__()
         self.robot_flange_frame = robot_flange_frame  # type: Frame
         self.robot_configuration = robot_configuration  # type: Optional[Configuration]
-        self.tool_states = tool_states  # type: Dict[str, ToolState]
-        self.rigid_body_states = rigid_body_states  # type: Dict[str, RigidBodyState]
+        self.tool_states = tool_states or {}  # type: Dict[str, ToolState]
+        self.rigid_body_states = rigid_body_states or {}  # type: Dict[str, RigidBodyState]
 
     @property
     def tool_ids(self):
@@ -472,7 +472,7 @@ def get_attached_rigid_body_ids(self):
             if rigid_body_state.attached_to_link:
                 ids.append(rigid_body_id)
 
-    def set_tool_attached_to_group(self, tool_id, group, attachment_frame=None, touch_links=[], detach_others=True):
+    def set_tool_attached_to_group(self, tool_id, group, attachment_frame=None, touch_links=None, detach_others=True):
         # type: (str, str, Optional[Frame], Optional[List[str]], Optional[bool]) -> None
         """Sets the tool attached to the planning group.
 
@@ -500,14 +500,14 @@ def set_tool_attached_to_group(self, tool_id, group, attachment_frame=None, touc
         self.tool_states[tool_id].attached_to_group = group
         self.tool_states[tool_id].frame = None
         self.tool_states[tool_id].attachment_frame = attachment_frame
-        self.tool_states[tool_id].touch_links = touch_links
+        self.tool_states[tool_id].touch_links = touch_links or []
 
         if detach_others:
             for id, tool_state in self.tool_states.items():
                 if id != tool_id and tool_state.attached_to_group == group:
                     tool_state.attached_to_group = None
 
-    def set_rigid_body_attached_to_link(self, rigid_body_id, link_name, attachment_frame=None, touch_links=[]):
+    def set_rigid_body_attached_to_link(self, rigid_body_id, link_name, attachment_frame=None, touch_links=None):
         # type: (str, str, Optional[Frame | Transformation], Optional[List[str]]) -> None
         """Sets the rigid body attached to the link of the robot.
 
@@ -535,7 +535,7 @@ def set_rigid_body_attached_to_link(self, rigid_body_id, link_name, attachment_f
         self.rigid_body_states[rigid_body_id].attached_to_tool = None
         self.rigid_body_states[rigid_body_id].frame = None
         self.rigid_body_states[rigid_body_id].attachment_frame = attachment_frame
-        self.rigid_body_states[rigid_body_id].touch_links = touch_links
+        self.rigid_body_states[rigid_body_id].touch_links = touch_links or []
 
     def set_rigid_body_attached_to_tool(self, rigid_body_id, tool_id, attachment_frame=None):
         # type: (str, str, Optional[Frame]) -> None
@@ -666,20 +666,52 @@ class RigidBodyState(Data):
     ----------
     frame : :class:`compas.geometry.Frame`
         The base frame of the rigid body relative to the world coordinate frame.
-    attached_to_link : :obj:`str`, optional
-        The name of the robot link to which the rigid body is attached. Defaults to ``None``.
-    attached_to_tool : :obj:`str`, optional
-        The id of the tool to which the rigid body is attached. Defaults to ``None``.
+    attached_to_link : :obj:`str` | None
+        The name of the robot link to which the rigid body is attached.
+       ``None`` if not attached to a link.
+    attached_to_tool : :obj:`str` | None
+        The id of the tool to which the rigid body is attached.
+        ``None`` if not attached to a tool.
     touch_links : :obj:`list` of :obj:`str`
         The names of the robot links that are allowed to collide with the rigid body.
+        ``[]`` if the rigid body is not allowed to collide with any robot links.
     touch_bodies : :obj:`list` of :obj:`str`
         The names of other rigid bodies (including tools) that are allowed to collide with the rigid body.
+        ``[]`` if the rigid body is not allowed to collide with any other rigid bodies.
     attachment_frame : :class:`compas.geometry.Frame` | :class:`compas.geometry.Transformation`, optional
         The attachment (grasp) frame of the rigid body relative to (the base frame of) the attached link or
-        (the tool tip frame of) the tool. Defaults to ``None``.
+        (the tool tip frame of) the tool.
+        ``None`` if the rigid body is not attached to a link or a tool.
+    is_hidden : :obj:`bool`
+        Whether the rigid body is hidden in the scene. Collision checking will be turned off
+        for hidden objects.
+        Defaults to ``False``.
+
+    Parameters
+    ----------
+    frame : :class:`compas.geometry.Frame`
+        The base frame of the rigid body relative to the world coordinate frame.
+    attached_to_link : :obj:`str` , optional
+        The name of the robot link to which the rigid body is attached.
+        Defaults to ``None``, meaning that the rigid body is not attached to a link.
+    attached_to_tool : :obj:`str` , optional
+        The id of the tool to which the rigid body is attached.
+        Defaults to ``None``, meaning that the rigid body is not attached to a tool.
+    touch_links : :obj:`list` of :obj:`str`, optional
+        The names of the robot links that are allowed to collide with the rigid body.
+        Defaults to ``[]``, meaning that the rigid body is not allowed to collide with any robot links.
+    touch_bodies : :obj:`list` of :obj:`str`, optional
+        The names of other rigid bodies (including tools) that are allowed to collide with the rigid body.
+        Defaults to ``[]``, meaning that the rigid body is not allowed to collide with any other rigid bodies.
+    attachment_frame : :class:`compas.geometry.Frame` | :class:`compas.geometry.Transformation`, optional
+        The attachment (grasp) frame of the rigid body relative to (the base frame of) the attached link or
+        (the tool tip frame of) the tool.
+        Defaults to ``None``, meaning that the rigid body is not attached to a link or a tool.
     is_hidden : :obj:`bool`, optional
         Whether the rigid body is hidden in the scene. Collision checking will be turned off
-        for hidden objects. Defaults to ``False``.
+        for hidden objects.
+        Defaults to ``False``.
+
     """
 
     def __init__(