more tests on robot_cell

src/compas_fab/robots/robot_cell.py

@@ -19,7 +19,7 @@
 if not IPY:
     from typing import TYPE_CHECKING
 
-    if TYPE_CHECKING:
+    if TYPE_CHECKING:  # pragma: no cover
         from typing import Dict  # noqa: F401
         from typing import List  # noqa: F401
         from typing import Optional  # noqa: F401
@@ -78,12 +78,12 @@ def __init__(self, robot_model=None, robot_semantics=None, tool_models=None, rig
     @property
     def tool_ids(self):
         # type: () -> List[str]
-        return self.tool_models.keys()
+        return list(self.tool_models.keys())
 
     @property
     def rigid_body_ids(self):
         # type: () -> List[str]
-        return self.rigid_body_models.keys()
+        return list(self.rigid_body_models.keys())
 
     @property
     def __data__(self):
@@ -418,6 +418,9 @@ def get_all_configurable_joints(self):
         i.e., not ``Joint.FIXED``, not mimicking another joint and not a passive joint.
         See :meth:`compas_robots.model.Joint.is_configurable` for more details.
 
+        The result of this function is different from :meth:`RobotModel.get_configurable_joints()`
+        as this also filters out passive joints declared in the RobotSemantics.
+
         Returns
         -------
         :obj:`list` of :class:`compas_robots.model.Joint`
@@ -431,6 +434,18 @@ def get_all_configurable_joints(self):
                 joints.append(joint)
         return joints
 
+    def get_all_configurable_joint_names(self):
+        # type: () -> List[str]
+        """Get all the names of all configurable joints of a planning group.
+
+        Similar to :meth:`get_all_configurable_joints` but returning joint names.
+
+        Returns
+        -------
+        :obj:`list` of :obj:`str`
+        """
+        return [joint.name for joint in self.get_all_configurable_joints()]
+
     def get_configurable_joints(self, group=None):
         # type: (Optional[str]) -> List[Joint]
         """Get all configurable :class:`compas_robots.model.Joint` of a planning group.
@@ -439,8 +454,8 @@ def get_configurable_joints(self, group=None):
         i.e., not ``Joint.FIXED``, not mimicking another joint and not a passive joint.
         See :meth:`compas_robots.model.Joint.is_configurable` for more details.
 
-        Important: Setting the `group` to None, does not return all configurable joints of the robot.
-        If all configurable joints of the robot are needed, use :meth:`RobotModel.get_configurable_joints()`.
+        Important: Setting the `group` to None, does not return all configurable joints of the robot,
+        but that of the main planning group.
 
         Parameters
         ----------
@@ -452,6 +467,10 @@ def get_configurable_joints(self, group=None):
         :obj:`list` of :class:`compas_robots.model.Joint`
             A list of configurable joints.
 
+        Notes
+        -----
+        This function is different from :meth:`RobotModel.get_configurable_joints()`,
+        as this function filters only the joints of the specified group.
         """
         group = group or self.main_group_name
         joints = []
@@ -463,12 +482,11 @@ def get_configurable_joints(self, group=None):
         return joints
 
     def get_configurable_joint_names(self, group=None):
-        # type: (RobotModel, Optional[str]) -> List[str]
+        # type: (Optional[str]) -> List[str]
         """Get all the names of configurable joints of a planning group.
 
         Similar to :meth:`get_configurable_joints` but returning joint names.
 
-        If all configurable joints of the robot are needed, use :meth:`RobotModel.get_configurable_joint_names()`.
 
         Parameters
         ----------
@@ -478,6 +496,11 @@ def get_configurable_joint_names(self, group=None):
         Returns
         -------
         :obj:`list` of :obj:`str`
+
+        Notes
+        -----
+        This function is different from :meth:`RobotModel.get_configurable_joint_name()`,
+        as this function filters only the joints of the specified group.
         """
         group = group or self.main_group_name
         configurable_joints = self.get_configurable_joints(group)
@@ -511,7 +534,32 @@ def get_configurable_joint_types(self, group=None):
         configurable_joints = self.get_configurable_joints(group)
         return [j.type for j in configurable_joints]
 
-    def get_configuration_from_group_state(self, group, group_state):
+    def get_group_states_names(self, group):
+        # type: (str) -> List[str]
+        """Get the names of the group states of a planning group.
+
+        Parameters
+        ----------
+        group : :obj:`str`
+            The name of the planning group.
+
+        Returns
+        -------
+        :obj:`list` of :obj:`str`
+
+        Examples
+        --------
+        >>> robot_cell, robot_cell_state = RobotCellLibrary.ur5()
+        >>> robot_cell.get_group_states_names('manipulator')
+        ['home', 'up']
+        """
+        if group not in self.group_names:
+            raise ValueError("Group '{}' does not exist in the robot cell.".format(group))
+        if group not in self.group_states:
+            return []
+        return list(self.group_states[group].keys())
+
+    def get_configuration_from_group_state(self, group, group_state_name):
         # type: (str, str) -> Configuration
         """Get the :class:`compas_robots.Configuration` from a predefined group state.
 
@@ -521,15 +569,15 @@ def get_configuration_from_group_state(self, group, group_state):
         ----------
         group : :obj:`str`
             The name of the planning group.
-        group_state : :obj:`str`
+        group_state_name : :obj:`str`
             The name of the group_state.
 
         Returns
         -------
         :class:`compas_robots.Configuration`
             The configuration specified by the :attr:`group_state`.
         """
-        joint_dict = self.group_states[group][group_state]
+        joint_dict = self.group_states[group][group_state_name]
         group_joint_names = self.get_configurable_joint_names(group)
         group_joint_types = self.get_configurable_joint_types(group)
         values = [joint_dict[name] for name in group_joint_names]
@@ -543,6 +591,8 @@ def zero_full_configuration(self):
         # type: () -> Configuration
         """Get the zero configuration (all configurable joints) of the robot.
 
+        Does not include passive joints or fixed joints.
+
         If the joint value `0.0` is outside of joint limits ``(joint.limit.upper, joint.limit.lower)`` then
         ``(upper + lower) / 2`` is used as joint value.
 
@@ -653,17 +703,43 @@ def random_configuration(self, group=None):
 
         return Configuration(joint_values, joint_types, joint_names)
 
-    def get_group_configuration(self, group, full_configuration):
+    def configuration_to_full_configuration(self, configuration, full_configuration=None):
+        # type: (Configuration, Optional[Configuration]) -> Configuration
+        """Create a new Configuration object from a given configuration
+        and fill in the missing joints from a given full_configuration.
+        The joint values in the configuration takes precedence over the full_configuration.
+
+        Parameters
+        ----------
+        configuration : :class:`compas_robots.Configuration`
+            The configuration of the group.
+            The attributes `joint_names` and `joint_types` must be provided.
+        full_configuration : :class:`compas_robots.Configuration`, optional
+            The full configuration of the robot.
+            If not provided, the full configuration is created using :meth:`zero_full_configuration`.
+
+        Returns
+        -------
+        :class:`compas_robots.Configuration`
+            A new  Configuration object with all the joints of the robot.
+        """
+        if not full_configuration:
+            full_configuration = self.zero_full_configuration()
+
+        full_configuration.merged(configuration)
+        return full_configuration
+
+    def full_configuration_to_group_configuration(self, full_configuration, group):
         # type: (str, Configuration) -> Configuration
         """Filter a full configuration and return only the joints of the specified group.
 
         Parameters
         ----------
-        group : :obj:`str`
-            The name of the planning group.
         full_configuration : :class:`compas_robots.Configuration`
             A full configuration (with all configurable joints of the robot).
             Note that this object is not modified.
+        group : :obj:`str`
+            The name of the planning group.
 
         Returns
         -------
@@ -773,7 +849,7 @@ def get_attached_workpieces(self, robot_cell_state, group):
         bodies = []
         workpiece_id = robot_cell_state.get_attached_workpiece_ids(group)
         for id in workpiece_id:
-            self.rigid_body_models[id]
+            bodies.append(self.rigid_body_models[id])
         return bodies
 
     def get_attached_rigid_bodies(self, robot_cell_state, group):

src/compas_fab/robots/robot_library.py

@@ -133,14 +133,17 @@ def printing_tool(cls, load_geometry=True, tool_size=1.0):
         tool_frame = Frame([0.2, 0.0, 1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0])
         tool_frame.scale(tool_size)
 
+        tool_model = ToolModel(visual=None, frame_in_tool0_frame=tool_frame, name="printing_tool")
+        meshes = []
         if load_geometry:
             obj = compas_fab.get("tool_library/printing_tool.obj")
             tool_mesh = Mesh.from_obj(obj)
             tool_mesh.scale(tool_size)
-        else:
-            tool_mesh = None
+            meshes.append(tool_mesh)
 
-        return ToolModel(visual=tool_mesh, frame_in_tool0_frame=tool_frame, collision=tool_mesh, name="printing_tool")
+        tool_model.add_link("printing_tool_link", visual_mesh=meshes, collision_mesh=meshes)
+
+        return tool_model
 
     @classmethod
     def static_gripper(cls, load_geometry=True):
@@ -175,8 +178,8 @@ def static_gripper(cls, load_geometry=True):
         # Before the transformation,
         # X is the direction of the jaw movement, Y direction is the long axis when holding bar materials.
 
+        meshes = []
         if load_geometry:
-            meshes = []
             # Transformation to rotate the gripper to match the X axis
             t = Frame([0, 0, 0], [0, 0, 1], [1, 0, 0]).to_transformation().inverted()
             # Create the gripper body
@@ -188,7 +191,7 @@ def static_gripper(cls, load_geometry=True):
             # Create the gripper finger
             shape = Box.from_corner_corner_height([0.2, -0.05, 0.1], [0.1, 0.05, 0.1], 0.2)
             meshes.append(Mesh.from_shape(shape).transformed(t))
-            tool_model.add_link("gripper_body", visual_meshes=meshes, collision_meshes=meshes)
+        tool_model.add_link("gripper_body", visual_meshes=meshes, collision_meshes=meshes)
 
         return tool_model
 
@@ -225,8 +228,8 @@ def static_gripper_small(cls, load_geometry=True):
         # Before the transformation,
         # X is the direction of the jaw movement, Y direction is the long axis when holding bar materials.
 
+        meshes = []
         if load_geometry:
-            meshes = []
             # Transformation to rotate the gripper to match the X axis
             t = Frame([0, 0, 0], [0, 0, 1], [1, 0, 0]).to_transformation().inverted()
             # Create the gripper body
@@ -238,7 +241,7 @@ def static_gripper_small(cls, load_geometry=True):
             # Create the gripper finger
             shape = Box.from_corner_corner_height([0.1, -0.05, 0.05], [0.05, 0.05, 0.05], 0.1)
             meshes.append(Mesh.from_shape(shape).transformed(t))
-            tool_model.add_link("gripper_body", visual_meshes=meshes, collision_meshes=meshes)
+        tool_model.add_link("gripper_body", visual_meshes=meshes, collision_meshes=meshes)
 
         return tool_model