Fix Moveit FK Example and Verbose print in the backend feature

docs/examples/03_backends_ros/files/03_forward_kinematics_with_tools.py

@@ -0,0 +1,30 @@
+from compas.geometry import Frame
+from compas_fab.robots import RobotCellState
+from compas_fab.robots import TargetMode
+from compas_fab.robots import RobotCellLibrary
+from compas_fab.backends import RosClient
+from compas_fab.backends import MoveItPlanner
+
+with RosClient() as client:
+    robot_cell, robot_cell_state = RobotCellLibrary.ur5_gripper_one_beam(client)
+
+    planner = MoveItPlanner(client)
+    planner.set_robot_cell(robot_cell)
+
+    robot_cell_state.robot_configuration.joint_values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]
+    # Change the attachment location (grasp) of the beam
+    robot_cell_state.rigid_body_states["beam"].attachment_frame = Frame(
+        [0.0, 0.0, -0.1], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]
+    )
+
+    print("Robot Planner Coordinate Frame (PCF) relative to the world coordinate system (WCF):")
+    frame = planner.forward_kinematics(robot_cell_state, TargetMode.ROBOT)
+    print("- {}".format(frame))
+
+    print("Tool Coordinate Frame (TCF) relative to the world coordinate system (WCF):")
+    frame = planner.forward_kinematics(robot_cell_state, TargetMode.TOOL)
+    print("- {}".format(frame))
+
+    print("Workpiece's Object Coordinate Frame (OCF) relative to the world coordinate system (WCF):")
+    frame = planner.forward_kinematics(robot_cell_state, TargetMode.WORKPIECE)
+    print("- {}".format(frame))

src/compas_fab/backends/interfaces/backend_features.py

@@ -181,17 +181,16 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, native_s
         The function can return the planner coordinate frame (PCF), the tool coordinate frame (TCF),
         or the workpiece's object coordinate frame (OCF) based on the ``target_mode`` provided.
 
-        - ``"Target.ROBOT"`` will return the planner coordinate frame (PCF).
-        - ``"Target.TOOL"`` will return the tool coordinate frame (TCF) if a tool is attached.
+        - ``"Target.ROBOT"`` will return the planner coordinate frame (PCF) of the planning group.
+        - ``"Target.TOOL"`` will return the tool coordinate frame (TCF) if a tool is attached to the planning group.
         - ``"Target.WORKPIECE"`` will return the workpiece's object coordinate frame (OCF)
-          if a workpiece is attached.
+          if a workpiece is attached to the planning group via a tool.
 
         Parameters
         ----------
         robot_cell_state : :class:`compas_fab.robots.RobotCellState`
             The robot cell state describing the robot cell.
             The attribute `robot_configuration`, must contain the full configuration of the robot corresponding to the planning group.
-            The Configuration object must include ``joint_names``.
             The robot cell state should also reflect the attachment of tools, if any.
         target_mode : :class:`compas_fab.robots.TargetMode` or str
             The target mode to select which frame to return.
@@ -204,22 +203,22 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, native_s
             For example, if the resulting frame is to be used in a millimeters environment, `native_scale` should be set to ``'0.001'``.
             Defaults to None, which means no scaling is applied.
         options : dict, optional
-            Dictionary containing kwargs for arguments specific to
-            the client being queried.
+            Dictionary containing planner-specific options.
+            See the planner's `ForwardKinematics` documentation for supported options.
 
         Returns
         -------
         :class:`Frame`
             The frame in the world's coordinate system (WCF).
 
         """
-        pass
+        raise NotImplementedError("This feature is not supported by the planner.")
 
         # The implementation code is located in the backend's module:
         # "src/compas_fab/backends/<backend_name>/backend_features/<planner_name>_forward_kinematics.py"
 
-    def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=None, native_scale=None, options=None):
-        # type: (RobotCellState, Optional[str], Optional[str], Optional[float], Optional[dict]) -> Frame
+    def forward_kinematics_to_link(self, robot_cell_state, link_name=None, native_scale=None, options=None):
+        # type: (RobotCellState, Optional[str], Optional[float], Optional[dict]) -> Frame
         """Calculate the frame of the specified robot link from the provided RobotCellState.
 
         This function operates similar to :meth:`compas_fab.backends.PyBulletForwardKinematics.forward_kinematics`,
@@ -236,19 +235,18 @@ def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=Non
         link_name : str, optional
             The name of the link to calculate the forward kinematics for.
             Defaults to the last link of the provided planning group.
-        group : str, optional
-            The planning group of the robot.
-            Defaults to the robot's main planning group.
         native_scale : float, optional
             The scaling factor to apply to the resulting frame.
             It is defined as `user_object_value * native_scale = meter_object_value`.
             For example, if the resulting frame is to be used in a millimeters environment, `native_scale` should be set to ``'0.001'``.
             Defaults to None, which means no scaling is applied.
         options : dict, optional
-            Dictionary for passing planner specific options.
-            Currently unused.
+            Dictionary containing planner-specific options.
+            See the planner's `ForwardKinematics` documentation for supported options.
         """
 
+        raise NotImplementedError("This feature is not supported by the planner.")
+
 
 class InverseKinematics(BackendFeature):
     """Mix-in interface for implementing a planner's inverse kinematics feature."""

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

@@ -1,6 +1,7 @@
 from compas import IPY
 
 from compas_fab.backends.interfaces import SetRobotCell
+from compas_fab.robots import RobotCell
 
 if not IPY:
     from typing import TYPE_CHECKING
@@ -10,7 +11,6 @@
         from typing import Optional  # noqa: F401
 
         from compas_fab.backends import PyBulletClient  # noqa: F401
-        from compas_fab.robots import RobotCell  # noqa: F401
         from compas_fab.robots import RobotCellState  # noqa: F401
 
 
@@ -25,6 +25,12 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None):
         It should be called only if the robot cell models have changed.
 
         """
+
+        if not isinstance(robot_cell, RobotCell):
+            raise TypeError(
+                "robot_cell should be an instance of RobotCell instead of: {}".format(type(robot_cell).__name__)
+            )
+
         client = self.client  # type: PyBulletClient
 
         # TODO: Check for new, modified and removed objects compared to the

src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py

@@ -20,6 +20,7 @@
     from typing import TYPE_CHECKING
 
     if TYPE_CHECKING:  # pragma: no cover
+        from typing import Optional  # noqa: F401
         from compas_fab.backends.ros.client import RosClient  # noqa: F401
         from compas_fab.backends.ros.planner import MoveItPlanner  # noqa: F401
         from compas_fab.robots import RobotCell  # noqa: F401
@@ -39,29 +40,39 @@ class MoveItForwardKinematics(ForwardKinematics):
         "/compute_fk", "GetPositionFK", GetPositionFKRequest, GetPositionFKResponse, validate_response
     )
 
-    def forward_kinematics(self, robot_cell_state, target_mode, group=None, options=None):
-        # type: (RobotCellState, TargetMode, str, dict) -> Frame
-        """Calculate the robot's forward kinematic.
+    def forward_kinematics(self, robot_cell_state, target_mode, group=None, native_scale=None, options=None):
+        # type: (RobotCellState, TargetMode, Optional[str], Optional[float], Optional[dict]) -> Frame
+        """Calculate the target frame of the robot from the provided RobotCellState.
+
+        The function can return the planner coordinate frame (PCF), the tool coordinate frame (TCF),
+        or the workpiece's object coordinate frame (OCF) based on the ``target_mode`` provided.
+
+        - ``"Target.ROBOT"`` will return the planner coordinate frame (PCF) of the planning group.
+        - ``"Target.TOOL"`` will return the tool coordinate frame (TCF) if a tool is attached to the planning group.
+        - ``"Target.WORKPIECE"`` will return the workpiece's object coordinate frame (OCF)
+          if a workpiece is attached to the planning group via a tool.
 
         Parameters
         ----------
         robot_cell_state : :class:`compas_fab.robots.RobotCellState`
-            The robot cell state.
-        target_mode : :class:`compas_fab.robots.TargetMode`
-            The target mode.
+            The robot cell state describing the robot cell.
+            The attribute `robot_configuration`, must contain the full configuration of the robot corresponding to the planning group.
+            The robot cell state should also reflect the attachment of tools, if any.
+        target_mode : :class:`compas_fab.robots.TargetMode` or str
+            The target mode to select which frame to return.
         group : str, optional
-            Unused parameter.
+            The planning group of the robot.
+            Defaults to the robot's main planning group.
+        native_scale : float, optional
+            The scaling factor to apply to the resulting frame.
+            It is defined as `user_object_value * native_scale = meter_object_value`.
+            For example, if the resulting frame is to be used in a millimeters environment, `native_scale` should be set to ``'0.001'``.
+            Defaults to None, which means no scaling is applied.
         options : dict, optional
             Dictionary containing the following key-value pairs:
 
             - ``"base_link"``: (:obj:`str`) Name of the base link.
               Defaults to the model's root link.
-            - ``"link"``: (:obj:`str`, optional) The name of the link to
-              calculate the forward kinematics for. Defaults to the group's end
-              effector link.
-              Backwards compatibility note: if there's no ``link`` option, the
-              planner will try also ``tool0`` as fallback before defaulting
-              to the end effector's link.
 
         Returns
         -------
@@ -73,25 +84,63 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, options=
         robot_cell = client.robot_cell  # type: RobotCell
         options = options or {}
 
+        group = group or robot_cell.main_group_name
+
+        link_name = robot_cell.get_end_effector_link_name(group)
+        pcf_frame = self.forward_kinematics_to_link(robot_cell_state, link_name, native_scale, options)
+        target_frame = client.robot_cell.pcf_to_target_frames(robot_cell_state, pcf_frame, target_mode, group)
+
+        return target_frame
+
+    def forward_kinematics_to_link(self, robot_cell_state, link_name=None, native_scale=None, options=None):
+        # type: (RobotCellState, Optional[str], Optional[float], Optional[dict]) -> Frame
+        """Calculate the frame of the specified robot link from the provided RobotCellState.
+
+        This function operates similar to :meth:`compas_fab.backends.PyBulletForwardKinematics.forward_kinematics`,
+        but allows the user to specify which link to return. The function will return the frame of the specified
+        link relative to the world coordinate frame (WCF).
+
+        This can be convenient in scenarios where user objects (such as a camera) are attached to one of the
+        robot's links and the user needs to know the position of the object relative to the world coordinate frame.
+
+        Parameters
+        ----------
+        robot_cell_state : :class:`compas_fab.robots.RobotCellState`
+            The robot cell state describing the robot cell.
+        link_name : str, optional
+            The name of the link to calculate the forward kinematics for.
+            Defaults to the last link of the provided planning group.
+        native_scale : float, optional
+            The scaling factor to apply to the resulting frame.
+            It is defined as `user_object_value * native_scale = meter_object_value`.
+            For example, if the resulting frame is to be used in a millimeters environment, `native_scale` should be set to ``'0.001'``.
+            Defaults to None, which means no scaling is applied.
+        options : dict, optional
+            Dictionary containing the following key-value pairs:
+
+            - ``"base_link"``: (:obj:`str`) Name of the base link.
+              Defaults to the model's root link.
+        """
+        planner = self  # type: MoveItPlanner
+        client = planner.client  # type: RosClient
+        robot_cell = client.robot_cell  # type: RobotCell
+        options = options or {}
+
+        if robot_cell.robot_model.get_link_by_name(link_name) is None:
+            raise ValueError("Link name {} does not exist in robot model".format(link_name))
+
         planner.set_robot_cell_state(robot_cell_state)
 
+        # Compose the kwargs for the forward kinematics
         kwargs = {}
         kwargs["configuration"] = robot_cell_state.robot_configuration
         kwargs["options"] = options
         kwargs["errback_name"] = "errback"
-
-        # Use base_link or fallback to model's root link
         options["base_link"] = options.get("base_link", robot_cell.root_name)
+        options["link"] = link_name
 
-        group = group or robot_cell.main_group_name
-
-        # Use selected link or default to group's end effector
-        options["link"] = options.get("link", options.get("tool0")) or robot_cell.get_end_effector_link_name(group)
-        if options["link"] not in robot_cell.get_link_names(group):
-            raise ValueError("Link name {} does not exist in planning group".format(options["link"]))
-
-        pcf_frame = await_callback(self.forward_kinematics_async, **kwargs)
-        return client.robot_cell.pcf_to_target_frames(robot_cell_state, pcf_frame, target_mode, group)
+        lcf_frame = await_callback(self.forward_kinematics_async, **kwargs)
+        return lcf_frame
 
     def forward_kinematics_async(self, callback, errback, configuration, options):
         """Asynchronous handler of MoveIt FK service."""

src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py

@@ -44,14 +44,14 @@ def reset_planning_scene(self, options=None):
         # The complete removal of the planning scene is done in two steps because
         # the attached collision objects need to be removed before the collision objects
         # can be removed.
-        step_1_result = await_callback(self._remove_aco_async, **kwargs)
+        step_1_result = await_callback(self._reset_planning_scene_remove_aco_async, **kwargs)
         assert step_1_result.success, "Failed to remove attached collision objects"
-        step_2_result = await_callback(self._remove_co_async, **kwargs)
+        step_2_result = await_callback(self._reset_planning_scene_remove_co_async, **kwargs)
         assert step_2_result.success, "Failed to remove collision objects"
 
         return (step_1_result, step_2_result)
 
-    def _remove_aco_async(self, callback, errback):
+    def _reset_planning_scene_remove_aco_async(self, callback, errback):
         scene = self.get_planning_scene()  # type: PlanningScene
         for attached_collision_object in scene.robot_state.attached_collision_objects:
             attached_collision_object.object["operation"] = CollisionObject.REMOVE
@@ -60,7 +60,7 @@ def _remove_aco_async(self, callback, errback):
         request = scene.to_request(self.client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.client, request, callback, errback)
 
-    def _remove_co_async(self, callback, errback):
+    def _reset_planning_scene_remove_co_async(self, callback, errback):
         scene = self.get_planning_scene()  # type: PlanningScene
         for collision_object in scene.world.collision_objects:
             collision_object.operation = CollisionObject.REMOVE