From 0316c7428bf38f9df38019ecb4e58a23bd4e3f05 Mon Sep 17 00:00:00 2001 From: Nishanth Kumar Date: Mon, 23 Oct 2023 13:27:42 -0400 Subject: [PATCH] try this for fun --- .../active_sampler_learning_approach.py | 2 +- predicators/envs/sticky_table.py | 20 ++++++++++++++++--- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/predicators/approaches/active_sampler_learning_approach.py b/predicators/approaches/active_sampler_learning_approach.py index 5c6125a64d..ec0c6e98d6 100644 --- a/predicators/approaches/active_sampler_learning_approach.py +++ b/predicators/approaches/active_sampler_learning_approach.py @@ -618,7 +618,7 @@ def _sample(state: State, goal: Set[GroundAtom], rng: np.random.Generator, print(best_sample_will_work) print(learned_sampler_input) print(option.name) - import ipdb; ipdb.set_trace() + # import ipdb; ipdb.set_trace() if strategy in ["greedy", "epsilon_greedy"]: idx = int(np.argmax(scores)) if strategy == "epsilon_greedy" and rng.uniform( diff --git a/predicators/envs/sticky_table.py b/predicators/envs/sticky_table.py index 0e1ca04746..be35ad3353 100644 --- a/predicators/envs/sticky_table.py +++ b/predicators/envs/sticky_table.py @@ -39,7 +39,7 @@ class StickyTableEnv(BaseEnv): x_ub: ClassVar[float] = 1.0 y_lb: ClassVar[float] = 0.0 y_ub: ClassVar[float] = 1.0 - reachable_thresh: ClassVar[float] = 0.25 + reachable_thresh: ClassVar[float] = 0.1 objs_scale: ClassVar[float] = 0.25 # as a function of table radius sticky_surface_mode: ClassVar[str] = "half" # half or whole # Types @@ -151,10 +151,12 @@ def simulate(self, state: State, action: Action) -> State: if self._action_grasps_object(act_x, act_y, cube, state): next_state.set(cube, "held", 1.0) + assert self._Holding_holds(next_state, [cube]) elif obj_type_id == 1.0: if self._action_grasps_object(act_x, act_y, ball, state): next_state.set(ball, "held", 1.0) + assert self._Holding_holds(next_state, [ball]) else: assert obj_type_id == 2.0 if self._action_grasps_object(act_x, act_y, cup, @@ -162,6 +164,8 @@ def simulate(self, state: State, action: Action) -> State: next_state.set(cup, "held", 1.0) if ball_in_cup: next_state.set(ball, "held", 1.0) + assert self._Holding_holds(next_state, [ball]) + assert self._Holding_holds(next_state, [cup]) # Placing logic. else: if obj_being_held is not None: @@ -223,6 +227,8 @@ def simulate(self, state: State, action: Action) -> State: next_state.set(ball, "y", act_y) next_state.set(ball, "held", 0.0) assert self._BallInCup_holds(next_state, [ball, cup]) + if self._OnFloor_holds(next_state, [cup]): + assert self._OnFloor_holds(next_state, [ball]) if ball_only < 0.5: assert self._HandEmpty_holds(next_state, []) else: @@ -555,9 +561,17 @@ def _sample_floor_point_around_table( x = state.get(table, "x") y = state.get(table, "y") radius = state.get(table, "radius") - dist = radius + rng.uniform(radius / 10, radius / 4) + dist_from_table = self.objs_scale * radius + dist = radius + rng.uniform(radius + dist_from_table, radius + (1.15 * dist_from_table)) theta = rng.uniform(0, 2 * np.pi) - return (x + dist * np.cos(theta), y + dist * np.sin(theta)) + sampled_x = x + dist * np.cos(theta) + sampled_y = y + dist * np.sin(theta) + while sampled_x < self.x_lb or sampled_x > self.x_ub or sampled_y < self.y_lb or sampled_y > self.y_ub: + dist = radius + rng.uniform(radius + dist_from_table, radius + (1.15 * dist_from_table)) + theta = rng.uniform(0, 2 * np.pi) + sampled_x = x + dist * np.cos(theta) + sampled_y = y + dist * np.sin(theta) + return (sampled_x, sampled_y) @classmethod def exists_robot_collision(self, state: State) -> bool: