Skip to content

Add slice operator and support for ObjectMultiplexer #68

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add slice operator and support for ObjectMultiplexer #68

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 48 additions & 2 deletions predicators/behavior_utils/behavior_utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,11 @@
'document', 'bottom_cabinet_no_top', 'folder', 'bottom_cabinet',
'top_cabinet', 'sofa', 'oatmeal', 'chip', 'vegetable_oil', 'sugar',
'cabinet', 'floor', 'pasta', 'sauce', 'electric_refrigerator', 'olive_oil',
'sugar_jar', 'spaghetti_sauce', 'mayonnaise', 'fridge'
'sugar_jar', 'spaghetti_sauce', 'mayonnaise', 'fridge',
'tomato', 'strawberry', 'peach', 'jar', 'carving_knife', 'mushroom',
'chestnut', 'vidalia_onion', 'table_knife', 'sink', 'teapot', 'tea_bag',
'lemon', 'stove', 'casserole', 'pocketknife', 'half_tomato',
'half_mushroom', 'half_chestnut', 'half_vidalia_onion'
}
PICK_PLACE_OBJECT_TYPES = {
'mineral_water', 'oatmeal', 'blueberry', 'headset', 'jug', 'flank',
Expand Down Expand Up @@ -122,7 +126,7 @@
'peppermint', 'cruciferous_vegetable', 'soup_ladle', 'jean', 'teddy',
'chestnut', 'sauce', 'piece_of_cloth', 'whitefish', 'siren', 'balloon',
'celery', 'hot_pepper', 'raisin', 'sugar_jar', 'toy', 'sticky_note',
't-shirt'
't-shirt', 'table_knife'
}
PLACE_ONTOP_SURFACE_OBJECT_TYPES = {
'towel', 'tabletop', 'face', 'brim', 'cheddar', 'chaise_longue', 'stove',
Expand Down Expand Up @@ -184,6 +188,48 @@
'bottom_cabinet', 'trash_can'
}

SLICEABLE_OBJECT_TYPES = {
'cucumber', 'cheddar', 'kabob', 'lime', 'gauze', 'cotton', 'bulbous_plant',
'french_bread', 'mango', 'cauliflower', 'gumbo', 'salami', 'fish', 'blueberry',
'flank', 'hot_pepper', 'sweet_pepper', 'sauerkraut', 'chickpea', 'trout', 'kale',
'bun', 'vascular_plant', 'licorice', 'walnut', 'muskmelon', 'shrub', 'drupelet',
'shank', 'frozen_dessert', 'cherry', 'bell_pepper', 'pork', 'sandwich', 'pumpkin',
'turkey', 'pepper', 'winter_melon', 'rib', 'pretzel', 'curd', 'summer_squash',
'melon', 'lettuce', 'honeydew', 'nectarine', 'bean_curd', 'dish', 'gouda',
'cayenne', 'pear', 'gingerbread', 'apricot', 'noodle', 'sausage', 'plum', 'salmon',
'cheese', 'mint', 'eggplant', 'marjoram', 'gorgonzola', 'beefsteak', 'cabbage',
'hazelnut', 'saltwater_fish', 'cupcake', 'solanaceous_vegetable', 'turnip',
'venison', 'breast', 'bird', 'artichoke', 'pomegranate', 'brownie', 'pea',
'white_bread', 'soy_sauce', 'taco', 'egg', 'brie', 'ginger', 'rosemary', 'poultry',
'butter', 'citrus', 'anchovy', 'burrito', 'cornbread', 'grape', 'foil', 'lemon',
'chicory_escarole', 'marshmallow', 'ham', 'sushi', 'prosciutto', 'chicken',
'biscuit', 'rayon', 'parsley', 'red_salmon', 'fillet', 'chives', 'pepperoni',
'muffin', 'thyme', 'baguet', 'potato', 'radish', 'cracker', 'clove',
'alliaceous_plant', 'beef', 'peanut', 'tinfoil', 'julienne', 'garlic', 'scone',
'crust', 'cantaloup', 'porterhouse', 'avocado', 'pasta', 'pie', 'lint', 'pineapple',
'gooseberry', 'cut_of_pork', 'cheesecake', 'tomato', 'dried_fruit', 'banana',
'shallot', 'bow', 'chocolate', 'lasagna', 'blackberry', 'piece', 'salad_green',
'meat_loaf', 'pizza', 'brisket', 'bagel', 'freshwater_fish', 'dill', 'ice_lolly',
'asparagus', 'toast', 'watermelon', 'cut_of_beef', 'quick_bread', 'mushroom',
'olive', 'date', 'sheet_metal', 'saute', 'bacon', 'raspberry', 'alfalfa', 'tart',
'dough', 'pancake', 'tortilla', 'fig', 'strawberry', 'chili', 'knitwear', 'coconut',
'onion', 'spinach', 'carrot', 'grapefruit', 'mozzarella', 'pomelo', 'peach',
'drumstick', 'prawn', 'silver_salmon', 'parmesan', 'feijoa', 'pome', 'papaya',
'basil', 'chicory', 'almond', 'green_onion', 'woody_plant', 'head_cabbage', 'meatball',
'comfrey', 'drupe', 'currant', 'crouton', 'bleu', 'cos', 'margarine', 'radicchio',
'pita', 'nut', 'tuna', 'sirloin', 'veal', 'flatbread', 'steak', 'broccoli', 'casserole',
'peppermint', 'cruciferous_vegetable', 'cornice', 'loaf_of_bread', 'coriander', 'lamb',
'chestnut', 'liliaceous_plant', 'rump', 'zucchini', 'whitefish', 'vidalia_onion',
'kiwi', 'mandarin', 'game', 'celery', 'squash', 'hamburger', 'acetate_rayon', 'orange',
'cutlet', 'apple', 'tenderloin', 'fudge', 'raisin'
}

SLICER_OBJECT_TYPES = {
'hacksaw', 'stiletto', 'carving_knife', 'clipper', 'garden_tool', 'dagger',
'table_knife', 'razor', 'shaver', 'pruning_saw', 'knife', 'chisel', 'lawn_mower',
'saw', 'edge_tool', 'cleaver', 'scissors', 'handsaw', 'pocketknife', 'trimmer',
'shear', 'pruner', 'cutter'
}

def get_aabb_volume(lo: Array, hi: Array) -> float:
"""Simple utility function to compute the volume of an aabb.
Expand Down
25 changes: 19 additions & 6 deletions predicators/behavior_utils/motion_planner_fns.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
from igibson.object_states.on_floor import \
RoomFloor # pylint: disable=unused-import
from igibson.objects.articulated_object import URDFObject
from igibson.objects.multi_object_wrappers import ObjectMultiplexer
from igibson.utils.behavior_robot_planning_utils import \
plan_base_motion_br, plan_hand_motion_br

Expand Down Expand Up @@ -86,7 +87,7 @@ def sample_fn(env: "BehaviorEnv",

if not isinstance(
obj,
URDFObject): # must be a URDFObject so we can get its position!
(URDFObject, ObjectMultiplexer)): # must be a URDFObject so we can get its position!
logging.error("ERROR! Object to navigate to is not valid (not an "
"instance of URDFObject).")
p.restoreState(state)
Expand Down Expand Up @@ -201,7 +202,7 @@ def make_grasp_plan(

# If the object we're trying to grasp doesn't have all the attributes
# we'll need for assistive grasping, fail and return None
if not (isinstance(obj, URDFObject) and hasattr(obj, "states")
if not (isinstance(obj, (URDFObject, ObjectMultiplexer)) and hasattr(obj, "states")
and object_states.AABB in obj.states):
logging.info(f"PRIMITIVE: grasp {obj.name} fail, no object")
return None
Expand Down Expand Up @@ -385,7 +386,7 @@ def make_place_plan(
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
if (not isinstance(obj, ObjectMultiplexer) or obj.current_index == 0) and obj.get_body_id() == obj_in_hand_idx
][0]
logging.info(f"PRIMITIVE: attempt to place {obj_in_hand.name} ontop"
f"/inside {obj.name} with params {place_rel_pos}")
Expand All @@ -402,7 +403,7 @@ def make_place_plan(
return None

# if the object is not a urdf object, fail and return None
if not isinstance(obj, URDFObject):
if not isinstance(obj, (URDFObject, ObjectMultiplexer)):
logging.info(f"PRIMITIVE: place {obj_in_hand.name} ontop/inside "
f"{obj.name} fail, too far")
return None
Expand All @@ -423,7 +424,7 @@ def make_place_plan(
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
if (not isinstance(obj, ObjectMultiplexer) or obj.current_index == 0) and 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()
Expand All @@ -436,7 +437,19 @@ def make_place_plan(
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)
if isinstance(obj_in_hand, ObjectMultiplexer):
# assert obj_in_hand.current_index == 0
# obj_in_hand.set_selection(1)
# obj_in_hand_idx_0 = obj_in_hand.objects[0].get_body_id()
# obj_in_hand_idx_1 = obj_in_hand.objects[1].get_body_id()
# obstacles.remove(obj_in_hand_idx_0)
# obstacles.remove(obj_in_hand_idx_1)
# obj_in_hand.set_selection(0)
# Note: it seems that object multiplexers don't show up in the list
# of obstacles, so we can just skip this step
pass
else:
obstacles.remove(env.robots[0].parts["right_hand"].object_in_hand)
end_conf = [
x,
y,
Expand Down
27 changes: 26 additions & 1 deletion predicators/behavior_utils/option_model_fns.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
BehaviorEnv # pylint: disable=unused-import
from igibson.objects.articulated_object import \
URDFObject # pylint: disable=unused-import
from igibson.objects.multi_object_wrappers import ObjectMultiplexer
except (ImportError, ModuleNotFoundError) as e:
pass

Expand Down Expand Up @@ -210,7 +211,7 @@ def placeInsideObjectOptionModel(_init_state: State,
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
if (not isinstance(obj, ObjectMultiplexer) or obj.current_index == 0) and obj.get_body_id() == obj_in_hand_idx
][0]
rh_orig_grasp_position = env.robots[0].parts[
"right_hand"].get_position()
Expand Down Expand Up @@ -268,3 +269,27 @@ def placeInsideObjectOptionModel(_init_state: State,
env.step(np.zeros(env.action_space.shape))

return placeInsideObjectOptionModel

def create_slice_option_model(
plan: List[List[float]], _original_orientation: List[List[float]],
obj_to_slice: "URDFObject") -> Callable[[State, "BehaviorEnv"], None]:
"""Instantiates and returns a slice option model given a dummy plan."""
del plan

def sliceObjectOptionModel(_init_state: State, env: "BehaviorEnv") -> None:
logging.info(f"PRIMITIVE: Attempting to slice {obj_to_slice.name}")
if np.linalg.norm(
np.array(obj_to_slice.get_position()) -
np.array(env.robots[0].get_position())) < 2:
if hasattr(obj_to_slice,
"states") and object_states.Sliced in obj_to_slice.states:
obj_to_slice.states[object_states.Sliced].set_value(True)
else:
logging.info("PRIMITIVE slice failed, cannot be sliced")
else:
logging.info("PRIMITIVE slice failed, too far")
obj_to_slice.force_wakeup()
# Step the simulator to update visuals.
env.step(np.zeros(env.action_space.shape))

return sliceObjectOptionModel
Loading