diff --git a/.gitignore b/.gitignore index 7631435dc6..dc647e6083 100644 --- a/.gitignore +++ b/.gitignore @@ -27,5 +27,4 @@ tests/_fake_results sas_plan # Jetbrains IDEs -.idea/ -analyze_sticky_data.py \ No newline at end of file +.idea/ \ No newline at end of file diff --git a/predicators/envs/ball_and_cup_sticky_table.py b/predicators/envs/ball_and_cup_sticky_table.py index 492ba89c4b..38f9b34fce 100644 --- a/predicators/envs/ball_and_cup_sticky_table.py +++ b/predicators/envs/ball_and_cup_sticky_table.py @@ -18,12 +18,13 @@ class BallAndCupStickyTableEnv(BaseEnv): tables. This environment is a more-complex (but significantly different) version of the sticky-table environment. - Most of the tables are completely flat, but one is half is smooth and half - is sticky. If the agent tries to place the ball directly on any table, - it will roll off with high probability. If it tries to place it on a - half-flat half-smooth table, the ball will *certainly* roll off. However, - if the ball is placed inside a cup first, then the ball + cup system stays - on the sticky and normal table surfaces with high probability. + Most of the tables are completely flat, but one is half is mostly smooth + sticky in a particular circular region on the table. If the agent tries + to place the ball directly on any table, it will roll off with high + probability. If it tries to place it on a special table, + the ball will *certainly* roll off. However, if the ball is placed inside + a cup first, then the ball + cup system stays on the sticky and normal + table surfaces with high probability. Note that unlike almost all of our other environments, there is real stochasticity in the outcomes of placing. @@ -148,7 +149,7 @@ def _place_smooth_fall_prob(self) -> float: return CFG.sticky_table_place_smooth_fall_prob @classmethod - def _object_to_geom(self, obj: Object, state: State) -> utils._Geom2D: + def _object_to_geom(cls, obj: Object, state: State) -> utils._Geom2D: x = state.get(obj, "x") y = state.get(obj, "y") radius = state.get(obj, "radius") @@ -182,7 +183,7 @@ def _get_tasks(self, num: int, # Add a random spin to offset the circle. This is to ensure # the tables are in different positions along the circle every # time. - theta_offset = 0.0 #rng.uniform(0, 2 * np.pi) + theta_offset = 0.0 #rng.uniform(0, 2 * np.pi) sticky_region_radius = radius * self.sticky_region_radius_scale # Now, actually instantiate the tables. for i, theta in enumerate(thetas): @@ -195,16 +196,26 @@ def _get_tasks(self, num: int, prefix = "sticky" sticky = 1.0 obj = Object(f"{prefix}-table-{i}", self._table_type) - sticky_region_dist_from_center = rng.uniform(0.0, radius - sticky_region_radius) + sticky_region_dist_from_center = rng.uniform( + 0.0, radius - sticky_region_radius) sticky_region_theta_from_center = rng.uniform(0.0, 2 * np.pi) state_dict[obj] = { - "x": x, - "y": y, - "radius": radius, - "sticky": sticky, - "sticky_region_x_offset": sticky_region_dist_from_center * np.cos(sticky_region_theta_from_center), - "sticky_region_y_offset": sticky_region_dist_from_center * np.sin(sticky_region_theta_from_center), - "sticky_region_radius": sticky_region_radius + "x": + x, + "y": + y, + "radius": + radius, + "sticky": + sticky, + "sticky_region_x_offset": + sticky_region_dist_from_center * + np.cos(sticky_region_theta_from_center), + "sticky_region_y_offset": + sticky_region_dist_from_center * + np.sin(sticky_region_theta_from_center), + "sticky_region_radius": + sticky_region_radius } tables = sorted(state_dict) target_table = tables[-1] @@ -270,15 +281,15 @@ def _get_tasks(self, num: int, return tasks @classmethod - def exists_robot_collision(self, state: State) -> bool: + def exists_robot_collision(cls, state: State) -> bool: """Return true if there is a collision between the robot and any other object in the environment.""" - robot, = state.get_objects(self._robot_type) + robot, = state.get_objects(cls._robot_type) all_possible_collision_objs = state.get_objects( - self._table_type) + state.get_objects( - self._cup_type) + state.get_objects(self._ball_type) + cls._table_type) + state.get_objects( + cls._cup_type) + state.get_objects(cls._ball_type) for obj in all_possible_collision_objs: - obj_geom = self._object_to_geom(obj, state) + obj_geom = cls._object_to_geom(obj, state) if obj_geom.contains_point(state.get(robot, "x"), state.get(robot, "y")): return True @@ -410,6 +421,7 @@ def simulate(self, state: State, action: Action) -> State: # Placing logic. else: if not hand_empty: + assert obj_being_held is not None # Find the table for placing, if any. table: Optional[Object] = None for target in state.get_objects(self._table_type): @@ -441,26 +453,38 @@ def simulate(self, state: State, action: Action) -> State: if obj_being_held is not None: next_state.set(obj_being_held, "held", 0.0) if obj_type_id == 3.0: - # Possibly put on the table, or have it fall somewhere near. + # Possibly put on the table, or have it fall + # somewhere near. fall_prob = self._place_sticky_fall_prob if obj_being_held == ball: fall_prob = self._place_ball_fall_prob if self._table_is_sticky(table, state): - # Check if placing on the smooth side of the sticky table, - # and set fall prob accordingly. - sticky_region_x = state.get(table, "sticky_region_x_offset") + table_x - sticky_region_y = state.get(table, "sticky_region_y_offset") + table_y - sticky_region = utils.Circle(sticky_region_x, sticky_region_y, state.get(table, "sticky_region_radius")) + # Check if placing on the smooth part of + # the sticky table, and set fall prob + # accordingly. + sticky_region_x = state.get( + table, + "sticky_region_x_offset") + table_x + sticky_region_y = state.get( + table, + "sticky_region_y_offset") + table_y + sticky_region = utils.Circle( + sticky_region_x, sticky_region_y, + state.get(table, + "sticky_region_radius")) if not sticky_region.contains_point( act_x, act_y): if obj_being_held == cup: - fall_prob = self._place_smooth_fall_prob + fall_prob = \ + self._place_smooth_fall_prob else: assert obj_being_held == ball fall_prob = 1.0 - # Handle object falling or placing on table surface. + # Handle object falling or placing on table + # surface. if self._noise_rng.uniform() < fall_prob: - fall_x, fall_y = self._sample_floor_point_around_table( + fall_x, fall_y = \ + self._sample_floor_point_around_table( table, state, self._noise_rng) next_state = self._handle_placing_object( fall_x, fall_y, next_state, @@ -476,7 +500,8 @@ def simulate(self, state: State, action: Action) -> State: assert self._OnTable_holds( next_state, [obj_being_held, table]) else: - assert obj_type_id == 2.0 # corresponding to placing in cup + # corresponding to placing in cup + assert obj_type_id == 2.0 assert obj_being_held == ball next_state.set(ball, "x", act_x) next_state.set(ball, "y", act_y) diff --git a/predicators/ground_truth_models/ball_and_cup_sticky_table/nsrts.py b/predicators/ground_truth_models/ball_and_cup_sticky_table/nsrts.py index 1982c839ce..bd8fdbb66d 100644 --- a/predicators/ground_truth_models/ball_and_cup_sticky_table/nsrts.py +++ b/predicators/ground_truth_models/ball_and_cup_sticky_table/nsrts.py @@ -344,7 +344,7 @@ def place_ball_on_floor_sampler(state: State, goal: Set[GroundAtom], def place_ball_in_cup_sampler(state: State, goal: Set[GroundAtom], rng: np.random.Generator, objs: Sequence[Object]) -> Array: - del goal # unused + del rng, goal # unused cup = objs[2] # Just place the ball in the middle of the cup. Set # the type id to be 2.0 to correspond to the cup @@ -427,10 +427,10 @@ def place_ball_in_cup_sampler(state: State, goal: Set[GroundAtom], # LiftedAtom(HoldingBall, [ball]) # } # placecupwithballontable_nsrt = NSRT("PlaceCupWithBallOnTable", - # parameters, preconditions, - # add_effects, delete_effects, set(), - # option, option_vars, - # place_on_table_sampler) + # parameters, preconditions, + # add_effects, delete_effects, set(), + # option, option_vars, + # place_on_table_sampler) # nsrts.add(placecupwithballontable_nsrt) # PlaceCupWithoutBallOnFloor @@ -482,7 +482,7 @@ def place_ball_in_cup_sampler(state: State, goal: Set[GroundAtom], option_vars = parameters option = NavigateToBall preconditions = set() - add_effects = {ReachableBall([robot, ball])} + add_effects = {LiftedAtom(ReachableBall, [robot, ball])} ignore_effects = {ReachableSurface, ReachableBall, ReachableCup} def navigate_to_obj_sampler(state: State, goal: Set[GroundAtom], @@ -539,7 +539,7 @@ def navigate_to_obj_sampler(state: State, goal: Set[GroundAtom], option_vars = parameters option = NavigateToCup preconditions = set() - add_effects = {ReachableCup([robot, cup])} + add_effects = {LiftedAtom(ReachableSurface, [robot, table])} ignore_effects = {ReachableSurface, ReachableBall, ReachableCup} navigatetocup_nsrt = NSRT("NavigateToCup", parameters, preconditions, add_effects, set(), ignore_effects, option, @@ -551,7 +551,7 @@ def navigate_to_obj_sampler(state: State, goal: Set[GroundAtom], option_vars = parameters option = NavigateToTable preconditions = set() - add_effects = {ReachableSurface([robot, table])} + add_effects = {LiftedAtom(ReachableSurface, [robot, table])} ignore_effects = {ReachableSurface, ReachableBall, ReachableCup} navigatetotable_nsrt = NSRT("NavigateToTable", parameters, preconditions, add_effects, diff --git a/predicators/ground_truth_models/ball_and_cup_sticky_table/options.py b/predicators/ground_truth_models/ball_and_cup_sticky_table/options.py index b571ff33ac..0a74cf1b0e 100644 --- a/predicators/ground_truth_models/ball_and_cup_sticky_table/options.py +++ b/predicators/ground_truth_models/ball_and_cup_sticky_table/options.py @@ -27,7 +27,8 @@ def get_options(cls, env_name: str, types: Dict[str, Type], cup_type = types["cup"] ball_type = types["ball"] table_type = types["table"] - # Parameters are move_or_pickplace, obj_type_id, ball_only, absolute x, y actions. + # Parameters are move_or_pickplace, obj_type_id, ball_only, + # absolute x, y actions. params_space = Box( np.array([ 0.0, 0.0, 0.0, BallAndCupStickyTableEnv.x_lb, diff --git a/predicators/utils.py b/predicators/utils.py index 5e3f94ebab..9bee466537 100644 --- a/predicators/utils.py +++ b/predicators/utils.py @@ -301,42 +301,24 @@ def construct_active_sampler_input(state: State, objects: Sequence[Object], sampler_input_lst.append(params[0] - target_pos) elif CFG.env == "ball_and_cup_sticky_table": if "PlaceCup" in param_option.name and "Table" in param_option.name: - robot, ball, cup, table = objects - robot_y = state.get(robot, "y") - robot_x = state.get(robot, "x") + _, _, _, table = objects table_y = state.get(table, "y") table_x = state.get(table, "x") - ball_x = state.get(ball, "x") - ball_y = state.get(ball, "y") - cup_x = state.get(cup, "x") - cup_y = state.get(cup, "y") sticky = state.get(table, "sticky") - # sticky_radius = state.get(table, "sticky_radius") sticky_region_x = state.get(table, "sticky_region_x_offset") sticky_region_y = state.get(table, "sticky_region_y_offset") sticky_region_radius = state.get(table, "sticky_region_radius") table_radius = state.get(table, "radius") - a, b, c, param_x, param_y = params + _, _, _, param_x, param_y = params sampler_input_lst.append(table_radius) sampler_input_lst.append(sticky) sampler_input_lst.append(sticky_region_x) sampler_input_lst.append(sticky_region_y) sampler_input_lst.append(sticky_region_radius) - # sampler_input_lst.append(ball_x) - # sampler_input_lst.append(ball_y) - # sampler_input_lst.append(cup_x) - # sampler_input_lst.append(cup_y) - # sampler_input_lst.append(robot_x) - # sampler_input_lst.append(robot_y) sampler_input_lst.append(table_x) sampler_input_lst.append(table_y) - # sampler_input_lst.append(a) - # sampler_input_lst.append(b) - # sampler_input_lst.append(c) sampler_input_lst.append(param_x) sampler_input_lst.append(param_y) - # sampler_input_lst.append(param_x - table_x) - # sampler_input_lst.append(param_y - table_y) else: raise NotImplementedError("Oracle feature selection not " f"implemented for {CFG.env}") @@ -403,25 +385,8 @@ def plot(self, ax: plt.Axes, **kwargs: Any) -> None: def contains_point(self, x: float, y: float) -> bool: return (x - self.x)**2 + (y - self.y)**2 <= self.radius**2 - def sector_contains_point(self, x: float, y: float, - sector_start_angle: float, - sector_end_angle: float) -> bool: - """Returns true if the point x, y is contained within the sector - starting at sector_start_angle radians and ending at sector_end_angle - radians.""" - # First, check that the point is even on the circle. - if not self.contains_point(x, y): - return False - # Next, convert (x, y) relative to the table's center - # to polar coordinates. - relative_x = x - self.x - relative_y = y - self.y - theta = np.arctan2(relative_y, relative_x) - if theta < 0: - theta = np.pi - theta - return sector_start_angle <= theta <= sector_end_angle - def contains_circle(self, other_circle: Circle) -> bool: + """Check whether this circle wholly contains another one.""" dist_between_centers = np.sqrt((other_circle.x - self.x)**2 + (other_circle.y - self.y)**2) return (dist_between_centers + other_circle.radius) <= self.radius diff --git a/tests/envs/test_ball_and_cup_sticky_table_env.py b/tests/envs/test_ball_and_cup_sticky_table_env.py index e9697a1d49..9e491b5d5f 100644 --- a/tests/envs/test_ball_and_cup_sticky_table_env.py +++ b/tests/envs/test_ball_and_cup_sticky_table_env.py @@ -5,7 +5,6 @@ from predicators import utils from predicators.envs.ball_and_cup_sticky_table import BallAndCupStickyTableEnv from predicators.ground_truth_models import get_gt_nsrts, get_gt_options -from predicators.structs import Action def test_sticky_table():