Skip to content

Commit

Permalink
Some additional fixes for fetch RRT (grasp samples unused)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jorge-a-mendez
jorge-a-mendez committed Jan 17, 2024
1 parent 4b6a6a0 commit 1e8e5e3
Show file tree
Hide file tree
Showing 3 changed files with 52 additions and 9 deletions.
20 changes: 18 additions & 2 deletions predicators/behavior_utils/behavior_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -837,6 +837,22 @@ 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_grasp_relevant_scene_body_ids(env: "BehaviorEnv") -> List[int]:
ids = []
cnt_skipped = 0
for obj in env.scene.get_objects():
if isinstance(obj, URDFObject):
# for body_id in obj.body_ids:
# closest_points = p.getClosestPoints(env.robots[0].body_id, body_id, distance=3)
# if len(closest_points) > 0:
# ids.append(body_id)
# else:
# cnt_skipped += 1
if "bed" in obj.name:
ids.extend(obj.body_ids)
print("TOTAL OBSTACLES: ", len(ids), "( skipped:", cnt_skipped, ")")
return ids

def get_relevant_scene_body_ids(
env: "BehaviorEnv",
include_self: bool = False,
Expand All @@ -853,8 +869,8 @@ def get_relevant_scene_body_ids(
# 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 "bed" in obj.name:
ids.extend(obj.body_ids)

if include_self:
ids.append(env.robots[0].parts["left_hand"].get_body_id())
Expand Down
31 changes: 25 additions & 6 deletions predicators/behavior_utils/motion_planner_fns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@

from predicators.behavior_utils.behavior_utils import check_nav_end_pose, \
get_aabb_volume, get_closest_point_on_aabb, get_relevant_scene_body_ids, \
reset_and_release_hand, get_aabb_centroid, get_valid_orientation
reset_and_release_hand, get_aabb_centroid, get_valid_orientation, \
get_grasp_relevant_scene_body_ids
from predicators.settings import CFG
from predicators.structs import Array

Expand Down Expand Up @@ -171,6 +172,8 @@ def sample_fn(env: "BehaviorEnv",
rng=rng,
max_distance=0.01
)
if plan is not None:
plan = plan + [end_conf]
p.restoreState(state)
else:
pos = env.robots[0].get_position()
Expand Down Expand Up @@ -212,6 +215,9 @@ def make_grasp_plan(
will only be one step (i.e, the pose to move the hand to to try
grasping the object).
"""
if not isinstance(env.robots[0], BehaviorRobot):
env.robots[0].set_joint_positions(env.robots[0].untucked_default_joints)

if distribution_samples is not None:
obj_pos = obj.get_position()
print("Motion planner grasp, distr shape:", distribution_samples.shape)
Expand Down Expand Up @@ -283,9 +289,10 @@ def make_grasp_plan(
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 = get_grasp_relevant_scene_body_ids(env) #get_relevant_scene_body_ids(env)
for body_id in obj.body_ids:
if body_id in obstacles:
obstacles.remove(body_id)
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]
Expand Down Expand Up @@ -398,6 +405,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
# assert np.allclose(plan[0], env.robots[0].untucked_default_joints, atol=1e-2)
plan = plan + [end_conf]
else:
if isinstance(env.robots[0], BehaviorRobot):
Expand Down Expand Up @@ -485,7 +493,7 @@ def make_grasp_plan(
# # 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}"
# assert np.allclose(joint_pos, closest_point_joint_pos), f"\n{joint_pos}\n{closest_point_joint_pos}, atol=1e-2"
# logging.info(f"\tTarget joint positions will be -- {closest_point_joint_pos}")
# logging.info(f"\t\t{delta_pos_to_obj}")
# exit()
Expand Down Expand Up @@ -530,6 +538,9 @@ 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 not isinstance(env.robots[0], BehaviorRobot):
env.robots[0].set_joint_positions(env.robots[0].untucked_default_joints)

if distribution_samples is not None:
obj_pos = obj.get_position()
print("Motion planner place, distr shape:", distribution_samples.shape)
Expand Down Expand Up @@ -604,8 +615,8 @@ def make_place_plan(
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 = get_relevant_scene_body_ids(env, include_self=False)
if env.robots[0].parts["right_hand"].object_in_hand in obstacles:
obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand)
end_conf = [
Expand All @@ -617,6 +628,7 @@ def make_place_plan(
0,
]
else:
obstacles = get_grasp_relevant_scene_body_ids(env)
if env.robots[0].object_in_hand in obstacles:
obstacles.remove(env.robots[0].object_in_hand)

Expand Down Expand Up @@ -660,6 +672,13 @@ def make_place_plan(
rng=rng,
)
p.restoreState(state)
# If RRT planning fails, fail and return None
if plan is None:
logging.info(f"PRIMITIVE: place {obj_in_hand.name} fail, failed "
f"to find plan to continuous params {place_rel_pos}")
return None
# assert np.allclose(plan[0], env.robots[0].untucked_default_joints, atol=1e-2)
plan = plan + [end_conf]
else:
if isinstance(env.robots[0], BehaviorRobot):
pos = env.robots[0].parts["right_hand"].get_position()
Expand Down
10 changes: 9 additions & 1 deletion predicators/behavior_utils/option_model_fns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,9 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv", distribution_sampl
final_joint_position = plan[hand_i]
robot = env.robots[0]
orig_joint_positions = get_joint_positions(robot.robot_ids[0], robot.joint_ids)

# assert np.allclose(orig_joint_positions, plan[0], atol=1e-3), f"{np.abs(orig_joint_positions - plan[0])}"
# assert np.allclose(orig_joint_positions, robot.untucked_default_joints, atol=1e-3), f"{np.abs(orig_joint_positions - robot.untucked_default_joints)}"
orig_joint_positions = robot.untucked_default_joints
# Simulate Arm Movement
if CFG.behavior_option_model_rrt:
for step in plan:
Expand Down Expand Up @@ -303,6 +305,9 @@ def placeOntopObjectOptionModel(_init_state: State,
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)
# assert np.allclose(orig_joint_positions, plan[0], atol=1e-3), f"{np.abs(orig_joint_positions - plan[0])}"
# assert np.allclose(orig_joint_positions, env.robots[0].untucked_default_joints, atol=1e-3), f"{np.abs(orig_joint_positions - env.robots[0].untucked_default_joints)}"
orig_joint_positions = env.robots[0].untucked_default_joints
obj_in_hand = [
obj for obj in env.scene.get_objects()
if obj.get_body_id() == obj_in_hand_idx
Expand Down Expand Up @@ -552,6 +557,9 @@ def placeInsideObjectOptionModel(_init_state: State,
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)
# assert np.allclose(orig_joint_positions, plan[0], atol=1e-2), f"{np.abs(orig_joint_positions - plan[0])}"
# assert np.allclose(orig_joint_positions, env.robots[0].untucked_default_joints, atol=1e-3), f"{np.abs(orig_joint_positions - env.robots[0].untucked_default_joints)}"
orig_joint_positions = env.robots[0].untucked_default_joints
obj_in_hand = [
obj for obj in env.scene.get_objects()
if obj.get_body_id() == obj_in_hand_idx
Expand Down

0 comments on commit 1e8e5e3

Please sign in to comment.