added thesis tasks

Author: wmcclinton

predicators/envs/spot_env.py

@@ -84,6 +84,10 @@ def _move_to_hand_view_object_sampler(state: State, goal: Set[GroundAtom],
     robot_obj = objs[0]
     obj_to_nav_to = objs[1]
 
+    if obj_to_nav_to.name == "blue_toy_chair":
+        min_dist = 1.8
+        max_dist = 1.9
+
     min_angle, max_angle = _get_approach_angle_bounds(obj_to_nav_to, state)
 
     return _move_offset_sampler(state, robot_obj, obj_to_nav_to, rng, min_dist,
@@ -103,6 +107,12 @@ def _move_to_reach_object_sampler(state: State, goal: Set[GroundAtom],
     robot_obj = objs[0]
     obj_to_nav_to = objs[1]
 
+    if obj_to_nav_to.name == "wooden_table":
+        # For the table, we want to be a bit farther so we can reach over it.
+        # import ipdb; ipdb.set_trace()
+        min_dist = 0.8
+        max_dist = 0.9
+
     min_angle, max_angle = _get_approach_angle_bounds(obj_to_nav_to, state)
     ret_val = _move_offset_sampler(state, robot_obj, obj_to_nav_to, rng,
                                    min_dist, max_dist, min_angle, max_angle)
@@ -114,6 +124,13 @@ def _get_approach_angle_bounds(obj: Object,
     """Helper for move samplers."""
     angle_bounds = load_spot_metadata().get("approach_angle_bounds", {})
     if obj.name in angle_bounds:
+        if obj.name == "blue_toy_chair":
+            # if location of chair is far from the table
+            # we cant approach it from the side
+            # 1.45993    0.6244    -0.0938282
+            # 0.082429   0.991738  -0.178306
+            if state.get(obj, "x") > 1.0:
+                return [-1.6, -1.53]
         return angle_bounds[obj.name]
     # Mega-hack for when the container is next to something with angle bounds,
     # i.e., it is ready to sweep.
@@ -230,7 +247,7 @@ def _drag_to_unblock_object_sampler(state: State, goal: Set[GroundAtom],
                                     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.0, 0.0, np.pi / 1.5])
+    return np.array([0.0, 0.0, -np.pi / 2])
 
 def _drag_to_open_object_sampler(state: State, goal: Set[GroundAtom],
                                     rng: np.random.Generator,
@@ -252,14 +269,15 @@ def _drag_to_block_object_sampler(state: State, goal: Set[GroundAtom],
                                   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.0, 0.0, -np.pi / 1.5])
+    return np.array([0.0, 0.0, np.pi / 2])
 
 
 def _sweep_into_container_sampler(state: State, goal: Set[GroundAtom],
                                   rng: np.random.Generator,
                                   objs: Sequence[Object]) -> Array:
     # Parameters are just one number, a velocity.
     del goal
+    # TODO # return np.array([2.0])
     if CFG.spot_use_perfect_samplers:
         if CFG.spot_run_dry:
             if len(objs) == 6:  # SweepTwoObjectsIntoContainer
@@ -295,6 +313,25 @@ def _prepare_sweeping_sampler(state: State, goal: Set[GroundAtom],
     return np.array([param_dict["dx"], param_dict["dy"], param_dict["angle"]])
 
 
+def _wipe_table_sampler(state: State, goal: Set[GroundAtom],
+                        rng: np.random.Generator,
+                        objs: Sequence[Object]) -> Array:
+    # Parameters are stroke_dx, stroke_dy, num_strokes, duration.
+    del state, goal, objs  # not used for now
+    if CFG.spot_use_perfect_samplers:
+        # Default values for wiping
+        stroke_dx = 0.0
+        stroke_dy = 0.4
+        num_strokes = 5
+        duration = 1.0
+    else:
+        stroke_dx = rng.uniform(-0.1, 0.1)
+        stroke_dy = rng.uniform(0.3, 0.5)
+        num_strokes = rng.integers(3, 8)
+        duration = rng.uniform(0.8, 1.5)
+    return np.array([stroke_dx, stroke_dy, num_strokes, duration])
+
+
 class SpotEnvsGroundTruthNSRTFactory(GroundTruthNSRTFactory):
     """Ground-truth NSRTs for the Spot Env."""
 
@@ -305,7 +342,8 @@ def get_env_names(cls) -> Set[str]:
             "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", "lis_spot_block_drawer_env",
-            "lis_spot_collect_misplaced_items_env"
+            "lis_spot_collect_misplaced_items_env", "lis_spot_balls_yellow_table_env",
+            "lis_spot_bear_panda_bucket_sweep_env", "lis_spot_wipe_table_env"
         }
 
     @staticmethod
@@ -339,6 +377,7 @@ def get_nsrts(env_name: str, types: Dict[str, Type],
             "PrepareContainerForSweeping": _prepare_sweeping_sampler,
             "DropNotPlaceableObject": utils.null_sampler,
             "MoveToReadySweep": utils.null_sampler,
+            "WipeTable": _wipe_table_sampler,
         }
 
         # If we're doing proper bilevel planning with a simulator, then

predicators/ground_truth_models/spot_env/nsrts.py

@@ -34,6 +34,7 @@
 from predicators.spot_utils.skills.spot_place import place_at_relative_position
 from predicators.spot_utils.skills.spot_stow_arm import stow_arm
 from predicators.spot_utils.skills.spot_sweep import sweep
+from predicators.spot_utils.skills.spot_wipe_table import wipe_multiple_strokes
 from predicators.spot_utils.spot_localization import SpotLocalizer
 from predicators.spot_utils.utils import DEFAULT_HAND_DROP_OBJECT_POSE, \
     DEFAULT_HAND_LOOK_STRAIGHT_DOWN_POSE, DEFAULT_HAND_POST_DUMP_POSE, \
@@ -440,22 +441,22 @@ def _sweep_objects_into_container_policy(name: str, robot_obj_idx: int,
     ) * middle_bottom_surface_pose
     # Now, compute the actual pose the hand should start sweeping from by
     # clamping it between the surface poses.
-    start_x = np.clip(middle_bottom_surface_rel_pose.x, mean_x + 0.175,
+    start_x = np.clip(middle_bottom_surface_rel_pose.x, mean_x + 0.275,
                       upper_left_surface_rel_pose.x)
     start_y = np.clip(middle_bottom_surface_rel_pose.y, mean_y + 0.41,
                       upper_left_surface_rel_pose.y)
     # use absolute value so that we don't get messed up by noise in the
     # perception height estimate.
-    start_z = 0.14
+    start_z = 0.17
     pitch = math_helpers.Quat.from_pitch(np.pi / 2)
     yaw = math_helpers.Quat.from_yaw(np.pi / 4)
     rot = pitch * yaw
-    sweep_start_pose = math_helpers.SE3Pose(x=start_x,
-                                            y=start_y,
+    sweep_start_pose = math_helpers.SE3Pose(x=start_x + 1.0,
+                                            y=start_y + 0.4,
                                             z=start_z,
                                             rot=rot)
     sweep_move_dx = 0.0
-    sweep_move_dy = -0.8
+    sweep_move_dy = -1.0
     sweep_move_dz = 0.0
 
     # Execute the sweep. Note simulation fn and args not implemented yet.
@@ -496,13 +497,19 @@ def _pick_and_dump_policy(name: str, robot_obj_idx: int, target_obj_idx: int,
 
     def _fn() -> None:
         for action in actions:
-            assert isinstance(action.extra_info, (list, tuple))
-            _, _, action_fn, action_fn_args, _, _ = action.extra_info
-            action_fn(*action_fn_args)
+            if isinstance(action.extra_info, (list, tuple)):
+                _, _, action_fn, action_fn_args, _, _ = action.extra_info
+                action_fn(*action_fn_args)
+                continue
+            else:
+                action_fn = action.extra_info.real_world_fn
+                action_fn_args = action.extra_info.real_world_fn_args
+                action_fn(*action_fn_args)
+                continue
 
     # Note simulation fn and args not implemented yet.
-    action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None,
-                                            tuple())
+    action_extra_info = SpotActionExtraInfo(name, objects, _fn, tuple(), None, tuple())
+    
     return utils.create_spot_env_action(action_extra_info)
 
 
@@ -595,7 +602,7 @@ 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':
+    if objects[target_obj_idx ].name == 'green_handle' or 'chair' in objects[target_obj_idx].name:
         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)
 
@@ -892,7 +899,7 @@ def _prepare_container_for_sweeping_policy(state: State, memory: Dict,
     rot = math_helpers.Quat.from_pitch(np.pi / 2)
     place_rel_pose = math_helpers.SE3Pose(x=0.6,
                                           y=0.0,
-                                          z=container_z - 0.15,
+                                          z=container_z, # TODO - 0.15,
                                           rot=rot)
 
     # Push towards the target a little bit after placing.
@@ -917,7 +924,7 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
     name = "MoveToReadySweep"
 
     # Always approach from the same angle.
-    yaw = np.pi / 2.0
+    yaw = 0.0 #np.pi / 2.0
     # Make up new params.
     distance = 0.8
     params = np.array([distance, yaw])
@@ -931,6 +938,55 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
                                   state, memory, objects, params)
 
 
+def _wipe_table_policy(state: State, memory: Dict,
+                       objects: Sequence[Object],
+                       params: Array) -> Action:
+    del memory  # not used
+
+    robot, _, _ = get_robot()
+    name = "WipeTable"
+
+    robot_obj = objects[0]
+    surface_obj = objects[2]
+
+    robot_pose = utils.get_se3_pose_from_state(state, robot_obj)
+    surface_pose = utils.get_se3_pose_from_state(state, surface_obj)
+    surface_height = state.get(surface_obj, "height")
+    if surface_obj.name == "wooden_table":
+        surface_height -= 1.0
+
+    # Compute relative pose for wiping start position
+    surface_rel_pose = robot_pose.inverse() * surface_pose
+
+    # Extract parameters
+    stroke_dx, stroke_dy, num_strokes_float, duration = params
+    num_strokes = max(1, int(num_strokes_float))
+
+    # Define wipe start pose relative to robot
+    pitch = math_helpers.Quat.from_pitch(np.pi / 2)
+    wipe_start_pose = math_helpers.SE3Pose(
+        x=surface_rel_pose.x,
+        y=surface_rel_pose.y - 0.2,
+        z=surface_height + 0.05,
+        rot=pitch)
+
+    # End look pose after wiping
+    end_look_pose = math_helpers.SE3Pose(
+        x=surface_rel_pose.x - 0.1,
+        y=surface_rel_pose.y,
+        z=surface_height + 0.3,
+        rot=math_helpers.Quat.from_pitch(np.pi / 2.5))
+
+    # Delta between strokes
+    delta_between_strokes = (0.05, 0.0)
+
+    action_extra_info = SpotActionExtraInfo(
+        name, objects, wipe_multiple_strokes,
+        (robot, wipe_start_pose, end_look_pose, stroke_dx, stroke_dy,
+         delta_between_strokes, num_strokes, duration), None, ())
+    return utils.create_spot_env_action(action_extra_info)
+
+
 ###############################################################################
 #                       Parameterized option factory                          #
 ###############################################################################
@@ -963,6 +1019,7 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
     "PrepareContainerForSweeping": Box(-np.inf, np.inf, (3, )),  # dx, dy, dyaw
     "DropNotPlaceableObject": Box(0, 1, (0, )),  # empty
     "MoveToReadySweep": Box(0, 1, (0, )),  # empty
+    "WipeTable": Box(-np.inf, np.inf, (4, )),  # stroke_dx, stroke_dy, num_strokes, duration
 }
 
 # NOTE: the policies MUST be unique because they output actions with extra info
@@ -988,6 +1045,7 @@ def _move_to_ready_sweep_policy(state: State, memory: Dict,
     "PrepareContainerForSweeping": _prepare_container_for_sweeping_policy,
     "DropNotPlaceableObject": _drop_not_placeable_object_policy,
     "MoveToReadySweep": _move_to_ready_sweep_policy,
+    "WipeTable": _wipe_table_policy,
 }
 
 
@@ -1034,7 +1092,10 @@ def get_env_names(cls) -> Set[str]:
             "spot_brush_shelf_env",
             "lis_spot_block_floor_env",
             "lis_spot_block_drawer_env",
-            "lis_spot_collect_misplaced_items_env"
+            "lis_spot_collect_misplaced_items_env",
+            "lis_spot_balls_yellow_table_env",
+            "lis_spot_bear_panda_bucket_sweep_env",
+            "lis_spot_wipe_table_env"
         }
 
     @classmethod

predicators/ground_truth_models/spot_env/options.py

@@ -1,5 +1,6 @@
 """A perceiver specific to spot envs."""
 
+from datetime import datetime
 import logging
 import time
 from pathlib import Path
@@ -284,6 +285,11 @@ def _create_state(self) -> State:
         # Now finish the state.
         state = _PartialPerceptionState(percept_state.data,
                                         simulator_state=simulator_state)
+        
+        # Save state for debugging
+        now = datetime.now()
+        with open(CFG.spot_perception_outdir + f"/0_{now.strftime('%Y%m%d_%H%M%S')}_latest_perceived_state.txt", "w") as f:
+            f.write(state.pretty_str())
 
         return state
 
@@ -452,6 +458,28 @@ def _create_goal(self, state: State,
             return {
                 GroundAtom(On, [bucket, shelf]),
             }
+        if goal_description == "sweep the brown bear toy and panda toy into the bucket":
+            brown_bear = Object("brown_bear_toy", _movable_object_type)
+            panda = Object("panda_toy", _movable_object_type)
+            chick_toy = Object("chick_toy", _movable_object_type)
+            bucket = Object("bucket", _container_type)
+            Inside = pred_name_to_pred["Inside"]
+            # table = Object("wooden_table", _immovable_object_type)
+            # On = pred_name_to_pred["On"]
+            wooden_table = Object("wooden_table", _immovable_object_type)
+            NotBlocked = pred_name_to_pred["NotBlocked"]
+            blue_toy_chair = Object("blue_toy_chair", _movable_object_type)
+            Blocking = pred_name_to_pred["Blocking"]
+            NotInsideAnyContainer = pred_name_to_pred["NotInsideAnyContainer"]
+            return {
+                GroundAtom(Inside, [brown_bear, bucket]),
+                GroundAtom(Inside, [panda, bucket]),
+                # GroundAtom(Inside, [chick_toy, bucket]),
+                # GroundAtom(NotBlocked, [wooden_table]),
+                # GroundAtom(Blocking, [blue_toy_chair, wooden_table]),
+                #GroundAtom(NotInsideAnyContainer, [brown_bear]),
+                #GroundAtom(NotInsideAnyContainer, [panda])
+            }
         if goal_description == "pick up the brush":
             robot = Object("robot", _robot_type)
             brush = Object("brush", _movable_object_type)
@@ -492,6 +520,26 @@ def _create_goal(self, state: State,
                 GroundAtom(Inside, [yellow_cup, cardboard_box]),
                 GroundAtom(Inside, [toy_plane, cardboard_box]),
             }
+        if goal_description == "put the tennis ball and red ball on the yellow table":
+            tennis_ball = Object("tennis_ball", _movable_object_type)
+            red_ball = Object("red_ball", _movable_object_type)
+            yellow_table = Object("yellow_table", _immovable_object_type)
+            On = pred_name_to_pred["On"]
+            Inside = pred_name_to_pred["Inside"]
+            return {
+                GroundAtom(On, [tennis_ball, yellow_table]),
+                GroundAtom(On, [red_ball, yellow_table]),
+            }
+        if goal_description == "wipe the wooden table with the sponge":
+            wooden_table = Object("wooden_table", _immovable_object_type)
+            sponge = Object("sponge", _movable_object_type)
+            orange_bucket = Object("orange_bucket", _container_type)
+            SurfaceWiped = pred_name_to_pred["SurfaceWiped"]
+            Inside = pred_name_to_pred["Inside"]
+            return {
+                GroundAtom(SurfaceWiped, [wooden_table]),
+                GroundAtom(Inside, [sponge, orange_bucket]),
+            }
         if goal_description == "setup sweeping":
             robot = Object("robot", _robot_type)
             brush = Object("brush", _movable_object_type)

predicators/perception/spot_perceiver.py

@@ -322,11 +322,11 @@ def task_plan(
     in tests/test_planning for usage examples.
     """
     if not goal.issubset(reachable_atoms):
-        logging.info(f"Detected goal unreachable. Goal: {goal}")
-        logging.info(f"Initial atoms: {init_atoms}")
+        logging.info(f"Detected goal unreachable. Goal: {sorted(list(goal))}\n")
+        logging.info(f"Initial atoms: {sorted(list(init_atoms))}\n")
         logging.info(
-            f"Reachable atoms not in init: {reachable_atoms - init_atoms}")
-        raise PlanningFailure(f"Goal {goal} not dr-reachable")
+            f"Reachable atoms not in init: {sorted(list(reachable_atoms - init_atoms))}\n")
+        raise PlanningFailure(f"Goal {sorted(list(goal))} not dr-reachable\n")
     dummy_task = Task(DefaultState, goal)
     metrics: Metrics = defaultdict(float)
     generator = _skeleton_generator(