diff --git a/CHANGELOG.md b/CHANGELOG.md index 83f2c3ab2..a01dde1dc 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`, `LinearMotion`, `FreeMotion`, `OpenGripper`, `CloseGripper`, `ManualWorkpieceMotion`, `HideWorkpieces` and `ShowWorkpieces`. +* Added `SceneState` as container for `WorkpieceState`, `ToolState` and `RobotState`. + ### Changed * `CollisionMesh` now inherit from `compas.data.Data` diff --git a/docs/api/compas_fab.planning.rst b/docs/api/compas_fab.planning.rst new file mode 100644 index 000000000..8652f7626 --- /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 f0d7c0170..300cb2f31 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 new file mode 100644 index 000000000..b24420c61 --- /dev/null +++ b/src/compas_fab/planning/__init__.py @@ -0,0 +1,107 @@ +""" +******************************************************************************** +compas_fab.planning +******************************************************************************** + +.. currentmodule:: compas_fab.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 are not supported. + +The FabricationProcess class and its subclasses (such as :class:`PickAndPlaceProcess` and +: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 +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 +-------- + +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 a human-robot collaboration process + +.. autosummary:: + :toctree: generated/ + :nosignatures: + + Action + RoboticAction + LinearMotion + FreeMotion + OpenGripper + CloseGripper + ManualWorkpieceMotion + HideWorkpieces + ShowWorkpieces + +States +------ + +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`. +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 + + +""" + +from .action import ( + Action, + RoboticAction, + LinearMotion, + FreeMotion, + OpenGripper, + CloseGripper, + ManualWorkpieceMotion, + HideWorkpieces, + ShowWorkpieces, +) + +from .state import ( + SceneState, + WorkpieceState, + ToolState, + RobotState, +) + +__all__ = [ + "Action", + "RoboticAction", + "LinearMotion", + "FreeMotion", + "OpenGripper", + "CloseGripper", + "ManualWorkpieceMotion", + "HideWorkpieces", + "ShowWorkpieces", + "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 000000000..cfa99d25d --- /dev/null +++ b/src/compas_fab/planning/action.py @@ -0,0 +1,760 @@ +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 + +try: + from compas_fab.planning import SceneState # noqa: F401 +except ImportError: + from .state import SceneState # noqa: F401 + +try: + from typing import Optional # noqa: F401 + from typing import Tuple # noqa: F401 +except ImportError: + pass + +__all__ = [ + "Action", + "RoboticAction", + "LinearMotion", + "FreeMotion", + "OpenGripper", + "CloseGripper", + "ManualWorkpieceMotion", + "HideWorkpieces", + "ShowWorkpieces", +] + + +class Action(Data): + """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.FabricationProcess`. + Some actions contain references to workpieces, tools, or other objects in the scene. Those objects are + 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. + 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. + + 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: [str | int] + self.tag = "Generic Action" # type: str + + @property + def data(self): + data = {} + # For class inhereited from Action, use the following line + # data = super(Action, self).data + data["act_id"] = self.act_id + 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_id = data["act_id"] + self.tag = data["tag"] + + 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 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)) + + +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 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 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.RoboticAction.apply_effects` for more details. + + 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)) + List of pairs of collision objects that are allowed to collide. + Objects are identified by workpiece_id, tool_id, or static_object_id. + 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 + 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. + 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. + speed_data_id : str + The id (or name) of the speed data to be used for execution. + + """ + + def __init__(self): + super(RoboticAction, self).__init__() + self.tag = "Generic Action" + + # 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.planning_group = None # type: Optional[str] + + # For Execution + self.speed_data_id = None # type: Optional[str] + + @property + def data(self): + 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 + data["fixed_target_configuration"] = self.fixed_target_configuration + data["fixed_trajectory"] = self.fixed_trajectory + data["planning_group"] = self.planning_group + data["speed_data_id"] = self.speed_data_id + return data + + @data.setter + def data(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) + self.fixed_target_configuration = data.get("fixed_target_configuration", self.fixed_target_configuration) + self.fixed_trajectory = data.get("fixed_trajectory", self.fixed_trajectory) + 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): + # type: (SceneState, bool) -> None + """Applies the action to a scene 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. + + Parameters + ---------- + 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 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 + robot_state.frame = self.target_robot_flange_frame + 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) + attached_workpiece.frame = Frame.from_transformation( + Transformation.from_frame(robot_state.frame) * attached_workpiece.attached_to_robot_grasp + ) + if debug: + print("- Attached Workpiece %s Followed." % attached_workpiece_id) + + +class LinearMotion(RoboticAction): + """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). + + Typically, the orientation between the starting frame and the target frame is the same. + However some linear motion planners can also interpolate between different orientations, check + the documentation of the planner for more details. + + + Attributes + ---------- + 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.intermediate_targets = [] # type: Optional[list[Point]] + self.tag = "Linear Movement" + + @property + def data(self): + data = super(LinearMotion, self).data + data["intermediate_targets"] = self.intermediate_targets + return data + + @data.setter + def data(self, data): + super(LinearMotion, type(self)).data.fset(self, data) + self.intermediate_targets = data.get("intermediate_targets", self.intermediate_targets) + + +class FreeMotion(RoboticAction): + """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.RoboticAction` for other attributes. + """ + + def __init__(self): + super(FreeMotion, 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(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 + return data + + @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.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. + + 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. + """ + + 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 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. + + 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 + ---------- + scene_state : :class:`compas_fab.planning.SceneState` + The scene state to apply the action to. + """ + + # 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_robot = False + attached_workpiece.attached_to_robot_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. + attaching_workpiece_id : str, optional + The id of the workpiece attached to the gripper. + 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, 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] + self.attaching_workpiece_grasp = attaching_workpiece_grasp # type: Optional[Transformation] + + @property + def data(self): + data = super(CloseGripper, self).data + 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.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_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. + 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. + """ + # TODO: Change the configuration of the gripper to closed + + if debug: + print("Gripper closed.") + # Transform attached workpieces + 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 + 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.attaching_workpiece_id) + + +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 attaching a workpiece into a gripper. + Can also be used to model the manual attachment of scaffolding (modeled as a :class:`Workpiece`) + + Attributes + ---------- + workpiece_id : str + 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, workpiece_id=None, frame=Frame.worldXY()): + super(ManualWorkpieceMotion, self).__init__() + self.workpiece_id = workpiece_id # type: str + self.frame = frame # type: Frame + + @property + def data(self): + data = super(ManualWorkpieceMotion, self).data + data["workpiece_id"] = self.workpiece_id + data["frame"] = self.frame + return data + + @data.setter + def data(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) + + 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 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. + """ + # Transform attached objects + workpiece_state = scene_state.get_workpiece_state(self.workpiece_id) + 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) diff --git a/src/compas_fab/planning/state.py b/src/compas_fab/planning/state.py new file mode 100644 index 000000000..efbdf8809 --- /dev/null +++ b/src/compas_fab/planning/state.py @@ -0,0 +1,310 @@ +from compas.data import Data +from compas.geometry import Frame # noqa F401 +from compas.geometry import 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): + """Container for the states of all workpieces, tools and robot in a static scene. + + 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 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. + + When constructing a SceneState, all the workpieces and tools id should be provided + 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 + ---------- + 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) + + @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 + """Returns the state of the only robot in the scene.""" + 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` 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: + 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_robot: + return workpiece_id + return None + + +class WorkpieceState(Data): + """Class for describing the state of a workpiece. + + 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 + ---------- + workpiece_id : str + 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`) + 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 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.planning 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.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]) + >>> # 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__( + self, + workpiece_id="undefined_workpiece", + frame=None, + attached_to_robot=False, + attached_to_robot_grasp=None, + is_hidden=False, + ): + super(WorkpieceState, self).__init__() + self.workpiece_id = workpiece_id + 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_robot"] = self.attached_to_robot + data["attached_to_robot_grasp"] = self.attached_to_robot_grasp + data["is_hidden"] = self.is_hidden + 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_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) + + +class ToolState(Data): + """Class for describing the state of a tool. + + 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 + ---------- + 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 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", frame=None, attached_to_robot_grasp=None, configuration=None): + super(ToolState, self).__init__() + self.tool_id = tool_id + self.frame = frame or Frame.worldXY() # type: Frame + self.attached_to_robot = False # type: bool + self.attached_to_robot_grasp = attached_to_robot_grasp or Transformation() # type: Transformation + self.configuration = 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. + + 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 + ---------- + 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, frame=None, configuration=None): + super(RobotState, self).__init__() + self.frame = frame # type: Frame + self.configuration = 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) diff --git a/tests/planning/test_action.py b/tests/planning/test_action.py new file mode 100644 index 000000000..7b720924a --- /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 LinearMotion +from compas_fab.planning import FreeMotion +from compas_fab.planning import OpenGripper +from compas_fab.planning import CloseGripper +from compas_fab.planning import ManualWorkpieceMotion + +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(LinearMotion(), Action) + assert isinstance(FreeMotion(), Action) + assert isinstance(OpenGripper(), Action) + assert isinstance(CloseGripper(), Action) + assert isinstance(ManualWorkpieceMotion(), Action) + # Test child class of RoboticAction + assert isinstance(LinearMotion(), 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 = 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" + 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 + # 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 = 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 = ManualWorkpieceMotion("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"