From 94887115263eb8674cc3c06539b00efd4f4217d4 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Mon, 4 Nov 2024 14:44:26 +0800 Subject: [PATCH] Fix Moveit FK Example and Verbose print in the backend feature --- .../files/03_forward_kinematics_with_tools.py | 30 ++++++ .../backends/interfaces/backend_features.py | 26 +++-- .../pybullet_set_robot_cell.py | 8 +- .../move_it_forward_kinematics.py | 97 ++++++++++++++----- .../move_it_reset_planning_scene.py | 8 +- .../move_it_set_robot_cell.py | 64 +++++++----- .../move_it_set_robot_cell_state.py | 57 +++++++---- src/compas_fab/robots/robot_library.py | 95 ++++++++++++++++++ 8 files changed, 299 insertions(+), 86 deletions(-) create mode 100644 docs/examples/03_backends_ros/files/03_forward_kinematics_with_tools.py diff --git a/docs/examples/03_backends_ros/files/03_forward_kinematics_with_tools.py b/docs/examples/03_backends_ros/files/03_forward_kinematics_with_tools.py new file mode 100644 index 000000000..469347aad --- /dev/null +++ b/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)) diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index c0d3f6a27..eb5b19eaf 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/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,8 +203,8 @@ 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 ------- @@ -213,13 +212,13 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, native_s 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_features/_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.""" diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py index ec1128ecc..885a83dbf 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py +++ b/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 diff --git a/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py index 81d1b6f53..f4f12d945 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py +++ b/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.""" diff --git a/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py b/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py index 51ba65e59..02f89057c 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_reset_planning_scene.py +++ b/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 diff --git a/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell.py b/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell.py index e2925ad76..55269b2a9 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell.py @@ -18,6 +18,8 @@ from compas_fab.backends.ros.messages.shape_msgs import Mesh from compas_fab.backends.ros.service_description import ServiceDescription +from compas_fab.robots import RobotCell + if not IPY: from typing import TYPE_CHECKING @@ -29,7 +31,7 @@ from compas_fab.backends import MoveItPlanner # noqa: F401 from compas_fab.backends import PyBulletClient # noqa: F401 from compas_fab.backends import RosClient # noqa: F401 - from compas_fab.robots import RobotCell # noqa: F401 + from compas_fab.robots import RobotCellState # noqa: F401 @@ -79,15 +81,20 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None): kwargs["options"] = options kwargs["errback_name"] = "errback" + if not isinstance(robot_cell, RobotCell): + raise TypeError( + "robot_cell should be an instance of RobotCell instead of: {}".format(type(robot_cell).__name__) + ) + # The first step is to remove all the ACO because we cannot remove a CO if it is attached - step_1_result = await_callback(self._remove_aco_async, **kwargs) + step_1_result = await_callback(self._set_robot_cell_remove_aco_async, **kwargs) assert step_1_result.success, "Failed to remove attached collision objects" # Second step is to remove the non-existent objects and add the new ones # Objects that has the same id and the same object hash will be skipped # Objects that has the same id but different object hash will be updated - kwargs["robot_cell"] = robot_cell - step_2_result = await_callback(self._modify_co_async, **kwargs) + kwargs["new_robot_cell"] = robot_cell + step_2_result = await_callback(self._set_robot_cell_modify_co_async, **kwargs) assert step_2_result.success, "Failed to modify the objects in the robot cell" # Update the robot cell in the client @@ -99,7 +106,7 @@ def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None): return (step_1_result, step_2_result) - def _remove_aco_async(self, callback, errback, options=None): + def _set_robot_cell_remove_aco_async(self, callback, errback, options=None): # type: (Callable, Callable, RobotCellState, Optional[Dict]) -> None """Remove AttachedCollisionObject from the client. @@ -111,6 +118,13 @@ def _remove_aco_async(self, callback, errback, options=None): planner = self # type: MoveItPlanner # noqa: F841 client = planner.client # type: RosClient + options = options or {} + verbose = options.get("verbose", False) + + def verbose_print(msg): + if verbose: + print(msg) + # Get the last robot state planning_scene = planner.get_planning_scene() # type: PlanningScene robot_state = planning_scene.robot_state @@ -133,7 +147,7 @@ def _remove_aco_async(self, callback, errback, options=None): ) link_names.append(link_name) attached_collision_objects.append(aco) - print("SET_RC_1: Removed all ACOs from link {}".format(link_name)) + verbose_print("SET_RC_1: Removed all ACOs from link {}".format(link_name)) # NOTE: I'm not sure how we can skip the request if there are no ACOs to remove robot_state = RobotState( @@ -145,7 +159,7 @@ def _remove_aco_async(self, callback, errback, options=None): request = scene.to_request(client.ros_distro) self.APPLY_PLANNING_SCENE(client, request, callback, errback) - def _modify_co_async(self, callback, errback, robot_cell, options=None): + def _set_robot_cell_modify_co_async(self, callback, errback, new_robot_cell, options=None): # type: (Callable, Callable, RobotCell, Optional[Dict]) -> None """ @@ -159,10 +173,16 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): """ planner = self # type: MoveItPlanner # noqa: F841 client = planner.client # type: PyBulletClient - robot_cell = client.robot_cell - assert robot_cell, "Robot cell should not be None" - assert robot_cell.robot, "Robot cell should have a robot" + options = options or {} + verbose = options.get("verbose", False) + + def verbose_print(msg): + if verbose: + print(msg) + + assert new_robot_cell, "Robot cell should not be None" + assert new_robot_cell.robot_model, "Robot cell should have a robot model" # NOTE: This function will only create CollisionObjects (not attached), # The `set_robot_cell_state`` is responsible of the creation of AttachedCollisionObjects. @@ -170,7 +190,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): collision_objects = [] # Step 0 filter Rigid Bodies that has no collision geometry - rigid_body_models = {k: v for k, v in robot_cell.rigid_body_models.items() if v.collision_meshes} + rigid_body_models = {k: v for k, v in new_robot_cell.rigid_body_models.items() if v.collision_meshes} # Step 1 # Remove rigid bodies from the previous planning scene that are not in the new robot cell @@ -183,7 +203,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): collision_objects.append(co) # Remove the hash from the dictionary del planner._current_rigid_body_hashes[rigid_body_id] - print("SET_RC_2: Rigid body '{}' removed from the planning scene".format(rigid_body_id)) + verbose_print("SET_RC_2: Rigid body '{}' removed from the planning scene".format(rigid_body_id)) # Step 2 # Add / update rigid bodies to the planning scene @@ -194,7 +214,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): # Compare the hash of the rigid body with the previous one if rigid_body_id in planner._current_rigid_body_hashes: if rigid_body_hash == planner._current_rigid_body_hashes[rigid_body_id]: - print( + verbose_print( "SET_RC_2: Rigid body '{}' skipped because is already in the planning scene".format( rigid_body_id ) @@ -205,7 +225,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): ros_meshes = [Mesh.from_mesh(m) for m in rigid_body.collision_meshes_in_meters] collision_object = CollisionObject( # Header is robot.root_name when the object is not attached - header=Header(frame_id=robot_cell.root_name), + header=Header(frame_id=new_robot_cell.root_name), # The identifier of the rigid body, matches with the robot_cell rigid_body_id id=rigid_body_id, # List of `compas_fab.backends.ros.messages.shape_msgs.Meshes` @@ -219,7 +239,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): operation=CollisionObject.ADD, ) collision_objects.append(collision_object) - print("SET_RC_2: Rigid body '{}' added to the planning scene".format(rigid_body_id)) + verbose_print("SET_RC_2: Rigid body '{}' added to the planning scene".format(rigid_body_id)) # Update the hash of the rigid body planner._current_rigid_body_hashes[rigid_body_id] = rigid_body_hash @@ -229,7 +249,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): # Tool CollisionObjects have an id equal to `tool_id` + "_" + `link_name` tool_ids = [] - for tool_id, tool_model in robot_cell.tool_models.items(): + for tool_id, tool_model in new_robot_cell.tool_models.items(): for link in tool_model.iter_links(): if link.collision: tool_ids.append("{}_{}".format(tool_id, link.name)) @@ -244,11 +264,11 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): collision_objects.append(co) # Remove the hash from the dictionary del planner._current_tool_hashes[collision_object_id] - print("SET_RC_2: Tool Link '{}' removed from the planning scene".format(collision_object_id)) + verbose_print("SET_RC_2: Tool Link '{}' removed from the planning scene".format(collision_object_id)) # Step 4 # Add / updated tools to the planning scene - for tool_id, tool_model in robot_cell.tool_models.items(): + for tool_id, tool_model in new_robot_cell.tool_models.items(): # For each tool, iterate through its links, each link that has geometry(s) will create one collision object for link in tool_model.iter_links(): # In theory the `tool_id` from the tool_model dictionary is the same as the `tool_model.name`. @@ -258,7 +278,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): # Skip links that are already in the backend if collision_object_id in planner._current_tool_hashes: if link_hash == planner._current_tool_hashes[collision_object_id]: - print( + verbose_print( "SET_RC_2: Link '{}' skipped because it is already in the planning scene".format( collision_object_id ) @@ -269,7 +289,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): collision_meshes = tool_model.get_link_collision_meshes(link) # Skip links that have no collision geometry if not collision_meshes: - print( + verbose_print( "SET_RC_2: Link '{}' skipped because it has no collision geometry".format(collision_object_id) ) continue @@ -277,7 +297,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): # For each mesh in the link, create a CollisionMesh kwargs = {} # The frame_id needs be the root name of the robot - kwargs["header"] = Header(frame_id=robot_cell.root_name) + kwargs["header"] = Header(frame_id=new_robot_cell.root_name) # id is the identifier of the rigid body kwargs["id"] = collision_object_id # List of `compas_fab.backends.ros.messages.shape_msgs.Meshes` @@ -291,7 +311,7 @@ def _modify_co_async(self, callback, errback, robot_cell, options=None): kwargs["pose"] = Pose() co = CollisionObject(**kwargs) collision_objects.append(co) - print("SET_RC_2: Tool Link '{}' added to the planning scene".format(link.name)) + verbose_print("SET_RC_2: Tool Link '{}' added to the planning scene".format(link.name)) # Update the hash of the tool planner._current_tool_hashes[collision_object_id] = link_hash diff --git a/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell_state.py b/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell_state.py index d33175f09..18e4524e4 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell_state.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_set_robot_cell_state.py @@ -72,7 +72,7 @@ def set_robot_cell_state(self, robot_cell_state, options=None): """ kwargs = {} kwargs["robot_cell_state"] = robot_cell_state - kwargs["options"] = options + kwargs["options"] = options or {} kwargs["errback_name"] = "errback" robot_cell = self.client.robot_cell # type: RobotCell @@ -83,16 +83,16 @@ def set_robot_cell_state(self, robot_cell_state, options=None): # First step is to remove all the ACO attachments, this facilitates moving the position of attached objects # Robot configuration is also updated next step will move objects correctly based on this new configuration - step_1_result = await_callback(self.remove_aco_and_update_config_async, **kwargs) + step_1_result = await_callback(self._set_rcs_remove_aco_and_update_config_async, **kwargs) assert step_1_result.success, "Failed to remove attached collision objects" # Second step is to update the position of all tools and rigid bodies in the scene # Attached objects positions are updated relative to the links they attached to # Non-attached objects positions are relative to the robot base link (aka world) - step_2_result = await_callback(self.update_co_async, **kwargs) + step_2_result = await_callback(self._set_rcs_update_co_async, **kwargs) assert step_2_result.success # Third step is to re-create the ACO attachments # This will ensure the objects move with the robot when planning - step_3_result = await_callback(self.create_aco_async, **kwargs) + step_3_result = await_callback(self._set_rcs_create_aco_async, **kwargs) assert step_3_result.success # Update the client's robot cell state @@ -100,8 +100,8 @@ def set_robot_cell_state(self, robot_cell_state, options=None): return (step_1_result, step_2_result, step_3_result) - def remove_aco_and_update_config_async(self, callback, errback, robot_cell_state, options=None): - # type: (Callable, Callable, RobotCellState, Optional[Dict]) -> None + def _set_rcs_remove_aco_and_update_config_async(self, callback, errback, robot_cell_state, options): + # type: (Callable, Callable, RobotCellState, Dict) -> None """Remove AttachedCollisionObject from the client. All AttachedCollisionObjects are removed, converting them back to CollisionObjects in the PlanningSceneWorld. @@ -110,6 +110,11 @@ def remove_aco_and_update_config_async(self, callback, errback, robot_cell_state client = planner.client # type: RosClient robot_cell = client.robot_cell + # Convenience function for printing verbose messages + def verbose_print(msg): + if options.get("verbose", False): + print(msg) + # Get the last robot state planning_scene = planner.get_planning_scene() # type: PlanningScene robot_state = planning_scene.robot_state @@ -132,7 +137,7 @@ def remove_aco_and_update_config_async(self, callback, errback, robot_cell_state ) link_names.append(link_name) attached_collision_objects.append(aco) - print("SET_RCS_1: Removed all ACOs from link {}".format(link_name)) + verbose_print("SET_RCS_1: Removed all ACOs from link {}".format(link_name)) # Update robot configuration configuration = robot_cell_state.robot_configuration @@ -159,8 +164,8 @@ def remove_aco_and_update_config_async(self, callback, errback, robot_cell_state request = scene.to_request(client.ros_distro) self.APPLY_PLANNING_SCENE(client, request, callback, errback) - def update_co_async(self, callback, errback, robot_cell_state, options=None): - # type: (Callable, Callable, RobotCellState, Optional[Dict]) -> None + def _set_rcs_update_co_async(self, callback, errback, robot_cell_state, options): + # type: (Callable, Callable, RobotCellState, Dict) -> None """Update the position of CollisionObjects in the client. Non-attached objects (RigidBody and Tools) are updated in the PlanningSceneWorld. @@ -173,6 +178,11 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): robot_cell = client.robot_cell root_name = robot_cell.root_name + # Convenience function for printing verbose messages + def verbose_print(msg): + if options.get("verbose", False): + print(msg) + # Update pose of all non-attached rigid bodies collision_objects = [] for rb_id, rigid_body_state in robot_cell_state.rigid_body_states.items(): @@ -185,7 +195,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print( + verbose_print( "SET_RCS_2: Moved CO {} for attachment to position of link {}".format( rb_id, rigid_body_state.attached_to_link ) @@ -202,7 +212,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print( + verbose_print( "SET_RCS_2: Moved CO {} for attachment to position of tool {}".format( rb_id, rigid_body_state.attached_to_tool ) @@ -216,7 +226,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print("SET_RCS_2: Moved CO {} to frame {}".format(rb_id, rigid_body_state.frame)) + verbose_print("SET_RCS_2: Moved CO {} to frame {}".format(rb_id, rigid_body_state.frame)) # Update the tools for tool_id, tool_state in robot_cell_state.tool_states.items(): @@ -236,7 +246,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print( + verbose_print( "SET_RCS_2: Moved Tool Base Link {} to attachment at Robot Link {}".format( collision_object_id, end_effector_link_name ) @@ -260,7 +270,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print( + verbose_print( "SET_RCS_2: Moved Tool Link {} to attachment at Robot Link {}".format( collision_object_id, end_effector_link_name ) @@ -279,7 +289,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print("SET_RCS_2: Moved Tool Base Link {} to stationary pose".format(collision_object_id)) + verbose_print("SET_RCS_2: Moved Tool Base Link {} to stationary pose".format(collision_object_id)) # If the tool is a kinematic tool, it has configurable joints # We need to find the poses of each links @@ -299,7 +309,7 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): operation=CollisionObject.MOVE, ) collision_objects.append(collision_object) - print("SET_RCS_2: Moved Tool Link {} to stationary pose".format(collision_object_id)) + verbose_print("SET_RCS_2: Moved Tool Link {} to stationary pose".format(collision_object_id)) # Apply the planning scene to the client world = PlanningSceneWorld(collision_objects=collision_objects) @@ -307,8 +317,8 @@ def update_co_async(self, callback, errback, robot_cell_state, options=None): request = scene.to_request(client.ros_distro) self.APPLY_PLANNING_SCENE(client, request, callback, errback) - def create_aco_async(self, callback, errback, robot_cell_state, options=None): - # type: (Callable, Callable, RobotCellState, Optional[Dict]) -> None + def _set_rcs_create_aco_async(self, callback, errback, robot_cell_state, options): + # type: (Callable, Callable, RobotCellState, Dict) -> None """Update the position of CollisionObjects in the client. Non-attached objects (RigidBody and Tools) are updated in the PlanningSceneWorld. @@ -320,6 +330,11 @@ def create_aco_async(self, callback, errback, robot_cell_state, options=None): client = planner.client # type: RosClient robot_cell = client.robot_cell + # Convenience function for printing verbose messages + def verbose_print(msg): + if options.get("verbose", False): + print(msg) + # ACO for attached rigid bodies attached_collision_objects = [] for rb_id, rigid_body_state in robot_cell_state.rigid_body_states.items(): @@ -337,7 +352,7 @@ def create_aco_async(self, callback, errback, robot_cell_state, options=None): weight=1.0, ) attached_collision_objects.append(aco) - print("SET_RCS_3: Added ACO for RigidBody {} to link {}".format(rb_id, link_name)) + verbose_print("SET_RCS_3: Added ACO for RigidBody {} to link {}".format(rb_id, link_name)) # For rigid bodies attached to tools elif rigid_body_state.attached_to_tool: tool_parent_group = robot_cell_state.tool_states[rigid_body_state.attached_to_tool].attached_to_group @@ -353,7 +368,7 @@ def create_aco_async(self, callback, errback, robot_cell_state, options=None): weight=1.0, ) attached_collision_objects.append(aco) - print( + verbose_print( "SET_RCS_3: Added ACO for RigidBody {} to tool {}".format(rb_id, rigid_body_state.attached_to_tool) ) # ACO for attached tools @@ -375,7 +390,7 @@ def create_aco_async(self, callback, errback, robot_cell_state, options=None): weight=1.0, ) attached_collision_objects.append(aco) - print( + verbose_print( "SET_RCS_3: Added ACO for Tool Link {} to Robot Link {}".format( collision_object_id, end_effector_link_name ) diff --git a/src/compas_fab/robots/robot_library.py b/src/compas_fab/robots/robot_library.py index bc285b48f..2a420134d 100644 --- a/src/compas_fab/robots/robot_library.py +++ b/src/compas_fab/robots/robot_library.py @@ -606,6 +606,101 @@ def ur5_cone_tool(cls, load_geometry=True): return robot_cell, robot_cell_state + @classmethod + def ur5_gripper_one_beam(cls, load_geometry=True): + # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState] + """Create and return the ur5 robot with a gripper tool attached. + One beam (a RigidBody) is included and is attached to the gripper. + A floor is also included. + + See :meth:`compas_fab.robots.RobotCellLibrary.ur5` and :meth:`compas_fab.robots.ToolLibrary.static_gripper_small` + for details on the robot and tool. + + Parameters + ---------- + load_geometry: :obj:`bool`, optional + Default is `True`, which means that the robot and tool geometry are loaded. + `False` can be used to speed up the creation of the robot cell, + but without geometry, the robot cell cannot be visualized and backend planners + cannot perform collision checking during planning. + + Returns + ------- + Tuple[:class:`compas_fab.robots.RobotCell`, :class:`compas_fab.robots.RobotCellState`] + Newly created instance of the robot cell and robot cell state. + """ + # --------------------------------------------------------------------- + # Load Robot and create RobotCell + # --------------------------------------------------------------------- + robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=load_geometry) + + # --------------------------------------------------------------------- + # Load Tools + # --------------------------------------------------------------------- + + gripper = ToolLibrary.static_gripper_small(load_geometry=load_geometry) + robot_cell.tool_models["gripper"] = gripper + + # --------------------------------------------------------------------- + # Load Rigid Bodies + # --------------------------------------------------------------------- + + # Z axis is the length of the beam, X axis points away from the robot + beam_length = 0.4 + beam = Box.from_corner_corner_height( + [0.0, -0.05, -beam_length * 0.5], [0.1, 0.05, -beam_length * 0.5], beam_length + ) + beam_mesh = Mesh.from_shape(beam) + robot_cell.rigid_body_models["beam"] = RigidBody.from_mesh(beam_mesh) + + # Static Floor as Collision Geometry + floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + robot_cell.rigid_body_models["floor"] = RigidBody.from_mesh(floor_mesh) + + # ------------------------------------------------------------------------ + # Re-Create RobotCellState after modifying the RobotCell + # ------------------------------------------------------------------------ + robot_cell_state = RobotCellState.from_robot_cell(robot_cell) + + # ------------------------------------------------------------------------ + # Tool Attachment + # ------------------------------------------------------------------------ + + # Attach the tool to the robot's main group + attachment_frame = Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]) + + # Gripper is allowed to touch the last link of the robot. + # However, do not use the following line to get the end effector link name, + # because it is not guaranteed to be the last link that has geometry in the robot chain. + # ee_link_name = robot.get_end_effector_link_name(robot_cell.main_group_name) + + # Instead, check the robot model and hard code the actual link name. + touch_links = ["wrist_3_link"] + # For UR10e, the last logical link is `tool0` (from robot.get_end_effector_link_name) + # However the last link with geometry attached is `wrist_3_link`. + + robot_cell_state.set_tool_attached_to_group( + "gripper", robot_cell.main_group_name, attachment_frame, touch_links + ) + # Note: There is a rotation to match the gripper's orientation because the last link in the abb robot + # does not follow the REP 199 convention. + + # ------------------------------------------------------------------------ + # Workpiece Attachment + # ------------------------------------------------------------------------ + + # Attach the beam to the gripper + robot_cell_state.set_rigid_body_attached_to_tool("beam", "gripper") + + # ------------------------------------------------------------------------ + # Static Rigid Body Touch Links + # ------------------------------------------------------------------------ + + # The floor is not attached to the robot, but it is allowed to touch the robot's base link. + robot_cell_state.rigid_body_states["floor"].touch_links = ["base_link_inertia"] + + return robot_cell, robot_cell_state + @classmethod def abb_irb4600_40_255_gripper_one_beam(cls, load_geometry=True): # type: (Optional[bool]) -> Tuple[RobotCell, RobotCellState]