From f356b4aabbdd84e077dde8bc3a60012c52536dd7 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 27 Jun 2024 11:56:17 +0800 Subject: [PATCH] WIP for setting robot cell and cell state --- .../files/01_set_robot_cell.py | 21 ++ .../backends/interfaces/__init__.py | 4 + .../backends/interfaces/backend_features.py | 41 ++++ src/compas_fab/backends/interfaces/client.py | 63 +++++- .../pybullet/backend_features/__init__.py | 2 + .../pybullet_set_robot_cell.py | 59 ++++++ .../move_it_inverse_kinematics.py | 14 +- src/compas_fab/robots/robot_cell.py | 180 +++++++++++++++++- 8 files changed, 364 insertions(+), 20 deletions(-) create mode 100644 docs/examples/05_backends_pybullet/files/01_set_robot_cell.py create mode 100644 src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py diff --git a/docs/examples/05_backends_pybullet/files/01_set_robot_cell.py b/docs/examples/05_backends_pybullet/files/01_set_robot_cell.py new file mode 100644 index 000000000..aa8d343f0 --- /dev/null +++ b/docs/examples/05_backends_pybullet/files/01_set_robot_cell.py @@ -0,0 +1,21 @@ +from compas.datastructures import Mesh + +import compas_fab +from compas_fab.backends import PyBulletClient +from compas_fab.backends import PyBulletPlanner +from compas_fab.robots import RobotCell +from compas_fab.robots import RigidBody + +with PyBulletClient() as client: + urdf_filepath = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf") + robot = client.load_robot(urdf_filepath) + + robot_cell = RobotCell(robot) + floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl")) + robot_cell.rigid_body_models["floor"] = RigidBody(floor_mesh) + + planner = PyBulletPlanner(client) + planner.set_robot_cell(robot_cell) + # The floor should appear in the PyBullet's GUI + + input("Press Enter to continue...") diff --git a/src/compas_fab/backends/interfaces/__init__.py b/src/compas_fab/backends/interfaces/__init__.py index 1e297bab1..101ad6ded 100644 --- a/src/compas_fab/backends/interfaces/__init__.py +++ b/src/compas_fab/backends/interfaces/__init__.py @@ -56,6 +56,8 @@ from .backend_features import RemoveAttachedCollisionMesh from .backend_features import RemoveCollisionMesh from .backend_features import ResetPlanningScene +from .backend_features import SetRobotCell +from .backend_features import SetRobotCellState from .client import ClientInterface from .client import PlannerInterface @@ -74,4 +76,6 @@ "RemoveCollisionMesh", "RemoveAttachedCollisionMesh", "ResetPlanningScene", + "SetRobotCell", + "SetRobotCellState", ] diff --git a/src/compas_fab/backends/interfaces/backend_features.py b/src/compas_fab/backends/interfaces/backend_features.py index 595027f5c..aa9ddf4e1 100644 --- a/src/compas_fab/backends/interfaces/backend_features.py +++ b/src/compas_fab/backends/interfaces/backend_features.py @@ -17,6 +17,7 @@ from compas_robots import Configuration # noqa: F401 from compas.geometry import Frame # noqa: F401 from compas_fab.backends.interfaces import ClientInterface # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 from compas_fab.robots import RobotCellState # noqa: F401 from compas_fab.robots import FrameTarget # noqa: F401 @@ -35,6 +36,13 @@ def __init__(self, client=None): # All backend features are assumed to be associated with a backend client. if client: self.client = client # type: ClientInterface + + # The current robot cell last set by user calling Planner.set_robot_cell() + self.robot_cell = None # type: RobotCell + + # The current robot cell state last set by user calling Planner.set_robot_cell_state() + self.robot_cell_state = None # type: RobotCellState + super(BackendFeature, self).__init__() @@ -45,6 +53,39 @@ def __init__(self, client=None): # does not support that feature. +class SetRobotCell(BackendFeature): + """Mix-in interface for implementing a planner's set robot cell feature.""" + + def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None): + # type: (RobotCell, Optional[RobotCellState], Optional[Dict]) -> None + """Pass the models in the robot cell to the planning client. + + The client keeps the robot cell models in memory and uses them for planning. + Calling this method will override the previous robot cell in the client. + It should be called only if the robot cell models have changed. + + """ + pass + + +class SetRobotCellState(BackendFeature): + """Mix-in interface for implementing a planner's set robot cell state feature.""" + + def set_robot_cell_state(self, robot_cell_state): + # type: (RobotCellState) -> None + """Set the robot cell state to the client. + + The client requires a robot cell state at the beginning of each planning request. + This cell state must correspond to the robot cell set earlier by :meth:`set_robot_cell`. + + This function is called automatically by planning functions that takes a start_state as input. + Therefore it is typically not necessary for the user to call this function, + except when used to trigger visualization using native backend canvas. + + """ + pass + + class ForwardKinematics(BackendFeature): """Mix-in interface for implementing a planner's forward kinematics feature.""" diff --git a/src/compas_fab/backends/interfaces/client.py b/src/compas_fab/backends/interfaces/client.py index e8a050386..200d10ad1 100644 --- a/src/compas_fab/backends/interfaces/client.py +++ b/src/compas_fab/backends/interfaces/client.py @@ -12,6 +12,7 @@ if TYPE_CHECKING: from compas_fab.robots import Robot from compas_fab.robots import RobotCell + from compas_fab.robots import RobotCellState class ClientInterface(object): @@ -29,7 +30,6 @@ class ClientInterface(object): def __init__(self): self._robot = None # type: Robot - self._robot_cell = None # type: RobotCell print("ClientInterface init") @property @@ -37,11 +37,6 @@ def robot(self): # type: () -> Robot return self._robot - @property - def robot_cell(self): - # type: () -> RobotCell - return self._robot_cell - class PlannerInterface(object): """Interface for all backend planners. @@ -62,18 +57,72 @@ class PlannerInterface(object): client : :class:`compas_fab.backends.interfaces.ClientInterface`, read-only The client instance associated with the planner. It cannot be changed after initialization. + + robot_cell : :class:`compas_fab.robots.RobotCell`, read-only + The robot cell instance previously passed to the client. + + robot_cell_state : :class:`compas_fab.robots.RobotCellState`, read-only + The last robot cell state instance passed to the client. + + """ def __init__(self, client=None): if client: self._client = client - + self._robot_cell = None # type: RobotCell + self._robot_cell_state = None # type: RobotCellState super(PlannerInterface, self).__init__() @property def client(self): return self._client + @property + def robot_cell(self): + # type: () -> RobotCell + return self._robot_cell + + @property + def robot_cell_state(self): + # type: () -> RobotCellState + return self._robot_cell_state + + # ========================================================================== + # collision objects robot cell and cell state + # ========================================================================== + + def set_robot_cell(self, *args, **kwargs): + """Pass the models in the robot cell to the planning client. + + The client keeps the robot cell models in memory and uses them for planning. + Calling this method will override the previous robot cell in the client. + It should be called only if the robot cell models have changed. + + Raises + ------ + BackendFeatureNotSupportedError + Planner does not have this feature. + """ + raise BackendFeatureNotSupportedError("Assigned planner does not have this feature.") + + def set_robot_cell_state(self, *args, **kwargs): + """Set the robot cell state to the client. + + The client requires a robot cell state at the beginning of each planning request. + This cell state must correspond to the robot cell set earlier by :meth:`set_robot_cell`. + + This function is called automatically by planning functions that takes a start_state as input. + Therefore it is typically not necessary for the user to call this function, + except when used to trigger visualization using native backend canvas. + + Raises + ------ + BackendFeatureNotSupportedError + Planner does not have this feature. + """ + raise BackendFeatureNotSupportedError("Assigned planner does not have this feature.") + # ========================================================================== # planning services # ========================================================================== diff --git a/src/compas_fab/backends/pybullet/backend_features/__init__.py b/src/compas_fab/backends/pybullet/backend_features/__init__.py index 3b1be94c9..13a5e7a41 100644 --- a/src/compas_fab/backends/pybullet/backend_features/__init__.py +++ b/src/compas_fab/backends/pybullet/backend_features/__init__.py @@ -32,6 +32,7 @@ PyBulletRemoveAttachedCollisionMesh, ) from compas_fab.backends.pybullet.backend_features.pybullet_remove_collision_mesh import PyBulletRemoveCollisionMesh +from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell import PyBulletSetRobotCell __all__ = [ @@ -42,4 +43,5 @@ "PyBulletInverseKinematics", "PyBulletRemoveAttachedCollisionMesh", "PyBulletRemoveCollisionMesh", + "PyBulletSetRobotCell", ] 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 new file mode 100644 index 000000000..77b46b132 --- /dev/null +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py @@ -0,0 +1,59 @@ +from compas_fab.backends.interfaces import SetRobotCell + +import compas + +if not compas.IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from typing import Dict # noqa: F401 + from typing import List # noqa: F401 + from typing import Tuple # noqa: F401 + + from compas_fab.robots import Robot # noqa: F401 + from compas_robots import Configuration # noqa: F401 + from compas.geometry import Frame # noqa: F401 + from compas_fab.backends.interfaces import ClientInterface # noqa: F401 + from compas_fab.robots import RobotCell # noqa: F401 + from compas_fab.robots import RobotCellState # noqa: F401 + from compas_fab.backends import PyBulletClient # noqa: F401 + +from compas_fab.backends.pybullet.const import STATIC_MASS + + +class PyBulletSetRobotCell(SetRobotCell): + + def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None): + # type: (RobotCell, Optional[RobotCellState], Optional[Dict]) -> None + """Pass the models in the robot cell to the Pybullet client. + + The client keeps the robot cell models in memory and uses them for planning. + Calling this method will override the previous robot cell in the client. + It should be called only if the robot cell models have changed. + + """ + client = self.client # type: PyBulletClient + + # TODO: Check for new, modified and removed objects compared to the + # TODO: previous robot cell state and update the PyBullet world accordingly + + # At the moment, all previous object in the PyBullet world are removed + # and the new robot cell is added to the PyBullet world + + # Remove all objects from the PyBullet world + client.remove_all_tools() + client.remove_all_rigid_bodies() + + # Add the robot cell to the PyBullet world + for name, tool in robot_cell.tool_models.items(): + client.add_tool(name, tool.mesh, tool.mass) + for name, rigid_body in robot_cell.rigid_body_models.items(): + client.add_rigid_body(name, rigid_body.mesh, rigid_body.mass) + + # Update the robot cell in the client + self._robot_cell = robot_cell + + # If a robot cell state is provided, update the client's robot cell state + if robot_cell_state: + self.set_robot_cell_state(robot_cell_state, options) diff --git a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py index 595078cb1..81cf6ebe5 100644 --- a/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py +++ b/src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py @@ -142,6 +142,10 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options= ------ :obj:`tuple` of (:obj:`list` of :obj:`float`, :obj:`list` of :obj:`str`) A tuple of 2 elements containing a list of joint positions and a list of matching joint names. + + Examples + -------- + """ options = options or {} robot = self.client.robot @@ -151,7 +155,7 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options= # Set scene # TODO: Implement start_state - self.client.robot_cell.assert_cell_state_match(start_state) + self.robot_cell.assert_cell_state_match(start_state) start_state = start_state or RobotCellState.from_robot_configuration(robot) # Start configuration @@ -164,15 +168,17 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options= if robot.need_scaling: target_frame = target_frame.scaled(1.0 / robot.scale_factor) # Now target_frame is back in meter scale - attached_tool = self.client.robot_cell.get_attached_tool(start_state, group) + attached_tool = self.robot_cell.get_attached_tool(start_state, group) if attached_tool: target_frame = from_tcf_to_t0cf(target_frame, attached_tool.frame) # Attached tool frames does not need scaling because Tools are modelled in meter scale # Scale Attached Collision Meshes + attached_collision_meshes = self.robot_cell.get_attached_rigid_bodies_as_attached_collision_meshes( + start_state, group + ) if robot.need_scaling: - acms = options.get("attached_collision_meshes", []) # type: List[AttachedCollisionMesh] - for acm in acms: + for acm in attached_collision_meshes: acm.collision_mesh.scale(1.0 / robot.scale_factor) # Compose the kwargs for the async function diff --git a/src/compas_fab/robots/robot_cell.py b/src/compas_fab/robots/robot_cell.py index dab033c06..5998aa1ee 100644 --- a/src/compas_fab/robots/robot_cell.py +++ b/src/compas_fab/robots/robot_cell.py @@ -1,6 +1,8 @@ import compas from compas.data import Data +from compas_fab.robots import AttachedCollisionMesh +from compas_fab.robots import CollisionMesh if not compas.IPY: from typing import TYPE_CHECKING @@ -23,6 +25,9 @@ class RobotCell(Data): Tools include those that are attached to the robot, and those that are not attached but placed in the environment. + Notes regarding scaling: All models in the RobotCell should be in meters. If user models are in millimeters, + they should be scaled to meters before being added to the RobotCell. + Rigid bodies are objects can be used to represent: - Workpieces that are attached to the tip of a tool - Workpieces that are placed in the environment and are not attached @@ -30,7 +35,7 @@ class RobotCell(Data): - Static obstacles in the environment """ - def __init__(self, robot_model, tool_models={}, rigid_body_models={}, static_environment_models={}): + def __init__(self, robot_model, tool_models={}, rigid_body_models={}): super(RobotCell, self).__init__() self.robot_model = robot_model # type: RobotModel self.tool_models = tool_models # type: Dict[str, ToolModel] @@ -72,25 +77,116 @@ def assert_cell_state_match(self, cell_state): def get_attached_tool(self, robot_cell_state, group): # type: (RobotCellState, str) -> Optional[ToolModel] - """Returns the tool attached to the group in the robot cell state.""" + """Returns the tool attached to the group in the robot cell state. + + There can only be a maximum of one tool attached to a planning group. + + Returns + ------- + :class:`compas_robots.ToolModel` | None + The tool attached to the group in the robot cell state. + None if no tool is attached. + """ tool_id = robot_cell_state.get_attached_tool_id(group) - if tool_id: - return self.tool_models[tool_id] + return self.tool_models.get(tool_id) + + def get_attached_workpieces(self, robot_cell_state, group): + # type: (RobotCellState, str) -> List[RigidBody] + """Returns the workpiece attached to the tool attached to the group in the robot cell state. + + There can be more than one workpiece attached to a tool. + + Returns + ------- + :class:`compas_fab.robots.RigidBody` | None + The workpiece attached to the tool attached to the group in the robot cell state. + None if no workpiece is attached or no tool is attached. + """ + bodies = [] + workpiece_id = robot_cell_state.get_attached_workpiece_ids(group) + for id in workpiece_id: + self.rigid_body_models[id] + return bodies + + def get_attached_rigid_bodies(self, robot_cell_state, group): + # type: (RobotCellState, str) -> List[Mesh] + """Returns the rigid bodies attached to the links of the robot as AttachedCollisionMesh. + + This does not include the tool and the workpieces attached to the tools. + Use `get_attached_tool` and `get_attached_workpieces` to get those. + + """ + bodies = [] + rigid_body_ids = robot_cell_state.get_attached_rigid_body_ids() + for id in rigid_body_ids: + bodies.append(self.rigid_body_models[id]) + return bodies + + def get_attached_rigid_bodies_as_attached_collision_meshes(self, robot_cell_state, group): + # type: (RobotCellState, str) -> List[AttachedCollisionMesh] + """Returns the rigid bodies attached to the links of the robot as AttachedCollisionMesh. + + This does not include the tool and the workpieces attached to the tools. + Use `get_attached_tool` and `get_attached_workpieces` to get those. + + """ + rigid_body_ids = robot_cell_state.get_attached_rigid_body_ids() + attached_collision_meshes = [] + for id in rigid_body_ids: + rb_state = robot_cell_state.rigid_body_states[id] + mesh = self.rigid_body_models[id].collision_meshes # TODO join? + collision_mesh = CollisionMesh(mesh, id, rb_state.frame) + link_name = rb_state.attached_to_link + attached_collision_meshes.append(AttachedCollisionMesh(rigid_body, id)) + return attached_collision_meshes class RigidBody(Data): """Represents a rigid body.""" - def __init__(self, name, visual_meshes=[], collision_meshes=[]): + def __init__(self, visual_meshes=[], collision_meshes=[]): + # type: (List[Mesh] | Mesh, List[Mesh] | Mesh) -> None + """Represents a rigid body. + + A rigid body can have different visual and collision meshes. + + Notes + ----- + The number of objects in the collision meshes does not have to be the same as the visual meshes. + If the user wants to use the same mesh for both visualization and collision checking, + place the meshes in visual_meshes and leave the collision_meshes empty. + + Parameters + ---------- + visual_meshes : list of :class:`compas.datastructures.Mesh` | :class:`compas.datastructures.Mesh` + The visual meshes of the rigid body used for visualization purpose. + They can be more detailed for realistic visualization without affecting planning performance. + collision_meshes : list of :class:`compas.datastructures.Mesh` | :class:`compas.datastructures.Mesh` + The collision meshes of the rigid body used for collision checking. + They should be less detailed (fewer polygons) for better planning performance. + If this list is empty, the visual meshes will be used for collision checking. + + Attributes + ---------- + visual_meshes : list of :class:`compas.datastructures.Mesh` + A list of meshes for visualization purpose. + collision_meshes : list of :class:`compas.datastructures.Mesh` + A list of meshes for collision checking. + """ + # type: (str, List[Mesh], List[Mesh]) -> None super(RigidBody, self).__init__() - self.name = name # type: str self.visual_meshes = visual_meshes # type: List[Mesh] + # Ensure that visual_meshes is a list + if not isinstance(visual_meshes, list): + self.visual_meshes = [visual_meshes] self.collision_meshes = collision_meshes # type: List[Mesh] + # Ensure that collision_meshes is a list + if not isinstance(collision_meshes, list): + self.collision_meshes = [collision_meshes] @property def __data__(self): return { - "name": self.name, "visual_meshes": self.visual_meshes, "collision_meshes": self.collision_meshes, } @@ -154,11 +250,57 @@ def from_robot_configuration(cls, robot, configuration=None, group=None): def get_attached_tool_id(self, group): # type: (str) -> Optional[str] - """Returns the id of the tool attached to the planning group.""" + """Returns the id of the tool attached to the planning group. + + There can only be a maximum of one tool attached to a planning group. + + Returns + ------- + str | None + The id of the tool attached to the planning group. + None if no tool is attached. + """ for tool_id, tool_state in self.tool_states.items(): if tool_state.attached_to_group == group: return tool_id + def get_attached_workpiece_ids(self, group): + # type: (str) -> Optional[str] + """Returns the id of the workpiece attached to the tool attached to the planning group. + + There can be more than one workpiece attached to a tool. + + Returns + ------- + List[str] + The ids of the workpieces attached to the tool attached to the planning group. + + """ + tool_id = self.get_attached_tool_id(group) + if not tool_id: + return None + ids = [] + for rigid_body_id, rigid_body_state in self.rigid_body_states.items(): + if rigid_body_state.attached_to_tool == tool_id: + ids.append(rigid_body_id) + return ids + + def get_attached_rigid_body_ids(self): + # type: (str) -> List[str] + """Returns the ids of the rigid bodies attached to the robot + + This does not include the tools attached to the robot and the workpieces attached to the tools. + + Returns + ------- + List[str] + The ids of the rigid bodies attached to the robot. + """ + ids = [] + for rigid_body_id, rigid_body_state in self.rigid_body_states.items(): + if rigid_body_state.attached_to_link: + ids.append(rigid_body_id) + class ToolState(Data): """Represents the state of a tool in a RobotCell. @@ -209,7 +351,8 @@ class RigidBodyState(Data): When representing a workpiece that is attached to a tool, the `attached_to_tool` attribute should be set. When representing a collision geometry (such as robotic backpacks) that is attached to a link of the robot, - the `attached_to_link` attribute should be set. Otherwise, they should be `None`. + the `attached_to_link` attribute should be set. Otherwise, if the rigid body is stationary in the environment, + both attributes should be set to `None`. When representing a workpiece that is currently not in the scene, the `is_hidden` attribute should be set to True. This will turn off collision checking for the workpiece. @@ -231,5 +374,24 @@ class RigidBodyState(Data): """ def __init__(self, frame, attached_to_link=None, attached_to_tool=None, grasp=None, is_hidden=False): + super(RigidBodyState, self).__init__() + self.frame = frame + self.attached_to_link = attached_to_link + self.attached_to_tool = attached_to_tool if attached_to_link and attached_to_tool: raise ValueError("A RigidBodyState cannot be attached to both a link and a tool.") + self.grasp = grasp + # Convert grasp to a Frame if it is a Transformation + if isinstance(grasp, Transformation): + self.grasp = Frame.from_transformation(grasp) + self.is_hidden = is_hidden + + @property + def __data__(self): + return { + "frame": self.frame, + "attached_to_link": self.attached_to_link, + "attached_to_tool": self.attached_to_tool, + "grasp": self.grasp, + "is_hidden": self.is_hidden, + }