From d7f6b975245be4382e0c2cd33cd292327770cb2e Mon Sep 17 00:00:00 2001 From: Jorge Date: Wed, 20 Dec 2023 09:54:17 -0500 Subject: [PATCH] Fetch improvements (rrt, joint-space grasp/place, closest point from gripper) --- predicators/behavior_utils/behavior_utils.py | 317 ++++++++-- .../behavior_utils/motion_planner_fns.py | 402 +++++++++---- .../behavior_utils/option_model_fns.py | 544 ++++++++++++------ 3 files changed, 915 insertions(+), 348 deletions(-) diff --git a/predicators/behavior_utils/behavior_utils.py b/predicators/behavior_utils/behavior_utils.py index afb354c6a8..0b88cb61aa 100644 --- a/predicators/behavior_utils/behavior_utils.py +++ b/predicators/behavior_utils/behavior_utils.py @@ -13,6 +13,7 @@ from predicators.settings import CFG from predicators.structs import Array, GroundAtom, GroundAtomTrajectory, \ LowLevelTrajectory, Predicate, Set, State, Task +from predicators.mpi_utils import proc_id try: from igibson import object_states @@ -836,7 +837,7 @@ def get_aabb_centroid(lo: Array, hi: Array) -> List[float]: return [(hi[0] + lo[0]) / 2, (hi[1] + lo[1]) / 2, (hi[2] + lo[2]) / 2] -def get_scene_body_ids( +def get_relevant_scene_body_ids( env: "BehaviorEnv", include_self: bool = False, include_right_hand: bool = False, @@ -851,7 +852,9 @@ def get_scene_body_ids( # will never practically collide with it, but if we include it # in collision checking, we always seem to collide. if obj.name != "floors": - ids.extend(obj.body_ids) + # 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()) @@ -860,6 +863,12 @@ def get_scene_body_ids( 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 @@ -1087,12 +1096,9 @@ def check_nav_end_pose( return valid_position, status -def get_valid_orientation(env: "BehaviorEnv", obj: Union["URDFObject", - "RoomFloor"]) -> Tuple[float]: +def get_valid_orientation(env: "BehaviorEnv", position: List[float]) -> Tuple[float]: state = p.saveState() - obj_aabb = obj.states[object_states.AABB].get_value() - obj_closest_point = get_closest_point_on_aabb(env.robots[0].get_position(), obj_aabb[0], obj_aabb[1]) - ik_success, orn = env.robots[0].set_eef_position(obj_closest_point) + ik_success, orn = env.robots[0].set_eef_position(position) p.restoreState(state) p.removeState(state) return ik_success, orn @@ -1108,7 +1114,8 @@ def check_hand_end_pose(env: "BehaviorEnv", obj: Union["URDFObject", ret_bool = False state = p.saveState() obj_aabb = obj.states[object_states.AABB].get_value() - obj_closest_point = get_closest_point_on_aabb(env.robots[0].get_position(), obj_aabb[0], obj_aabb[1]) + # obj_closest_point = get_closest_point_on_aabb(env.robots[0].get_position(), obj_aabb[0], obj_aabb[1]) + obj_closest_point = get_closest_point_on_aabb(env.robots[0].get_end_effector_position(), obj_aabb[0], obj_aabb[1]) hand_pos = ( pos_offset[0] + obj_closest_point[0], @@ -1160,14 +1167,19 @@ def check_hand_end_pose(env: "BehaviorEnv", obj: Union["URDFObject", def sample_navigation_params(igibson_behavior_env: "BehaviorEnv", obj_to_sample_near: "URDFObject", - rng: np.random.Generator) -> Array: + rng: np.random.Generator, + max_internal_samples=None, + return_failed_samples=False, + return_distribution_samples=False) -> Array: """Main logic for navigation param sampler. Implemented in a separate method to enable code reuse in option_model_fns. """ - closeness_limit = 2.00# if isinstance(env.igibson_behavior_env.robots[0], BehaviorRobot) else 0.8 - nearness_limit = 0.15# if isinstance(env.igibson_behavior_env.robots[0], BehaviorRobot) else 0.3 + if max_internal_samples is None: + max_internal_samples = MAX_NAVIGATION_SAMPLES + closeness_limit = 2.00 + nearness_limit = 0.15 distance = nearness_limit + ( (closeness_limit - nearness_limit) * rng.random()) # NOTE: In a previous version, we attempted to sample at a distance d from @@ -1178,13 +1190,24 @@ def sample_navigation_params(igibson_behavior_env: "BehaviorEnv", x = distance * np.cos(yaw) y = distance * np.sin(yaw) sampler_output = np.array([x, y]) + if return_distribution_samples: + distance = nearness_limit + ( + (closeness_limit - nearness_limit) * rng.random(size=CFG.behavior_distribution_viz_num_samples)) + yaw = rng.random(size=CFG.behavior_distribution_viz_num_samples) + x = distance * np.cos(yaw) + y = distance * np.sin(yaw) + distribution_samples = np.array([x, y]) # The below while loop avoids sampling values that would put the # robot in collision with some object in the environment. It may # not always succeed at this and will exit after a certain number # of tries. - num_samples_tried = 0 + num_samples_tried = 1 + if return_failed_samples: + failed_samples = [] while (check_nav_end_pose(igibson_behavior_env, obj_to_sample_near, - sampler_output)[0] is None): + sampler_output) is None): + if return_failed_samples: + failed_samples.append(sampler_output) distance = closeness_limit * rng.random() yaw = rng.random() * (2 * np.pi) - np.pi x = distance * np.cos(yaw) @@ -1194,19 +1217,32 @@ def sample_navigation_params(igibson_behavior_env: "BehaviorEnv", if check_nav_end_pose(igibson_behavior_env, obj_to_sample_near, sampler_output, - ignore_blocked=True)[0]: - return sampler_output + ignore_blocked=True): + ret_tuple = [sampler_output, num_samples_tried] + if return_failed_samples: + ret_tuple.append(failed_samples) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) # NOTE: In many situations, it is impossible to find a good sample # no matter how many times we try. Thus, we break this loop after # a certain number of tries so the planner will backtrack. - if num_samples_tried > MAX_NAVIGATION_SAMPLES: + if num_samples_tried > max_internal_samples: break num_samples_tried += 1 - return sampler_output + ret_tuple = [sampler_output, num_samples_tried] + if return_failed_samples: + ret_tuple.append(failed_samples) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) def sample_place_inside_params(obj_to_place_inside: "URDFObject", - rng: np.random.Generator) -> Array: + rng: np.random.Generator, + max_internal_samples=None, + return_failed_samples=False, + return_distribution_samples=False) -> Array: """Main logic for place inside param sampler. Implemented in a separate method to enable code reuse in @@ -1231,7 +1267,24 @@ def sample_place_inside_params(obj_to_place_inside: "URDFObject", rng.uniform(objB_sampling_bounds[2] + 0.15, objB_sampling_bounds[2] + 0.5) ]) - return sample_params + if return_distribution_samples: + distribution_samples = np.array([ + rng.uniform(-objB_sampling_bounds[0] / 2, + objB_sampling_bounds[0] / 2, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-objB_sampling_bounds[1] / 2, + objB_sampling_bounds[1] / 2, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(objB_sampling_bounds[2] + 0.15, + objB_sampling_bounds[2] + 0.5, + size=CFG.behavior_distribution_viz_num_samples) + ]) + ret_tuple = [sample_params, 1] + if return_failed_samples: + ret_tuple.append([]) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) if obj_to_place_inside.category == "trash_can": objB_sampling_bounds = obj_to_place_inside.bounding_box / 2 # Since the trash can's hole is generally in the center, @@ -1246,14 +1299,45 @@ def sample_place_inside_params(obj_to_place_inside: "URDFObject", rng.uniform(objB_sampling_bounds[2] + 0.05, objB_sampling_bounds[2] + 0.15) ]) - return sample_params + if return_distribution_samples: + distribution__params = np.array([ + rng.uniform(-objB_sampling_bounds[0] / 4, + objB_sampling_bounds[0] / 4, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-objB_sampling_bounds[1] / 4, + objB_sampling_bounds[1] / 4, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(objB_sampling_bounds[2] + 0.05, + objB_sampling_bounds[2] + 0.15, + size=CFG.behavior_distribution_viz_num_samples) + ]) + ret_tuple = (sample_params, 1) + if return_failed_samples: + ret_tuple.append([]) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) # If there's no object specific sampler, just return a # random sample. - return np.array([ - rng.uniform(-0.5, 0.5), - rng.uniform(-0.5, 0.5), - rng.uniform(0.3, 1.0) - ]) + if return_distribution_samples: + distribution_samples = np.array([ + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(0.3, 1.0, + size=CFG.behavior_distribution_viz_num_samples) + ]) + ret_tuple = [np.array([ + rng.uniform(-0.5, 0.5), + rng.uniform(-0.5, 0.5), + rng.uniform(0.3, 1.0) + ]), 1] + if return_failed_samples: + ret_tuple.append([]) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) MAX_PLACEONTOP_SAMPLES = 25 @@ -1261,7 +1345,10 @@ def sample_place_inside_params(obj_to_place_inside: "URDFObject", def sample_place_ontop_params(igibson_behavior_env: "BehaviorEnv", obj_to_place_ontop: "URDFObject", - rng: np.random.Generator) -> Array: + rng: np.random.Generator, + max_internal_samples=None, + return_failed_samples=False, + return_distribution_samples=False) -> Array: """Main logic for place ontop param sampler. Implemented in a separate method to enable code reuse in @@ -1269,6 +1356,8 @@ def sample_place_ontop_params(igibson_behavior_env: "BehaviorEnv", """ # If sampling fails, fall back onto custom-defined object-specific # samplers + if max_internal_samples is None: + max_internal_samples = MAX_PLACEONTOP_SAMPLES if obj_to_place_ontop.category == "shelf": # Get the current env for collision checking. obj_to_place_ontop_sampling_bounds = obj_to_place_ontop.bounding_box / 2 @@ -1280,14 +1369,26 @@ def sample_place_ontop_params(igibson_behavior_env: "BehaviorEnv", rng.uniform(-obj_to_place_ontop_sampling_bounds[2] + 0.3, obj_to_place_ontop_sampling_bounds[2]) + 0.3 ]) - # NOTE: In a previous implementation, we used to check the distance - # to the sampled point for the Fetch, because if the object to place - # on is very large, many samples might fail. This might be overkill, - # so we're removing it for now. - logging.info("Sampling params for placeOnTop shelf...") - num_samples_tried = 0 + if return_distribution_samples: + distribution_samples = np.array([ + rng.uniform(-obj_to_place_ontop_sampling_bounds[0], + obj_to_place_ontop_sampling_bounds[0], + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-obj_to_place_ontop_sampling_bounds[1], + obj_to_place_ontop_sampling_bounds[1], + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-obj_to_place_ontop_sampling_bounds[2] + 0.3, + obj_to_place_ontop_sampling_bounds[2], + size=CFG.behavior_distribution_viz_num_samples) + 0.3 + ]) + # logging.info("Sampling params for placeOnTop shelf...") + num_samples_tried = 1 + if return_failed_samples: + failed_samples = [] while not check_hand_end_pose(igibson_behavior_env, obj_to_place_ontop, sample_params): + if return_failed_samples: + failed_samples.append(sample_params) sample_params = np.array([ rng.uniform(-obj_to_place_ontop_sampling_bounds[0], obj_to_place_ontop_sampling_bounds[0]), @@ -1300,29 +1401,52 @@ def sample_place_ontop_params(igibson_behavior_env: "BehaviorEnv", # good sample no matter how many times we try. Thus, we # break this loop after a certain number of tries so the # planner will backtrack. - if num_samples_tried > MAX_PLACEONTOP_SAMPLES: + if num_samples_tried > max_internal_samples: break num_samples_tried += 1 - return sample_params + ret_tuple = [sample_params, num_samples_tried] + if return_failed_samples: + ret_tuple.append(failed_samples) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) # If there's no object specific sampler, just return a # random sample. - return np.array([ - rng.uniform(-0.5, 0.5), - rng.uniform(-0.5, 0.5), - rng.uniform(0.3, 1.0) - ]) + if return_distribution_samples: + distribution_samples = np.array([ + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(0.3, 1.0, + size=CFG.behavior_distribution_viz_num_samples) + ]) + ret_tuple = [np.array([ + rng.uniform(-0.5, 0.5), + rng.uniform(-0.5, 0.5), + rng.uniform(0.3, 1.0) + ]), 1] + if return_failed_samples: + ret_tuple.append([]) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) def sample_place_next_to_params(igibson_behavior_env: "BehaviorEnv", obj_to_place_nextto: "URDFObject", - rng: np.random.Generator) -> Array: + rng: np.random.Generator, + max_internal_samples=None, + return_failed_samples=False, + return_distribution_samples=False) -> Array: """Main logic for place next to param sampler. Implemented in a separate method to enable code reuse in option_model_fns. """ - + if max_internal_samples is None: + max_internal_samples = MAX_PLACEONTOP_SAMPLES if obj_to_place_nextto.category == "toilet": # Get the current env for collision checking. obj_to_place_nextto_sampling_bounds = \ @@ -1341,12 +1465,31 @@ def sample_place_next_to_params(igibson_behavior_env: "BehaviorEnv", rng.uniform(-obj_to_place_nextto_sampling_bounds[2], obj_to_place_nextto_sampling_bounds[2]) ]) + if return_distribution_samples: + x_location = rng.uniform(-obj_to_place_nextto_sampling_bounds[0], + obj_to_place_nextto_sampling_bounds[0], + size=CFG.behavior_distribution_viz_num_samples) + x_location[x_location < 0] -= obj_to_place_nextto_sampling_bounds[0] + x_location[~(x_location < 0)] += obj_to_place_nextto_sampling_bounds[0] + distribution_samples = np.array([ + x_location, + rng.uniform(-obj_to_place_nextto_sampling_bounds[1], + obj_to_place_nextto_sampling_bounds[1], + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-obj_to_place_nextto_sampling_bounds[2], + obj_to_place_nextto_sampling_bounds[2], + size=CFG.behavior_distribution_viz_num_samples) + ]) - logging.info("Sampling params for placeNextTo table...") + # logging.info("Sampling params for placeNextTo table...") - num_samples_tried = 0 + num_samples_tried = 1 + if return_failed_samples: + failed_samples = [] while not check_hand_end_pose(igibson_behavior_env, obj_to_place_nextto, sample_params): + if return_failed_samples: + failed_samples.append(sample_params) x_location = rng.uniform(-obj_to_place_nextto_sampling_bounds[0], obj_to_place_nextto_sampling_bounds[0]) if x_location < 0: @@ -1365,27 +1508,50 @@ def sample_place_next_to_params(igibson_behavior_env: "BehaviorEnv", # good sample no matter how many times we try. Thus, we # break this loop after a certain number of tries so the # planner will backtrack. - if num_samples_tried > MAX_PLACEONTOP_SAMPLES: + if num_samples_tried > max_internal_samples: break num_samples_tried += 1 - return sample_params + ret_tuple = [sample_params, num_samples_tried] + if return_failed_samples: + ret_tuple.append(failed_samples) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) sample_params = np.array([ rng.uniform(-0.5, 0.5), rng.uniform(-0.5, 0.5), rng.uniform(0.3, 1.0) ]) - return sample_params - + if return_distribution_samples: + distribution_samples = np.array([ + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(0.3, 1.0, + size=CFG.behavior_distribution_viz_num_samples) + ]) + ret_tuple = [sample_params, 1] + if return_failed_samples: + ret_tuple.append(failed_samples) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) def sample_place_under_params(igibson_behavior_env: "BehaviorEnv", obj_to_place_under: "URDFObject", - rng: np.random.Generator) -> Array: + rng: np.random.Generator, + max_internal_samples=None, + return_failed_samples=False, + return_distribution_samples=False) -> Array: """Main logic for place under param sampler. Implemented in a separate method to enable code reuse in option_model_fns. """ + if max_internal_samples is None: + max_internal_samples = MAX_PLACEONTOP_SAMPLES if obj_to_place_under.category == "coffee_table": # Get the current env for collision checking. obj_to_place_under_sampling_bounds = obj_to_place_under.bounding_box / 2 @@ -1397,12 +1563,28 @@ def sample_place_under_params(igibson_behavior_env: "BehaviorEnv", rng.uniform(-obj_to_place_under_sampling_bounds[2] - 0.3, -obj_to_place_under_sampling_bounds[2]) ]) + if return_distribution_samples: + sample_params = np.array([ + rng.uniform(-obj_to_place_under_sampling_bounds[0], + obj_to_place_under_sampling_bounds[0], + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-obj_to_place_under_sampling_bounds[1], + obj_to_place_under_sampling_bounds[1], + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-obj_to_place_under_sampling_bounds[2] - 0.3, + -obj_to_place_under_sampling_bounds[2], + size=CFG.behavior_distribution_viz_num_samples) + ]) - logging.info("Sampling params for placeUnder table...") + # logging.info("Sampling params for placeUnder table...") - num_samples_tried = 0 + num_samples_tried = 1 + if return_failed_samples: + failed_samples = [] while not check_hand_end_pose(igibson_behavior_env, obj_to_place_under, sample_params): + if return_failed_samples: + failed_samples.append(sample_params) sample_params = np.array([ rng.uniform(-obj_to_place_under_sampling_bounds[0], obj_to_place_under_sampling_bounds[0]), @@ -1415,18 +1597,37 @@ def sample_place_under_params(igibson_behavior_env: "BehaviorEnv", # good sample no matter how many times we try. Thus, we # break this loop after a certain number of tries so the # planner will backtrack. - if num_samples_tried > MAX_PLACEONTOP_SAMPLES: + if num_samples_tried > max_internal_samples: break num_samples_tried += 1 - return sample_params + ret_tuple = [sample_params, num_samples_tried] + if return_failed_samples: + ret_tuple.append(failed_samples) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) # If there's no object specific sampler, just return a # random sample. - return np.array([ + if return_distribution_samples: + distribution_samples = np.array([ + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(-0.5, 0.5, + size=CFG.behavior_distribution_viz_num_samples), + rng.uniform(0.3, 1.0, + size=CFG.behavior_distribution_viz_num_samples) + ]) + ret_tuple = [np.array([ rng.uniform(-0.5, 0.5), rng.uniform(-0.5, 0.5), rng.uniform(0.3, 1.0) - ]) + ]), 1] + if return_failed_samples: + ret_tuple.append([]) + if return_distribution_samples: + ret_tuple.append(distribution_samples) + return tuple(ret_tuple) def load_checkpoint_state(s: State, @@ -1480,12 +1681,14 @@ def load_checkpoint_state(s: State, # tasks must have a task num below 10 and test tasks must have one # above 10. This is sketchy in general and we should probably come # up with something cleaner! - if env.task_instance_id < 10: + if len(CFG.behavior_task_list) != 1: + scene_name = env.scene_list[env.task_num] + elif env.task_instance_id < 10: scene_name = CFG.behavior_train_scene_name else: scene_name = CFG.behavior_test_scene_name checkpoint_file_str = ( - f"tmp_behavior_states/{scene_name}__" + + f"{CFG.tmp_dir}/tmp_behavior_states/mpi_{proc_id()}/{scene_name}__" + f"{behavior_task_name}__{CFG.num_train_tasks}__" + f"{CFG.seed}__{env.task_num}__{env.task_instance_id}") frame_num = int(s.simulator_state.split("-")[2]) @@ -1493,7 +1696,7 @@ def load_checkpoint_state(s: State, load_checkpoint(env.igibson_behavior_env.simulator, checkpoint_file_str, frame_num) except p.error as _: - print(f"tmp_behavior_states_dir: {os.listdir(checkpoint_file_str)}") + print(f"{CFG.tmp_dir}/tmp_behavior_states_dir: {os.listdir(checkpoint_file_str)}") raise ValueError( f"Could not load pybullet state for {checkpoint_file_str}, " + f"frame {frame_num}") diff --git a/predicators/behavior_utils/motion_planner_fns.py b/predicators/behavior_utils/motion_planner_fns.py index a06fc53118..f13bd44d27 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, get_aabb_centroid, get_valid_orientation from predicators.settings import CFG from predicators.structs import Array @@ -27,8 +27,10 @@ BehaviorRobot # pylint: disable=unused-import from igibson.utils.behavior_robot_planning_utils import \ plan_base_motion_br, plan_hand_motion_br + from igibson.utils.fetch_gripper_planning_utils import \ + plan_base_motion_fg, plan_gripper_motion_fg from igibson.external.pybullet_tools.utils import get_link_pose, get_aabb_center, \ - get_aabb_extent + get_aabb_extent, get_joint_positions import igibson.utils.transform_utils as T except (ImportError, ModuleNotFoundError) as e: pass @@ -38,7 +40,8 @@ def make_dummy_plan( env: "BehaviorEnv", obj: Union["URDFObject", "RoomFloor"], continuous_params: Array, - rng: Optional[Generator] = None + rng: Optional[Generator] = None, + distribution_samples: Optional[Array] = None, ) -> Optional[Tuple[List[List[float]], List[List[float]]]]: """Function to return a defualt 'dummy' plan. @@ -54,7 +57,8 @@ def make_navigation_plan( env: "BehaviorEnv", obj: Union["URDFObject", "RoomFloor"], pos_offset: Array, - rng: Optional[Generator] = None + rng: Optional[Generator] = None, + distribution_samples: Optional[Array] = None, ) -> Optional[Tuple[List[List[float]], List[List[float]]]]: """Function to return a series of actions to navigate to a particular offset from an object's position. @@ -67,6 +71,18 @@ def make_navigation_plan( or might not run RRT to find the plan. If it does not run RRT, the plan will only be one step (i.e, the final pose to be navigated to). """ + if distribution_samples is not None: + print("Motion planner nav, distr shape:", distribution_samples.shape) + obj_pos = obj.get_position() + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + robot_z = env.robots[0].get_position()[2] + z = np.ones_like(x) * robot_z + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) + env.step(np.zeros(env.action_space.shape)) + + if rng is None: rng = np.random.default_rng(23) @@ -137,16 +153,23 @@ def sample_fn(env: "BehaviorEnv", valid_position[1][2], ] if env.use_rrt: - obstacles = get_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( + obstacles = get_relevant_scene_body_ids(env) + if isinstance(env.robots[0], BehaviorRobot): + base_motion_planner_fn = plan_base_motion_br + if env.robots[0].parts["right_hand"].object_in_hand in obstacles: + obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand) + else: + base_motion_planner_fn = plan_base_motion_fg + if env.robots[0].object_in_hand in obstacles: + obstacles.remove(env.robots[0].object_in_hand) + plan = base_motion_planner_fn( robot=env.robots[0], end_conf=end_conf, base_limits=(), obstacles=obstacles, override_sample_fn=lambda: sample_fn(env, rng), rng=rng, + max_distance=0.01 ) p.restoreState(state) else: @@ -174,6 +197,7 @@ def make_grasp_plan( obj: Union["URDFObject", "RoomFloor"], grasp_offset: Array, rng: Optional[Generator] = None, + distribution_samples: Optional[Array] = None, ) -> Optional[Tuple[List[List[float]], List[List[float]]]]: """Function to return a series of actions to grasp an object at a particular offset from an object's position. @@ -188,6 +212,16 @@ def make_grasp_plan( will only be one step (i.e, the pose to move the hand to to try grasping the object). """ + if distribution_samples is not None: + obj_pos = obj.get_position() + print("Motion planner grasp, distr shape:", distribution_samples.shape) + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = obj_pos[2] + distribution_samples[2] + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) + env.step(np.zeros(env.action_space.shape)) + if rng is None: rng = np.random.default_rng(23) @@ -234,15 +268,34 @@ def make_grasp_plan( # try to create a plan to it. if isinstance(env.robots[0], BehaviorRobot): obj_pos = obj.get_position() + hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() + obstacles = get_relevant_scene_body_ids(env, + include_self=True, + include_right_hand=True) else: # aabb = obj.states[object_states.AABB].get_value() # obj_pos = get_closest_point_on_aabb(env.robots[0].get_position(), aabb[0], aabb[1]) - points = p.getClosestPoints(env.robots[0].get_body_id(), obj.get_body_id(), distance=10)#, linkIndexA=env.robots[0].grasp_point_joint_id) + points = p.getClosestPoints(env.robots[0].get_body_id(), obj.get_body_id(), distance=10, linkIndexA=env.robots[0].parts["gripper_link"].body_part_index) closest_point = min(points, key=lambda x:x[8]) obj_pos = closest_point[6] + robot_position_closest_to_obj = closest_point[5] + delta = (np.array(obj_pos) - np.array(robot_position_closest_to_obj)) + obj_pos = tuple(delta + delta / np.linalg.norm(delta) * .01 + np.array(robot_position_closest_to_obj)) # Add 1 cm in the same direction + + hand_x, hand_y, hand_z = env.robots[0].get_end_effector_position() + obstacles = get_relevant_scene_body_ids(env) + if env.robots[0].object_in_hand in obstacles: + obstacles.remove(env.robots[0].object_in_hand) + obstacles.append((env.robots[0].body_id, (env.robots[0].parts["base_link"].body_part_index, env.robots[0].parts["torso_lift_link"].body_part_index))) x = obj_pos[0] + grasp_offset[0] y = obj_pos[1] + grasp_offset[1] z = obj_pos[2] + grasp_offset[2] + 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 if isinstance(env.robots[0], BehaviorRobot): @@ -281,59 +334,84 @@ def make_grasp_plan( euler_angles = np.zeros(3, dtype=float) else: euler_angles = np.array([0.0, np.pi, 0.0]) + end_conf = [ + x, + y, + z, + euler_angles[0], + euler_angles[1], + euler_angles[2], + ] else: # For Fetch, we might not be able to get the gripper to get # align the gripper with the grasp trajectory. Instead, we # get any valid orientation for reaching the object's bbox # and we use that throughout the trajectory - from predicators.envs import get_or_create_env - ig_env = get_or_create_env("behavior").igibson_behavior_env - ik_success, orn = get_valid_orientation(ig_env, obj) + # from predicators.envs import get_or_create_env + # ig_env = get_or_create_env("behavior").igibson_behavior_env + # obj_aabb = obj.states[object_states.AABB].get_value() + obj_closest_point = obj_pos # this is already the closest point on the actual object #get_closest_point_on_aabb(env.robots[0].get_position(), obj_aabb[0], obj_aabb[1]) + ik_success, closest_point_orn = get_valid_orientation(env, obj_closest_point) + # ik_success, closest_point_orn = get_valid_orientation(env, [x, y, z]) if not ik_success: logging.info(f"PRIMITIVE: grasp {obj.name} fail, no valid IK") # logging.info(f"\tFailed to grasp object at {obj.get_position()} from {ig_env.robots[0].get_position()}") return None - logging.info(f"PRIMITIVE: grasp orientation {orn}") - euler_angles = p.getEulerFromQuaternion(orn) + logging.info(f"PRIMITIVE: grasp orientation {closest_point_orn}") + euler_angles = p.getEulerFromQuaternion(closest_point_orn) + # joint_pos = env.robots[0].calculate_eef_ik([x, y, z], closest_point_orn) + joint_pos = env.robots[0].calculate_eef_ik(obj_closest_point, closest_point_orn) + # assert joint_pos is not None + if joint_pos is None: + logging.info(f"PRIMITIVE: grasp {obj.name} fail, no valid IK on second call (l 326)") + return None + # end_conf = [x, y, z] + euler_angles + joint_pos # pos and orn for validation + end_conf = joint_pos state = p.saveState() - end_conf = [ - x, - y, - z, - euler_angles[0], - euler_angles[1], - 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]] - # if isinstance(env.robots[0], BehaviorRobot): - # 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, - # ) - # else: - # raise NotImplementedError('RRT hand motion planning is only implemented for the Behavior bot') - # p.restoreState(state) - # else: - if isinstance(env.robots[0], BehaviorRobot): - 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 env.use_rrt: + # plan a motion to the pose [x, y, z, euler_angles[0], + # euler_angles[1], euler_angles[2]] + if isinstance(env.robots[0], BehaviorRobot): + 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=obstacles, + rng=rng, + ) + else: + plan = plan_gripper_motion_fg( + robot=env.robots[0], + obj_in_hand=None, + end_conf=end_conf, + joint_limits=(env.robots[0].lower_joint_limits, + env.robots[0].upper_joint_limits), + obstacles=obstacles, + rng=rng, + ) + p.restoreState(state) + # 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] else: - pos = env.robots[0].get_end_effector_position() - orn = list(p.getEulerFromQuaternion( - T.mat2quat(T.pose2mat(get_link_pose(env.robots[0].robot_ids[0], env.robots[0].eef_link_id))[:3, :3]))) - plan = [[pos[0], pos[1], pos[2], orn[0], orn[1], orn[2]], end_conf] + if isinstance(env.robots[0], BehaviorRobot): + 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] + else: + pos = env.robots[0].get_end_effector_position() + orn = list(p.getEulerFromQuaternion( + T.mat2quat(T.pose2mat(get_link_pose(env.robots[0].robot_ids[0], env.robots[0].eef_link_id))[:3, :3]))) + orig_joint_positions = get_joint_positions(env.robots[0].robot_ids[0], env.robots[0].joint_ids) + # plan = [pos + orn + orig_joint_positions, end_conf] + plan = [orig_joint_positions, end_conf] # NOTE: This below line is *VERY* important after the # pybullet state is restored. The hands keep an internal @@ -346,43 +424,74 @@ 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 - # Grasping Phase 2: Move along the vector from the # position the hand ends up in to the object and # then try to grasp. - hand_pos = plan[-1][0:3] - hand_orn = plan[-1][3:6] - # Get the closest point on the object's bounding - # box at which we can try to put the hand if isinstance(env.robots[0], BehaviorRobot): + hand_pos = plan[-1][0:3] + hand_orn = plan[-1][3:6] + # Get the closest point on the object's bounding + # box at which we can try to put the hand closest_point_on_aabb = get_closest_point_on_aabb(hand_pos, lo, hi) - else: - closest_point_on_aabb = obj_pos # Already got this before, but for contact instead of bbox - delta_pos_to_obj = [ - closest_point_on_aabb[0] - hand_pos[0], - closest_point_on_aabb[1] - hand_pos[1], - closest_point_on_aabb[2] - hand_pos[2], - ] - # we want to accomplish the motion in 25 timesteps - # NOTE: this is an arbitrary choice - delta_step_to_obj = [delta_pos / 25.0 for delta_pos in delta_pos_to_obj] - - # move the hand along the vector to the object until it - # touches the object - # for _ in range(25): - for _ in range(26): - new_hand_pos = [ - hand_pos[0] + delta_step_to_obj[0], - hand_pos[1] + delta_step_to_obj[1], - hand_pos[2] + delta_step_to_obj[2], + delta_pos_to_obj = [ + closest_point_on_aabb[0] - hand_pos[0], + closest_point_on_aabb[1] - hand_pos[1], + closest_point_on_aabb[2] - hand_pos[2], ] - plan.append(new_hand_pos + list(hand_orn)) - hand_pos = new_hand_pos + # we want to accomplish the motion in 25 timesteps + # NOTE: this is an arbitrary choice + delta_step_to_obj = [delta_pos / 25.0 for delta_pos in delta_pos_to_obj] + + # move the hand along the vector to the object until it + # touches the object + # for _ in range(25): + for _ in range(26): + new_hand_pos = [ + hand_pos[0] + delta_step_to_obj[0], + hand_pos[1] + delta_step_to_obj[1], + hand_pos[2] + delta_step_to_obj[2], + ] + plan.append(new_hand_pos + list(hand_orn)) + hand_pos = new_hand_pos + else: + # # hand_pos = plan[-1][0:3] + # # hand_orn = plan[-1][3:6] + # # joint_pos = plan[-1][6:] + # joint_pos = plan[-1] + # closest_point_on_aabb = obj_pos # Already got this before, but for contact instead of bbox + # # ik_success, closest_point_orn = get_valid_orientation(env, closest_point_on_aabb) + # # if not ik_success: + # # logging.info(f"PRIMITIVE: grasp {obj.name} fail, failed " + # # f"to find final stretch plan") + # # return None + # closest_point_joint_pos = env.robots[0].calculate_eef_ik(closest_point_on_aabb, closest_point_orn) + # # assert closest_point_joint_pos is not None + # if closest_point_joint_pos is None: + # logging.info(f"PRIMITIVE: grasp {obj.name} fail, failed " + # f"to find a joint position on a second IK call (l468)") + # return None + # delta_pos_to_obj = [closest - current for closest, current in + # # zip(closest_point_on_aabb + hand_orn + closest_point_joint_pos, plan[-1])] + # zip(closest_point_joint_pos, plan[-1])] + # logging.info(f"\tTarget grasp: pos -- {closest_point_on_aabb}, orn -- {closest_point_orn}") + # delta_step_to_obj = [delta_pos / 25.0 for delta_pos in delta_pos_to_obj] + # logging.info(f"\tJoint positions before last stretch -- {joint_pos}") + # for _ in range(25): + # new_step = [prev_step + delta for prev_step, delta in + # # zip(hand_pos + hand_orn + joint_pos, delta_step_to_obj)] + # zip(joint_pos, delta_step_to_obj)] + # plan.append(new_step) + # # hand_pos = new_step[0:3] + # # hand_orn = new_step[3:6] + # # joint_pos = new_step[6:] + # joint_pos = new_step + # assert np.allclose(joint_pos, closest_point_joint_pos), f"\n{joint_pos}\n{closest_point_joint_pos}" + # logging.info(f"\tTarget joint positions will be -- {closest_point_joint_pos}") + # logging.info(f"\t\t{delta_pos_to_obj}") + # exit() + logging.info(f"\tTarget grasp: pos -- {obj_closest_point}, orn -- {closest_point_orn}") + logging.info(f"\tTarget joint positions will be -- {plan[-1]}") + p.restoreState(state) p.removeState(state) @@ -406,6 +515,7 @@ def make_place_plan( obj: Union["URDFObject", "RoomFloor"], place_rel_pos: Array, rng: Optional[Generator] = None, + distribution_samples: Optional[Array] = None, ) -> Optional[Tuple[List[List[float]], List[List[float]]]]: """Function to return a series of actions to place an object at a particular offset from another object's position. @@ -420,6 +530,16 @@ def make_place_plan( run RRT, the plan will only be one step (i.e, the pose to move the hand to to try placing the object). """ + if distribution_samples is not None: + obj_pos = obj.get_position() + print("Motion planner place, distr shape:", distribution_samples.shape) + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = obj_pos[2] + distribution_samples[2] + 0.2 + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) + env.step(np.zeros(env.action_space.shape)) + if rng is None: rng = np.random.default_rng(23) @@ -429,8 +549,8 @@ def make_place_plan( else: obj_in_hand_idx = env.robots[0].object_in_hand obj_in_hand = [ - obj for obj in env.scene.get_objects() - if obj.get_body_id() == obj_in_hand_idx + o for o in env.scene.get_objects() + if o.get_body_id() == obj_in_hand_idx ][0] logging.info(f"PRIMITIVE: attempt to place {obj_in_hand.name} ontop" f"/inside {obj.name} with params {place_rel_pos}") @@ -468,17 +588,26 @@ def make_place_plan( if isinstance(env.robots[0], BehaviorRobot): obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand + hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position() else: obj_in_hand_idx = env.robots[0].object_in_hand + hand_x, hand_y, hand_z = env.robots[0].get_end_effector_position() obj_in_hand = [ - obj for obj in env.scene.get_objects() - if obj.get_body_id() == obj_in_hand_idx + o for o in env.scene.get_objects() + if o.get_body_id() == obj_in_hand_idx ][0] x, y, z = np.add(place_rel_pos, obj.get_position()) - - obstacles = get_scene_body_ids(env, include_self=False) + 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 + + obstacles = get_relevant_scene_body_ids(env, include_self=False) if isinstance(env.robots[0], BehaviorRobot): - 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) end_conf = [ x, y, @@ -488,41 +617,62 @@ def make_place_plan( 0, ] else: - obstacles.remove(env.robots[0].object_in_hand) - end_conf = [ - x, - y, - z, - 0, - np.pi / 2, - 0, - ] + if env.robots[0].object_in_hand in obstacles: + obstacles.remove(env.robots[0].object_in_hand) + + # from predicators.envs import get_or_create_env + # ig_env = get_or_create_env("behavior").igibson_behavior_env + ik_success, orn = get_valid_orientation(env, [x, y, z]) + if not ik_success: + logging.info(f"PRIMITIVE: place {obj.name} fail, no valid IK") + # logging.info(f"\tFailed to grasp object at {obj.get_position()} from {ig_env.robots[0].get_position()}") + return None + logging.info(f"PRIMITIVE: place orientation {orn}") + euler_angles = p.getEulerFromQuaternion(orn) + joint_pos = env.robots[0].calculate_eef_ik([x, y, z], orn) + # assert joint_pos is not None + if joint_pos is None: + logging.info(f"PRIMITIVE: place {obj.name} fail, no valid IK on second call (l625)") + return None + # end_conf = [x, y, z] + euler_angles + joint_pos # pos and orn for validation + end_conf = joint_pos - # if env.use_rrt: - # if isinstance(env.robots[0], BehaviorRobot): - # 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: - # raise NotImplementedError('RRT hand motion planning is only implemented for the Behavior bot') - # else: - if isinstance(env.robots[0], BehaviorRobot): - 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 env.use_rrt: + if isinstance(env.robots[0], BehaviorRobot): + 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: + plan = plan_gripper_motion_fg( + robot=env.robots[0], + obj_in_hand=obj_in_hand, + end_conf=end_conf, + joint_limits=(env.robots[0].lower_joint_limits, + env.robots[0].upper_joint_limits), + obstacles=obstacles, + rng=rng, + ) + p.restoreState(state) else: - pos = env.robots[0].get_end_effector_position() - orn = list(p.getEulerFromQuaternion( - T.mat2quat(T.pose2mat(get_link_pose(env.robots[0].robot_ids[0], env.robots[0].eef_link_id))[:3, :3]))) - plan = [[pos[0], pos[1], pos[2], orn[0], orn[1], orn[2]], end_conf] + if isinstance(env.robots[0], BehaviorRobot): + 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] + else: + pos = env.robots[0].get_end_effector_position() + orn = list(p.getEulerFromQuaternion( + T.mat2quat(T.pose2mat(get_link_pose(env.robots[0].robot_ids[0], env.robots[0].eef_link_id))[:3, :3]))) + orig_joint_positions = get_joint_positions(env.robots[0].robot_ids[0], env.robots[0].joint_ids) + # plan = [pos + orn + orig_joint_positions, end_conf] + plan = [orig_joint_positions, end_conf] # NOTE: This below line is *VERY* important after the # pybullet state is restored. The hands keep an internal @@ -535,11 +685,11 @@ 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 if isinstance(env.robots[0], BehaviorRobot): original_orientation = list( diff --git a/predicators/behavior_utils/option_model_fns.py b/predicators/behavior_utils/option_model_fns.py index ac9017909b..03a0e64120 100644 --- a/predicators/behavior_utils/option_model_fns.py +++ b/predicators/behavior_utils/option_model_fns.py @@ -31,7 +31,8 @@ from igibson.robots.fetch_gripper_robot import FetchGripper from igibson.external.pybullet_tools.utils import \ - get_joint_positions + get_joint_positions, get_link_pose + import igibson.utils.transform_utils as T except (ImportError, ModuleNotFoundError) as e: pass @@ -39,7 +40,6 @@ # NavigateToOptionModel. prng = RandomState(10000) - def create_navigate_option_model( plan: List[List[float]], _original_orientation: List[List[float]], _obj_to_nav_to: "URDFObject" @@ -48,7 +48,7 @@ def create_navigate_option_model( plan, which is a list of 3-element lists each containing a series of (x, y, rot) waypoints for the robot to pass through.""" - def navigateToOptionModel(_init_state: State, env: "BehaviorEnv") -> None: + def navigateToOptionModel(_init_state: State, env: "BehaviorEnv", distribution_samples=None) -> None: robot_z = env.robots[0].get_position()[2] robot_orn = p.getEulerFromQuaternion(env.robots[0].get_orientation()) # If we're not overriding the learned samplers, then we will directly @@ -66,34 +66,45 @@ def navigateToOptionModel(_init_state: State, env: "BehaviorEnv") -> None: desired_xpos = sample_arr[0] + obj_pos[0] desired_ypos = sample_arr[1] + obj_pos[1] desired_zrot = np.arctan2(sample_arr[1], sample_arr[0]) - np.pi - logging.info(f"PRIMITIVE: Overriding sample ({plan[-1][0]}" + - f", {plan[-1][1]}) and attempting to " + - f"navigate to {_obj_to_nav_to.name} with " - f"params {sample_arr}") + # logging.info(f"PRIMITIVE: Overriding sample ({plan[-1][0]}" + + # f", {plan[-1][1]}) and attempting to " + + # f"navigate to {_obj_to_nav_to.name} with " + # f"params {sample_arr}") + + if distribution_samples is not None: + obj_pos = _obj_to_nav_to.get_position() + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = np.ones_like(x) * robot_z + print("For navigation: ", distribution_samples.shape) + # add_points = list(zip(*(x, y, z))) + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) if CFG.simulate_nav: + curr_plan = plan[:] done_bit = False while not done_bit: - # Get expected position and orientation from plan. - expected_pos = np.array([plan[0][0], plan[0][1], robot_z]) + # Get expected position and orientation from curr_plan. + expected_pos = np.array([curr_plan[0][0], curr_plan[0][1], robot_z]) expected_orn = p.getQuaternionFromEuler( - np.array([robot_orn[0], robot_orn[1], plan[0][2]])) + np.array([robot_orn[0], robot_orn[1], curr_plan[0][2]])) # In this case, we're at the final position we wanted to reach. - if len(plan) == 1: + if len(curr_plan) == 1: done_bit = True - logging.info( - "PRIMITIVE: navigation policy completed execution!") + # logging.info( + # "PRIMITIVE: navigation policy completed execution!") env.robots[0].set_position_orientation(expected_pos, expected_orn) - env.step(np.zeros(env.action_space.shape)) - plan.pop(0) + env.step(np.zeros(env.action_space.shape))#, add_points=add_points) + curr_plan.pop(0) target_pos = np.array([desired_xpos, desired_ypos, robot_z]) target_orn = p.getQuaternionFromEuler( np.array([robot_orn[0], robot_orn[1], desired_zrot])) env.robots[0].set_position_orientation(target_pos, target_orn) # this is running a zero action to step simulator so # the environment updates to the correct final position - env.step(np.zeros(env.action_space.shape)) + env.step(np.zeros(env.action_space.shape))#, add_points=add_points) return navigateToOptionModel @@ -105,26 +116,48 @@ def create_grasp_option_model( which is a list of 6-element lists containing a series of (x, y, z, roll, pitch, yaw) waypoints for the hand to pass through.""" - # NOTE: -1 because there are 25 timesteps that we move along the vector - # between the hand the object for until finally grasping, and we want - # just the final orientation. hand_i = -1 - rh_final_grasp_postion = plan[hand_i][0:3] - rh_final_grasp_orn = plan[hand_i][3:6] - - def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: + def graspObjectOptionModel(_state: State, env: "BehaviorEnv", distribution_samples=None) -> None: nonlocal hand_i + + if distribution_samples is not None: + obj_pos = obj_to_grasp.get_position() + print("For grasp: ", distribution_samples.shape) + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = obj_pos[2] + distribution_samples[2] + # add_points = list(zip(*(x, y, z))) + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) + if isinstance(env.robots[0], BehaviorRobot): + # NOTE: -1 because there are 25 timesteps that we move along the vector + # between the hand the object for until finally grasping, and we want + # just the final orientation. + rh_final_grasp_postion = plan[hand_i][0:3] + rh_final_grasp_orn = plan[hand_i][3:6] rh_orig_grasp_postion = env.robots[0].parts["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)) + + # 1.1 step the environment a few timesteps to update location. + for _ in range(3): + env.step(np.zeros(env.action_space.shape)) - # 3. Close hand and simulate grasp + # 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) @@ -147,6 +180,14 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: 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_postion, rh_orig_grasp_orn) @@ -159,13 +200,42 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: # environment. env.step(np.zeros(env.action_space.shape)) else: + # NOTE: -1 because there are 25 timesteps that we move along the vector + # between the hand the object for until finally grasping, and we want + # just the final orientation. + hand_i = -1 + # rh_final_grasp_postion = plan[hand_i][0:3] + # rh_final_grasp_orn = plan[hand_i][3:6] + final_joint_position = plan[hand_i] robot = env.robots[0] orig_joint_positions = get_joint_positions(robot.robot_ids[0], robot.joint_ids) + + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + # env.robots[0].set_eef_position_orientation( + # step[0:3], p.getQuaternionFromEuler(step[3:6])) + robot.set_joint_positions(step) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + + # 1 Teleport Hand to Grasp offset location - robot.set_eef_position_orientation(rh_final_grasp_postion, - p.getQuaternionFromEuler(rh_final_grasp_orn)) + # robot.set_eef_position_orientation(rh_final_grasp_postion, + # p.getQuaternionFromEuler(rh_final_grasp_orn)) + robot.set_joint_positions(final_joint_position) + ## Debug + pos = robot.get_end_effector_position() + orn = T.mat2quat(T.pose2mat(get_link_pose(robot.robot_ids[0], robot.eef_link_id))[:3, :3]) + logging.info(f"\tObtained grasp: pos -- {pos}, orn -- {orn}") + logging.info(f"\tActual joint pos were {final_joint_position}") + + + # 1.1 step the environment a few timesteps to update location. + for _ in range(3): + env.step(np.zeros(env.action_space.shape)) - # 3. Close hand and simulate grasp + # 2. Close hand and simulate grasp a = np.zeros(env.action_space.shape, dtype=float) a[10] = -1.0 assisted_grasp_action = np.zeros(11, dtype=float) @@ -181,8 +251,17 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None: 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].set_eef_position_orientation( + # step[0:3], p.getQuaternionFromEuler(step[3:6])) + robot.set_joint_positions(step) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + # 4 Move Hand to Original Location - robot.set_joint_positions(orig_joint_positions) # saves us one call to IK + robot.set_joint_positions(orig_joint_positions) if robot.object_in_hand is not None: # NOTE: This below line is necessary to update the visualizer. # Also, it only works for URDF objects (but if the object is @@ -204,16 +283,32 @@ def create_place_option_model( pitch, yaw) waypoints for the hand to pass through.""" def placeOntopObjectOptionModel(_init_state: State, - env: "BehaviorEnv") -> None: + env: "BehaviorEnv", distribution_samples=None) -> None: + + if distribution_samples is not None: + obj_pos = obj_to_place_onto.get_position() + print("For place: ", distribution_samples.shape) + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = obj_pos[2] + distribution_samples[2] + 0.2 + # add_points = list(zip(*(x, y, z))) + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) + if isinstance(env.robots[0], BehaviorRobot): obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand - obj_in_hand = [ - obj for obj in env.scene.get_objects() - if obj.get_body_id() == obj_in_hand_idx - ][0] rh_orig_grasp_position = env.robots[0].parts[ "right_hand"].get_position() rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation() + else: + obj_in_hand_idx = env.robots[0].object_in_hand + orig_joint_positions = get_joint_positions(env.robots[0].robot_ids[0], env.robots[0].joint_ids) + obj_in_hand = [ + obj for obj in env.scene.get_objects() + if obj.get_body_id() == obj_in_hand_idx + ][0] + + if isinstance(env.robots[0], BehaviorRobot): # If we're not overriding the learned samplers, then we will directly # use the elements of `plan`, which in turn use the outputs of the # learned samplers. Otherwise, we will ignore these and use our @@ -226,17 +321,28 @@ def placeOntopObjectOptionModel(_init_state: State, sample_arr = sample_place_ontop_params(env, obj_to_place_onto, rng) target_pos = np.add(sample_arr, \ obj_to_place_onto.get_position()).tolist() + target_pos[2] += 0.2 target_orn = [0, np.pi * 7 / 6, 0] logging.info(f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + "and attempting to " + 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() obj_in_hand.set_position_orientation( target_pos, p.getQuaternionFromEuler(target_orn)) - obj_to_place.force_wakeup() + obj_to_place_onto.force_wakeup() + # this is running a zero action to step simulator env.step(np.zeros(env.action_space.shape)) # reset the released object to zero velocity so it doesn't @@ -246,56 +352,72 @@ 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): - env.step(np.zeros(env.action_space.shape)) + else: - robot = env.robots[0] - released_obj_bid = robot.object_in_hand - orig_joint_positions = get_joint_positions(robot.robot_ids[0], robot.joint_ids) # If we're not overriding the learned samplers, then we will directly # use the elements of `plan`, which in turn use the outputs of the # learned samplers. Otherwise, we will ignore these and use our # oracle sampler to give us values to use. if not CFG.behavior_override_learned_samplers: - target_pos = plan[-1][0:3] - target_orn = plan[-1][3:6] + target_joint_pos = plan[-1] else: - rng = np.random.default_rng(prng.randint(10000)) - sample_arr = sample_place_ontop_params(env, obj_to_place_onto, rng) - target_pos = np.add(sample_arr, \ - obj_to_place_onto.get_position()).tolist() - # target_orn = [0, np.pi * 7 / 6, 0] - logging.info(f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + - "and attempting to " + - f"place ontop {obj_to_place_onto.name} with " - f"params {target_pos}") + raise NotImplementedError + + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + # env.robots[0].set_eef_position_orientation( + # step[0:3], p.getQuaternionFromEuler(step[3:6])) + env.robots[0].set_joint_positions(step) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) # robot.set_eef_position_orientation(target_pos, p.getQuaternionFromEuler(target_orn)) - robot.set_eef_position(target_pos) + # env.robots[0].set_eef_position(target_pos) + env.robots[0].set_joint_positions(target_joint_pos) a = np.zeros(env.action_space.shape, dtype=float) a[10] = 1.0 for _ in range(5): env.step(a) - obj_to_place.force_wakeup() + + obj_to_place_onto.force_wakeup() # this is running a zero action to step simulator env.step(np.zeros(env.action_space.shape)) # reset the released object to zero velocity so it doesn't # fly away because of residual warp speeds from teleportation! p.resetBaseVelocity( - released_obj_bid, + obj_in_hand_idx, linearVelocity=[0, 0, 0], angularVelocity=[0, 0, 0], ) - robot.set_joint_positions(orig_joint_positions) - # this is running a series of zero action to step simulator - # to let the object fall into its place - for _ in range(15): - env.step(np.zeros(env.action_space.shape)) + # Simulate Arm Movement (Backwards) + if CFG.behavior_option_model_rrt: + for step in reversed(plan): + # env.robots[0].set_eef_position_orientation( + # step[0:3], p.getQuaternionFromEuler(step[3:6])) + env.robots[0].set_joint_positions(step) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + + env.robots[0].set_joint_positions(orig_joint_positions) + + # this is running a series of zero action to step simulator + # to let the object fall into its place + 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() for obj in env.scene.get_objects(): @@ -356,8 +478,8 @@ def create_open_option_model( """Instantiates and returns an open option model given a dummy plan.""" del plan - def openObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None: - logging.info(f"PRIMITIVE: Attempting to open {obj_to_open.name}") + def openObjectOptionModel(_init_state: State, env: "BehaviorEnv", distribution_samples=None) -> None: + # logging.info(f"PRIMITIVE: Attempting to open {obj_to_open.name}") if np.linalg.norm( np.array(obj_to_open.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -365,9 +487,11 @@ def openObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None: "states") and object_states.Open in obj_to_open.states: obj_to_open.states[object_states.Open].set_value(True) else: - logging.info("PRIMITIVE open failed, cannot be opened") + pass + # logging.info("PRIMITIVE open failed, cannot be opened") else: - logging.info("PRIMITIVE open failed, too far") + pass + # logging.info("PRIMITIVE open failed, too far") obj_to_open.force_wakeup() # Step the simulator to update visuals. env.step(np.zeros(env.action_space.shape)) @@ -381,8 +505,8 @@ def create_close_option_model( """Instantiates and returns an close option model given a dummy plan.""" del plan - def closeObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None: - logging.info(f"PRIMITIVE: Attempting to close {obj_to_close.name}") + def closeObjectOptionModel(_init_state: State, env: "BehaviorEnv", distribution_samples=None) -> None: + # logging.info(f"PRIMITIVE: Attempting to close {obj_to_close.name}") if np.linalg.norm( np.array(obj_to_close.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -390,9 +514,11 @@ def closeObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None: "states") and object_states.Open in obj_to_close.states: obj_to_close.states[object_states.Open].set_value(False) else: - logging.info("PRIMITIVE close failed, cannot be opened") + pass + # logging.info("PRIMITIVE close failed, cannot be opened") else: - logging.info("PRIMITIVE close failed, too far") + pass + # logging.info("PRIMITIVE close failed, too far") obj_to_close.force_wakeup() # Step the simulator to update visuals. env.step(np.zeros(env.action_space.shape)) @@ -408,7 +534,16 @@ def create_place_inside_option_model( plan.""" def placeInsideObjectOptionModel(_init_state: State, - env: "BehaviorEnv") -> None: + env: "BehaviorEnv", distribution_samples=None) -> None: + if distribution_samples is not None: + obj_pos = obj_to_place_into.get_position() + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = obj_pos[2] + distribution_samples[2] + # add_points = list(zip(*(x, y, z))) + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) + if isinstance(env.robots[0], BehaviorRobot): obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand rh_orig_grasp_position = env.robots[0].parts[ @@ -423,9 +558,9 @@ def placeInsideObjectOptionModel(_init_state: State, ][0] if obj_in_hand is not None and obj_in_hand != obj_to_place_into and \ isinstance(obj_to_place_into, URDFObject): - logging.info( - f"PRIMITIVE: attempt to place {obj_in_hand.name} inside " - f"{obj_to_place_into.name}") + # logging.info( + # f"PRIMITIVE: attempt to place {obj_in_hand.name} inside " + # f"{obj_to_place_into.name}") if np.linalg.norm( np.array(obj_to_place_into.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -435,73 +570,132 @@ def placeInsideObjectOptionModel(_init_state: State, get_value()) or (hasattr(obj_to_place_into, "states") and not object_states.Open in obj_to_place_into.states): - logging.info(f"PRIMITIVE: place {obj_in_hand.name} inside " - f"{obj_to_place_into.name} success") + # logging.info(f"PRIMITIVE: place {obj_in_hand.name} inside " + # f"{obj_to_place_into.name} success") - # If we're not overriding the learned samplers, then we - # will directly use the elements of `plan`, which in turn - # use the outputs of the learned samplers. Otherwise, we - # will ignore these and use our oracle sampler to give us - # values to use. - if not CFG.behavior_override_learned_samplers: - target_pos = plan[-1][0:3] - target_orn = plan[-1][3:6] - else: - rng = np.random.default_rng(prng.randint(10000)) - place_rel_pos = sample_place_inside_params( - obj_to_place_into, rng) - target_pos_list = np.add( - place_rel_pos, obj_to_place_into.get_position()) - target_pos_list[2] += 0.2 - target_pos = target_pos_list.tolist() - target_orn = plan[-1][3:6] - logging.info( - f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + - f", {plan[-1][3:6]}) and attempting to " + - f"place inside to {obj_to_place_into.name} with " - f"params {target_pos}") if isinstance(env.robots[0], BehaviorRobot): + # If we're not overriding the learned samplers, then we + # will directly use the elements of `plan`, which in turn + # use the outputs of the learned samplers. Otherwise, we + # will ignore these and use our oracle sampler to give us + # values to use. + if not CFG.behavior_override_learned_samplers: + target_pos = plan[-1][0:3] + target_orn = plan[-1][3:6] + else: + rng = np.random.default_rng(prng.randint(10000)) + place_rel_pos = sample_place_inside_params( + obj_to_place_into, rng) + target_pos_list = np.add( + place_rel_pos, obj_to_place_into.get_position()) + target_pos_list[2] += 0.2 + target_pos = target_pos_list.tolist() + target_orn = plan[-1][3:6] + logging.info( + f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + + 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"].set_position_orientation( + target_pos, p.getQuaternionFromEuler(target_orn)) env.robots[0].parts["right_hand"].force_release_obj() obj_to_place_into.force_wakeup() obj_in_hand.set_position_orientation( target_pos, p.getQuaternionFromEuler(target_orn)) + + # this is running a zero action to step simulator + env.step(np.zeros(env.action_space.shape)) + # reset the released object to zero velocity so it + # doesn't fly away because of residual warp speeds + # from teleportation! + p.resetBaseVelocity( + obj_in_hand_idx, + 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) + else: + if not CFG.behavior_override_learned_samplers: + # target_pos = plan[-1][0:3] + # target_orn = plan[-1][3:6] + target_joint_pos = plan[-1] + else: + raise NotImplementedError + + # Simulate Arm Movement + if CFG.behavior_option_model_rrt: + for step in plan: + # env.robots[0].set_eef_position_orientation( + # step[0:3], p.getQuaternionFromEuler(step[3:6])) + env.robots[0].set_joint_positions(step) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) + # env.robots[0].set_eef_position_orientation(target_pos, p.getQuaternionFromEuler(target_orn)) - env.robots[0].set_eef_position(target_pos) + # env.robots[0].set_eef_position(target_pos) + + env.robots[0].set_joint_positions(target_joint_pos) a = np.zeros(env.action_space.shape, dtype=float) a[10] = 1.0 for _ in range(5): env.step(a) obj_to_place_into.force_wakeup() - # this is running a zero action to step simulator - env.step(np.zeros(env.action_space.shape)) - # reset the released object to zero velocity so it - # doesn't fly away because of residual warp speeds - # from teleportation! - p.resetBaseVelocity( - obj_in_hand_idx, - linearVelocity=[0, 0, 0], - angularVelocity=[0, 0, 0], - ) - if isinstance(env.robots[0], BehaviorRobot): - env.robots[0].parts["right_hand"].set_position_orientation( - rh_orig_grasp_position, rh_orig_grasp_orn) - else: + # this is running a zero action to step simulator + env.step(np.zeros(env.action_space.shape)) + # reset the released object to zero velocity so it + # doesn't fly away because of residual warp speeds + # from teleportation! + p.resetBaseVelocity( + obj_in_hand_idx, + 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].set_eef_position_orientation( + # step[0:3], p.getQuaternionFromEuler(step[3:6])) + env.robots[0].set_joint_positions(step) + for _ in range(1): + env.step(np.zeros(env.action_space.shape)) env.robots[0].set_joint_positions(orig_joint_positions) - # this is running a series of zero action to step - # simulator to let the object fall into its place + for _ in range(15): env.step(np.zeros(env.action_space.shape)) else: - logging.info( - f"PRIMITIVE: place {obj_in_hand.name} inside " - f"{obj_to_place_into.name} fail, need open not open") + pass + # logging.info( + # f"PRIMITIVE: place {obj_in_hand.name} inside " + # f"{obj_to_place_into.name} fail, need open not open") else: - logging.info(f"PRIMITIVE: place {obj_in_hand.name} inside " - f"{obj_to_place_into.name} fail, too far") + pass + # logging.info(f"PRIMITIVE: place {obj_in_hand.name} inside " + # f"{obj_to_place_into.name} fail, too far") else: - logging.info("PRIMITIVE: place failed with invalid obj params.") + pass + # logging.info("PRIMITIVE: place failed with invalid obj params.") obj_to_place_into.force_wakeup() # Step the simulator to update visuals. @@ -518,7 +712,7 @@ def create_place_under_option_model( plan.""" def placeUnderObjectOptionModel(_init_state: State, - env: "BehaviorEnv") -> None: + env: "BehaviorEnv", distribution_samples=None) -> None: obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand obj_in_hand = [ obj for obj in env.scene.get_objects() @@ -527,11 +721,19 @@ def placeUnderObjectOptionModel(_init_state: State, rh_orig_grasp_position = env.robots[0].parts[ "right_hand"].get_position() rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation() + if distribution_samples is not None: + obj_pos = obj_to_place_under.get_position() + x = obj_pos[0] + distribution_samples[0] + y = obj_pos[1] + distribution_samples[1] + z = obj_pos[2] + distribution_samples[2] + # add_points = list(zip(*(x, y, z))) + for sphere, x_i, y_i, z_i in zip(env.viz_spheres, x, y, z): + sphere.set_position((x_i, y_i, z_i)) if obj_in_hand is not None and obj_in_hand != obj_to_place_under and \ isinstance(obj_to_place_under, URDFObject): - logging.info( - f"PRIMITIVE: attempt to place {obj_in_hand.name} under " - f"{obj_to_place_under.name}") + # logging.info( + # f"PRIMITIVE: attempt to place {obj_in_hand.name} under " + # f"{obj_to_place_under.name}") if np.linalg.norm( np.array(obj_to_place_under.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -540,9 +742,10 @@ def placeUnderObjectOptionModel(_init_state: State, if obj_in_hand.states[object_states.Under].get_value( obj_to_place_under) or obj_to_place_under.states[ object_states.Under].get_value(obj_in_hand): - logging.info( - f"PRIMITIVE: place {obj_in_hand.name} under " - f"{obj_to_place_under.name} success") + pass + # logging.info( + # f"PRIMITIVE: place {obj_in_hand.name} under " + # f"{obj_to_place_under.name} success") # If we're not overriding the learned samplers, then we # will directly use the elements of `plan`, which in turn @@ -561,11 +764,11 @@ def placeUnderObjectOptionModel(_init_state: State, target_pos_list[2] += 0.2 target_pos = target_pos_list.tolist() target_orn = plan[-1][3:6] - logging.info( - f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + - f", {plan[-1][3:6]}) and attempting to " + - f"place under to {obj_to_place_under.name} with " - f"params {target_pos}") + # logging.info( + # f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + + # f", {plan[-1][3:6]}) and attempting to " + + # f"place under to {obj_to_place_under.name} with " + # f"params {target_pos}") env.robots[0].parts["right_hand"].force_release_obj() obj_to_place_under.force_wakeup() obj_in_hand.set_position_orientation( @@ -587,14 +790,17 @@ def placeUnderObjectOptionModel(_init_state: State, for _ in range(15): env.step(np.zeros(env.action_space.shape)) else: - logging.info(f"PRIMITIVE: place {obj_in_hand.name} under " - f"{obj_to_place_under.name} fail, not under") + pass + # logging.info(f"PRIMITIVE: place {obj_in_hand.name} under " + # f"{obj_to_place_under.name} fail, not under") else: - logging.info(f"PRIMITIVE: place {obj_in_hand.name} under " - f"{obj_to_place_under.name} fail, too far") + pass + # logging.info(f"PRIMITIVE: place {obj_in_hand.name} under " + # f"{obj_to_place_under.name} fail, too far") else: - logging.info( - "PRIMITIVE: place under failed with invalid obj params.") + pass + # logging.info( + # "PRIMITIVE: place under failed with invalid obj params.") obj_to_place_under.force_wakeup() # Step the simulator to update visuals. @@ -611,9 +817,9 @@ def create_toggle_on_option_model( del plan def toggleOnObjectOptionModel(_init_state: State, - env: "BehaviorEnv") -> None: - logging.info( - f"PRIMITIVE: Attempting to toggle on {obj_to_toggled_on.name}") + env: "BehaviorEnv", distribution_samples=None) -> None: + # logging.info( + # f"PRIMITIVE: Attempting to toggle on {obj_to_toggled_on.name}") if np.linalg.norm( np.array(obj_to_toggled_on.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -623,9 +829,11 @@ def toggleOnObjectOptionModel(_init_state: State, obj_to_toggled_on.states[object_states.ToggledOn].set_value( True) else: - logging.info("PRIMITIVE toggle failed, cannot be toggled on") + pass + # logging.info("PRIMITIVE toggle failed, cannot be toggled on") else: - logging.info("PRIMITIVE toggle failed, too far") + pass + # logging.info("PRIMITIVE toggle failed, too far") obj_to_toggled_on.force_wakeup() # Step the simulator to update visuals. env.step(np.zeros(env.action_space.shape)) @@ -641,7 +849,7 @@ def create_place_nextto_option_model( plan.""" def placeNextToObjectOptionModel(_init_state: State, - env: "BehaviorEnv") -> None: + env: "BehaviorEnv", distribution_samples=None) -> None: obj_in_hand_idx = env.robots[0].parts["right_hand"].object_in_hand obj_in_hand = [ obj for obj in env.scene.get_objects() @@ -652,9 +860,9 @@ def placeNextToObjectOptionModel(_init_state: State, rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation() if obj_in_hand is not None and obj_in_hand != obj_to_place_nextto and \ isinstance(obj_to_place_nextto, URDFObject): - logging.info( - f"PRIMITIVE: attempt to place {obj_in_hand.name} next to " - f"{obj_to_place_nextto.name}") + # logging.info( + # f"PRIMITIVE: attempt to place {obj_in_hand.name} next to " + # f"{obj_to_place_nextto.name}") if np.linalg.norm( np.array(obj_to_place_nextto.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -663,9 +871,10 @@ def placeNextToObjectOptionModel(_init_state: State, if obj_in_hand.states[object_states.NextTo].get_value( obj_to_place_nextto) or obj_to_place_nextto.states[ object_states.NextTo].get_value(obj_in_hand): - logging.info( - f"PRIMITIVE: place {obj_in_hand.name} next to " - f"{obj_to_place_nextto.name} success") + pass + # logging.info( + # f"PRIMITIVE: place {obj_in_hand.name} next to " + # f"{obj_to_place_nextto.name} success") # If we're not overriding the learned samplers, then we # will directly use the elements of `plan`, which in turn @@ -684,11 +893,11 @@ def placeNextToObjectOptionModel(_init_state: State, target_pos_list[2] += 0.2 target_pos = target_pos_list.tolist() target_orn = plan[-1][3:6] - logging.info( - f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + - f", {plan[-1][3:6]}) and attempting to " + - f"place next to {obj_to_place_nextto.name} with " - f"params {target_pos}") + # logging.info( + # f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" + + # f", {plan[-1][3:6]}) and attempting to " + + # f"place next to {obj_to_place_nextto.name} with " + # f"params {target_pos}") env.robots[0].parts["right_hand"].force_release_obj() obj_to_place_nextto.force_wakeup() obj_in_hand.set_position_orientation( @@ -710,15 +919,18 @@ def placeNextToObjectOptionModel(_init_state: State, for _ in range(15): env.step(np.zeros(env.action_space.shape)) else: - logging.info( - f"PRIMITIVE: place {obj_in_hand.name} next to " - f"{obj_to_place_nextto.name} fail, not next to") + pass + # logging.info( + # f"PRIMITIVE: place {obj_in_hand.name} next to " + # f"{obj_to_place_nextto.name} fail, not next to") else: - logging.info(f"PRIMITIVE: place {obj_in_hand.name} next to " - f"{obj_to_place_nextto.name} fail, too far") + pass + # logging.info(f"PRIMITIVE: place {obj_in_hand.name} next to " + # f"{obj_to_place_nextto.name} fail, too far") else: - logging.info( - "PRIMITIVE: place under failed with invalid obj params.") + pass + # logging.info( + # "PRIMITIVE: place under failed with invalid obj params.") obj_to_place_nextto.force_wakeup() # Step the simulator to update visuals. @@ -735,8 +947,8 @@ def create_clean_dusty_option_model( del plan def cleanDustyObjectOptionModel(_init_state: State, - env: "BehaviorEnv") -> None: - logging.info(f"PRIMITIVE: Attempting to clean {obj_to_clean.name}") + env: "BehaviorEnv", distribution_samples=None) -> None: + # logging.info(f"PRIMITIVE: Attempting to clean {obj_to_clean.name}") if np.linalg.norm( np.array(obj_to_clean.get_position()) - np.array(env.robots[0].get_position())) < 2: @@ -745,9 +957,11 @@ def cleanDustyObjectOptionModel(_init_state: State, "states") and object_states.Dusty in obj_to_clean.states: obj_to_clean.states[object_states.Dusty].set_value(False) else: - logging.info("PRIMITIVE clean failed, cannot be cleaned") + pass + # logging.info("PRIMITIVE clean failed, cannot be cleaned") else: - logging.info("PRIMITIVE clean failed, too far") + pass + # logging.info("PRIMITIVE clean failed, too far") obj_to_clean.force_wakeup() # Step the simulator to update visuals. env.step(np.zeros(env.action_space.shape))