Fix tbcf / tcf test

Author: gonzalocasas

src/compas_fab/robots/robot_cell.py

@@ -906,8 +906,7 @@ def get_attached_rigid_bodies(self, robot_cell_state, group):
     # Transformation functions
     # ----------------------------------------
 
-    def t_pcf_tcf(self, robot_cell_state, tool_id):
-        # type: (RobotCellState, str) -> Transformation
+    def t_pcf_tcf(self, robot_cell_state : RobotCellState, tool_id: str) -> Transformation:
         """Returns the transformation from the PCF (Planner Coordinate Frame) to the TCF (Tool Coordinate Frame).
 
         Parameters
@@ -922,9 +921,33 @@ def t_pcf_tcf(self, robot_cell_state, tool_id):
             Transformation from the tool's TCF to TBCF.
         """
 
+        t_pcf_tbcf = self.t_pcf_tbcf(robot_cell_state, tool_id)
+
+        # t_tbcf_tcf is Tool Frame, a property of the tool model, describing Tool Coordinate Frame (TCF) relative to Tool Base Frame (TBCF)
+        tool_model = self.tool_models[tool_id]
+        t_tbcf_tcf = Transformation.from_frame(tool_model.frame)
+
+        t_pcf_tcf = t_pcf_tbcf * t_tbcf_tcf
+        return t_pcf_tcf
+
+    def t_pcf_tbcf(self, robot_cell_state : RobotCellState, tool_id: str) -> Transformation:
+        """Returns the transformation from the PCF (Planner Coordinate Frame) to the TBCF (Tool Base Coordinate Frame).
+
+        Parameters
+        ----------
+        tool_id : str
+            The id of a tool found in `self.robot_cell.tool_models`.
+            The tool must be attached to the robot.
+
+        Returns
+        -------
+        :class:`~compas.geometry.Transformation`
+            Transformation from the tool's TCF to TBCF.
+        """
+
         if tool_id not in self.tool_models:
             raise ValueError("Tool with id '{}' not found in robot cell.".format(tool_id))
-        tool_model = self.tool_models[tool_id]
+
         tool_state = robot_cell_state.tool_states[tool_id]
         if not tool_state.attached_to_group:
             raise ValueError("Tool with id '{}' is not attached to the robot.".format(tool_id))
@@ -933,14 +956,10 @@ def t_pcf_tcf(self, robot_cell_state, tool_id):
         # t_pcf_tbcf is Tool Attachment Frame, describing Tool Base Coordinate Frame (TBCF) relative to PCF Frame on the Robot (PCF)
         attachment_frame = tool_state.attachment_frame or Frame.worldXY()
         t_pcf_tbcf = Transformation.from_frame(attachment_frame)
-        # t_tbcf_tcf is Tool Frame, a property of the tool model, describing Tool Coordinate Frame (TCF) relative to Tool Base Frame (TBCF)
-        t_tbcf_tcf = Transformation.from_frame(tool_model.frame)
 
-        t_pcf_tcf = t_pcf_tbcf * t_tbcf_tcf
-        return t_pcf_tcf
+        return t_pcf_tbcf
 
-    def t_pcf_ocf(self, robot_cell_state, workpiece_id):
-        # type: (RobotCellState, str) -> Transformation
+    def t_pcf_ocf(self, robot_cell_state : RobotCellState, workpiece_id : str) -> Transformation:
         """Returns the transformation from the PCF (Planner Coordinate Frame) to the OCF (Object Coordinate Frame).
 
         Parameters

tests/robots/test_robot_cell.py

@@ -413,17 +413,17 @@ def test_compute_attach_objects_frames(ur10e_gripper_one_beam):
     rcs.rigid_body_states[workpiece_id].attachment_frame = work_attachment_frame
 
     # Use the transformation functions for the test
-    t_pcf_tcf = rc.t_pcf_tcf(rcs, tool_id)
+    t_pcf_tbcf = rc.t_pcf_tbcf(rcs, tool_id)
     t_pcf_ocf = rc.t_pcf_ocf(rcs, workpiece_id)
 
     # Define the expected frames
     t_wcf_rcf = Transformation.from_frame(rcs.robot_base_frame)
     ee_link_name = rc.get_end_effector_link_name(group=rc.main_group_name)
     t_rcf_pcf = Transformation.from_frame(rc.robot_model.forward_kinematics(rcs.robot_configuration, ee_link_name))
-    t_wcf_tcf = t_wcf_rcf * t_rcf_pcf * t_pcf_tcf
+    t_wcf_tbcf = t_wcf_rcf * t_rcf_pcf * t_pcf_tbcf
     t_wcf_ocf = t_wcf_rcf * t_rcf_pcf * t_pcf_ocf
 
-    expected_tool_frame = Frame.from_transformation(t_wcf_tcf)
+    expected_tool_frame = Frame.from_transformation(t_wcf_tbcf)
     expected_workpiece_frame = Frame.from_transformation(t_wcf_ocf)
 
     # Test the function