From caa1341f44b56c537b2018f8af395ca0db2b587f Mon Sep 17 00:00:00 2001 From: Nishanth Kumar Date: Thu, 26 Oct 2023 16:00:46 -0400 Subject: [PATCH] should be ready! --- predicators/envs/ball_and_cup_sticky_table.py | 5 +- .../explorers/active_sampler_explorer.py | 2 +- .../ball_and_cup_sticky_table/nsrts.py | 7 +-- predicators/utils.py | 7 ++- .../test_active_sampler_learning_approach.py | 3 -- .../test_ball_and_cup_sticky_table_env.py | 47 +++++++++++------- tests/test_utils.py | 48 ++++++++++++++++++- 7 files changed, 87 insertions(+), 32 deletions(-) diff --git a/predicators/envs/ball_and_cup_sticky_table.py b/predicators/envs/ball_and_cup_sticky_table.py index 38f9b34fce..05618cc808 100644 --- a/predicators/envs/ball_and_cup_sticky_table.py +++ b/predicators/envs/ball_and_cup_sticky_table.py @@ -508,9 +508,6 @@ def simulate(self, state: State, action: Action) -> State: 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: @@ -519,7 +516,7 @@ def simulate(self, state: State, action: Action) -> State: pseudo_next_state.set(robot, "x", act_x) pseudo_next_state.set(robot, "y", act_y) if self.exists_robot_collision(pseudo_next_state): - return next_state + return next_state # pragma: no cover next_state.set(robot, "x", act_x) next_state.set(robot, "y", act_y) return next_state diff --git a/predicators/explorers/active_sampler_explorer.py b/predicators/explorers/active_sampler_explorer.py index e783e3611e..6f23e7ebff 100644 --- a/predicators/explorers/active_sampler_explorer.py +++ b/predicators/explorers/active_sampler_explorer.py @@ -217,7 +217,7 @@ def generate_goals() -> Iterator[Set[GroundAtom]]: # call the policy, since we don't need to execute # anything. if len(goal) == 0: - return _option_policy(state) + return _option_policy(state) # pragma: no cover # Add this task to the re-planning task queue. self._replanning_tasks.append(task) 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 c6e175337e..3ae144dd87 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 @@ -290,10 +290,7 @@ def place_on_floor_sampler(state: State, goal: Set[GroundAtom], objs: Sequence[Object]) -> Array: del goal # not used obj_to_place = objs[-1] - if obj_to_place.type.name == "cube": - size = state.get(obj_to_place, "size") * 2 - else: - size = state.get(obj_to_place, "radius") * 2 + size = state.get(obj_to_place, "radius") * 2 dist = rng.uniform(0, size) theta = rng.uniform(0, 2 * np.pi) # Just place in a small radius near the center of the room. @@ -510,7 +507,7 @@ def navigate_to_obj_sampler(state: State, goal: Set[GroundAtom], x > BallAndCupStickyTableEnv.x_ub or \ y < BallAndCupStickyTableEnv.y_lb or \ y > BallAndCupStickyTableEnv.y_ub: - continue + continue # pragma: no cover pseudo_next_state = state.copy() pseudo_next_state.set(robot, "x", x) pseudo_next_state.set(robot, "y", y) diff --git a/predicators/utils.py b/predicators/utils.py index 9bee466537..df27742f6e 100644 --- a/predicators/utils.py +++ b/predicators/utils.py @@ -319,6 +319,10 @@ def construct_active_sampler_input(state: State, objects: Sequence[Object], sampler_input_lst.append(table_y) sampler_input_lst.append(param_x) sampler_input_lst.append(param_y) + else: # Use all features. + for obj in objects: + sampler_input_lst.extend(state[obj]) + sampler_input_lst.extend(params) else: raise NotImplementedError("Oracle feature selection not " f"implemented for {CFG.env}") @@ -3508,7 +3512,8 @@ def run_ground_nsrt_with_assertions(ground_nsrt: _GroundNSRT, f"Precondition for {ground_nsrt_str} failed: {atom}" option = ground_nsrt.sample_option(state, set(), rng) if override_params is not None: - option = option.parent.ground(option.objects, override_params) + option = option.parent.ground(option.objects, + override_params) # pragma: no cover assert option.initiable(state) for _ in range(max_steps): act = option.policy(state) diff --git a/tests/approaches/test_active_sampler_learning_approach.py b/tests/approaches/test_active_sampler_learning_approach.py index 491c6a6466..9b6769d5d0 100644 --- a/tests/approaches/test_active_sampler_learning_approach.py +++ b/tests/approaches/test_active_sampler_learning_approach.py @@ -91,6 +91,3 @@ def test_active_sampler_learning_approach(model_name, right_targets, num_demo, # an action. action = policy(task.init) assert env.action_space.contains(action.arr) - -# TODO: test the ball and cup sticky table env here, and also test the case -# where the we try to use oracle feature selection for a non-existent env. 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 72bf9d5bff..3008ecdcd2 100644 --- a/tests/envs/test_ball_and_cup_sticky_table_env.py +++ b/tests/envs/test_ball_and_cup_sticky_table_env.py @@ -1,6 +1,7 @@ """Test cases for the Ball and Cup Sticky Table environment.""" import numpy as np +import pytest from predicators import utils from predicators.envs.ball_and_cup_sticky_table import BallAndCupStickyTableEnv @@ -74,6 +75,11 @@ def test_sticky_table(): assert len(env_test_tasks) == 2 env_task = env_test_tasks[1] + # Test rendering. + env.reset("test", 1) + with pytest.raises(NotImplementedError): + env.render(caption="Test") + # Extract objects for NSRT testing. init_state = env_test_tasks[0].task.init rng = np.random.default_rng(123) @@ -97,6 +103,7 @@ def test_sticky_table(): ball_init_table = ball_init_tables[0] # Test noise-free CUP picking and placing on the floor and normal tables. + # Also test placing the ball into the cup on the floor. first_table = normal_tables[0] ground_nsrt_plan = [ NavigateToCup.ground([robot, cup]), @@ -115,6 +122,11 @@ def test_sticky_table(): [robot, cup, ball, normal_tables[-1]])) ground_nsrt_plan.append( PlaceCupWithoutBallOnFloor.ground([robot, ball, cup])) + ground_nsrt_plan.append(NavigateToTable.ground([robot, ball_init_table])) + ground_nsrt_plan.append( + PickBallFromTable.ground([robot, ball, cup, ball_init_table])) + ground_nsrt_plan.append(NavigateToCup.ground([robot, cup])) + ground_nsrt_plan.append(PlaceBallInCupOnFloor.ground([robot, ball, cup])) state = env.reset("test", 0) for ground_nsrt in ground_nsrt_plan: state = utils.run_ground_nsrt_with_assertions(ground_nsrt, state, env, @@ -155,23 +167,24 @@ def test_sticky_table(): state = utils.run_ground_nsrt_with_assertions(ground_nsrt, state, env, rng) - # # Test putting the ball in the cup first and then both on the table. - # table = ball_init_table - # ground_nsrt_plan = [ - # NavigateToTable.ground([robot, table]), - # PickBallFromTable.ground([robot, ball, cup, table]), - # NavigateToCup.ground([robot, cup]), - # PlaceBallInCupOnFloor.ground([robot, ball, cup]), - # PickCupWithBallFromFloor.ground([robot, cup, ball]), - # NavigateToTable.ground([robot, table]), - # PlaceCupWithBallOnTable.ground([robot, ball, cup, table]), - # PickCupWithBallFromTable.ground([robot, cup, ball, table]), - # PlaceCupWithBallOnFloor.ground([robot, ball, cup]), - # ] - # state = env.reset("test", 0) - # for ground_nsrt in ground_nsrt_plan: - # state = utils.run_ground_nsrt_with_assertions(ground_nsrt, state, env, - # rng) + # Test putting the ball in the cup first and then going to the table, + # then placing back onto the floor. + table = ball_init_table + ground_nsrt_plan = [ + NavigateToTable.ground([robot, table]), + PickBallFromTable.ground([robot, ball, cup, table]), + NavigateToCup.ground([robot, cup]), + PlaceBallInCupOnFloor.ground([robot, ball, cup]), + PickCupWithBallFromFloor.ground([robot, cup, ball]), + NavigateToTable.ground([robot, table]), + # PlaceCupWithBallOnTable.ground([robot, ball, cup, table]), + # PickCupWithBallFromTable.ground([robot, cup, ball, table]), + PlaceCupWithBallOnFloor.ground([robot, ball, cup]), + ] + state = env.reset("test", 0) + for ground_nsrt in ground_nsrt_plan: + state = utils.run_ground_nsrt_with_assertions(ground_nsrt, state, env, + rng) # Test picking the ball from inside the cup on the floor. table = ball_init_table diff --git a/tests/test_utils.py b/tests/test_utils.py index 06e73cb340..45cff4e776 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -1,7 +1,7 @@ """Test cases for utils.""" import os import time -from typing import Iterator, Tuple +from typing import Iterator, Optional, Tuple from typing import Type as TypingType import matplotlib.pyplot as plt @@ -10,6 +10,7 @@ from gym.spaces import Box from predicators import utils +from predicators.envs.ball_and_cup_sticky_table import BallAndCupStickyTableEnv from predicators.envs.cover import CoverEnv, CoverMultistepOptions from predicators.envs.pddl_env import ProceduralTasksGripperPDDLEnv, \ ProceduralTasksSpannerPDDLEnv @@ -3258,3 +3259,48 @@ def test_motion_planning(): # Test that query_to_goal_fn for BiRRT raises a NotImplementedError with pytest.raises(NotImplementedError): birrt.query_to_goal_fn(0, lambda: 1, lambda x: False) + + +def test_oracle_feature_selection(): + """Test the oracle feature selection code.""" + utils.reset_config({ + "env": "ball_and_cup_sticky_table", + "active_sampler_learning_feature_selection": "oracle" + }) + env = BallAndCupStickyTableEnv() + train_tasks = [t.task for t in env.get_train_tasks()] + state = train_tasks[0].init + options = get_gt_options(env.get_name()) + PlaceCupWithoutBallOnTable: Optional[ParameterizedOption] = None + NavigateToCup: Optional[ParameterizedOption] = None + for opt in options: + if "PlaceCupWithoutBallOnTable" in opt.name: + PlaceCupWithoutBallOnTable = opt + elif "NavigateToCup" in opt.name: + NavigateToCup = opt + assert PlaceCupWithoutBallOnTable is not None + params = [0.0, 0.0, 0.0, 0.0, 0.0] + cup = state.get_objects(env._cup_type)[0] + robot = state.get_objects(env._robot_type)[0] + ball = state.get_objects(env._ball_type)[0] + table = state.get_objects(env._table_type)[0] + # Construct input for special place skill and test that it has only + # 10 features. + sampler_input = utils.construct_active_sampler_input( + state, [cup, robot, ball, table], params, PlaceCupWithoutBallOnTable) + assert len(sampler_input) == 10 + # Construct input for navigation skill and test that it has 12 + # features. + sampler_input = utils.construct_active_sampler_input( + state, [robot, cup], params, NavigateToCup) + assert len(sampler_input) == 12 + # Try a non-existent feature selection method and test that an + # error is raised. + utils.reset_config({ + "env": "not-a-real-env", + "active_sampler_learning_feature_selection": "oracle" + }) + with pytest.raises(NotImplementedError) as e: + utils.construct_active_sampler_input(state, [robot, cup], params, + NavigateToCup) + assert "Oracle feature selection" in str(e)