Skip to content

Commit

Permalink
added hacks
Browse files Browse the repository at this point in the history
  • Loading branch information
@wmcclinton
wmcclinton committed Nov 5, 2023
1 parent 99a9ba9 commit 7b97262
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 29 deletions.
6 changes: 4 additions & 2 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_relevant_scene_body_ids, \
reset_and_release_hand
reset_and_release_hand, get_scene_body_ids
from predicators.settings import CFG
from predicators.structs import Array

Expand Down Expand Up @@ -126,6 +126,7 @@ def sample_fn(env: "BehaviorEnv",
]
if CFG.behavior_option_model_rrt:
obstacles = get_relevant_scene_body_ids(env)
# obstacles = get_scene_body_ids(env)
if env.robots[0].parts["right_hand"].object_in_hand is not None:
if env.robots[0].parts["right_hand"].object_in_hand in obstacles:
obstacles.remove(
Expand Down Expand Up @@ -214,7 +215,7 @@ def make_grasp_plan(
# If the object is too far away, fail and return None
if (np.linalg.norm(
np.array(obj.get_position()) -
np.array(env.robots[0].get_position())) > 2):
np.array(env.robots[0].get_position())) > CFG.behavior_closeness_limit):
logging.info(f"PRIMITIVE: grasp {obj.name} fail, too far")
return None

Expand Down Expand Up @@ -433,6 +434,7 @@ def make_place_plan(
maxz = max(z, hand_z) + 0.5

obstacles = get_relevant_scene_body_ids(env, include_self=False)
# obstacles = get_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 Down
25 changes: 14 additions & 11 deletions predicators/behavior_utils/option_model_fns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,10 +192,13 @@ def create_place_option_model(
def placeOntopObjectOptionModel(_init_state: State,
env: "BehaviorEnv") -> 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()
if obj.get_body_id() == obj_in_hand_idx
][0]
try:
obj_in_hand = [
obj for obj in env.scene.get_objects()
if obj.get_body_id() == obj_in_hand_idx
][0]
except:
return
rh_orig_grasp_position = env.robots[0].parts[
"right_hand"].get_position()
rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation()
Expand Down Expand Up @@ -320,7 +323,7 @@ def openObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> 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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if hasattr(obj_to_open,
"states") and object_states.Open in obj_to_open.states:
obj_to_open.states[object_states.Open].set_value(True)
Expand All @@ -345,7 +348,7 @@ def closeObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> 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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if hasattr(obj_to_close,
"states") and object_states.Open in obj_to_close.states:
obj_to_close.states[object_states.Open].set_value(False)
Expand Down Expand Up @@ -384,7 +387,7 @@ def placeInsideObjectOptionModel(_init_state: State,
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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if (hasattr(obj_to_place_into, "states")
and object_states.Open in obj_to_place_into.states
and obj_to_place_into.states[object_states.Open].
Expand Down Expand Up @@ -498,7 +501,7 @@ def placeUnderObjectOptionModel(_init_state: State,
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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if (hasattr(obj_to_place_under, "states")
and object_states.Under in obj_to_place_under.states):
if obj_in_hand.states[object_states.Under].get_value(
Expand Down Expand Up @@ -602,7 +605,7 @@ def toggleOnObjectOptionModel(_init_state: State,
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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if hasattr(
obj_to_toggled_on, "states"
) and object_states.ToggledOn in obj_to_toggled_on.states:
Expand Down Expand Up @@ -643,7 +646,7 @@ def placeNextToObjectOptionModel(_init_state: State,
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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if (hasattr(obj_to_place_nextto, "states") and
object_states.NextTo in obj_to_place_nextto.states):
if obj_in_hand.states[object_states.NextTo].get_value(
Expand Down Expand Up @@ -747,7 +750,7 @@ def cleanDustyObjectOptionModel(_init_state: State,
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:
np.array(env.robots[0].get_position())) < CFG.behavior_closeness_limit:
if hasattr(
obj_to_clean,
"states") and object_states.Dusty in obj_to_clean.states:
Expand Down
2 changes: 1 addition & 1 deletion predicators/settings.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ class GlobalSettings:
behavior_option_model_eval = False
behavior_option_model_rrt = False
behavior_override_learned_samplers = False
behavior_closeness_limit = 1.00
behavior_closeness_limit = 2.00
# if this is True, then we will not use discovered failures in behavior.
behavior_ignore_discover_failures = True
is_running_rrt = False
Expand Down
Loading

0 comments on commit 7b97262

Please sign in to comment.