weird fd issues?

predicators/approaches/bilevel_planning_approach.py

@@ -65,6 +65,8 @@ def _solve(self, task: Task, timeout: int) -> Callable[[State], Action]:
             self._last_atoms_seq = atoms_seq
             policy = utils.nsrt_plan_to_greedy_policy(nsrt_plan, task.goal,
                                                       self._rng)
+            logging.debug("Current State:")
+            logging.debug(utils.abstract(task.init, preds))
             logging.debug("Current Task Plan:")
             for act in nsrt_plan:
                 logging.debug(act)

predicators/envs/sticky_table.py

@@ -189,12 +189,16 @@ def simulate(self, state: State, action: Action) -> State:
                         if self._table_is_sticky(table, state):
                             # Check if placing on the smooth side of the sticky table.
                             table_y = state.get(table, "y")
-                            if self.sticky_surface_mode == "half" and act_y < table_y:  #+ 0.3 * (state.get(table, "radius") - (state.get(cube, "size") / 2)):
+                            if self.sticky_surface_mode == "half" and act_y < table_y + 0.3 * (state.get(table, "radius") - (state.get(cube, "size") / 2)):
                                 if obj_being_held in [cube, cup]:
                                     fall_prob = self._place_smooth_fall_prob
                                 else:
                                     assert obj_being_held == ball
                                     fall_prob = 1.0
+
+                        if obj_being_held == cup and fall_prob != 1.0:
+                            import ipdb; ipdb.set_trace()
+
                         if self._noise_rng.uniform() < fall_prob:
                             fall_x, fall_y = self._sample_floor_point_around_table(
                                 table, state, self._noise_rng)

predicators/explorers/active_sampler_explorer.py

@@ -185,6 +185,14 @@ def generate_goals() -> Iterator[Set[GroundAtom]]:
 
                     def generate_goals() -> Iterator[Set[GroundAtom]]:
                         nonlocal next_practice_nsrt
+
+                        for op in sorted(self._ground_op_hist,
+                                         key=self._score_ground_op,
+                                         reverse=True):
+                            print(f"{op.name}{op.objects} score: {self._score_ground_op(op)}")
+                        if "PlaceBallInCup" in str(self._ground_op_hist):
+                            import ipdb; ipdb.set_trace()
+
                         # Generate goals sorted by their descending score.
                         for op in sorted(self._ground_op_hist,
                                          key=self._score_ground_op,

predicators/ground_truth_models/sticky_table/nsrts.py

@@ -58,8 +58,10 @@ def get_nsrts(env_name: str, types: Dict[str, Type],
         PickBallFromFloor = options["PickBallFromFloor"]
         PlaceBallOnTable = options["PlaceBallOnTable"]
         PlaceBallOnFloor = options["PlaceBallOnFloor"]
-        PickCupFromTable = options["PickCupFromTable"]
-        PickCupFromFloor = options["PickCupFromFloor"]
+        PickCupWithoutBallFromTable = options["PickCupWithoutBallFromTable"]
+        PickCupWithBallFromTable = options["PickCupWithBallFromTable"]
+        PickCupWithoutBallFromFloor = options["PickCupWithoutBallFromFloor"]
+        PickCupWithBallFromFloor = options["PickCupWithBallFromFloor"]
         PlaceCupWithBallOnTable = options["PlaceCupWithBallOnTable"]
         PlaceCupWithoutBallOnTable = options["PlaceCupWithoutBallOnTable"]
         PlaceCupWithBallOnFloor = options["PlaceCupWithBallOnFloor"]
@@ -186,14 +188,16 @@ def pick_obj_sampler(state: State, goal: Set[GroundAtom],
                                       option_vars, pick_obj_sampler)
         nsrts.add(pickballfromfloor_nsrt)
 
-        # PickCupFromTable
+        # PickCupWithoutBallFromTable
         robot = Variable("?robot", robot_type)
         cup = Variable("?cup", cup_type)
+        ball = Variable("?ball", ball_type)
         table = Variable("?table", table_type)
-        parameters = [robot, cup, table]
+        parameters = [robot, cup, ball, table]
         option_vars = parameters
-        option = PickCupFromTable
+        option = PickCupWithoutBallFromTable
         preconditions = {
+            LiftedAtom(BallNotInCup, [ball, cup]),
             LiftedAtom(ReachableSurface, [robot, table]),
             LiftedAtom(CupOnTable, [cup, table]),
             LiftedAtom(HandEmpty, []),
@@ -203,17 +207,45 @@ def pick_obj_sampler(state: State, goal: Set[GroundAtom],
             LiftedAtom(CupOnTable, [cup, table]),
             LiftedAtom(HandEmpty, []),
         }
-        pickcupfromtable_nsrt = NSRT("PickCupFromTable", parameters,
+        pickcupwithoutballfromtable_nsrt = NSRT("PickCupWithoutBallFromTable", parameters,
                                      preconditions, add_effects,
                                      delete_effects, set(), option,
                                      option_vars, pick_obj_sampler)
-        nsrts.add(pickcupfromtable_nsrt)
+        nsrts.add(pickcupwithoutballfromtable_nsrt)
 
-        # PickCupFromFloor
-        parameters = [robot, cup]
+        # PickCupWithBallFromTable
+        robot = Variable("?robot", robot_type)
+        cup = Variable("?cup", cup_type)
+        ball = Variable("?ball", ball_type)
+        table = Variable("?table", table_type)
+        parameters = [robot, cup, ball, table]
+        option_vars = parameters
+        option = PickCupWithBallFromTable
+        preconditions = {
+            LiftedAtom(BallInCup, [ball, cup]),
+            LiftedAtom(ReachableSurface, [robot, table]),
+            LiftedAtom(CupOnTable, [cup, table]),
+            LiftedAtom(HandEmpty, []),
+            LiftedAtom(BallOnTable, [ball, table])
+        }
+        add_effects = {LiftedAtom(HoldingCup, [cup])}
+        delete_effects = {
+            LiftedAtom(CupOnTable, [cup, table]),
+            LiftedAtom(HandEmpty, []),
+            LiftedAtom(BallOnTable, [ball, table])
+        }
+        pickcupwithoutballfromtable_nsrt = NSRT("PickCupWithBallFromTable", parameters,
+                                     preconditions, add_effects,
+                                     delete_effects, set(), option,
+                                     option_vars, pick_obj_sampler)
+        nsrts.add(pickcupwithoutballfromtable_nsrt)
+
+        # PickCupWithoutBallFromFloor
+        parameters = [robot, cup, ball]
         option_vars = parameters
-        option = PickCupFromFloor
+        option = PickCupWithoutBallFromFloor
         preconditions = {
+            LiftedAtom(BallNotInCup, [ball, cup]),
             LiftedAtom(ReachableCup, [robot, cup]),
             LiftedAtom(CupOnFloor, [cup]),
             LiftedAtom(HandEmpty, []),
@@ -223,12 +255,34 @@ def pick_obj_sampler(state: State, goal: Set[GroundAtom],
             LiftedAtom(CupOnFloor, [cup]),
             LiftedAtom(HandEmpty, []),
         }
+        pickcupwithoutballfromfloor_nsrt = NSRT("PickCupWithoutBallFromFloor", parameters,
+                                     preconditions, add_effects,
+                                     delete_effects, set(), option,
+                                     option_vars, pick_obj_sampler)
+        nsrts.add(pickcupwithoutballfromfloor_nsrt)
 
-        pickcupfromfloor_nsrt = NSRT("PickCupFromFloor", parameters,
+        # PickCupWithBallFromFloor
+        parameters = [robot, cup, ball]
+        option_vars = parameters
+        option = PickCupWithBallFromFloor
+        preconditions = {
+            LiftedAtom(BallOnFloor, [ball]),
+            LiftedAtom(BallInCup, [ball, cup]),
+            LiftedAtom(ReachableCup, [robot, cup]),
+            LiftedAtom(CupOnFloor, [cup]),
+            LiftedAtom(HandEmpty, []),
+        }
+        add_effects = {LiftedAtom(HoldingCup, [cup])}
+        delete_effects = {
+            LiftedAtom(CupOnFloor, [cup]),
+            LiftedAtom(HandEmpty, []),
+            LiftedAtom(BallOnFloor, [ball])
+        }
+        pickcupwithballfromfloor_nsrt = NSRT("PickCupWithBallFromFloor", parameters,
                                      preconditions, add_effects,
                                      delete_effects, set(), option,
                                      option_vars, pick_obj_sampler)
-        nsrts.add(pickcupfromfloor_nsrt)
+        nsrts.add(pickcupwithballfromfloor_nsrt)
 
         # PlaceCubeOnTable
         parameters = [robot, cube, table]
@@ -339,6 +393,7 @@ def place_on_floor_sampler(state: State, goal: Set[GroundAtom],
         option_vars = parameters
         option = PlaceBallInCupOnFloor
         preconditions = {
+            LiftedAtom(BallNotInCup, [ball, cup]),
             LiftedAtom(ReachableCup, [robot, cup]),
             LiftedAtom(CupOnFloor, [cup]),
             LiftedAtom(HoldingBall, [ball])
@@ -348,7 +403,7 @@ def place_on_floor_sampler(state: State, goal: Set[GroundAtom],
             LiftedAtom(BallOnFloor, [ball]),
             LiftedAtom(HandEmpty, []),
         }
-        delete_effects = {LiftedAtom(HoldingBall, [ball])}
+        delete_effects = {LiftedAtom(HoldingBall, [ball]), LiftedAtom(BallNotInCup, [ball, cup])}
 
         def place_ball_in_cup_sampler(state: State, goal: Set[GroundAtom],
                                       rng: np.random.Generator,
@@ -376,14 +431,15 @@ def place_ball_in_cup_sampler(state: State, goal: Set[GroundAtom],
         preconditions = {
             LiftedAtom(ReachableCup, [robot, cup]),
             LiftedAtom(CupOnTable, [cup, table]),
-            LiftedAtom(HoldingBall, [ball])
+            LiftedAtom(HoldingBall, [ball]),
+            LiftedAtom(BallNotInCup, [ball, cup]),
         }
         add_effects = {
             LiftedAtom(BallInCup, [ball, cup]),
             LiftedAtom(BallOnTable, [ball, table]),
             LiftedAtom(HandEmpty, []),
         }
-        delete_effects = {LiftedAtom(HoldingBall, [ball])}
+        delete_effects = {LiftedAtom(HoldingBall, [ball]), LiftedAtom(BallNotInCup, [ball, cup]),}
         placeballincupontable_nsrt = NSRT("PlaceBallInCupOnTable", parameters,
                                           preconditions, add_effects,
                                           delete_effects, set(), option,

predicators/ground_truth_models/sticky_table/options.py

@@ -96,20 +96,35 @@ def get_options(cls, env_name: str, types: Dict[str, Type],
             params_space=params_space,
             types=[robot_type, ball_type])
 
-        PickCupFromTable = utils.SingletonParameterizedOption(
-            # variables: [robot, cup, table]
-            "PickCupFromTable",
+        PickCupWithoutBallFromTable = utils.SingletonParameterizedOption(
+            # variables: [robot, cup, ball, table]
+            "PickCupWithoutBallFromTable",
             cls._create_pass_through_policy(action_space),
             params_space=params_space,
-            types=[robot_type, cup_type, table_type])
+            types=[robot_type, cup_type, ball_type, table_type])
+
+        PickCupWithBallFromTable = utils.SingletonParameterizedOption(
+            # variables: [robot, cup, ball, table]
+            "PickCupWithBallFromTable",
+            cls._create_pass_through_policy(action_space),
+            params_space=params_space,
+            types=[robot_type, cup_type, ball_type, table_type])
 
-        PickCupFromFloor = utils.SingletonParameterizedOption(
-            # variables: [robot, cup]
-            "PickCupFromFloor",
+        PickCupWithoutBallFromFloor = utils.SingletonParameterizedOption(
+            # variables: [robot, cup, ball]
+            "PickCupWithoutBallFromFloor",
             cls._create_pass_through_policy(action_space),
             # Parameters are absolute x, y actions.
             params_space=params_space,
-            types=[robot_type, cup_type])
+            types=[robot_type, cup_type, ball_type])
+
+        PickCupWithBallFromFloor = utils.SingletonParameterizedOption(
+            # variables: [robot, cup, ball]
+            "PickCupWithBallFromFloor",
+            cls._create_pass_through_policy(action_space),
+            # Parameters are absolute x, y actions.
+            params_space=params_space,
+            types=[robot_type, cup_type, ball_type])
 
         PlaceCupWithBallOnTable = utils.SingletonParameterizedOption(
             # variables: [robot, ball, cup, table]
@@ -195,7 +210,7 @@ def get_options(cls, env_name: str, types: Dict[str, Type],
             PickCubeFromTable, PickCubeFromFloor, PlaceCubeOnTable,
             PlaceCubeOnFloor, NavigateToCube, NavigateToTable,
             PickBallFromTable, PickBallFromFloor, PlaceBallOnTable,
-            PlaceBallOnFloor, PickCupFromTable, PickCupFromFloor,
+            PlaceBallOnFloor,PickCupWithoutBallFromTable, PickCupWithBallFromTable, PickCupWithoutBallFromFloor, PickCupWithBallFromFloor,
             PlaceCupWithBallOnTable, PlaceCupWithoutBallOnTable,
             PlaceCupWithBallOnFloor, PlaceCupWithoutBallOnFloor,
             PlaceBallInCupOnFloor, PlaceBallInCupOnTable, NavigateToBall,

predicators/planning.py

@@ -1060,6 +1060,7 @@ def fd_plan_from_sas_file(
     metrics: Metrics = defaultdict(float)
     num_nodes_expanded = re.findall(r"Expanded (\d+) state", output)
     num_nodes_created = re.findall(r"Evaluated (\d+) state", output)
+    import ipdb; ipdb.set_trace()
     assert len(num_nodes_expanded) == 1
     assert len(num_nodes_created) == 1
     metrics["num_nodes_expanded"] = float(num_nodes_expanded[0])