Skip to content

Commit

Permalink
Fixing videos (#87)
Browse files Browse the repository at this point in the history 
* 20 task yaml update

* updated yaml

* updated to 10 seeds

* always create dataset

* script to run 8 tasks

* added extra 5 tasks for eval

* still broken

* added yaml file

* added hand rrt

* added grasp and place movement

* fixed uneccessary changes

* reverted

* removed TODOs

* ran checks

* fixed lint

* fixed lint

* lint

---------

Co-authored-by: Nishanth Kumar <NishanthJKumar@users.noreply.github.com>
  • Loading branch information
@wmcclinton @NishanthJKumar
wmcclinton and NishanthJKumar authored Nov 2, 2023
1 parent 980026d commit 365bc9e
Show file tree
Hide file tree
Showing 7 changed files with 254 additions and 87 deletions.
37 changes: 36 additions & 1 deletion predicators/behavior_utils/behavior_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -856,6 +856,41 @@ 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
the robot's hand)"""
Expand Down Expand Up @@ -1076,7 +1111,7 @@ def sample_navigation_params(igibson_behavior_env: "BehaviorEnv",
Implemented in a separate method to enable code reuse in
option_model_fns.
"""
closeness_limit = 2.00
closeness_limit = CFG.behavior_closeness_limit
nearness_limit = 0.15
distance = nearness_limit + (
(closeness_limit - nearness_limit) * rng.random())
Expand Down
145 changes: 77 additions & 68 deletions predicators/behavior_utils/motion_planner_fns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -24,7 +24,7 @@
RoomFloor # pylint: disable=unused-import
from igibson.objects.articulated_object import URDFObject
from igibson.utils.behavior_robot_planning_utils import \
plan_base_motion_br # , plan_hand_motion_br
plan_base_motion_br, plan_hand_motion_br

except (ImportError, ModuleNotFoundError) as e:
pass
Expand Down Expand Up @@ -124,17 +124,20 @@ 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)
if env.robots[0].parts["right_hand"].object_in_hand in obstacles:
obstacles.remove(
env.robots[0].parts["right_hand"].object_in_hand)
plan = plan_base_motion_br(
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:
Expand All @@ -147,6 +150,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)
Expand Down Expand Up @@ -221,13 +225,13 @@ def make_grasp_plan(
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
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

# compute the angle the hand must be in such that it can
# grasp the object from its current offset position
Expand Down Expand Up @@ -275,25 +279,26 @@ def make_grasp_plan(
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]]
# 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,
# )
# p.restoreState(state)
# else:
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 CFG.behavior_option_model_rrt:
# plan a motion to the pose [x, y, z, euler_angles[0],
# euler_angles[1], euler_angles[2]]
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_relevant_scene_body_ids(env,
include_self=True,
include_right_hand=True),
rng=rng,
)
p.restoreState(state)
else:
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]

# NOTE: This below line is *VERY* important after the
# pybullet state is restored. The hands keep an internal
Expand All @@ -305,11 +310,12 @@ 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
# 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]

# Grasping Phase 2: Move along the vector from the
# position the hand ends up in to the object and
Expand Down Expand Up @@ -417,17 +423,18 @@ 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)
obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand)
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_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 = [
x,
y,
Expand All @@ -437,22 +444,23 @@ def make_place_plan(
0,
]
# For now we are not running rrt with place.
# if False:
# 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:
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 CFG.behavior_option_model_rrt:
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:
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]

# NOTE: This below line is *VERY* important after the
# pybullet state is restored. The hands keep an internal
Expand All @@ -464,11 +472,12 @@ 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
plan = plan + [end_conf]

original_orientation = list(
p.getEulerFromQuaternion(
Expand Down
Loading

0 comments on commit 365bc9e

Please sign in to comment.