From 67aef254358f285e93d504ae735c86b0b7b0a718 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Mon, 6 Nov 2023 10:07:56 +0800 Subject: [PATCH 01/28] Initial intro of Action and State classes --- src/compas_fab/planning/__init__.py | 54 +++++ src/compas_fab/planning/action.py | 342 ++++++++++++++++++++++++++++ src/compas_fab/planning/state.py | 238 +++++++++++++++++++ 3 files changed, 634 insertions(+) create mode 100644 src/compas_fab/planning/__init__.py create mode 100644 src/compas_fab/planning/action.py create mode 100644 src/compas_fab/planning/state.py diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py new file mode 100644 index 0000000000..c6efacaf5c --- /dev/null +++ b/src/compas_fab/planning/__init__.py @@ -0,0 +1,54 @@ +""" +******************************************************************************** +compas_fab.planning +******************************************************************************** + +.. currentmodule:: compas_fab.planning + +Planning +-------- + +This package contains classes for modeling the actions that are performed by the robots. +Actions are abstractions of the robot's capability and they are used for planning tasks +that are performed by the robot. Actions are to +Process class planning and execution backends to exchange information. + +.. autosummary:: + :toctree: generated/ + :nosignatures: + + Action + SceneState + +""" + +from .action import ( + Action, + RoboticMovement, + LinearMovement, + FreeMovement, + OpenGripper, + CloseGripper, + LoadWorkpiece, +) + +from .state import ( + SceneState, + WorkpieceState, + ToolState, + RobotState, +) + +__all__ = [ + "Action", + "RoboticMovement", + "LinearMovement", + "FreeMovement", + "OpenGripper", + "CloseGripper", + "LoadWorkpiece", + "SceneState", + "WorkpieceState", + "ToolState", + "RobotState", +] diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py new file mode 100644 index 0000000000..4ee5ec1e7e --- /dev/null +++ b/src/compas_fab/planning/action.py @@ -0,0 +1,342 @@ +from compas.data import Data +from compas.geometry import Frame, Transformation +from compas_fab.robots import Configuration, JointTrajectory # noqa: F401 + +try: + from compas_fab.planning import SceneState # noqa: F401 +except ImportError: + from .state import SceneState # noqa: F401 + +try: + from typing import Optional # noqa: F401 +except ImportError: + pass + +__all__ = [ + "Action", + "RoboticMovement", + "LinearMovement", + "FreeMovement", + "OpenGripper", + "CloseGripper", +] + + +class Action(Data): + """Base class for all actions.""" + + def __init__(self): + super(Action, self).__init__() + self.act_n = -1 # type: int + self.tag = "Generic Action" + + @property + def data(self): + data = {} + # For class inhereited from Action, use the following line + # data = super(Action, self).data + data["act_n"] = self.act_n + data["tag"] = self.tag + return data + + @data.setter + def data(self, data): + # For class inhereited from Action, use the following line + # super(Action, type(self)).data.fset(self, data) + self.act_n = data["act_n"] + self.tag = data["tag"] + + def apply_to(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + This method is called by the assembly process to apply the action to the scene state. + """ + raise NotImplementedError + + +class RoboticMovement(Action): + """Base class for all robotic movements. + - Robotic movements are actions that changes the configuration of the robot. + - Robotic movements require motion planning and are planned by a motion planner. + + Attributes + ---------- + robot_target : :class:`compas.geometry.Frame` + The target frame of the robot. In world coordinate frame. + allowed_collision_pairs : list(tuple(str,str)) + List of pairs of collision objects that are allowed to collide. + fixed_configuration : :class:`compas_fab.robots.Configuration`, optional + The configuration of the robot if the target needs a fixed configuration. + For example, if a taught position is used as a target. + intermediate_planning_waypoint : list(:class:`compas_fab.robots.Configuration`), optional + List of configurations that are used as waypoints during planning. + planned_trajectory : :class:`compas_fab.robots.JointTrajectory` + The planned trajectory of the robotic movement. + planner_seed : int + The random seed used by the planner to generate the trajectory. + """ + + def __init__(self): + super(RoboticMovement, self).__init__() + self.tag = "Generic Action" + + # Before Planning + self.robot_target = None # type: Frame + self.allowed_collision_pairs = [] # type: list(tuple(str,str)) + self.fixed_configuration = None # type: Optional[Configuration] + self.intermediate_planning_waypoint = [] # type: list(Configuration) + + # After Planning + self.planned_trajectory = None # type: Optional[JointTrajectory] + self.planner_seed = None # type: Optional[int] + + @property + def data(self): + data = super(RoboticMovement, self).data + data["tag"] = self.tag + data["robot_target"] = self.robot_target + data["allowed_collision_pairs"] = self.allowed_collision_pairs + data["fixed_configuration"] = self.fixed_configuration + data["intermediate_planning_waypoint"] = self.intermediate_planning_waypoint + data["planned_trajectory"] = self.planned_trajectory + data["planner_seed"] = self.planner_seed + return data + + @data.setter + def data(self, data): + super(RoboticMovement, type(self)).data.fset(self, data) + self.tag = data.get("tag", self.tag) + self.robot_target = data.get("robot_target", self.robot_target) + self.allowed_collision_pairs = data.get("allowed_collision_pairs", self.allowed_collision_pairs) + self.fixed_configuration = data.get("fixed_configuration", self.fixed_configuration) + self.intermediate_planning_waypoint = data.get( + "intermediate_planning_waypoint", self.intermediate_planning_waypoint + ) + self.planned_trajectory = data.get("planned_trajectory", self.planned_trajectory) + self.planner_seed = data.get("planner_seed", self.planner_seed) + + def apply_to(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + + The SceneState is updated with the new robot state. + If a trajectory is available, the robot state is updated with the last configuration of the trajectory. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + robot_state = scene_state.get_robot_state() + if self.planned_trajectory is not None: + robot_state.configuration = self.planned_trajectory.points[-1] + elif self.fixed_configuration is not None: + robot_state.configuration = self.fixed_configuration + robot_state.frame = self.robot_target + if debug: + print("Robot Moved.") + + # Transform attached objects + attached_tool_id = scene_state.get_attached_tool_id() + if attached_tool_id is not None: + attached_tool_state = scene_state.get_tool_state(attached_tool_id) + assert attached_tool_state.attached_to_robot, "Inconsistency: Attached tool must be attached to robot." + # attached_tool_state.frame = robot_state.frame * attached_tool_state.attached_to_robot_grasp + attached_tool_state.frame = Frame.from_transformation( + Transformation.from_frame(robot_state.frame) * attached_tool_state.attached_to_robot_grasp + ) + if debug: + print("- Attached Tool %s Followed." % attached_tool_id) + # Transform attached workpieces + attached_workpiece_id = scene_state.get_attached_workpiece_id() + if attached_workpiece_id is not None: + attached_workpiece = scene_state.get_workpiece_state(attached_workpiece_id) + assert ( + attached_workpiece.attached_to_tool_id == attached_tool_id + ), "Inconsistency: Attached workpiece must be attached to the attached tool." + attached_workpiece.frame = Frame.from_transformation( + Transformation.from_frame(attached_tool_state.frame) * attached_workpiece.attached_to_tool_grasp + ) + if debug: + print("- Attached Workpiece %s Followed." % attached_workpiece_id) + + +class LinearMovement(RoboticMovement): + """Base class for all linear robotic movements. + Linear robotic movements are planned by Linear Motion Planners. + """ + + +class FreeMovement(RoboticMovement): + """Base class for all free robotic movements. + Free robotic movements are planned by Free Motion Planners. + """ + + +class OpenGripper(Action): + """Action to open the gripper. + If the gripper is closed around a workpiece, the workpiece is detached from the gripper. + It is possible to open the gripper with or without a workpiece attached. + """ + + def __init__(self): + super(OpenGripper, self).__init__() + + @property + def data(self): + data = super(OpenGripper, self).data + return data + + @data.setter + def data(self, data): + super(OpenGripper, type(self)).data.fset(self, data) + + def apply_to(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + + The SceneState is updated with the new robot state. + It is possible to open the gripper with or without a workpiece attached. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + # Transform attached objects + attached_tool_id = scene_state.get_attached_tool_id() + assert attached_tool_id is not None, "Inconsistency: No tool attached to robot." + # attached_tool_state = scene_state.get_tool_state(attached_tool_id) + # TODO: Check if tool is a gripper + # TODO: Change the configuration of the gripper to opened + + # Transform attached workpieces + attached_workpiece_id = scene_state.get_attached_workpiece_id() + if debug: + print("Gripper opened.") + if attached_workpiece_id is not None: + attached_workpiece = scene_state.get_workpiece_state(attached_workpiece_id) + attached_workpiece.attached_to_tool_id = None + attached_workpiece.attached_to_tool_grasp = None + if debug: + print("- Workpiece %s detached from tool." % attached_workpiece_id) + + +class CloseGripper(Action): + """Action to close the gripper. + + If the gripper is closed around a workpiece, the workpiece is attached to the gripper. + + Attributes + ---------- + tool_id : str + The id of the gripper tool that is used. + attached_workpiece_id : str, optional + The id of the workpiece attached to the gripper. + If the workpiece is not specified, the gripper is assumed to be empty. + attached_workpiece_grasp : :class:`compas.geometry.Transformation`, optional + The grasp frame of the workpiece relative to the robot flange. + If the workpiece_frame is not specified, the workpiece Frame is + assumed to be the same as the gripper's TCP. + """ + + def __init__(self): + super(CloseGripper, self).__init__() + self.tool_id = None # type: Frame + self.attached_workpiece_id = None # type: Frame + self.attached_workpiece_grasp = None # type: Frame + + @property + def data(self): + data = super(CloseGripper, self).data + data["attached_workpiece_id"] = self.attached_workpiece_id + data["attached_workpiece_grasp"] = self.attached_workpiece_grasp + return data + + @data.setter + def data(self, data): + super(CloseGripper, type(self)).data.fset(self, data) + self.attached_workpiece_id = data.get("attached_workpiece_id", self.attached_workpiece_id) + self.attached_workpiece_grasp = data.get("attached_workpiece_grasp", self.attached_workpiece_grasp) + + def apply_to(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + + The SceneState is updated with the new robot state. + It is possible to open the gripper with or without a workpiece attached. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + # Transform attached objects + attached_tool_id = scene_state.get_attached_tool_id() + assert attached_tool_id is not None, "Inconsistency: No tool attached to robot." + attached_tool_state = scene_state.get_tool_state(attached_tool_id) + # TODO: Check if tool is a gripper + # TODO: Change the configuration of the gripper to closed + + if debug: + print("Gripper closed.") + # Transform attached workpieces + if self.attached_workpiece_id is not None: + existing_workpiece_id = scene_state.get_attached_workpiece_id() + assert existing_workpiece_id is None, "Inconsistency: Another workpiece is already attached to the tool." + attached_workpiece = scene_state.get_workpiece_state(self.attached_workpiece_id) + # Update the workpiece grasp and frame + attached_workpiece.attached_to_tool_id = attached_tool_id + attached_workpiece.attached_to_tool_grasp = self.attached_workpiece_grasp or Transformation() + if debug: + print("- Workpiece %s attached to tool." % self.attached_workpiece_id) + # attached_workpiece.frame = attached_tool_state.frame * attached_workpiece.attached_to_tool_grasp + attached_workpiece.frame = Frame.from_transformation( + Transformation.from_frame(attached_tool_state.frame) * attached_workpiece.attached_to_tool_grasp + ) + + +class LoadWorkpiece(Action): + """Action to load a workpiece, probably performed manually. + This moves the workpiece to a specific frame. + + Attributes + ---------- + workpiece_id : str + The id of the workpiece to be loaded. + """ + + def __init__(self): + super(LoadWorkpiece, self).__init__() + self.workpiece_id = None # type: str + self.frame = None # type: Frame + + @property + def data(self): + data = super(LoadWorkpiece, self).data + data["workpiece_id"] = self.workpiece_id + data["frame"] = self.frame + return data + + @data.setter + def data(self, data): + super(LoadWorkpiece, type(self)).data.fset(self, data) + self.workpiece_id = data.get("workpiece_id", self.workpiece_id) + self.frame = data.get("frame", self.frame) + + def apply_to(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + The SceneState is updated with the new robot state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + # Transform attached objects + workpiece_state = scene_state.get_workpiece_state(self.workpiece_id) + assert workpiece_state.attached_to_tool_id is None, "Inconsistency: Workpiece is already attached to a tool." + workpiece_state.frame = self.frame + if debug: + print("Workpiece %s loaded to new location." % self.workpiece_id) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py new file mode 100644 index 0000000000..5018afedb4 --- /dev/null +++ b/src/compas_fab/planning/state.py @@ -0,0 +1,238 @@ +from compas.data import Data +from compas.geometry import Frame, Transformation # noqa F401 +from compas.robots import Configuration # noqa F401 + +try: + from typing import Optional # noqa F401 + from typing import Dict # noqa F401 +except ImportError: + pass + +__all__ = [ + "SceneState", + "WorkpieceState", + "ToolState", + "RobotState", +] + + +class SceneState(Data): + """Class for describing a scene. A scene contains the states of all objects. + Current implementation supports only one robot in the scene. + + There can be multiple tools and workpieces in the scene. However, only + one (or none) tool can be attached to the robot at any time. Similarly, + there can only be one (or none) workpiece attached to the tool at any time. + + Attributes + ---------- + workpiece_ids : list of str + The ids of the workpieces in the scene. + tool_ids : list of str + The ids of the tools in the scene. + workpiece_states : `dict` of :class:`WorkpieceState` + The states of the workpieces in the scene. The keys are the workpiece ids. + tool_states : `dict` of :class:`ToolState` + The states of the tools in the scene. The keys are the tool ids. + robot_state : :class:`RobotState` + The state of the robot in the scene. + """ + + def __init__(self, workpiece_ids=[], tool_ids=[]): + super(SceneState, self).__init__() + self.workpiece_ids = workpiece_ids # type: list[str] + self.tool_ids = tool_ids + self.workpiece_states = {} # type: dict[str, WorkpieceState] + self.tool_states = {} # type: dict[str, ToolState] + self.robot_state = RobotState() # type: RobotState + + # Create initial states for all workpieces and tools + for workpiece_id in workpiece_ids: + self.workpiece_states[workpiece_id] = WorkpieceState(workpiece_id) + for tool_id in tool_ids: + self.tool_states[tool_id] = ToolState(tool_id) + self.workpiece_states['s'].attached_to_tool_grasp + self.tool_states['s'].attached_to_robot_grasp + @property + def data(self): + data = {} + data["workpiece_ids"] = self.workpiece_ids + data["tool_ids"] = self.tool_ids + data["workpiece_states"] = self.workpiece_states + data["tool_states"] = self.tool_states + data["robot_state"] = self.robot_state + return data + + @data.setter + def data(self, data): + self.workpiece_ids = data.get("workpiece_ids", self.workpiece_ids) + self.tool_ids = data.get("tool_ids", self.tool_ids) + self.workpiece_states = data.get("workpiece_states", self.workpiece_states) + self.tool_states = data.get("tool_states", self.tool_states) + self.robot_state = data.get("robot_state", self.robot_state) + + def get_robot_state(self): + # type: () -> RobotState + return self.robot_state + + def get_tool_state(self, tool_id=None): + # type: (str) -> ToolState + """Returns the state of a tool by tool id. + If there is only one tool in the scene, the tool id can be omitted. + + Parameters + ---------- + tool_id : str, optional + The id of the tool. + """ + if tool_id is None: + if len(self.tool_ids) > 1: + raise ValueError("There is more than one tool in the scene. Please specify the tool id.") + tool_id = self.tool_ids[0] + return self.tool_states[tool_id] + + def get_workpiece_state(self, workpiece_id): + # type: (str) -> WorkpieceState + """Returns the state of a workpiece by workpiece id. + + Parameters + ---------- + workpiece_id : str + The id of the workpiece. + """ + return self.workpiece_states[workpiece_id] + + def get_attached_tool_id(self): + # type: () -> Optional[str] + """Returns the id of the tool that is currently attached to the robot. + This function assumes there is only one possible tool attached to the robot. + If no tool is attached, `None` is returned. + """ + for tool_id, tool_state in self.tool_states.items(): + if tool_state.attached_to_robot: + return tool_id + return None + + def get_attached_workpiece_id(self): + # type: () -> Optional[str] + """Returns the id of the workpiece that is currently attached to the robot. + This function assumes there is only one possible workpiece attached to the robot. + If no workpiece is attached, `None` is returned. + """ + for workpiece_id, workpiece_state in self.workpiece_states.items(): + if workpiece_state.attached_to_tool_id: + return workpiece_id + return None + + +class WorkpieceState(Data): + """Class for describing the state of a workpiece. + + Attributes + ---------- + workpiece_id : str + The id of the workpiece. + frame : :class:`compas.geometry.Frame`, optional + The current location of the workpiece. + attached_to_tool_id : str, optional + If the workpiece is attached to a tool, the id of the tool. + attached_to_tool_grasp : :class:`compas.geometry.Transformation`, optional + If the workpiece is attached to a tool, the grasp frame of the workpiece. + + """ + + def __init__(self, workpiece_id="undefined_workpiece"): + super(WorkpieceState, self).__init__() + self.workpiece_id = workpiece_id + self.frame = Frame.worldXY() # type: Frame + self.attached_to_tool_id = None # type: Optional[str] + self.attached_to_tool_grasp = None # type: Optional[Transformation] + + @property + def data(self): + data = {} + data["workpiece_id"] = self.workpiece_id + data["frame"] = self.frame + data["attached_to_tool_id"] = self.attached_to_tool_id + data["attached_to_tool_grasp"] = self.attached_to_tool_grasp + return data + + @data.setter + def data(self, data): + self.workpiece_id = data.get("workpiece_id", self.workpiece_id) + self.frame = data.get("frame", self.frame) + self.attached_to_tool_id = data.get("attached_to_tool_id", self.attached_to_tool_id) + self.attached_to_tool_grasp = data.get("attached_to_tool_grasp", self.attached_to_tool_grasp) + + +class ToolState(Data): + """Class for describing the state of a tool. + + Attributes + ---------- + tool_id : str + The id of the tool. + frame : :class:`compas.geometry.Frame` + The current location of the tool. + attached_to_robot : bool + If the tool is attached to a robot, `True`. Else, `False`. + attached_to_robot_grasp : :class:`compas.geometry.Transformation` + If the tool is attached to a robot, the base frame of the tool relative to the robot flange. + configuration : :class:`compas.robots.Configuration`, optional + If the tool is kinematic, the current configuration of the tool. + """ + + def __init__(self, tool_id="undefined_tool", initial_configuration=None): + super(ToolState, self).__init__() + self.tool_id = tool_id + self.frame = Frame.worldXY() # type: Frame + self.attached_to_robot = False # type: bool + self.attached_to_robot_grasp = None # type: Optional[Transformation] + self.configuration = initial_configuration # type: Optional[Configuration] + + @property + def data(self): + data = {} + data["tool_id"] = self.tool_id + data["frame"] = self.frame + data["attached_to_robot"] = self.attached_to_robot + data["attached_to_robot_grasp"] = self.attached_to_robot_grasp + data["configuration"] = self.configuration + return data + + @data.setter + def data(self, data): + self.tool_id = data.get("tool_id", self.tool_id) + self.frame = data.get("frame", self.frame) + self.attached_to_robot = data.get("attached_to_robot", self.attached_to_robot) + self.attached_to_robot_grasp = data.get("attached_to_robot_grasp", self.attached_to_robot_grasp) + self.configuration = data.get("configuration", self.configuration) + + +class RobotState(Data): + """Class for describing the state of a robot. + + Attributes + ---------- + frame : :class:`compas.geometry.Frame` + The current flange frame (robot target) of the robot. + configuration : :class:`compas.robots.Configuration` + The current configuration of the robot. + """ + + def __init__(self, initial_configuration=None, initial_frame=None): + super(RobotState, self).__init__() + self.frame = initial_frame # type: Frame + self.configuration = initial_configuration # type: Optional[Configuration] + + @property + def data(self): + data = {} + data["frame"] = self.frame + data["configuration"] = self.configuration + return data + + @data.setter + def data(self, data): + self.frame = data.get("frame", self.frame) + self.configuration = data.get("configuration", self.configuration) From 57c78816cf111e5afdd084c584bfac8a91b3527d Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Tue, 7 Nov 2023 20:53:45 +0800 Subject: [PATCH 02/28] doc string wip --- docs/api/compas_fab.planning.rst | 2 ++ src/compas_fab/__init__.py | 16 +++++++-- src/compas_fab/planning/__init__.py | 50 ++++++++++++++++++++++++++--- src/compas_fab/planning/action.py | 22 +++++++++---- src/compas_fab/planning/state.py | 37 ++++++++++++++++----- 5 files changed, 106 insertions(+), 21 deletions(-) create mode 100644 docs/api/compas_fab.planning.rst diff --git a/docs/api/compas_fab.planning.rst b/docs/api/compas_fab.planning.rst new file mode 100644 index 0000000000..8652f76261 --- /dev/null +++ b/docs/api/compas_fab.planning.rst @@ -0,0 +1,2 @@ + +.. automodule:: compas_fab.planning \ No newline at end of file diff --git a/src/compas_fab/__init__.py b/src/compas_fab/__init__.py index f0d7c01701..300cb2f31b 100644 --- a/src/compas_fab/__init__.py +++ b/src/compas_fab/__init__.py @@ -12,8 +12,10 @@ Robots ------ -The core features are located in the ``robots`` module with the specific backend -implementations in the ``backends`` modules: +Features for modeling robots and methods to perform static inverse kinematics and +single-action motion planning. +The user-facing features are located in the ``robots`` module. +The implementations of planning backends are located in the ``backends`` modules: .. toctree:: :maxdepth: 1 @@ -21,6 +23,16 @@ compas_fab.robots compas_fab.backends +Planning +-------- + +Features for modeling multi-action robotic processes and methods for planning them. + +.. toctree:: + :maxdepth: 1 + + compas_fab.planning + CAD integration --------------- diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index c6efacaf5c..209fc167a4 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -5,20 +5,60 @@ .. currentmodule:: compas_fab.planning -Planning +This package contains data classes for modeling robotic process and algorithms for planning robotic motions. +The current implementation supports single-robot (:class:`compas_fab.robots.Robot`) +processes with one or more workpieces (:class:`Workpiece`) and tools (:class:`compas_fab.robots.Tool`). +The processes contains actions that are assumed to be sequentially executed by the robot +or by the operator (manually). Concurrent actions either by multuiple robots, or by robot-operator +collaboration are not supported. + + +Actions -------- -This package contains classes for modeling the actions that are performed by the robots. -Actions are abstractions of the robot's capability and they are used for planning tasks -that are performed by the robot. Actions are to -Process class planning and execution backends to exchange information. +Actions are abstractions of the robot's (and, or operator's) capability to manupulate tools and +workpieces in the scene. Action classes are used for modelling a process for the following purpose: + +* To plan trajectories for robotic motions +* To simulate and visualize the process in a virtual environment +* To execute the process on a real robot (or by an operator) .. autosummary:: :toctree: generated/ :nosignatures: Action + RoboticMovement + LinearMovement + FreeMovement + OpenGripper + CloseGripper + LoadWorkpiece + +State +----- + +State classes are used to model the static state of objects in the planning scene. This include: +:class:`RobotState` for :class:`compas_fab.robots.Robot`, +:class:`ToolState` for :class:`compas_fab.robots.Tool` and +:class:`WorkpieceState` for :class:`compas_fab.robots.Workpiece`. +The :class:`SceneState` class is a container that holds the state of all objects in the scene. + +Typically a robotic process will have an initial (starting) state that is defined manually. +If sequential planning is used, the initial state is used to plan the first action in the process. +The resulting state of the first action is used as the initial state for the next action, and so on. +If non-sequential planning is used, the starting state of actions are inferred from the list of +actions in the process. See tutorial on :ref:`planning_process` for more details. + +.. autosummary:: + :toctree: generated/ + :nosignatures: + SceneState + WorkpieceState + ToolState + RobotState + """ diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 4ee5ec1e7e..b2fb36c212 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -27,7 +27,7 @@ class Action(Data): def __init__(self): super(Action, self).__init__() - self.act_n = -1 # type: int + self.act_id = -1 # type: int self.tag = "Generic Action" @property @@ -35,7 +35,7 @@ def data(self): data = {} # For class inhereited from Action, use the following line # data = super(Action, self).data - data["act_n"] = self.act_n + data["act_id"] = self.act_id data["tag"] = self.tag return data @@ -43,7 +43,7 @@ def data(self): def data(self, data): # For class inhereited from Action, use the following line # super(Action, type(self)).data.fset(self, data) - self.act_n = data["act_n"] + self.act_id = data["act_id"] self.tag = data["tag"] def apply_to(self, scene_state, debug=False): @@ -68,6 +68,13 @@ class RoboticMovement(Action): fixed_configuration : :class:`compas_fab.robots.Configuration`, optional The configuration of the robot if the target needs a fixed configuration. For example, if a taught position is used as a target. + fixed_trajectory : :class:`compas_fab.robots.JointTrajectory`, optional + The trajectory of the robot if the trajectory is fixed. + For example, if a pre-planned or recorded trajectory is preferred for this action. + Specifying a fixed trajectory will force the chained motion planner to respect this trajectory + when planning neighbouring robotic movements. This can be used to improve repeatibility. + Users must be careful to specify a trajectory that is collision free and does not violate + the kinematic limits of the robot. intermediate_planning_waypoint : list(:class:`compas_fab.robots.Configuration`), optional List of configurations that are used as waypoints during planning. planned_trajectory : :class:`compas_fab.robots.JointTrajectory` @@ -84,6 +91,7 @@ def __init__(self): self.robot_target = None # type: Frame self.allowed_collision_pairs = [] # type: list(tuple(str,str)) self.fixed_configuration = None # type: Optional[Configuration] + self.fixed_trajectory = None # type: Optional[JointTrajectory] self.intermediate_planning_waypoint = [] # type: list(Configuration) # After Planning @@ -162,13 +170,13 @@ def apply_to(self, scene_state, debug=False): class LinearMovement(RoboticMovement): - """Base class for all linear robotic movements. + """Action class for linear robotic movements. Linear robotic movements are planned by Linear Motion Planners. """ class FreeMovement(RoboticMovement): - """Base class for all free robotic movements. + """Action class for free robotic movements. Free robotic movements are planned by Free Motion Planners. """ @@ -297,8 +305,10 @@ def apply_to(self, scene_state, debug=False): class LoadWorkpiece(Action): - """Action to load a workpiece, probably performed manually. + """Action to load a workpiece, assumed to be performed by a human operator. This moves the workpiece to a specific frame. + Typically used for loading a workpiece into a gripper. + Can also be used to model the manual attachment of scaffolding (modeled as a :class:`Workpiece`) Attributes ---------- diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index 5018afedb4..bd415646f4 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -17,12 +17,21 @@ class SceneState(Data): - """Class for describing a scene. A scene contains the states of all objects. + """Class for describing a static scene, aka. a single moment in time. + The SceneState describe the states of all workpieces, tools and robot. Current implementation supports only one robot in the scene. There can be multiple tools and workpieces in the scene. However, only one (or none) tool can be attached to the robot at any time. Similarly, there can only be one (or none) workpiece attached to the tool at any time. + See `SceneState.get_attached_tool_id()` and `SceneState.get_attached_workpiece_id()`. + + When constructing a SceneState, all the workpieces and tools id should be provided + to the constructor. The initial :class:`WorkpieceState` and :class:`ToolState` are + automatically created and can be accessed by the `SceneState.get_workpiece_state()` and + `SceneState.get_tool_state()`. An empty RobotState is created by default and + can be accessed by `SceneState.get_robot_state()`. + Attributes ---------- @@ -51,8 +60,9 @@ def __init__(self, workpiece_ids=[], tool_ids=[]): self.workpiece_states[workpiece_id] = WorkpieceState(workpiece_id) for tool_id in tool_ids: self.tool_states[tool_id] = ToolState(tool_id) - self.workpiece_states['s'].attached_to_tool_grasp - self.tool_states['s'].attached_to_robot_grasp + self.workpiece_states["s"].attached_to_tool_grasp + self.tool_states["s"].attached_to_robot_grasp + @property def data(self): data = {} @@ -73,6 +83,7 @@ def data(self, data): def get_robot_state(self): # type: () -> RobotState + """Returns the state of the only robot in the scene.""" return self.robot_state def get_tool_state(self, tool_id=None): @@ -84,6 +95,7 @@ def get_tool_state(self, tool_id=None): ---------- tool_id : str, optional The id of the tool. + If tool_id is `None` and there is only one tool in the scene, the tool id is inferred. """ if tool_id is None: if len(self.tool_ids) > 1: @@ -132,13 +144,19 @@ class WorkpieceState(Data): ---------- workpiece_id : str The id of the workpiece. - frame : :class:`compas.geometry.Frame`, optional + frame : :class:`compas.geometry.Frame` The current location of the workpiece. - attached_to_tool_id : str, optional + (default: :class:`compas.geometry.Frame.worldXY`) + attached_to_tool_id : str or None If the workpiece is attached to a tool, the id of the tool. - attached_to_tool_grasp : :class:`compas.geometry.Transformation`, optional + If the workpiece is not attached to a tool, `None`. (default: `None`) + attached_to_tool_grasp : :class:`compas.geometry.Transformation` If the workpiece is attached to a tool, the grasp frame of the workpiece. - + If not specified, defaults to the identity transformation. + If the workpiece is not attached to a tool, `None`. + is_hidden : bool + If the workpiece is hidden, `True`. Else, `False`. (default: `False`) + A hidden workpiece will not be included for collision detection of the scene. """ def __init__(self, workpiece_id="undefined_workpiece"): @@ -146,7 +164,8 @@ def __init__(self, workpiece_id="undefined_workpiece"): self.workpiece_id = workpiece_id self.frame = Frame.worldXY() # type: Frame self.attached_to_tool_id = None # type: Optional[str] - self.attached_to_tool_grasp = None # type: Optional[Transformation] + self.attached_to_tool_grasp = Transformation() # type: Optional[Transformation] + self.is_hidden = False # type: bool @property def data(self): @@ -155,6 +174,7 @@ def data(self): data["frame"] = self.frame data["attached_to_tool_id"] = self.attached_to_tool_id data["attached_to_tool_grasp"] = self.attached_to_tool_grasp + data["is_hidden"] = self.is_hidden return data @data.setter @@ -163,6 +183,7 @@ def data(self, data): self.frame = data.get("frame", self.frame) self.attached_to_tool_id = data.get("attached_to_tool_id", self.attached_to_tool_id) self.attached_to_tool_grasp = data.get("attached_to_tool_grasp", self.attached_to_tool_grasp) + self.is_hidden = data.get("is_hidden", self.is_hidden) class ToolState(Data): From 87c8a10859a636ec902ead9ab71a1b253967e9f9 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 9 Nov 2023 17:00:53 +0800 Subject: [PATCH 03/28] Refactor SceneState, WorkpieceState, ToolState and RobotState classes --- src/compas_fab/planning/state.py | 50 ++++++++++++++++++++------------ 1 file changed, 32 insertions(+), 18 deletions(-) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index bd415646f4..beec80b163 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -17,20 +17,24 @@ class SceneState(Data): - """Class for describing a static scene, aka. a single moment in time. - The SceneState describe the states of all workpieces, tools and robot. - Current implementation supports only one robot in the scene. + """Container for the states of all workpieces, tools and robot in a static scene. - There can be multiple tools and workpieces in the scene. However, only - one (or none) tool can be attached to the robot at any time. Similarly, - there can only be one (or none) workpiece attached to the tool at any time. - See `SceneState.get_attached_tool_id()` and `SceneState.get_attached_workpiece_id()`. + Current implementation supports multiple workpieces and tools, but only one robot. + + No more than one tool can be attached to the robot at any time. Similarly, + no more than one workpiece can be attached to the tool at any time. + It is not possible to attach a workpiece to the robot directly. + Use a dummy tool (with identity transformation and no visual or collision meshes) if necessary. + + SceneState provides convinence functions such as :meth:`SceneState.get_attached_tool_id()` + and :meth:`SceneState.get_attached_workpiece_id()` for identifying which tool and workpiece + are currently attached to the robot. When constructing a SceneState, all the workpieces and tools id should be provided - to the constructor. The initial :class:`WorkpieceState` and :class:`ToolState` are - automatically created and can be accessed by the `SceneState.get_workpiece_state()` and - `SceneState.get_tool_state()`. An empty RobotState is created by default and - can be accessed by `SceneState.get_robot_state()`. + to the constructor. Default :class:`WorkpieceState`, :class:`ToolState` and + :class:`RobotState` are automatically created and can be accessed by + :meth:`SceneState.get_workpiece_state()`, :meth:`SceneState.get_tool_state()` and + :meth:`SceneState.get_robot_state()`. Attributes @@ -140,10 +144,12 @@ def get_attached_workpiece_id(self): class WorkpieceState(Data): """Class for describing the state of a workpiece. + WorkpieceState objects are typically created by the constructor of :class:`SceneState`. + Attributes ---------- workpiece_id : str - The id of the workpiece. + Unique identifier of the workpiece used in Process.workpieces and SceneState.workpiece_states. frame : :class:`compas.geometry.Frame` The current location of the workpiece. (default: :class:`compas.geometry.Frame.worldXY`) @@ -189,10 +195,13 @@ def data(self, data): class ToolState(Data): """Class for describing the state of a tool. + ToolState objects are typically created by the constructor of :class:`SceneState`. + + Attributes ---------- tool_id : str - The id of the tool. + Unique identifier of the tool used in Process.tools and SceneState.tool_states. frame : :class:`compas.geometry.Frame` The current location of the tool. attached_to_robot : bool @@ -203,13 +212,13 @@ class ToolState(Data): If the tool is kinematic, the current configuration of the tool. """ - def __init__(self, tool_id="undefined_tool", initial_configuration=None): + def __init__(self, tool_id="undefined_tool", configuration=None): super(ToolState, self).__init__() self.tool_id = tool_id self.frame = Frame.worldXY() # type: Frame self.attached_to_robot = False # type: bool self.attached_to_robot_grasp = None # type: Optional[Transformation] - self.configuration = initial_configuration # type: Optional[Configuration] + self.configuration = configuration # type: Optional[Configuration] @property def data(self): @@ -233,6 +242,11 @@ def data(self, data): class RobotState(Data): """Class for describing the state of a robot. + RobotState objects are typically created by the constructor of :class:`SceneState`. + However it is possible to create a RobotState object manually. For example, when specifying + the initial state of a robot in a planning process. + + Attributes ---------- frame : :class:`compas.geometry.Frame` @@ -241,10 +255,10 @@ class RobotState(Data): The current configuration of the robot. """ - def __init__(self, initial_configuration=None, initial_frame=None): + def __init__(self, frame=None, configuration=None): super(RobotState, self).__init__() - self.frame = initial_frame # type: Frame - self.configuration = initial_configuration # type: Optional[Configuration] + self.frame = frame # type: Frame + self.configuration = configuration # type: Optional[Configuration] @property def data(self): From b7157a190271b6a1d468364b4eb2cc703a6e747e Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 9 Nov 2023 17:02:00 +0800 Subject: [PATCH 04/28] Improved documentation of Planning Module --- src/compas_fab/planning/__init__.py | 11 +++++-- src/compas_fab/planning/action.py | 47 +++++++++++++++++++++++++---- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index 209fc167a4..8aeec469f9 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -9,8 +9,15 @@ The current implementation supports single-robot (:class:`compas_fab.robots.Robot`) processes with one or more workpieces (:class:`Workpiece`) and tools (:class:`compas_fab.robots.Tool`). The processes contains actions that are assumed to be sequentially executed by the robot -or by the operator (manually). Concurrent actions either by multuiple robots, or by robot-operator -collaboration are not supported. +or by the operator (manually). Concurrent actions are not supported. + +The FabricationProcess class and its subclasses (such as :class:`PickAndPlaceProcess` and +:class:`ExtrusionProcess`) are used to model a process. They provider helper methods for +creating a ordered list of actions that are used for planning and execution. The beining of the +process is defined by the initial state of the scene, which is a container for the state of all +objects in the scene (see :class:`SceneState`). The initial state is used to plan the first action +in the process. The resulting state of the first action is used as the initial state for the next +action, and so on. See tutorial on :ref:`planning_process` for more details. Actions diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index b2fb36c212..a7cdb9d468 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -23,12 +23,32 @@ class Action(Data): - """Base class for all actions.""" + """Base class for all actions. + + The Action Classes are intended to be used to model the actions of a human operator or a robot. + Current Implementation assumes discrete actions that are executed sequentially, aka. one after another. + + Actions can be created automatically or manually. They are stored in a :class:`compas_fab.planning.Processs`. + Some actions contain references to workpieces, tools, or other objects in the scene. Those objects are + stored in dictionaries under the Process object and are identified by their id. + + Actions are closely related to the :class:`compas_fab.planning.SceneState` because each action changes the scene state. + Provided with an starting scene state, the actions can be *applied to* create the ending scene state. + The only exception are robotic configurations that are changed by :class:`compas_fab.planning.RoboticMovement` actions. + Those actions changes the robot configuration and their effect is not known until after the planning process. + + Attributes + ---------- + act_id : [str | int] + A unique id of the action, it is optional but recommended for debugging. + tag : str + A human readable tag that describes the action. It is printed out during visualization, debugging, execution and logging. + """ def __init__(self): super(Action, self).__init__() - self.act_id = -1 # type: int - self.tag = "Generic Action" + self.act_id = -1 # type: [str | int] + self.tag = "Generic Action" # type: str @property def data(self): @@ -51,13 +71,22 @@ def apply_to(self, scene_state, debug=False): """Applies the action to a scene state. This method is called by the assembly process to apply the action to the scene state. """ - raise NotImplementedError + raise NotImplementedError("Action.apply_to() is not implemented by %s." % type(self)) class RoboticMovement(Action): """Base class for all robotic movements. - - Robotic movements are actions that changes the configuration of the robot. - - Robotic movements require motion planning and are planned by a motion planner. + Robotic movements are actions that changes the robot flange frame and + configuration of the robot. + + Robotic movements contain robotic trajectory that are planned by a motion planner. + After motion planning, the trajectory is stored in the action and can be used + for visualization and execution. + + When applied to a scene state, Robotic movements also changes the state of the + attached tool and workpiece. If the trajectory have been planned, the configuration + of the robot is updated to the last configuration of the trajectory. See + :meth:`compas_fab.planning.RoboticMovement.apply_to` for more details. Attributes ---------- @@ -65,6 +94,7 @@ class RoboticMovement(Action): The target frame of the robot. In world coordinate frame. allowed_collision_pairs : list(tuple(str,str)) List of pairs of collision objects that are allowed to collide. + Objects are identified by workpiece_id, tool_id, or static_object_id. fixed_configuration : :class:`compas_fab.robots.Configuration`, optional The configuration of the robot if the target needs a fixed configuration. For example, if a taught position is used as a target. @@ -183,6 +213,11 @@ class FreeMovement(RoboticMovement): class OpenGripper(Action): """Action to open the gripper. + + Current implementation only changes the attachment state of the workpiece. + It does not change the configuration of the gripper, even if it is a + :class:`compas_fab.robots.ToolModel` with kinematic joints. + If the gripper is closed around a workpiece, the workpiece is detached from the gripper. It is possible to open the gripper with or without a workpiece attached. """ From 1920e28e1040ed7a45359472cc62e4b8d6b3fed8 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 9 Nov 2023 17:02:28 +0800 Subject: [PATCH 05/28] Fix formatting and typos in planning module --- src/compas_fab/planning/action.py | 4 ++-- src/compas_fab/planning/state.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index a7cdb9d468..d1b310dc41 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -85,7 +85,7 @@ class RoboticMovement(Action): When applied to a scene state, Robotic movements also changes the state of the attached tool and workpiece. If the trajectory have been planned, the configuration - of the robot is updated to the last configuration of the trajectory. See + of the robot is updated to the last configuration of the trajectory. See :meth:`compas_fab.planning.RoboticMovement.apply_to` for more details. Attributes @@ -215,7 +215,7 @@ class OpenGripper(Action): """Action to open the gripper. Current implementation only changes the attachment state of the workpiece. - It does not change the configuration of the gripper, even if it is a + It does not change the configuration of the gripper, even if it is a :class:`compas_fab.robots.ToolModel` with kinematic joints. If the gripper is closed around a workpiece, the workpiece is detached from the gripper. diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index beec80b163..75b1d1a17b 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -255,7 +255,7 @@ class RobotState(Data): The current configuration of the robot. """ - def __init__(self, frame=None, configuration=None): + def __init__(self, frame=None, configuration=None): super(RobotState, self).__init__() self.frame = frame # type: Frame self.configuration = configuration # type: Optional[Configuration] From 9253e243dca3ca34b5ff4fc05804971804566e67 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Tue, 14 Nov 2023 17:09:52 +0800 Subject: [PATCH 06/28] Added more details to Linear and Free Movement --- src/compas_fab/planning/action.py | 145 ++++++++++++++++++++++++------ 1 file changed, 120 insertions(+), 25 deletions(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index d1b310dc41..beeae75048 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -1,5 +1,8 @@ from compas.data import Data -from compas.geometry import Frame, Transformation +from compas.geometry import Point +from compas.geometry import Frame +from compas.geometry import Transformation + from compas_fab.robots import Configuration, JointTrajectory # noqa: F401 try: @@ -76,26 +79,32 @@ def apply_to(self, scene_state, debug=False): class RoboticMovement(Action): """Base class for all robotic movements. - Robotic movements are actions that changes the robot flange frame and - configuration of the robot. - Robotic movements contain robotic trajectory that are planned by a motion planner. - After motion planning, the trajectory is stored in the action and can be used - for visualization and execution. + Robotic movements are actions that changes the robot configuration and + hence also the frame of the robot flange and all attached tools and workpieces. + + The RoboticMovement class only describes the target (ending state) + of the robotic movement, whereas the starting state is defined using a + :class:`compas_fab.planning.SceneState` object. The starting state also + defines the attached tools and workpieces. + Both objects are required by the motion planner to plan a trajectory. + + After motion planning, the trajectory can be stored in the same RoboticMovement class + and can be used for visualization and execution. When applied to a scene state, Robotic movements also changes the state of the attached tool and workpiece. If the trajectory have been planned, the configuration of the robot is updated to the last configuration of the trajectory. See :meth:`compas_fab.planning.RoboticMovement.apply_to` for more details. - Attributes - ---------- - robot_target : :class:`compas.geometry.Frame` - The target frame of the robot. In world coordinate frame. + Attributes (Before Planning) + ---------------------------- + target_robot_flange_frame : :class:`compas.geometry.Frame` + The robot flange frame of the robot at target. In world coordinate frame. allowed_collision_pairs : list(tuple(str,str)) List of pairs of collision objects that are allowed to collide. Objects are identified by workpiece_id, tool_id, or static_object_id. - fixed_configuration : :class:`compas_fab.robots.Configuration`, optional + fixed_target_configuration : :class:`compas.robots.Configuration`, optional The configuration of the robot if the target needs a fixed configuration. For example, if a taught position is used as a target. fixed_trajectory : :class:`compas_fab.robots.JointTrajectory`, optional @@ -105,12 +114,24 @@ class RoboticMovement(Action): when planning neighbouring robotic movements. This can be used to improve repeatibility. Users must be careful to specify a trajectory that is collision free and does not violate the kinematic limits of the robot. - intermediate_planning_waypoint : list(:class:`compas_fab.robots.Configuration`), optional - List of configurations that are used as waypoints during planning. + tag : str + A human readable tag that describes the action. + It can printed out during visualization, debugging, execution and logging. + + Attributes (After Planning) + --------------------------- planned_trajectory : :class:`compas_fab.robots.JointTrajectory` - The planned trajectory of the robotic movement. + The planned trajectory of the robotic movement. Available after planning. The first and last + trajectory points corresponds to the starting and ending configuration of the robotic movement. planner_seed : int - The random seed used by the planner to generate the trajectory. + The random seed used by the planner to generate the trajectory. Used by some planners. + + Attributes (For Execution) + -------------------------- + speed_data_id : str + The id (or name) of the speed data to be used for execution. + ---------- + """ def __init__(self): @@ -118,9 +139,9 @@ def __init__(self): self.tag = "Generic Action" # Before Planning - self.robot_target = None # type: Frame + self.target_robot_flange_frame = None # type: Frame self.allowed_collision_pairs = [] # type: list(tuple(str,str)) - self.fixed_configuration = None # type: Optional[Configuration] + self.fixed_target_configuration = None # type: Optional[Configuration] self.fixed_trajectory = None # type: Optional[JointTrajectory] self.intermediate_planning_waypoint = [] # type: list(Configuration) @@ -128,37 +149,45 @@ def __init__(self): self.planned_trajectory = None # type: Optional[JointTrajectory] self.planner_seed = None # type: Optional[int] + # For Execution + self.speed_data_id = None # type: Optional[str] + @property def data(self): data = super(RoboticMovement, self).data data["tag"] = self.tag - data["robot_target"] = self.robot_target + data["target_robot_flange_frame"] = self.target_robot_flange_frame data["allowed_collision_pairs"] = self.allowed_collision_pairs - data["fixed_configuration"] = self.fixed_configuration + data["fixed_target_configuration"] = self.fixed_target_configuration + data["fixed_trajectory"] = self.fixed_trajectory data["intermediate_planning_waypoint"] = self.intermediate_planning_waypoint data["planned_trajectory"] = self.planned_trajectory data["planner_seed"] = self.planner_seed + data["speed_data_id"] = self.speed_data_id return data @data.setter def data(self, data): super(RoboticMovement, type(self)).data.fset(self, data) self.tag = data.get("tag", self.tag) - self.robot_target = data.get("robot_target", self.robot_target) + self.target_robot_flange_frame = data.get("target_robot_flange_frame", self.target_robot_flange_frame) self.allowed_collision_pairs = data.get("allowed_collision_pairs", self.allowed_collision_pairs) - self.fixed_configuration = data.get("fixed_configuration", self.fixed_configuration) + self.fixed_target_configuration = data.get("fixed_target_configuration", self.fixed_target_configuration) + self.fixed_trajectory = data.get("fixed_trajectory", self.fixed_trajectory) self.intermediate_planning_waypoint = data.get( "intermediate_planning_waypoint", self.intermediate_planning_waypoint ) self.planned_trajectory = data.get("planned_trajectory", self.planned_trajectory) self.planner_seed = data.get("planner_seed", self.planner_seed) + self.speed_data_id = data.get("speed_data_id", self.speed_data_id) def apply_to(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. The SceneState is updated with the new robot state. - If a trajectory is available, the robot state is updated with the last configuration of the trajectory. + If a fixed_trajectory or planned_trajectory is available, the robot_state.configuration is updated with the last configuration of the trajectory. + If fixed_target_configuration is available, the robot_state.configuration is updated with the fixed_target_configuration. Parameters ---------- @@ -168,9 +197,11 @@ def apply_to(self, scene_state, debug=False): robot_state = scene_state.get_robot_state() if self.planned_trajectory is not None: robot_state.configuration = self.planned_trajectory.points[-1] - elif self.fixed_configuration is not None: - robot_state.configuration = self.fixed_configuration - robot_state.frame = self.robot_target + elif self.fixed_trajectory is not None: + robot_state.configuration = self.fixed_trajectory.points[-1] + elif self.fixed_target_configuration is not None: + robot_state.configuration = self.fixed_target_configuration + robot_state.frame = self.target_robot_flange_frame if debug: print("Robot Moved.") @@ -202,14 +233,77 @@ def apply_to(self, scene_state, debug=False): class LinearMovement(RoboticMovement): """Action class for linear robotic movements. Linear robotic movements are planned by Linear Motion Planners. + + Attributes + ---------- + polyline_target : list(:class:`compas.geometry.Point`) + List of points to define a linear movement that is a polyline. + Specified in world coordinate frame. + The first point (the starting point) is not required. + The second point onwards, including the last point (the ending point) is required. + + see :class:`compas_fab.planning.RoboticMovement` for other attributes. """ + def __init__(self): + super(LinearMovement, self).__init__() + self.polyline_target = [] # type: list(Point) + self.tag = "Linear Movement" + + @property + def data(self): + data = super(LinearMovement, self).data + data["polyline_target"] = self.polyline_target + return data + + @data.setter + def data(self, data): + super(LinearMovement, type(self)).data.fset(self, data) + self.polyline_target = data.get("polyline_target", self.polyline_target) + class FreeMovement(RoboticMovement): """Action class for free robotic movements. Free robotic movements are planned by Free Motion Planners. + + Attributes + ---------- + intermediate_planning_waypoint : list(:class:`compas.robots.Configuration`) + List of configurations to define a multi-step free movement. + The first and last configuration (the starting and target configuration) is not required. + Only include the intermediate waypoints. + Waypoints can be used to help the planner 'manually' to find a feasible path around obstacles. + smoothing_required : bool + If True, the trajectory smoothing algorithm is invoked after planning to smooth the trajectory. + Note that smoothing may not be available for all planners. (default: True) + smoothing_keep_waypoints : bool + If True, the trajectory smoothing algorithm is allowed to remove the provided waypoints. (default: False) + + + see :class:`compas_fab.planning.RoboticMovement` for other attributes. """ + def __init__(self): + super(FreeMovement, self).__init__() + self.intermediate_planning_waypoint = [] # type: list(Configuration) + self.smoothing_required = True + self.smoothing_keep_waypoints = False + self.tag = "Free Movement" + + @property + def data(self): + data = super(FreeMovement, self).data + data["intermediate_planning_waypoint"] = self.intermediate_planning_waypoint + data["smoothing_required"] = self.smoothing_required + data["smoothing_keep_waypoints"] = self.smoothing_keep_waypoints + return data + + @data.setter + def data(self, data): + super(FreeMovement, type(self)).data.fset(self, data) + self.intermediate_planning_waypoint = data.get("intermediate_planning_waypoint", self.intermediate_planning_waypoint) + self.smoothing_required = data.get("smoothing_required", self.smoothing_required) + self.smoothing_keep_waypoints = data.get("smoothing_keep_waypoints", self.smoothing_keep_waypoints) class OpenGripper(Action): """Action to open the gripper. @@ -385,3 +479,4 @@ def apply_to(self, scene_state, debug=False): workpiece_state.frame = self.frame if debug: print("Workpiece %s loaded to new location." % self.workpiece_id) + From 33711f64ece6f5c78b2bf4f97c6b72deb8e70b62 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 15 Nov 2023 17:29:59 +0800 Subject: [PATCH 07/28] CartesianMovement --- src/compas_fab/planning/action.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index beeae75048..e68a127a9b 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -18,7 +18,7 @@ __all__ = [ "Action", "RoboticMovement", - "LinearMovement", + "CartesianMovement", "FreeMovement", "OpenGripper", "CloseGripper", @@ -230,9 +230,12 @@ def apply_to(self, scene_state, debug=False): print("- Attached Workpiece %s Followed." % attached_workpiece_id) -class LinearMovement(RoboticMovement): - """Action class for linear robotic movements. - Linear robotic movements are planned by Linear Motion Planners. +class CartesianMovement(RoboticMovement): + """Action class to describe a Cartesian robotic movement. + + The trajectory of the robot flange is interpolated in Cartesian space. + Typically, the orientation between the starting frame and the target frame is the same. + However some linear motion planners can interpolate between different orientations. Attributes ---------- @@ -246,19 +249,19 @@ class LinearMovement(RoboticMovement): """ def __init__(self): - super(LinearMovement, self).__init__() + super(CartesianMovement, self).__init__() self.polyline_target = [] # type: list(Point) self.tag = "Linear Movement" @property def data(self): - data = super(LinearMovement, self).data + data = super(CartesianMovement, self).data data["polyline_target"] = self.polyline_target return data @data.setter def data(self, data): - super(LinearMovement, type(self)).data.fset(self, data) + super(CartesianMovement, type(self)).data.fset(self, data) self.polyline_target = data.get("polyline_target", self.polyline_target) @@ -305,6 +308,7 @@ def data(self, data): self.smoothing_required = data.get("smoothing_required", self.smoothing_required) self.smoothing_keep_waypoints = data.get("smoothing_keep_waypoints", self.smoothing_keep_waypoints) + class OpenGripper(Action): """Action to open the gripper. From 2b881b83d9b91331992c2328c9551e98ea3f54e9 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 16 Nov 2023 16:55:45 +0800 Subject: [PATCH 08/28] Naming change --- CHANGELOG.md | 4 +++ src/compas_fab/planning/__init__.py | 12 ++++----- src/compas_fab/planning/action.py | 42 ++++++++++++++--------------- 3 files changed, 31 insertions(+), 27 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 83f2c3ab2e..3411f20269 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +* Added `compas_fab.planning` that contains `Action` classes and `SceneState` +* Added `RoboticAction`, `CartesianMotion`, `FreeMotion`, `OpenGripper` and `CloseGripper`. +* Added `SceneState` as container for `WorkpieceState`, `ToolState` and `RobotState`. + ### Changed * `CollisionMesh` now inherit from `compas.data.Data` diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index 8aeec469f9..278336812c 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -35,9 +35,9 @@ :nosignatures: Action - RoboticMovement + RoboticAction LinearMovement - FreeMovement + FreeMotion OpenGripper CloseGripper LoadWorkpiece @@ -71,9 +71,9 @@ from .action import ( Action, - RoboticMovement, + RoboticAction, LinearMovement, - FreeMovement, + FreeMotion, OpenGripper, CloseGripper, LoadWorkpiece, @@ -88,9 +88,9 @@ __all__ = [ "Action", - "RoboticMovement", + "RoboticAction", "LinearMovement", - "FreeMovement", + "FreeMotion", "OpenGripper", "CloseGripper", "LoadWorkpiece", diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index e68a127a9b..a8e9f3e5c2 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -17,9 +17,9 @@ __all__ = [ "Action", - "RoboticMovement", - "CartesianMovement", - "FreeMovement", + "RoboticAction", + "CartesianMotion", + "FreeMotion", "OpenGripper", "CloseGripper", ] @@ -37,7 +37,7 @@ class Action(Data): Actions are closely related to the :class:`compas_fab.planning.SceneState` because each action changes the scene state. Provided with an starting scene state, the actions can be *applied to* create the ending scene state. - The only exception are robotic configurations that are changed by :class:`compas_fab.planning.RoboticMovement` actions. + The only exception are robotic configurations that are changed by :class:`compas_fab.planning.RoboticAction` actions. Those actions changes the robot configuration and their effect is not known until after the planning process. Attributes @@ -77,25 +77,25 @@ def apply_to(self, scene_state, debug=False): raise NotImplementedError("Action.apply_to() is not implemented by %s." % type(self)) -class RoboticMovement(Action): +class RoboticAction(Action): """Base class for all robotic movements. Robotic movements are actions that changes the robot configuration and hence also the frame of the robot flange and all attached tools and workpieces. - The RoboticMovement class only describes the target (ending state) + The RoboticAction class only describes the target (ending state) of the robotic movement, whereas the starting state is defined using a :class:`compas_fab.planning.SceneState` object. The starting state also defines the attached tools and workpieces. Both objects are required by the motion planner to plan a trajectory. - After motion planning, the trajectory can be stored in the same RoboticMovement class + After motion planning, the trajectory can be stored in the same RoboticAction class and can be used for visualization and execution. When applied to a scene state, Robotic movements also changes the state of the attached tool and workpiece. If the trajectory have been planned, the configuration of the robot is updated to the last configuration of the trajectory. See - :meth:`compas_fab.planning.RoboticMovement.apply_to` for more details. + :meth:`compas_fab.planning.RoboticAction.apply_to` for more details. Attributes (Before Planning) ---------------------------- @@ -135,7 +135,7 @@ class RoboticMovement(Action): """ def __init__(self): - super(RoboticMovement, self).__init__() + super(RoboticAction, self).__init__() self.tag = "Generic Action" # Before Planning @@ -154,7 +154,7 @@ def __init__(self): @property def data(self): - data = super(RoboticMovement, self).data + data = super(RoboticAction, self).data data["tag"] = self.tag data["target_robot_flange_frame"] = self.target_robot_flange_frame data["allowed_collision_pairs"] = self.allowed_collision_pairs @@ -168,7 +168,7 @@ def data(self): @data.setter def data(self, data): - super(RoboticMovement, type(self)).data.fset(self, data) + super(RoboticAction, type(self)).data.fset(self, data) self.tag = data.get("tag", self.tag) self.target_robot_flange_frame = data.get("target_robot_flange_frame", self.target_robot_flange_frame) self.allowed_collision_pairs = data.get("allowed_collision_pairs", self.allowed_collision_pairs) @@ -230,7 +230,7 @@ def apply_to(self, scene_state, debug=False): print("- Attached Workpiece %s Followed." % attached_workpiece_id) -class CartesianMovement(RoboticMovement): +class CartesianMotion(RoboticAction): """Action class to describe a Cartesian robotic movement. The trajectory of the robot flange is interpolated in Cartesian space. @@ -245,27 +245,27 @@ class CartesianMovement(RoboticMovement): The first point (the starting point) is not required. The second point onwards, including the last point (the ending point) is required. - see :class:`compas_fab.planning.RoboticMovement` for other attributes. + see :class:`compas_fab.planning.RoboticAction` for other attributes. """ def __init__(self): - super(CartesianMovement, self).__init__() + super(CartesianMotion, self).__init__() self.polyline_target = [] # type: list(Point) self.tag = "Linear Movement" @property def data(self): - data = super(CartesianMovement, self).data + data = super(CartesianMotion, self).data data["polyline_target"] = self.polyline_target return data @data.setter def data(self, data): - super(CartesianMovement, type(self)).data.fset(self, data) + super(CartesianMotion, type(self)).data.fset(self, data) self.polyline_target = data.get("polyline_target", self.polyline_target) -class FreeMovement(RoboticMovement): +class FreeMotion(RoboticAction): """Action class for free robotic movements. Free robotic movements are planned by Free Motion Planners. @@ -283,11 +283,11 @@ class FreeMovement(RoboticMovement): If True, the trajectory smoothing algorithm is allowed to remove the provided waypoints. (default: False) - see :class:`compas_fab.planning.RoboticMovement` for other attributes. + see :class:`compas_fab.planning.RoboticAction` for other attributes. """ def __init__(self): - super(FreeMovement, self).__init__() + super(FreeMotion, self).__init__() self.intermediate_planning_waypoint = [] # type: list(Configuration) self.smoothing_required = True self.smoothing_keep_waypoints = False @@ -295,7 +295,7 @@ def __init__(self): @property def data(self): - data = super(FreeMovement, self).data + data = super(FreeMotion, self).data data["intermediate_planning_waypoint"] = self.intermediate_planning_waypoint data["smoothing_required"] = self.smoothing_required data["smoothing_keep_waypoints"] = self.smoothing_keep_waypoints @@ -303,7 +303,7 @@ def data(self): @data.setter def data(self, data): - super(FreeMovement, type(self)).data.fset(self, data) + super(FreeMotion, type(self)).data.fset(self, data) self.intermediate_planning_waypoint = data.get("intermediate_planning_waypoint", self.intermediate_planning_waypoint) self.smoothing_required = data.get("smoothing_required", self.smoothing_required) self.smoothing_keep_waypoints = data.get("smoothing_keep_waypoints", self.smoothing_keep_waypoints) From 8e693464393c0c0d545ecf8078148521c1a00ed6 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 17 Nov 2023 00:19:03 +0800 Subject: [PATCH 09/28] Renaming --- CHANGELOG.md | 2 +- src/compas_fab/planning/__init__.py | 10 +- src/compas_fab/planning/action.py | 226 ++++++++++++++++++++-------- src/compas_fab/planning/state.py | 51 ++++--- 4 files changed, 201 insertions(+), 88 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3411f20269..93150afccf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added * Added `compas_fab.planning` that contains `Action` classes and `SceneState` -* Added `RoboticAction`, `CartesianMotion`, `FreeMotion`, `OpenGripper` and `CloseGripper`. +* Added `RoboticAction`, `CartesianMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, and `ManuallyMoveWorkpiece` actions. * Added `SceneState` as container for `WorkpieceState`, `ToolState` and `RobotState`. ### Changed diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index 278336812c..c1d0154259 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -40,7 +40,7 @@ FreeMotion OpenGripper CloseGripper - LoadWorkpiece + ManuallyMoveWorkpiece State ----- @@ -72,11 +72,11 @@ from .action import ( Action, RoboticAction, - LinearMovement, + CartesianMotion, FreeMotion, OpenGripper, CloseGripper, - LoadWorkpiece, + ManuallyMoveWorkpiece, ) from .state import ( @@ -89,11 +89,11 @@ __all__ = [ "Action", "RoboticAction", - "LinearMovement", + "CartesianMotion", "FreeMotion", "OpenGripper", "CloseGripper", - "LoadWorkpiece", + "ManuallyMoveWorkpiece", "SceneState", "WorkpieceState", "ToolState", diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index a8e9f3e5c2..a9133fc6b3 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -12,6 +12,7 @@ try: from typing import Optional # noqa: F401 + from typing import Tuple # noqa: F401 except ImportError: pass @@ -22,6 +23,7 @@ "FreeMotion", "OpenGripper", "CloseGripper", + "ManuallyMoveWorkpiece", ] @@ -69,12 +71,38 @@ def data(self, data): self.act_id = data["act_id"] self.tag = data["tag"] - def apply_to(self, scene_state, debug=False): + def check_preconditions(self, scene_state): + # type: (SceneState) -> Tuple(bool, str) + """Checks if the action can be applied to a scene state. + + This function can be used to check if the action can be applied to a scene state. + It can be used to ensure consistancy of the scene state before applying an action. + + Default implementation always returns True. Child classes can override this function + to implement their own preconditions. This function must not modify the scene state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + + Returns + ------- + Tuple(bool, str) + A tuple of a boolean and a string. + The boolean indicates if the action can be applied to the scene state. + The string is a human readable message that describes the reason for failure. + """ + return (True, None) + + def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. + This method is called by the assembly process to apply the action to the scene state. + The SceneState object is modified in place. """ - raise NotImplementedError("Action.apply_to() is not implemented by %s." % type(self)) + raise NotImplementedError("Action.apply_effects() is not implemented by %s." % type(self)) class RoboticAction(Action): @@ -95,7 +123,7 @@ class RoboticAction(Action): When applied to a scene state, Robotic movements also changes the state of the attached tool and workpiece. If the trajectory have been planned, the configuration of the robot is updated to the last configuration of the trajectory. See - :meth:`compas_fab.planning.RoboticAction.apply_to` for more details. + :meth:`compas_fab.planning.RoboticAction.apply_effects` for more details. Attributes (Before Planning) ---------------------------- @@ -181,7 +209,7 @@ def data(self, data): self.planner_seed = data.get("planner_seed", self.planner_seed) self.speed_data_id = data.get("speed_data_id", self.speed_data_id) - def apply_to(self, scene_state, debug=False): + def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. @@ -216,15 +244,13 @@ def apply_to(self, scene_state, debug=False): ) if debug: print("- Attached Tool %s Followed." % attached_tool_id) + # Transform attached workpieces attached_workpiece_id = scene_state.get_attached_workpiece_id() if attached_workpiece_id is not None: attached_workpiece = scene_state.get_workpiece_state(attached_workpiece_id) - assert ( - attached_workpiece.attached_to_tool_id == attached_tool_id - ), "Inconsistency: Attached workpiece must be attached to the attached tool." attached_workpiece.frame = Frame.from_transformation( - Transformation.from_frame(attached_tool_state.frame) * attached_workpiece.attached_to_tool_grasp + Transformation.from_frame(robot_state.frame) * attached_workpiece.attached_to_robot_grasp ) if debug: print("- Attached Workpiece %s Followed." % attached_workpiece_id) @@ -304,7 +330,9 @@ def data(self): @data.setter def data(self, data): super(FreeMotion, type(self)).data.fset(self, data) - self.intermediate_planning_waypoint = data.get("intermediate_planning_waypoint", self.intermediate_planning_waypoint) + self.intermediate_planning_waypoint = data.get( + "intermediate_planning_waypoint", self.intermediate_planning_waypoint + ) self.smoothing_required = data.get("smoothing_required", self.smoothing_required) self.smoothing_keep_waypoints = data.get("smoothing_keep_waypoints", self.smoothing_keep_waypoints) @@ -320,19 +348,49 @@ class OpenGripper(Action): It is possible to open the gripper with or without a workpiece attached. """ - def __init__(self): + def __init__(self, tool_id = None, ): super(OpenGripper, self).__init__() + self.tool_id = tool_id # type: Frame @property def data(self): data = super(OpenGripper, self).data + data["tool_id"] = self.tool_id return data @data.setter def data(self, data): super(OpenGripper, type(self)).data.fset(self, data) + self.tool_id = data.get("tool_id", self.tool_id) - def apply_to(self, scene_state, debug=False): + def check_preconditions(self, scene_state): + # type: (SceneState) -> Tuple(bool, str) + """Checks if the action can be applied to a scene state. + + This function does not change the scene state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + + Returns + ------- + Tuple(bool, str) + A tuple of a boolean and a string. + The boolean indicates if the action can be applied to the scene state. + The string is a human readable message that describes the reason for failure. + """ + attached_tool_id = scene_state.get_attached_tool_id() + if attached_tool_id is None: + return (False, "Inconsistency: No tool attached to robot.") + if self.tool_id != attached_tool_id: + return (False, "Inconsistency: OpenGripper.tool_id does not match the attached tool id.") + # attached_tool_state = scene_state.get_tool_state(attached_tool_id) + # TODO: Check if tool is a gripper + return (True, None) + + def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. @@ -344,11 +402,7 @@ def apply_to(self, scene_state, debug=False): scene_state : :class:`compas_fab.planning.SceneState` The scene state to apply the action to. """ - # Transform attached objects - attached_tool_id = scene_state.get_attached_tool_id() - assert attached_tool_id is not None, "Inconsistency: No tool attached to robot." - # attached_tool_state = scene_state.get_tool_state(attached_tool_id) - # TODO: Check if tool is a gripper + # TODO: Change the configuration of the gripper to opened # Transform attached workpieces @@ -357,8 +411,8 @@ def apply_to(self, scene_state, debug=False): print("Gripper opened.") if attached_workpiece_id is not None: attached_workpiece = scene_state.get_workpiece_state(attached_workpiece_id) - attached_workpiece.attached_to_tool_id = None - attached_workpiece.attached_to_tool_grasp = None + attached_workpiece.attached_to_robot = False + attached_workpiece.attached_to_robot_grasp = None if debug: print("- Workpiece %s detached from tool." % attached_workpiece_id) @@ -372,35 +426,69 @@ class CloseGripper(Action): ---------- tool_id : str The id of the gripper tool that is used. - attached_workpiece_id : str, optional + attaching_workpiece_id : str, optional The id of the workpiece attached to the gripper. - If the workpiece is not specified, the gripper is assumed to be empty. - attached_workpiece_grasp : :class:`compas.geometry.Transformation`, optional - The grasp frame of the workpiece relative to the robot flange. - If the workpiece_frame is not specified, the workpiece Frame is - assumed to be the same as the gripper's TCP. + If no workpiece is attached during the gripper closure, None. + attaching_workpiece_grasp : :class:`compas.geometry.Transformation`, optional + If attaching_workpiece_id is not None, the grasp frame of the workpiece + relative to the robot flange. Default is the identity transformation. + If attaching_workpiece_id is None, this attribute is meaningless. """ - def __init__(self): + def __init__(self, tool_id = None, attaching_workpiece_id=None, attaching_workpiece_grasp=Transformation()): super(CloseGripper, self).__init__() - self.tool_id = None # type: Frame - self.attached_workpiece_id = None # type: Frame - self.attached_workpiece_grasp = None # type: Frame + self.tool_id = tool_id # type: Frame + self.attaching_workpiece_id = attaching_workpiece_id # type: Optional[str] + self.attaching_workpiece_grasp = attaching_workpiece_grasp # type: Optional[Transformation] @property def data(self): data = super(CloseGripper, self).data - data["attached_workpiece_id"] = self.attached_workpiece_id - data["attached_workpiece_grasp"] = self.attached_workpiece_grasp + data["attaching_workpiece_id"] = self.attaching_workpiece_id + data["attaching_workpiece_grasp"] = self.attaching_workpiece_grasp return data @data.setter def data(self, data): super(CloseGripper, type(self)).data.fset(self, data) - self.attached_workpiece_id = data.get("attached_workpiece_id", self.attached_workpiece_id) - self.attached_workpiece_grasp = data.get("attached_workpiece_grasp", self.attached_workpiece_grasp) + self.attaching_workpiece_id = data.get("attaching_workpiece_id", self.attaching_workpiece_id) + self.attaching_workpiece_grasp = data.get("attaching_workpiece_grasp", self.attaching_workpiece_grasp) + + def check_preconditions(self, scene_state): + # type: (SceneState) -> Tuple(bool, str) + """Checks if the action can be applied to a scene state. + + This function does not change the scene state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + + Returns + ------- + Tuple(bool, str) + A tuple of a boolean and a string. + The boolean indicates if the action can be applied to the scene state. + The string is a human readable message that describes the reason for failure. + """ + attached_tool_id = scene_state.get_attached_tool_id() + if attached_tool_id is None: + return (False, "Inconsistency: No tool attached to robot.") + if self.tool_id != attached_tool_id: + return (False, "Inconsistency: CloseGripper.tool_id does not match the attached tool id.") + if self.attaching_workpiece_id is not None: + attached_workpiece_id = scene_state.get_attached_workpiece_id() + if attached_workpiece_id == self.attaching_workpiece_id: + return (False, "Inconsistency: Workpiece is already attached to the tool.") + if attached_workpiece_id is not None: + return (False, "Inconsistency: Another workpiece is already attached to the tool.") + if self.attaching_workpiece_id not in scene_state.workpiece_states: + return (False, "Inconsistency: Workpiece is not in the scene.") + # TODO: Check if tool is a gripper + return (True, None) - def apply_to(self, scene_state, debug=False): + def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. @@ -412,32 +500,24 @@ def apply_to(self, scene_state, debug=False): scene_state : :class:`compas_fab.planning.SceneState` The scene state to apply the action to. """ - # Transform attached objects - attached_tool_id = scene_state.get_attached_tool_id() - assert attached_tool_id is not None, "Inconsistency: No tool attached to robot." - attached_tool_state = scene_state.get_tool_state(attached_tool_id) - # TODO: Check if tool is a gripper # TODO: Change the configuration of the gripper to closed if debug: print("Gripper closed.") # Transform attached workpieces - if self.attached_workpiece_id is not None: - existing_workpiece_id = scene_state.get_attached_workpiece_id() - assert existing_workpiece_id is None, "Inconsistency: Another workpiece is already attached to the tool." - attached_workpiece = scene_state.get_workpiece_state(self.attached_workpiece_id) + if self.attaching_workpiece_id is not None: + workpiece_state = scene_state.get_workpiece_state(self.attaching_workpiece_id) # Update the workpiece grasp and frame - attached_workpiece.attached_to_tool_id = attached_tool_id - attached_workpiece.attached_to_tool_grasp = self.attached_workpiece_grasp or Transformation() + workpiece_state.attached_to_robot = True + workpiece_state.attached_to_robot_grasp = self.attaching_workpiece_grasp or Transformation() + robot_flange_frame = scene_state.get_robot_state().frame + workpiece_state.frame = Frame.from_transformation( + Transformation.from_frame(robot_flange_frame) * workpiece_state.attached_to_robot_grasp) if debug: - print("- Workpiece %s attached to tool." % self.attached_workpiece_id) - # attached_workpiece.frame = attached_tool_state.frame * attached_workpiece.attached_to_tool_grasp - attached_workpiece.frame = Frame.from_transformation( - Transformation.from_frame(attached_tool_state.frame) * attached_workpiece.attached_to_tool_grasp - ) + print("- Workpiece %s attached to tool." % self.attaching_workpiece_id) -class LoadWorkpiece(Action): +class ManuallyMoveWorkpiece(Action): """Action to load a workpiece, assumed to be performed by a human operator. This moves the workpiece to a specific frame. Typically used for loading a workpiece into a gripper. @@ -446,28 +526,56 @@ class LoadWorkpiece(Action): Attributes ---------- workpiece_id : str - The id of the workpiece to be loaded. + The id of the workpiece to be moved. + frame : :class:`compas.geometry.Frame` + The frame of the workpiece at the target location. + Specified in world coordinate frame. """ - def __init__(self): - super(LoadWorkpiece, self).__init__() - self.workpiece_id = None # type: str - self.frame = None # type: Frame + def __init__(self, workpiece_id=None, frame=Frame.worldXY()): + super(ManuallyMoveWorkpiece, self).__init__() + self.workpiece_id = workpiece_id # type: str + self.frame = frame # type: Frame @property def data(self): - data = super(LoadWorkpiece, self).data + data = super(ManuallyMoveWorkpiece, self).data data["workpiece_id"] = self.workpiece_id data["frame"] = self.frame return data @data.setter def data(self, data): - super(LoadWorkpiece, type(self)).data.fset(self, data) + super(ManuallyMoveWorkpiece, type(self)).data.fset(self, data) self.workpiece_id = data.get("workpiece_id", self.workpiece_id) self.frame = data.get("frame", self.frame) - def apply_to(self, scene_state, debug=False): + def check_preconditions(self, scene_state): + # type: (SceneState) -> Tuple(bool, str) + """Checks if the action can be applied to a scene state. + + This function does not change the scene state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + + Returns + ------- + Tuple(bool, str) + A tuple of a boolean and a string. + The boolean indicates if the action can be applied to the scene state. + The string is a human readable message that describes the reason for failure. + """ + if self.workpiece_id not in scene_state.workpiece_states: + return (False, "Inconsistency: Workpiece is not in the scene.") + workpiece_state = scene_state.get_workpiece_state(self.workpiece_id) + if workpiece_state.attached_to_robot is True: + return (False, "Inconsistency: Workpiece is already attached to the robot.") + return (True, None) + + def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. The SceneState is updated with the new robot state. @@ -479,8 +587,6 @@ def apply_to(self, scene_state, debug=False): """ # Transform attached objects workpiece_state = scene_state.get_workpiece_state(self.workpiece_id) - assert workpiece_state.attached_to_tool_id is None, "Inconsistency: Workpiece is already attached to a tool." workpiece_state.frame = self.frame if debug: print("Workpiece %s loaded to new location." % self.workpiece_id) - diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index 75b1d1a17b..fae50e8ce8 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -64,8 +64,6 @@ def __init__(self, workpiece_ids=[], tool_ids=[]): self.workpiece_states[workpiece_id] = WorkpieceState(workpiece_id) for tool_id in tool_ids: self.tool_states[tool_id] = ToolState(tool_id) - self.workpiece_states["s"].attached_to_tool_grasp - self.tool_states["s"].attached_to_robot_grasp @property def data(self): @@ -136,7 +134,7 @@ def get_attached_workpiece_id(self): If no workpiece is attached, `None` is returned. """ for workpiece_id, workpiece_state in self.workpiece_states.items(): - if workpiece_state.attached_to_tool_id: + if workpiece_state.attached_to_robot: return workpiece_id return None @@ -153,33 +151,41 @@ class WorkpieceState(Data): frame : :class:`compas.geometry.Frame` The current location of the workpiece. (default: :class:`compas.geometry.Frame.worldXY`) - attached_to_tool_id : str or None - If the workpiece is attached to a tool, the id of the tool. - If the workpiece is not attached to a tool, `None`. (default: `None`) - attached_to_tool_grasp : :class:`compas.geometry.Transformation` - If the workpiece is attached to a tool, the grasp frame of the workpiece. + attached_to_robot : bool + If the workpiece is attached to the robot, `True`. Else, `False`. + attached_to_robot_grasp : :class:`compas.geometry.Transformation`, optional + The grasp transformation of the workpiece if it is attached to the robot. The grasp + is defined as the transformation that can transform the robot flange frame + into the workpiece frame. If not specified, defaults to the identity transformation. - If the workpiece is not attached to a tool, `None`. + If the workpiece is not attached to the robot, `None`. is_hidden : bool If the workpiece is hidden, `True`. Else, `False`. (default: `False`) A hidden workpiece will not be included for collision detection of the scene. """ - def __init__(self, workpiece_id="undefined_workpiece"): + def __init__( + self, + workpiece_id="undefined_workpiece", + frame=None, + attached_to_robot=False, + attached_to_robot_grasp=Transformation(), + is_hidden=False, + ): super(WorkpieceState, self).__init__() self.workpiece_id = workpiece_id - self.frame = Frame.worldXY() # type: Frame - self.attached_to_tool_id = None # type: Optional[str] - self.attached_to_tool_grasp = Transformation() # type: Optional[Transformation] - self.is_hidden = False # type: bool + self.frame = frame or Frame.worldXY() # type: Frame + self.attached_to_robot = attached_to_robot # type: Optional[str] + self.attached_to_robot_grasp = attached_to_robot_grasp # type: Optional[Transformation] + self.is_hidden = is_hidden # type: bool @property def data(self): data = {} data["workpiece_id"] = self.workpiece_id data["frame"] = self.frame - data["attached_to_tool_id"] = self.attached_to_tool_id - data["attached_to_tool_grasp"] = self.attached_to_tool_grasp + data["attached_to_robot"] = self.attached_to_robot + data["attached_to_robot_grasp"] = self.attached_to_robot_grasp data["is_hidden"] = self.is_hidden return data @@ -187,8 +193,8 @@ def data(self): def data(self, data): self.workpiece_id = data.get("workpiece_id", self.workpiece_id) self.frame = data.get("frame", self.frame) - self.attached_to_tool_id = data.get("attached_to_tool_id", self.attached_to_tool_id) - self.attached_to_tool_grasp = data.get("attached_to_tool_grasp", self.attached_to_tool_grasp) + self.attached_to_robot = data.get("attached_to_robot", self.attached_to_robot) + self.attached_to_robot_grasp = data.get("attached_to_robot_grasp", self.attached_to_robot_grasp) self.is_hidden = data.get("is_hidden", self.is_hidden) @@ -203,21 +209,22 @@ class ToolState(Data): tool_id : str Unique identifier of the tool used in Process.tools and SceneState.tool_states. frame : :class:`compas.geometry.Frame` - The current location of the tool. + The current location of the base frame of the tool. attached_to_robot : bool If the tool is attached to a robot, `True`. Else, `False`. attached_to_robot_grasp : :class:`compas.geometry.Transformation` If the tool is attached to a robot, the base frame of the tool relative to the robot flange. + Defaults to the identity transformation. configuration : :class:`compas.robots.Configuration`, optional If the tool is kinematic, the current configuration of the tool. """ - def __init__(self, tool_id="undefined_tool", configuration=None): + def __init__(self, tool_id="undefined_tool", frame=None, attached_to_robot_grasp=None, configuration=None): super(ToolState, self).__init__() self.tool_id = tool_id - self.frame = Frame.worldXY() # type: Frame + self.frame = frame or Frame.worldXY() # type: Frame self.attached_to_robot = False # type: bool - self.attached_to_robot_grasp = None # type: Optional[Transformation] + self.attached_to_robot_grasp = attached_to_robot_grasp or Transformation() # type: Transformation self.configuration = configuration # type: Optional[Configuration] @property From d6575688d894cc292c52cfb6156c9333188e0eea Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 17 Nov 2023 00:23:21 +0800 Subject: [PATCH 10/28] lint --- src/compas_fab/planning/action.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index a9133fc6b3..f0fa2ae96d 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -1,5 +1,4 @@ from compas.data import Data -from compas.geometry import Point from compas.geometry import Frame from compas.geometry import Transformation @@ -348,7 +347,7 @@ class OpenGripper(Action): It is possible to open the gripper with or without a workpiece attached. """ - def __init__(self, tool_id = None, ): + def __init__(self, tool_id=None): super(OpenGripper, self).__init__() self.tool_id = tool_id # type: Frame @@ -435,7 +434,7 @@ class CloseGripper(Action): If attaching_workpiece_id is None, this attribute is meaningless. """ - def __init__(self, tool_id = None, attaching_workpiece_id=None, attaching_workpiece_grasp=Transformation()): + def __init__(self, tool_id=None, attaching_workpiece_id=None, attaching_workpiece_grasp=Transformation()): super(CloseGripper, self).__init__() self.tool_id = tool_id # type: Frame self.attaching_workpiece_id = attaching_workpiece_id # type: Optional[str] @@ -512,7 +511,8 @@ def apply_effects(self, scene_state, debug=False): workpiece_state.attached_to_robot_grasp = self.attaching_workpiece_grasp or Transformation() robot_flange_frame = scene_state.get_robot_state().frame workpiece_state.frame = Frame.from_transformation( - Transformation.from_frame(robot_flange_frame) * workpiece_state.attached_to_robot_grasp) + Transformation.from_frame(robot_flange_frame) * workpiece_state.attached_to_robot_grasp + ) if debug: print("- Workpiece %s attached to tool." % self.attaching_workpiece_id) From 20cd04d10fa75cd5c1522a6b242d642db2ea5024 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 17 Nov 2023 00:23:27 +0800 Subject: [PATCH 11/28] tests --- tests/planning/test_action.py | 260 ++++++++++++++++++++++++++++++++++ 1 file changed, 260 insertions(+) create mode 100644 tests/planning/test_action.py diff --git a/tests/planning/test_action.py b/tests/planning/test_action.py new file mode 100644 index 0000000000..4d47ff9a0f --- /dev/null +++ b/tests/planning/test_action.py @@ -0,0 +1,260 @@ +import pytest + + +from compas.geometry import Frame +from compas.geometry import Point +from compas.geometry import Vector +from compas.geometry import Transformation +from compas.geometry import Translation + +from compas.robots import Configuration + +from compas_fab.robots import Duration +from compas_fab.robots import JointTrajectory +from compas_fab.robots import JointTrajectoryPoint + +from compas_fab.planning import Action +from compas_fab.planning import RoboticAction +from compas_fab.planning import CartesianMotion +from compas_fab.planning import FreeMotion +from compas_fab.planning import OpenGripper +from compas_fab.planning import CloseGripper +from compas_fab.planning import ManuallyMoveWorkpiece + +from compas_fab.planning import SceneState +from compas_fab.planning import WorkpieceState +from compas_fab.planning import ToolState +from compas_fab.planning import RobotState + + +@pytest.fixture +def robot_state(): + return RobotState( + frame=Frame.worldXY(), + configuration=Configuration.from_revolute_values( + [10, 20, 30, 40, 50, 60], ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + ), + ) + + +@pytest.fixture +def start_scene_state(robot_state): + # type: (RobotState) -> SceneState + scene_state = SceneState( + workpiece_ids=["w1", "w2"], + tool_ids=["t1", "t2"], + ) + scene_state.workpiece_states["w1"] = WorkpieceState( + "w1", Frame(Point(10.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0), Vector(0.0, 0.0, 1.0)) + ) + scene_state.workpiece_states["w2"] = WorkpieceState( + "w2", Frame(Point(20.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0), Vector(0.0, 0.0, 1.0)) + ) + scene_state.tool_states["t1"] = ToolState("t1", Frame.worldYZ()) + scene_state.tool_states["t2"] = ToolState("t2", Frame.worldZX()) + # T1 is attached to the robot and the attachment point has an offset of 100mm in z-direction + scene_state.tool_states["t1"].attached_to_robot = True + scene_state.tool_states["t1"].attached_to_robot_grasp = Translation.from_vector([0, 0, 100]) + # W1 is attached to T1 and the attachment direction is rotated by -90 degrees around the Y-axis + scene_state.workpiece_states["w1"].attached_to_robot = True + scene_state.workpiece_states["w1"].attached_to_robot_grasp = Transformation.from_frame(Frame.worldYZ()) + scene_state.robot_state = robot_state + return scene_state + + +@pytest.fixture +def jtp(): + return JointTrajectoryPoint( + [1.571, 0, 0, 0.262, 0, 0], + [0] * 6, + [3.0] * 6, + time_from_start=Duration(2, 1293), + joint_names=["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"], + ) + + +@pytest.fixture +def trj(): + joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + p1 = JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.0] * 6, time_from_start=Duration(2, 1293)) + p2 = JointTrajectoryPoint([0.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.0] * 6, time_from_start=Duration(6, 0)) + start_configuration = Configuration.from_prismatic_and_revolute_values( + p1.prismatic_values, p1.revolute_values, joint_names + ) + + return JointTrajectory( + trajectory_points=[p1, p2], joint_names=joint_names, start_configuration=start_configuration, fraction=1.0 + ) + + +def test_action_members(): + # Test child class of Action + assert isinstance(RoboticAction(), Action) + assert isinstance(CartesianMotion(), Action) + assert isinstance(FreeMotion(), Action) + assert isinstance(OpenGripper(), Action) + assert isinstance(CloseGripper(), Action) + assert isinstance(ManuallyMoveWorkpiece(), Action) + # Test child class of RoboticAction + assert isinstance(CartesianMotion(), RoboticAction) + assert isinstance(FreeMotion(), RoboticAction) + + +def test_robot_action_serialization(trj, jtp): + action = RoboticAction() + action.target_robot_flange_frame = Frame.worldXY() + action.allowed_collision_pairs = [("a1", "b1"), ("c1", "d1")] + action.fixed_target_configuration = Configuration.from_data(jtp.data) + action.fixed_trajectory = trj + data = action.to_data() + new_action = RoboticAction.from_data(data) + assert new_action.to_data() == data + assert new_action.target_robot_flange_frame == Frame.worldXY() + assert ("a1", "b1") in new_action.allowed_collision_pairs + assert ("c1", "d1") in new_action.allowed_collision_pairs + assert new_action.fixed_target_configuration["joint_1"] == 1.571 + trajectory_point = new_action.fixed_trajectory.points[1] + full_config = Configuration.from_prismatic_and_revolute_values( + trajectory_point.prismatic_values, trajectory_point.revolute_values, new_action.fixed_trajectory.joint_names + ) + assert full_config["joint_1"] == 0.571 + assert new_action.fixed_trajectory.time_from_start == Duration(6, 0).seconds + + +def test_robotic_motion_effect(start_scene_state): + motion = RoboticAction() + # Target frame is on the YZ plane of the world + target_flange_frame = Frame(Point(100.0, 200.0, 300.0), Vector(0.0, 1.0, 0.0), Vector(0.0, 0.0, 1.0)) + motion.target_robot_flange_frame = target_flange_frame + scene_state = start_scene_state.copy() + motion.apply_effects(scene_state) + + # Robot Frame + assert scene_state.robot_state.frame == target_flange_frame + # Attached Tool + assert scene_state.get_attached_tool_id() == "t1" + grasp = Translation.from_vector([0, 0, 100]) + assert scene_state.get_tool_state("t1").attached_to_robot_grasp == grasp + tool_frame = Frame(Point(200.000, 200.000, 300.000), Vector(0.000, 1.000, 0.000), Vector(0.000, 0.000, 1.000)) + assert scene_state.get_tool_state("t1").frame == tool_frame + # Stationary Tool + assert scene_state.get_tool_state("t2").frame == Frame.worldZX() + # Attached Workpiece + assert scene_state.get_attached_workpiece_id() == "w1" + assert scene_state.get_workpiece_state("w1").attached_to_robot_grasp == Transformation.from_frame(Frame.worldYZ()) + # Note that the workpiece frame is not stacked on top of the tool frame transformation + workpiece_frame = Frame( + Point(100.000, 200.000, 300.000), Vector(0.000, 0.000, 1.000), Vector(1.000, -0.000, -0.000) + ) + assert scene_state.get_workpiece_state("w1").frame == workpiece_frame + + +def test_open_gripper_effect(start_scene_state): + action = OpenGripper() + action.apply_effects(start_scene_state) + assert start_scene_state.get_attached_workpiece_id() is None + assert start_scene_state.workpiece_states["w1"].attached_to_robot is False + assert start_scene_state.workpiece_states["w2"].attached_to_robot is False + + +def test_close_gripper_effect(start_scene_state): + # Set that no objects are attached to the robot + start_scene_state.workpiece_states["w1"].attached_to_robot = False + start_scene_state.workpiece_states["w2"].attached_to_robot = False + robot_flange_frame = Frame(Point(10.0, 20.0, 30.0), Vector(0.0, 1.0, 0.0), Vector(0.0, 0.0, 1.0)) + start_scene_state.robot_state.frame = robot_flange_frame + # Attach workpiece w1 to the robot + grasp = Translation.from_vector([0, 0, 10]) + action = CloseGripper("t1", "w1", grasp) + action.apply_effects(start_scene_state) + # Check that the workpiece w1 is attached to the robot + assert start_scene_state.get_attached_workpiece_id() == "w1" + assert start_scene_state.workpiece_states["w1"].attached_to_robot is True + assert start_scene_state.workpiece_states["w1"].attached_to_robot_grasp == grasp + workpiece_frame = Frame(Point(20.0, 20.0, 30.0), Vector(0.0, 1.0, 0.0), Vector(0.0, 0.0, 1.0)) + assert start_scene_state.workpiece_states["w1"].frame == workpiece_frame + # Workpiece w2 remains unattached + assert start_scene_state.workpiece_states["w2"].attached_to_robot is False + + +def test_open_gripper_preconditions(start_scene_state): + action = OpenGripper("t1") + assert action.check_preconditions(start_scene_state)[0] is True + # Detach the tool from the robot and check that the preconditions are not met + start_scene_state.tool_states["t1"].attached_to_robot = False + assert action.check_preconditions(start_scene_state)[0] is False + # Attach a wrong tool to the robot and check that the preconditions are not met + start_scene_state.tool_states["t2"].attached_to_robot = True + assert action.check_preconditions(start_scene_state)[0] is False + + +def test_close_gripper_preconditions(start_scene_state): + action = CloseGripper("t1", "w1") + # Default state object has one object attached to the robot + assert start_scene_state.get_attached_tool_id() == "t1" + assert start_scene_state.get_attached_workpiece_id() is not None + assert action.check_preconditions(start_scene_state)[0] is False + + # Set that no objects are attached to the robot + start_scene_state.workpiece_states["w1"].attached_to_robot = False + start_scene_state.workpiece_states["w2"].attached_to_robot = False + # Attach workpiece w1 to the robot + assert action.check_preconditions(start_scene_state)[0] is True + + # Detach the tool from the robot and check that the preconditions are not met + start_scene_state.tool_states["t1"].attached_to_robot = False + assert action.check_preconditions(start_scene_state)[0] is False + + # Attach workpiece w2 to the robot and check that the preconditions are not met + start_scene_state.tool_states["t1"].attached_to_robot = True + start_scene_state.workpiece_states["w2"].attached_to_robot = True + assert action.check_preconditions(start_scene_state)[0] is False + + # Attach a wrong tool to the robot and check that the preconditions are not met + action = CloseGripper("t1", "w1") + start_scene_state.tool_states["t1"].attached_to_robot = False + start_scene_state.tool_states["t2"].attached_to_robot = True + start_scene_state.workpiece_states["w1"].attached_to_robot = False + start_scene_state.workpiece_states["w2"].attached_to_robot = False + assert action.check_preconditions(start_scene_state)[0] is False + + action = CloseGripper("t2", "w2") + assert action.check_preconditions(start_scene_state)[0] is True + + # Check non-existing tool + action = CloseGripper("t3", "w2") + assert action.check_preconditions(start_scene_state)[0] is False + + # Check non-existing workpiece + action = CloseGripper("t2", "w3") + assert action.check_preconditions(start_scene_state)[0] is False + + +def test_load_workpiece_preconditions(start_scene_state): + action = ManuallyMoveWorkpiece("w1", None) + # Default state object has one object attached to the robot + assert start_scene_state.get_attached_tool_id() == "t1" + assert start_scene_state.get_attached_workpiece_id() == "w1" + assert action.check_preconditions(start_scene_state)[0] is False + + # Detach the workpiece from the robot + start_scene_state.workpiece_states["w1"].attached_to_robot = False + # ManuallyMoveWorkpiece action requires that the workpiece is not attached to the robot + assert action.check_preconditions(start_scene_state)[0] is True + + # Check non-existing workpiece + action = ManuallyMoveWorkpiece("w3", None) + assert action.check_preconditions(start_scene_state)[0] is False + + +def test_load_workpiece_effect(start_scene_state): + workpiece_frame = Frame(Point(100.0, 200.0, 300.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0)) + assert start_scene_state.get_attached_workpiece_id() == "w1" + action = ManuallyMoveWorkpiece("w2", workpiece_frame) + + action.apply_effects(start_scene_state) + # Workpiece w2 should remain unattached but moved + assert start_scene_state.workpiece_states["w2"].attached_to_robot is False + assert start_scene_state.workpiece_states["w2"].frame == workpiece_frame + # Workpiece w1 should remain attached + assert start_scene_state.get_attached_workpiece_id() == "w1" From ba607cb7efe2d558c32b7f41846b7a5dc4e7cb4c Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 17 Nov 2023 00:32:08 +0800 Subject: [PATCH 12/28] lint --- src/compas_fab/planning/action.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index f0fa2ae96d..4cd7735a89 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -1,5 +1,6 @@ from compas.data import Data from compas.geometry import Frame +from compas.geometry import Point # noqa: F401 from compas.geometry import Transformation from compas_fab.robots import Configuration, JointTrajectory # noqa: F401 @@ -275,7 +276,7 @@ class CartesianMotion(RoboticAction): def __init__(self): super(CartesianMotion, self).__init__() - self.polyline_target = [] # type: list(Point) + self.polyline_target = [] # type: list[Point] self.tag = "Linear Movement" @property From aebb0a3418dcf155c897d9d317709b5a88b5ff58 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Fri, 17 Nov 2023 15:35:33 +0800 Subject: [PATCH 13/28] HideWorkpieces and ShowWorkpieces Actions --- src/compas_fab/planning/__init__.py | 10 +- src/compas_fab/planning/action.py | 169 +++++++++++++++++++++++++++- 2 files changed, 171 insertions(+), 8 deletions(-) diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index c1d0154259..f553f1821c 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -23,7 +23,7 @@ Actions -------- -Actions are abstractions of the robot's (and, or operator's) capability to manupulate tools and +Action classes are abstractions of the robot's (and, or the operator's) capability to manupulate tools and workpieces in the scene. Action classes are used for modelling a process for the following purpose: * To plan trajectories for robotic motions @@ -42,8 +42,8 @@ CloseGripper ManuallyMoveWorkpiece -State ------ +States +------ State classes are used to model the static state of objects in the planning scene. This include: :class:`RobotState` for :class:`compas_fab.robots.Robot`, @@ -77,6 +77,8 @@ OpenGripper, CloseGripper, ManuallyMoveWorkpiece, + HideWorkpieces, + ShowWorkpieces, ) from .state import ( @@ -94,6 +96,8 @@ "OpenGripper", "CloseGripper", "ManuallyMoveWorkpiece", + "HideWorkpieces", + "ShowWorkpieces", "SceneState", "WorkpieceState", "ToolState", diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 4cd7735a89..23d1758155 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -213,7 +213,7 @@ def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. - The SceneState is updated with the new robot state. + The SceneState is modified in place with the effect of the Action. If a fixed_trajectory or planned_trajectory is available, the robot_state.configuration is updated with the last configuration of the trajectory. If fixed_target_configuration is available, the robot_state.configuration is updated with the fixed_target_configuration. @@ -394,7 +394,7 @@ def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. - The SceneState is updated with the new robot state. + The SceneState is modified in place with the effect of the Action. It is possible to open the gripper with or without a workpiece attached. Parameters @@ -492,7 +492,7 @@ def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. - The SceneState is updated with the new robot state. + The SceneState is modified in place with the effect of the Action. It is possible to open the gripper with or without a workpiece attached. Parameters @@ -519,7 +519,7 @@ def apply_effects(self, scene_state, debug=False): class ManuallyMoveWorkpiece(Action): - """Action to load a workpiece, assumed to be performed by a human operator. + """Operator action to move a workpiece from one place to another. This moves the workpiece to a specific frame. Typically used for loading a workpiece into a gripper. Can also be used to model the manual attachment of scaffolding (modeled as a :class:`Workpiece`) @@ -579,7 +579,8 @@ def check_preconditions(self, scene_state): def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. - The SceneState is updated with the new robot state. + + The SceneState is modified in place with the effect of the Action. Parameters ---------- @@ -591,3 +592,161 @@ def apply_effects(self, scene_state, debug=False): workpiece_state.frame = self.frame if debug: print("Workpiece %s loaded to new location." % self.workpiece_id) + + +class HideWorkpieces(Action): + """Action to hide workpieces. Can be used to model objects that are removed the scene. + + This changes the WorkpieceState.is_hidden property. + Can also be used to model the manual detachment of scaffolding (modeled as a :class:`Workpiece`) + + Attributes + ---------- + workpiece_ids : list(str) + List of workpiece ids to be hidden. + """ + + def __init__(self, workpiece_ids=[]): + super(HideWorkpieces, self).__init__() + self.workpiece_ids = workpiece_ids # type: list[str] + + @property + def data(self): + data = super(HideWorkpieces, self).data + data["workpiece_ids"] = self.workpiece_ids + return data + + @data.setter + def data(self, data): + super(HideWorkpieces, type(self)).data.fset(self, data) + self.workpiece_ids = data.get("workpiece_ids", self.workpiece_ids) + + def check_preconditions(self, scene_state): + # type: (SceneState) -> Tuple(bool, str) + """Checks if the action can be applied to a scene state. + + All the workpiece_ids must be: + - in the scene + - not hidden + - not attached to the robot + + This function does not change the scene state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + + Returns + ------- + Tuple(bool, str) + A tuple of a boolean and a string. + The boolean indicates if the action can be applied to the scene state. + The string is a human readable message that describes the reason for failure. + """ + for workpiece_id in self.workpiece_ids: + if workpiece_id not in scene_state.workpiece_states: + return (False, "Inconsistency: Workpiece %s is not in the scene." % workpiece_id) + workpiece_state = scene_state.get_workpiece_state(workpiece_id) + if workpiece_state.is_hidden: + return (False, "Inconsistency: Workpiece %s is already hidden." % workpiece_id) + if workpiece_state.attached_to_robot: + return (False, "Inconsistency: Workpiece %s is attached to the robot." % workpiece_id) + + return (True, None) + + def apply_effects(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + + The SceneState is modified in place with the effect of the Action. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + for workpiece_id in self.workpiece_ids: + workpiece_state = scene_state.get_workpiece_state(workpiece_id) + workpiece_state.is_hidden = True + if debug: + print("Workpiece %s is hidden." % workpiece_id) + + +class ShowWorkpieces(Action): + """Action to show workpieces. Can be used to model objects that are added to the scene. + + This changes the WorkpieceState.is_hidden property. + Can also be used to model the manual attachment of scaffolding (modeled as a :class:`Workpiece`) + + Attributes + ---------- + workpiece_ids : list(str) + List of workpiece ids to be shown. + """ + + def __init__(self, workpiece_ids=[]): + super(ShowWorkpieces, self).__init__() + self.workpiece_ids = workpiece_ids # type: list[str] + + @property + def data(self): + data = super(ShowWorkpieces, self).data + data["workpiece_ids"] = self.workpiece_ids + return data + + @data.setter + def data(self, data): + super(ShowWorkpieces, type(self)).data.fset(self, data) + self.workpiece_ids = data.get("workpiece_ids", self.workpiece_ids) + + def check_preconditions(self, scene_state): + # type: (SceneState) -> Tuple(bool, str) + """Checks if the action can be applied to a scene state. + + All the workpiece_ids must be: + - in the scene + - hidden + - not attached to the robot + + This function does not change the scene state. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + + Returns + ------- + Tuple(bool, str) + A tuple of a boolean and a string. + The boolean indicates if the action can be applied to the scene state. + The string is a human readable message that describes the reason for failure. + """ + for workpiece_id in self.workpiece_ids: + if workpiece_id not in scene_state.workpiece_states: + return (False, "Inconsistency: Workpiece %s is not in the scene." % workpiece_id) + workpiece_state = scene_state.get_workpiece_state(workpiece_id) + if not workpiece_state.is_hidden: + return (False, "Inconsistency: Workpiece %s is not hidden." % workpiece_id) + if workpiece_state.attached_to_robot: + return (False, "Inconsistency: Workpiece %s is attached to the robot." % workpiece_id) + + return (True, None) + + def apply_effects(self, scene_state, debug=False): + # type: (SceneState, bool) -> None + """Applies the action to a scene state. + + The SceneState is modified in place with the effect of the Action. + + Parameters + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + for workpiece_id in self.workpiece_ids: + workpiece_state = scene_state.get_workpiece_state(workpiece_id) + workpiece_state.is_hidden = False + if debug: + print("Workpiece %s is shown." % workpiece_id) From 0c3cd1cb09b23756a7281eeae2c5f0095c91b0fe Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 22 Nov 2023 14:15:38 +0800 Subject: [PATCH 14/28] Change Naming: CartesianMotion to LinearMotion --- CHANGELOG.md | 2 +- src/compas_fab/planning/__init__.py | 4 ++-- src/compas_fab/planning/action.py | 10 +++++----- tests/planning/test_action.py | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 93150afccf..bb7c0434e3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added * Added `compas_fab.planning` that contains `Action` classes and `SceneState` -* Added `RoboticAction`, `CartesianMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, and `ManuallyMoveWorkpiece` actions. +* Added `RoboticAction`, `LinearMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, and `ManuallyMoveWorkpiece` actions. * Added `SceneState` as container for `WorkpieceState`, `ToolState` and `RobotState`. ### Changed diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index f553f1821c..df5ac9d51b 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -72,7 +72,7 @@ from .action import ( Action, RoboticAction, - CartesianMotion, + LinearMotion, FreeMotion, OpenGripper, CloseGripper, @@ -91,7 +91,7 @@ __all__ = [ "Action", "RoboticAction", - "CartesianMotion", + "LinearMotion", "FreeMotion", "OpenGripper", "CloseGripper", diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 23d1758155..f337e5f93f 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -19,7 +19,7 @@ __all__ = [ "Action", "RoboticAction", - "CartesianMotion", + "LinearMotion", "FreeMotion", "OpenGripper", "CloseGripper", @@ -256,7 +256,7 @@ def apply_effects(self, scene_state, debug=False): print("- Attached Workpiece %s Followed." % attached_workpiece_id) -class CartesianMotion(RoboticAction): +class LinearMotion(RoboticAction): """Action class to describe a Cartesian robotic movement. The trajectory of the robot flange is interpolated in Cartesian space. @@ -275,19 +275,19 @@ class CartesianMotion(RoboticAction): """ def __init__(self): - super(CartesianMotion, self).__init__() + super(LinearMotion, self).__init__() self.polyline_target = [] # type: list[Point] self.tag = "Linear Movement" @property def data(self): - data = super(CartesianMotion, self).data + data = super(LinearMotion, self).data data["polyline_target"] = self.polyline_target return data @data.setter def data(self, data): - super(CartesianMotion, type(self)).data.fset(self, data) + super(LinearMotion, type(self)).data.fset(self, data) self.polyline_target = data.get("polyline_target", self.polyline_target) diff --git a/tests/planning/test_action.py b/tests/planning/test_action.py index 4d47ff9a0f..38193bbcc8 100644 --- a/tests/planning/test_action.py +++ b/tests/planning/test_action.py @@ -90,13 +90,13 @@ def trj(): def test_action_members(): # Test child class of Action assert isinstance(RoboticAction(), Action) - assert isinstance(CartesianMotion(), Action) + assert isinstance(LinearMotion(), Action) assert isinstance(FreeMotion(), Action) assert isinstance(OpenGripper(), Action) assert isinstance(CloseGripper(), Action) assert isinstance(ManuallyMoveWorkpiece(), Action) # Test child class of RoboticAction - assert isinstance(CartesianMotion(), RoboticAction) + assert isinstance(LinearMotion(), RoboticAction) assert isinstance(FreeMotion(), RoboticAction) From 2779bc93a6cf61f57803a91152a006ec01bd01c2 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 22 Nov 2023 14:26:49 +0800 Subject: [PATCH 15/28] Update LinearMotion class to include multiple linear segments by specifying intermediate_targets. --- src/compas_fab/planning/action.py | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index f337e5f93f..12e1ab72d8 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -257,38 +257,44 @@ def apply_effects(self, scene_state, debug=False): class LinearMotion(RoboticAction): - """Action class to describe a Cartesian robotic movement. + """Action class to describe a Linear robotic movement. + + A linear robotic movement moves the robot flange linearly in Cartesian space. The motion can + contain multiple linear segments (see intermediate_targets attribute). - The trajectory of the robot flange is interpolated in Cartesian space. Typically, the orientation between the starting frame and the target frame is the same. - However some linear motion planners can interpolate between different orientations. + However some linear motion planners can also interpolate between different orientations, check + the documentation of the planner for more details. + Attributes ---------- - polyline_target : list(:class:`compas.geometry.Point`) - List of points to define a linear movement that is a polyline. - Specified in world coordinate frame. - The first point (the starting point) is not required. - The second point onwards, including the last point (the ending point) is required. + intermediate_targets : list(:class:`compas.geometry.Frame`), optional + List of frames to define a linear movement that has multiple linear segments. + The frames are specified in the world coordinate frame. + Note that only the intermediate targets are specified, the starting and ending frames + should not be included in this list. The ending frame is specified by the target_robot_flange_frame attribute. + + Default is an empty list, meaning the movement only has a single linear segment. see :class:`compas_fab.planning.RoboticAction` for other attributes. """ def __init__(self): super(LinearMotion, self).__init__() - self.polyline_target = [] # type: list[Point] + self.intermediate_targets = [] # type: Optional[list[Point]] self.tag = "Linear Movement" @property def data(self): data = super(LinearMotion, self).data - data["polyline_target"] = self.polyline_target + data["intermediate_targets"] = self.intermediate_targets return data @data.setter def data(self, data): super(LinearMotion, type(self)).data.fset(self, data) - self.polyline_target = data.get("polyline_target", self.polyline_target) + self.intermediate_targets = data.get("intermediate_targets", self.intermediate_targets) class FreeMotion(RoboticAction): From 8775f354a58627bb5d4e2d07a7d443040de00f48 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 22 Nov 2023 14:30:29 +0800 Subject: [PATCH 16/28] missing HideWorkpieces and ShowWorkpieces in action.all --- src/compas_fab/planning/action.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 12e1ab72d8..43c5f5c313 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -24,6 +24,8 @@ "OpenGripper", "CloseGripper", "ManuallyMoveWorkpiece", + "HideWorkpieces", + "ShowWorkpieces", ] From 1e120e9004550d3c594ffbf9b783be2d2bb1d0c9 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 22 Nov 2023 14:45:49 +0800 Subject: [PATCH 17/28] Add doc to Action Class about check preconditions and apply effects --- src/compas_fab/planning/action.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 43c5f5c313..e582d9bce0 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -35,12 +35,17 @@ class Action(Data): The Action Classes are intended to be used to model the actions of a human operator or a robot. Current Implementation assumes discrete actions that are executed sequentially, aka. one after another. - Actions can be created automatically or manually. They are stored in a :class:`compas_fab.planning.Processs`. + Actions can be created automatically or manually. They are stored in a :class:`compas_fab.planning.FabricationProcess`. Some actions contain references to workpieces, tools, or other objects in the scene. Those objects are - stored in dictionaries under the Process object and are identified by their id. + stored in dictionaries under the FabricationProcess object and are identified by their id. Actions are closely related to the :class:`compas_fab.planning.SceneState` because each action changes the scene state. - Provided with an starting scene state, the actions can be *applied to* create the ending scene state. + The actions can be *applied* to a starting scene state to create the ending scene state. This is done by + calling the :meth:`compas_fab.planning.Action.apply_effects` method. It is also possible to check if an action can be applied + to a scene state by calling the :meth:`compas_fab.planning.Action.check_preconditions` method. Users that implement their own + actions must override these methods for the automatic planning function in the Robot class. See + :ref:`chained_planning_process` for more details. + The only exception are robotic configurations that are changed by :class:`compas_fab.planning.RoboticAction` actions. Those actions changes the robot configuration and their effect is not known until after the planning process. From f0ceebeeebd91d2f4dbd9e66ab07dcc793c0eef8 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 22 Nov 2023 14:47:11 +0800 Subject: [PATCH 18/28] update test --- tests/planning/test_action.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/planning/test_action.py b/tests/planning/test_action.py index 38193bbcc8..c02c2d639a 100644 --- a/tests/planning/test_action.py +++ b/tests/planning/test_action.py @@ -15,7 +15,7 @@ from compas_fab.planning import Action from compas_fab.planning import RoboticAction -from compas_fab.planning import CartesianMotion +from compas_fab.planning import LinearMotion from compas_fab.planning import FreeMotion from compas_fab.planning import OpenGripper from compas_fab.planning import CloseGripper From 97bbbb4ef5183df5c2c03fb2f1988715ac067811 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 7 Dec 2023 04:37:58 +0100 Subject: [PATCH 19/28] Apply suggestions (docstrings) from code review Co-authored-by: Gonzalo Casas --- src/compas_fab/planning/__init__.py | 14 +++++++------- src/compas_fab/planning/state.py | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index df5ac9d51b..116c85cafe 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -12,7 +12,7 @@ or by the operator (manually). Concurrent actions are not supported. The FabricationProcess class and its subclasses (such as :class:`PickAndPlaceProcess` and -:class:`ExtrusionProcess`) are used to model a process. They provider helper methods for +:class:`ExtrusionProcess`) are used to model a process. They provide helper methods for creating a ordered list of actions that are used for planning and execution. The beining of the process is defined by the initial state of the scene, which is a container for the state of all objects in the scene (see :class:`SceneState`). The initial state is used to plan the first action @@ -23,12 +23,12 @@ Actions -------- -Action classes are abstractions of the robot's (and, or the operator's) capability to manupulate tools and -workpieces in the scene. Action classes are used for modelling a process for the following purpose: +Action classes are abstractions of the robot's (and, or the operator's) capability to manipulate tools and +workpieces in the scene. Action classes are used for modeling a process for the following purpose: * To plan trajectories for robotic motions * To simulate and visualize the process in a virtual environment -* To execute the process on a real robot (or by an operator) +* To execute the process on a real robot or a human-robot collaboration process .. autosummary:: :toctree: generated/ @@ -36,16 +36,16 @@ Action RoboticAction - LinearMovement + LinearMotion FreeMotion OpenGripper CloseGripper - ManuallyMoveWorkpiece + ManualWorkpieceMotion States ------ -State classes are used to model the static state of objects in the planning scene. This include: +State classes are used to model the immutable, static state of objects in the planning scene. These include: :class:`RobotState` for :class:`compas_fab.robots.Robot`, :class:`ToolState` for :class:`compas_fab.robots.Tool` and :class:`WorkpieceState` for :class:`compas_fab.robots.Workpiece`. diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index fae50e8ce8..91fa6203c4 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -26,7 +26,7 @@ class SceneState(Data): It is not possible to attach a workpiece to the robot directly. Use a dummy tool (with identity transformation and no visual or collision meshes) if necessary. - SceneState provides convinence functions such as :meth:`SceneState.get_attached_tool_id()` + SceneState provides convenience functions such as :meth:`SceneState.get_attached_tool_id()` and :meth:`SceneState.get_attached_workpiece_id()` for identifying which tool and workpiece are currently attached to the robot. From 11dfe5cd4e92d86c9f24c91d37d9d256c26dde95 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 7 Dec 2023 11:40:05 +0800 Subject: [PATCH 20/28] Rename ManuallyMoveWorkpiece to ManualWorkpieceMotion --- CHANGELOG.md | 2 +- src/compas_fab/planning/__init__.py | 4 ++-- src/compas_fab/planning/action.py | 10 +++++----- tests/planning/test_action.py | 12 ++++++------ 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index bb7c0434e3..c54d07bea6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added * Added `compas_fab.planning` that contains `Action` classes and `SceneState` -* Added `RoboticAction`, `LinearMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, and `ManuallyMoveWorkpiece` actions. +* Added `RoboticAction`, `LinearMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, and `ManualWorkpieceMotion` actions. * Added `SceneState` as container for `WorkpieceState`, `ToolState` and `RobotState`. ### Changed diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index 116c85cafe..79d37cdebd 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -76,7 +76,7 @@ FreeMotion, OpenGripper, CloseGripper, - ManuallyMoveWorkpiece, + ManualWorkpieceMotion, HideWorkpieces, ShowWorkpieces, ) @@ -95,7 +95,7 @@ "FreeMotion", "OpenGripper", "CloseGripper", - "ManuallyMoveWorkpiece", + "ManualWorkpieceMotion", "HideWorkpieces", "ShowWorkpieces", "SceneState", diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index e582d9bce0..32c7a9539d 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -23,7 +23,7 @@ "FreeMotion", "OpenGripper", "CloseGripper", - "ManuallyMoveWorkpiece", + "ManualWorkpieceMotion", "HideWorkpieces", "ShowWorkpieces", ] @@ -531,7 +531,7 @@ def apply_effects(self, scene_state, debug=False): print("- Workpiece %s attached to tool." % self.attaching_workpiece_id) -class ManuallyMoveWorkpiece(Action): +class ManualWorkpieceMotion(Action): """Operator action to move a workpiece from one place to another. This moves the workpiece to a specific frame. Typically used for loading a workpiece into a gripper. @@ -547,20 +547,20 @@ class ManuallyMoveWorkpiece(Action): """ def __init__(self, workpiece_id=None, frame=Frame.worldXY()): - super(ManuallyMoveWorkpiece, self).__init__() + super(ManualWorkpieceMotion, self).__init__() self.workpiece_id = workpiece_id # type: str self.frame = frame # type: Frame @property def data(self): - data = super(ManuallyMoveWorkpiece, self).data + data = super(ManualWorkpieceMotion, self).data data["workpiece_id"] = self.workpiece_id data["frame"] = self.frame return data @data.setter def data(self, data): - super(ManuallyMoveWorkpiece, type(self)).data.fset(self, data) + super(ManualWorkpieceMotion, type(self)).data.fset(self, data) self.workpiece_id = data.get("workpiece_id", self.workpiece_id) self.frame = data.get("frame", self.frame) diff --git a/tests/planning/test_action.py b/tests/planning/test_action.py index c02c2d639a..7b720924ab 100644 --- a/tests/planning/test_action.py +++ b/tests/planning/test_action.py @@ -19,7 +19,7 @@ from compas_fab.planning import FreeMotion from compas_fab.planning import OpenGripper from compas_fab.planning import CloseGripper -from compas_fab.planning import ManuallyMoveWorkpiece +from compas_fab.planning import ManualWorkpieceMotion from compas_fab.planning import SceneState from compas_fab.planning import WorkpieceState @@ -94,7 +94,7 @@ def test_action_members(): assert isinstance(FreeMotion(), Action) assert isinstance(OpenGripper(), Action) assert isinstance(CloseGripper(), Action) - assert isinstance(ManuallyMoveWorkpiece(), Action) + assert isinstance(ManualWorkpieceMotion(), Action) # Test child class of RoboticAction assert isinstance(LinearMotion(), RoboticAction) assert isinstance(FreeMotion(), RoboticAction) @@ -231,7 +231,7 @@ def test_close_gripper_preconditions(start_scene_state): def test_load_workpiece_preconditions(start_scene_state): - action = ManuallyMoveWorkpiece("w1", None) + action = ManualWorkpieceMotion("w1", None) # Default state object has one object attached to the robot assert start_scene_state.get_attached_tool_id() == "t1" assert start_scene_state.get_attached_workpiece_id() == "w1" @@ -239,18 +239,18 @@ def test_load_workpiece_preconditions(start_scene_state): # Detach the workpiece from the robot start_scene_state.workpiece_states["w1"].attached_to_robot = False - # ManuallyMoveWorkpiece action requires that the workpiece is not attached to the robot + # ManualWorkpieceMotion action requires that the workpiece is not attached to the robot assert action.check_preconditions(start_scene_state)[0] is True # Check non-existing workpiece - action = ManuallyMoveWorkpiece("w3", None) + action = ManualWorkpieceMotion("w3", None) assert action.check_preconditions(start_scene_state)[0] is False def test_load_workpiece_effect(start_scene_state): workpiece_frame = Frame(Point(100.0, 200.0, 300.0), Vector(1.0, 0.0, 0.0), Vector(0.0, 1.0, 0.0)) assert start_scene_state.get_attached_workpiece_id() == "w1" - action = ManuallyMoveWorkpiece("w2", workpiece_frame) + action = ManualWorkpieceMotion("w2", workpiece_frame) action.apply_effects(start_scene_state) # Workpiece w2 should remain unattached but moved From 77b61e3110a40b9bacff4f2f1dfc5554e6ed8dc8 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 7 Dec 2023 11:42:19 +0800 Subject: [PATCH 21/28] Fixed Missing autosummary classes --- src/compas_fab/planning/__init__.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/compas_fab/planning/__init__.py b/src/compas_fab/planning/__init__.py index 79d37cdebd..b24420c613 100644 --- a/src/compas_fab/planning/__init__.py +++ b/src/compas_fab/planning/__init__.py @@ -41,6 +41,8 @@ OpenGripper CloseGripper ManualWorkpieceMotion + HideWorkpieces + ShowWorkpieces States ------ From 3aab57b2ae997f8b6b77020387aed8906142e471 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 7 Dec 2023 13:22:29 +0800 Subject: [PATCH 22/28] fix multiple import per line --- src/compas_fab/planning/state.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index 91fa6203c4..b38713fab9 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -1,5 +1,6 @@ from compas.data import Data -from compas.geometry import Frame, Transformation # noqa F401 +from compas.geometry import Frame # noqa F401 +from compas.geometry import Transformation # noqa F401 from compas.robots import Configuration # noqa F401 try: From af5788d348f6f3721ca2862a959ec3d452b5ef56 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Thu, 7 Dec 2023 13:42:05 +0800 Subject: [PATCH 23/28] Better Doc String for States, Test adding an example to WorkpieceState --- src/compas_fab/planning/state.py | 49 +++++++++++++++++++++++++------- 1 file changed, 39 insertions(+), 10 deletions(-) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index b38713fab9..c963de2fdd 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -143,7 +143,8 @@ def get_attached_workpiece_id(self): class WorkpieceState(Data): """Class for describing the state of a workpiece. - WorkpieceState objects are typically created by the constructor of :class:`SceneState`. + WorkpieceState objects are typically created automatically by the :class:`FabricationProcess`. + However it is possible to customize the initial state of the process by creating or modifying the WorkpieceStates manually. Attributes ---------- @@ -154,15 +155,45 @@ class WorkpieceState(Data): (default: :class:`compas.geometry.Frame.worldXY`) attached_to_robot : bool If the workpiece is attached to the robot, `True`. Else, `False`. + (default: `False`) attached_to_robot_grasp : :class:`compas.geometry.Transformation`, optional The grasp transformation of the workpiece if it is attached to the robot. The grasp is defined as the transformation that can transform the robot flange frame into the workpiece frame. - If not specified, defaults to the identity transformation. - If the workpiece is not attached to the robot, `None`. + If the workpiece is not attached to the robot, `None`. (default: `None`) is_hidden : bool If the workpiece is hidden, `True`. Else, `False`. (default: `False`) A hidden workpiece will not be included for collision detection of the scene. + + Example + ------- + + Example of a workpiece state that is not attached to robot: + + >>> from compas.geometry import Frame + >>> from compas_fab.robots import WorkpieceState + >>> workpiece_state = WorkpieceState( + >>> workpiece_id="wbeam1", + >>> frame=Frame.worldXY() + >>> ) + + Example of a workpiece state that is attached to robot: + + >>> from compas.geometry import Frame + >>> from compas.geometry import Transformation + >>> from compas_fab.robots import WorkpieceState + >>> # In this example the workpiece is located at the pickup location + >>> # the pickup frame of the workpiece is defined in world coordinates + >>> beam_pickup_frame = Frame([200, 200, 500], [1, 0, 0], [0, 1, 0]) + >>> # The grasp transformation of the workpiece is relative to the robot flange + >>> grasp = Transformation.from_frame(Frame([0, 0, 100], [1, 0, 0], [0, 1, 0])) + >>> workpiece_state = WorkpieceState( + >>> workpiece_id="wbeam2", + >>> frame=beam_pickup_frame, + >>> attached_to_robot=True, + >>> attached_to_robot_grasp=grasp, + >>> ) + """ def __init__( @@ -170,7 +201,7 @@ def __init__( workpiece_id="undefined_workpiece", frame=None, attached_to_robot=False, - attached_to_robot_grasp=Transformation(), + attached_to_robot_grasp=None, is_hidden=False, ): super(WorkpieceState, self).__init__() @@ -202,8 +233,8 @@ def data(self, data): class ToolState(Data): """Class for describing the state of a tool. - ToolState objects are typically created by the constructor of :class:`SceneState`. - + ToolState objects are typically created automatically by the :class:`FabricationProcess`. + However it is possible to customize the initial state of the process by creating or modifying the ToolStates manually. Attributes ---------- @@ -250,10 +281,8 @@ def data(self, data): class RobotState(Data): """Class for describing the state of a robot. - RobotState objects are typically created by the constructor of :class:`SceneState`. - However it is possible to create a RobotState object manually. For example, when specifying - the initial state of a robot in a planning process. - + RobotState objects are typically created automatically by the :class:`FabricationProcess`. + However it is possible to customize the initial state of the robot by creating or modifying the RobotState manually. Attributes ---------- From ecc8cea033648e39eade9ad481369996078122dd Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Mon, 11 Dec 2023 14:06:24 +0800 Subject: [PATCH 24/28] Fix import WorkpieceState error in example oode --- src/compas_fab/planning/state.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index c963de2fdd..848d5e8626 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -171,7 +171,7 @@ class WorkpieceState(Data): Example of a workpiece state that is not attached to robot: >>> from compas.geometry import Frame - >>> from compas_fab.robots import WorkpieceState + >>> from compas_fab.planning import WorkpieceState >>> workpiece_state = WorkpieceState( >>> workpiece_id="wbeam1", >>> frame=Frame.worldXY() @@ -181,7 +181,7 @@ class WorkpieceState(Data): >>> from compas.geometry import Frame >>> from compas.geometry import Transformation - >>> from compas_fab.robots import WorkpieceState + >>> from compas_fab.planning import WorkpieceState >>> # In this example the workpiece is located at the pickup location >>> # the pickup frame of the workpiece is defined in world coordinates >>> beam_pickup_frame = Frame([200, 200, 500], [1, 0, 0], [0, 1, 0]) From 8c83e2ea8066a6ba470521dee3d146fe5aff4ce7 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Mon, 11 Dec 2023 14:21:11 +0800 Subject: [PATCH 25/28] Try something else with the example code --- src/compas_fab/planning/state.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py index 848d5e8626..efbdf88096 100644 --- a/src/compas_fab/planning/state.py +++ b/src/compas_fab/planning/state.py @@ -173,9 +173,9 @@ class WorkpieceState(Data): >>> from compas.geometry import Frame >>> from compas_fab.planning import WorkpieceState >>> workpiece_state = WorkpieceState( - >>> workpiece_id="wbeam1", - >>> frame=Frame.worldXY() - >>> ) + ... workpiece_id="wbeam1", + ... frame=Frame.worldXY() + ... ) Example of a workpiece state that is attached to robot: @@ -188,11 +188,11 @@ class WorkpieceState(Data): >>> # The grasp transformation of the workpiece is relative to the robot flange >>> grasp = Transformation.from_frame(Frame([0, 0, 100], [1, 0, 0], [0, 1, 0])) >>> workpiece_state = WorkpieceState( - >>> workpiece_id="wbeam2", - >>> frame=beam_pickup_frame, - >>> attached_to_robot=True, - >>> attached_to_robot_grasp=grasp, - >>> ) + ... workpiece_id="wbeam2", + ... frame=beam_pickup_frame, + ... attached_to_robot=True, + ... attached_to_robot_grasp=grasp, + ... ) """ From 38e6de07ed0a14722ec0b24232397e88fcae6879 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Mon, 11 Dec 2023 07:24:42 +0100 Subject: [PATCH 26/28] Update src/compas_fab/planning/action.py Co-authored-by: Gonzalo Casas --- src/compas_fab/planning/action.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 32c7a9539d..0016ccbd66 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -534,7 +534,7 @@ def apply_effects(self, scene_state, debug=False): class ManualWorkpieceMotion(Action): """Operator action to move a workpiece from one place to another. This moves the workpiece to a specific frame. - Typically used for loading a workpiece into a gripper. + Typically used for attaching a workpiece into a gripper. Can also be used to model the manual attachment of scaffolding (modeled as a :class:`Workpiece`) Attributes From b5da5724bd2cc22ce82994488b3d0c93b75cc6a6 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 13 Dec 2023 15:48:12 +0800 Subject: [PATCH 27/28] Added Planning Group as attribute. Remove planned results. Moved intermediate_planning_waypoint to FreeMotion RobotAction.apply_effects does not take the robot config from ActionPlanResult yet, That will come in next PR --- CHANGELOG.md | 2 +- src/compas_fab/planning/action.py | 60 +++++++++++++++---------------- 2 files changed, 29 insertions(+), 33 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c54d07bea6..a01dde1dcd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added * Added `compas_fab.planning` that contains `Action` classes and `SceneState` -* Added `RoboticAction`, `LinearMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, and `ManualWorkpieceMotion` actions. +* Added `RoboticAction`, `LinearMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, `ManualWorkpieceMotion`, `HideWorkpieces` and `ShowWorkpieces`. * Added `SceneState` as container for `WorkpieceState`, `ToolState` and `RobotState`. ### Changed diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 0016ccbd66..30e037b430 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -106,8 +106,16 @@ def apply_effects(self, scene_state, debug=False): # type: (SceneState, bool) -> None """Applies the action to a scene state. - This method is called by the assembly process to apply the action to the scene state. - The SceneState object is modified in place. + This method is called by the assembly process to apply the action to the given scene state. + The SceneState object is modified in place to represent the state after the action is taken. + + If the action is a :class:`compas_fab.planning.RoboticAction`, the parameter `action_plan_result` + can be provided to update the robot configuration to the last configuration of the planned trajectory. + Otherwise, the robot configuration is not changed. + + See Also + -------- + :meth:`compas_fab.planning.RoboticAction.apply_effects` """ raise NotImplementedError("Action.apply_effects() is not implemented by %s." % type(self)) @@ -132,8 +140,8 @@ class RoboticAction(Action): of the robot is updated to the last configuration of the trajectory. See :meth:`compas_fab.planning.RoboticAction.apply_effects` for more details. - Attributes (Before Planning) - ---------------------------- + Attributes + ---------- target_robot_flange_frame : :class:`compas.geometry.Frame` The robot flange frame of the robot at target. In world coordinate frame. allowed_collision_pairs : list(tuple(str,str)) @@ -149,20 +157,12 @@ class RoboticAction(Action): when planning neighbouring robotic movements. This can be used to improve repeatibility. Users must be careful to specify a trajectory that is collision free and does not violate the kinematic limits of the robot. + planning_group : str, optional + The planning group to be used for planning. If None, the default planning group of the robot is used. + It is safe to leave this as None (default), unless the robot has multiple planning groups. tag : str A human readable tag that describes the action. It can printed out during visualization, debugging, execution and logging. - - Attributes (After Planning) - --------------------------- - planned_trajectory : :class:`compas_fab.robots.JointTrajectory` - The planned trajectory of the robotic movement. Available after planning. The first and last - trajectory points corresponds to the starting and ending configuration of the robotic movement. - planner_seed : int - The random seed used by the planner to generate the trajectory. Used by some planners. - - Attributes (For Execution) - -------------------------- speed_data_id : str The id (or name) of the speed data to be used for execution. ---------- @@ -173,16 +173,12 @@ def __init__(self): super(RoboticAction, self).__init__() self.tag = "Generic Action" - # Before Planning + # For Planning self.target_robot_flange_frame = None # type: Frame self.allowed_collision_pairs = [] # type: list(tuple(str,str)) self.fixed_target_configuration = None # type: Optional[Configuration] self.fixed_trajectory = None # type: Optional[JointTrajectory] - self.intermediate_planning_waypoint = [] # type: list(Configuration) - - # After Planning - self.planned_trajectory = None # type: Optional[JointTrajectory] - self.planner_seed = None # type: Optional[int] + self.planning_group = None # type: Optional[str] # For Execution self.speed_data_id = None # type: Optional[str] @@ -195,9 +191,7 @@ def data(self): data["allowed_collision_pairs"] = self.allowed_collision_pairs data["fixed_target_configuration"] = self.fixed_target_configuration data["fixed_trajectory"] = self.fixed_trajectory - data["intermediate_planning_waypoint"] = self.intermediate_planning_waypoint - data["planned_trajectory"] = self.planned_trajectory - data["planner_seed"] = self.planner_seed + data["planning_group"] = self.planning_group data["speed_data_id"] = self.speed_data_id return data @@ -209,11 +203,7 @@ def data(self, data): self.allowed_collision_pairs = data.get("allowed_collision_pairs", self.allowed_collision_pairs) self.fixed_target_configuration = data.get("fixed_target_configuration", self.fixed_target_configuration) self.fixed_trajectory = data.get("fixed_trajectory", self.fixed_trajectory) - self.intermediate_planning_waypoint = data.get( - "intermediate_planning_waypoint", self.intermediate_planning_waypoint - ) - self.planned_trajectory = data.get("planned_trajectory", self.planned_trajectory) - self.planner_seed = data.get("planner_seed", self.planner_seed) + self.planning_group = data.get("planning_group", self.planning_group) self.speed_data_id = data.get("speed_data_id", self.speed_data_id) def apply_effects(self, scene_state, debug=False): @@ -228,11 +218,17 @@ def apply_effects(self, scene_state, debug=False): ---------- scene_state : :class:`compas_fab.planning.SceneState` The scene state to apply the action to. + action_plan_result : :class:`compas_fab.robots.ActionPlanResult`, optional + + Todo + ---- + ActionPlanResult input is not yet supported. This function needs to support + updating the config after planning by accepting an ActionPlanResult object. """ robot_state = scene_state.get_robot_state() - if self.planned_trajectory is not None: - robot_state.configuration = self.planned_trajectory.points[-1] - elif self.fixed_trajectory is not None: + # if action_plan_result is not None: + # robot_state.configuration = action_plan_result.planned_trajectory[-1] + if self.fixed_trajectory is not None: robot_state.configuration = self.fixed_trajectory.points[-1] elif self.fixed_target_configuration is not None: robot_state.configuration = self.fixed_target_configuration From 1ff8d204174f798c0130ff5182175cfde3a0d396 Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Wed, 13 Dec 2023 15:57:57 +0800 Subject: [PATCH 28/28] docstring typo --- src/compas_fab/planning/action.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/compas_fab/planning/action.py b/src/compas_fab/planning/action.py index 30e037b430..cfa99d25dd 100644 --- a/src/compas_fab/planning/action.py +++ b/src/compas_fab/planning/action.py @@ -165,7 +165,6 @@ class RoboticAction(Action): It can printed out during visualization, debugging, execution and logging. speed_data_id : str The id (or name) of the speed data to be used for execution. - ---------- """