From 1743bdd43562c8f9832030fa8c1f8c00cad2ca4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 5 Sep 2023 13:39:47 -0700 Subject: [PATCH 01/29] Some fixes and a knife test --- omnigibson/configs/homeboy.yaml | 1 + omnigibson/robots/manipulation_robot.py | 2 +- omnigibson/utils/grasping_planning_utils.py | 2 +- tests/test_symbolic_primitives.py | 27 +++++++++++++++++---- 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/omnigibson/configs/homeboy.yaml b/omnigibson/configs/homeboy.yaml index 171888c2b..c28beb53e 100644 --- a/omnigibson/configs/homeboy.yaml +++ b/omnigibson/configs/homeboy.yaml @@ -32,6 +32,7 @@ robots: action_normalize: false action_type: continuous grasping_mode: sticky + disable_grasp_handling: True rigid_trunk: false default_trunk_offset: 0.365 default_arm_pose: diagonal30 diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index c41cca57d..4ec51809e 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -112,7 +112,7 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", - disable_grasp_handling=False, + disable_grasp_handling=True, # TODO: revert to False. This is for debugging purposes **kwargs, ): diff --git a/omnigibson/utils/grasping_planning_utils.py b/omnigibson/utils/grasping_planning_utils.py index b81c4abbf..16b09cd49 100644 --- a/omnigibson/utils/grasping_planning_utils.py +++ b/omnigibson/utils/grasping_planning_utils.py @@ -4,7 +4,7 @@ from math import ceil import omnigibson.utils.transform_utils as T -from omnigibson.object_states.open import _get_relevant_joints +from omnigibson.object_states.open_state import _get_relevant_joints from omnigibson.utils.constants import JointType, JointAxis from omni.isaac.core.utils.rotations import gf_quat_to_np_array diff --git a/tests/test_symbolic_primitives.py b/tests/test_symbolic_primitives.py index 97c28a142..17da562c7 100644 --- a/tests/test_symbolic_primitives.py +++ b/tests/test_symbolic_primitives.py @@ -34,6 +34,7 @@ def start_env(): "action_normalize": False, "action_type": "continuous", "grasping_mode": "sticky", + "disable_grasp_handling": True, "rigid_trunk": False, "default_trunk_offset": 0.365, "default_arm_pose": "diagonal30", @@ -75,6 +76,13 @@ def start_env(): "model": "mhndon", "position": [5.31, 10.75, 1.], }, + { + "type": "DatasetObject", + "name": "knife", + "category": "carving_knife", + "model": "awvoox", + "position": [5.31, 10.75, 1.2], + }, { "type": "DatasetObject", "name": "steak", @@ -130,6 +138,10 @@ def steak(env): def sponge(env): return next(iter(env.scene.object_registry("category", "sponge"))) +@pytest.fixture +def knife(env): + return next(iter(env.scene.object_registry("category", "carving_knife"))) + # def test_navigate(): # pass @@ -177,8 +189,13 @@ def test_place_inside(env, prim_gen, steak, fridge): # def test_wipe(): # pass -# def test_cut(): -# pass +def test_cut(env, prim_gen, steak, knife): + # assert not steak.states[object_states.Cut].get_value(knife) + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.GRASP, knife): + env.step(action) + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.CUT, steak): + env.step(action) + # assert steak.states[object_states.Sliced].get_value(knife) # def test_place_near_heating_element(): # pass @@ -192,14 +209,14 @@ def main(): robot = env.robots[0] prim_gen = SymbolicSemanticActionPrimitives(None, scene, robot) steak = next(iter(env.scene.object_registry("category", "steak"))) - fridge = next(iter(env.scene.object_registry("category", "fridge"))) + knife = next(iter(env.scene.object_registry("category", "carving_knife"))) try: - test_place_inside(env, prim_gen, steak, fridge) + test_cut(env, prim_gen, steak, knife) except: pass while True: - og.sim.render() + og.sim.step() if __name__ == "__main__": main() \ No newline at end of file From 6600df96f6f676de297ade6e670edee6ac07132b Mon Sep 17 00:00:00 2001 From: cremebrule <84cremebrule@gmail.com> Date: Tue, 5 Sep 2023 14:25:24 -0700 Subject: [PATCH 02/29] add ability to specify sensor config for robots --- omnigibson/robots/fetch.py | 6 ++ omnigibson/robots/manipulation_robot.py | 6 ++ omnigibson/robots/robot_base.py | 137 +++++++++++++++++++++--- omnigibson/robots/tiago.py | 6 ++ omnigibson/sensors/__init__.py | 11 +- 5 files changed, 150 insertions(+), 16 deletions(-) diff --git a/omnigibson/robots/fetch.py b/omnigibson/robots/fetch.py index d956cf2a8..ebbc189d8 100644 --- a/omnigibson/robots/fetch.py +++ b/omnigibson/robots/fetch.py @@ -56,6 +56,7 @@ def __init__( # Unique to BaseRobot obs_modalities="all", proprio_obs="default", + sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", @@ -99,10 +100,14 @@ def __init__( obs_modalities (str or list of str): Observation modalities to use for this robot. Default is "all", which corresponds to all modalities being used. Otherwise, valid options should be part of omnigibson.sensors.ALL_SENSOR_MODALITIES. + Note: If @sensor_config explicitly specifies `modalities` for a given sensor class, it will + override any values specified from @obs_modalities! proprio_obs (str or list of str): proprioception observation key(s) to use for generating proprioceptive observations. If str, should be exactly "default" -- this results in the default proprioception observations being used, as defined by self.default_proprio_obs. See self._get_proprioception_dict for valid key choices + sensor_config (None or dict): nested dictionary mapping sensor class name(s) to specific sensor + configurations for this object. This will override any default values specified by this class. grasping_mode (str): One of {"physical", "assisted", "sticky"}. If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. @@ -149,6 +154,7 @@ def __init__( reset_joint_pos=reset_joint_pos, obs_modalities=obs_modalities, proprio_obs=proprio_obs, + sensor_config=sensor_config, grasping_mode=grasping_mode, **kwargs, ) diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index bdc899f68..6927e7e31 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -102,6 +102,7 @@ def __init__( # Unique to BaseRobot obs_modalities="all", proprio_obs="default", + sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", @@ -141,10 +142,14 @@ def __init__( obs_modalities (str or list of str): Observation modalities to use for this robot. Default is "all", which corresponds to all modalities being used. Otherwise, valid options should be part of omnigibson.sensors.ALL_SENSOR_MODALITIES. + Note: If @sensor_config explicitly specifies `modalities` for a given sensor class, it will + override any values specified from @obs_modalities! proprio_obs (str or list of str): proprioception observation key(s) to use for generating proprioceptive observations. If str, should be exactly "default" -- this results in the default proprioception observations being used, as defined by self.default_proprio_obs. See self._get_proprioception_dict for valid key choices + sensor_config (None or dict): nested dictionary mapping sensor class name(s) to specific sensor + configurations for this object. This will override any default values specified by this class. grasping_mode (str): One of {"physical", "assisted", "sticky"}. If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. @@ -189,6 +194,7 @@ def __init__( reset_joint_pos=reset_joint_pos, obs_modalities=obs_modalities, proprio_obs=proprio_obs, + sensor_config=sensor_config, **kwargs, ) diff --git a/omnigibson/robots/robot_base.py b/omnigibson/robots/robot_base.py index 21c8a61d4..266cf9a59 100644 --- a/omnigibson/robots/robot_base.py +++ b/omnigibson/robots/robot_base.py @@ -7,7 +7,7 @@ from omnigibson.objects.usd_object import USDObject from omnigibson.objects.controllable_object import ControllableObject from omnigibson.utils.gym_utils import GymObservable -from omnigibson.utils.python_utils import classproperty +from omnigibson.utils.python_utils import classproperty, merge_nested_dicts from omnigibson.utils.vision_utils import segmentation_to_rgb from omnigibson.utils.constants import PrimType from pxr import PhysxSchema @@ -59,6 +59,7 @@ def __init__( # Unique to this class obs_modalities="all", proprio_obs="default", + sensor_config=None, **kwargs, ): @@ -95,10 +96,14 @@ def __init__( obs_modalities (str or list of str): Observation modalities to use for this robot. Default is "all", which corresponds to all modalities being used. Otherwise, valid options should be part of omnigibson.sensors.ALL_SENSOR_MODALITIES. + Note: If @sensor_config explicitly specifies `modalities` for a given sensor class, it will + override any values specified from @obs_modalities! proprio_obs (str or list of str): proprioception observation key(s) to use for generating proprioceptive observations. If str, should be exactly "default" -- this results in the default proprioception observations being used, as defined by self.default_proprio_obs. See self._get_proprioception_dict for valid key choices + sensor_config (None or dict): nested dictionary mapping sensor class name(s) to specific sensor + configurations for this object. This will override any default values specified by this class. kwargs (dict): Additional keyword arguments that are used for other super() calls from subclasses, allowing for flexible compositions of various object subclasses (e.g.: Robot is USDObject + ControllableObject). """ @@ -106,6 +111,7 @@ def __init__( self._obs_modalities = obs_modalities if obs_modalities == "all" else \ {obs_modalities} if isinstance(obs_modalities, str) else set(obs_modalities) # this will get updated later when we fill in our sensors self._proprio_obs = self.default_proprio_obs if proprio_obs == "default" else list(proprio_obs) + self._sensor_config = sensor_config # Process abilities robot_abilities = {"robot": {}} @@ -148,6 +154,32 @@ def _post_load(self): # Run super post load first super()._post_load() + # Load the sensors + self._load_sensors() + + def _initialize(self): + # Run super first + super()._initialize() + + # Initialize all sensors + for sensor in self._sensors.values(): + sensor.initialize() + + # Load the observation space for this robot + self.load_observation_space() + + # Validate this robot configuration + self._validate_configuration() + + def _load_sensors(self): + """ + Loads sensor(s) to retrieve observations from this object. + Stores created sensors as dictionary mapping sensor names to specific sensor + instances used by this object. + """ + # Populate sensor config + self._sensor_config = self._generate_sensor_config(custom_config=self._sensor_config) + # Search for any sensors this robot might have attached to any of its links self._sensors = dict() obs_modalities = set() @@ -158,15 +190,17 @@ def _post_load(self): if prim_type in SENSOR_PRIMS_TO_SENSOR_CLS: # Infer what obs modalities to use for this sensor sensor_cls = SENSOR_PRIMS_TO_SENSOR_CLS[prim_type] - modalities = sensor_cls.all_modalities if self._obs_modalities == "all" else \ - sensor_cls.all_modalities.intersection(self._obs_modalities) - obs_modalities = obs_modalities.union(modalities) + sensor_kwargs = self._sensor_config[sensor_cls.__name__] + if "modalities" not in sensor_kwargs: + sensor_kwargs["modalities"] = sensor_cls.all_modalities if self._obs_modalities == "all" else \ + sensor_cls.all_modalities.intersection(self._obs_modalities) + obs_modalities = obs_modalities.union(sensor_kwargs["modalities"]) # Create the sensor and store it internally sensor = create_sensor( sensor_type=prim_type, prim_path=str(prim.GetPrimPath()), name=f"{self.name}:{link_name}_{prim_type}_sensor", - modalities=modalities, + **sensor_kwargs, ) self._sensors[sensor.name] = sensor @@ -177,19 +211,28 @@ def _post_load(self): # Update our overall obs modalities self._obs_modalities = obs_modalities - def _initialize(self): - # Run super first - super()._initialize() + def _generate_sensor_config(self, custom_config=None): + """ + Generates a fully-populated sensor config, overriding any default values with the corresponding values + specified in @custom_config - # Initialize all sensors - for sensor in self._sensors.values(): - sensor.initialize() + Args: + custom_config (None or Dict[str, ...]): nested dictionary mapping sensor class name(s) to specific custom + sensor configurations for this object. This will override any default values specified by this class - # Load the observation space for this robot - self.load_observation_space() + Returns: + dict: Fully-populated nested dictionary mapping sensor class name(s) to specific sensor configurations + for this object + """ + sensor_config = {} if custom_config is None else deepcopy(custom_config) - # Validate this robot configuration - self._validate_configuration() + # Merge the sensor dictionaries + sensor_config = merge_nested_dicts( + base_dict=self._default_sensor_config, + extra_dict=sensor_config, + ) + + return sensor_config def _validate_configuration(self): """ @@ -392,6 +435,70 @@ def proprioception_dim(self): """ return len(self.get_proprioception()) + @property + def _default_sensor_config(self): + """ + Returns: + dict: default nested dictionary mapping sensor class name(s) to specific sensor + configurations for this object. See kwargs from omnigibson/sensors/__init__/create_sensor for more + details + + Expected structure is as follows: + SensorClassName1: + modalities: ... + enabled: ... + noise_type: ... + noise_kwargs: + ... + sensor_kwargs: + ... + SensorClassName2: + modalities: ... + enabled: ... + noise_type: ... + noise_kwargs: + ... + sensor_kwargs: + ... + ... + """ + return { + "VisionSensor": { + "enabled": True, + "noise_type": None, + "noise_kwargs": None, + "sensor_kwargs": { + "image_height": 128, + "image_width": 128, + }, + }, + "ScanSensor": { + "enabled": True, + "noise_type": None, + "noise_kwargs": None, + "sensor_kwargs": { + + # Basic LIDAR kwargs + "min_range": 0.05, + "max_range": 10.0, + "horizontal_fov": 360.0, + "vertical_fov": 1.0, + "yaw_offset": 0.0, + "horizontal_resolution": 1.0, + "vertical_resolution": 1.0, + "rotation_rate": 0.0, + "draw_points": False, + "draw_lines": False, + + # Occupancy Grid kwargs + "occupancy_grid_resolution": 128, + "occupancy_grid_range": 5.0, + "occupancy_grid_inner_radius": 0.5, + "occupancy_grid_local_link": None, + }, + }, + } + @property def default_proprio_obs(self): """ diff --git a/omnigibson/robots/tiago.py b/omnigibson/robots/tiago.py index 1387778a5..104fbd994 100644 --- a/omnigibson/robots/tiago.py +++ b/omnigibson/robots/tiago.py @@ -71,6 +71,7 @@ def __init__( # Unique to BaseRobot obs_modalities="all", proprio_obs="default", + sensor_config=None, # Unique to ManipulationRobot grasping_mode="physical", @@ -116,10 +117,14 @@ def __init__( obs_modalities (str or list of str): Observation modalities to use for this robot. Default is "all", which corresponds to all modalities being used. Otherwise, valid options should be part of omnigibson.sensors.ALL_SENSOR_MODALITIES. + Note: If @sensor_config explicitly specifies `modalities` for a given sensor class, it will + override any values specified from @obs_modalities! proprio_obs (str or list of str): proprioception observation key(s) to use for generating proprioceptive observations. If str, should be exactly "default" -- this results in the default proprioception observations being used, as defined by self.default_proprio_obs. See self._get_proprioception_dict for valid key choices + sensor_config (None or dict): nested dictionary mapping sensor class name(s) to specific sensor + configurations for this object. This will override any default values specified by this class. grasping_mode (str): One of {"physical", "assisted", "sticky"}. If "physical", no assistive grasping will be applied (relies on contact friction + finger force). If "assisted", will magnetize any object touching and within the gripper's fingers. @@ -169,6 +174,7 @@ def __init__( reset_joint_pos=reset_joint_pos, obs_modalities=obs_modalities, proprio_obs=proprio_obs, + sensor_config=sensor_config, grasping_mode=grasping_mode, **kwargs, ) diff --git a/omnigibson/sensors/__init__.py b/omnigibson/sensors/__init__.py index 11ba84d2c..215aa0924 100644 --- a/omnigibson/sensors/__init__.py +++ b/omnigibson/sensors/__init__.py @@ -19,6 +19,7 @@ def create_sensor( prim_path, name, modalities="all", + enabled=True, sensor_kwargs=None, noise_type=None, noise_kwargs=None @@ -35,6 +36,7 @@ def create_sensor( name (str): Name for the sensor. Names need to be unique per scene. modalities (str or list of str): Modality(s) supported by this sensor. Valid options are part of sensor.all_modalities. Default is "all", which corresponds to all modalities being used + enabled (bool): Whether this sensor should be enabled or not sensor_kwargs (dict): Any keyword kwargs to pass to the constructor noise_type (str): Type of sensor to create. Should be one of REGISTERED_SENSOR_NOISES (i.e.: the string name of the desired class to create) @@ -64,6 +66,13 @@ def create_sensor( # Create the sensor sensor_kwargs = dict() if sensor_kwargs is None else sensor_kwargs - sensor = sensor_cls(prim_path=prim_path, name=name, modalities=modalities, noise=noise, **sensor_kwargs) + sensor = sensor_cls( + prim_path=prim_path, + name=name, + modalities=modalities, + enabled=enabled, + noise=noise, + **sensor_kwargs, + ) return sensor From e555affbc0d8c2f7d9a55a959a15b169a88803e1 Mon Sep 17 00:00:00 2001 From: cremebrule <84cremebrule@gmail.com> Date: Tue, 5 Sep 2023 14:28:03 -0700 Subject: [PATCH 03/29] update configs with sensor_config --- omnigibson/configs/fetch_behavior.yaml | 9 +++++++ omnigibson/configs/robots/fetch.yaml | 9 +++++++ omnigibson/configs/robots/freight.yaml | 9 +++++++ omnigibson/configs/robots/husky.yaml | 9 +++++++ omnigibson/configs/robots/locobot.yaml | 9 +++++++ omnigibson/configs/robots/turtlebot.yaml | 9 +++++++ omnigibson/configs/sensors/scan.yaml | 30 ++++++++++++++++++++++++ omnigibson/configs/sensors/vision.yaml | 14 +++++++++++ omnigibson/configs/turtlebot_nav.yaml | 9 +++++++ 9 files changed, 107 insertions(+) create mode 100644 omnigibson/configs/sensors/scan.yaml create mode 100644 omnigibson/configs/sensors/vision.yaml diff --git a/omnigibson/configs/fetch_behavior.yaml b/omnigibson/configs/fetch_behavior.yaml index 84da35e39..a2e57cad0 100644 --- a/omnigibson/configs/fetch_behavior.yaml +++ b/omnigibson/configs/fetch_behavior.yaml @@ -33,6 +33,15 @@ robots: default_trunk_offset: 0.365 default_arm_pose: diagonal30 reset_joint_pos: tuck + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: DifferentialDriveController diff --git a/omnigibson/configs/robots/fetch.yaml b/omnigibson/configs/robots/fetch.yaml index 42bdeb8ac..9d8054e17 100644 --- a/omnigibson/configs/robots/fetch.yaml +++ b/omnigibson/configs/robots/fetch.yaml @@ -19,6 +19,15 @@ robot: rigid_trunk: false default_trunk_offset: 0.365 default_arm_pose: vertical + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: DifferentialDriveController diff --git a/omnigibson/configs/robots/freight.yaml b/omnigibson/configs/robots/freight.yaml index e3de96e4f..7431b464d 100644 --- a/omnigibson/configs/robots/freight.yaml +++ b/omnigibson/configs/robots/freight.yaml @@ -10,6 +10,15 @@ robot: base_name: null scale: 1.0 self_collision: false + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: DifferentialDriveController \ No newline at end of file diff --git a/omnigibson/configs/robots/husky.yaml b/omnigibson/configs/robots/husky.yaml index 9f63cd58b..d35680b5b 100644 --- a/omnigibson/configs/robots/husky.yaml +++ b/omnigibson/configs/robots/husky.yaml @@ -10,6 +10,15 @@ robot: base_name: null scale: 1.0 self_collision: false + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: JointController \ No newline at end of file diff --git a/omnigibson/configs/robots/locobot.yaml b/omnigibson/configs/robots/locobot.yaml index f5565fb75..c36e144f3 100644 --- a/omnigibson/configs/robots/locobot.yaml +++ b/omnigibson/configs/robots/locobot.yaml @@ -10,6 +10,15 @@ robot: base_name: null scale: 1.0 self_collision: false + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: DifferentialDriveController \ No newline at end of file diff --git a/omnigibson/configs/robots/turtlebot.yaml b/omnigibson/configs/robots/turtlebot.yaml index 28fdc8613..0431a430f 100644 --- a/omnigibson/configs/robots/turtlebot.yaml +++ b/omnigibson/configs/robots/turtlebot.yaml @@ -10,6 +10,15 @@ robot: base_name: null scale: 1.0 self_collision: false + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: DifferentialDriveController \ No newline at end of file diff --git a/omnigibson/configs/sensors/scan.yaml b/omnigibson/configs/sensors/scan.yaml new file mode 100644 index 000000000..632bbb793 --- /dev/null +++ b/omnigibson/configs/sensors/scan.yaml @@ -0,0 +1,30 @@ +# Example ScanSensor sensor config +# See omnigibson/sensors/__init__/create_sensor and omnigibson/sensors/scan_sensor for docstring of arguments +# Arguments below are the arguments that should be specified by external user (other kwargs +# used in constructor are generated automatically at runtime) +robot: + sensor_config: + VisionSensor: + modalities: [scan, occupancy_grid] # if specified, this will override the values in robots_config["obs_modalities"] + enabled: true + noise_type: null + noise_kwargs: null + sensor_kwargs: + + # Basic LIDAR kwargs + min_range: 0.05 + max_range: 10.0 + horizontal_fov: 360.0 + vertical_fov: 1.0 + yaw_offset: 0.0 + horizontal_resolution: 1.0 + vertical_resolution: 1.0 + rotation_rate: 0.0 + draw_points: false + draw_lines: false + + # Occupancy Grid kwargs + occupancy_grid_resolution: 128 + occupancy_grid_range: 5.0 + occupancy_grid_inner_radius: 0.5 + occupancy_grid_local_link: null \ No newline at end of file diff --git a/omnigibson/configs/sensors/vision.yaml b/omnigibson/configs/sensors/vision.yaml new file mode 100644 index 000000000..44fdfc6c6 --- /dev/null +++ b/omnigibson/configs/sensors/vision.yaml @@ -0,0 +1,14 @@ +# Example VisionSensor sensor config +# See omnigibson/sensors/__init__/create_sensor and omnigibson/sensors/vision_sensor for docstring of arguments +# Arguments below are the arguments that should be specified by external user (other kwargs +# used in constructor are generated automatically at runtime) +robot: + sensor_config: + VisionSensor: + modalities: [rgb, depth] # if specified, this will override the values in robots_config["obs_modalities"] + enabled: true + noise_type: null + noise_kwargs: null + sensor_kwargs: + image_height: 128 + image_width: 128 \ No newline at end of file diff --git a/omnigibson/configs/turtlebot_nav.yaml b/omnigibson/configs/turtlebot_nav.yaml index 864af5d12..36abc695f 100644 --- a/omnigibson/configs/turtlebot_nav.yaml +++ b/omnigibson/configs/turtlebot_nav.yaml @@ -29,6 +29,15 @@ robots: self_collision: false action_normalize: true action_type: continuous + sensor_config: + VisionSensor: + sensor_kwargs: + image_height: 128 + image_width: 128 + ScanSensor: + sensor_kwargs: + min_range: 0.05 + max_range: 10.0 controller_config: base: name: DifferentialDriveController From 20040b8cf9a938edb0f90fae3d736922e16871cd Mon Sep 17 00:00:00 2001 From: cremebrule <84cremebrule@gmail.com> Date: Tue, 5 Sep 2023 15:58:01 -0700 Subject: [PATCH 04/29] fix typo --- omnigibson/configs/sensors/scan.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/configs/sensors/scan.yaml b/omnigibson/configs/sensors/scan.yaml index 632bbb793..318b761de 100644 --- a/omnigibson/configs/sensors/scan.yaml +++ b/omnigibson/configs/sensors/scan.yaml @@ -4,7 +4,7 @@ # used in constructor are generated automatically at runtime) robot: sensor_config: - VisionSensor: + ScanSensor: modalities: [scan, occupancy_grid] # if specified, this will override the values in robots_config["obs_modalities"] enabled: true noise_type: null From e4b3ded34c5ca57e525b73f2d54e828c0cda0f25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 12 Sep 2023 13:16:09 -0700 Subject: [PATCH 05/29] Implement ability to trigger rules manually --- .../symbolic_semantic_action_primitives.py | 18 +++++-- omnigibson/transition_rules.py | 4 ++ tests/test_symbolic_primitives.py | 48 +++++++++++++------ 3 files changed, 52 insertions(+), 18 deletions(-) diff --git a/omnigibson/action_primitives/symbolic_semantic_action_primitives.py b/omnigibson/action_primitives/symbolic_semantic_action_primitives.py index a073b7f64..c2b6ffeff 100644 --- a/omnigibson/action_primitives/symbolic_semantic_action_primitives.py +++ b/omnigibson/action_primitives/symbolic_semantic_action_primitives.py @@ -16,6 +16,7 @@ import gym import numpy as np +from omnigibson.transition_rules import REGISTERED_RULES, TransitionRuleAPI from scipy.spatial.transform import Rotation, Slerp from omnigibson.utils.constants import JointType from pxr import PhysxSchema @@ -270,6 +271,9 @@ def _grasp(self, obj: DatasetObject): {"target object": obj.name, "object currently in hand": obj_in_hand.name}, ) + # Get close + yield from self._navigate_if_needed(obj) + # Perform forced assisted grasp obj.set_position(self.robot.get_eef_position(self.arm)) self.robot._ag_data[self.arm] = (obj, obj.root_link) @@ -613,12 +617,18 @@ def _cut(self, obj): "The target object is not sliceable or diceable.", {"target object": obj.name} ) + + # Get close + yield from self._navigate_if_needed(obj) - # TODO: Trigger the slicing rule manually. - - - pass + # TODO: Do some more validation + added_obj_attrs = [] + removed_objs = [] + output = REGISTERED_RULES["SlicingRule"].transition({"sliceable": [obj]}) + added_obj_attrs += output.add + removed_objs += output.remove + TransitionRuleAPI.execute_transition(added_obj_attrs=added_obj_attrs, removed_objs=removed_objs) def _place_near_heating_element(self, heat_source_obj): obj_in_hand = self._get_obj_in_hand() diff --git a/omnigibson/transition_rules.py b/omnigibson/transition_rules.py index 4fbd1d9c1..87329b640 100644 --- a/omnigibson/transition_rules.py +++ b/omnigibson/transition_rules.py @@ -153,6 +153,10 @@ def step(cls): added_obj_attrs += output.add removed_objs += output.remove + cls.execute_transition(added_obj_attrs=added_obj_attrs, removed_objs=removed_objs) + + @classmethod + def execute_transition(cls, added_obj_attrs, removed_objs): # Process all transition results if len(removed_objs) > 0: disclaimer( diff --git a/tests/test_symbolic_primitives.py b/tests/test_symbolic_primitives.py index 17da562c7..c34255012 100644 --- a/tests/test_symbolic_primitives.py +++ b/tests/test_symbolic_primitives.py @@ -2,10 +2,18 @@ import pytest import yaml +import omnigibson.macros as gm +gm.USE_GPU_DYNAMICS = True +gm.USE_FLATCACHE = True + + import omnigibson as og from omnigibson import object_states from omnigibson.action_primitives.symbolic_semantic_action_primitives import SymbolicSemanticActionPrimitiveSet, SymbolicSemanticActionPrimitives + + + def start_env(): config = { "env": { @@ -85,18 +93,19 @@ def start_env(): }, { "type": "DatasetObject", - "name": "steak", - "category": "steak", - "model": "ppykkp", - "position": [5.31, 10.28, 1.], - }, - { - "type": "DatasetObject", - "name": "sponge", - "category": "sponge", - "model": "qewotb", + "name": "apple", + "category": "apple", + "model": "agveuv", "position": [4.75, 10.75, 1.], + "bounding_box": [0.098, 0.098, 0.115] }, + # { + # "type": "DatasetObject", + # "name": "sponge", + # "category": "sponge", + # "model": "qewotb", + # "position": [4.75, 10.75, 1.], + # }, ] } @@ -189,12 +198,19 @@ def test_place_inside(env, prim_gen, steak, fridge): # def test_wipe(): # pass -def test_cut(env, prim_gen, steak, knife): +def test_cut(env, prim_gen, steak, knife, countertop): # assert not steak.states[object_states.Cut].get_value(knife) + print("Grasping knife") for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.GRASP, knife): env.step(action) + for _ in range(60): og.sim.step() + print("Cutting steak") for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.CUT, steak): env.step(action) + for _ in range(60): og.sim.step() + print("Putting knife back on countertop") + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.PLACE_ON_TOP, countertop): + env.step(action) # assert steak.states[object_states.Sliced].get_value(knife) # def test_place_near_heating_element(): @@ -208,13 +224,17 @@ def main(): scene = env.scene robot = env.robots[0] prim_gen = SymbolicSemanticActionPrimitives(None, scene, robot) - steak = next(iter(env.scene.object_registry("category", "steak"))) + steak = next(iter(env.scene.object_registry("category", "apple"))) knife = next(iter(env.scene.object_registry("category", "carving_knife"))) + countertop = next(iter(env.scene.object_registry("category", "countertop"))) + + print("Will start in 3 seconds") + for _ in range(120): og.sim.step() try: - test_cut(env, prim_gen, steak, knife) + test_cut(env, prim_gen, steak, knife, countertop) except: - pass + raise while True: og.sim.step() From 675939943f41694d7e80b7642712e1a58d335074 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 12 Sep 2023 13:16:18 -0700 Subject: [PATCH 06/29] Update graph_builder.py --- omnigibson/scene_graphs/graph_builder.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omnigibson/scene_graphs/graph_builder.py b/omnigibson/scene_graphs/graph_builder.py index 1a2f202bc..15255d679 100644 --- a/omnigibson/scene_graphs/graph_builder.py +++ b/omnigibson/scene_graphs/graph_builder.py @@ -164,7 +164,7 @@ def step(self, scene): # Get the bounding box. if hasattr(obj, "get_base_aligned_bbox"): - bbox_center, bbox_orn, bbox_extent, _ = obj.get_base_aligned_bbox(visual=True) + bbox_center, bbox_orn, bbox_extent, _ = obj.get_base_aligned_bbox(visual=True, fallback_to_aabb=True) bbox_pose = T.pose2mat((bbox_center, bbox_orn)) else: bbox_pose, bbox_extent = get_robot_bbox(robot) From 5899aa120893d95de65a23bc13ee7101daa3ca8b Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Tue, 12 Sep 2023 19:18:16 -0700 Subject: [PATCH 07/29] add grasp task, reward function, termination condition --- .../starter_semantic_action_primitives.py | 13 +- omnigibson/reward_functions/grasp_reward.py | 54 ++++++ omnigibson/tasks/__init__.py | 1 + omnigibson/tasks/grasp_task.py | 79 +++++++++ .../termination_conditions/grasp_goal.py | 21 +++ tests/test_reward_function.py | 167 ++++++++++++++++++ 6 files changed, 329 insertions(+), 6 deletions(-) create mode 100644 omnigibson/reward_functions/grasp_reward.py create mode 100644 omnigibson/tasks/grasp_task.py create mode 100644 omnigibson/termination_conditions/grasp_goal.py create mode 100644 tests/test_reward_function.py diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 4f43b5240..fd4bad792 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -554,12 +554,13 @@ def _grasp(self, obj): # Step once to update yield self._empty_action() - if self._get_obj_in_hand() is None: - raise ActionPrimitiveError( - ActionPrimitiveError.Reason.POST_CONDITION_ERROR, - "Grasp completed, but no object detected in hand after executing grasp", - {"target object": obj.name}, - ) + # if self._get_obj_in_hand() is None: + # print("error") + # raise ActionPrimitiveError( + # ActionPrimitiveError.Reason.POST_CONDITION_ERROR, + # "Grasp completed, but no object detected in hand after executing grasp", + # {"target object": obj.name}, + # ) yield from self._reset_hand() diff --git a/omnigibson/reward_functions/grasp_reward.py b/omnigibson/reward_functions/grasp_reward.py new file mode 100644 index 000000000..daae9014d --- /dev/null +++ b/omnigibson/reward_functions/grasp_reward.py @@ -0,0 +1,54 @@ +from omnigibson.reward_functions.reward_function_base import BaseRewardFunction +import omnigibson.utils.transform_utils as T + + +class GraspReward(BaseRewardFunction): + """ + Grasp reward + """ + + def __init__(self, obj_name, dist_coeff, grasp_reward): + # Store internal vars + self.prev_grasping = False + self.obj_name = obj_name + self.obj = None + self.dist_coeff = dist_coeff + self.grasp_reward = grasp_reward + + # Run super + super().__init__() + + def _step(self, task, env, action): + self.obj = env.scene.object_registry("name", self.obj_name) if self.obj is None else self.obj + + robot = env.robots[0] + obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + current_grasping = True if obj_in_hand == self.obj else False + + # Reward varying based on combination of whether the robot was previously grasping the desired and object + # and is currently grasping the desired object + + # not grasping -> not grasping = Distance reward + # not grasping -> grasping = Large reward + # grapsing -> not grapsing = Large negative reward + # grasping -> grasping = Minimizing moment of inertia + + reward = None + + if not self.prev_grasping and not current_grasping: + eef_pos = robot.get_eef_position(robot.arm) + obj_pos = self.obj.get_position() + reward = T.l2_distance(eef_pos, obj_pos) * self.dist_coeff + + elif not self.prev_grasping and current_grasping: + reward = self.grasp_reward + + elif self.prev_grasping and not current_grasping: + reward = -self.grasp_reward + + elif self.prev_grasping and current_grasping: + # Need to finish + reward = 0 + + self.prev_grasping = current_grasping + return reward, {} diff --git a/omnigibson/tasks/__init__.py b/omnigibson/tasks/__init__.py index 2a6bfaa8f..550a873f0 100644 --- a/omnigibson/tasks/__init__.py +++ b/omnigibson/tasks/__init__.py @@ -3,3 +3,4 @@ from omnigibson.tasks.point_navigation_task import PointNavigationTask from omnigibson.tasks.point_reaching_task import PointReachingTask from omnigibson.tasks.behavior_task import BehaviorTask +from omnigibson.tasks.grasp_task import GraspTask diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py new file mode 100644 index 000000000..96b972a39 --- /dev/null +++ b/omnigibson/tasks/grasp_task.py @@ -0,0 +1,79 @@ +import numpy as np +from omnigibson.reward_functions.grasp_reward import GraspReward + +from omnigibson.tasks.task_base import BaseTask +from omnigibson.scenes.scene_base import Scene +from omnigibson.termination_conditions.grasp_goal import GraspGoal +from omnigibson.termination_conditions.timeout import Timeout +from omnigibson.utils.python_utils import classproperty +from omnigibson.utils.sim_utils import land_object + +DIST_COEFF = 0.1 +GRASP_REWARD = 1.0 + +class GraspTask(BaseTask): + """ + Grasp task + """ + + def __init__( + self, + obj_name, + termination_config=None, + reward_config=None, + ): + self.obj_name = obj_name + + super().__init__(termination_config=termination_config, reward_config=reward_config) + + + def _load(self, env): + # Do nothing here + pass + + def _create_termination_conditions(self): + # Run super first + terminations = super()._create_termination_conditions() + + terminations["graspgoal"] = GraspGoal( + self.obj_name + ) + terminations["timeout"] = Timeout(max_steps=self._termination_config["max_steps"]) + + return terminations + + + def _create_reward_functions(self): + rewards = dict() + rewards["grasp"] = GraspReward( + self.obj_name, + dist_coeff=self._reward_config["r_dist_coeff"], + grasp_reward=self._reward_config["r_grasp"] + ) + return rewards + + def _get_obs(self, env): + # No task-specific obs of any kind + return dict(), dict() + + def _load_non_low_dim_observation_space(self): + # No non-low dim observations so we return an empty dict + return dict() + + @classproperty + def valid_scene_types(cls): + # Any scene works + return {Scene} + + @classproperty + def default_termination_config(cls): + # Empty dict + return {} + + @classproperty + def default_reward_config(cls): + return { + "r_dist_coeff": DIST_COEFF, + "r_grasp": GRASP_REWARD, + } + diff --git a/omnigibson/termination_conditions/grasp_goal.py b/omnigibson/termination_conditions/grasp_goal.py new file mode 100644 index 000000000..e5e8b0dd5 --- /dev/null +++ b/omnigibson/termination_conditions/grasp_goal.py @@ -0,0 +1,21 @@ +from omnigibson.termination_conditions.termination_condition_base import SuccessCondition +import omnigibson.utils.transform_utils as T + + +class GraspGoal(SuccessCondition): + """ + GraspGoal (success condition) + """ + + def __init__(self, obj_name): + self.obj_name = obj_name + self.obj = None + + # Run super init + super().__init__() + + def _step(self, task, env, action): + self.obj = env.scene.object_registry("name", self.obj_name) if self.obj is None else self.obj + robot = env.robots[0] + obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + return obj_in_hand == self.obj diff --git a/tests/test_reward_function.py b/tests/test_reward_function.py new file mode 100644 index 000000000..dd4e2e2fd --- /dev/null +++ b/tests/test_reward_function.py @@ -0,0 +1,167 @@ +import numpy as np +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + +def set_start_pose(robot): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + +def step_sim(time): + for _ in range(int(time*100)): + og.sim.step() + +def execute_controller(ctrl_gen, env): + for action in ctrl_gen: + env.step(action) + +def test_grasp_reward(): + DIST_COEFF = 0.1 + GRASP_REWARD = 1.0 + + cfg = { + "scene": { + "type": "InteractiveTraversableScene", + "scene_model": "Rs_int", + "load_object_categories": ["floors", "coffee_table"], + }, + "robots": [ + { + "type": "Tiago", + "obs_modalities": ["scan", "rgb", "depth"], + "scale": 1.0, + "self_collisions": True, + "action_normalize": False, + "action_type": "continuous", + "grasping_mode": "sticky", + "rigid_trunk": False, + "default_arm_pose": "diagonal30", + "default_trunk_offset": 0.365, + "controller_config": { + "base": { + "name": "JointController", + "motor_type": "velocity" + }, + "arm_left": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": None, + "command_output_limits": None, + "use_delta_commands": False + }, + "arm_right": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": None, + "command_output_limits": None, + "use_delta_commands": False + }, + "gripper_left": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": [-1, 1], + "command_output_limits": None, + "use_delta_commands": True, + "use_single_command": True + }, + "gripper_right": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": [-1, 1], + "command_output_limits": None, + "use_delta_commands": True, + "use_single_command": True + }, + "camera": { + "name": "JointController", + "motor_type": "velocity", + "use_delta_commands": False + } + } + } + ], + "task": { + "type": "GraspTask", + "obj_name": "cologne", + "termination_config": { + "max_steps": 100000, + }, + "reward_config": { + "r_dist_coeff": DIST_COEFF, + "r_grasp": GRASP_REWARD + } + } + } + + # Make sure sim is stopped + og.sim.stop() + + # Make sure GPU dynamics are enabled (GPU dynamics needed for cloth) and no flatcache + gm.ENABLE_OBJECT_STATES = True + gm.USE_GPU_DYNAMICS = False + gm.ENABLE_FLATCACHE = False + + # Create the environment + env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) + scene = env.scene + robot = env.robots[0] + env.reset() + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + # Place object in scene + obj = DatasetObject( + name="cologne", + category="bottle_of_cologne", + model="lyipur" + ) + og.sim.import_object(obj) + obj.set_position([-0.3, -0.8, 0.5]) + + # Set robot position so it can grasp without movement + set_start_pose(robot) + pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) + robot.set_position_orientation(*pose) + og.sim.step() + + ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj) + + # Check reward going from not grasping to not grasping + _, reward, _, _ = env.step(next(ctrl_gen)) + assert reward == 10 + + for action in ctrl_gen: + prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + _, reward, _, _ = env.step(action) + obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + if prev_obj_in_hand is None and obj_in_hand is not None: + # Check reward going from not grapsing to after grasping + print(reward) + assert reward == GRASP_REWARD + elif prev_obj_in_hand is not None and obj_in_hand is not None: + # Check reward going from grasping to grasping + # assert reward > GRASP_REWARD + break + + for action in ctrl_gen: + prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + _, reward, _, _ = env.step(action) + obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + if prev_obj_in_hand is not None and obj_in_hand is None: + # Check reward going from grapsing to not grasping + assert reward == -GRASP_REWARD + print(reward) + + +test_grasp_reward() \ No newline at end of file From f9c95c7f8a7884c112e785762a0eeedf372284b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Wed, 13 Sep 2023 17:35:36 -0700 Subject: [PATCH 08/29] Fix transition rule object removal code --- omnigibson/transition_rules.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/omnigibson/transition_rules.py b/omnigibson/transition_rules.py index 87329b640..652b3e6b7 100644 --- a/omnigibson/transition_rules.py +++ b/omnigibson/transition_rules.py @@ -1270,10 +1270,13 @@ def _execute_recipe(cls, container, recipe, in_volume): system.remove_all_group_particles(group=group_name) # Remove either all objects or only the recipe-relevant objects inside the container - objs_to_remove.extend(np.concatenate([ - cls._OBJECTS[np.where(in_volume[cls._CATEGORY_IDXS[obj_category]])[0]] - for obj_category in recipe["input_objects"].keys() - ]) if cls.ignore_nonrecipe_objects else cls._OBJECTS[np.where(in_volume)[0]]) + object_mask = in_volume.copy() + if cls.ignore_nonrecipe_objects: + object_category_mask = np.zeros_like(object_mask, dtype=bool) + for obj_category in recipe["input_objects"].keys(): + object_category_mask[cls._CATEGORY_IDXS[obj_category]] = True + object_mask &= object_category_mask + objs_to_remove.extend(cls._OBJECTS[object_mask]) volume += sum(obj.volume for obj in objs_to_remove) # Define callback for spawning new objects inside container From d4c36f95c8376a30dd3e36a4897082de1a2975de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Wed, 13 Sep 2023 17:37:17 -0700 Subject: [PATCH 09/29] Improved & up-to-date docker with no Vulkan mount --- .github/workflows/build-push-containers.yml | 1 + docker/dev.Dockerfile | 2 +- docker/run_docker.sh | 3 --- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-push-containers.yml b/.github/workflows/build-push-containers.yml index aa27c728b..9471b4d3c 100644 --- a/.github/workflows/build-push-containers.yml +++ b/.github/workflows/build-push-containers.yml @@ -7,6 +7,7 @@ on: branches: - 'main' - 'og-develop' + - 'action-primitives' jobs: docker: diff --git a/docker/dev.Dockerfile b/docker/dev.Dockerfile index 113889b7d..8f897cdfb 100644 --- a/docker/dev.Dockerfile +++ b/docker/dev.Dockerfile @@ -1,4 +1,4 @@ -FROM nvcr.io/nvidia/isaac-sim:2022.2.0 +FROM nvcr.io/nvidia/isaac-sim:2022.2.1 # Set up all the prerequisites. RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \ diff --git a/docker/run_docker.sh b/docker/run_docker.sh index 146ae218c..78302514b 100755 --- a/docker/run_docker.sh +++ b/docker/run_docker.sh @@ -117,9 +117,6 @@ docker run \ -e DISPLAY=${DOCKER_DISPLAY} \ -e OMNIGIBSON_HEADLESS=${OMNIGIBSON_HEADLESS} \ -v $DATA_PATH/datasets:/data \ - -v ${ICD_PATH}:/etc/vulkan/icd.d/nvidia_icd.json \ - -v ${LAYERS_PATH}:/etc/vulkan/implicit_layer.d/nvidia_layers.json \ - -v ${EGL_VENDOR_PATH}:/usr/share/glvnd/egl_vendor.d/10_nvidia.json \ -v $DATA_PATH/isaac-sim/cache/kit:/isaac-sim/kit/cache/Kit:rw \ -v $DATA_PATH/isaac-sim/cache/ov:/root/.cache/ov:rw \ -v $DATA_PATH/isaac-sim/cache/pip:/root/.cache/pip:rw \ From 3c837b3a443b1957fec1da8b352195cd14b852f4 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 13 Sep 2023 18:07:08 -0700 Subject: [PATCH 10/29] wip --- .../starter_semantic_action_primitives.py | 2 +- omnigibson/reward_functions/grasp_reward.py | 2 +- omnigibson/robots/manipulation_robot.py | 3 ++- omnigibson/tasks/grasp_task.py | 12 ++++-------- tests/test_reward_function.py | 2 +- 5 files changed, 9 insertions(+), 12 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index fd4bad792..9bb451524 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -550,7 +550,7 @@ def _grasp(self, obj): # It's okay if we can't go all the way because we run into the object. indented_print("Performing grasp approach") yield from self._move_hand_direct_cartesian(approach_pose, stop_on_contact=True) - + print("touched") # Step once to update yield self._empty_action() diff --git a/omnigibson/reward_functions/grasp_reward.py b/omnigibson/reward_functions/grasp_reward.py index daae9014d..2743153a0 100644 --- a/omnigibson/reward_functions/grasp_reward.py +++ b/omnigibson/reward_functions/grasp_reward.py @@ -36,7 +36,7 @@ def _step(self, task, env, action): reward = None if not self.prev_grasping and not current_grasping: - eef_pos = robot.get_eef_position(robot.arm) + eef_pos = robot.get_eef_position(robot.default_arm) obj_pos = self.obj.get_position() reward = T.l2_distance(eef_pos, obj_pos) * self.dist_coeff diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index 4ec51809e..ca2318e98 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -58,6 +58,7 @@ def can_assisted_grasp(obj, link_name): # Allow based on mass mass = obj.links[link_name].mass + print(mass) if mass <= m.ASSIST_GRASP_MASS_THRESHOLD: return True @@ -769,7 +770,7 @@ def _calculate_in_hand_object_rigid(self, arm="default"): ag_obj_prim_path = "/".join(ag_prim_path.split("/")[:-1]) ag_obj_link_name = ag_prim_path.split("/")[-1] ag_obj = og.sim.scene.object_registry("prim_path", ag_obj_prim_path) - + from IPython import embed; embed() # Return None if object cannot be assisted grasped or not touching at least two fingers if ag_obj is None or (not can_assisted_grasp(ag_obj, ag_obj_link_name)) or (not touching_at_least_two_fingers): return None diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 96b972a39..bda5dd715 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -23,18 +23,14 @@ def __init__( reward_config=None, ): self.obj_name = obj_name - super().__init__(termination_config=termination_config, reward_config=reward_config) - def _load(self, env): # Do nothing here pass def _create_termination_conditions(self): - # Run super first - terminations = super()._create_termination_conditions() - + terminations = dict() terminations["graspgoal"] = GraspGoal( self.obj_name ) @@ -42,7 +38,6 @@ def _create_termination_conditions(self): return terminations - def _create_reward_functions(self): rewards = dict() rewards["grasp"] = GraspReward( @@ -67,8 +62,9 @@ def valid_scene_types(cls): @classproperty def default_termination_config(cls): - # Empty dict - return {} + return { + "max_steps": 100000 + } @classproperty def default_reward_config(cls): diff --git a/tests/test_reward_function.py b/tests/test_reward_function.py index dd4e2e2fd..c75fd24f2 100644 --- a/tests/test_reward_function.py +++ b/tests/test_reward_function.py @@ -139,7 +139,7 @@ def test_grasp_reward(): # Check reward going from not grasping to not grasping _, reward, _, _ = env.step(next(ctrl_gen)) - assert reward == 10 + # assert reward == 10 for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] From 0c65725b95e29e8bd32274e7a5d6199e02fd12d2 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 13 Sep 2023 23:03:17 -0700 Subject: [PATCH 11/29] modify reward and add test --- dev_environment/test.yaml | 77 ++++ dev_environment/test_IK.yaml | 72 ++++ dev_environment/test_IK_controller.py | 117 ++++++ dev_environment/test_arm.py | 202 ++++++++++ dev_environment/test_chain.py | 142 +++++++ dev_environment/test_collisions.py | 244 ++++++++++++ dev_environment/test_grasp.py | 266 ++++++++++++ dev_environment/test_interpolation.py | 80 ++++ dev_environment/test_open_revolute.py | 377 ++++++++++++++++++ dev_environment/test_tiago.py | 285 +++++++++++++ dev_environment/test_tiago.yaml | 71 ++++ dev_environment/test_toggle.py | 84 ++++ .../starter_semantic_action_primitives.py | 19 +- omnigibson/reward_functions/grasp_reward.py | 12 +- omnigibson/robots/manipulation_robot.py | 4 +- tests/test_reward_function.py | 16 +- 16 files changed, 2046 insertions(+), 22 deletions(-) create mode 100644 dev_environment/test.yaml create mode 100644 dev_environment/test_IK.yaml create mode 100644 dev_environment/test_IK_controller.py create mode 100644 dev_environment/test_arm.py create mode 100644 dev_environment/test_chain.py create mode 100644 dev_environment/test_collisions.py create mode 100644 dev_environment/test_grasp.py create mode 100644 dev_environment/test_interpolation.py create mode 100644 dev_environment/test_open_revolute.py create mode 100644 dev_environment/test_tiago.py create mode 100644 dev_environment/test_tiago.yaml create mode 100644 dev_environment/test_toggle.py diff --git a/dev_environment/test.yaml b/dev_environment/test.yaml new file mode 100644 index 000000000..d040c7a34 --- /dev/null +++ b/dev_environment/test.yaml @@ -0,0 +1,77 @@ +env: + initial_pos_z_offset: 0.1 + +render: + viewer_width: 1280 + viewer_height: 720 + +scene: + type: InteractiveTraversableScene + scene_model: Rs_int + trav_map_resolution: 0.1 + trav_map_erosion: 2 + trav_map_with_objects: true + build_graph: true + num_waypoints: 1 + waypoint_resolution: 0.2 + load_object_categories: null + not_load_object_categories: null + load_room_types: null + load_room_instances: null + seg_map_resolution: 0.1 + scene_source: OG + include_robots: true + +robots: + - type: Fetch + obs_modalities: [scan, rgb, depth] + scale: 1.0 + self_collisions: true + # action_normalize: false + action_normalize: false + action_type: continuous + grasping_mode: sticky + rigid_trunk: false + default_trunk_offset: 0.365 + default_arm_pose: diagonal30 + reset_joint_pos: tuck + controller_config: + base: + name: DifferentialDriveController + arm_0: + name: JointController + motor_type: position + command_input_limits: null + command_output_limits: null + use_delta_commands: false + gripper_0: + name: MultiFingerGripperController + mode: binary + camera: + name: JointController + use_delta_commands: false + +objects: [] + +# task: +# type: PointNavigationTask +# robot_idn: 0 +# floor: 0 +# initial_pos: null +# initial_quat: null +# goal_pos: null +# goal_tolerance: 0.36 # turtlebot bodywidth +# goal_in_polar: false +# path_range: [1.0, 10.0] +# visualize_goal: true +# visualize_path: false +# n_vis_waypoints: 25 +# reward_type: geodesic +# termination_config: +# max_collisions: 500 +# max_steps: 500 +# fall_height: 0.03 +# reward_config: +# r_potential: 1.0 +# r_collision: 0.1 +# r_pointgoal: 10.0 diff --git a/dev_environment/test_IK.yaml b/dev_environment/test_IK.yaml new file mode 100644 index 000000000..8f43dc659 --- /dev/null +++ b/dev_environment/test_IK.yaml @@ -0,0 +1,72 @@ +env: + initial_pos_z_offset: 0.1 + +render: + viewer_width: 1280 + viewer_height: 720 + +scene: + type: InteractiveTraversableScene + scene_model: Rs_int + trav_map_resolution: 0.1 + trav_map_erosion: 2 + trav_map_with_objects: true + build_graph: true + num_waypoints: 1 + waypoint_resolution: 0.2 + load_object_categories: ["floors", "walls", "window", "door", "shelf", "bottom_cabinet", "top_cabinet", "countertop", "mirror", "picture", "coffee_table"] + not_load_object_categories: null + load_room_types: null + load_room_instances: null + seg_map_resolution: 0.15 + scene_source: OG + include_robots: true + +robots: + - type: Tiago + obs_modalities: [scan, rgb, depth, proprio] + proprio_obs: [robot_pose, joint_qpos, joint_qvel, eef_left_pos, eef_left_quat, grasp_left] + scale: 1.0 + self_collisions: true + action_normalize: false + action_type: continuous + grasping_mode: sticky + rigid_trunk: false + default_arm_pose: diagonal30 + controller_config: + base: + name: JointController + motor_type: velocity + arm_left: + name: InverseKinematicsController + motor_type: velocity + command_input_limits: null + command_output_limits: null + mode : pose_absolute_ori + kv: 3.0 + arm_right: + name: JointController + motor_type: position + command_input_limits: null + command_output_limits: null + use_delta_commands: true + gripper_left: + name: JointController + motor_type: position + command_input_limits: [-1, 1] + command_output_limits: null + use_delta_commands: true + use_single_command: true + gripper_right: + name: JointController + motor_type: position + command_input_limits: [-1, 1] + command_output_limits: null + use_delta_commands: true + use_single_command: true + camera: + name: JointController + motor_type: position + command_input_limits: null + command_output_limits: null + use_delta_commands: false \ No newline at end of file diff --git a/dev_environment/test_IK_controller.py b/dev_environment/test_IK_controller.py new file mode 100644 index 000000000..49695b65c --- /dev/null +++ b/dev_environment/test_IK_controller.py @@ -0,0 +1,117 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + +import cProfile, pstats, io +import time +import os +import argparse + + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + for action in ctrl_gen: + env.step(action[0]) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + table = DatasetObject( + name="table", + category="breakfast_table", + model="rjgmmy", + scale = [0.3, 0.3, 0.3] + ) + og.sim.import_object(table) + # table.set_position([-0.7, -2.0, 0.2]) + table.set_position([-0.7, 0.5, 0.2]) + og.sim.step() + + grasp_obj = DatasetObject( + name="cologne", + category="bottle_of_cologne", + model="lyipur" + ) + og.sim.import_object(grasp_obj) + grasp_obj.set_position([-0.3, -0.8, 0.5]) + og.sim.step() + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + def test_navigate_to_obj(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # execute_controller(controller._reset_hand(), env) + set_start_pose() + execute_controller(controller._navigate_to_obj(table), env) + + def test_grasp_no_navigation(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + set_start_pose() + pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) + robot.set_position_orientation(*pose) + og.sim.step() + # replay_controller(env, "./replays/test_grasp_pose.yaml") + execute_controller(controller._grasp(grasp_obj), env) + + def test_grasp(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # execute_controller(controller._reset_hand(), env) + set_start_pose() + # pause(2) + execute_controller(controller._grasp(grasp_obj), env) + + def test_place(): + test_grasp() + pause(1) + execute_controller(controller._place_on_top(table), env) + + test_grasp_no_navigation() + pause(5) + + + +if __name__ == "__main__": + main() + + + diff --git a/dev_environment/test_arm.py b/dev_environment/test_arm.py new file mode 100644 index 000000000..db6b037d2 --- /dev/null +++ b/dev_environment/test_arm.py @@ -0,0 +1,202 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject +from omnigibson.utils.motion_planning_utils import detect_robot_collision, arm_planning_validity_fn +from omnigibson.utils.control_utils import FKSolver + +import cProfile, pstats, io +import time +import os +import argparse + +from omni.usd.commands import CopyPrimsCommand, DeletePrimsCommand, CopyPrimCommand, TransformPrimsCommand, TransformPrimCommand +from omnigibson.prims import CollisionGeomPrim +from pxr import Gf, Usd +from omni.isaac.core.utils.prims import get_prim_at_path + +def pause(time): + for _ in range(int(time*100)): + og.sim.render() + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "walls", "coffee_table"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + # robot.tuck() + # pause(2) + + # import random + def get_random_joint_position(): + joint_positions = [] + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + joints = np.array([joint for joint in robot.joints.values()]) + arm_joints = joints[joint_control_idx] + for i, joint in enumerate(arm_joints): + val = np.random.uniform(joint.lower_limit, joint.upper_limit) + joint_positions.append(val) + return joint_positions + + def set_joint_position(joint_pos): + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + robot.set_joint_positions(joint_pos, joint_control_idx) + + robot.tuck() + og.sim.step() + # while True: + # joint_pos = get_random_joint_position() + # set_joint_position(joint_pos) + # pause(2) + # print(joint_pos) + # print("------------------------") + + # sample_table_collision = [0.05725323620041453, 0.5163557640853469, 1.510323032160434, -4.410407307232964, -1.1433260958390707, -5.606768602222553, 1.0313821643138894, -4.181284701460742] + # sample_self_collision = [0.03053552120088664, 1.0269865478752571, 1.1344740372495958, 6.158997020615134, 1.133466907494042, -4.544473644642829, 0.6930819484783561, 4.676661155308317] + # collision_free = [0.17814310139520295, -0.8082173382782226, 1.3469484097869393, 1.6222072455290446, 2.0591874971218145, -2.9063608379063557, -0.04634827601286595, 4.505122702016582] + + #Tiago + collision = [0.14065403286781475, 0.7298650222286143, -0.4780975016232605, 1.0888713731557247, -0.03729107004351029, 3.274825625013916, 1.2937221767307419, 1.8178545818287346, 0.269868125704424, -1.1858447020249343, 1.079587475865726, 0.11286700467163624, -1.3232706255151934, 0.3340342084010399, 1.4264938203455721] + no_collision = [0.3184793294422698, 1.5142631693122297, 1.2405191873837995, 0.21394545305074741, -0.6831575211130013, -0.7389958913494964, 2.725925427761072, 1.1755425218590514, 0.6547571278019166, 0.7133282478444771, 0.5163046628994854, -1.0098026849625532, 0.09229435376315243, 1.9379299096653453, 2.0534229844998677] + + # set_joint_position(no_collision) + # pause(100) + # pause(100) + # pos = np.array([ + # 0.0, + # 0.0, + # 0.0, + # 0.0, + # 0.0, + # 0.0, + # 0.1, # trunk + # -1.1, + # -1.1, + # 0.0, + # 1.47, + # 1.47, + # 0.0, + # 2.71, + # 2.71, + # 1.71, + # 1.71, + # -1.57, + # -1.57, + # 1.39, + # 1.39, + # 0.0, + # 0.0, + # 0.045, + # 0.045, + # 0.045, + # 0.045, + # ]) + joint_pos = np.array( + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, # trunk + -1.1, + -1.1, + 0.0, + 1.47, + 1.47, + 0.0, + 2.71, + 2.71, + 1.71, + 1.71, + -1.57, + -1.57, + 1.39, + 1.39, + 0.0, + 0.0, + 0.045, + 0.045, + 0.045, + 0.045, + ] + ) + + + + pose_2d = [0.299906, -0.918024, 2.94397] + + pos = np.array([pose_2d[0], pose_2d[1], 0.05]) + orn = T.euler2quat([0, 0, pose_2d[2]]) + + # robot.set_position_orientation([-1.08215380e+00, -3.35281938e-01, -2.77837131e-07], [ 1.78991655e-07, -4.65450078e-08, -2.67762393e-01, 9.63485003e-01]) + # robot.set_position_orientation(pos, orn) + # og.sim.step() + positions = [[0.09640930593013763, -1.0999783277511597, 1.470136046409607, 2.7100629806518555, 1.710019826889038, -1.5699725151062012, 1.3899997472763062, -2.2541275939147454e-06], [0.11789778377430027, 1.57079632679, -0.00844635003731165, 1.6066719362098862, 0.4473218694991109, 0.019161401102889112, -1.2949643256956296, -1.9651135606361847]] + + joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) + control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] + joint_pos = joint_pos[joint_combined_idx] + joint_pos[control_idx_in_joint_pos] = [0.263221, 1.51202 ,0.277794 ,1.5376, 0.852972, -0.23253 ,-1.41372 ,1.55155] + # print(pos) + # robot.set_joint_positions([0.263221, 1.51202 ,0.277794 ,1.5376, 0.852972, -0.23253 ,-1.41372 ,1.55155], joint_control_idx) + # pause(100) + # from omnigibson.controllers.controller_base import ControlType + # action = np.zeros(robot.action_dim) + # for name, controller in robot._controllers.items(): + # joint_idx = controller.dof_idx + # action_idx = robot.controller_action_idx[name] + # if controller.control_type == ControlType.POSITION and len(joint_idx) == len(action_idx): + # action[action_idx] = robot.get_joint_positions()[joint_idx] + # action[robot.controller_action_idx["arm_left"]] = positions[1] + # for p in positions: + # robot.set_joint_positions(p, joint_control_idx) + # og.sim.step() + # pause(2) + # with UndoableContext(robot, "arm") as context: + # while True: + # jp = np.array(robot.get_joint_positions()[joint_combined_idx]) + # print(not arm_planning_validity_fn(context, jp)) + # env.step(action) + +# joint_pos = [-1.0821538e+00, -3.3528194e-01, -2.7783713e-07, 3.8149398e-07, +# -2.0263911e-07, -5.4213971e-01 , 1.1684235e-01 , 1.5707957e+00, +# -1.0999898e+00 ,-2.8282230e-07 , 5.6211746e-01 , 1.4701120e+00, +# 1.0374244e-08 , 1.6099000e+00 , 2.6821127e+00 , 4.4674629e-01, +# 1.8163613e+00 ,-2.2369886e-02, -1.5652136e+00, -1.2442690e+00, +# 1.3900158e+00, -2.0943952e+00, -5.9008621e-06, 4.4999883e-02, +# 4.5000002e-02, 4.4999868e-02, 4.5000002e-02] +# joint_pos = np.array(joint_pos)[joint_combined_idx] + # with UndoableContext(robot, "arm") as context: + # print(not arm_planning_validity_fn(context, joint_pos)) + # for i in range(10000): + # og.sim.render() + + with UndoableContext(robot, "base") as context: + print(detect_robot_collision(context, [pos, orn])) + for link in context.robot_meshes_copy: + for mesh in context.robot_meshes_copy[link]: + mesh.collision_enabled = True + for i in range(10000): + og.sim.render() + + # breakpoint() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/dev_environment/test_chain.py b/dev_environment/test_chain.py new file mode 100644 index 000000000..a2b44e91b --- /dev/null +++ b/dev_environment/test_chain.py @@ -0,0 +1,142 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + +import cProfile, pstats, io +import time +import os +import argparse + +from omnigibson import object_states + + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + actions = [] + for action in ctrl_gen: + env.step(action) + actions.append(action.tolist()) + if filename is not None: + with open(filename, "w") as f: + yaml.dump(actions, f) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + table = DatasetObject( + name="table", + category="breakfast_table", + model="rjgmmy", + scale = [0.3, 0.3, 0.3] + ) + og.sim.import_object(table) + table.set_position([-0.7, 0.5, 0.2]) + og.sim.step() + + open_obj = DatasetObject( + name="bottom_cabinet", + category="bottom_cabinet", + model="bamfsz", + scale=[0.5, 0.5, 0.5] + ) + og.sim.import_object(open_obj) + open_obj.set_position_orientation([-1.2, -0.4, 0.5], T.euler2quat([0, 0, np.pi/2])) + pause(3) + + calculator = DatasetObject( + name="calculator", + category="calculator", + model="kwmmty", + scale=[0.3, 0.3, 0.3] + ) + og.sim.import_object(calculator) + # calculator.set_position_orientation([, 0.0, 0.0], T.euler2quat([0, 0, 0])) + calculator.set_position_orientation([-0.95, -0.31, 0.4], T.euler2quat([0, 0, 0])) + og.sim.step() + from IPython import embed; embed() + # calculator.states[object_states.Inside].set_value(open_obj, True) + + # breakpoint() + # from IPython import embed; embed() + + + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + # def test_navigate_to_obj(): + # # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # # execute_controller(controller._reset_hand(), env) + # set_start_pose() + # execute_controller(controller._navigate_to_obj(table), env) + + # def test_grasp_no_navigation(): + # # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # # set_start_pose() + # pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) + # robot.set_position_orientation(*pose) + # og.sim.step() + # # replay_controller(env, "./replays/test_grasp_pose.yaml") + # execute_controller(controller._grasp(grasp_obj), env) + + # def test_grasp(): + # # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # # execute_controller(controller._reset_hand(), env) + # set_start_pose() + # # pause(2) + # execute_controller(controller._grasp(grasp_obj), env) + + # def test_place(): + # test_grasp() + # pause(1) + # execute_controller(controller._place_on_top(table), env) + + + # test_grasp_no_navigation() + + pause(500) + + +if __name__ == "__main__": + main() + + + diff --git a/dev_environment/test_collisions.py b/dev_environment/test_collisions.py new file mode 100644 index 000000000..919f5a171 --- /dev/null +++ b/dev_environment/test_collisions.py @@ -0,0 +1,244 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject +from omnigibson.utils.motion_planning_utils import set_arm_and_detect_collision, set_base_and_detect_collision + +import cProfile, pstats, io +import time +import os +import argparse + +def pause(time): + for _ in range(int(time*100)): + og.sim.render() + +def pause_step(time): + for _ in range(int(time*100)): + og.sim.step() + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "walls", "coffee_table"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + # def set_start_pose(): + # control_idx = np.concatenate([robot.trunk_control_idx]) + # robot.set_joint_positions([0.1], control_idx) + # og.sim.step() + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + + table = DatasetObject( + name="table", + category="breakfast_table", + model="rjgmmy", + scale = [0.3, 0.3, 0.3] + ) + og.sim.import_object(table) + table.set_position([-0.7, -2.0, 0.2]) + og.sim.step() + + + # pose_2d = [-1.0, 0.0, 0.1] + # pose = StarterSemanticActionPrimitives._get_robot_pose_from_2d_pose(pose_2d) + # pose = StarterSemanticActionPrimitives._get_robot_pose_from_2d_pose([-0.433881, -0.230183, -1.87]) + # robot.set_position_orientation(*pose) + set_start_pose() + pause_step(4) + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + # for position in positions: + # with UndoableContext(controller.robot, controller.robot_copy, "original", True) as context: + # print(set_base_and_detect_collision(context,(position, [0, 0, 0, 1]))) + # print("--------------------") + + # pause(100) + + # joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) + + def get_random_joint_position(): + import random + joint_positions = [] + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) + joints = np.array([joint for joint in robot.joints.values()]) + arm_joints = joints[joint_control_idx] + for i, joint in enumerate(arm_joints): + val = random.uniform(joint.lower_limit, joint.upper_limit) + joint_positions.append(val) + return joint_positions, joint_control_idx + + + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) + joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) + control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] + + # joint_pos = initial_joint_pos + # joint_pos[control_idx_in_joint_pos] = [0.0133727 ,0.216775 ,0.683931 ,2.04371 ,1.88204 ,0.720747 ,1.23276 ,1.72251] + # from IPython import embed; embed() + + # def print_link(): + # for link in robot.links.values(): + # link_name = link.prim_path.split("/")[-1] + # for mesh_name, mesh in link.collision_meshes.items(): + # if link_name == "arm_right_1_link": + # pose = T.relative_pose_transform(*link.get_position_orientation(), *robot.get_position_orientation()) + # print(pose[0], T.quat2euler(pose[1])) + # print("-------") + while True: + joint_pos, joint_control_idx = get_random_joint_position() + robot.set_joint_positions(joint_pos, joint_control_idx) + pause_step(2) + # print_link() + # from IPython import embed; embed() + with UndoableContext(controller.robot, controller.robot_copy, "original") as context: + # from IPython import embed; embed() + initial_joint_pos[control_idx_in_joint_pos] = joint_pos + # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) + print(set_arm_and_detect_collision(context, initial_joint_pos)) + print("--------------------") + from IPython import embed; embed() + + # inter_path = [[0.09997262060642242, -1.0293538570404053, 1.4700201749801636, 2.709970235824585, 1.749015212059021, -1.5699917078018188, 1.3900072574615479, -4.500867362366989e-06], [0.11691628270503057, -0.8830267327942037, 1.3513760662081975, 2.544509606147747, 1.7484604307752005, -1.5653005250323326, 1.3170965590802435, -0.054800000852916544], [0.13385994480363872, -0.7366996085480022, 1.2327319574362314, 2.379048976470909, 1.74790564949138, -1.5606093422628464, 1.2441858606989389, -0.10959550083847072], [0.15080360690224687, -0.5903724843018008, 1.1140878486642654, 2.213588346794071, 1.7473508682075598, -1.55591815949336, 1.1712751623176345, -0.16439100082402489], [0.16774726900085501, -0.44404536005559925, 0.9954437398922993, 2.048127717117233, 1.7467960869237393, -1.5512269767238738, 1.09836446393633, -0.21918650080957908], [0.18469093109946316, -0.2977182358093978, 0.8767996311203332, 1.882667087440395, 1.7462413056399189, -1.5465357939543876, 1.0254537655550255, -0.27398200079513324], [0.2016345931980713, -0.1513911115631964, 0.7581555223483671, 1.717206457763557, 1.7456865243560984, -1.5418446111849013, 0.9525430671737212, -0.3287775007806874], [0.21857825529667946, -0.005063987316994867, 0.6395114135764011, 1.5517458280867191, 1.745131743072278, -1.5371534284154151, 0.8796323687924167, -0.38357300076624157], [0.2355219173952876, 0.14126313692920678, 0.520867304804435, 1.386285198409881, 1.7445769617884577, -1.5324622456459287, 0.8067216704111121, -0.4383685007517958], [0.25246557949389575, 0.2875902611754082, 0.4022231960324689, 1.220824568733043, 1.7440221805046372, -1.5277710628764425, 0.7338109720298077, -0.49316400073734995], [0.2694092415925039, 0.4339173854216096, 0.28357908726050285, 1.055363939056205, 1.7434673992208167, -1.5230798801069563, 0.6609002736485032, -0.5479595007229041], [0.2783637696421741, 0.47426669293427165, 0.31462729394691785, 1.116599611260824, 1.6978851495815748, -1.4143148289833873, 0.4303872927273669, -0.5602623084353068], [0.28731829769184425, 0.5146160004469337, 0.3456755006333329, 1.1778352834654429, 1.6523028999423328, -1.3055497778598182, 0.1998743118062306, -0.5725651161477093], [0.2962728257415144, 0.5549653079595958, 0.3767237073197479, 1.2390709556700619, 1.606720650303091, -1.196784726736249, -0.030638669114905648, -0.584867923860112], [0.3052273537911846, 0.5953146154722577, 0.4077719140061629, 1.3003066278746809, 1.5611384006638491, -1.08801967561268, -0.261151650036042, -0.5971707315725147], [0.3141818818408548, 0.6356639229849199, 0.438820120692578, 1.3615423000792997, 1.5155561510246072, -0.979254624489111, -0.4916646309571784, -0.6094735392849172], [0.323136409890525, 0.6760132304975819, 0.469868327378993, 1.4227779722839187, 1.4699739013853654, -0.870489573365542, -0.7221776118783145, -0.6217763469973199], [0.3320909379401952, 0.7163625380102439, 0.500916534065408, 1.4840136444885377, 1.4243916517461233, -0.761724522241973, -0.9526905927994511, -0.6340791547097225], [0.34104546598986535, 0.7567118455229059, 0.531964740751823, 1.5452493166931565, 1.3788094021068815, -0.6529594711184039, -1.1832035737205873, -0.6463819624221251], [0.3499999940395355, 0.797061153035568, 0.563012947438238, 1.6064849888977755, 1.3332271524676396, -0.5441944199948349, -1.4137165546417236, -0.6586847701345278]] + + # for p in inter_path: + # with UndoableContext(controller.robot, controller.robot_copy, "original") as context: + # # from IPython import embed; embed() + # initial_joint_pos[control_idx_in_joint_pos] = p + # # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) + # print(set_arm_and_detect_collision(context, initial_joint_pos)) + # print("--------------------") + # pause(0.1) + # from IPython import embed; embed() + + # move_path = [[-0.00017805075913202018, 3.202066363883205e-05, 0.0002361552458696181], [0.02343988658739001, -0.18208819000753182, -0.05332718383977012], [0.047057823933912044, -0.36420840067870247, -0.10689052292540986], [0.07067576128043407, -0.5463286113498731, -0.16045386201104958], [0.09429369862695611, -0.7284488220210438, -0.21401720109668934], [0.11791163597347813, -0.9105690326922143, -0.2675805401823291], [0.14152957332000016, -1.092689243363385, -0.3211438792679688], [0.1651475106665222, -1.2748094540345556, -0.37470721835360854], [0.18876544801304423, -1.4569296647057264, -0.4282705574392483], [0.21238338535956627, -1.6390498753768972, -0.481833896524888], [0.23600132270608828, -1.8211700860480675, -0.5353972356105278], [0.2596192600526103, -2.0032902967192383, -0.5889605746961675], [0.1313322452447971, -2.1147419038982056, -0.5567734369234738], [0.0030452304369839034, -2.2261935110771733, -0.5245862991507801], [-0.12524178437082933, -2.3376451182561406, -0.4923991613780865], [-0.2535287991786425, -2.4490967254351084, -0.4602120236053928], [-0.38181581398645575, -2.5605483326140757, -0.42802488583269915], [-0.6515587727066181, -2.54553348876709, -0.43195654044347825], [-0.9213017314267804, -2.5305186449201047, -0.4358881950542574], [-1.191044690146943, -2.515503801073119, -0.4398198496650365]] + # for p in move_path: + # with UndoableContext(controller.robot, controller.robot_copy, "original") as context: + # # from IPython import embed; embed() + # pose = [p[0], p[1], 0.0], T.euler2quat((0, 0, p[2])) + # print(set_base_and_detect_collision(context, pose)) + # print("--------------------") + # pause(0.1) + # from IPython import embed; embed() + + # from math import ceil + # ANGLE_DIFF = 0.3 + # DIST_DIFF = 0.1 + + # def checkMotion(context, start, goal): + # segment_theta = get_angle_between_poses(start, goal) + + # # from IPython import embed; embed() + + # # Start rotation + # is_valid_rotation(context, start, segment_theta) + # # Navigation + # dist = np.linalg.norm(goal[:2] - start[:2]) + # num_points = ceil(dist / DIST_DIFF) + 1 + # nav_x = np.linspace(start[0], goal[0], num_points).tolist() + # nav_y = np.linspace(start[1], goal[1], num_points).tolist() + # for i in range(num_points): + # pose = [nav_x[i], nav_y[i], 0.0], T.euler2quat((0, 0, segment_theta)) + # print(set_base_and_detect_collision(context, pose)) + # print("--------------------") + # pause(0.1) + # from IPython import embed; embed() + + # # Goal rotation + # is_valid_rotation(context, [goal[0], goal[1], segment_theta], goal[2]) + + # def is_valid_rotation(context, start_conf, final_orientation): + # diff = T.wrap_angle(final_orientation - start_conf[2]) + # direction = np.sign(diff) + # diff = abs(diff) + # num_points = ceil(diff / ANGLE_DIFF) + 1 + # nav_angle = np.linspace(0.0, diff, num_points) * direction + # angles = nav_angle + start_conf[2] + # for i in range(num_points): + # pose = [start_conf[0], start_conf[1], 0.0], T.euler2quat((0, 0, angles[i])) + # print(pose) + # print(set_base_and_detect_collision(context, pose)) + # print("--------------------") + # pause(0.1) + # from IPython import embed; embed() + + # def get_angle_between_poses(p1, p2): + # segment = [] + # segment.append(p2[0] - p1[0]) + # segment.append(p2[1] - p1[1]) + # return np.arctan2(segment[1], segment[0]) + + # move_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [-1.1167034268072273, -0.6603483567393302, 1.6380703271255967], [-1.4364447586070876, -2.1578042124791095, 1.031035481733344], [-0.494043711296063, -2.727928917138556, 0.36954198879575983]] + # interpolated_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [-0.139743706963583, -0.08251553245482543, 0.20496527921988983], [-0.27930938122696075, -0.16506307878118326, 0.40969457177784796], [-0.41887505549033854, -0.2476106251075411, 0.6144238643358061], [-0.5584407297537163, -0.3301581714338989, 0.8191531568937642], [-0.698006404017094, -0.4127057177602568, 1.0238824494517225], [-0.8375720782804719, -0.4952532640866146, 1.2286117420096805], [-0.9771377525438496, -0.5778008104129724, 1.4333410345676385], [-1.1167034268072273, -0.6603483567393302, 1.6380703271255967], [-1.162380759921493, -0.8742706218450129, 1.551351063498132], [-1.208058093035759, -1.0881928869506958, 1.4646317998706675], [-1.2537354261500246, -1.3021151520563783, 1.3779125362432028], [-1.2994127592642903, -1.5160374171620612, 1.291193272615738], [-1.3450900923785563, -1.729959682267744, 1.2044740089882735], [-1.390767425492822, -1.9438819473734266, 1.1177547453608088], [-1.4364447586070876, -2.1578042124791095, 1.031035481733344], [-1.2008444967793315, -2.3003353886439712, 0.865662108498948], [-0.9652442349515753, -2.442866564808833, 0.7002887352645519], [-0.7296439731238191, -2.5853977409736943, 0.5349153620301559], [-0.494043711296063, -2.727928917138556, 0.36954198879575983]] + # cleaned_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], (-1.1167034268072273, -0.6603483567393302, -2.6074760565062323), (-1.4364447586070876, -2.1578042124791095, -1.781160358496523), [-0.494043711296063, -2.727928917138556, 0.36954198879575983]] + + # move_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [0.08206962226267768, -1.7229286791889955, -1.9627716115738194], [-0.20405345335578362, -2.566953738240694, 2.888701559348255], [-1.1347811542998147, -2.657722324995175, -0.19615768830098013]] + # interpolated_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [0.010102924170155141, -0.2153380727610336, -0.2451399631175372], [0.020383881040515504, -0.4307081593935996, -0.4905159128970061], [0.030664837910875868, -0.6460782460261656, -0.735891862676475], [0.04094579478123623, -0.8614483326587316, -0.9812678124559439], [0.05122675165159659, -1.0768184192912975, -1.2266437622354127], [0.06150770852195696, -1.2921885059238636, -1.4720197120148817], [0.07178866539231732, -1.5075585925564294, -1.7173956617943504], [0.08206962226267768, -1.7229286791889955, -1.9627716115738194], [0.024845007138985423, -1.8917336909993352, -2.2491140388253217], [-0.032379607984706835, -2.060538702809675, -2.535456466076824], [-0.08960422310839909, -2.229343714620015, -2.8217988933283267], [-0.14682883823209136, -2.3981487264303545, -3.108141320579829], [-0.20405345335578362, -2.566953738240694, 2.888701559348255], [-0.35917473684645546, -2.5820818360331077, 2.3745583514067157], [-0.5142960203371273, -2.5972099338255212, 1.8604151434651768], [-0.6694173038277992, -2.6123380316179343, 1.3462719355236374], [-0.824538587318471, -2.627466129410348, 0.8321287275820985], [-0.979659870809143, -2.6425942272027614, 0.31798551964055877], [-1.1347811542998147, -2.657722324995175, -0.19615768830098013]] + # cleaned_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], (0.08206962226267768, -1.7229286791889955, -1.5230963028314004), (-0.20405345335578362, -2.566953738240694, -1.8976366735518704), [-1.1347811542998147, -2.657722324995175, -0.19615768830098013]] + # for i in range(len(move_path) - 1): + # with UndoableContext(controller.robot, controller.robot_copy, "simplified") as context: + # # print(move_path[i]) + # # print(move_path[i+1]) + # # print("start and finish") + # checkMotion(context, np.array(move_path[i]), np.array(move_path[i+1])) + + # with UndoableContext(controller.robot, controller.robot_copy, "original") as context: + # # pose = ([-0.15, -2.0, 0.0], T.euler2quat((0, 0, 0.0))) + # pose = ([-0.0771680802758046, -2.1166874751986207, 0.0], [0, 0, 0.93719889, 0.3487954]) + # print(set_base_and_detect_collision(context, pose)) + # print("--------------------") + # pause(0.1) + # from IPython import embed; embed() + + pause(1) + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run test script") + parser.add_argument( + "--profile", + action="store_true", + help="If set, profile code and generate .prof file", + ) + args = parser.parse_args() + if args.profile: + pr = cProfile.Profile() + pr.enable() + main() + pr.disable() + results = pstats.Stats(pr) + filename = f'profile-{os.path.basename(__file__)}-{time.strftime("%Y%m%d-%H%M%S")}' + results.dump_stats(f"./profiles/{filename}.prof") + os.system(f"flameprof ./profiles/{filename}.prof > ./profiles/{filename}.svg") + # Run `snakeviz ./profiles/.prof` to visualize stack trace or open .svg in a browser + else: + main() \ No newline at end of file diff --git a/dev_environment/test_grasp.py b/dev_environment/test_grasp.py new file mode 100644 index 000000000..6a809e238 --- /dev/null +++ b/dev_environment/test_grasp.py @@ -0,0 +1,266 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject +from omnigibson.utils.grasping_planning_utils import get_grasp_position_for_open + +import cProfile, pstats, io +import time +import os +import argparse + + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + actions = [] + for action in ctrl_gen: + env.step(action) + actions.append(action.tolist()) + if filename is not None: + with open(filename, "w") as f: + yaml.dump(actions, f) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "bottom_cabinet"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + for o in scene.objects: + if o.prim_path == "/World/bottom_cabinet_bamfsz_0": + cabinet = o + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + def test_open_no_navigation(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + set_start_pose() + pose = controller._get_robot_pose_from_2d_pose([-1.0, -0.5, np.pi/2]) + robot.set_position_orientation(*pose) + og.sim.step() + get_grasp_position_for_open(robot, cabinet, True) + # execute_controller(controller._open_or_close(cabinet), env) + + def test_open(): + set_start_pose() + pose_2d = [-0.762831, -0.377231, 2.72892] + pose = controller._get_robot_pose_from_2d_pose(pose_2d) + robot.set_position_orientation(*pose) + og.sim.step() + + # joint_pos = [0.0133727 ,0.216775 ,0.683931 ,2.04371 ,1.88204 ,0.720747 ,1.23276 ,1.72251] + # control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) + # robot.set_joint_positions(joint_pos, control_idx) + # og.sim.step() + # pause(100) + execute_controller(controller._open_or_close(cabinet, True), env) + # replay_controller(env, "./replays/test_tiago_open.yaml") + + test_open() + + marker = None + def set_marker(position): + marker = PrimitiveObject( + prim_path=f"/World/marker", + name="marker", + primitive_type="Cube", + size=0.07, + visual_only=True, + rgba=[1.0, 0, 0, 1.0], + ) + og.sim.import_object(marker) + marker.set_position(position) + og.sim.step() + + def remove_marker(): + marker.remove() + og.sim.step() + + def get_closest_point_to_point_in_world_frame(vectors_in_arbitrary_frame, arbitrary_frame_to_world_frame, point_in_world): + vectors_in_world = np.array( + [ + T.pose_transform(*arbitrary_frame_to_world_frame, vector, [0, 0, 0, 1])[0] + for vector in vectors_in_arbitrary_frame + ] + ) + + vector_distances_to_point = np.linalg.norm(vectors_in_world - np.array(point_in_world)[None, :], axis=1) + closer_option_idx = np.argmin(vector_distances_to_point) + vector_in_arbitrary_frame = vectors_in_arbitrary_frame[closer_option_idx] + vector_in_world_frame = vectors_in_world[closer_option_idx] + + return closer_option_idx, vector_in_arbitrary_frame, vector_in_world_frame + + ######################################################## + + # from omnigibson.objects.primitive_object import PrimitiveObject + # import numpy as np + # import random + # from scipy.spatial.transform import Rotation + + # import omnigibson.utils.transform_utils as T + # from omnigibson.object_states.open import _get_relevant_joints + # from omnigibson.utils.constants import JointType, JointAxis + # from omni.isaac.core.utils.rotations import gf_quat_to_np_array + # from scipy.spatial.transform import Rotation as R + + # p = None + # grasp_position_for_open_on_revolute_joint = None + # should_open = True + # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS = (0.2, 0.8) + # GRASP_OFFSET = np.array([0, 0.05, -0.08]) + # OPEN_GRASP_OFFSET = np.array([0, 0.05, -0.12]) # 5cm back and 12cm up. + + # # Pick a moving link of the object. + # relevant_joints_full = _get_relevant_joints(cabinet) + # relevant_joints = relevant_joints_full[1] + + # # If a particular link ID was specified, filter our candidates down to that one. + # # if link_id is not None: + # # relevant_joints = [ji for ji in relevant_joints if ji.jointIndex == link_id] + + # if len(relevant_joints) == 0: + # raise ValueError("Cannot open/close object without relevant joints.") + + # # Make sure what we got is an appropriately open/close joint. + # random.shuffle(relevant_joints) + # selected_joint = None + # for joint in relevant_joints: + # current_position = joint.get_state()[0][0] + # joint_range = joint.upper_limit - joint.lower_limit + # openness_fraction = (current_position - joint.lower_limit) / joint_range + # if (should_open and openness_fraction < 0.8) or (not should_open and openness_fraction > 0.05): + # selected_joint = joint + + + # target_obj = cabinet + # relevant_joint = relevant_joints_full[1][2] + + # link_name = relevant_joint.body1.split("/")[-1] + + # # Get the bounding box of the child link. + # ( + # bbox_center_in_world, + # bbox_quat_in_world, + # bbox_extent_in_link_frame, + # _, + # ) = target_obj.get_base_aligned_bbox(link_name=link_name, visual=False) + # from IPython import embed; embed() + # # Match the push axis to one of the bb axes. + # push_axis_idx = JointAxis.index(relevant_joint.axis) + # canonical_push_axis = np.eye(3)[push_axis_idx] + # joint_orientation = gf_quat_to_np_array(relevant_joint.get_attribute("physics:localRot0"))[[1, 2, 3, 0]] + # push_axis = R.from_quat(joint_orientation).apply([1, 0, 0]) + # assert np.isclose(np.max(np.abs(push_axis)), 1.0) # Make sure we're aligned with a bb axis. + # push_axis_idx = np.argmax(np.abs(push_axis)) + # canonical_push_axis = np.eye(3)[push_axis_idx] + + + # # TODO: Need to figure out how to get the correct push direction. + # canonical_push_direction = canonical_push_axis * np.sign(push_axis[push_axis_idx]) + + # # Pick the closer of the two faces along the push axis as our favorite. + # points_along_push_axis = ( + # np.array([canonical_push_axis, -canonical_push_axis]) * bbox_extent_in_link_frame[push_axis_idx] / 2 + # ) + # ( + # push_axis_closer_side_idx, + # center_of_selected_surface_along_push_axis, + # _, + # ) = get_closest_point_to_point_in_world_frame( + # points_along_push_axis, (bbox_center_in_world, bbox_quat_in_world), robot.get_position() + # ) + # push_axis_closer_side_sign = 1 if push_axis_closer_side_idx == 0 else -1 + + # # Pick the other axes. + # all_axes = list(set(range(3)) - {push_axis_idx}) + # x_axis_idx, y_axis_idx = tuple(sorted(all_axes)) + # canonical_x_axis = np.eye(3)[x_axis_idx] + # canonical_y_axis = np.eye(3)[y_axis_idx] + + # # Find the correct side of the lateral axis & go some distance along that direction. + # min_lateral_pos_wrt_surface_center = (canonical_x_axis + canonical_y_axis) * -bbox_extent_in_link_frame / 2 + # max_lateral_pos_wrt_surface_center = (canonical_x_axis + canonical_y_axis) * bbox_extent_in_link_frame / 2 + # diff_lateral_pos_wrt_surface_center = max_lateral_pos_wrt_surface_center - min_lateral_pos_wrt_surface_center + # sampled_lateral_pos_wrt_min = np.random.uniform( + # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[0] * diff_lateral_pos_wrt_surface_center, + # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[1] * diff_lateral_pos_wrt_surface_center, + # ) + # lateral_pos_wrt_surface_center = min_lateral_pos_wrt_surface_center + sampled_lateral_pos_wrt_min + # grasp_position_in_bbox_frame = center_of_selected_surface_along_push_axis + lateral_pos_wrt_surface_center + # grasp_quat_in_bbox_frame = T.quat_inverse(joint_orientation) + + # # Now apply the grasp offset. + # offset_grasp_pose_in_bbox_frame = (grasp_position_in_bbox_frame, grasp_quat_in_bbox_frame) + # offset_grasp_pose_in_world_frame = T.pose_transform( + # bbox_center_in_world, bbox_quat_in_world, *offset_grasp_pose_in_bbox_frame + # ) + + # # To compute the rotation position, we want to decide how far along the rotation axis we'll go. + # target_joint_pos = relevant_joint.upper_limit if should_open else relevant_joint.lower_limit + # current_joint_pos = relevant_joint.get_state()[0][0] + + # required_pos_change = target_joint_pos - current_joint_pos + # push_vector_in_bbox_frame = canonical_push_direction * required_pos_change + # target_hand_pos_in_bbox_frame = grasp_position_in_bbox_frame + push_vector_in_bbox_frame + # target_hand_pos_in_world_frame = T.pose_transform( + # bbox_center_in_world, bbox_quat_in_world, target_hand_pos_in_bbox_frame, grasp_quat_in_bbox_frame + # ) + + # # Compute the approach direction. + # approach_direction_in_world_frame = R.from_quat(bbox_quat_in_world).apply(canonical_push_axis * -push_axis_closer_side_sign) + + # # Decide whether a grasp is required. If approach direction and displacement are similar, no need to grasp. + # grasp_required = np.dot(push_vector_in_bbox_frame, canonical_push_axis * -push_axis_closer_side_sign) < 0 + + # # return ( + # # offset_grasp_pose_in_world_frame, + # # [target_hand_pos_in_world_frame], + # # approach_direction_in_world_frame, + # # relevant_joint, + # # grasp_required, + # # ) + # from IPython import embed; embed() + + +if __name__ == "__main__": + main() + + + diff --git a/dev_environment/test_interpolation.py b/dev_environment/test_interpolation.py new file mode 100644 index 000000000..f722f0277 --- /dev/null +++ b/dev_environment/test_interpolation.py @@ -0,0 +1,80 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + +import argparse + + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + actions = [] + for action in ctrl_gen: + env.step(action) + actions.append(action.tolist()) + if filename is not None: + with open(filename, "w") as f: + yaml.dump(actions, f) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + grasp_obj = DatasetObject( + name="cologne", + category="cologne", + model="lyipur", + scale=[0.01, 0.01, 0.01] + ) + og.sim.import_object(grasp_obj) + grasp_obj.set_position([-0.3, -0.8, 0.5]) + og.sim.step() + + pause(2) + controller = StarterSemanticActionPrimitives(None, scene, robot) + + def set_start_pose(): + control_idx = np.concatenate([robot.trunk_control_idx]) + robot.set_joint_positions([0.1], control_idx) + og.sim.step() + + def test_grasp_no_navigation(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + set_start_pose() + pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.230183, -1.87]) + robot.set_position_orientation(*pose) + og.sim.step() + execute_controller(controller._grasp(grasp_obj), env) + + test_grasp_no_navigation() + +if __name__ == "__main__": + main() + + + diff --git a/dev_environment/test_open_revolute.py b/dev_environment/test_open_revolute.py new file mode 100644 index 000000000..55bed30f5 --- /dev/null +++ b/dev_environment/test_open_revolute.py @@ -0,0 +1,377 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject +from omnigibson.utils.grasping_planning_utils import get_grasp_position_for_open + +import cProfile, pstats, io +import time +import os +import argparse + +gm.DEBUG = True + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + actions = [] + for action in ctrl_gen: + env.step(action) + actions.append(action.tolist()) + if filename is not None: + with open(filename, "w") as f: + yaml.dump(actions, f) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + # open_obj = DatasetObject( + # name="fridge", + # category="fridge", + # model="dszchb", + # scale=[0.7, 0.7, 0.7] + # ) + + open_obj = DatasetObject( + name="bottom_cabinet", + category="bottom_cabinet", + model="bamfsz", + scale=[0.7, 0.7, 0.7] + ) + + og.sim.import_object(open_obj) + # open_obj.set_position([-1.2, -0.4, 0.5]) + open_obj.set_position_orientation([-1.2, -0.4, 0.5], T.euler2quat([0, 0, np.pi/2])) + og.sim.step() + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + def test_open_no_navigation(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + set_start_pose() + pose = controller._get_robot_pose_from_2d_pose([-1.0, -0.5, np.pi/2]) + robot.set_position_orientation(*pose) + og.sim.step() + get_grasp_position_for_open(robot, open_obj, True) + # execute_controller(controller._open_or_close(cabinet), env) + + def test_open(): + set_start_pose() + # pose_2d = [-0.231071, -0.272773, 2.55196] + + # # pose_2d = [-0.282843, 0.297682, -3.07804] + # pose = controller._get_robot_pose_from_2d_pose(pose_2d) + # robot.set_position_orientation(*pose) + # og.sim.step() + + # joint_pos = [0.0133727 ,0.216775 ,0.683931 ,2.04371 ,1.88204 ,0.720747 ,1.23276 ,1.72251] + # control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) + # robot.set_joint_positions(joint_pos, control_idx) + # og.sim.step() + # pause(100) + # execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.OPEN, *(open_obj,)), env) + execute_controller(controller._open_or_close(open_obj, False), env) + + markers = [] + for i in range(20): + marker = PrimitiveObject( + prim_path=f"/World/test_{i}", + name=f"test_{i}", + primitive_type="Cube", + size=0.07, + visual_only=True, + rgba=[1.0, 0, 0, 1.0]) + markers.append(marker) + og.sim.import_object(marker) + + # from omnigibson.object_states.open import _get_relevant_joints + controller.markers = markers + # j = _get_relevant_joints(open_obj)[1][0] + # j.set_pos(0.5) + # pause(2) + open_obj.joints["j_link_2"].set_pos(0.4) + og.sim.step() + test_open() + pause(5) + + # markers = [] + # for i in range(20): + # marker = PrimitiveObject( + # prim_path=f"/World/test_{i}", + # name=f"test_{i}", + # primitive_type="Cube", + # size=0.07, + # visual_only=True, + # rgba=[1.0, 0, 0, 1.0]) + # markers.append(marker) + # og.sim.import_object(marker) + + def set_marker(position, idx): + markers[idx].set_position(position) + og.sim.step() + + def get_closest_point_to_point_in_world_frame(vectors_in_arbitrary_frame, arbitrary_frame_to_world_frame, point_in_world): + vectors_in_world = np.array( + [ + T.pose_transform(*arbitrary_frame_to_world_frame, vector, [0, 0, 0, 1])[0] + for vector in vectors_in_arbitrary_frame + ] + ) + + vector_distances_to_point = np.linalg.norm(vectors_in_world - np.array(point_in_world)[None, :], axis=1) + closer_option_idx = np.argmin(vector_distances_to_point) + vector_in_arbitrary_frame = vectors_in_arbitrary_frame[closer_option_idx] + vector_in_world_frame = vectors_in_world[closer_option_idx] + + return closer_option_idx, vector_in_arbitrary_frame, vector_in_world_frame + + def get_quaternion_between_vectors(v1, v2): + """ + Get the quaternion between two vectors. + + Args: + v1: The first vector. + v2: The second vector. + + Returns: + The quaternion between the two vectors. + """ + q = np.cross(v1, v2).tolist() + w = np.sqrt((np.linalg.norm(v1) ** 2) * (np.linalg.norm(v2) ** 2)) + np.dot(v1, v2) + # np.sqrt((np.linalg.norm(a) ^ 2) * (np.linalg.norm(b) ** 2)) + np.dot(a, b) + q.append(w) + q = np.array(q) / np.linalg.norm(q) + return q + + def rotate_point_around_axis(point_wrt_arbitrary_frame, arbitrary_frame_wrt_origin, joint_axis, yaw_change): + # grasp_pose_in_bbox_frame, bbox_wrt_origin, joint_axis, partial_yaw_change + # rotation_to_joint = get_quaternion_between_vectors([1, 0, 0], joint_axis) + # rotation = T.quat_multiply(rotation_to_joint, T.euler2quat([yaw_change, 0, 0])) + # rotation = T.quat_multiply(rotation, T.quat_inverse(rotation_to_joint)) + rotation = R.from_rotvec(joint_axis * yaw_change).as_quat() + origin_wrt_arbitrary_frame = T.invert_pose_transform(*arbitrary_frame_wrt_origin) + + pose_in_origin_frame = T.pose_transform(*arbitrary_frame_wrt_origin, *point_wrt_arbitrary_frame) + rotated_pose_in_origin_frame = T.pose_transform([0, 0, 0], rotation, *pose_in_origin_frame) + rotated_pose_in_arbitrary_frame = T.pose_transform(*origin_wrt_arbitrary_frame, *rotated_pose_in_origin_frame) + return rotated_pose_in_arbitrary_frame + + def get_orientation_facing_vector_with_random_yaw(vector): + forward = vector / np.linalg.norm(vector) + rand_vec = np.random.rand(3) + rand_vec /= np.linalg.norm(3) + side = np.cross(rand_vec, forward) + side /= np.linalg.norm(3) + up = np.cross(forward, side) + assert np.isclose(np.linalg.norm(up), 1) + rotmat = np.array([forward, side, up]).T + return R.from_matrix(rotmat).as_quat() + + ######################################################## + + # # from omnigibson.objects.primitive_object import PrimitiveObject + # # import numpy as np + # import random + # from scipy.spatial.transform import Rotation + + # # import omnigibson.utils.transform_utils as T + # from omnigibson.object_states.open import _get_relevant_joints + # from omnigibson.utils.constants import JointType, JointAxis + # from omni.isaac.core.utils.rotations import gf_quat_to_np_array + # from scipy.spatial.transform import Rotation as R + # from math import ceil + + # p = None + # grasp_position_for_open_on_revolute_joint = None + # should_open = True + # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS = (0.2, 0.8) + # GRASP_OFFSET = np.array([0, 0.05, -0.08]) + # OPEN_GRASP_OFFSET = np.array([0, 0.05, -0.12]) # 5cm back and 12cm up. + # ROTATION_ARC_SEGMENT_LENGTHS = 0.1 + + # # Pick a moving link of the object. + # relevant_joints_full = _get_relevant_joints(open_obj) + # relevant_joints = relevant_joints_full[1] + + # if len(relevant_joints) == 0: + # raise ValueError("Cannot open/close object without relevant joints.") + + # # Make sure what we got is an appropriately open/close joint. + # random.shuffle(relevant_joints) + # selected_joint = None + # for joint in relevant_joints: + # current_position = joint.get_state()[0][0] + # joint_range = joint.upper_limit - joint.lower_limit + # openness_fraction = (current_position - joint.lower_limit) / joint_range + # if (should_open and openness_fraction < 0.8) or (not should_open and openness_fraction > 0.05): + # selected_joint = joint + + # robot = robot + # target_obj = open_obj + # relevant_joint = selected_joint + # should_open = True + # REVOLUTE_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS = (0.4, 0.6) + + # link_name = relevant_joint.body1.split("/")[-1] + # link = target_obj.links[link_name] + + # # Get the bounding box of the child link. + # ( + # bbox_center_in_world, + # bbox_quat_in_world, + # bbox_extent_in_link_frame, + # bbox_center_in_obj_frame + # ) = target_obj.get_base_aligned_bbox(link_name=link_name, visual=False) + + # # Get the part of the object away from the joint position/axis. + # # The link origin is where the joint is. Let's get the position of the origin w.r.t the CoM. + # # from IPython import embed; embed() + # # [target_bid] = target_obj.get_body_ids() + # # dynamics_info = p.getDynamicsInfo(target_bid, link_id) + # # com_wrt_origin = (dynamics_info[3], dynamics_info[4]) + + # # bbox_wrt_origin = T.pose_transform(link.get_position(), [0, 0, 0, 1], bbox_center_in_link_frame, [0, 0, 0, 1]) + # bbox_center_in_world_frame = T.pose_transform(*target_obj.get_position_orientation(), bbox_center_in_obj_frame, [0, 0, 0, 1])[0] + # bbox_wrt_origin = T.relative_pose_transform(bbox_center_in_world_frame, bbox_quat_in_world, *link.get_position_orientation()) + # origin_wrt_bbox = T.invert_pose_transform(*bbox_wrt_origin) + # # from IPython import embed; embed() + + # joint_orientation = gf_quat_to_np_array(relevant_joint.get_attribute("physics:localRot0"))[[1, 2, 3, 0]] + # joint_axis = R.from_quat(joint_orientation).apply([1, 0, 0]) + # joint_axis /= np.linalg.norm(joint_axis) + # origin_towards_bbox = np.array(bbox_wrt_origin[0]) + # open_direction = np.cross(joint_axis, origin_towards_bbox) + # open_direction /= np.linalg.norm(open_direction) + # lateral_axis = np.cross(open_direction, joint_axis) + + # # Match the axes to the canonical axes of the link bb. + # lateral_axis_idx = np.argmax(np.abs(lateral_axis)) + # open_axis_idx = np.argmax(np.abs(open_direction)) + # joint_axis_idx = np.argmax(np.abs(joint_axis)) + # assert lateral_axis_idx != open_axis_idx + # assert lateral_axis_idx != joint_axis_idx + # assert open_axis_idx != joint_axis_idx + + # # Find the correct side of the push/pull axis to grasp from. To do this, imagine the closed position of the object. + # # In that position, which side is the robot on? + # canonical_open_direction = np.eye(3)[open_axis_idx] + # points_along_open_axis = ( + # np.array([canonical_open_direction, -canonical_open_direction]) * bbox_extent_in_link_frame[open_axis_idx] / 2 + # ) + # current_yaw = relevant_joint.get_state()[0][0] + # closed_yaw = relevant_joint.lower_limit + # points_along_open_axis_after_rotation = [ + # rotate_point_around_axis((point, [0, 0, 0, 1]), bbox_wrt_origin, joint_axis, closed_yaw - current_yaw)[0] + # for point in points_along_open_axis + # ] + # open_axis_closer_side_idx, _, _ = get_closest_point_to_point_in_world_frame( + # points_along_open_axis_after_rotation, (bbox_center_in_world, bbox_quat_in_world), robot.get_position() + # ) + # open_axis_closer_side_sign = 1 if open_axis_closer_side_idx == 0 else -1 + # center_of_selected_surface_along_push_axis = points_along_open_axis[open_axis_closer_side_idx] + + # # Find the correct side of the lateral axis & go some distance along that direction. + # canonical_joint_axis = np.eye(3)[joint_axis_idx] + # lateral_away_from_origin = np.eye(3)[lateral_axis_idx] * np.sign(origin_towards_bbox[lateral_axis_idx]) + # min_lateral_pos_wrt_surface_center = ( + # lateral_away_from_origin * -np.array(origin_wrt_bbox[0]) + # - canonical_joint_axis * bbox_extent_in_link_frame[lateral_axis_idx] / 2 + # ) + # max_lateral_pos_wrt_surface_center = ( + # lateral_away_from_origin * bbox_extent_in_link_frame[lateral_axis_idx] / 2 + # + canonical_joint_axis * bbox_extent_in_link_frame[lateral_axis_idx] / 2 + # ) + # diff_lateral_pos_wrt_surface_center = max_lateral_pos_wrt_surface_center - min_lateral_pos_wrt_surface_center + # sampled_lateral_pos_wrt_min = np.random.uniform( + # REVOLUTE_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[0] * diff_lateral_pos_wrt_surface_center, + # REVOLUTE_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[1] * diff_lateral_pos_wrt_surface_center, + # ) + # lateral_pos_wrt_surface_center = min_lateral_pos_wrt_surface_center + sampled_lateral_pos_wrt_min + # grasp_position = center_of_selected_surface_along_push_axis + lateral_pos_wrt_surface_center + # # Get the appropriate rotation + + # grasp_quat_in_bbox_frame = get_quaternion_between_vectors([1, 0, 0], canonical_open_direction * open_axis_closer_side_sign * -1) + + # # grasp_quat_in_bbox_frame = get_orientation_facing_vector_with_random_yaw(canonical_open_direction * open_axis_closer_side_sign * -1) + # # Now apply the grasp offset. + # offset_in_bbox_frame = canonical_open_direction * open_axis_closer_side_sign * 0.2 + # offset_grasp_pose_in_bbox_frame = (grasp_position + offset_in_bbox_frame, grasp_quat_in_bbox_frame) + # offset_grasp_pose_in_world_frame = T.pose_transform( + # bbox_center_in_world, bbox_quat_in_world, *offset_grasp_pose_in_bbox_frame + # ) + # grasp_pose_in_world_frame = T.pose_transform(bbox_center_in_world, bbox_quat_in_world, grasp_position, grasp_quat_in_bbox_frame) + + # # To compute the rotation position, we want to decide how far along the rotation axis we'll go. + # desired_yaw = relevant_joint.upper_limit if should_open else relevant_joint.lower_limit + # required_yaw_change = desired_yaw - current_yaw + + # # Now we'll rotate the grasp position around the origin by the desired rotation. + # # Note that we use the non-offset position here since the joint can't be pulled all the way to the offset. + # grasp_pose_in_bbox_frame = grasp_position, grasp_quat_in_bbox_frame + # grasp_pose_in_origin_frame = T.pose_transform(*bbox_wrt_origin, *grasp_pose_in_bbox_frame) + + # # Get the arc length and divide it up to 10cm segments + # arc_length = abs(required_yaw_change) * np.linalg.norm(grasp_pose_in_origin_frame[0]) + # turn_steps = int(ceil(arc_length / ROTATION_ARC_SEGMENT_LENGTHS)) + # targets = [] + # for i in range(turn_steps): + # partial_yaw_change = (i + 1) / turn_steps * required_yaw_change + # rotated_grasp_pose_in_bbox_frame = rotate_point_around_axis( + # grasp_pose_in_bbox_frame, bbox_wrt_origin, joint_axis, partial_yaw_change + # ) + # rotated_grasp_pose_in_world_frame = T.pose_transform( + # bbox_center_in_world, bbox_quat_in_world, *rotated_grasp_pose_in_bbox_frame + # ) + # targets.append(rotated_grasp_pose_in_world_frame) + + # # Compute the approach direction. + # approach_direction_in_world_frame = Rotation.from_quat(bbox_quat_in_world).apply(canonical_open_direction * -open_axis_closer_side_sign) + + # # Decide whether a grasp is required. If approach direction and displacement are similar, no need to grasp. + # movement_in_world_frame = np.array(targets[-1][0]) - np.array(offset_grasp_pose_in_world_frame[0]) + # grasp_required = np.dot(movement_in_world_frame, approach_direction_in_world_frame) < 0 + + # from IPython import embed; embed() +if __name__ == "__main__": + main() + + + diff --git a/dev_environment/test_tiago.py b/dev_environment/test_tiago.py new file mode 100644 index 000000000..25632aafe --- /dev/null +++ b/dev_environment/test_tiago.py @@ -0,0 +1,285 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + +import cProfile, pstats, io +import time +import os +import argparse + + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + actions = [] + for action in ctrl_gen: + env.step(action) + # actions.append(action.tolist()) + # if filename is not None: + # with open(filename, "w") as f: + # yaml.dump(actions, f) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + table = DatasetObject( + name="table", + category="breakfast_table", + model="rjgmmy", + scale = [0.3, 0.3, 0.3] + ) + og.sim.import_object(table) + # table.set_position([-0.7, -2.0, 0.2]) + table.set_position([-0.7, 0.5, 0.2]) + og.sim.step() + + grasp_obj = DatasetObject( + name="cologne", + category="bottle_of_cologne", + model="lyipur" + ) + og.sim.import_object(grasp_obj) + grasp_obj.set_position([-0.3, -0.8, 0.5]) + og.sim.step() + + # marker = PrimitiveObject( + # prim_path=f"/World/marker", + # name="marker", + # primitive_type="Cube", + # size=0.07, + # visual_only=True, + # rgba=[1.0, 0, 0, 1.0], + # ) + # og.sim.import_object(marker) + # marker.set_position_orientation([-0.29840604, -0.79821703, 0.59273211], [0. , 0.70710678, 0. , 0.70710678]) + # og.sim.step() + + + # robot.set_position([-2.0, 0.0, 0.0]) + # pause(2) + controller = StarterSemanticActionPrimitives(None, scene, robot) + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + def test_navigate_to_obj(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # execute_controller(controller._reset_hand(), env) + set_start_pose() + execute_controller(controller._navigate_to_obj(table), env) + + def test_grasp_no_navigation(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + set_start_pose() + pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) + robot.set_position_orientation(*pose) + og.sim.step() + # replay_controller(env, "./replays/test_grasp_pose.yaml") + execute_controller(controller._grasp(grasp_obj), env) + + def test_grasp(): + # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself + # execute_controller(controller._reset_hand(), env) + set_start_pose() + # pause(2) + execute_controller(controller._grasp(grasp_obj), env) + + def test_place(): + test_grasp() + pause(1) + execute_controller(controller._place_on_top(table), env) + + # positions = [[0.09988395869731903, -1.0999969244003296, 1.4699827432632446, 2.710009813308716, 1.710004448890686, -1.5700018405914307, 1.3899955749511719, 2.001982011279324e-07], [0.15954897383415584, -0.9759483151785584, 1.051426922254121, 1.3919954813427862, 1.9255247232751793, -0.46858315638703396, 1.135518807525537, 0.5174528326963662], [0.15062408833826937, -0.4437143998615267, 0.8304433521196042, 1.437534104367112, 1.6164805582338932, -0.37533100951328124, 0.6381778036539293, -0.0283867578914061], [0.13373369762078724, 0.5635409634642365, 0.41223083348993295, 1.5237161776241306, 1.0316129205581803, -0.1988508522420569, -0.30304254915485307, -1.0613909301407032], [0.11684330690330513, 1.57079632679, -0.005981685139738268, 1.6098982508811492, 0.4467452828824673, -0.022370694970832543, -1.244262901963635, -2.09439510239]] + control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) + # robot.set_position_orientation([-1.08215380e+00, -3.35281938e-01, -2.77837131e-07], [ 1.78991655e-07, -4.65450078e-08, -2.67762393e-01, 9.63485003e-01]) + # og.sim.step() + # for p in positions: + # robot.set_joint_positions(p, control_idx) + # pause(0.2) + # pause(100) + # Work more reliably + # og.sim._physics_context.set_gravity(value=-0.1) + # execute_controller(controller._reset_hand(), env) + # robot.set_position_orientation([-1.08215380e+00, -3.35281938e-01, -2.77837131e-07], [ 1.78991655e-07, -4.65450078e-08, -2.67762393e-01, 9.63485003e-01]) + # og.sim.step() + # execute_controller(controller._reset_hand(), env) + # grasp_pose = [-0.29840604, -0.79821703, 0.59273211], [0. , 0.70710678, 0. , 0.70710678] + # execute_controller(controller._reset_hand(), env) + # execute_controller(controller._move_hand(grasp_pose), env) + # test_grasp_no_navigation() + + # end_joint = [0.11684331, 1.57079633, -0.00598169, 1.60989825, 0.44674528, -0.02237069, -1.2442629, -2.0943951] + # robot.set_joint_positions(end_joint, control_idx) + # pause(1) + # from IPython import embed; embed() + # og.sim.step() + # Don't work as reliably + + # pose_2d = [-0.543999, -0.233287,-1.16071] + # pos = np.array([pose_2d[0], pose_2d[1], 0.05]) + # orn = T.euler2quat([0, 0, pose_2d[2]]) + # robot.set_position_orientation(pos, orn) + # og.sim.step() + # pause(10) + # t_pose = ([-0.29840604, -0.79821703, 0.59273211], [0. , 0.70710678, 0. , 0.70710678]) + # execute_controller(controller._reset_hand(), env) + # execute_controller(controller._move_hand(t_pose), env) + + # try: + # test_grasp_no_navigation() + # except: + # pass + + + test_grasp_no_navigation() + # test_grasp() + # test_navigate_to_obj() + # set_start_pose() + # execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, grasp_obj), env) + # execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.PLACE_ON_TOP, table), env) + # test_place() + pause(5) + + # test_grasp_no_navigation() + # set_start_pose() + # pose_2d = [-0.15, -0.269397, -2.0789] + # pos = np.array([pose_2d[0], pose_2d[1], 0.05]) + # orn = T.euler2quat([0, 0, pose_2d[2]]) + # robot.set_position_orientation(pos, orn) + # pause(2) + + # with UndoableContext(robot, "base") as context: + # print(not detect_robot_collision(context, (pos, orn))) + # for i in range(10000): + # og.sim.render() + # test_grasp() + # test_place() + + # replay_controller(env, "./replays/tiago_grasp.yaml") + # execute_controller(controller.place_on_top(table), env) + # from IPython import embed; embed() + # set_start_pose() + # execute_controller(controller._navigate_to_pose([-0.3, -2.3, 0.0]), env) + # execute_controller(controller._navigate_to_pose(pose_2d), env) + # test_place() + # test_grasp_no_navigation() + + # pause(100) + + ################################################################################### + # Random test code below + ################################################################################### + # def detect_robot_collision(robot, filter_objs=[]): + # filter_categories = ["floors"] + + # obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] + # if obj_in_hand is not None: + # filter_objs.append(obj_in_hand) + + # collision_prims = list(robot.states[ContactBodies].get_value(ignore_objs=tuple(filter_objs))) + + # for col_prim in collision_prims: + # tokens = col_prim.prim_path.split("/") + # obj_prim_path = "/".join(tokens[:-1]) + # col_obj = og.sim.scene.object_registry("prim_path", obj_prim_path) + # if col_obj.category in filter_categories: + # collision_prims.remove(col_prim) + + # return len(collision_prims) > 0 or detect_self_collision(robot) + + # def detect_self_collision(robot): + # contacts = robot.contact_list() + # robot_links = [link.prim_path for link in robot.links.values()] + # disabled_pairs = [set(p) for p in robot.disabled_collision_pairs] + # for c in contacts: + # link0 = c.body0.split("/")[-1] + # link1 = c.body1.split("/")[-1] + # if {link0, link1} not in disabled_pairs and c.body0 in robot_links and c.body1 in robot_links: + # return True + # return False + + # robot.set_position([-0.1, -0.35, 0.05]) + # robot.set_orientation(T.euler2quat([0, 0,-np.pi/1.5])) + # og.sim.step() + # control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) + # joint_pos = [0.284846, 1.22316, 0.323617, 1.72149, 1.4959, -0.31599, -1.4043, 0.152401] + # robot.set_joint_positions(joint_pos, control_idx) + # og.sim.step() + # while True: + # coll = [] + # robot_links = [link.prim_path for link in robot.links.values()] + # for c in robot.contact_list(): + # if c.body0 in robot_links and c.body1 in robot_links: + # link0 = c.body0.split("/")[-1] + # link1 = c.body1.split("/")[-1] + # pair = {link0, link1} + # if pair not in coll: + # coll.append(pair) + + # print(coll) + # print(detect_robot_collision(robot)) + # print(detect_self_collision(robot)) + # print("---------------") + # pause(2) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run test script") + parser.add_argument( + "--profile", + action="store_true", + help="If set, profile code and generate .prof file", + ) + args = parser.parse_args() + if args.profile: + pr = cProfile.Profile() + pr.enable() + main() + pr.disable() + s = io.StringIO() + results = pstats.Stats(pr) + filename = f'profile-{os.path.basename(__file__)}-{time.strftime("%Y%m%d-%H%M%S")}' + results.dump_stats(f"./profiles/{filename}.prof") + os.system(f"flameprof ./profiles/{filename}.prof > ./profiles/{filename}.svg") + # Run `snakeviz ./profiles/.prof` to visualize stack trace or open .svg in a browser + else: + main() + + + diff --git a/dev_environment/test_tiago.yaml b/dev_environment/test_tiago.yaml new file mode 100644 index 000000000..11af1ae94 --- /dev/null +++ b/dev_environment/test_tiago.yaml @@ -0,0 +1,71 @@ +env: + initial_pos_z_offset: 0.1 + +render: + viewer_width: 1280 + viewer_height: 720 + +scene: + type: InteractiveTraversableScene + scene_model: Rs_int + trav_map_resolution: 0.1 + trav_map_erosion: 2 + trav_map_with_objects: true + build_graph: true + num_waypoints: 1 + waypoint_resolution: 0.2 + load_object_categories: null + not_load_object_categories: null + load_room_types: null + load_room_instances: null + seg_map_resolution: 0.1 + scene_source: OG + include_robots: true + +robots: + - type: Tiago + obs_modalities: [scan, rgb, depth] + scale: 1.0 + self_collisions: true + action_normalize: false + action_type: continuous + grasping_mode: sticky + rigid_trunk: false + default_arm_pose: diagonal30 + default_trunk_offset: 0.365 + controller_config: + base: + name: JointController + motor_type: velocity + arm_left: + name: JointController + motor_type: position + command_input_limits: null + command_output_limits: null + use_delta_commands: false + arm_right: + name: JointController + motor_type: position + command_input_limits: null + command_output_limits: null + use_delta_commands: false + gripper_left: + name: JointController + motor_type: position + command_input_limits: [-1, 1] + command_output_limits: null + use_delta_commands: true + use_single_command: true + gripper_right: + name: JointController + motor_type: position + command_input_limits: [-1, 1] + command_output_limits: null + use_delta_commands: true + use_single_command: true + camera: + name: JointController + motor_type: velocity + use_delta_commands: false + +objects: [] \ No newline at end of file diff --git a/dev_environment/test_toggle.py b/dev_environment/test_toggle.py new file mode 100644 index 000000000..b90af868a --- /dev/null +++ b/dev_environment/test_toggle.py @@ -0,0 +1,84 @@ +import yaml +import numpy as np +import argparse + +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet +from omnigibson.objects.primitive_object import PrimitiveObject +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + +import cProfile, pstats, io +import time +import os +import argparse + + +def pause(time): + for _ in range(int(time*100)): + og.sim.step() + +def replay_controller(env, filename): + actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) + for action in actions: + env.step(action) + +def execute_controller(ctrl_gen, env, filename=None): + actions = [] + for action in ctrl_gen: + env.step(action) + actions.append(action.tolist()) + if filename is not None: + with open(filename, "w") as f: + yaml.dump(actions, f) + +def main(): + # Load the config + config_filename = "test_tiago.yaml" + config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) + + config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] + + # Load the environment + env = og.Environment(configs=config) + scene = env.scene + robot = env.robots[0] + + # Allow user to move camera more easily + og.sim.enable_viewer_camera_teleoperation() + + grasp_obj = DatasetObject( + name="floor_lamp", + category="floor_lamp", + model="vdxlda" + ) + og.sim.import_object(grasp_obj) + grasp_obj.set_position([-0.3, -0.8, 0.5]) + og.sim.step() + + controller = StarterSemanticActionPrimitives(None, scene, robot) + + def set_start_pose(): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + + set_start_pose() + execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.TOGGLE_ON, grasp_obj), env) + pause(5) + + +if __name__ == "__main__": + main() + + + diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 9bb451524..9d467b17a 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -471,7 +471,8 @@ def _open_or_close(self, obj, should_open): yield from self._move_hand_direct_cartesian(approach_pose, ignore_failure=False, stop_on_contact=True) # Step once to update - yield self._empty_action() + for i in range(MAX_STEPS_FOR_GRASP_OR_RELEASE): + yield self._empty_action() # if grasp_required: # if self._get_obj_in_hand() is None: @@ -550,17 +551,16 @@ def _grasp(self, obj): # It's okay if we can't go all the way because we run into the object. indented_print("Performing grasp approach") yield from self._move_hand_direct_cartesian(approach_pose, stop_on_contact=True) - print("touched") + # Step once to update yield self._empty_action() - # if self._get_obj_in_hand() is None: - # print("error") - # raise ActionPrimitiveError( - # ActionPrimitiveError.Reason.POST_CONDITION_ERROR, - # "Grasp completed, but no object detected in hand after executing grasp", - # {"target object": obj.name}, - # ) + if self._get_obj_in_hand() is None: + raise ActionPrimitiveError( + ActionPrimitiveError.Reason.POST_CONDITION_ERROR, + "Grasp completed, but no object detected in hand after executing grasp", + {"target object": obj.name}, + ) yield from self._reset_hand() @@ -1110,6 +1110,7 @@ def _get_reset_joint_pos(self): 4.50000000e-02, 4.50000000e-02 ]) + return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): diff --git a/omnigibson/reward_functions/grasp_reward.py b/omnigibson/reward_functions/grasp_reward.py index 2743153a0..305fc7de8 100644 --- a/omnigibson/reward_functions/grasp_reward.py +++ b/omnigibson/reward_functions/grasp_reward.py @@ -1,3 +1,4 @@ +import math from omnigibson.reward_functions.reward_function_base import BaseRewardFunction import omnigibson.utils.transform_utils as T @@ -37,8 +38,9 @@ def _step(self, task, env, action): if not self.prev_grasping and not current_grasping: eef_pos = robot.get_eef_position(robot.default_arm) - obj_pos = self.obj.get_position() - reward = T.l2_distance(eef_pos, obj_pos) * self.dist_coeff + obj_center = self.obj.aabb_center + dist = T.l2_distance(eef_pos, obj_center) + reward = math.exp(dist) * self.dist_coeff elif not self.prev_grasping and current_grasping: reward = self.grasp_reward @@ -47,8 +49,10 @@ def _step(self, task, env, action): reward = -self.grasp_reward elif self.prev_grasping and current_grasping: - # Need to finish - reward = 0 + robot_center = robot.aabb_center + obj_center = self.obj.aabb_center + dist = T.l2_distance(robot_center, obj_center) + reward = math.exp(dist) * self.dist_coeff self.prev_grasping = current_grasping return reward, {} diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index ca2318e98..b8f03e127 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -58,7 +58,6 @@ def can_assisted_grasp(obj, link_name): # Allow based on mass mass = obj.links[link_name].mass - print(mass) if mass <= m.ASSIST_GRASP_MASS_THRESHOLD: return True @@ -113,7 +112,7 @@ def __init__( # Unique to ManipulationRobot grasping_mode="physical", - disable_grasp_handling=True, # TODO: revert to False. This is for debugging purposes + disable_grasp_handling=False, # TODO: revert to False. This is for debugging purposes **kwargs, ): @@ -770,7 +769,6 @@ def _calculate_in_hand_object_rigid(self, arm="default"): ag_obj_prim_path = "/".join(ag_prim_path.split("/")[:-1]) ag_obj_link_name = ag_prim_path.split("/")[-1] ag_obj = og.sim.scene.object_registry("prim_path", ag_obj_prim_path) - from IPython import embed; embed() # Return None if object cannot be assisted grasped or not touching at least two fingers if ag_obj is None or (not can_assisted_grasp(ag_obj, ag_obj_link_name)) or (not touching_at_least_two_fingers): return None diff --git a/tests/test_reward_function.py b/tests/test_reward_function.py index c75fd24f2..97d5b5cc2 100644 --- a/tests/test_reward_function.py +++ b/tests/test_reward_function.py @@ -1,3 +1,4 @@ +import math import numpy as np import omnigibson as og from omnigibson.macros import gm @@ -28,7 +29,7 @@ def execute_controller(ctrl_gen, env): def test_grasp_reward(): DIST_COEFF = 0.1 - GRASP_REWARD = 1.0 + GRASP_REWARD = 0.3 cfg = { "scene": { @@ -139,7 +140,10 @@ def test_grasp_reward(): # Check reward going from not grasping to not grasping _, reward, _, _ = env.step(next(ctrl_gen)) - # assert reward == 10 + eef_pos = robot.get_eef_position(robot.default_arm) + obj_center = obj.aabb_center + expected_reward = math.exp(T.l2_distance(eef_pos, obj_center)) * DIST_COEFF + assert math.isclose(reward, expected_reward, abs_tol=0.01) for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] @@ -147,13 +151,15 @@ def test_grasp_reward(): obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] if prev_obj_in_hand is None and obj_in_hand is not None: # Check reward going from not grapsing to after grasping - print(reward) assert reward == GRASP_REWARD elif prev_obj_in_hand is not None and obj_in_hand is not None: # Check reward going from grasping to grasping - # assert reward > GRASP_REWARD + expected_reward = math.exp(T.l2_distance(robot.aabb_center, obj_center)) * DIST_COEFF + assert math.isclose(reward, expected_reward, abs_tol=0.01) break + ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.RELEASE) + for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] _, reward, _, _ = env.step(action) @@ -161,7 +167,5 @@ def test_grasp_reward(): if prev_obj_in_hand is not None and obj_in_hand is None: # Check reward going from grapsing to not grasping assert reward == -GRASP_REWARD - print(reward) - test_grasp_reward() \ No newline at end of file From 9b1528d377f5e2ef5983cb9f143e692829c23907 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 13 Sep 2023 23:05:47 -0700 Subject: [PATCH 12/29] clean up code --- dev_environment/test.yaml | 77 ------ dev_environment/test_IK.yaml | 72 ----- dev_environment/test_IK_controller.py | 117 -------- dev_environment/test_arm.py | 202 -------------- dev_environment/test_chain.py | 142 ---------- dev_environment/test_collisions.py | 244 ----------------- dev_environment/test_grasp.py | 266 ------------------ dev_environment/test_interpolation.py | 80 ------ dev_environment/test_open_revolute.py | 377 -------------------------- dev_environment/test_tiago.py | 285 ------------------- dev_environment/test_tiago.yaml | 71 ----- dev_environment/test_toggle.py | 84 ------ 12 files changed, 2017 deletions(-) delete mode 100644 dev_environment/test.yaml delete mode 100644 dev_environment/test_IK.yaml delete mode 100644 dev_environment/test_IK_controller.py delete mode 100644 dev_environment/test_arm.py delete mode 100644 dev_environment/test_chain.py delete mode 100644 dev_environment/test_collisions.py delete mode 100644 dev_environment/test_grasp.py delete mode 100644 dev_environment/test_interpolation.py delete mode 100644 dev_environment/test_open_revolute.py delete mode 100644 dev_environment/test_tiago.py delete mode 100644 dev_environment/test_tiago.yaml delete mode 100644 dev_environment/test_toggle.py diff --git a/dev_environment/test.yaml b/dev_environment/test.yaml deleted file mode 100644 index d040c7a34..000000000 --- a/dev_environment/test.yaml +++ /dev/null @@ -1,77 +0,0 @@ -env: - initial_pos_z_offset: 0.1 - -render: - viewer_width: 1280 - viewer_height: 720 - -scene: - type: InteractiveTraversableScene - scene_model: Rs_int - trav_map_resolution: 0.1 - trav_map_erosion: 2 - trav_map_with_objects: true - build_graph: true - num_waypoints: 1 - waypoint_resolution: 0.2 - load_object_categories: null - not_load_object_categories: null - load_room_types: null - load_room_instances: null - seg_map_resolution: 0.1 - scene_source: OG - include_robots: true - -robots: - - type: Fetch - obs_modalities: [scan, rgb, depth] - scale: 1.0 - self_collisions: true - # action_normalize: false - action_normalize: false - action_type: continuous - grasping_mode: sticky - rigid_trunk: false - default_trunk_offset: 0.365 - default_arm_pose: diagonal30 - reset_joint_pos: tuck - controller_config: - base: - name: DifferentialDriveController - arm_0: - name: JointController - motor_type: position - command_input_limits: null - command_output_limits: null - use_delta_commands: false - gripper_0: - name: MultiFingerGripperController - mode: binary - camera: - name: JointController - use_delta_commands: false - -objects: [] - -# task: -# type: PointNavigationTask -# robot_idn: 0 -# floor: 0 -# initial_pos: null -# initial_quat: null -# goal_pos: null -# goal_tolerance: 0.36 # turtlebot bodywidth -# goal_in_polar: false -# path_range: [1.0, 10.0] -# visualize_goal: true -# visualize_path: false -# n_vis_waypoints: 25 -# reward_type: geodesic -# termination_config: -# max_collisions: 500 -# max_steps: 500 -# fall_height: 0.03 -# reward_config: -# r_potential: 1.0 -# r_collision: 0.1 -# r_pointgoal: 10.0 diff --git a/dev_environment/test_IK.yaml b/dev_environment/test_IK.yaml deleted file mode 100644 index 8f43dc659..000000000 --- a/dev_environment/test_IK.yaml +++ /dev/null @@ -1,72 +0,0 @@ -env: - initial_pos_z_offset: 0.1 - -render: - viewer_width: 1280 - viewer_height: 720 - -scene: - type: InteractiveTraversableScene - scene_model: Rs_int - trav_map_resolution: 0.1 - trav_map_erosion: 2 - trav_map_with_objects: true - build_graph: true - num_waypoints: 1 - waypoint_resolution: 0.2 - load_object_categories: ["floors", "walls", "window", "door", "shelf", "bottom_cabinet", "top_cabinet", "countertop", "mirror", "picture", "coffee_table"] - not_load_object_categories: null - load_room_types: null - load_room_instances: null - seg_map_resolution: 0.15 - scene_source: OG - include_robots: true - -robots: - - type: Tiago - obs_modalities: [scan, rgb, depth, proprio] - proprio_obs: [robot_pose, joint_qpos, joint_qvel, eef_left_pos, eef_left_quat, grasp_left] - scale: 1.0 - self_collisions: true - action_normalize: false - action_type: continuous - grasping_mode: sticky - rigid_trunk: false - default_arm_pose: diagonal30 - controller_config: - base: - name: JointController - motor_type: velocity - arm_left: - name: InverseKinematicsController - motor_type: velocity - command_input_limits: null - command_output_limits: null - mode : pose_absolute_ori - kv: 3.0 - arm_right: - name: JointController - motor_type: position - command_input_limits: null - command_output_limits: null - use_delta_commands: true - gripper_left: - name: JointController - motor_type: position - command_input_limits: [-1, 1] - command_output_limits: null - use_delta_commands: true - use_single_command: true - gripper_right: - name: JointController - motor_type: position - command_input_limits: [-1, 1] - command_output_limits: null - use_delta_commands: true - use_single_command: true - camera: - name: JointController - motor_type: position - command_input_limits: null - command_output_limits: null - use_delta_commands: false \ No newline at end of file diff --git a/dev_environment/test_IK_controller.py b/dev_environment/test_IK_controller.py deleted file mode 100644 index 49695b65c..000000000 --- a/dev_environment/test_IK_controller.py +++ /dev/null @@ -1,117 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject - -import cProfile, pstats, io -import time -import os -import argparse - - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - for action in ctrl_gen: - env.step(action[0]) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - table = DatasetObject( - name="table", - category="breakfast_table", - model="rjgmmy", - scale = [0.3, 0.3, 0.3] - ) - og.sim.import_object(table) - # table.set_position([-0.7, -2.0, 0.2]) - table.set_position([-0.7, 0.5, 0.2]) - og.sim.step() - - grasp_obj = DatasetObject( - name="cologne", - category="bottle_of_cologne", - model="lyipur" - ) - og.sim.import_object(grasp_obj) - grasp_obj.set_position([-0.3, -0.8, 0.5]) - og.sim.step() - - controller = StarterSemanticActionPrimitives(None, scene, robot) - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - def test_navigate_to_obj(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # execute_controller(controller._reset_hand(), env) - set_start_pose() - execute_controller(controller._navigate_to_obj(table), env) - - def test_grasp_no_navigation(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - set_start_pose() - pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) - robot.set_position_orientation(*pose) - og.sim.step() - # replay_controller(env, "./replays/test_grasp_pose.yaml") - execute_controller(controller._grasp(grasp_obj), env) - - def test_grasp(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # execute_controller(controller._reset_hand(), env) - set_start_pose() - # pause(2) - execute_controller(controller._grasp(grasp_obj), env) - - def test_place(): - test_grasp() - pause(1) - execute_controller(controller._place_on_top(table), env) - - test_grasp_no_navigation() - pause(5) - - - -if __name__ == "__main__": - main() - - - diff --git a/dev_environment/test_arm.py b/dev_environment/test_arm.py deleted file mode 100644 index db6b037d2..000000000 --- a/dev_environment/test_arm.py +++ /dev/null @@ -1,202 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject -from omnigibson.utils.motion_planning_utils import detect_robot_collision, arm_planning_validity_fn -from omnigibson.utils.control_utils import FKSolver - -import cProfile, pstats, io -import time -import os -import argparse - -from omni.usd.commands import CopyPrimsCommand, DeletePrimsCommand, CopyPrimCommand, TransformPrimsCommand, TransformPrimCommand -from omnigibson.prims import CollisionGeomPrim -from pxr import Gf, Usd -from omni.isaac.core.utils.prims import get_prim_at_path - -def pause(time): - for _ in range(int(time*100)): - og.sim.render() - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "walls", "coffee_table"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - # robot.tuck() - # pause(2) - - # import random - def get_random_joint_position(): - joint_positions = [] - joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) - joints = np.array([joint for joint in robot.joints.values()]) - arm_joints = joints[joint_control_idx] - for i, joint in enumerate(arm_joints): - val = np.random.uniform(joint.lower_limit, joint.upper_limit) - joint_positions.append(val) - return joint_positions - - def set_joint_position(joint_pos): - joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) - robot.set_joint_positions(joint_pos, joint_control_idx) - - robot.tuck() - og.sim.step() - # while True: - # joint_pos = get_random_joint_position() - # set_joint_position(joint_pos) - # pause(2) - # print(joint_pos) - # print("------------------------") - - # sample_table_collision = [0.05725323620041453, 0.5163557640853469, 1.510323032160434, -4.410407307232964, -1.1433260958390707, -5.606768602222553, 1.0313821643138894, -4.181284701460742] - # sample_self_collision = [0.03053552120088664, 1.0269865478752571, 1.1344740372495958, 6.158997020615134, 1.133466907494042, -4.544473644642829, 0.6930819484783561, 4.676661155308317] - # collision_free = [0.17814310139520295, -0.8082173382782226, 1.3469484097869393, 1.6222072455290446, 2.0591874971218145, -2.9063608379063557, -0.04634827601286595, 4.505122702016582] - - #Tiago - collision = [0.14065403286781475, 0.7298650222286143, -0.4780975016232605, 1.0888713731557247, -0.03729107004351029, 3.274825625013916, 1.2937221767307419, 1.8178545818287346, 0.269868125704424, -1.1858447020249343, 1.079587475865726, 0.11286700467163624, -1.3232706255151934, 0.3340342084010399, 1.4264938203455721] - no_collision = [0.3184793294422698, 1.5142631693122297, 1.2405191873837995, 0.21394545305074741, -0.6831575211130013, -0.7389958913494964, 2.725925427761072, 1.1755425218590514, 0.6547571278019166, 0.7133282478444771, 0.5163046628994854, -1.0098026849625532, 0.09229435376315243, 1.9379299096653453, 2.0534229844998677] - - # set_joint_position(no_collision) - # pause(100) - # pause(100) - # pos = np.array([ - # 0.0, - # 0.0, - # 0.0, - # 0.0, - # 0.0, - # 0.0, - # 0.1, # trunk - # -1.1, - # -1.1, - # 0.0, - # 1.47, - # 1.47, - # 0.0, - # 2.71, - # 2.71, - # 1.71, - # 1.71, - # -1.57, - # -1.57, - # 1.39, - # 1.39, - # 0.0, - # 0.0, - # 0.045, - # 0.045, - # 0.045, - # 0.045, - # ]) - joint_pos = np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.1, # trunk - -1.1, - -1.1, - 0.0, - 1.47, - 1.47, - 0.0, - 2.71, - 2.71, - 1.71, - 1.71, - -1.57, - -1.57, - 1.39, - 1.39, - 0.0, - 0.0, - 0.045, - 0.045, - 0.045, - 0.045, - ] - ) - - - - pose_2d = [0.299906, -0.918024, 2.94397] - - pos = np.array([pose_2d[0], pose_2d[1], 0.05]) - orn = T.euler2quat([0, 0, pose_2d[2]]) - - # robot.set_position_orientation([-1.08215380e+00, -3.35281938e-01, -2.77837131e-07], [ 1.78991655e-07, -4.65450078e-08, -2.67762393e-01, 9.63485003e-01]) - # robot.set_position_orientation(pos, orn) - # og.sim.step() - positions = [[0.09640930593013763, -1.0999783277511597, 1.470136046409607, 2.7100629806518555, 1.710019826889038, -1.5699725151062012, 1.3899997472763062, -2.2541275939147454e-06], [0.11789778377430027, 1.57079632679, -0.00844635003731165, 1.6066719362098862, 0.4473218694991109, 0.019161401102889112, -1.2949643256956296, -1.9651135606361847]] - - joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) - joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) - control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] - joint_pos = joint_pos[joint_combined_idx] - joint_pos[control_idx_in_joint_pos] = [0.263221, 1.51202 ,0.277794 ,1.5376, 0.852972, -0.23253 ,-1.41372 ,1.55155] - # print(pos) - # robot.set_joint_positions([0.263221, 1.51202 ,0.277794 ,1.5376, 0.852972, -0.23253 ,-1.41372 ,1.55155], joint_control_idx) - # pause(100) - # from omnigibson.controllers.controller_base import ControlType - # action = np.zeros(robot.action_dim) - # for name, controller in robot._controllers.items(): - # joint_idx = controller.dof_idx - # action_idx = robot.controller_action_idx[name] - # if controller.control_type == ControlType.POSITION and len(joint_idx) == len(action_idx): - # action[action_idx] = robot.get_joint_positions()[joint_idx] - # action[robot.controller_action_idx["arm_left"]] = positions[1] - # for p in positions: - # robot.set_joint_positions(p, joint_control_idx) - # og.sim.step() - # pause(2) - # with UndoableContext(robot, "arm") as context: - # while True: - # jp = np.array(robot.get_joint_positions()[joint_combined_idx]) - # print(not arm_planning_validity_fn(context, jp)) - # env.step(action) - -# joint_pos = [-1.0821538e+00, -3.3528194e-01, -2.7783713e-07, 3.8149398e-07, -# -2.0263911e-07, -5.4213971e-01 , 1.1684235e-01 , 1.5707957e+00, -# -1.0999898e+00 ,-2.8282230e-07 , 5.6211746e-01 , 1.4701120e+00, -# 1.0374244e-08 , 1.6099000e+00 , 2.6821127e+00 , 4.4674629e-01, -# 1.8163613e+00 ,-2.2369886e-02, -1.5652136e+00, -1.2442690e+00, -# 1.3900158e+00, -2.0943952e+00, -5.9008621e-06, 4.4999883e-02, -# 4.5000002e-02, 4.4999868e-02, 4.5000002e-02] -# joint_pos = np.array(joint_pos)[joint_combined_idx] - # with UndoableContext(robot, "arm") as context: - # print(not arm_planning_validity_fn(context, joint_pos)) - # for i in range(10000): - # og.sim.render() - - with UndoableContext(robot, "base") as context: - print(detect_robot_collision(context, [pos, orn])) - for link in context.robot_meshes_copy: - for mesh in context.robot_meshes_copy[link]: - mesh.collision_enabled = True - for i in range(10000): - og.sim.render() - - # breakpoint() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/dev_environment/test_chain.py b/dev_environment/test_chain.py deleted file mode 100644 index a2b44e91b..000000000 --- a/dev_environment/test_chain.py +++ /dev/null @@ -1,142 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject - -import cProfile, pstats, io -import time -import os -import argparse - -from omnigibson import object_states - - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - actions = [] - for action in ctrl_gen: - env.step(action) - actions.append(action.tolist()) - if filename is not None: - with open(filename, "w") as f: - yaml.dump(actions, f) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - table = DatasetObject( - name="table", - category="breakfast_table", - model="rjgmmy", - scale = [0.3, 0.3, 0.3] - ) - og.sim.import_object(table) - table.set_position([-0.7, 0.5, 0.2]) - og.sim.step() - - open_obj = DatasetObject( - name="bottom_cabinet", - category="bottom_cabinet", - model="bamfsz", - scale=[0.5, 0.5, 0.5] - ) - og.sim.import_object(open_obj) - open_obj.set_position_orientation([-1.2, -0.4, 0.5], T.euler2quat([0, 0, np.pi/2])) - pause(3) - - calculator = DatasetObject( - name="calculator", - category="calculator", - model="kwmmty", - scale=[0.3, 0.3, 0.3] - ) - og.sim.import_object(calculator) - # calculator.set_position_orientation([, 0.0, 0.0], T.euler2quat([0, 0, 0])) - calculator.set_position_orientation([-0.95, -0.31, 0.4], T.euler2quat([0, 0, 0])) - og.sim.step() - from IPython import embed; embed() - # calculator.states[object_states.Inside].set_value(open_obj, True) - - # breakpoint() - # from IPython import embed; embed() - - - - controller = StarterSemanticActionPrimitives(None, scene, robot) - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - # def test_navigate_to_obj(): - # # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # # execute_controller(controller._reset_hand(), env) - # set_start_pose() - # execute_controller(controller._navigate_to_obj(table), env) - - # def test_grasp_no_navigation(): - # # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # # set_start_pose() - # pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) - # robot.set_position_orientation(*pose) - # og.sim.step() - # # replay_controller(env, "./replays/test_grasp_pose.yaml") - # execute_controller(controller._grasp(grasp_obj), env) - - # def test_grasp(): - # # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # # execute_controller(controller._reset_hand(), env) - # set_start_pose() - # # pause(2) - # execute_controller(controller._grasp(grasp_obj), env) - - # def test_place(): - # test_grasp() - # pause(1) - # execute_controller(controller._place_on_top(table), env) - - - # test_grasp_no_navigation() - - pause(500) - - -if __name__ == "__main__": - main() - - - diff --git a/dev_environment/test_collisions.py b/dev_environment/test_collisions.py deleted file mode 100644 index 919f5a171..000000000 --- a/dev_environment/test_collisions.py +++ /dev/null @@ -1,244 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject -from omnigibson.utils.motion_planning_utils import set_arm_and_detect_collision, set_base_and_detect_collision - -import cProfile, pstats, io -import time -import os -import argparse - -def pause(time): - for _ in range(int(time*100)): - og.sim.render() - -def pause_step(time): - for _ in range(int(time*100)): - og.sim.step() - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "walls", "coffee_table"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - # def set_start_pose(): - # control_idx = np.concatenate([robot.trunk_control_idx]) - # robot.set_joint_positions([0.1], control_idx) - # og.sim.step() - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - - table = DatasetObject( - name="table", - category="breakfast_table", - model="rjgmmy", - scale = [0.3, 0.3, 0.3] - ) - og.sim.import_object(table) - table.set_position([-0.7, -2.0, 0.2]) - og.sim.step() - - - # pose_2d = [-1.0, 0.0, 0.1] - # pose = StarterSemanticActionPrimitives._get_robot_pose_from_2d_pose(pose_2d) - # pose = StarterSemanticActionPrimitives._get_robot_pose_from_2d_pose([-0.433881, -0.230183, -1.87]) - # robot.set_position_orientation(*pose) - set_start_pose() - pause_step(4) - - controller = StarterSemanticActionPrimitives(None, scene, robot) - - # for position in positions: - # with UndoableContext(controller.robot, controller.robot_copy, "original", True) as context: - # print(set_base_and_detect_collision(context,(position, [0, 0, 0, 1]))) - # print("--------------------") - - # pause(100) - - # joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) - # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) - - def get_random_joint_position(): - import random - joint_positions = [] - joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) - joints = np.array([joint for joint in robot.joints.values()]) - arm_joints = joints[joint_control_idx] - for i, joint in enumerate(arm_joints): - val = random.uniform(joint.lower_limit, joint.upper_limit) - joint_positions.append(val) - return joint_positions, joint_control_idx - - - joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) - joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) - initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) - control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] - - # joint_pos = initial_joint_pos - # joint_pos[control_idx_in_joint_pos] = [0.0133727 ,0.216775 ,0.683931 ,2.04371 ,1.88204 ,0.720747 ,1.23276 ,1.72251] - # from IPython import embed; embed() - - # def print_link(): - # for link in robot.links.values(): - # link_name = link.prim_path.split("/")[-1] - # for mesh_name, mesh in link.collision_meshes.items(): - # if link_name == "arm_right_1_link": - # pose = T.relative_pose_transform(*link.get_position_orientation(), *robot.get_position_orientation()) - # print(pose[0], T.quat2euler(pose[1])) - # print("-------") - while True: - joint_pos, joint_control_idx = get_random_joint_position() - robot.set_joint_positions(joint_pos, joint_control_idx) - pause_step(2) - # print_link() - # from IPython import embed; embed() - with UndoableContext(controller.robot, controller.robot_copy, "original") as context: - # from IPython import embed; embed() - initial_joint_pos[control_idx_in_joint_pos] = joint_pos - # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) - print(set_arm_and_detect_collision(context, initial_joint_pos)) - print("--------------------") - from IPython import embed; embed() - - # inter_path = [[0.09997262060642242, -1.0293538570404053, 1.4700201749801636, 2.709970235824585, 1.749015212059021, -1.5699917078018188, 1.3900072574615479, -4.500867362366989e-06], [0.11691628270503057, -0.8830267327942037, 1.3513760662081975, 2.544509606147747, 1.7484604307752005, -1.5653005250323326, 1.3170965590802435, -0.054800000852916544], [0.13385994480363872, -0.7366996085480022, 1.2327319574362314, 2.379048976470909, 1.74790564949138, -1.5606093422628464, 1.2441858606989389, -0.10959550083847072], [0.15080360690224687, -0.5903724843018008, 1.1140878486642654, 2.213588346794071, 1.7473508682075598, -1.55591815949336, 1.1712751623176345, -0.16439100082402489], [0.16774726900085501, -0.44404536005559925, 0.9954437398922993, 2.048127717117233, 1.7467960869237393, -1.5512269767238738, 1.09836446393633, -0.21918650080957908], [0.18469093109946316, -0.2977182358093978, 0.8767996311203332, 1.882667087440395, 1.7462413056399189, -1.5465357939543876, 1.0254537655550255, -0.27398200079513324], [0.2016345931980713, -0.1513911115631964, 0.7581555223483671, 1.717206457763557, 1.7456865243560984, -1.5418446111849013, 0.9525430671737212, -0.3287775007806874], [0.21857825529667946, -0.005063987316994867, 0.6395114135764011, 1.5517458280867191, 1.745131743072278, -1.5371534284154151, 0.8796323687924167, -0.38357300076624157], [0.2355219173952876, 0.14126313692920678, 0.520867304804435, 1.386285198409881, 1.7445769617884577, -1.5324622456459287, 0.8067216704111121, -0.4383685007517958], [0.25246557949389575, 0.2875902611754082, 0.4022231960324689, 1.220824568733043, 1.7440221805046372, -1.5277710628764425, 0.7338109720298077, -0.49316400073734995], [0.2694092415925039, 0.4339173854216096, 0.28357908726050285, 1.055363939056205, 1.7434673992208167, -1.5230798801069563, 0.6609002736485032, -0.5479595007229041], [0.2783637696421741, 0.47426669293427165, 0.31462729394691785, 1.116599611260824, 1.6978851495815748, -1.4143148289833873, 0.4303872927273669, -0.5602623084353068], [0.28731829769184425, 0.5146160004469337, 0.3456755006333329, 1.1778352834654429, 1.6523028999423328, -1.3055497778598182, 0.1998743118062306, -0.5725651161477093], [0.2962728257415144, 0.5549653079595958, 0.3767237073197479, 1.2390709556700619, 1.606720650303091, -1.196784726736249, -0.030638669114905648, -0.584867923860112], [0.3052273537911846, 0.5953146154722577, 0.4077719140061629, 1.3003066278746809, 1.5611384006638491, -1.08801967561268, -0.261151650036042, -0.5971707315725147], [0.3141818818408548, 0.6356639229849199, 0.438820120692578, 1.3615423000792997, 1.5155561510246072, -0.979254624489111, -0.4916646309571784, -0.6094735392849172], [0.323136409890525, 0.6760132304975819, 0.469868327378993, 1.4227779722839187, 1.4699739013853654, -0.870489573365542, -0.7221776118783145, -0.6217763469973199], [0.3320909379401952, 0.7163625380102439, 0.500916534065408, 1.4840136444885377, 1.4243916517461233, -0.761724522241973, -0.9526905927994511, -0.6340791547097225], [0.34104546598986535, 0.7567118455229059, 0.531964740751823, 1.5452493166931565, 1.3788094021068815, -0.6529594711184039, -1.1832035737205873, -0.6463819624221251], [0.3499999940395355, 0.797061153035568, 0.563012947438238, 1.6064849888977755, 1.3332271524676396, -0.5441944199948349, -1.4137165546417236, -0.6586847701345278]] - - # for p in inter_path: - # with UndoableContext(controller.robot, controller.robot_copy, "original") as context: - # # from IPython import embed; embed() - # initial_joint_pos[control_idx_in_joint_pos] = p - # # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) - # print(set_arm_and_detect_collision(context, initial_joint_pos)) - # print("--------------------") - # pause(0.1) - # from IPython import embed; embed() - - # move_path = [[-0.00017805075913202018, 3.202066363883205e-05, 0.0002361552458696181], [0.02343988658739001, -0.18208819000753182, -0.05332718383977012], [0.047057823933912044, -0.36420840067870247, -0.10689052292540986], [0.07067576128043407, -0.5463286113498731, -0.16045386201104958], [0.09429369862695611, -0.7284488220210438, -0.21401720109668934], [0.11791163597347813, -0.9105690326922143, -0.2675805401823291], [0.14152957332000016, -1.092689243363385, -0.3211438792679688], [0.1651475106665222, -1.2748094540345556, -0.37470721835360854], [0.18876544801304423, -1.4569296647057264, -0.4282705574392483], [0.21238338535956627, -1.6390498753768972, -0.481833896524888], [0.23600132270608828, -1.8211700860480675, -0.5353972356105278], [0.2596192600526103, -2.0032902967192383, -0.5889605746961675], [0.1313322452447971, -2.1147419038982056, -0.5567734369234738], [0.0030452304369839034, -2.2261935110771733, -0.5245862991507801], [-0.12524178437082933, -2.3376451182561406, -0.4923991613780865], [-0.2535287991786425, -2.4490967254351084, -0.4602120236053928], [-0.38181581398645575, -2.5605483326140757, -0.42802488583269915], [-0.6515587727066181, -2.54553348876709, -0.43195654044347825], [-0.9213017314267804, -2.5305186449201047, -0.4358881950542574], [-1.191044690146943, -2.515503801073119, -0.4398198496650365]] - # for p in move_path: - # with UndoableContext(controller.robot, controller.robot_copy, "original") as context: - # # from IPython import embed; embed() - # pose = [p[0], p[1], 0.0], T.euler2quat((0, 0, p[2])) - # print(set_base_and_detect_collision(context, pose)) - # print("--------------------") - # pause(0.1) - # from IPython import embed; embed() - - # from math import ceil - # ANGLE_DIFF = 0.3 - # DIST_DIFF = 0.1 - - # def checkMotion(context, start, goal): - # segment_theta = get_angle_between_poses(start, goal) - - # # from IPython import embed; embed() - - # # Start rotation - # is_valid_rotation(context, start, segment_theta) - # # Navigation - # dist = np.linalg.norm(goal[:2] - start[:2]) - # num_points = ceil(dist / DIST_DIFF) + 1 - # nav_x = np.linspace(start[0], goal[0], num_points).tolist() - # nav_y = np.linspace(start[1], goal[1], num_points).tolist() - # for i in range(num_points): - # pose = [nav_x[i], nav_y[i], 0.0], T.euler2quat((0, 0, segment_theta)) - # print(set_base_and_detect_collision(context, pose)) - # print("--------------------") - # pause(0.1) - # from IPython import embed; embed() - - # # Goal rotation - # is_valid_rotation(context, [goal[0], goal[1], segment_theta], goal[2]) - - # def is_valid_rotation(context, start_conf, final_orientation): - # diff = T.wrap_angle(final_orientation - start_conf[2]) - # direction = np.sign(diff) - # diff = abs(diff) - # num_points = ceil(diff / ANGLE_DIFF) + 1 - # nav_angle = np.linspace(0.0, diff, num_points) * direction - # angles = nav_angle + start_conf[2] - # for i in range(num_points): - # pose = [start_conf[0], start_conf[1], 0.0], T.euler2quat((0, 0, angles[i])) - # print(pose) - # print(set_base_and_detect_collision(context, pose)) - # print("--------------------") - # pause(0.1) - # from IPython import embed; embed() - - # def get_angle_between_poses(p1, p2): - # segment = [] - # segment.append(p2[0] - p1[0]) - # segment.append(p2[1] - p1[1]) - # return np.arctan2(segment[1], segment[0]) - - # move_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [-1.1167034268072273, -0.6603483567393302, 1.6380703271255967], [-1.4364447586070876, -2.1578042124791095, 1.031035481733344], [-0.494043711296063, -2.727928917138556, 0.36954198879575983]] - # interpolated_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [-0.139743706963583, -0.08251553245482543, 0.20496527921988983], [-0.27930938122696075, -0.16506307878118326, 0.40969457177784796], [-0.41887505549033854, -0.2476106251075411, 0.6144238643358061], [-0.5584407297537163, -0.3301581714338989, 0.8191531568937642], [-0.698006404017094, -0.4127057177602568, 1.0238824494517225], [-0.8375720782804719, -0.4952532640866146, 1.2286117420096805], [-0.9771377525438496, -0.5778008104129724, 1.4333410345676385], [-1.1167034268072273, -0.6603483567393302, 1.6380703271255967], [-1.162380759921493, -0.8742706218450129, 1.551351063498132], [-1.208058093035759, -1.0881928869506958, 1.4646317998706675], [-1.2537354261500246, -1.3021151520563783, 1.3779125362432028], [-1.2994127592642903, -1.5160374171620612, 1.291193272615738], [-1.3450900923785563, -1.729959682267744, 1.2044740089882735], [-1.390767425492822, -1.9438819473734266, 1.1177547453608088], [-1.4364447586070876, -2.1578042124791095, 1.031035481733344], [-1.2008444967793315, -2.3003353886439712, 0.865662108498948], [-0.9652442349515753, -2.442866564808833, 0.7002887352645519], [-0.7296439731238191, -2.5853977409736943, 0.5349153620301559], [-0.494043711296063, -2.727928917138556, 0.36954198879575983]] - # cleaned_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], (-1.1167034268072273, -0.6603483567393302, -2.6074760565062323), (-1.4364447586070876, -2.1578042124791095, -1.781160358496523), [-0.494043711296063, -2.727928917138556, 0.36954198879575983]] - - # move_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [0.08206962226267768, -1.7229286791889955, -1.9627716115738194], [-0.20405345335578362, -2.566953738240694, 2.888701559348255], [-1.1347811542998147, -2.657722324995175, -0.19615768830098013]] - # interpolated_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], [0.010102924170155141, -0.2153380727610336, -0.2451399631175372], [0.020383881040515504, -0.4307081593935996, -0.4905159128970061], [0.030664837910875868, -0.6460782460261656, -0.735891862676475], [0.04094579478123623, -0.8614483326587316, -0.9812678124559439], [0.05122675165159659, -1.0768184192912975, -1.2266437622354127], [0.06150770852195696, -1.2921885059238636, -1.4720197120148817], [0.07178866539231732, -1.5075585925564294, -1.7173956617943504], [0.08206962226267768, -1.7229286791889955, -1.9627716115738194], [0.024845007138985423, -1.8917336909993352, -2.2491140388253217], [-0.032379607984706835, -2.060538702809675, -2.535456466076824], [-0.08960422310839909, -2.229343714620015, -2.8217988933283267], [-0.14682883823209136, -2.3981487264303545, -3.108141320579829], [-0.20405345335578362, -2.566953738240694, 2.888701559348255], [-0.35917473684645546, -2.5820818360331077, 2.3745583514067157], [-0.5142960203371273, -2.5972099338255212, 1.8604151434651768], [-0.6694173038277992, -2.6123380316179343, 1.3462719355236374], [-0.824538587318471, -2.627466129410348, 0.8321287275820985], [-0.979659870809143, -2.6425942272027614, 0.31798551964055877], [-1.1347811542998147, -2.657722324995175, -0.19615768830098013]] - # cleaned_path = [[-0.00017803270020522177, 3.201387153239921e-05, 0.00023598666193169748], (0.08206962226267768, -1.7229286791889955, -1.5230963028314004), (-0.20405345335578362, -2.566953738240694, -1.8976366735518704), [-1.1347811542998147, -2.657722324995175, -0.19615768830098013]] - # for i in range(len(move_path) - 1): - # with UndoableContext(controller.robot, controller.robot_copy, "simplified") as context: - # # print(move_path[i]) - # # print(move_path[i+1]) - # # print("start and finish") - # checkMotion(context, np.array(move_path[i]), np.array(move_path[i+1])) - - # with UndoableContext(controller.robot, controller.robot_copy, "original") as context: - # # pose = ([-0.15, -2.0, 0.0], T.euler2quat((0, 0, 0.0))) - # pose = ([-0.0771680802758046, -2.1166874751986207, 0.0], [0, 0, 0.93719889, 0.3487954]) - # print(set_base_and_detect_collision(context, pose)) - # print("--------------------") - # pause(0.1) - # from IPython import embed; embed() - - pause(1) - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Run test script") - parser.add_argument( - "--profile", - action="store_true", - help="If set, profile code and generate .prof file", - ) - args = parser.parse_args() - if args.profile: - pr = cProfile.Profile() - pr.enable() - main() - pr.disable() - results = pstats.Stats(pr) - filename = f'profile-{os.path.basename(__file__)}-{time.strftime("%Y%m%d-%H%M%S")}' - results.dump_stats(f"./profiles/{filename}.prof") - os.system(f"flameprof ./profiles/{filename}.prof > ./profiles/{filename}.svg") - # Run `snakeviz ./profiles/.prof` to visualize stack trace or open .svg in a browser - else: - main() \ No newline at end of file diff --git a/dev_environment/test_grasp.py b/dev_environment/test_grasp.py deleted file mode 100644 index 6a809e238..000000000 --- a/dev_environment/test_grasp.py +++ /dev/null @@ -1,266 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject -from omnigibson.utils.grasping_planning_utils import get_grasp_position_for_open - -import cProfile, pstats, io -import time -import os -import argparse - - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - actions = [] - for action in ctrl_gen: - env.step(action) - actions.append(action.tolist()) - if filename is not None: - with open(filename, "w") as f: - yaml.dump(actions, f) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "bottom_cabinet"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - controller = StarterSemanticActionPrimitives(None, scene, robot) - - for o in scene.objects: - if o.prim_path == "/World/bottom_cabinet_bamfsz_0": - cabinet = o - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - def test_open_no_navigation(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - set_start_pose() - pose = controller._get_robot_pose_from_2d_pose([-1.0, -0.5, np.pi/2]) - robot.set_position_orientation(*pose) - og.sim.step() - get_grasp_position_for_open(robot, cabinet, True) - # execute_controller(controller._open_or_close(cabinet), env) - - def test_open(): - set_start_pose() - pose_2d = [-0.762831, -0.377231, 2.72892] - pose = controller._get_robot_pose_from_2d_pose(pose_2d) - robot.set_position_orientation(*pose) - og.sim.step() - - # joint_pos = [0.0133727 ,0.216775 ,0.683931 ,2.04371 ,1.88204 ,0.720747 ,1.23276 ,1.72251] - # control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) - # robot.set_joint_positions(joint_pos, control_idx) - # og.sim.step() - # pause(100) - execute_controller(controller._open_or_close(cabinet, True), env) - # replay_controller(env, "./replays/test_tiago_open.yaml") - - test_open() - - marker = None - def set_marker(position): - marker = PrimitiveObject( - prim_path=f"/World/marker", - name="marker", - primitive_type="Cube", - size=0.07, - visual_only=True, - rgba=[1.0, 0, 0, 1.0], - ) - og.sim.import_object(marker) - marker.set_position(position) - og.sim.step() - - def remove_marker(): - marker.remove() - og.sim.step() - - def get_closest_point_to_point_in_world_frame(vectors_in_arbitrary_frame, arbitrary_frame_to_world_frame, point_in_world): - vectors_in_world = np.array( - [ - T.pose_transform(*arbitrary_frame_to_world_frame, vector, [0, 0, 0, 1])[0] - for vector in vectors_in_arbitrary_frame - ] - ) - - vector_distances_to_point = np.linalg.norm(vectors_in_world - np.array(point_in_world)[None, :], axis=1) - closer_option_idx = np.argmin(vector_distances_to_point) - vector_in_arbitrary_frame = vectors_in_arbitrary_frame[closer_option_idx] - vector_in_world_frame = vectors_in_world[closer_option_idx] - - return closer_option_idx, vector_in_arbitrary_frame, vector_in_world_frame - - ######################################################## - - # from omnigibson.objects.primitive_object import PrimitiveObject - # import numpy as np - # import random - # from scipy.spatial.transform import Rotation - - # import omnigibson.utils.transform_utils as T - # from omnigibson.object_states.open import _get_relevant_joints - # from omnigibson.utils.constants import JointType, JointAxis - # from omni.isaac.core.utils.rotations import gf_quat_to_np_array - # from scipy.spatial.transform import Rotation as R - - # p = None - # grasp_position_for_open_on_revolute_joint = None - # should_open = True - # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS = (0.2, 0.8) - # GRASP_OFFSET = np.array([0, 0.05, -0.08]) - # OPEN_GRASP_OFFSET = np.array([0, 0.05, -0.12]) # 5cm back and 12cm up. - - # # Pick a moving link of the object. - # relevant_joints_full = _get_relevant_joints(cabinet) - # relevant_joints = relevant_joints_full[1] - - # # If a particular link ID was specified, filter our candidates down to that one. - # # if link_id is not None: - # # relevant_joints = [ji for ji in relevant_joints if ji.jointIndex == link_id] - - # if len(relevant_joints) == 0: - # raise ValueError("Cannot open/close object without relevant joints.") - - # # Make sure what we got is an appropriately open/close joint. - # random.shuffle(relevant_joints) - # selected_joint = None - # for joint in relevant_joints: - # current_position = joint.get_state()[0][0] - # joint_range = joint.upper_limit - joint.lower_limit - # openness_fraction = (current_position - joint.lower_limit) / joint_range - # if (should_open and openness_fraction < 0.8) or (not should_open and openness_fraction > 0.05): - # selected_joint = joint - - - # target_obj = cabinet - # relevant_joint = relevant_joints_full[1][2] - - # link_name = relevant_joint.body1.split("/")[-1] - - # # Get the bounding box of the child link. - # ( - # bbox_center_in_world, - # bbox_quat_in_world, - # bbox_extent_in_link_frame, - # _, - # ) = target_obj.get_base_aligned_bbox(link_name=link_name, visual=False) - # from IPython import embed; embed() - # # Match the push axis to one of the bb axes. - # push_axis_idx = JointAxis.index(relevant_joint.axis) - # canonical_push_axis = np.eye(3)[push_axis_idx] - # joint_orientation = gf_quat_to_np_array(relevant_joint.get_attribute("physics:localRot0"))[[1, 2, 3, 0]] - # push_axis = R.from_quat(joint_orientation).apply([1, 0, 0]) - # assert np.isclose(np.max(np.abs(push_axis)), 1.0) # Make sure we're aligned with a bb axis. - # push_axis_idx = np.argmax(np.abs(push_axis)) - # canonical_push_axis = np.eye(3)[push_axis_idx] - - - # # TODO: Need to figure out how to get the correct push direction. - # canonical_push_direction = canonical_push_axis * np.sign(push_axis[push_axis_idx]) - - # # Pick the closer of the two faces along the push axis as our favorite. - # points_along_push_axis = ( - # np.array([canonical_push_axis, -canonical_push_axis]) * bbox_extent_in_link_frame[push_axis_idx] / 2 - # ) - # ( - # push_axis_closer_side_idx, - # center_of_selected_surface_along_push_axis, - # _, - # ) = get_closest_point_to_point_in_world_frame( - # points_along_push_axis, (bbox_center_in_world, bbox_quat_in_world), robot.get_position() - # ) - # push_axis_closer_side_sign = 1 if push_axis_closer_side_idx == 0 else -1 - - # # Pick the other axes. - # all_axes = list(set(range(3)) - {push_axis_idx}) - # x_axis_idx, y_axis_idx = tuple(sorted(all_axes)) - # canonical_x_axis = np.eye(3)[x_axis_idx] - # canonical_y_axis = np.eye(3)[y_axis_idx] - - # # Find the correct side of the lateral axis & go some distance along that direction. - # min_lateral_pos_wrt_surface_center = (canonical_x_axis + canonical_y_axis) * -bbox_extent_in_link_frame / 2 - # max_lateral_pos_wrt_surface_center = (canonical_x_axis + canonical_y_axis) * bbox_extent_in_link_frame / 2 - # diff_lateral_pos_wrt_surface_center = max_lateral_pos_wrt_surface_center - min_lateral_pos_wrt_surface_center - # sampled_lateral_pos_wrt_min = np.random.uniform( - # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[0] * diff_lateral_pos_wrt_surface_center, - # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[1] * diff_lateral_pos_wrt_surface_center, - # ) - # lateral_pos_wrt_surface_center = min_lateral_pos_wrt_surface_center + sampled_lateral_pos_wrt_min - # grasp_position_in_bbox_frame = center_of_selected_surface_along_push_axis + lateral_pos_wrt_surface_center - # grasp_quat_in_bbox_frame = T.quat_inverse(joint_orientation) - - # # Now apply the grasp offset. - # offset_grasp_pose_in_bbox_frame = (grasp_position_in_bbox_frame, grasp_quat_in_bbox_frame) - # offset_grasp_pose_in_world_frame = T.pose_transform( - # bbox_center_in_world, bbox_quat_in_world, *offset_grasp_pose_in_bbox_frame - # ) - - # # To compute the rotation position, we want to decide how far along the rotation axis we'll go. - # target_joint_pos = relevant_joint.upper_limit if should_open else relevant_joint.lower_limit - # current_joint_pos = relevant_joint.get_state()[0][0] - - # required_pos_change = target_joint_pos - current_joint_pos - # push_vector_in_bbox_frame = canonical_push_direction * required_pos_change - # target_hand_pos_in_bbox_frame = grasp_position_in_bbox_frame + push_vector_in_bbox_frame - # target_hand_pos_in_world_frame = T.pose_transform( - # bbox_center_in_world, bbox_quat_in_world, target_hand_pos_in_bbox_frame, grasp_quat_in_bbox_frame - # ) - - # # Compute the approach direction. - # approach_direction_in_world_frame = R.from_quat(bbox_quat_in_world).apply(canonical_push_axis * -push_axis_closer_side_sign) - - # # Decide whether a grasp is required. If approach direction and displacement are similar, no need to grasp. - # grasp_required = np.dot(push_vector_in_bbox_frame, canonical_push_axis * -push_axis_closer_side_sign) < 0 - - # # return ( - # # offset_grasp_pose_in_world_frame, - # # [target_hand_pos_in_world_frame], - # # approach_direction_in_world_frame, - # # relevant_joint, - # # grasp_required, - # # ) - # from IPython import embed; embed() - - -if __name__ == "__main__": - main() - - - diff --git a/dev_environment/test_interpolation.py b/dev_environment/test_interpolation.py deleted file mode 100644 index f722f0277..000000000 --- a/dev_environment/test_interpolation.py +++ /dev/null @@ -1,80 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject - -import argparse - - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - actions = [] - for action in ctrl_gen: - env.step(action) - actions.append(action.tolist()) - if filename is not None: - with open(filename, "w") as f: - yaml.dump(actions, f) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - grasp_obj = DatasetObject( - name="cologne", - category="cologne", - model="lyipur", - scale=[0.01, 0.01, 0.01] - ) - og.sim.import_object(grasp_obj) - grasp_obj.set_position([-0.3, -0.8, 0.5]) - og.sim.step() - - pause(2) - controller = StarterSemanticActionPrimitives(None, scene, robot) - - def set_start_pose(): - control_idx = np.concatenate([robot.trunk_control_idx]) - robot.set_joint_positions([0.1], control_idx) - og.sim.step() - - def test_grasp_no_navigation(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - set_start_pose() - pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.230183, -1.87]) - robot.set_position_orientation(*pose) - og.sim.step() - execute_controller(controller._grasp(grasp_obj), env) - - test_grasp_no_navigation() - -if __name__ == "__main__": - main() - - - diff --git a/dev_environment/test_open_revolute.py b/dev_environment/test_open_revolute.py deleted file mode 100644 index 55bed30f5..000000000 --- a/dev_environment/test_open_revolute.py +++ /dev/null @@ -1,377 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject -from omnigibson.utils.grasping_planning_utils import get_grasp_position_for_open - -import cProfile, pstats, io -import time -import os -import argparse - -gm.DEBUG = True - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - actions = [] - for action in ctrl_gen: - env.step(action) - actions.append(action.tolist()) - if filename is not None: - with open(filename, "w") as f: - yaml.dump(actions, f) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - controller = StarterSemanticActionPrimitives(None, scene, robot) - - # open_obj = DatasetObject( - # name="fridge", - # category="fridge", - # model="dszchb", - # scale=[0.7, 0.7, 0.7] - # ) - - open_obj = DatasetObject( - name="bottom_cabinet", - category="bottom_cabinet", - model="bamfsz", - scale=[0.7, 0.7, 0.7] - ) - - og.sim.import_object(open_obj) - # open_obj.set_position([-1.2, -0.4, 0.5]) - open_obj.set_position_orientation([-1.2, -0.4, 0.5], T.euler2quat([0, 0, np.pi/2])) - og.sim.step() - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - def test_open_no_navigation(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - set_start_pose() - pose = controller._get_robot_pose_from_2d_pose([-1.0, -0.5, np.pi/2]) - robot.set_position_orientation(*pose) - og.sim.step() - get_grasp_position_for_open(robot, open_obj, True) - # execute_controller(controller._open_or_close(cabinet), env) - - def test_open(): - set_start_pose() - # pose_2d = [-0.231071, -0.272773, 2.55196] - - # # pose_2d = [-0.282843, 0.297682, -3.07804] - # pose = controller._get_robot_pose_from_2d_pose(pose_2d) - # robot.set_position_orientation(*pose) - # og.sim.step() - - # joint_pos = [0.0133727 ,0.216775 ,0.683931 ,2.04371 ,1.88204 ,0.720747 ,1.23276 ,1.72251] - # control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) - # robot.set_joint_positions(joint_pos, control_idx) - # og.sim.step() - # pause(100) - # execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.OPEN, *(open_obj,)), env) - execute_controller(controller._open_or_close(open_obj, False), env) - - markers = [] - for i in range(20): - marker = PrimitiveObject( - prim_path=f"/World/test_{i}", - name=f"test_{i}", - primitive_type="Cube", - size=0.07, - visual_only=True, - rgba=[1.0, 0, 0, 1.0]) - markers.append(marker) - og.sim.import_object(marker) - - # from omnigibson.object_states.open import _get_relevant_joints - controller.markers = markers - # j = _get_relevant_joints(open_obj)[1][0] - # j.set_pos(0.5) - # pause(2) - open_obj.joints["j_link_2"].set_pos(0.4) - og.sim.step() - test_open() - pause(5) - - # markers = [] - # for i in range(20): - # marker = PrimitiveObject( - # prim_path=f"/World/test_{i}", - # name=f"test_{i}", - # primitive_type="Cube", - # size=0.07, - # visual_only=True, - # rgba=[1.0, 0, 0, 1.0]) - # markers.append(marker) - # og.sim.import_object(marker) - - def set_marker(position, idx): - markers[idx].set_position(position) - og.sim.step() - - def get_closest_point_to_point_in_world_frame(vectors_in_arbitrary_frame, arbitrary_frame_to_world_frame, point_in_world): - vectors_in_world = np.array( - [ - T.pose_transform(*arbitrary_frame_to_world_frame, vector, [0, 0, 0, 1])[0] - for vector in vectors_in_arbitrary_frame - ] - ) - - vector_distances_to_point = np.linalg.norm(vectors_in_world - np.array(point_in_world)[None, :], axis=1) - closer_option_idx = np.argmin(vector_distances_to_point) - vector_in_arbitrary_frame = vectors_in_arbitrary_frame[closer_option_idx] - vector_in_world_frame = vectors_in_world[closer_option_idx] - - return closer_option_idx, vector_in_arbitrary_frame, vector_in_world_frame - - def get_quaternion_between_vectors(v1, v2): - """ - Get the quaternion between two vectors. - - Args: - v1: The first vector. - v2: The second vector. - - Returns: - The quaternion between the two vectors. - """ - q = np.cross(v1, v2).tolist() - w = np.sqrt((np.linalg.norm(v1) ** 2) * (np.linalg.norm(v2) ** 2)) + np.dot(v1, v2) - # np.sqrt((np.linalg.norm(a) ^ 2) * (np.linalg.norm(b) ** 2)) + np.dot(a, b) - q.append(w) - q = np.array(q) / np.linalg.norm(q) - return q - - def rotate_point_around_axis(point_wrt_arbitrary_frame, arbitrary_frame_wrt_origin, joint_axis, yaw_change): - # grasp_pose_in_bbox_frame, bbox_wrt_origin, joint_axis, partial_yaw_change - # rotation_to_joint = get_quaternion_between_vectors([1, 0, 0], joint_axis) - # rotation = T.quat_multiply(rotation_to_joint, T.euler2quat([yaw_change, 0, 0])) - # rotation = T.quat_multiply(rotation, T.quat_inverse(rotation_to_joint)) - rotation = R.from_rotvec(joint_axis * yaw_change).as_quat() - origin_wrt_arbitrary_frame = T.invert_pose_transform(*arbitrary_frame_wrt_origin) - - pose_in_origin_frame = T.pose_transform(*arbitrary_frame_wrt_origin, *point_wrt_arbitrary_frame) - rotated_pose_in_origin_frame = T.pose_transform([0, 0, 0], rotation, *pose_in_origin_frame) - rotated_pose_in_arbitrary_frame = T.pose_transform(*origin_wrt_arbitrary_frame, *rotated_pose_in_origin_frame) - return rotated_pose_in_arbitrary_frame - - def get_orientation_facing_vector_with_random_yaw(vector): - forward = vector / np.linalg.norm(vector) - rand_vec = np.random.rand(3) - rand_vec /= np.linalg.norm(3) - side = np.cross(rand_vec, forward) - side /= np.linalg.norm(3) - up = np.cross(forward, side) - assert np.isclose(np.linalg.norm(up), 1) - rotmat = np.array([forward, side, up]).T - return R.from_matrix(rotmat).as_quat() - - ######################################################## - - # # from omnigibson.objects.primitive_object import PrimitiveObject - # # import numpy as np - # import random - # from scipy.spatial.transform import Rotation - - # # import omnigibson.utils.transform_utils as T - # from omnigibson.object_states.open import _get_relevant_joints - # from omnigibson.utils.constants import JointType, JointAxis - # from omni.isaac.core.utils.rotations import gf_quat_to_np_array - # from scipy.spatial.transform import Rotation as R - # from math import ceil - - # p = None - # grasp_position_for_open_on_revolute_joint = None - # should_open = True - # PRISMATIC_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS = (0.2, 0.8) - # GRASP_OFFSET = np.array([0, 0.05, -0.08]) - # OPEN_GRASP_OFFSET = np.array([0, 0.05, -0.12]) # 5cm back and 12cm up. - # ROTATION_ARC_SEGMENT_LENGTHS = 0.1 - - # # Pick a moving link of the object. - # relevant_joints_full = _get_relevant_joints(open_obj) - # relevant_joints = relevant_joints_full[1] - - # if len(relevant_joints) == 0: - # raise ValueError("Cannot open/close object without relevant joints.") - - # # Make sure what we got is an appropriately open/close joint. - # random.shuffle(relevant_joints) - # selected_joint = None - # for joint in relevant_joints: - # current_position = joint.get_state()[0][0] - # joint_range = joint.upper_limit - joint.lower_limit - # openness_fraction = (current_position - joint.lower_limit) / joint_range - # if (should_open and openness_fraction < 0.8) or (not should_open and openness_fraction > 0.05): - # selected_joint = joint - - # robot = robot - # target_obj = open_obj - # relevant_joint = selected_joint - # should_open = True - # REVOLUTE_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS = (0.4, 0.6) - - # link_name = relevant_joint.body1.split("/")[-1] - # link = target_obj.links[link_name] - - # # Get the bounding box of the child link. - # ( - # bbox_center_in_world, - # bbox_quat_in_world, - # bbox_extent_in_link_frame, - # bbox_center_in_obj_frame - # ) = target_obj.get_base_aligned_bbox(link_name=link_name, visual=False) - - # # Get the part of the object away from the joint position/axis. - # # The link origin is where the joint is. Let's get the position of the origin w.r.t the CoM. - # # from IPython import embed; embed() - # # [target_bid] = target_obj.get_body_ids() - # # dynamics_info = p.getDynamicsInfo(target_bid, link_id) - # # com_wrt_origin = (dynamics_info[3], dynamics_info[4]) - - # # bbox_wrt_origin = T.pose_transform(link.get_position(), [0, 0, 0, 1], bbox_center_in_link_frame, [0, 0, 0, 1]) - # bbox_center_in_world_frame = T.pose_transform(*target_obj.get_position_orientation(), bbox_center_in_obj_frame, [0, 0, 0, 1])[0] - # bbox_wrt_origin = T.relative_pose_transform(bbox_center_in_world_frame, bbox_quat_in_world, *link.get_position_orientation()) - # origin_wrt_bbox = T.invert_pose_transform(*bbox_wrt_origin) - # # from IPython import embed; embed() - - # joint_orientation = gf_quat_to_np_array(relevant_joint.get_attribute("physics:localRot0"))[[1, 2, 3, 0]] - # joint_axis = R.from_quat(joint_orientation).apply([1, 0, 0]) - # joint_axis /= np.linalg.norm(joint_axis) - # origin_towards_bbox = np.array(bbox_wrt_origin[0]) - # open_direction = np.cross(joint_axis, origin_towards_bbox) - # open_direction /= np.linalg.norm(open_direction) - # lateral_axis = np.cross(open_direction, joint_axis) - - # # Match the axes to the canonical axes of the link bb. - # lateral_axis_idx = np.argmax(np.abs(lateral_axis)) - # open_axis_idx = np.argmax(np.abs(open_direction)) - # joint_axis_idx = np.argmax(np.abs(joint_axis)) - # assert lateral_axis_idx != open_axis_idx - # assert lateral_axis_idx != joint_axis_idx - # assert open_axis_idx != joint_axis_idx - - # # Find the correct side of the push/pull axis to grasp from. To do this, imagine the closed position of the object. - # # In that position, which side is the robot on? - # canonical_open_direction = np.eye(3)[open_axis_idx] - # points_along_open_axis = ( - # np.array([canonical_open_direction, -canonical_open_direction]) * bbox_extent_in_link_frame[open_axis_idx] / 2 - # ) - # current_yaw = relevant_joint.get_state()[0][0] - # closed_yaw = relevant_joint.lower_limit - # points_along_open_axis_after_rotation = [ - # rotate_point_around_axis((point, [0, 0, 0, 1]), bbox_wrt_origin, joint_axis, closed_yaw - current_yaw)[0] - # for point in points_along_open_axis - # ] - # open_axis_closer_side_idx, _, _ = get_closest_point_to_point_in_world_frame( - # points_along_open_axis_after_rotation, (bbox_center_in_world, bbox_quat_in_world), robot.get_position() - # ) - # open_axis_closer_side_sign = 1 if open_axis_closer_side_idx == 0 else -1 - # center_of_selected_surface_along_push_axis = points_along_open_axis[open_axis_closer_side_idx] - - # # Find the correct side of the lateral axis & go some distance along that direction. - # canonical_joint_axis = np.eye(3)[joint_axis_idx] - # lateral_away_from_origin = np.eye(3)[lateral_axis_idx] * np.sign(origin_towards_bbox[lateral_axis_idx]) - # min_lateral_pos_wrt_surface_center = ( - # lateral_away_from_origin * -np.array(origin_wrt_bbox[0]) - # - canonical_joint_axis * bbox_extent_in_link_frame[lateral_axis_idx] / 2 - # ) - # max_lateral_pos_wrt_surface_center = ( - # lateral_away_from_origin * bbox_extent_in_link_frame[lateral_axis_idx] / 2 - # + canonical_joint_axis * bbox_extent_in_link_frame[lateral_axis_idx] / 2 - # ) - # diff_lateral_pos_wrt_surface_center = max_lateral_pos_wrt_surface_center - min_lateral_pos_wrt_surface_center - # sampled_lateral_pos_wrt_min = np.random.uniform( - # REVOLUTE_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[0] * diff_lateral_pos_wrt_surface_center, - # REVOLUTE_JOINT_FRACTION_ACROSS_SURFACE_AXIS_BOUNDS[1] * diff_lateral_pos_wrt_surface_center, - # ) - # lateral_pos_wrt_surface_center = min_lateral_pos_wrt_surface_center + sampled_lateral_pos_wrt_min - # grasp_position = center_of_selected_surface_along_push_axis + lateral_pos_wrt_surface_center - # # Get the appropriate rotation - - # grasp_quat_in_bbox_frame = get_quaternion_between_vectors([1, 0, 0], canonical_open_direction * open_axis_closer_side_sign * -1) - - # # grasp_quat_in_bbox_frame = get_orientation_facing_vector_with_random_yaw(canonical_open_direction * open_axis_closer_side_sign * -1) - # # Now apply the grasp offset. - # offset_in_bbox_frame = canonical_open_direction * open_axis_closer_side_sign * 0.2 - # offset_grasp_pose_in_bbox_frame = (grasp_position + offset_in_bbox_frame, grasp_quat_in_bbox_frame) - # offset_grasp_pose_in_world_frame = T.pose_transform( - # bbox_center_in_world, bbox_quat_in_world, *offset_grasp_pose_in_bbox_frame - # ) - # grasp_pose_in_world_frame = T.pose_transform(bbox_center_in_world, bbox_quat_in_world, grasp_position, grasp_quat_in_bbox_frame) - - # # To compute the rotation position, we want to decide how far along the rotation axis we'll go. - # desired_yaw = relevant_joint.upper_limit if should_open else relevant_joint.lower_limit - # required_yaw_change = desired_yaw - current_yaw - - # # Now we'll rotate the grasp position around the origin by the desired rotation. - # # Note that we use the non-offset position here since the joint can't be pulled all the way to the offset. - # grasp_pose_in_bbox_frame = grasp_position, grasp_quat_in_bbox_frame - # grasp_pose_in_origin_frame = T.pose_transform(*bbox_wrt_origin, *grasp_pose_in_bbox_frame) - - # # Get the arc length and divide it up to 10cm segments - # arc_length = abs(required_yaw_change) * np.linalg.norm(grasp_pose_in_origin_frame[0]) - # turn_steps = int(ceil(arc_length / ROTATION_ARC_SEGMENT_LENGTHS)) - # targets = [] - # for i in range(turn_steps): - # partial_yaw_change = (i + 1) / turn_steps * required_yaw_change - # rotated_grasp_pose_in_bbox_frame = rotate_point_around_axis( - # grasp_pose_in_bbox_frame, bbox_wrt_origin, joint_axis, partial_yaw_change - # ) - # rotated_grasp_pose_in_world_frame = T.pose_transform( - # bbox_center_in_world, bbox_quat_in_world, *rotated_grasp_pose_in_bbox_frame - # ) - # targets.append(rotated_grasp_pose_in_world_frame) - - # # Compute the approach direction. - # approach_direction_in_world_frame = Rotation.from_quat(bbox_quat_in_world).apply(canonical_open_direction * -open_axis_closer_side_sign) - - # # Decide whether a grasp is required. If approach direction and displacement are similar, no need to grasp. - # movement_in_world_frame = np.array(targets[-1][0]) - np.array(offset_grasp_pose_in_world_frame[0]) - # grasp_required = np.dot(movement_in_world_frame, approach_direction_in_world_frame) < 0 - - # from IPython import embed; embed() -if __name__ == "__main__": - main() - - - diff --git a/dev_environment/test_tiago.py b/dev_environment/test_tiago.py deleted file mode 100644 index 25632aafe..000000000 --- a/dev_environment/test_tiago.py +++ /dev/null @@ -1,285 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject - -import cProfile, pstats, io -import time -import os -import argparse - - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - actions = [] - for action in ctrl_gen: - env.step(action) - # actions.append(action.tolist()) - # if filename is not None: - # with open(filename, "w") as f: - # yaml.dump(actions, f) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - table = DatasetObject( - name="table", - category="breakfast_table", - model="rjgmmy", - scale = [0.3, 0.3, 0.3] - ) - og.sim.import_object(table) - # table.set_position([-0.7, -2.0, 0.2]) - table.set_position([-0.7, 0.5, 0.2]) - og.sim.step() - - grasp_obj = DatasetObject( - name="cologne", - category="bottle_of_cologne", - model="lyipur" - ) - og.sim.import_object(grasp_obj) - grasp_obj.set_position([-0.3, -0.8, 0.5]) - og.sim.step() - - # marker = PrimitiveObject( - # prim_path=f"/World/marker", - # name="marker", - # primitive_type="Cube", - # size=0.07, - # visual_only=True, - # rgba=[1.0, 0, 0, 1.0], - # ) - # og.sim.import_object(marker) - # marker.set_position_orientation([-0.29840604, -0.79821703, 0.59273211], [0. , 0.70710678, 0. , 0.70710678]) - # og.sim.step() - - - # robot.set_position([-2.0, 0.0, 0.0]) - # pause(2) - controller = StarterSemanticActionPrimitives(None, scene, robot) - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - def test_navigate_to_obj(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # execute_controller(controller._reset_hand(), env) - set_start_pose() - execute_controller(controller._navigate_to_obj(table), env) - - def test_grasp_no_navigation(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - set_start_pose() - pose = controller._get_robot_pose_from_2d_pose([-0.433881, -0.210183, -2.96118]) - robot.set_position_orientation(*pose) - og.sim.step() - # replay_controller(env, "./replays/test_grasp_pose.yaml") - execute_controller(controller._grasp(grasp_obj), env) - - def test_grasp(): - # Need to set start pose to reset_hand because default tuck pose for Tiago collides with itself - # execute_controller(controller._reset_hand(), env) - set_start_pose() - # pause(2) - execute_controller(controller._grasp(grasp_obj), env) - - def test_place(): - test_grasp() - pause(1) - execute_controller(controller._place_on_top(table), env) - - # positions = [[0.09988395869731903, -1.0999969244003296, 1.4699827432632446, 2.710009813308716, 1.710004448890686, -1.5700018405914307, 1.3899955749511719, 2.001982011279324e-07], [0.15954897383415584, -0.9759483151785584, 1.051426922254121, 1.3919954813427862, 1.9255247232751793, -0.46858315638703396, 1.135518807525537, 0.5174528326963662], [0.15062408833826937, -0.4437143998615267, 0.8304433521196042, 1.437534104367112, 1.6164805582338932, -0.37533100951328124, 0.6381778036539293, -0.0283867578914061], [0.13373369762078724, 0.5635409634642365, 0.41223083348993295, 1.5237161776241306, 1.0316129205581803, -0.1988508522420569, -0.30304254915485307, -1.0613909301407032], [0.11684330690330513, 1.57079632679, -0.005981685139738268, 1.6098982508811492, 0.4467452828824673, -0.022370694970832543, -1.244262901963635, -2.09439510239]] - control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) - # robot.set_position_orientation([-1.08215380e+00, -3.35281938e-01, -2.77837131e-07], [ 1.78991655e-07, -4.65450078e-08, -2.67762393e-01, 9.63485003e-01]) - # og.sim.step() - # for p in positions: - # robot.set_joint_positions(p, control_idx) - # pause(0.2) - # pause(100) - # Work more reliably - # og.sim._physics_context.set_gravity(value=-0.1) - # execute_controller(controller._reset_hand(), env) - # robot.set_position_orientation([-1.08215380e+00, -3.35281938e-01, -2.77837131e-07], [ 1.78991655e-07, -4.65450078e-08, -2.67762393e-01, 9.63485003e-01]) - # og.sim.step() - # execute_controller(controller._reset_hand(), env) - # grasp_pose = [-0.29840604, -0.79821703, 0.59273211], [0. , 0.70710678, 0. , 0.70710678] - # execute_controller(controller._reset_hand(), env) - # execute_controller(controller._move_hand(grasp_pose), env) - # test_grasp_no_navigation() - - # end_joint = [0.11684331, 1.57079633, -0.00598169, 1.60989825, 0.44674528, -0.02237069, -1.2442629, -2.0943951] - # robot.set_joint_positions(end_joint, control_idx) - # pause(1) - # from IPython import embed; embed() - # og.sim.step() - # Don't work as reliably - - # pose_2d = [-0.543999, -0.233287,-1.16071] - # pos = np.array([pose_2d[0], pose_2d[1], 0.05]) - # orn = T.euler2quat([0, 0, pose_2d[2]]) - # robot.set_position_orientation(pos, orn) - # og.sim.step() - # pause(10) - # t_pose = ([-0.29840604, -0.79821703, 0.59273211], [0. , 0.70710678, 0. , 0.70710678]) - # execute_controller(controller._reset_hand(), env) - # execute_controller(controller._move_hand(t_pose), env) - - # try: - # test_grasp_no_navigation() - # except: - # pass - - - test_grasp_no_navigation() - # test_grasp() - # test_navigate_to_obj() - # set_start_pose() - # execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, grasp_obj), env) - # execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.PLACE_ON_TOP, table), env) - # test_place() - pause(5) - - # test_grasp_no_navigation() - # set_start_pose() - # pose_2d = [-0.15, -0.269397, -2.0789] - # pos = np.array([pose_2d[0], pose_2d[1], 0.05]) - # orn = T.euler2quat([0, 0, pose_2d[2]]) - # robot.set_position_orientation(pos, orn) - # pause(2) - - # with UndoableContext(robot, "base") as context: - # print(not detect_robot_collision(context, (pos, orn))) - # for i in range(10000): - # og.sim.render() - # test_grasp() - # test_place() - - # replay_controller(env, "./replays/tiago_grasp.yaml") - # execute_controller(controller.place_on_top(table), env) - # from IPython import embed; embed() - # set_start_pose() - # execute_controller(controller._navigate_to_pose([-0.3, -2.3, 0.0]), env) - # execute_controller(controller._navigate_to_pose(pose_2d), env) - # test_place() - # test_grasp_no_navigation() - - # pause(100) - - ################################################################################### - # Random test code below - ################################################################################### - # def detect_robot_collision(robot, filter_objs=[]): - # filter_categories = ["floors"] - - # obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] - # if obj_in_hand is not None: - # filter_objs.append(obj_in_hand) - - # collision_prims = list(robot.states[ContactBodies].get_value(ignore_objs=tuple(filter_objs))) - - # for col_prim in collision_prims: - # tokens = col_prim.prim_path.split("/") - # obj_prim_path = "/".join(tokens[:-1]) - # col_obj = og.sim.scene.object_registry("prim_path", obj_prim_path) - # if col_obj.category in filter_categories: - # collision_prims.remove(col_prim) - - # return len(collision_prims) > 0 or detect_self_collision(robot) - - # def detect_self_collision(robot): - # contacts = robot.contact_list() - # robot_links = [link.prim_path for link in robot.links.values()] - # disabled_pairs = [set(p) for p in robot.disabled_collision_pairs] - # for c in contacts: - # link0 = c.body0.split("/")[-1] - # link1 = c.body1.split("/")[-1] - # if {link0, link1} not in disabled_pairs and c.body0 in robot_links and c.body1 in robot_links: - # return True - # return False - - # robot.set_position([-0.1, -0.35, 0.05]) - # robot.set_orientation(T.euler2quat([0, 0,-np.pi/1.5])) - # og.sim.step() - # control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["left"]]) - # joint_pos = [0.284846, 1.22316, 0.323617, 1.72149, 1.4959, -0.31599, -1.4043, 0.152401] - # robot.set_joint_positions(joint_pos, control_idx) - # og.sim.step() - # while True: - # coll = [] - # robot_links = [link.prim_path for link in robot.links.values()] - # for c in robot.contact_list(): - # if c.body0 in robot_links and c.body1 in robot_links: - # link0 = c.body0.split("/")[-1] - # link1 = c.body1.split("/")[-1] - # pair = {link0, link1} - # if pair not in coll: - # coll.append(pair) - - # print(coll) - # print(detect_robot_collision(robot)) - # print(detect_self_collision(robot)) - # print("---------------") - # pause(2) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Run test script") - parser.add_argument( - "--profile", - action="store_true", - help="If set, profile code and generate .prof file", - ) - args = parser.parse_args() - if args.profile: - pr = cProfile.Profile() - pr.enable() - main() - pr.disable() - s = io.StringIO() - results = pstats.Stats(pr) - filename = f'profile-{os.path.basename(__file__)}-{time.strftime("%Y%m%d-%H%M%S")}' - results.dump_stats(f"./profiles/{filename}.prof") - os.system(f"flameprof ./profiles/{filename}.prof > ./profiles/{filename}.svg") - # Run `snakeviz ./profiles/.prof` to visualize stack trace or open .svg in a browser - else: - main() - - - diff --git a/dev_environment/test_tiago.yaml b/dev_environment/test_tiago.yaml deleted file mode 100644 index 11af1ae94..000000000 --- a/dev_environment/test_tiago.yaml +++ /dev/null @@ -1,71 +0,0 @@ -env: - initial_pos_z_offset: 0.1 - -render: - viewer_width: 1280 - viewer_height: 720 - -scene: - type: InteractiveTraversableScene - scene_model: Rs_int - trav_map_resolution: 0.1 - trav_map_erosion: 2 - trav_map_with_objects: true - build_graph: true - num_waypoints: 1 - waypoint_resolution: 0.2 - load_object_categories: null - not_load_object_categories: null - load_room_types: null - load_room_instances: null - seg_map_resolution: 0.1 - scene_source: OG - include_robots: true - -robots: - - type: Tiago - obs_modalities: [scan, rgb, depth] - scale: 1.0 - self_collisions: true - action_normalize: false - action_type: continuous - grasping_mode: sticky - rigid_trunk: false - default_arm_pose: diagonal30 - default_trunk_offset: 0.365 - controller_config: - base: - name: JointController - motor_type: velocity - arm_left: - name: JointController - motor_type: position - command_input_limits: null - command_output_limits: null - use_delta_commands: false - arm_right: - name: JointController - motor_type: position - command_input_limits: null - command_output_limits: null - use_delta_commands: false - gripper_left: - name: JointController - motor_type: position - command_input_limits: [-1, 1] - command_output_limits: null - use_delta_commands: true - use_single_command: true - gripper_right: - name: JointController - motor_type: position - command_input_limits: [-1, 1] - command_output_limits: null - use_delta_commands: true - use_single_command: true - camera: - name: JointController - motor_type: velocity - use_delta_commands: false - -objects: [] \ No newline at end of file diff --git a/dev_environment/test_toggle.py b/dev_environment/test_toggle.py deleted file mode 100644 index b90af868a..000000000 --- a/dev_environment/test_toggle.py +++ /dev/null @@ -1,84 +0,0 @@ -import yaml -import numpy as np -import argparse - -import omnigibson as og -from omnigibson.macros import gm -from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, UndoableContext, StarterSemanticActionPrimitiveSet -from omnigibson.objects.primitive_object import PrimitiveObject -import omnigibson.utils.transform_utils as T -from omnigibson.objects.dataset_object import DatasetObject - -import cProfile, pstats, io -import time -import os -import argparse - - -def pause(time): - for _ in range(int(time*100)): - og.sim.step() - -def replay_controller(env, filename): - actions = yaml.load(open(filename, "r"), Loader=yaml.FullLoader) - for action in actions: - env.step(action) - -def execute_controller(ctrl_gen, env, filename=None): - actions = [] - for action in ctrl_gen: - env.step(action) - actions.append(action.tolist()) - if filename is not None: - with open(filename, "w") as f: - yaml.dump(actions, f) - -def main(): - # Load the config - config_filename = "test_tiago.yaml" - config = yaml.load(open(config_filename, "r"), Loader=yaml.FullLoader) - - config["scene"]["load_object_categories"] = ["floors", "ceilings", "walls", "coffee_table"] - - # Load the environment - env = og.Environment(configs=config) - scene = env.scene - robot = env.robots[0] - - # Allow user to move camera more easily - og.sim.enable_viewer_camera_teleoperation() - - grasp_obj = DatasetObject( - name="floor_lamp", - category="floor_lamp", - model="vdxlda" - ) - og.sim.import_object(grasp_obj) - grasp_obj.set_position([-0.3, -0.8, 0.5]) - og.sim.step() - - controller = StarterSemanticActionPrimitives(None, scene, robot) - - def set_start_pose(): - reset_pose_tiago = np.array([ - -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, - 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, - -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, - 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, - 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, - 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, - 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, - ]) - robot.set_joint_positions(reset_pose_tiago) - og.sim.step() - - set_start_pose() - execute_controller(controller.apply_ref(StarterSemanticActionPrimitiveSet.TOGGLE_ON, grasp_obj), env) - pause(5) - - -if __name__ == "__main__": - main() - - - From 3c60f92398e0b0b15477c62c8ad0c15fc7758407 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 13 Sep 2023 23:08:22 -0700 Subject: [PATCH 13/29] clean up code --- .../action_primitives/starter_semantic_action_primitives.py | 3 +-- omnigibson/robots/manipulation_robot.py | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 9d467b17a..286c5fa82 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -471,8 +471,7 @@ def _open_or_close(self, obj, should_open): yield from self._move_hand_direct_cartesian(approach_pose, ignore_failure=False, stop_on_contact=True) # Step once to update - for i in range(MAX_STEPS_FOR_GRASP_OR_RELEASE): - yield self._empty_action() + yield self._empty_action() # if grasp_required: # if self._get_obj_in_hand() is None: diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index b8f03e127..10cd1c00f 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -769,6 +769,7 @@ def _calculate_in_hand_object_rigid(self, arm="default"): ag_obj_prim_path = "/".join(ag_prim_path.split("/")[:-1]) ag_obj_link_name = ag_prim_path.split("/")[-1] ag_obj = og.sim.scene.object_registry("prim_path", ag_obj_prim_path) + # Return None if object cannot be assisted grasped or not touching at least two fingers if ag_obj is None or (not can_assisted_grasp(ag_obj, ag_obj_link_name)) or (not touching_at_least_two_fingers): return None From 43daec377e7a110a4dd24db4bb84eb17e2cf1d87 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 13 Sep 2023 23:15:19 -0700 Subject: [PATCH 14/29] clean up code --- .../action_primitives/starter_semantic_action_primitives.py | 1 - omnigibson/robots/manipulation_robot.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 286c5fa82..4f43b5240 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -1109,7 +1109,6 @@ def _get_reset_joint_pos(self): 4.50000000e-02, 4.50000000e-02 ]) - return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): diff --git a/omnigibson/robots/manipulation_robot.py b/omnigibson/robots/manipulation_robot.py index 10cd1c00f..70eafb69a 100644 --- a/omnigibson/robots/manipulation_robot.py +++ b/omnigibson/robots/manipulation_robot.py @@ -769,7 +769,7 @@ def _calculate_in_hand_object_rigid(self, arm="default"): ag_obj_prim_path = "/".join(ag_prim_path.split("/")[:-1]) ag_obj_link_name = ag_prim_path.split("/")[-1] ag_obj = og.sim.scene.object_registry("prim_path", ag_obj_prim_path) - + # Return None if object cannot be assisted grasped or not touching at least two fingers if ag_obj is None or (not can_assisted_grasp(ag_obj, ag_obj_link_name)) or (not touching_at_least_two_fingers): return None From 7fd891f4666a4215de64f9cc90856e495aec1e02 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Thu, 14 Sep 2023 12:07:40 -0700 Subject: [PATCH 15/29] fix reward function --- tests/test_reward_function.py | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/tests/test_reward_function.py b/tests/test_reward_function.py index 97d5b5cc2..2a8227084 100644 --- a/tests/test_reward_function.py +++ b/tests/test_reward_function.py @@ -1,11 +1,13 @@ import math import numpy as np +import matplotlib.pyplot as plt import omnigibson as og from omnigibson.macros import gm from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet import omnigibson.utils.transform_utils as T from omnigibson.objects.dataset_object import DatasetObject + def set_start_pose(robot): reset_pose_tiago = np.array([ -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, @@ -138,23 +140,30 @@ def test_grasp_reward(): ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj) + # rewards = [0] + # total_rewards = [0] + # Check reward going from not grasping to not grasping _, reward, _, _ = env.step(next(ctrl_gen)) + # rewards.append(reward) + # total_rewards.append(total_rewards[-1] + reward) eef_pos = robot.get_eef_position(robot.default_arm) obj_center = obj.aabb_center - expected_reward = math.exp(T.l2_distance(eef_pos, obj_center)) * DIST_COEFF + expected_reward = math.exp(-T.l2_distance(eef_pos, obj_center)) * DIST_COEFF assert math.isclose(reward, expected_reward, abs_tol=0.01) for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] _, reward, _, _ = env.step(action) + # rewards.append(reward) + # total_rewards.append(total_rewards[-1] + reward) obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] if prev_obj_in_hand is None and obj_in_hand is not None: # Check reward going from not grapsing to after grasping assert reward == GRASP_REWARD elif prev_obj_in_hand is not None and obj_in_hand is not None: # Check reward going from grasping to grasping - expected_reward = math.exp(T.l2_distance(robot.aabb_center, obj_center)) * DIST_COEFF + expected_reward = math.exp(-T.l2_distance(robot.aabb_center, obj_center)) * DIST_COEFF assert math.isclose(reward, expected_reward, abs_tol=0.01) break @@ -163,9 +172,18 @@ def test_grasp_reward(): for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] _, reward, _, _ = env.step(action) + # rewards.append(reward) + # total_rewards.append(total_rewards[-1] + reward) obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] if prev_obj_in_hand is not None and obj_in_hand is None: # Check reward going from grapsing to not grasping assert reward == -GRASP_REWARD + # plt.figure(1) + # plt.subplot(211) + # plt.plot(rewards) + # plt.subplot(212) + # plt.plot(total_rewards) + # plt.show() + test_grasp_reward() \ No newline at end of file From f0d137741956e124094abb660a28142525ae6f55 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Thu, 14 Sep 2023 19:19:45 -0700 Subject: [PATCH 16/29] wip --- .../starter_semantic_action_primitives.py | 118 +++++++-------- omnigibson/reward_functions/grasp_reward.py | 24 ++- omnigibson/tasks/grasp_task.py | 49 +++++- tests/grasp_RL_script.py | 141 ++++++++++++++++++ tests/test_reward_function.py | 46 +++--- 5 files changed, 287 insertions(+), 91 deletions(-) create mode 100644 tests/grasp_RL_script.py diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 4f43b5240..a4be5e0f2 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -554,12 +554,12 @@ def _grasp(self, obj): # Step once to update yield self._empty_action() - if self._get_obj_in_hand() is None: - raise ActionPrimitiveError( - ActionPrimitiveError.Reason.POST_CONDITION_ERROR, - "Grasp completed, but no object detected in hand after executing grasp", - {"target object": obj.name}, - ) + # if self._get_obj_in_hand() is None: + # raise ActionPrimitiveError( + # ActionPrimitiveError.Reason.POST_CONDITION_ERROR, + # "Grasp completed, but no object detected in hand after executing grasp", + # {"target object": obj.name}, + # ) yield from self._reset_hand() @@ -1050,65 +1050,65 @@ def _get_reset_joint_pos(self): ] ) - # reset_pose_tiago = np.array([ - # -1.78029833e-04, - # 3.20231302e-05, - # -1.85759447e-07, - # -1.16488536e-07, - # 4.55182843e-08, - # 2.36128806e-04, - # 0.15, - # 0.94, - # -1.1, - # 0.0, - # -0.9, - # 1.47, - # 0.0, - # 2.1, - # 2.71, - # 1.5, - # 1.71, - # 1.3, - # -1.57, - # -1.4, - # 1.39, - # 0.0, - # 0.0, - # 0.045, - # 0.045, - # 0.045, - # 0.045, - # ]) - reset_pose_tiago = np.array([ -1.78029833e-04, 3.20231302e-05, - -1.85759447e-07, + -1.85759447e-07, + -1.16488536e-07, + 4.55182843e-08, + 2.36128806e-04, + 0.15, + 0.94, + -1.1, 0.0, - -0.2, + -0.9, + 1.47, + 0.0, + 2.1, + 2.71, + 1.5, + 1.71, + 1.3, + -1.57, + -1.4, + 1.39, + 0.0, 0.0, - 0.1, - -6.10000000e-01, - -1.10000000e+00, - 0.00000000e+00, - -1.10000000e+00, - 1.47000000e+00, - 0.00000000e+00, - 8.70000000e-01, - 2.71000000e+00, - 1.50000000e+00, - 1.71000000e+00, - -1.50000000e+00, - -1.57000000e+00, - 4.50000000e-01, - 1.39000000e+00, - 0.00000000e+00, - 0.00000000e+00, - 4.50000000e-02, - 4.50000000e-02, - 4.50000000e-02, - 4.50000000e-02 + 0.045, + 0.045, + 0.045, + 0.045, ]) + + # reset_pose_tiago = np.array([ + # -1.78029833e-04, + # 3.20231302e-05, + # -1.85759447e-07, + # 0.0, + # -0.2, + # 0.0, + # 0.1, + # -6.10000000e-01, + # -1.10000000e+00, + # 0.00000000e+00, + # -1.10000000e+00, + # 1.47000000e+00, + # 0.00000000e+00, + # 8.70000000e-01, + # 2.71000000e+00, + # 1.50000000e+00, + # 1.71000000e+00, + # -1.50000000e+00, + # -1.57000000e+00, + # 4.50000000e-01, + # 1.39000000e+00, + # 0.00000000e+00, + # 0.00000000e+00, + # 4.50000000e-02, + # 4.50000000e-02, + # 4.50000000e-02, + # 4.50000000e-02 + # ]) return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): diff --git a/omnigibson/reward_functions/grasp_reward.py b/omnigibson/reward_functions/grasp_reward.py index 305fc7de8..c0e1c26c2 100644 --- a/omnigibson/reward_functions/grasp_reward.py +++ b/omnigibson/reward_functions/grasp_reward.py @@ -29,10 +29,10 @@ def _step(self, task, env, action): # Reward varying based on combination of whether the robot was previously grasping the desired and object # and is currently grasping the desired object - # not grasping -> not grasping = Distance reward - # not grasping -> grasping = Large reward - # grapsing -> not grapsing = Large negative reward - # grasping -> grasping = Minimizing moment of inertia + # not grasping -> not grasping = Distance between eef and obj reward + # not grasping -> grasping = Minimizing MOI + grasp reward + # grapsing -> not grapsing = Distance between eef and obj reward + # grasping -> grasping = Minimizing MOI + grasp reward reward = None @@ -40,19 +40,27 @@ def _step(self, task, env, action): eef_pos = robot.get_eef_position(robot.default_arm) obj_center = self.obj.aabb_center dist = T.l2_distance(eef_pos, obj_center) - reward = math.exp(dist) * self.dist_coeff + reward = math.exp(-dist) * self.dist_coeff elif not self.prev_grasping and current_grasping: - reward = self.grasp_reward + robot_center = robot.aabb_center + obj_center = self.obj.aabb_center + dist = T.l2_distance(robot_center, obj_center) + dist_reward = math.exp(-dist) * self.dist_coeff + reward = dist_reward + self.grasp_reward elif self.prev_grasping and not current_grasping: - reward = -self.grasp_reward + eef_pos = robot.get_eef_position(robot.default_arm) + obj_center = self.obj.aabb_center + dist = T.l2_distance(eef_pos, obj_center) + reward = math.exp(-dist) * self.dist_coeff elif self.prev_grasping and current_grasping: robot_center = robot.aabb_center obj_center = self.obj.aabb_center dist = T.l2_distance(robot_center, obj_center) - reward = math.exp(dist) * self.dist_coeff + dist_reward = math.exp(-dist) * self.dist_coeff + reward = dist_reward + self.grasp_reward self.prev_grasping = current_grasping return reward, {} diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index bda5dd715..32507382d 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -1,15 +1,21 @@ +import random import numpy as np +import omnigibson as og +from omnigibson.action_primitives.starter_semantic_action_primitives import UndoableContext from omnigibson.reward_functions.grasp_reward import GraspReward from omnigibson.tasks.task_base import BaseTask from omnigibson.scenes.scene_base import Scene from omnigibson.termination_conditions.grasp_goal import GraspGoal from omnigibson.termination_conditions.timeout import Timeout +from omnigibson.utils.grasping_planning_utils import get_grasp_poses_for_object_sticky +from omnigibson.utils.motion_planning_utils import set_arm_and_detect_collision from omnigibson.utils.python_utils import classproperty from omnigibson.utils.sim_utils import land_object DIST_COEFF = 0.1 GRASP_REWARD = 1.0 +MAX_JOINT_RANDOMIZATION_ATTEMPTS = 50 class GraspTask(BaseTask): """ @@ -26,8 +32,7 @@ def __init__( super().__init__(termination_config=termination_config, reward_config=reward_config) def _load(self, env): - # Do nothing here - pass + pass def _create_termination_conditions(self): terminations = dict() @@ -46,6 +51,43 @@ def _create_reward_functions(self): grasp_reward=self._reward_config["r_grasp"] ) return rewards + + def _reset_agent(self, env): + robot = env.robots[0] + # # Randomize the robots joint positions + # # @TODO: SHOULD WORK, BUT NEED TO TEST + # # joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) + # # joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + # # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) + # # control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] + + # # with UndoableContext(self.primitive_controller.robot, self.primitive_controller.robot_copy, "original") as context: + # # for _ in range(MAX_JOINT_RANDOMIZATION_ATTEMPTS): + # # joint_pos, joint_control_idx = self._get_random_joint_position(robot) + # # initial_joint_pos[control_idx_in_joint_pos] = joint_pos + # # if not set_arm_and_detect_collision(context, initial_joint_pos): + # # robot.set_joint_positions(joint_pos, joint_control_idx) + # # og.sim.step() + # # break + + # Randomize the robot's 2d pose + obj = env.scene.object_registry("name", self.obj_name) + if obj is not None: + grasp_poses = get_grasp_poses_for_object_sticky(obj) + grasp_pose, _ = random.choice(grasp_poses) + sampled_pose_2d = self.primitive_controller._sample_pose_near_object(obj, pose_on_obj=grasp_pose) + robot_pose = self.primitive_controller._get_robot_pose_from_2d_pose(sampled_pose_2d) + robot.set_position_orientation(*robot_pose) + + def _get_random_joint_position(self, robot): + joint_positions = [] + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) + joints = np.array([joint for joint in robot.joints.values()]) + arm_joints = joints[joint_control_idx] + for i, joint in enumerate(arm_joints): + val = random.uniform(joint.lower_limit, joint.upper_limit) + joint_positions.append(val) + return joint_positions, joint_control_idx def _get_obs(self, env): # No task-specific obs of any kind @@ -54,6 +96,9 @@ def _get_obs(self, env): def _load_non_low_dim_observation_space(self): # No non-low dim observations so we return an empty dict return dict() + + def add_primitive_controller(self, controller): + self.primitive_controller = controller @classproperty def valid_scene_types(cls): diff --git a/tests/grasp_RL_script.py b/tests/grasp_RL_script.py new file mode 100644 index 000000000..90060f164 --- /dev/null +++ b/tests/grasp_RL_script.py @@ -0,0 +1,141 @@ +import math +import numpy as np +import matplotlib.pyplot as plt +import omnigibson as og +from omnigibson.macros import gm +from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet +import omnigibson.utils.transform_utils as T +from omnigibson.objects.dataset_object import DatasetObject + + +def set_start_pose(robot): + reset_pose_tiago = np.array([ + -1.78029833e-04, 3.20231302e-05, -1.85759447e-07, -1.16488536e-07, + 4.55182843e-08, 2.36128806e-04, 1.50000000e-01, 9.40000000e-01, + -1.10000000e+00, 0.00000000e+00, -0.90000000e+00, 1.47000000e+00, + 0.00000000e+00, 2.10000000e+00, 2.71000000e+00, 1.50000000e+00, + 1.71000000e+00, 1.30000000e+00, -1.57000000e+00, -1.40000000e+00, + 1.39000000e+00, 0.00000000e+00, 0.00000000e+00, 4.50000000e-02, + 4.50000000e-02, 4.50000000e-02, 4.50000000e-02, + ]) + robot.set_joint_positions(reset_pose_tiago) + og.sim.step() + +def step_sim(time): + for _ in range(int(time*100)): + og.sim.step() + +def execute_controller(ctrl_gen, env): + for action in ctrl_gen: + env.step(action) + +DIST_COEFF = 0.1 +GRASP_REWARD = 0.3 +RL_ITERATIONS = 10 + +cfg = { + "scene": { + "type": "InteractiveTraversableScene", + "scene_model": "Rs_int", + "load_object_categories": ["floors", "coffee_table"], + }, + "robots": [ + { + "type": "Tiago", + "obs_modalities": ["scan", "rgb", "depth"], + "scale": 1.0, + "self_collisions": True, + "action_normalize": False, + "action_type": "continuous", + "grasping_mode": "sticky", + "rigid_trunk": False, + "default_arm_pose": "diagonal30", + "default_trunk_offset": 0.365, + "controller_config": { + "base": { + "name": "JointController", + "motor_type": "velocity" + }, + "arm_left": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": None, + "command_output_limits": None, + "use_delta_commands": False + }, + "arm_right": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": None, + "command_output_limits": None, + "use_delta_commands": False + }, + "gripper_left": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": [-1, 1], + "command_output_limits": None, + "use_delta_commands": True, + "use_single_command": True + }, + "gripper_right": { + "name": "JointController", + "motor_type": "position", + "command_input_limits": [-1, 1], + "command_output_limits": None, + "use_delta_commands": True, + "use_single_command": True + }, + "camera": { + "name": "JointController", + "motor_type": "velocity", + "use_delta_commands": False + } + } + } + ], + "task": { + "type": "GraspTask", + "obj_name": "cologne", + "termination_config": { + "max_steps": 100000, + }, + "reward_config": { + "r_dist_coeff": DIST_COEFF, + "r_grasp": GRASP_REWARD + } + } +} + +# Create the environment +env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) +scene = env.scene +robot = env.robots[0] + +# Place object in scene +obj = DatasetObject( + name="cologne", + category="bottle_of_cologne", + model="lyipur" +) +og.sim.import_object(obj) +obj.set_position([-0.3, -0.8, 0.5]) +set_start_pose(robot) +og.sim.step() +env.scene.update_initial_state() + +controller = StarterSemanticActionPrimitives(None, scene, robot) +env.task.add_primitive_controller(controller) + +ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj) + +for i in range(RL_ITERATIONS): + env.reset() + try: + for action in ctrl_gen: + state, reward, done, info = env.step(action) + if done: + break + except Exception as e: + print(e) + print("Error in iteration", i) \ No newline at end of file diff --git a/tests/test_reward_function.py b/tests/test_reward_function.py index 2a8227084..7fd853ea6 100644 --- a/tests/test_reward_function.py +++ b/tests/test_reward_function.py @@ -140,50 +140,52 @@ def test_grasp_reward(): ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj) - # rewards = [0] - # total_rewards = [0] + rewards = [0] + total_rewards = [0] # Check reward going from not grasping to not grasping _, reward, _, _ = env.step(next(ctrl_gen)) - # rewards.append(reward) - # total_rewards.append(total_rewards[-1] + reward) + rewards.append(reward) + total_rewards.append(total_rewards[-1] + reward) eef_pos = robot.get_eef_position(robot.default_arm) - obj_center = obj.aabb_center - expected_reward = math.exp(-T.l2_distance(eef_pos, obj_center)) * DIST_COEFF + expected_reward = math.exp(-T.l2_distance(eef_pos, obj.aabb_center)) * DIST_COEFF assert math.isclose(reward, expected_reward, abs_tol=0.01) for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] _, reward, _, _ = env.step(action) - # rewards.append(reward) - # total_rewards.append(total_rewards[-1] + reward) + rewards.append(reward) + total_rewards.append(total_rewards[-1] + reward) obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] if prev_obj_in_hand is None and obj_in_hand is not None: # Check reward going from not grapsing to after grasping - assert reward == GRASP_REWARD + expected_reward = math.exp(-T.l2_distance(robot.aabb_center, obj.aabb_center)) * DIST_COEFF + GRASP_REWARD + assert math.isclose(reward, expected_reward, abs_tol=0.01) elif prev_obj_in_hand is not None and obj_in_hand is not None: # Check reward going from grasping to grasping - expected_reward = math.exp(-T.l2_distance(robot.aabb_center, obj_center)) * DIST_COEFF - assert math.isclose(reward, expected_reward, abs_tol=0.01) - break + expected_reward = math.exp(-T.l2_distance(robot.aabb_center, obj.aabb_center)) * DIST_COEFF + GRASP_REWARD + # assert math.isclose(reward, expected_reward, abs_tol=0.01) + # break ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.RELEASE) for action in ctrl_gen: prev_obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] _, reward, _, _ = env.step(action) - # rewards.append(reward) - # total_rewards.append(total_rewards[-1] + reward) + rewards.append(reward) + total_rewards.append(total_rewards[-1] + reward) obj_in_hand = robot._ag_obj_in_hand[robot.default_arm] if prev_obj_in_hand is not None and obj_in_hand is None: # Check reward going from grapsing to not grasping - assert reward == -GRASP_REWARD - - # plt.figure(1) - # plt.subplot(211) - # plt.plot(rewards) - # plt.subplot(212) - # plt.plot(total_rewards) - # plt.show() + eef_pos = robot.get_eef_position(robot.default_arm) + expected_reward = math.exp(-T.l2_distance(eef_pos, obj.aabb_center)) * DIST_COEFF + assert math.isclose(reward, expected_reward, abs_tol=0.01) + + plt.figure(1) + plt.subplot(211) + plt.plot(rewards) + plt.subplot(212) + plt.plot(total_rewards) + plt.show() test_grasp_reward() \ No newline at end of file From 4a7f636f537de0d9b405b9c5e27e1ea1b29c3160 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Thu, 14 Sep 2023 19:25:07 -0700 Subject: [PATCH 17/29] wip --- .../starter_semantic_action_primitives.py | 118 +++++++++--------- 1 file changed, 59 insertions(+), 59 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index a4be5e0f2..4f43b5240 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -554,12 +554,12 @@ def _grasp(self, obj): # Step once to update yield self._empty_action() - # if self._get_obj_in_hand() is None: - # raise ActionPrimitiveError( - # ActionPrimitiveError.Reason.POST_CONDITION_ERROR, - # "Grasp completed, but no object detected in hand after executing grasp", - # {"target object": obj.name}, - # ) + if self._get_obj_in_hand() is None: + raise ActionPrimitiveError( + ActionPrimitiveError.Reason.POST_CONDITION_ERROR, + "Grasp completed, but no object detected in hand after executing grasp", + {"target object": obj.name}, + ) yield from self._reset_hand() @@ -1050,65 +1050,65 @@ def _get_reset_joint_pos(self): ] ) - reset_pose_tiago = np.array([ - -1.78029833e-04, - 3.20231302e-05, - -1.85759447e-07, - -1.16488536e-07, - 4.55182843e-08, - 2.36128806e-04, - 0.15, - 0.94, - -1.1, - 0.0, - -0.9, - 1.47, - 0.0, - 2.1, - 2.71, - 1.5, - 1.71, - 1.3, - -1.57, - -1.4, - 1.39, - 0.0, - 0.0, - 0.045, - 0.045, - 0.045, - 0.045, - ]) - # reset_pose_tiago = np.array([ # -1.78029833e-04, # 3.20231302e-05, - # -1.85759447e-07, + # -1.85759447e-07, + # -1.16488536e-07, + # 4.55182843e-08, + # 2.36128806e-04, + # 0.15, + # 0.94, + # -1.1, # 0.0, - # -0.2, + # -0.9, + # 1.47, + # 0.0, + # 2.1, + # 2.71, + # 1.5, + # 1.71, + # 1.3, + # -1.57, + # -1.4, + # 1.39, + # 0.0, # 0.0, - # 0.1, - # -6.10000000e-01, - # -1.10000000e+00, - # 0.00000000e+00, - # -1.10000000e+00, - # 1.47000000e+00, - # 0.00000000e+00, - # 8.70000000e-01, - # 2.71000000e+00, - # 1.50000000e+00, - # 1.71000000e+00, - # -1.50000000e+00, - # -1.57000000e+00, - # 4.50000000e-01, - # 1.39000000e+00, - # 0.00000000e+00, - # 0.00000000e+00, - # 4.50000000e-02, - # 4.50000000e-02, - # 4.50000000e-02, - # 4.50000000e-02 + # 0.045, + # 0.045, + # 0.045, + # 0.045, # ]) + + reset_pose_tiago = np.array([ + -1.78029833e-04, + 3.20231302e-05, + -1.85759447e-07, + 0.0, + -0.2, + 0.0, + 0.1, + -6.10000000e-01, + -1.10000000e+00, + 0.00000000e+00, + -1.10000000e+00, + 1.47000000e+00, + 0.00000000e+00, + 8.70000000e-01, + 2.71000000e+00, + 1.50000000e+00, + 1.71000000e+00, + -1.50000000e+00, + -1.57000000e+00, + 4.50000000e-01, + 1.39000000e+00, + 0.00000000e+00, + 0.00000000e+00, + 4.50000000e-02, + 4.50000000e-02, + 4.50000000e-02, + 4.50000000e-02 + ]) return reset_pose_tiago if self.robot_model == "Tiago" else reset_pose_fetch def _navigate_to_pose(self, pose_2d): From 007ec899925d6024b5ebb432391e714899d691b5 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 17 Sep 2023 15:15:39 -0700 Subject: [PATCH 18/29] randomize arm and base before every run --- .../starter_semantic_action_primitives.py | 10 +++ omnigibson/envs/env_base.py | 2 +- omnigibson/tasks/grasp_task.py | 63 +++++++++++++------ omnigibson/utils/motion_planning_utils.py | 8 ++- tests/grasp_RL_script.py | 56 ++++++++++++----- 5 files changed, 99 insertions(+), 40 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 4f43b5240..8d925cf91 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -388,6 +388,7 @@ def apply_ref(self, prim, *args, track_object=False, attempts=3): yield from ctrl(*args) success = True except ActionPrimitiveError as e: + print(e) errors.append(e) try: @@ -828,6 +829,7 @@ def _move_hand_direct_joint(self, joint_pos, control_idx, stop_on_contact=False, controller_name = "arm_{}".format(self.arm) action[self.robot.controller_action_idx[controller_name]] = joint_pos action = self._overwrite_head_action(action, self._tracking_object) if self._tracking_object is not None else action + prev_eef_pos = [0.0, 0.0, 0.0] for _ in range(max_steps_for_hand_move): current_joint_pos = self.robot.get_joint_positions()[control_idx] @@ -836,6 +838,14 @@ def _move_hand_direct_joint(self, joint_pos, control_idx, stop_on_contact=False, return if stop_on_contact and detect_robot_collision_in_sim(self.robot, ignore_obj_in_hand=False): return + + if max(np.abs(np.array(self.robot.get_eef_position(self.arm) - prev_eef_pos))) < 0.0001: + raise ActionPrimitiveError( + ActionPrimitiveError.Reason.EXECUTION_ERROR, + f"Hand is stuck" + ) + + prev_eef_pos = self.robot.get_eef_position(self.arm) yield action if not ignore_failure: diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 51ebda492..9b668e8bf 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -458,7 +458,7 @@ def reset(self): # Reset internal variables self._reset_variables() - # Run a single simulator step to make sure we can grab updated observations + # # Run a single simulator step to make sure we can grab updated observations og.sim.step() # Grab and return observations diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 32507382d..5be71a108 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -53,32 +53,55 @@ def _create_reward_functions(self): return rewards def _reset_agent(self, env): - robot = env.robots[0] - # # Randomize the robots joint positions - # # @TODO: SHOULD WORK, BUT NEED TO TEST - # # joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) - # # joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) - # # initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) - # # control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] - - # # with UndoableContext(self.primitive_controller.robot, self.primitive_controller.robot_copy, "original") as context: - # # for _ in range(MAX_JOINT_RANDOMIZATION_ATTEMPTS): - # # joint_pos, joint_control_idx = self._get_random_joint_position(robot) - # # initial_joint_pos[control_idx_in_joint_pos] = joint_pos - # # if not set_arm_and_detect_collision(context, initial_joint_pos): - # # robot.set_joint_positions(joint_pos, joint_control_idx) - # # og.sim.step() - # # break - - # Randomize the robot's 2d pose - obj = env.scene.object_registry("name", self.obj_name) - if obj is not None: + # from IPython import embed; embed() + if hasattr(self, 'primitive_controller'): + robot = env.robots[0] + # Randomize the robots joint positions + joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) + joint_combined_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx["combined"]]) + initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) + control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] + + with UndoableContext(self.primitive_controller.robot, self.primitive_controller.robot_copy, "original") as context: + for _ in range(MAX_JOINT_RANDOMIZATION_ATTEMPTS): + joint_pos, joint_control_idx = self._get_random_joint_position(robot) + initial_joint_pos[control_idx_in_joint_pos] = joint_pos + if not set_arm_and_detect_collision(context, initial_joint_pos): + robot.set_joint_positions(joint_pos, joint_control_idx) + og.sim.step() + break + + # Randomize the robot's 2d pose + obj = env.scene.object_registry("name", self.obj_name) grasp_poses = get_grasp_poses_for_object_sticky(obj) grasp_pose, _ = random.choice(grasp_poses) sampled_pose_2d = self.primitive_controller._sample_pose_near_object(obj, pose_on_obj=grasp_pose) robot_pose = self.primitive_controller._get_robot_pose_from_2d_pose(sampled_pose_2d) robot.set_position_orientation(*robot_pose) + # Overwrite reset by only removeing reset scene + def reset(self, env): + """ + Resets this task in the environment + + Args: + env (Environment): environment instance to reset + """ + # Reset the scene, agent, and variables + # self._reset_scene(env) + self._reset_agent(env) + self._reset_variables(env) + + # Also reset all termination conditions and reward functions + for termination_condition in self._termination_conditions.values(): + termination_condition.reset(self, env) + for reward_function in self._reward_functions.values(): + reward_function.reset(self, env) + + # Fill in low dim obs dim so we can use this to create the observation space later + self._low_dim_obs_dim = len(self.get_obs(env=env, flatten_low_dim=True)["low_dim"]) + + def _get_random_joint_position(self, robot): joint_positions = [] joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) diff --git a/omnigibson/utils/motion_planning_utils.py b/omnigibson/utils/motion_planning_utils.py index dc4f2e19c..7a8f733fa 100644 --- a/omnigibson/utils/motion_planning_utils.py +++ b/omnigibson/utils/motion_planning_utils.py @@ -239,8 +239,12 @@ def state_valid_fn(q): end_conf[i] = joint.upper_limit if end_conf[i] < joint.lower_limit: end_conf[i] = joint.lower_limit - bounds.setLow(i, float(joint.lower_limit)) - bounds.setHigh(i, float(joint.upper_limit)) + if joint.upper_limit - joint.lower_limit > 2 * np.pi: + bounds.setLow(i, 0.0) + bounds.setHigh(i, 2 * np.pi) + else: + bounds.setLow(i, float(joint.lower_limit)) + bounds.setHigh(i, float(joint.upper_limit)) space.setBounds(bounds) # create a simple setup object diff --git a/tests/grasp_RL_script.py b/tests/grasp_RL_script.py index 90060f164..6fb0bde32 100644 --- a/tests/grasp_RL_script.py +++ b/tests/grasp_RL_script.py @@ -29,6 +29,13 @@ def execute_controller(ctrl_gen, env): for action in ctrl_gen: env.step(action) +def reset_env(env, initial_poses): + objs = ["cologne", "coffee_table_fqluyq_0", "robot0"] + for o in objs: + env.scene.object_registry("name", o).set_position_orientation(*initial_poses[o]) + env.reset() + og.sim.step() + DIST_COEFF = 0.1 GRASP_REWARD = 0.3 RL_ITERATIONS = 10 @@ -104,38 +111,53 @@ def execute_controller(ctrl_gen, env): "r_dist_coeff": DIST_COEFF, "r_grasp": GRASP_REWARD } - } + }, + "objects": [ + { + "type": "DatasetObject", + "name": "cologne", + "category": "bottle_of_cologne", + "model": "lyipur", + "position": [-0.3, -0.8, 0.5], + }, + ] } # Create the environment env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) scene = env.scene robot = env.robots[0] +og.sim.step() +# og.sim.stop() +# og.sim.play() # Place object in scene -obj = DatasetObject( - name="cologne", - category="bottle_of_cologne", - model="lyipur" -) -og.sim.import_object(obj) -obj.set_position([-0.3, -0.8, 0.5]) -set_start_pose(robot) -og.sim.step() -env.scene.update_initial_state() +# obj = DatasetObject( +# name="cologne", +# category="bottle_of_cologne", +# model="lyipur" +# ) +# og.sim.import_object(obj) +# obj.set_position([-0.3, -0.8, 0.5]) +# set_start_pose(robot) +# og.sim.step() +# env.scene.update_initial_state() controller = StarterSemanticActionPrimitives(None, scene, robot) env.task.add_primitive_controller(controller) - -ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj) +initial_poses = {} +for o in env.scene.objects: + initial_poses[o.name] = o.get_position_orientation() +obj = env.scene.object_registry("name", "cologne") for i in range(RL_ITERATIONS): - env.reset() try: - for action in ctrl_gen: + reset_env(env, initial_poses) + for action in controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj): state, reward, done, info = env.step(action) if done: + for action in controller._execute_release(): + state, reward, done, info = env.step(action) break - except Exception as e: - print(e) + except: print("Error in iteration", i) \ No newline at end of file From f9998d1a09544cd0967c4c91a66b4afeb11334f3 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Sun, 17 Sep 2023 15:17:10 -0700 Subject: [PATCH 19/29] clean up code --- .../action_primitives/starter_semantic_action_primitives.py | 1 - omnigibson/envs/env_base.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 8d925cf91..84b9f9f87 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -388,7 +388,6 @@ def apply_ref(self, prim, *args, track_object=False, attempts=3): yield from ctrl(*args) success = True except ActionPrimitiveError as e: - print(e) errors.append(e) try: diff --git a/omnigibson/envs/env_base.py b/omnigibson/envs/env_base.py index 9b668e8bf..51ebda492 100644 --- a/omnigibson/envs/env_base.py +++ b/omnigibson/envs/env_base.py @@ -458,7 +458,7 @@ def reset(self): # Reset internal variables self._reset_variables() - # # Run a single simulator step to make sure we can grab updated observations + # Run a single simulator step to make sure we can grab updated observations og.sim.step() # Grab and return observations From aa0f2d3337a95acd131a8ed0e793ae38e6e319d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 18 Sep 2023 15:16:45 -0700 Subject: [PATCH 20/29] Set up docker build matrix --- .github/workflows/build-push-containers.yml | 40 +++++++-------------- 1 file changed, 13 insertions(+), 27 deletions(-) diff --git a/.github/workflows/build-push-containers.yml b/.github/workflows/build-push-containers.yml index 9471b4d3c..49fd3c4ba 100644 --- a/.github/workflows/build-push-containers.yml +++ b/.github/workflows/build-push-containers.yml @@ -12,6 +12,13 @@ on: jobs: docker: runs-on: [self-hosted, linux, gpu] + strategy: + matrix: + include: + - dockerfile: docker/dev.Dockerfile + image: ghcr.io/stanfordvl/omnigibson-dev + - dockerfile: docker/prod.Dockerfile + image: ghcr.io/stanfordvl/omnigibson steps: - name: Set up Docker Buildx @@ -30,42 +37,21 @@ jobs: username: ${{ secrets.DOCKER_HUB_USERNAME }} password: ${{ secrets.DOCKER_HUB_PASSWORD }} - - name: Metadata for dev Image - id: meta-dev + name: Metadata + id: meta uses: docker/metadata-action@v4 with: - images: | - stanfordvl/omnigibson-dev + images: ${{ matrix.image }} tags: | type=ref,event=branch type=semver,pattern={{version}} - - - name: Metadata for prod Image - id: meta-prod - uses: docker/metadata-action@v4 - with: - images: | - stanfordvl/omnigibson - tags: | - type=ref,event=branch - type=semver,pattern={{version}} - - - name: Build and push dev image - uses: docker/build-push-action@v4 - with: - push: true - tags: ${{ steps.meta-dev.outputs.tags }} - labels: ${{ steps.meta-dev.outputs.labels }} - file: docker/dev.Dockerfile - cache-from: type=gha - cache-to: type=gha,mode=max - name: Build and push prod image uses: docker/build-push-action@v4 with: push: true - tags: ${{ steps.meta-prod.outputs.tags }} - labels: ${{ steps.meta-prod.outputs.labels }} - file: docker/prod.Dockerfile + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + file: ${{ matrix.dockerfile }} cache-from: type=gha cache-to: type=gha,mode=max From cd19f0ff2cd60c5d967308003c3fdcb148d0e140 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 18 Sep 2023 15:33:37 -0700 Subject: [PATCH 21/29] Docker hub, not github --- .github/workflows/build-push-containers.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-push-containers.yml b/.github/workflows/build-push-containers.yml index 49fd3c4ba..4fd14457c 100644 --- a/.github/workflows/build-push-containers.yml +++ b/.github/workflows/build-push-containers.yml @@ -16,9 +16,9 @@ jobs: matrix: include: - dockerfile: docker/dev.Dockerfile - image: ghcr.io/stanfordvl/omnigibson-dev + image: stanfordvl/omnigibson-dev - dockerfile: docker/prod.Dockerfile - image: ghcr.io/stanfordvl/omnigibson + image: stanfordvl/omnigibson steps: - name: Set up Docker Buildx From 24900d4b80e69287636926113d3ee10d5baa2ad0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 18 Sep 2023 19:34:58 -0700 Subject: [PATCH 22/29] Fix docker dependency chain --- .github/workflows/build-push-containers.yml | 45 +++++++++++++++------ docker/prod.Dockerfile | 2 +- 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/.github/workflows/build-push-containers.yml b/.github/workflows/build-push-containers.yml index 4fd14457c..488a80e0a 100644 --- a/.github/workflows/build-push-containers.yml +++ b/.github/workflows/build-push-containers.yml @@ -12,13 +12,6 @@ on: jobs: docker: runs-on: [self-hosted, linux, gpu] - strategy: - matrix: - include: - - dockerfile: docker/dev.Dockerfile - image: stanfordvl/omnigibson-dev - - dockerfile: docker/prod.Dockerfile - image: stanfordvl/omnigibson steps: - name: Set up Docker Buildx @@ -37,21 +30,47 @@ jobs: username: ${{ secrets.DOCKER_HUB_USERNAME }} password: ${{ secrets.DOCKER_HUB_PASSWORD }} - - name: Metadata - id: meta + name: Metadata for dev Image + id: meta-dev uses: docker/metadata-action@v4 with: - images: ${{ matrix.image }} + images: | + stanfordvl/omnigibson-dev tags: | type=ref,event=branch type=semver,pattern={{version}} + - + name: Metadata for prod Image + id: meta-prod + uses: docker/metadata-action@v4 + with: + images: | + stanfordvl/omnigibson + tags: | + type=ref,event=branch + type=semver,pattern={{version}} + - + name: Build and push dev image + uses: docker/build-push-action@v4 + id: build-dev + with: + push: true + tags: ${{ steps.meta-dev.outputs.tags }} + labels: ${{ steps.meta-dev.outputs.labels }} + file: docker/dev.Dockerfile + cache-from: type=gha + cache-to: type=gha,mode=max + + - name: Update prod image Dockerfile with dev image tag + run: | + sed -i "s/omnigibson-dev:latest/omnigibson-dev@${{ steps.build-dev.outputs.digest }}/g" docker/prod.Dockerfile - name: Build and push prod image uses: docker/build-push-action@v4 with: push: true - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} - file: ${{ matrix.dockerfile }} + tags: ${{ steps.meta-prod.outputs.tags }} + labels: ${{ steps.meta-prod.outputs.labels }} + file: docker/prod.Dockerfile cache-from: type=gha cache-to: type=gha,mode=max diff --git a/docker/prod.Dockerfile b/docker/prod.Dockerfile index a0eaeef21..9feb45495 100644 --- a/docker/prod.Dockerfile +++ b/docker/prod.Dockerfile @@ -1,4 +1,4 @@ -FROM stanfordvl/omnigibson-dev +FROM stanfordvl/omnigibson-dev:latest # Copy over omnigibson source ADD . /omnigibson-src From 046e6f19f02c8d4def8e72e76c0ebce866c44edd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 18 Sep 2023 21:09:44 -0700 Subject: [PATCH 23/29] Print the updated Dockerfile. --- .github/workflows/build-push-containers.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-push-containers.yml b/.github/workflows/build-push-containers.yml index 488a80e0a..eb78637b9 100644 --- a/.github/workflows/build-push-containers.yml +++ b/.github/workflows/build-push-containers.yml @@ -63,7 +63,7 @@ jobs: - name: Update prod image Dockerfile with dev image tag run: | - sed -i "s/omnigibson-dev:latest/omnigibson-dev@${{ steps.build-dev.outputs.digest }}/g" docker/prod.Dockerfile + sed -i "s/omnigibson-dev:latest/omnigibson-dev@${{ steps.build-dev.outputs.digest }}/g" docker/prod.Dockerfile && cat docker/prod.Dockerfile - name: Build and push prod image uses: docker/build-push-action@v4 From b2d03c517067c9e08a336a47365684249d203e9b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= Date: Mon, 18 Sep 2023 23:18:13 -0700 Subject: [PATCH 24/29] Make the build command take the context into account --- .github/workflows/build-push-containers.yml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/build-push-containers.yml b/.github/workflows/build-push-containers.yml index eb78637b9..87ee68a8b 100644 --- a/.github/workflows/build-push-containers.yml +++ b/.github/workflows/build-push-containers.yml @@ -13,6 +13,9 @@ jobs: docker: runs-on: [self-hosted, linux, gpu] steps: + - + name: Checkout + uses: actions/checkout@v3 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v2 @@ -54,6 +57,7 @@ jobs: uses: docker/build-push-action@v4 id: build-dev with: + context: . push: true tags: ${{ steps.meta-dev.outputs.tags }} labels: ${{ steps.meta-dev.outputs.labels }} @@ -68,6 +72,7 @@ jobs: name: Build and push prod image uses: docker/build-push-action@v4 with: + context: . push: true tags: ${{ steps.meta-prod.outputs.tags }} labels: ${{ steps.meta-prod.outputs.labels }} From 0279da37ad29c1c08871bf211e9d2824d297260a Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Tue, 19 Sep 2023 00:16:32 -0700 Subject: [PATCH 25/29] code to save RL experiments --- tests/grasp_RL_script.py | 41 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 38 insertions(+), 3 deletions(-) diff --git a/tests/grasp_RL_script.py b/tests/grasp_RL_script.py index 6fb0bde32..bb22052f3 100644 --- a/tests/grasp_RL_script.py +++ b/tests/grasp_RL_script.py @@ -1,3 +1,4 @@ +from datetime import datetime import math import numpy as np import matplotlib.pyplot as plt @@ -6,6 +7,7 @@ from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet import omnigibson.utils.transform_utils as T from omnigibson.objects.dataset_object import DatasetObject +import h5py def set_start_pose(robot): @@ -38,7 +40,7 @@ def reset_env(env, initial_poses): DIST_COEFF = 0.1 GRASP_REWARD = 0.3 -RL_ITERATIONS = 10 +RL_ITERATIONS = 2 cfg = { "scene": { @@ -49,7 +51,7 @@ def reset_env(env, initial_poses): "robots": [ { "type": "Tiago", - "obs_modalities": ["scan", "rgb", "depth"], + "obs_modalities": ["rgb", "depth"], "scale": 1.0, "self_collisions": True, "action_normalize": False, @@ -123,6 +125,34 @@ def reset_env(env, initial_poses): ] } +class Recorder(): + def __init__(self): + self.filename = "./RL_data.h5" + self.state_keys = ["robot0:eyes_Camera_sensor_rgb", "robot0:eyes_Camera_sensor_depth"] + self.reset() + + def add(self, state, action, reward): + for k in self.state_keys: + self.states[k].append(state['robot0'][k]) + self.actions.append(action) + self.rewards.append(reward) + + def reset(self): + self.states = {} + for k in self.state_keys: + self.states[k] = [] + self.actions = [] + self.rewards = [] + + def save(self, group_name): + h5file = h5py.File(self.filename, 'a') + group = h5file.create_group(group_name) + for k in self.state_keys: + group.create_dataset(k[k.find(":") + 1:], data=np.array(self.states[k])) + group.create_dataset("actions", data=np.array(self.actions)) + group.create_dataset("rewards", data=np.array(self.rewards)) + h5file.close() + # Create the environment env = og.Environment(configs=cfg, action_timestep=1 / 60., physics_timestep=1 / 60.) scene = env.scene @@ -149,15 +179,20 @@ def reset_env(env, initial_poses): for o in env.scene.objects: initial_poses[o.name] = o.get_position_orientation() obj = env.scene.object_registry("name", "cologne") +recorder = Recorder() for i in range(RL_ITERATIONS): try: reset_env(env, initial_poses) for action in controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj): state, reward, done, info = env.step(action) + recorder.add(state, action, reward) if done: for action in controller._execute_release(): state, reward, done, info = env.step(action) + recorder.add(state, action, reward) break except: - print("Error in iteration", i) \ No newline at end of file + print("Error in iteration: ", i) + +recorder.save(datetime.now().strftime('%Y-%m-%d_%H-%M-%S')) \ No newline at end of file From 73d0cd61cd061839910e1e4e9fdb8ae9a209f3a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Tue, 19 Sep 2023 09:22:41 -0700 Subject: [PATCH 26/29] Update traversable_map.py --- omnigibson/maps/traversable_map.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/omnigibson/maps/traversable_map.py b/omnigibson/maps/traversable_map.py index 8d1e7a6e4..83f3fb299 100644 --- a/omnigibson/maps/traversable_map.py +++ b/omnigibson/maps/traversable_map.py @@ -155,8 +155,11 @@ def build_trav_graph(map_size, maps_path, floor, trav_map): # only take the largest connected component largest_cc = max(nx.connected_components(g), key=len) g = g.subgraph(largest_cc).copy() - with open(graph_file, "wb") as pfile: - pickle.dump(g, pfile) + try: + with open(graph_file, "wb") as pfile: + pickle.dump(g, pfile) + except: + log.warning("Cannot cache traversable graph to disk possibly because dataset is read-only. Will have to recompute each time.") floor_graph.append(g) From 91c2360bf4ecb08dc29e6c355c4efe9f083f0096 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Thu, 21 Sep 2023 13:21:57 -0700 Subject: [PATCH 27/29] Fix a couple things --- .../symbolic_semantic_action_primitives.py | 2 +- tests/test_symbolic_primitives.py | 10 +++------- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/omnigibson/action_primitives/symbolic_semantic_action_primitives.py b/omnigibson/action_primitives/symbolic_semantic_action_primitives.py index c2b6ffeff..08b1acc40 100644 --- a/omnigibson/action_primitives/symbolic_semantic_action_primitives.py +++ b/omnigibson/action_primitives/symbolic_semantic_action_primitives.py @@ -784,7 +784,7 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): """ if pose_on_obj is None: pos_on_obj = self._sample_position_on_aabb_face(obj) - pose_on_obj = np.array([pos_on_obj, [0, 0, 0, 1]]) + pose_on_obj = [pos_on_obj, np.array([0, 0, 0, 1])] with UndoableContext(self.robot, self.robot_copy, "simplified") as context: obj_rooms = obj.in_rooms if obj.in_rooms else [self.scene._seg_map.get_room_instance_by_point(pose_on_obj[0][:2])] diff --git a/tests/test_symbolic_primitives.py b/tests/test_symbolic_primitives.py index c34255012..3d4c61822 100644 --- a/tests/test_symbolic_primitives.py +++ b/tests/test_symbolic_primitives.py @@ -2,17 +2,13 @@ import pytest import yaml -import omnigibson.macros as gm -gm.USE_GPU_DYNAMICS = True -gm.USE_FLATCACHE = True - - import omnigibson as og +from omnigibson.macros import gm from omnigibson import object_states from omnigibson.action_primitives.symbolic_semantic_action_primitives import SymbolicSemanticActionPrimitiveSet, SymbolicSemanticActionPrimitives - - +gm.USE_GPU_DYNAMICS = True +gm.USE_FLATCACHE = True def start_env(): config = { From 336254fefc73303f417f4d96441f1c00342bce2e Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Thu, 21 Sep 2023 19:58:28 -0700 Subject: [PATCH 28/29] add penalty for colliding, change image resolution, update saving iteration data --- omnigibson/reward_functions/grasp_reward.py | 6 +++ omnigibson/tasks/grasp_task.py | 11 ++--- tests/grasp_RL_script.py | 46 +++++++++++++++++---- tests/test_reward_function.py | 1 + 4 files changed, 48 insertions(+), 16 deletions(-) diff --git a/omnigibson/reward_functions/grasp_reward.py b/omnigibson/reward_functions/grasp_reward.py index c0e1c26c2..0a3387779 100644 --- a/omnigibson/reward_functions/grasp_reward.py +++ b/omnigibson/reward_functions/grasp_reward.py @@ -1,5 +1,6 @@ import math from omnigibson.reward_functions.reward_function_base import BaseRewardFunction +from omnigibson.utils.motion_planning_utils import detect_robot_collision_in_sim import omnigibson.utils.transform_utils as T @@ -62,5 +63,10 @@ def _step(self, task, env, action): dist_reward = math.exp(-dist) * self.dist_coeff reward = dist_reward + self.grasp_reward + # Overwrite reward if robot is in collision + # The one step where it grasps the object, it is in collision and this triggers + if detect_robot_collision_in_sim(robot, ignore_obj_in_hand=True): + reward = -10 + self.prev_grasping = current_grasping return reward, {} diff --git a/omnigibson/tasks/grasp_task.py b/omnigibson/tasks/grasp_task.py index 5be71a108..31a79205a 100644 --- a/omnigibson/tasks/grasp_task.py +++ b/omnigibson/tasks/grasp_task.py @@ -54,7 +54,7 @@ def _create_reward_functions(self): def _reset_agent(self, env): # from IPython import embed; embed() - if hasattr(self, 'primitive_controller'): + if hasattr(env, '_primitive_controller'): robot = env.robots[0] # Randomize the robots joint positions joint_control_idx = np.concatenate([robot.trunk_control_idx, robot.arm_control_idx[robot.default_arm]]) @@ -62,7 +62,7 @@ def _reset_agent(self, env): initial_joint_pos = np.array(robot.get_joint_positions()[joint_combined_idx]) control_idx_in_joint_pos = np.where(np.in1d(joint_combined_idx, joint_control_idx))[0] - with UndoableContext(self.primitive_controller.robot, self.primitive_controller.robot_copy, "original") as context: + with UndoableContext(env._primitive_controller.robot, env._primitive_controller.robot_copy, "original") as context: for _ in range(MAX_JOINT_RANDOMIZATION_ATTEMPTS): joint_pos, joint_control_idx = self._get_random_joint_position(robot) initial_joint_pos[control_idx_in_joint_pos] = joint_pos @@ -75,8 +75,8 @@ def _reset_agent(self, env): obj = env.scene.object_registry("name", self.obj_name) grasp_poses = get_grasp_poses_for_object_sticky(obj) grasp_pose, _ = random.choice(grasp_poses) - sampled_pose_2d = self.primitive_controller._sample_pose_near_object(obj, pose_on_obj=grasp_pose) - robot_pose = self.primitive_controller._get_robot_pose_from_2d_pose(sampled_pose_2d) + sampled_pose_2d = env._primitive_controller._sample_pose_near_object(obj, pose_on_obj=grasp_pose) + robot_pose = env._primitive_controller._get_robot_pose_from_2d_pose(sampled_pose_2d) robot.set_position_orientation(*robot_pose) # Overwrite reset by only removeing reset scene @@ -119,9 +119,6 @@ def _get_obs(self, env): def _load_non_low_dim_observation_space(self): # No non-low dim observations so we return an empty dict return dict() - - def add_primitive_controller(self, controller): - self.primitive_controller = controller @classproperty def valid_scene_types(cls): diff --git a/tests/grasp_RL_script.py b/tests/grasp_RL_script.py index bb22052f3..577d163fc 100644 --- a/tests/grasp_RL_script.py +++ b/tests/grasp_RL_script.py @@ -1,10 +1,13 @@ from datetime import datetime import math +import uuid import numpy as np import matplotlib.pyplot as plt import omnigibson as og from omnigibson.macros import gm from omnigibson.action_primitives.starter_semantic_action_primitives import StarterSemanticActionPrimitives, StarterSemanticActionPrimitiveSet +from omnigibson.sensors.scan_sensor import ScanSensor +from omnigibson.sensors.vision_sensor import VisionSensor import omnigibson.utils.transform_utils as T from omnigibson.objects.dataset_object import DatasetObject import h5py @@ -41,6 +44,7 @@ def reset_env(env, initial_poses): DIST_COEFF = 0.1 GRASP_REWARD = 0.3 RL_ITERATIONS = 2 +h5py.get_config().track_order = True cfg = { "scene": { @@ -60,6 +64,15 @@ def reset_env(env, initial_poses): "rigid_trunk": False, "default_arm_pose": "diagonal30", "default_trunk_offset": 0.365, + "sensor_config": { + "VisionSensor": { + "modalities": ["rgb", "depth"], + "sensor_kwargs": { + "image_width": 224, + "image_height": 224 + } + } + }, "controller_config": { "base": { "name": "JointController", @@ -136,6 +149,7 @@ def add(self, state, action, reward): self.states[k].append(state['robot0'][k]) self.actions.append(action) self.rewards.append(reward) + self.ids.append(self.episode_id) def reset(self): self.states = {} @@ -143,14 +157,29 @@ def reset(self): self.states[k] = [] self.actions = [] self.rewards = [] + self.ids = [] + self.episode_id = str(uuid.uuid4()) + + def _add_to_dataset(self, group, name, data): + if name in group: + dset_len = len(group[name]) + dset = group[name] + dset.resize(len(dset) + len(data), 0) + dset[dset_len:] = data + else: + if isinstance(data[0], np.ndarray): + group.create_dataset(name, data=data, maxshape=(None, *data[0].shape)) + else: + group.create_dataset(name, data=data, maxshape=(None,)) def save(self, group_name): h5file = h5py.File(self.filename, 'a') - group = h5file.create_group(group_name) + group = h5file[group_name] if group_name in h5file else h5file.create_group(group_name) for k in self.state_keys: - group.create_dataset(k[k.find(":") + 1:], data=np.array(self.states[k])) - group.create_dataset("actions", data=np.array(self.actions)) - group.create_dataset("rewards", data=np.array(self.rewards)) + self._add_to_dataset(group, k[k.find(":") + 1:], self.states[k]) + self._add_to_dataset(group, "actions", self.actions) + self._add_to_dataset(group, "rewards", self.rewards) + self._add_to_dataset(group, "ids", self.ids) h5file.close() # Create the environment @@ -158,8 +187,6 @@ def save(self, group_name): scene = env.scene robot = env.robots[0] og.sim.step() -# og.sim.stop() -# og.sim.play() # Place object in scene # obj = DatasetObject( @@ -174,12 +201,13 @@ def save(self, group_name): # env.scene.update_initial_state() controller = StarterSemanticActionPrimitives(None, scene, robot) -env.task.add_primitive_controller(controller) +env._primitive_controller = controller initial_poses = {} for o in env.scene.objects: initial_poses[o.name] = o.get_position_orientation() obj = env.scene.object_registry("name", "cologne") recorder = Recorder() +group_name = datetime.now().strftime('%Y-%m-%d_%H-%M-%S') for i in range(RL_ITERATIONS): try: @@ -194,5 +222,5 @@ def save(self, group_name): break except: print("Error in iteration: ", i) - -recorder.save(datetime.now().strftime('%Y-%m-%d_%H-%M-%S')) \ No newline at end of file + recorder.save(group_name) + recorder.reset() \ No newline at end of file diff --git a/tests/test_reward_function.py b/tests/test_reward_function.py index 7fd853ea6..6efb94720 100644 --- a/tests/test_reward_function.py +++ b/tests/test_reward_function.py @@ -139,6 +139,7 @@ def test_grasp_reward(): og.sim.step() ctrl_gen = controller.apply_ref(StarterSemanticActionPrimitiveSet.GRASP, obj) + rewards = [0] total_rewards = [0] From b60d606548f7e0a31078733030a1ddaa1b5c321a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cem=20G=C3=B6kmen?= <1408354+cgokmen@users.noreply.github.com> Date: Fri, 22 Sep 2023 16:29:00 -0700 Subject: [PATCH 29/29] Updated tests --- .../starter_semantic_action_primitives.py | 11 ++-- .../symbolic_semantic_action_primitives.py | 13 ++-- tests/test_symbolic_primitives.py | 62 +++++++++++-------- 3 files changed, 50 insertions(+), 36 deletions(-) diff --git a/omnigibson/action_primitives/starter_semantic_action_primitives.py b/omnigibson/action_primitives/starter_semantic_action_primitives.py index 4f43b5240..57ef4a00a 100644 --- a/omnigibson/action_primitives/starter_semantic_action_primitives.py +++ b/omnigibson/action_primitives/starter_semantic_action_primitives.py @@ -1289,13 +1289,13 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): - 3-array: (x,y,z) Position in the world frame - 4-array: (x,y,z,w) Quaternion orientation in the world frame """ - if pose_on_obj is None: - pos_on_obj = self._sample_position_on_aabb_face(obj) - pose_on_obj = np.array([pos_on_obj, [0, 0, 0, 1]]) - with UndoableContext(self.robot, self.robot_copy, "simplified") as context: obj_rooms = obj.in_rooms if obj.in_rooms else [self.scene._seg_map.get_room_instance_by_point(pose_on_obj[0][:2])] for _ in range(MAX_ATTEMPTS_FOR_SAMPLING_POSE_NEAR_OBJECT): + if pose_on_obj is None: + pos_on_obj = self._sample_position_on_aabb_face(obj) + pose_on_obj = [pos_on_obj, np.array([0, 0, 0, 1])] + distance = np.random.uniform(0.0, 1.0) yaw = np.random.uniform(-np.pi, np.pi) avg_arm_workspace_range = np.mean(self.robot.arm_workspace_range[self.arm]) @@ -1313,7 +1313,8 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): return pose_2d raise ActionPrimitiveError( - ActionPrimitiveError.Reason.SAMPLING_ERROR, "Could not find valid position near object." + ActionPrimitiveError.Reason.SAMPLING_ERROR, "Could not find valid position near object.", + {"target object": obj.name, "target pos": obj.get_position(), "pose on target": pose_on_obj} ) @staticmethod diff --git a/omnigibson/action_primitives/symbolic_semantic_action_primitives.py b/omnigibson/action_primitives/symbolic_semantic_action_primitives.py index 08b1acc40..c68458d98 100644 --- a/omnigibson/action_primitives/symbolic_semantic_action_primitives.py +++ b/omnigibson/action_primitives/symbolic_semantic_action_primitives.py @@ -782,14 +782,14 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): - 3-array: (x,y,z) Position in the world frame - 4-array: (x,y,z,w) Quaternion orientation in the world frame """ - if pose_on_obj is None: - pos_on_obj = self._sample_position_on_aabb_face(obj) - pose_on_obj = [pos_on_obj, np.array([0, 0, 0, 1])] - with UndoableContext(self.robot, self.robot_copy, "simplified") as context: obj_rooms = obj.in_rooms if obj.in_rooms else [self.scene._seg_map.get_room_instance_by_point(pose_on_obj[0][:2])] for _ in range(MAX_ATTEMPTS_FOR_SAMPLING_POSE_NEAR_OBJECT): - distance = np.random.uniform(0.0, 1.0) + if pose_on_obj is None: + pos_on_obj = self._sample_position_on_aabb_face(obj) + pose_on_obj = [pos_on_obj, np.array([0, 0, 0, 1])] + + distance = np.random.uniform(0.0, 3.0) yaw = np.random.uniform(-np.pi, np.pi) pose_2d = np.array( [pose_on_obj[0][0] + distance * np.cos(yaw), pose_on_obj[0][1] + distance * np.sin(yaw), yaw + np.pi] @@ -806,7 +806,8 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs): return pose_2d raise ActionPrimitiveError( - ActionPrimitiveError.Reason.SAMPLING_ERROR, "Could not find valid position near object." + ActionPrimitiveError.Reason.SAMPLING_ERROR, "Could not find valid position near object.", + {"target object": obj.name, "target pos": obj.get_position(), "pose on target": pose_on_obj} ) @staticmethod diff --git a/tests/test_symbolic_primitives.py b/tests/test_symbolic_primitives.py index 3d4c61822..c8f45f8a7 100644 --- a/tests/test_symbolic_primitives.py +++ b/tests/test_symbolic_primitives.py @@ -109,10 +109,17 @@ def start_env(): return env -@pytest.fixture -def env(): +@pytest.fixture(scope="module") +def shared_env(): + """Load the environment just once using module scope.""" return start_env() +@pytest.fixture(scope="function") +def env(shared_env): + """Reset the environment before each test function.""" + og.sim.scene.reset() + return shared_env + @pytest.fixture def prim_gen(env): scene = env.scene @@ -133,11 +140,12 @@ def sink(env): @pytest.fixture def pan(env): - return next(iter(env.scene.object_registry("category", "pan"))) + return next(iter(env.scene.object_registry("category", "frying_pan"))) @pytest.fixture def steak(env): - return next(iter(env.scene.object_registry("category", "steak"))) + # TODO: Why apple? Use steak. + return next(iter(env.scene.object_registry("category", "apple"))) @pytest.fixture def sponge(env): @@ -150,18 +158,18 @@ def knife(env): # def test_navigate(): # pass -# def test_open(env, prim_gen, fridge): -# assert not fridge.states[object_states.Open].get_value() -# for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.OPEN, fridge): -# env.step(action) -# assert fridge.states[object_states.Open].get_value() +def test_open(env, prim_gen, fridge): + assert not fridge.states[object_states.Open].get_value() + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.OPEN, fridge): + env.step(action) + assert fridge.states[object_states.Open].get_value() -# def test_close(env, prim_gen, fridge): -# fridge.states[object_states.Open].set_value(True) -# assert fridge.states[object_states.Open].get_value() -# for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.CLOSE, fridge): -# env.step(action) -# assert not fridge.states[object_states.Open].get_value() +def test_close(env, prim_gen, fridge): + fridge.states[object_states.Open].set_value(True) + assert fridge.states[object_states.Open].get_value() + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.CLOSE, fridge): + env.step(action) + assert not fridge.states[object_states.Open].get_value() def test_place_inside(env, prim_gen, steak, fridge): assert not steak.states[object_states.Inside].get_value(fridge) @@ -174,16 +182,20 @@ def test_place_inside(env, prim_gen, steak, fridge): env.step(action) assert steak.states[object_states.Inside].get_value(fridge) -# def test_place_ontop(env, prim_gen, steak, pan): -# assert not steak.states[object_states.OnTop].get_value(pan) -# for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.GRASP, steak): -# env.step(action) -# for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.PLACE_ON_TOP, pan): -# env.step(action) -# assert steak.states[object_states.OnTop].get_value(pan) +def test_place_ontop(env, prim_gen, steak, pan): + assert not steak.states[object_states.OnTop].get_value(pan) + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.GRASP, steak): + env.step(action) + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.PLACE_ON_TOP, pan): + env.step(action) + assert steak.states[object_states.OnTop].get_value(pan) + +def test_toggle_on(env, prim_gen, stove): + assert not stove.states[object_states.ToggledOn].get_value() + for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.TOGGLE_ON, stove): + env.step(action) + assert stove.states[object_states.ToggledOn].get_value() -# def test_toggle_on(): -# pass # def test_soak_under(): # pass @@ -194,6 +206,7 @@ def test_place_inside(env, prim_gen, steak, fridge): # def test_wipe(): # pass +@pytest.mark.skip(reason="A bug with object addition causes the robot to crash after slicing.") def test_cut(env, prim_gen, steak, knife, countertop): # assert not steak.states[object_states.Cut].get_value(knife) print("Grasping knife") @@ -207,7 +220,6 @@ def test_cut(env, prim_gen, steak, knife, countertop): print("Putting knife back on countertop") for action in prim_gen.apply_ref(SymbolicSemanticActionPrimitiveSet.PLACE_ON_TOP, countertop): env.step(action) - # assert steak.states[object_states.Sliced].get_value(knife) # def test_place_near_heating_element(): # pass