diff --git a/predicators/behavior_utils/behavior_utils.py b/predicators/behavior_utils/behavior_utils.py index bc847a5fae..6a51276072 100644 --- a/predicators/behavior_utils/behavior_utils.py +++ b/predicators/behavior_utils/behavior_utils.py @@ -856,6 +856,41 @@ def get_scene_body_ids( return ids +def get_relevant_scene_body_ids( + env: "BehaviorEnv", + include_self: bool = False, + include_right_hand: bool = False, +) -> List[int]: + """Function to return list of body_ids for relveant objs in the scene for + collision checking depending on whether navigation or grasping/ placing is + being done.""" + ids = [] + for obj in env.scene.get_objects(): + if isinstance(obj, URDFObject): + # We want to exclude the floor since we're always floating and + # will never practically collide with it, but if we include it + # in collision checking, we always seem to collide. + if obj.name != "floors": + # Here are the list of relevant objects. + if "bed" in obj.name: + ids.extend(obj.body_ids) + + if include_self: + ids.append(env.robots[0].parts["left_hand"].get_body_id()) + ids.append(env.robots[0].parts["body"].get_body_id()) + ids.append(env.robots[0].parts["eye"].get_body_id()) + if not include_right_hand: + ids.append(env.robots[0].parts["right_hand"].get_body_id()) + + all_obj_ids = [] + for obj in env.scene.get_objects(): + if isinstance(obj, URDFObject): + all_obj_ids.append([obj.name, obj.body_ids]) + print("ALL OBJ IDS:", all_obj_ids) + + return ids + + def detect_collision(bodyA: int, object_in_hand: Optional[int] = None) -> bool: """Detects collisions between bodyA in the scene (except for the object in the robot's hand)""" @@ -1076,7 +1111,7 @@ def sample_navigation_params(igibson_behavior_env: "BehaviorEnv", Implemented in a separate method to enable code reuse in option_model_fns. """ - closeness_limit = 2.00 + closeness_limit = CFG.behavior_closeness_limit nearness_limit = 0.15 distance = nearness_limit + ( (closeness_limit - nearness_limit) * rng.random()) diff --git a/predicators/behavior_utils/motion_planner_fns.py b/predicators/behavior_utils/motion_planner_fns.py index 633e9b60f1..55701603c1 100644 --- a/predicators/behavior_utils/motion_planner_fns.py +++ b/predicators/behavior_utils/motion_planner_fns.py @@ -10,7 +10,7 @@ from numpy.random._generator import Generator from predicators.behavior_utils.behavior_utils import check_nav_end_pose, \ - get_aabb_volume, get_closest_point_on_aabb, get_scene_body_ids, \ + get_aabb_volume, get_closest_point_on_aabb, get_relevant_scene_body_ids, \ reset_and_release_hand from predicators.settings import CFG from predicators.structs import Array @@ -24,7 +24,7 @@ RoomFloor # pylint: disable=unused-import from igibson.objects.articulated_object import URDFObject from igibson.utils.behavior_robot_planning_utils import \ - plan_base_motion_br # , plan_hand_motion_br + plan_base_motion_br, plan_hand_motion_br except (ImportError, ModuleNotFoundError) as e: pass @@ -124,10 +124,12 @@ def sample_fn(env: "BehaviorEnv", valid_position[0][1], valid_position[1][2], ] - if env.use_rrt: - obstacles = get_scene_body_ids(env) + if CFG.behavior_option_model_rrt: + obstacles = get_relevant_scene_body_ids(env) if env.robots[0].parts["right_hand"].object_in_hand is not None: - obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) + if env.robots[0].parts["right_hand"].object_in_hand in obstacles: + obstacles.remove( + env.robots[0].parts["right_hand"].object_in_hand) plan = plan_base_motion_br( robot=env.robots[0], end_conf=end_conf, @@ -135,6 +137,7 @@ def sample_fn(env: "BehaviorEnv", obstacles=obstacles, override_sample_fn=lambda: sample_fn(env, rng), rng=rng, + max_distance=0.01, ) p.restoreState(state) else: @@ -147,6 +150,7 @@ def sample_fn(env: "BehaviorEnv", logging.info(f"PRIMITIVE: navigate to {obj.name} with params " f"{pos_offset} failed; birrt failed to sample a plan!") return None + plan = plan + [end_conf] p.restoreState(state) p.removeState(state) @@ -221,13 +225,13 @@ def make_grasp_plan( x = obj_pos[0] + grasp_offset[0] y = obj_pos[1] + grasp_offset[1] z = obj_pos[2] + grasp_offset[2] - # hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() - # minx = min(x, hand_x) - 0.5 - # miny = min(y, hand_y) - 0.5 - # minz = min(z, hand_z) - 0.5 - # maxx = max(x, hand_x) + 0.5 - # maxy = max(y, hand_y) + 0.5 - # maxz = max(z, hand_z) + 0.5 + hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() + minx = min(x, hand_x) - 0.5 + miny = min(y, hand_y) - 0.5 + minz = min(z, hand_z) - 0.5 + maxx = max(x, hand_x) + 0.5 + maxy = max(y, hand_y) + 0.5 + maxz = max(z, hand_z) + 0.5 # compute the angle the hand must be in such that it can # grasp the object from its current offset position @@ -275,25 +279,26 @@ def make_grasp_plan( euler_angles[2], ] # For now we are not running rrt with grasp. - # if env.use_rrt: - # # plan a motion to the pose [x, y, z, euler_angles[0], - # # euler_angles[1], euler_angles[2]] - # plan = plan_hand_motion_br( - # robot=env.robots[0], - # obj_in_hand=None, - # end_conf=end_conf, - # hand_limits=((minx, miny, minz), (maxx, maxy, maxz)), - # obstacles=get_scene_body_ids(env, - # include_self=True, - # include_right_hand=True), - # rng=rng, - # ) - # p.restoreState(state) - # else: - pos = env.robots[0].parts["right_hand"].get_position() - plan = [[pos[0], pos[1], pos[2]] + list( - p.getEulerFromQuaternion( - env.robots[0].parts["right_hand"].get_orientation())), end_conf] + if CFG.behavior_option_model_rrt: + # plan a motion to the pose [x, y, z, euler_angles[0], + # euler_angles[1], euler_angles[2]] + plan = plan_hand_motion_br( + robot=env.robots[0], + obj_in_hand=None, + end_conf=end_conf, + hand_limits=((minx, miny, minz), (maxx, maxy, maxz)), + obstacles=get_relevant_scene_body_ids(env, + include_self=True, + include_right_hand=True), + rng=rng, + ) + p.restoreState(state) + else: + pos = env.robots[0].parts["right_hand"].get_position() + plan = [[pos[0], pos[1], pos[2]] + list( + p.getEulerFromQuaternion( + env.robots[0].parts["right_hand"].get_orientation())), + end_conf] # NOTE: This below line is *VERY* important after the # pybullet state is restored. The hands keep an internal @@ -305,11 +310,12 @@ def make_grasp_plan( env.robots[0].parts["left_hand"].set_position( env.robots[0].parts["left_hand"].get_position()) - # # If RRT planning fails, fail and return None - # if plan is None: - # logging.info(f"PRIMITIVE: grasp {obj.name} fail, failed " - # f"to find plan to continuous params {grasp_offset}") - # return None + # If RRT planning fails, fail and return None + if plan is None: + logging.info(f"PRIMITIVE: grasp {obj.name} fail, failed " + f"to find plan to continuous params {grasp_offset}") + return None + plan = plan + [end_conf] # Grasping Phase 2: Move along the vector from the # position the hand ends up in to the object and @@ -417,17 +423,18 @@ def make_place_plan( if obj.get_body_id() == obj_in_hand_idx ][0] x, y, z = np.add(place_rel_pos, obj.get_position()) - # hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() - - # minx = min(x, hand_x) - 1 - # miny = min(y, hand_y) - 1 - # minz = min(z, hand_z) - 0.5 - # maxx = max(x, hand_x) + 1 - # maxy = max(y, hand_y) + 1 - # maxz = max(z, hand_z) + 0.5 - - obstacles = get_scene_body_ids(env, include_self=False) - obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) + hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() + + minx = min(x, hand_x) - 1 + miny = min(y, hand_y) - 1 + minz = min(z, hand_z) - 0.5 + maxx = max(x, hand_x) + 1 + maxy = max(y, hand_y) + 1 + maxz = max(z, hand_z) + 0.5 + + obstacles = get_relevant_scene_body_ids(env, include_self=False) + if env.robots[0].parts["right_hand"].object_in_hand in obstacles: + obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) end_conf = [ x, y, @@ -437,22 +444,23 @@ def make_place_plan( 0, ] # For now we are not running rrt with place. - # if False: - # plan = plan_hand_motion_br( - # robot=env.robots[0], - # obj_in_hand=obj_in_hand, - # end_conf=end_conf, - # hand_limits=((minx, miny, minz), (maxx, maxy, maxz)), - # obstacles=obstacles, - # rng=rng, - # ) - # p.restoreState(state) - # p.removeState(state) - # else: - pos = env.robots[0].parts["right_hand"].get_position() - plan = [[pos[0], pos[1], pos[2]] + list( - p.getEulerFromQuaternion( - env.robots[0].parts["right_hand"].get_orientation())), end_conf] + if CFG.behavior_option_model_rrt: + plan = plan_hand_motion_br( + robot=env.robots[0], + obj_in_hand=obj_in_hand, + end_conf=end_conf, + hand_limits=((minx, miny, minz), (maxx, maxy, maxz)), + obstacles=obstacles, + rng=rng, + ) + p.restoreState(state) + p.removeState(state) + else: + pos = env.robots[0].parts["right_hand"].get_position() + plan = [[pos[0], pos[1], pos[2]] + list( + p.getEulerFromQuaternion( + env.robots[0].parts["right_hand"].get_orientation())), + end_conf] # NOTE: This below line is *VERY* important after the # pybullet state is restored. The hands keep an internal @@ -464,11 +472,12 @@ def make_place_plan( env.robots[0].parts["left_hand"].set_position( env.robots[0].parts["left_hand"].get_position()) - # # If RRT planning fails, fail and return None - # if plan is None: - # logging.info(f"PRIMITIVE: placeOnTop/inside {obj.name} fail, failed " - # f"to find plan to continuous params {place_rel_pos}") - # return None + # If RRT planning fails, fail and return None + if plan is None: + logging.info(f"PRIMITIVE: placeOnTop/inside {obj.name} fail, failed " + f"to find plan to continuous params {place_rel_pos}") + return None + plan = plan + [end_conf] original_orientation = list( p.getEulerFromQuaternion( diff --git a/predicators/behavior_utils/option_model_fns.py b/predicators/behavior_utils/option_model_fns.py index 82eedfbc02..08eefce5ff 100644 --- a/predicators/behavior_utils/option_model_fns.py +++ b/predicators/behavior_utils/option_model_fns.py @@ -118,12 +118,24 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: "right_hand"].get_position() rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation() + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + env.robots[0].parts["right_hand"].set_position_orientation( + step[0:3], p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + # 1 Teleport Hand to Grasp offset location env.robots[0].parts["right_hand"].set_position_orientation( rh_final_grasp_postion, p.getQuaternionFromEuler(rh_final_grasp_orn)) - # 3. Close hand and simulate grasp + # 1.1 step the environment a few timesteps to update location. + for _ in range(3): + env.step(np.zeros(env.action_space.shape)) + + # 2. Close hand and simulate grasp. a = np.zeros(env.action_space.shape, dtype=float) a[16] = 1.0 assisted_grasp_action = np.zeros(28, dtype=float) @@ -141,11 +153,19 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: env.robots[0].parts["right_hand"].handle_assisted_grasping( assisted_grasp_action, override_ag_data=(grasp_obj_body_id, -1), - bypass_force_check=True) + bypass_force_check=False) # 3.2 step the environment a few timesteps to complete grasp for _ in range(5): env.step(a) + # Simulate Arm Movement (Backwards) + if CFG.behavior_option_model_rrt: + for step in reversed(plan): + env.robots[0].parts["right_hand"].set_position_orientation( + step[0:3], p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + # 4 Move Hand to Original Location env.robots[0].parts["right_hand"].set_position_orientation( rh_orig_grasp_position, rh_orig_grasp_orn) @@ -197,6 +217,14 @@ def placeOntopObjectOptionModel(_init_state: State, f"place ontop {obj_to_place_onto.name} with " f"params {target_pos}") + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + env.robots[0].parts["right_hand"].set_position_orientation( + step[0:3], p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].set_position_orientation( target_pos, p.getQuaternionFromEuler(target_orn)) env.robots[0].parts["right_hand"].force_release_obj() @@ -212,11 +240,20 @@ def placeOntopObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + + # Simulate Arm Movement (Backwards) + if CFG.behavior_option_model_rrt: + for step in reversed(plan): + env.robots[0].parts["right_hand"].set_position_orientation( + step[0:3], p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].set_position_orientation( rh_orig_grasp_position, rh_orig_grasp_orn) # this is running a series of zero action to step simulator # to let the object fall into its place - for _ in range(15): + for _ in range(25): env.step(np.zeros(env.action_space.shape)) # Check whether object is ontop of not a target object objs_under = set() @@ -379,6 +416,17 @@ def placeInsideObjectOptionModel(_init_state: State, f", {plan[-1][3:6]}) and attempting to " + f"place inside to {obj_to_place_into.name} with " f"params {target_pos}") + + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + env.robots[0].parts[ + "right_hand"].set_position_orientation( + step[0:3], + p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].force_release_obj() obj_to_place_into.force_wakeup() obj_in_hand.set_position_orientation( @@ -393,6 +441,16 @@ def placeInsideObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + # Simulate Arm Movement (Backwards) + if CFG.behavior_option_model_rrt: + for step in reversed(plan): + env.robots[0].parts[ + "right_hand"].set_position_orientation( + step[0:3], + p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].set_position_orientation( rh_orig_grasp_position, rh_orig_grasp_orn) # this is running a series of zero action to step @@ -472,6 +530,17 @@ def placeUnderObjectOptionModel(_init_state: State, f", {plan[-1][3:6]}) and attempting to " + f"place under to {obj_to_place_under.name} with " f"params {target_pos}") + + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + env.robots[0].parts[ + "right_hand"].set_position_orientation( + step[0:3], + p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].force_release_obj() obj_to_place_under.force_wakeup() obj_in_hand.set_position_orientation( @@ -486,6 +555,17 @@ def placeUnderObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + + # Simulate Arm Movement (Backwards) + if CFG.behavior_option_model_rrt: + for step in reversed(plan): + env.robots[0].parts[ + "right_hand"].set_position_orientation( + step[0:3], + p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].set_position_orientation( rh_orig_grasp_position, rh_orig_grasp_orn) # this is running a series of zero action to step @@ -595,6 +675,17 @@ def placeNextToObjectOptionModel(_init_state: State, f", {plan[-1][3:6]}) and attempting to " + f"place next to {obj_to_place_nextto.name} with " f"params {target_pos}") + + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + env.robots[0].parts[ + "right_hand"].set_position_orientation( + step[0:3], + p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].force_release_obj() obj_to_place_nextto.force_wakeup() obj_in_hand.set_position_orientation( @@ -609,6 +700,17 @@ def placeNextToObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + + # Simulate Arm Movement (Backwards) + if CFG.behavior_option_model_rrt: + for step in reversed(plan): + env.robots[0].parts[ + "right_hand"].set_position_orientation( + step[0:3], + p.getQuaternionFromEuler(step[3:6])) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + env.robots[0].parts["right_hand"].set_position_orientation( rh_orig_grasp_position, rh_orig_grasp_orn) # this is running a series of zero action to step diff --git a/predicators/envs/behavior.py b/predicators/envs/behavior.py index 82ef068d54..68cdcbb1f5 100644 --- a/predicators/envs/behavior.py +++ b/predicators/envs/behavior.py @@ -775,7 +775,7 @@ def _reachable_classifier(self, return False return (np.linalg.norm( # type: ignore np.array(robot_obj.get_position()) - - np.array(ig_obj.get_position())) < 2) + np.array(ig_obj.get_position())) < CFG.behavior_closeness_limit) def _reachable_nothing_classifier( self, diff --git a/predicators/main.py b/predicators/main.py index 65755b0f2a..b6313b7d49 100644 --- a/predicators/main.py +++ b/predicators/main.py @@ -34,6 +34,7 @@ --seed 0 --excluded_predicates all """ +import curses import logging import os import sys @@ -52,7 +53,7 @@ BilevelPlanningApproach from predicators.approaches.gnn_approach import GNNApproach from predicators.datasets import create_dataset -from predicators.envs import BaseEnv, create_new_env +from predicators.envs import BaseEnv, create_new_env, get_or_create_env from predicators.envs.behavior import BehaviorEnv from predicators.planning import _run_plan_with_option_model from predicators.settings import CFG @@ -114,13 +115,11 @@ def main() -> None: # Create the agent (approach). approach = create_approach(CFG.approach, preds, options, env.types, env.action_space, stripped_train_tasks) - if approach.is_learning_based: - # Create the offline dataset. Note that this needs to be done using - # the non-stripped train tasks because dataset generation may need - # to use the oracle predicates (e.g. demo data generation). - offline_dataset = create_dataset(env, train_tasks, options) - else: - offline_dataset = None + # Create the offline dataset. Note that this needs to be done using + # the non-stripped train tasks because dataset generation may need + # to use the oracle predicates (e.g. demo data generation). + offline_dataset = create_dataset(env, train_tasks, options) + # Run the full pipeline. _run_pipeline(env, approach, stripped_train_tasks, offline_dataset) script_time = time.perf_counter() - script_start @@ -343,6 +342,24 @@ def _run_testing(env: BaseEnv, approach: BaseApproach) -> Metrics: last_plan = approach.get_last_plan() last_traj = approach.get_last_traj() option_model_start_time = time.time() + if CFG.env == "behavior" and \ + CFG.behavior_mode == 'iggui': # pragma: no cover + env = get_or_create_env('behavior') + assert isinstance(env, BehaviorEnv) + win = curses.initscr() + win.nodelay(True) + win.addstr( + 0, 0, + "VIDEO CREATION MODE: You have time to position the \ + iggui window to the location you want for recording. \ + Type 'q' to indicate you have finished positioning: ") + flag = win.getch() + while flag == -1 or chr(flag) != 'q': + env.igibson_behavior_env.step( + np.zeros(env.action_space.shape)) + flag = win.getch() + curses.endwin() + logging.info("VIDEO CREATION MODE: Starting planning.") traj, solved = _run_plan_with_option_model( task, test_task_idx, approach.get_option_model(), last_plan, last_traj) diff --git a/predicators/planning.py b/predicators/planning.py index 20a7773607..4b4ce478c6 100644 --- a/predicators/planning.py +++ b/predicators/planning.py @@ -77,7 +77,8 @@ def sesame_plan( """ if CFG.env == "behavior" and \ - CFG.behavior_mode == 'iggui': # pragma: no cover + CFG.behavior_mode == 'iggui' and \ + CFG.plan_only_eval: # pragma: no cover env = get_or_create_env('behavior') assert isinstance(env, BehaviorEnv) win = curses.initscr() @@ -581,10 +582,10 @@ def run_low_level_search( if cur_idx == len(skeleton): return plan, True, traj # success! else: - # logging.info("Failure: Expected Atoms Check Failed.") - # for a in expected_atoms: - # if not a.holds(traj[cur_idx]): - # logging.info(a) + logging.info("Failure: Expected Atoms Check Failed.") + for a in expected_atoms: + if not a.holds(traj[cur_idx]): + logging.info(a) can_continue_on = False else: # If we're not checking expected_atoms, we need to @@ -1011,7 +1012,9 @@ def _sesame_plan_with_fast_downward( potentially add effects to null operators, but this ability is not implemented here. """ - init_atoms = utils.abstract(task.init, predicates) + init_atoms = utils.abstract(task.init, + predicates, + skip_allclose_check=True) objects = list(task.init) start_time = time.perf_counter() # Create the domain and problem strings, then write them to tempfiles. diff --git a/predicators/settings.py b/predicators/settings.py index 1c5118e6d3..1869da78c7 100644 --- a/predicators/settings.py +++ b/predicators/settings.py @@ -130,6 +130,7 @@ class GlobalSettings: behavior_option_model_eval = False behavior_option_model_rrt = False behavior_override_learned_samplers = False + behavior_closeness_limit = 1.00 # if this is True, then we will not use discovered failures in behavior. behavior_ignore_discover_failures = True create_training_dataset = False