open and close skill

Author: wmcclinton

predicators/envs/spot_env.py

@@ -1072,6 +1072,15 @@ def _holding_classifier(state: State, objects: Sequence[Object]) -> bool:
         return False
     return state.get(obj, "held") > 0.5
 
+def _open_classifier(state: State, objects: Sequence[Object]) -> bool:
+    obj = objects[0]
+    return False
+
+def _not_open_classifier(state: State, objects: Sequence[Object]) -> bool:
+    obj = objects[0]
+    if not obj.is_instance(_movable_object_type):
+        return True
+    return not _open_classifier(state, objects)
 
 def _not_holding_classifier(state: State, objects: Sequence[Object]) -> bool:
     _, obj = objects
@@ -1395,6 +1404,10 @@ def _get_sweeping_surface_for_container(container: Object,
     return None
 
 
+_Open = Predicate("Open", [_movable_object_type],
+                     _open_classifier)
+_NotOpen = Predicate("NotOpen", [_movable_object_type],
+                     _not_open_classifier)
 _NEq = Predicate("NEq", [_base_object_type, _base_object_type],
                  _neq_classifier)
 _On = Predicate("On", [_movable_object_type, _base_object_type],
@@ -1448,7 +1461,7 @@ def _get_sweeping_surface_for_container(container: Object,
     _HandEmpty, _Holding, _NotHolding, _InHandView, _InView, _Reachable,
     _Blocking, _NotBlocked, _ContainerReadyForSweeping, _IsPlaceable,
     _IsNotPlaceable, _IsSweeper, _HasFlatTopSurface, _RobotReadyForSweeping,
-    _IsSemanticallyGreaterThan
+    _IsSemanticallyGreaterThan, _Open, _NotOpen
 }
 _NONPERCEPT_PREDICATES: Set[Predicate] = set()
 
@@ -1685,6 +1698,44 @@ def _create_operators() -> Iterator[STRIPSOperator]:
     ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _Blocking}
     yield STRIPSOperator("DragToBlockObject", parameters, preconds, add_effs,
                          del_effs, ignore_effs)
+    
+    # DragToOpenObject
+    robot = Variable("?robot", _robot_type)
+    obj = Variable("?obj", _movable_object_type)
+    parameters = [robot, obj]
+    preconds = {
+        LiftedAtom(_Holding, [robot, obj]),
+    }
+    add_effs = {
+        LiftedAtom(_HandEmpty, [robot]),
+        LiftedAtom(_NotHolding, [robot, obj]),
+        LiftedAtom(_Open, [obj]),
+    }
+    del_effs = {
+        LiftedAtom(_Holding, [robot, obj]),
+    }
+    ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _Blocking}
+    yield STRIPSOperator("DragToOpenObject", parameters, preconds, add_effs,
+                         del_effs, ignore_effs)
+    
+    # DragToCloseObject
+    robot = Variable("?robot", _robot_type)
+    obj = Variable("?obj", _movable_object_type)
+    parameters = [robot, obj]
+    preconds = {
+        LiftedAtom(_Holding, [robot, obj]),
+    }
+    add_effs = {
+        LiftedAtom(_HandEmpty, [robot]),
+        LiftedAtom(_NotHolding, [robot, obj]),
+        LiftedAtom(_NotOpen, [obj]),
+    }
+    del_effs = {
+        LiftedAtom(_Holding, [robot, obj]),
+    }
+    ignore_effs = {_InHandView, _Reachable, _RobotReadyForSweeping, _Blocking}
+    yield STRIPSOperator("DragToCloseObject", parameters, preconds, add_effs,
+                         del_effs, ignore_effs)
 
     # MoveToReadySweep
     robot = Variable("?robot", _robot_type)
@@ -3062,10 +3113,10 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
 
         detection_id_to_obj: Dict[ObjectDetectionID, Object] = {}
 
-        red_block = Object("red_block", _movable_object_type)
-        red_block_detection = LanguageObjectDetectionID(
-            "red block/orange block/yellow block")
-        detection_id_to_obj[red_block_detection] = red_block
+        blue_block = Object("blue_block", _movable_object_type)
+        blue_block_detection = LanguageObjectDetectionID(
+            "blue block/blue-ish block/blue-green block")
+        detection_id_to_obj[blue_block_detection] = blue_block
 
         for obj, pose in get_known_immovable_objects().items():
             detection_id = KnownStaticObjectDetectionID(obj.name, pose)
@@ -3074,8 +3125,61 @@ def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
         return detection_id_to_obj
 
     def _generate_goal_description(self) -> GoalDescription:
-        return "pick up the red block"
+        return "pick up the blue block"
 
     def _get_dry_task(self, train_or_test: str,
                       task_idx: int) -> EnvironmentTask:
         raise NotImplementedError("Dry task generation not implemented.")
+    
+
+class LISSpotBlockDrawerEnv(SpotRearrangementEnv):
+    """An extremely basic environment where a block needs to be picked up and
+    is specifically used for testing in the LIS Spot room.
+
+    Very simple and mostly just for testing.
+    """
+
+    def __init__(self, use_gui: bool = True) -> None:
+        super().__init__(use_gui)
+
+        op_to_name = {o.name: o for o in _create_operators()}
+        op_names_to_keep = {
+            "MoveToReachObject",
+            "MoveToHandViewObject",
+            "PickObjectToDrag",
+            "DragToOpenObject",
+            "DragToCloseObject",
+        }
+        self._strips_operators = {op_to_name[o] for o in op_names_to_keep}
+
+    @classmethod
+    def get_name(cls) -> str:
+        return "lis_spot_block_drawer_env"
+
+    @property
+    def _detection_id_to_obj(self) -> Dict[ObjectDetectionID, Object]:
+
+        detection_id_to_obj: Dict[ObjectDetectionID, Object] = {}
+
+        blue_block = Object("blue_block", _movable_object_type)
+        blue_block_detection = LanguageObjectDetectionID(
+            "blue block/blue-ish block/blue-green block")
+        detection_id_to_obj[blue_block_detection] = blue_block
+
+        green_handle = Object("green_handle", _movable_object_type)
+        green_handle_detection = LanguageObjectDetectionID(
+            "green duct tape/green handle/green object")
+        detection_id_to_obj[green_handle_detection] = green_handle
+
+        for obj, pose in get_known_immovable_objects().items():
+            detection_id = KnownStaticObjectDetectionID(obj.name, pose)
+            detection_id_to_obj[detection_id] = obj
+
+        return detection_id_to_obj
+
+    def _generate_goal_description(self) -> GoalDescription:
+        return "open the drawer"
+
+    def _get_dry_task(self, train_or_test: str,
+                      task_idx: int) -> EnvironmentTask:
+        raise NotImplementedError("Dry task generation not implemented.")

predicators/ground_truth_models/spot_env/nsrts.py

@@ -121,6 +121,8 @@ def _get_approach_angle_bounds(obj: Object,
     if surface is not None and surface.name in angle_bounds:
         return angle_bounds[surface.name]
     # Default to all possible approach angles.
+    if obj.name == 'green_handle':
+        return (np.pi/2, np.pi/2)
     return (-np.pi, np.pi)
 
 
@@ -230,6 +232,20 @@ def _drag_to_unblock_object_sampler(state: State, goal: Set[GroundAtom],
     del state, goal, objs, rng  # randomization coming soon
     return np.array([0.0, 0.0, np.pi / 1.5])
 
+def _drag_to_open_object_sampler(state: State, goal: Set[GroundAtom],
+                                    rng: np.random.Generator,
+                                    objs: Sequence[Object]) -> Array:
+    # Parameters are relative dx, dy, dyaw to move while holding.
+    del state, goal, objs, rng  # randomization coming soon
+    return np.array([-0.5, 0.0, 0.0])
+
+def _drag_to_close_object_sampler(state: State, goal: Set[GroundAtom],
+                                    rng: np.random.Generator,
+                                    objs: Sequence[Object]) -> Array:
+    # Parameters are relative dx, dy, dyaw to move while holding.
+    del state, goal, objs, rng  # randomization coming soon
+    return np.array([0.5, 0.0, 0.0])
+
 
 def _drag_to_block_object_sampler(state: State, goal: Set[GroundAtom],
                                   rng: np.random.Generator,
@@ -288,7 +304,7 @@ def get_env_names(cls) -> Set[str]:
             "spot_cube_env", "spot_soda_floor_env", "spot_soda_table_env",
             "spot_soda_bucket_env", "spot_soda_chair_env",
             "spot_main_sweep_env", "spot_ball_and_cup_sticky_table_env",
-            "spot_brush_shelf_env", "lis_spot_block_floor_env"
+            "spot_brush_shelf_env", "lis_spot_block_floor_env", "lis_spot_block_drawer_env"
         }
 
     @staticmethod
@@ -314,6 +330,8 @@ def get_nsrts(env_name: str, types: Dict[str, Type],
             "DropObjectInside": _drop_object_inside_sampler,
             "DropObjectInsideContainerOnTop": _drop_object_inside_sampler,
             "DragToUnblockObject": _drag_to_unblock_object_sampler,
+            "DragToOpenObject": _drag_to_open_object_sampler,
+            "DragToCloseObject": _drag_to_close_object_sampler,
             "DragToBlockObject": _drag_to_block_object_sampler,
             "SweepIntoContainer": _sweep_into_container_sampler,
             "SweepTwoObjectsIntoContainer": _sweep_into_container_sampler,

predicators/ground_truth_models/spot_env/options.py

@@ -333,7 +333,8 @@ def _grasp_policy(name: str,
                   memory: Dict,
                   objects: Sequence[Object],
                   params: Array,
-                  do_dump: bool = False) -> Action:
+                  do_dump: bool = False,
+                  do_not_stow: bool = False) -> Action:
     del memory  # not used
 
     robot, _, _ = get_robot()
@@ -369,7 +370,8 @@ def _grasp_policy(name: str,
                          state.get(target_obj, "width"))
 
     do_stow = not do_dump and \
-              target_obj_volume < CFG.spot_grasp_stow_volume_threshold
+              target_obj_volume < CFG.spot_grasp_stow_volume_threshold and \
+              not do_not_stow
     fn = _grasp_at_pixel_and_maybe_stow_or_dump
     sim_fn = None  # NOTE: cannot simulate using this option, so this
     # shouldn't be called anyways...
@@ -593,6 +595,8 @@ def _pick_object_to_drag_policy(state: State, memory: Dict,
                                 params: Array) -> Action:
     name = "PickObjectToDrag"
     target_obj_idx = 1
+    if objects[target_obj_idx ].name == 'green_handle':
+        return _grasp_policy(name, target_obj_idx, state, memory, objects, params, do_not_stow=True)
     return _grasp_policy(name, target_obj_idx, state, memory, objects, params)
 
 
@@ -791,6 +795,35 @@ def _drag_to_unblock_object_policy(state: State, memory: Dict,
                                             tuple())
     return utils.create_spot_env_action(action_extra_info)
 
+def _drag_to_open_object_policy(state: State, memory: Dict,
+                                   objects: Sequence[Object],
+                                   params: Array) -> Action:
+    del state, memory  # not used
+
+    name = "DragToOpenObject"
+    robot, _, _ = get_robot()
+    dx, dy, dyaw = params
+    move_rel_pos = math_helpers.SE2Pose(dx, dy, angle=dyaw)
+    # Note that simulation fn and args not yet implemented.
+    action_extra_info = SpotActionExtraInfo(name, objects, _drag_and_release,
+                                            (robot, move_rel_pos), None,
+                                            tuple())
+    return utils.create_spot_env_action(action_extra_info)
+
+def _drag_to_close_object_policy(state: State, memory: Dict,
+                                   objects: Sequence[Object],
+                                   params: Array) -> Action:
+    del state, memory  # not used
+
+    name = "DragToCloseObject"
+    robot, _, _ = get_robot()
+    dx, dy, dyaw = params
+    move_rel_pos = math_helpers.SE2Pose(dx, dy, angle=dyaw)
+    # Note that simulation fn and args not yet implemented.
+    action_extra_info = SpotActionExtraInfo(name, objects, _drag_and_release,
+                                            (robot, move_rel_pos), None,
+                                            tuple())
+    return utils.create_spot_env_action(action_extra_info)
 
 def _drag_to_block_object_policy(state: State, memory: Dict,
                                  objects: Sequence[Object],
@@ -922,6 +955,8 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
     "DropObjectInsideContainerOnTop": Box(-np.inf, np.inf,
                                           (3, )),  # rel dx, dy, dz
     "DragToUnblockObject": Box(-np.inf, np.inf, (3, )),  # rel dx, dy, dyaw
+    "DragToOpenObject": Box(-np.inf, np.inf, (3, )),  # rel dx, dy, dyaw
+    "DragToCloseObject": Box(-np.inf, np.inf, (3, )),  # rel dx, dy, dyaw
     "DragToBlockObject": Box(-np.inf, np.inf, (3, )),  # rel dx, dy, dyaw
     "SweepIntoContainer": Box(-np.inf, np.inf, (1, )),  # velocity
     "SweepTwoObjectsIntoContainer": Box(-np.inf, np.inf, (1, )),  # same
@@ -945,6 +980,8 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
     "DropObjectInside": _drop_object_inside_policy,
     "DropObjectInsideContainerOnTop": _move_and_drop_object_inside_policy,
     "DragToUnblockObject": _drag_to_unblock_object_policy,
+    "DragToOpenObject": _drag_to_open_object_policy,
+    "DragToCloseObject": _drag_to_close_object_policy,
     "DragToBlockObject": _drag_to_block_object_policy,
     "SweepIntoContainer": _sweep_into_container_policy,
     "SweepTwoObjectsIntoContainer": _sweep_two_objects_into_container_policy,
@@ -996,6 +1033,7 @@ def get_env_names(cls) -> Set[str]:
             "spot_ball_and_cup_sticky_table_env",
             "spot_brush_shelf_env",
             "lis_spot_block_floor_env",
+            "lis_spot_block_drawer_env",
         }
 
     @classmethod

predicators/perception/spot_perceiver.py

@@ -464,6 +464,21 @@ def _create_goal(self, state: State,
             block = Object("red_block", _movable_object_type)
             Holding = pred_name_to_pred["Holding"]
             return {GroundAtom(Holding, [robot, block])}
+        if goal_description == "pick up the blue block":
+            robot = Object("robot", _robot_type)
+            block = Object("blue_block", _movable_object_type)
+            Holding = pred_name_to_pred["Holding"]
+            return {GroundAtom(Holding, [robot, block])}
+        if goal_description == "open the drawer":
+            robot = Object("robot", _robot_type)
+            handle = Object("green_handle", _movable_object_type)
+            Open = pred_name_to_pred["Open"]
+            return {GroundAtom(Open, [handle])}
+        if goal_description == "close the drawer":
+            robot = Object("robot", _robot_type)
+            handle = Object("green_handle", _movable_object_type)
+            NotOpen = pred_name_to_pred["NotOpen"]
+            return {GroundAtom(NotOpen, [handle])}
         if goal_description == "setup sweeping":
             robot = Object("robot", _robot_type)
             brush = Object("brush", _movable_object_type)

predicators/spot_utils/graph_nav_maps/b45-621/metadata.yaml

@@ -46,4 +46,18 @@ static-object-features:
     length: 0.1
     width: 0.1
     placeable: 1
+    is_sweeper: 0
+  blue_block:
+    shape: 2
+    height: 0.1
+    length: 0.1
+    width: 0.1
+    placeable: 1
+    is_sweeper: 0
+  green_handle:
+    shape: 2
+    height: 0.1
+    length: 0.1
+    width: 0.1
+    placeable: 0
     is_sweeper: 0