Skip to content

Commit

Permalink
started placeNextTo (#73)
Browse files Browse the repository at this point in the history 
* started placeNextTo

* debugging placeNextTo

* changed placeNextTo to placeNextToOnTop

* fixed

* fixed place under

* ran tests

* get rid of ipdb

---------

Co-authored-by: Willie McClinton <[email protected]>
  • Loading branch information
@kathrynle20 @wmcclinton
kathrynle20 and wmcclinton authored Mar 8, 2023
1 parent b0ba2f1 commit d7f2be3
Show file tree
Hide file tree
Showing 5 changed files with 273 additions and 30 deletions.
72 changes: 70 additions & 2 deletions predicators/behavior_utils/behavior_utils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@
'soap', 'wine_bottle', 'dishwasher', 'lollipop', 'cinnamon', 'pen', 'sink',
'bow', 'bath_towel', 'cruet', 'headset', 'coffee_cup', 'dishtowel',
'mouse', 'stove', 'duffel_bag', 'broom', 'stocking', 'parsley', 'yogurt',
'guacamole', 'paper_towel', 'modem', 'scanner', 'printer'
'guacamole', 'paper_towel', 'modem', 'scanner', 'printer', 'mousetrap',
'toilet'
}
PICK_PLACE_OBJECT_TYPES = {
'mineral_water',
Expand Down Expand Up @@ -582,7 +583,7 @@
'paper_towel', 'dishtowel', 'dial', 'folding_chair', 'deck', 'chair',
'hamper', 'bed', 'plate', 'work_surface', 'board', 'pallet',
'console_table', 'pool_table', 'electric_refrigerator', 'stand',
'room_floor', 'notebook', 'hardback'
'room_floor', 'notebook', 'hardback', 'toilet'
}
PLACE_INTO_SURFACE_OBJECT_TYPES = {
'shelf', 'sack', 'basket', 'dredging_bucket', 'cabinet', 'crock', 'bucket',
Expand Down Expand Up @@ -632,6 +633,9 @@
'facsimile',
}

PLACE_NEXT_TO_SURFACE_OBJECT_TYPES = {
'toilet',
}
CLEANING_OBJECT_TYPES = {
'toothbrush', 'towel', 'dinner_napkin', 'paper_towel', 'dishtowel',
'broom', 'vacuum', 'rag', 'carpet_sweeper', 'hand_towel', 'scraper',
Expand Down Expand Up @@ -1212,6 +1216,70 @@ def sample_place_ontop_params(igibson_behavior_env: "BehaviorEnv",
])


def sample_place_next_to_params(igibson_behavior_env: "BehaviorEnv",
obj_to_place_nextto: "URDFObject",
rng: np.random.Generator) -> Array:
"""Main logic for place next to param sampler.
Implemented in a separate method to enable code reuse in
option_model_fns.
"""

if obj_to_place_nextto.category == "toilet":
# Get the current env for collision checking.
obj_to_place_nextto_sampling_bounds = \
obj_to_place_nextto.bounding_box / 2
x_location = rng.uniform(-obj_to_place_nextto_sampling_bounds[0],
obj_to_place_nextto_sampling_bounds[0])
if x_location < 0:
x_location -= obj_to_place_nextto_sampling_bounds[0]
else:
x_location += obj_to_place_nextto_sampling_bounds[0]

sample_params = np.array([
x_location,
rng.uniform(-obj_to_place_nextto_sampling_bounds[1],
obj_to_place_nextto_sampling_bounds[1]),
rng.uniform(-obj_to_place_nextto_sampling_bounds[2],
obj_to_place_nextto_sampling_bounds[2])
])

logging.info("Sampling params for placeNextTo table...")

num_samples_tried = 0
while not check_hand_end_pose(igibson_behavior_env,
obj_to_place_nextto, sample_params):
x_location = rng.uniform(-obj_to_place_nextto_sampling_bounds[0],
obj_to_place_nextto_sampling_bounds[0])
if x_location < 0:
x_location -= obj_to_place_nextto_sampling_bounds[0]
else:
x_location += obj_to_place_nextto_sampling_bounds[0]

sample_params = np.array([
x_location,
rng.uniform(-obj_to_place_nextto_sampling_bounds[1],
obj_to_place_nextto_sampling_bounds[1]),
rng.uniform(-obj_to_place_nextto_sampling_bounds[2],
obj_to_place_nextto_sampling_bounds[2])
])
# 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 a certain number of tries so the
# planner will backtrack.
if num_samples_tried > MAX_PLACEONTOP_SAMPLES:
break
num_samples_tried += 1
return sample_params

sample_params = np.array([
rng.uniform(-0.5, 0.5),
rng.uniform(-0.5, 0.5),
rng.uniform(0.3, 1.0)
])
return sample_params


def sample_place_under_params(igibson_behavior_env: "BehaviorEnv",
obj_to_place_under: "URDFObject",
rng: np.random.Generator) -> Array:
Expand Down
99 changes: 96 additions & 3 deletions predicators/behavior_utils/option_model_fns.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@
from predicators import utils
from predicators.behavior_utils.behavior_utils import \
ALL_RELEVANT_OBJECT_TYPES, sample_navigation_params, \
sample_place_inside_params, sample_place_ontop_params, \
sample_place_under_params
sample_place_inside_params, sample_place_next_to_params, \
sample_place_ontop_params, sample_place_under_params
from predicators.settings import CFG
from predicators.structs import Object, State, Type

Expand Down Expand Up @@ -492,7 +492,6 @@ def placeUnderObjectOptionModel(_init_state: State,
else:
logging.info(f"PRIMITIVE: place {obj_in_hand.name} under "
f"{obj_to_place_under.name} fail, not under")
# import ipdb; ipdb.set_trace()
else:
logging.info(f"PRIMITIVE: place {obj_in_hand.name} under "
f"{obj_to_place_under.name} fail, too far")
Expand Down Expand Up @@ -537,6 +536,100 @@ def toggleOnObjectOptionModel(_init_state: State,
return toggleOnObjectOptionModel


def create_place_nextto_option_model(
plan: List[List[float]], _original_orientation: List[List[float]],
obj_to_place_nextto: "URDFObject"
) -> Callable[[State, "BehaviorEnv"], None]:
"""Instantiates and returns an placeNextTo option model given a dummy
plan."""

def placeNextToObjectOptionModel(_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]
rh_orig_grasp_position = env.robots[0].parts[
"right_hand"].get_position()
rh_orig_grasp_orn = env.robots[0].parts["right_hand"].get_orientation()
if obj_in_hand is not None and obj_in_hand != obj_to_place_nextto and \
isinstance(obj_to_place_nextto, URDFObject):
logging.info(
f"PRIMITIVE: attempt to place {obj_in_hand.name} next to "
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:
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(
obj_to_place_nextto) or obj_to_place_nextto.states[
object_states.NextTo].get_value(obj_in_hand):
logging.info(
f"PRIMITIVE: place {obj_in_hand.name} next to "
f"{obj_to_place_nextto.name} success")

# If we're not overriding the learned samplers, then we
# will directly use the elements of `plan`, which in turn
# use the outputs of the learned samplers. Otherwise, we
# will ignore these and use our oracle sampler to give us
# values to use.
if not CFG.behavior_override_learned_samplers:
target_pos = plan[-1][0:3]
target_orn = plan[-1][3:6]
else:
rng = np.random.default_rng(prng.randint(10000))
place_rel_pos = sample_place_next_to_params(
env, obj_to_place_nextto, rng)
target_pos_list = np.add(
place_rel_pos, obj_to_place_nextto.get_position())
target_pos_list[2] += 0.2
target_pos = target_pos_list.tolist()
target_orn = plan[-1][3:6]
logging.info(
f"PRIMITIVE: Overriding sample ({plan[-1][0:3]}" +
f", {plan[-1][3:6]}) and attempting to " +
f"place next to {obj_to_place_nextto.name} with "
f"params {target_pos}")
env.robots[0].parts["right_hand"].force_release_obj()
obj_to_place_nextto.force_wakeup()
obj_in_hand.set_position_orientation(
target_pos, p.getQuaternionFromEuler(target_orn))
# this is running a zero action to step simulator
env.step(np.zeros(env.action_space.shape))
# reset the released object to zero velocity so it
# doesn't fly away because of residual warp speeds
# from teleportation!
p.resetBaseVelocity(
obj_in_hand_idx,
linearVelocity=[0, 0, 0],
angularVelocity=[0, 0, 0],
)
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 to let the object fall into its place
for _ in range(15):
env.step(np.zeros(env.action_space.shape))
else:
logging.info(
f"PRIMITIVE: place {obj_in_hand.name} next to "
f"{obj_to_place_nextto.name} fail, not next to")
else:
logging.info(f"PRIMITIVE: place {obj_in_hand.name} next to "
f"{obj_to_place_nextto.name} fail, too far")
else:
logging.info(
"PRIMITIVE: place under failed with invalid obj params.")

obj_to_place_nextto.force_wakeup()
# Step the simulator to update visuals.
env.step(np.zeros(env.action_space.shape))

return placeNextToObjectOptionModel


def create_clean_dusty_option_model(
plan: List[List[float]], _original_orientation: List[List[float]],
obj_to_clean: "URDFObject") -> Callable[[State, "BehaviorEnv"], None]:
Expand Down
47 changes: 26 additions & 21 deletions predicators/envs/behavior.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,8 @@
create_clean_dusty_option_model, create_close_option_model, \
create_grasp_option_model, create_navigate_option_model, \
create_open_option_model, create_place_inside_option_model, \
create_place_option_model, create_place_under_option_model, \
create_toggle_on_option_model
create_place_nextto_option_model, create_place_option_model, \
create_place_under_option_model, create_toggle_on_option_model
from predicators.envs import BaseEnv
from predicators.settings import CFG
from predicators.structs import Action, Array, GroundAtom, Object, \
Expand Down Expand Up @@ -151,30 +151,35 @@ def set_options(self) -> None:
create_close_option_model,
create_place_inside_option_model,
create_toggle_on_option_model,
create_place_nextto_option_model,
create_clean_dusty_option_model,
create_place_under_option_model,
]

# name, planner_fn, option_policy_fn, option_model_fn,
# param_dim, arity, parameter upper and lower bounds
option_elems = [("NavigateTo", planner_fns[0], option_policy_fns[0],
option_model_fns[0], 2, 1, (-5.0, 5.0)),
("Grasp", planner_fns[1], option_policy_fns[1],
option_model_fns[1], 3, 1, (-np.pi, np.pi)),
("PlaceOnTop", planner_fns[2], option_policy_fns[2],
option_model_fns[2], 3, 1, (-1.0, 1.0)),
("Open", planner_fns[3], option_policy_fns[3],
option_model_fns[3], 3, 1, (-1.0, 1.0)),
("Close", planner_fns[3], option_policy_fns[3],
option_model_fns[4], 3, 1, (-1.0, 1.0)),
("PlaceInside", planner_fns[2], option_policy_fns[3],
option_model_fns[5], 3, 1, (-1.0, 1.0)),
("ToggleOn", planner_fns[3], option_policy_fns[3],
option_model_fns[6], 3, 1, (-1.0, 1.0)),
("CleanDusty", planner_fns[3], option_policy_fns[3],
option_model_fns[7], 3, 1, (-1.0, 1.0)),
("PlaceUnder", planner_fns[2], option_policy_fns[3],
option_model_fns[8], 3, 1, (-1.0, 1.0))]
option_elems = [
("NavigateTo", planner_fns[0], option_policy_fns[0],
option_model_fns[0], 2, 1, (-5.0, 5.0)),
("Grasp", planner_fns[1], option_policy_fns[1],
option_model_fns[1], 3, 1, (-np.pi, np.pi)),
("PlaceOnTop", planner_fns[2], option_policy_fns[2],
option_model_fns[2], 3, 1, (-1.0, 1.0)),
("Open", planner_fns[3], option_policy_fns[3], option_model_fns[3],
3, 1, (-1.0, 1.0)),
("Close", planner_fns[3], option_policy_fns[3],
option_model_fns[4], 3, 1, (-1.0, 1.0)),
("PlaceInside", planner_fns[2], option_policy_fns[3],
option_model_fns[5], 3, 1, (-1.0, 1.0)),
("ToggleOn", planner_fns[3], option_policy_fns[3],
option_model_fns[6], 3, 1, (-1.0, 1.0)),
("PlaceNextToOnTop", planner_fns[2], option_policy_fns[3],
option_model_fns[7], 3, 1, (-1.0, 1.0)),
("CleanDusty", planner_fns[3], option_policy_fns[3],
option_model_fns[8], 3, 1, (-1.0, 1.0)),
("PlaceUnder", planner_fns[2], option_policy_fns[3],
option_model_fns[9], 3, 1, (-1.0, 1.0))
]
self._options: Set[ParameterizedOption] = set()
for (name, planner_fn, policy_fn, option_model_fn, param_dim, num_args,
parameter_limits) in option_elems:
Expand Down Expand Up @@ -405,7 +410,7 @@ def predicates(self) -> Set[Predicate]:
# First, extract predicates from iGibson
for bddl_name in [
"inside",
# "nextto",
"nextto",
"ontop",
"under",
# "touching",
Expand Down
Loading

0 comments on commit d7f2be3

Please sign in to comment.