diff --git a/predicators/behavior_utils/behavior_utils.py b/predicators/behavior_utils/behavior_utils.py index 5af4fdbf0f..25530e510b 100644 --- a/predicators/behavior_utils/behavior_utils.py +++ b/predicators/behavior_utils/behavior_utils.py @@ -855,6 +855,40 @@ 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 diff --git a/predicators/behavior_utils/motion_planner_fns.py b/predicators/behavior_utils/motion_planner_fns.py index 5bc318a572..2e20298dc4 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 @@ -124,8 +124,8 @@ 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) plan = plan_base_motion_br( @@ -148,6 +148,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) @@ -284,7 +285,7 @@ def make_grasp_plan( obj_in_hand=None, end_conf=end_conf, hand_limits=((minx, miny, minz), (maxx, maxy, maxz)), - obstacles=get_scene_body_ids(env, + obstacles=get_relevant_scene_body_ids(env, include_self=True, include_right_hand=True), rng=rng, @@ -311,6 +312,7 @@ def make_grasp_plan( 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 @@ -427,7 +429,7 @@ def make_place_plan( maxy = max(y, hand_y) + 1 maxz = max(z, hand_z) + 0.5 - obstacles = get_scene_body_ids(env, include_self=False) + obstacles = get_relevant_scene_body_ids(env, include_self=False) obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) end_conf = [ x, @@ -470,6 +472,7 @@ def make_place_plan( 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 979fe57d83..2aff6d3a72 100644 --- a/predicators/behavior_utils/option_model_fns.py +++ b/predicators/behavior_utils/option_model_fns.py @@ -118,6 +118,14 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: "right_hand"].get_position() rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation() + # TODO 0 Simulate Arm Movement + 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, @@ -150,6 +158,14 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: for _ in range(5): env.step(a) + # TODO 0 Simulate Arm Movement (Backwards) + 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) @@ -201,6 +217,14 @@ def placeOntopObjectOptionModel(_init_state: State, f"place ontop {obj_to_place_onto.name} with " f"params {target_pos}") + # TODO 0 Simulate Arm Movement + 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() @@ -216,6 +240,15 @@ def placeOntopObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + + # TODO 0 Simulate Arm Movement (Backwards) + 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 @@ -383,6 +416,15 @@ 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}") + + # TODO 0 Simulate Arm Movement + 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( @@ -397,6 +439,14 @@ def placeInsideObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + # TODO 0 Simulate Arm Movement (Backwards) + 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 @@ -476,6 +526,15 @@ 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}") + + # TODO 0 Simulate Arm Movement + 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( @@ -490,6 +549,15 @@ def placeUnderObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + + # TODO 0 Simulate Arm Movement (Backwards) + 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 @@ -599,6 +667,15 @@ 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}") + + # TODO 0 Simulate Arm Movement + 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( @@ -613,6 +690,15 @@ def placeNextToObjectOptionModel(_init_state: State, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) + + # TODO 0 Simulate Arm Movement (Backwards) + 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/planning.py b/predicators/planning.py index 05ce01d359..a9b3d3e8f0 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