added grasp and place movement

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

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(

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

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