Skip to content

Commit

Permalink
Progress towards throwing_away_leftovers and collecting_aluminum_cans
Browse files Browse the repository at this point in the history
  • Loading branch information
@jorge-a-mendez
jorge-a-mendez committed Jul 7, 2023
1 parent 3dc704d commit 037b5e1
Show file tree
Hide file tree
Showing 4 changed files with 127 additions and 112 deletions.
26 changes: 19 additions & 7 deletions predicators/behavior_utils/behavior_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -874,6 +874,11 @@ def detect_collision(bodyA: int, ignore_objects: List[Optional[int]] = None) ->
continue
closest_points = p.getClosestPoints(bodyA, body_id, distance=0.01)
if len(closest_points) > 0:
# from predicators.envs import get_or_create_env
# env = get_or_create_env("behavior")
# for ig_obj in env._get_task_relevant_objects():
# if ig_obj.body_id == body_id:
# logging.info(f"Body in collision: {env._ig_object_name(ig_obj)}")
collision = True
break
return collision
Expand Down Expand Up @@ -1064,16 +1069,23 @@ def check_nav_end_pose(
obj.get_body_id(),
))
blocked=False
if not detect_robot_collision(env.robots[0]) and (not blocked
or ignore_blocked) and (isinstance(env.robots[0], BehaviorRobot)
or check_hand_end_pose(env, obj, np.zeros(3, dtype=float),
ignore_collisions=True)):

if detect_robot_collision(env.robots[0]):
status = 1
elif blocked and not ignore_blocked:
status = 2
elif not (isinstance(env.robots[0], BehaviorRobot) or
check_hand_end_pose(env, obj, np.zeros(3, dtype=float),
ignore_collisions=True)):
status = 3
else:
status = 0
valid_position = (pos, orn)

p.restoreState(state)
p.removeState(state)

return valid_position
return valid_position, status

def get_valid_orientation(env: "BehaviorEnv", obj: Union["URDFObject",
"RoomFloor"]) -> Tuple[float]:
Expand Down Expand Up @@ -1172,7 +1184,7 @@ def sample_navigation_params(igibson_behavior_env: "BehaviorEnv",
# of tries.
num_samples_tried = 0
while (check_nav_end_pose(igibson_behavior_env, obj_to_sample_near,
sampler_output) is None):
sampler_output)[0] is None):
distance = closeness_limit * rng.random()
yaw = rng.random() * (2 * np.pi) - np.pi
x = distance * np.cos(yaw)
Expand All @@ -1182,7 +1194,7 @@ 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):
ignore_blocked=True)[0]:
return sampler_output
# 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
Expand Down
100 changes: 51 additions & 49 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,7 @@

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

Expand Down Expand Up @@ -108,20 +108,28 @@ def sample_fn(env: "BehaviorEnv",
# during test time iff we're using the learned sampler. Right now,
# this code will also run during oracle execution.
if not CFG.behavior_override_learned_samplers:
valid_position = check_nav_end_pose(env, obj, pos_offset)
valid_position, nav_status = check_nav_end_pose(env, obj, pos_offset)
else:
rob_pos = env.robots[0].get_position()
valid_position = ([rob_pos[0], rob_pos[1],
rob_pos[2]], original_orientation)
nav_status = 0

if valid_position is None:
logging.warning("WARNING: Position commanded is in collision!")
if nav_status == 1:
logging.warning("WARNING: Position commanded is in collision!")
elif nav_status == 2:
logging.warning("WARNING: Position commanded is blocked!")
elif nav_status == 3:
logging.warning("WARNING: Position commanded fails to reach object!")
p.restoreState(state)
p.removeState(state)
logging.warning(f"PRIMITIVE: navigate to {obj.name} with params "
f"{pos_offset} fail")
return None

assert nav_status == 0
logging.info(f"\tNavigating to {valid_position} to reach object at {obj.get_position()}")
p.restoreState(state)
end_conf = [
valid_position[0][0],
Expand Down Expand Up @@ -215,7 +223,7 @@ def make_grasp_plan(
return None

# If the object is too far away, fail and return None
if (np.linalg.norm(
if isinstance(env.robots[0], BehaviorRobot) and (np.linalg.norm(
np.array(obj.get_position()) -
np.array(env.robots[0].get_position())) > 2):
logging.info(f"PRIMITIVE: grasp {obj.name} fail, too far")
Expand All @@ -226,21 +234,15 @@ 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())
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])
hand_x, hand_y, hand_z = env.robots[0].get_end_effector_position()
# 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)
closest_point = min(points, key=lambda x:x[8])
obj_pos = closest_point[6]
x = obj_pos[0] + grasp_offset[0]
y = obj_pos[1] + grasp_offset[1]
z = obj_pos[2] + grasp_offset[2]
# hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position()
# minx = min(x, hand_x) - 0.5
# miny = min(y, hand_y) - 0.5
# minz = min(z, hand_z) - 0.5
# maxx = max(x, hand_x) + 0.5
# maxy = max(y, hand_y) + 0.5
# maxz = max(z, hand_z) + 0.5


if isinstance(env.robots[0], BehaviorRobot):
Expand All @@ -261,30 +263,38 @@ def make_grasp_plan(
unit_z_vector = np.array([0.0, 0.0, -1.0])
# This is because we assume the hand is originally oriented
# so -z is coming out of the palm
else:
# Always grasp downward
hand_to_obj_unit_vector = np.array([0., 0., 1.])
unit_z_vector = np.array([-1.0, 0.0, 0.0])
# This is because we assume the hand is originally oriented
# so -x is coming out of the palm
c_var = np.dot(unit_z_vector, hand_to_obj_unit_vector)
if c_var not in [-1.0, 1.0]:
v_var = np.cross(unit_z_vector, hand_to_obj_unit_vector)
s_var = np.linalg.norm(v_var)
v_x = np.array([
[0, -v_var[2], v_var[1]],
[v_var[2], 0, -v_var[0]],
[-v_var[1], v_var[0], 0],
])
R = (np.eye(3) + v_x + np.linalg.matrix_power(v_x, 2) * ((1 - c_var) /
(s_var**2)))
r = scipy.spatial.transform.Rotation.from_matrix(R)
euler_angles = r.as_euler("xyz")
else:
if c_var == 1.0:
euler_angles = np.zeros(3, dtype=float)
c_var = np.dot(unit_z_vector, hand_to_obj_unit_vector)
if c_var not in [-1.0, 1.0]:
v_var = np.cross(unit_z_vector, hand_to_obj_unit_vector)
s_var = np.linalg.norm(v_var)
v_x = np.array([
[0, -v_var[2], v_var[1]],
[v_var[2], 0, -v_var[0]],
[-v_var[1], v_var[0], 0],
])
R = (np.eye(3) + v_x + np.linalg.matrix_power(v_x, 2) * ((1 - c_var) /
(s_var**2)))
r = scipy.spatial.transform.Rotation.from_matrix(R)
euler_angles = r.as_euler("xyz")
else:
euler_angles = np.array([0.0, np.pi, 0.0])
if c_var == 1.0:
euler_angles = np.zeros(3, dtype=float)
else:
euler_angles = np.array([0.0, np.pi, 0.0])
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)
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)

state = p.saveState()
end_conf = [
Expand Down Expand Up @@ -352,9 +362,7 @@ def make_grasp_plan(
if isinstance(env.robots[0], BehaviorRobot):
closest_point_on_aabb = get_closest_point_on_aabb(hand_pos, lo, hi)
else:
aabb_center = get_aabb_center([lo, hi])
closest_point_on_aabb = get_closest_point_on_aabb (hand_pos, lo, hi)
closest_point_on_aabb[2] = aabb_center[2] # make sure the height is really on the object
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],
Expand All @@ -366,7 +374,8 @@ def make_grasp_plan(

# move the hand along the vector to the object until it
# touches the object
for _ in range(25):
# 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],
Expand Down Expand Up @@ -466,13 +475,6 @@ def make_place_plan(
if obj.get_body_id() == obj_in_hand_idx
][0]
x, y, z = np.add(place_rel_pos, obj.get_position())
# hand_x, hand_y, hand_z = env.robots[0].parts["right_hand"].get_position()
# minx = min(x, hand_x) - 1
# miny = min(y, hand_y) - 1
# minz = min(z, hand_z) - 0.5
# maxx = max(x, hand_x) + 1
# maxy = max(y, hand_y) + 1
# maxz = max(z, hand_z) + 0.5

obstacles = get_scene_body_ids(env, include_self=False)
if isinstance(env.robots[0], BehaviorRobot):
Expand Down
14 changes: 7 additions & 7 deletions predicators/behavior_utils/option_model_fns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -175,9 +175,8 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None:
else:
grasp_obj_body_id = obj_to_grasp.body_id
# 3.1 Call code that does assisted grasping
robot.handle_assisted_grasping(assisted_grasp_action,#)
override_ag_data=(grasp_obj_body_id, -1),)
assert hasattr(robot, 'child_frame_transform')
robot.handle_assisted_grasping(assisted_grasp_action,)
# override_ag_data=(grasp_obj_body_id, -1),)
# 3.2 step the environment a few timesteps to complete grasp
for _ in range(5):
env.step(a)
Expand All @@ -193,7 +192,6 @@ def graspObjectOptionModel(_state: State, env: "BehaviorEnv") -> None:
# environment.
env.step(np.zeros(env.action_space.shape))

assert hasattr(robot, 'child_frame_transform')
return graspObjectOptionModel


Expand Down Expand Up @@ -270,13 +268,14 @@ 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_orn = [0, np.pi * 7 / 6, 0]
# 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}")

robot.set_eef_position_orientation(target_pos, p.getQuaternionFromEuler(target_orn))
# robot.set_eef_position_orientation(target_pos, p.getQuaternionFromEuler(target_orn))
robot.set_eef_position(target_pos)
a = np.zeros(env.action_space.shape, dtype=float)
a[10] = 1.0
for _ in range(5):
Expand Down Expand Up @@ -467,7 +466,8 @@ def placeInsideObjectOptionModel(_init_state: State,
obj_in_hand.set_position_orientation(
target_pos, p.getQuaternionFromEuler(target_orn))
else:
env.robots[0].set_eef_position_orientation(target_pos, p.getQuaternionFromEuler(target_orn))
# env.robots[0].set_eef_position_orientation(target_pos, p.getQuaternionFromEuler(target_orn))
env.robots[0].set_eef_position(target_pos)
a = np.zeros(env.action_space.shape, dtype=float)
a[10] = 1.0
for _ in range(5):
Expand Down
99 changes: 50 additions & 49 deletions predicators/ground_truth_nsrts.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2969,53 +2969,53 @@ def grasp_obj_param_sampler(state: State, goal: Set[GroundAtom],
z_offset = rng.random() * 0.2
return np.array([x_offset, y_offset, z_offset])

def grasp_obj_param_sampler_fetch(state: State, goal: Set[GroundAtom],
rng: Generator,
objects: Sequence["URDFObject"]) -> Array:
"""Sampler for grasp option (Fetch)."""
ig_env = get_or_create_env("behavior").igibson_behavior_env
obj = objects[0]
num_tries = 100
# logging.info("Sampling params for grasp...")

robot_pos = ig_env.robots[0].get_position()
aabb = obj.states[object_states.AABB].get_value()
aabb_extent = get_aabb_extent(aabb)
obj_closest_point = get_closest_point_on_aabb(robot_pos, aabb[0], aabb[1])

# for samples in range(num_tries):
# # x_offset = (rng.random() * 0.4) - 0.2
# # y_offset = (rng.random() * 0.4) - 0.2
# # z_offset = rng.random() * .2
# def grasp_obj_param_sampler_fetch(state: State, goal: Set[GroundAtom],
# rng: Generator,
# objects: Sequence["URDFObject"]) -> Array:
# """Sampler for grasp option (Fetch)."""
# ig_env = get_or_create_env("behavior").igibson_behavior_env
# obj = objects[0]
# num_tries = 100
# # logging.info("Sampling params for grasp...")

# robot_pos = ig_env.robots[0].get_position()
# aabb = obj.states[object_states.AABB].get_value()
# aabb_extent = get_aabb_extent(aabb)
# obj_closest_point = get_closest_point_on_aabb(robot_pos, aabb[0], aabb[1])

# # for samples in range(num_tries):
# # # x_offset = (rng.random() * 0.4) - 0.2
# # # y_offset = (rng.random() * 0.4) - 0.2
# # # z_offset = rng.random() * .2

# x = rng.random() * aabb_extent[0] + aabb[0][0]
# y = rng.random() * aabb_extent[1] + aabb[0][1]
# # z = obj.get_position()[2] + rng.random() * 0.02
# z = obj.get_position() + rng.ran

# x_offset = x - obj_closest_point[0]
# y_offset = y - obj_closest_point[1]
# z_offset = z - obj_closest_point[2]#rng.random() * 0.02

# if check_hand_end_pose(ig_env, obj, [x_offset, y_offset, z_offset], ignore_collisions=True):
# break
# else:
# logging.info("Did not find params for grasp, return bad params and retry")

# I'm going to reverse engineer this thing: sample some pose that can reach the object,
# and then find a point in that direction vector.
# Step 1: find successful orientation
ik_success, orn = get_valid_orientation(ig_env, obj)
if not ik_success:
# logging.info("Failed to find valid orientation for grasping")
x_offset = rng.random() * aabb_extent[0] + aabb[0][0] - obj_closest_point[0]
y_offset = rng.random() * aabb_extent[1] + aabb[0][1] - obj_closest_point[1]
z_offset = rng.random() * aabb_extent[2] + aabb[0][2] - obj_closest_point[2]
else:
# logging.info(f"Found valid orientation for grasping")
x_offset = y_offset = z_offset = 0

return np.array([x_offset, y_offset, z_offset])
# # x = rng.random() * aabb_extent[0] + aabb[0][0]
# # y = rng.random() * aabb_extent[1] + aabb[0][1]
# # # z = obj.get_position()[2] + rng.random() * 0.02
# # z = obj.get_position() + rng.ran

# # x_offset = x - obj_closest_point[0]
# # y_offset = y - obj_closest_point[1]
# # z_offset = z - obj_closest_point[2]#rng.random() * 0.02

# # if check_hand_end_pose(ig_env, obj, [x_offset, y_offset, z_offset], ignore_collisions=True):
# # break
# # else:
# # logging.info("Did not find params for grasp, return bad params and retry")

# # I'm going to reverse engineer this thing: sample some pose that can reach the object,
# # and then find a point in that direction vector.
# # Step 1: find successful orientation
# ik_success, orn = get_valid_orientation(ig_env, obj)
# if not ik_success:
# # logging.info("Failed to find valid orientation for grasping")
# x_offset = rng.random() * aabb_extent[0] + aabb[0][0] - obj_closest_point[0]
# y_offset = rng.random() * aabb_extent[1] + aabb[0][1] - obj_closest_point[1]
# z_offset = rng.random() * aabb_extent[2] + aabb[0][2] - obj_closest_point[2]
# else:
# # logging.info(f"Found valid orientation for grasping")
# x_offset = y_offset = z_offset = 0

# return np.array([x_offset, y_offset, z_offset])

# Place OnTop sampler definition.
def place_ontop_obj_pos_sampler(
Expand Down Expand Up @@ -3215,9 +3215,10 @@ def place_under_obj_pos_sampler(
add_effects = {targ_holding}
delete_effects_ontop = {handempty, ontop, targ_reachable}
delete_effects_inside = {handempty, inside, targ_reachable}
sampler = (grasp_obj_param_sampler if
isinstance(env.igibson_behavior_env.robots[0], BehaviorRobot)
else grasp_obj_param_sampler_fetch)
# sampler = (grasp_obj_param_sampler if
# isinstance(env.igibson_behavior_env.robots[0], BehaviorRobot)
# else grasp_obj_param_sampler_fetch)
sampler = grasp_obj_param_sampler
# NSRT for grasping an object from ontop an object.
nsrt = NSRT(
f"{option.name}-{next(op_name_count_pick)}",
Expand Down

0 comments on commit 037b5e1

Please sign in to comment.