Merge branch 'action-primitives' into action-primitives-moma

.github/workflows/build-push-containers.yml

@@ -7,11 +7,15 @@ on:
     branches:
       - 'main'
       - 'og-develop'
+      - 'action-primitives'
 
 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
@@ -51,17 +55,24 @@ jobs:
       -
         name: Build and push dev image
         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 }}
           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 && cat docker/prod.Dockerfile
       -
         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 }}

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 \

docker/prod.Dockerfile

@@ -1,4 +1,4 @@
-FROM stanfordvl/omnigibson-dev
+FROM stanfordvl/omnigibson-dev:latest
 
 # Copy over omnigibson source
 ADD . /omnigibson-src

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 \

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -991,6 +991,9 @@ def _move_hand_direct_joint(self, joint_pos, control_idx, stop_on_contact=False,
 
         action = self._empty_action()
         controller_name = "arm_{}".format(self.arm)
+
+        action[self.robot.controller_action_idx[controller_name]] = joint_pos
+        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]
@@ -999,12 +1002,17 @@ 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"
+                    )
 
             if use_delta:
                 action[self.robot.controller_action_idx[controller_name]] = self._compute_delta_command(controller_name, diff_joint_pos)
-            else:
-                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 = self.robot.get_eef_position(self.arm)
             yield self.with_context(action)
 
         if not ignore_failure:
@@ -1595,13 +1603,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])
@@ -1619,7 +1627,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

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()
@@ -772,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 = 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):
-                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]
@@ -796,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

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

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

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

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

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

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

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

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:
+    ScanSensor:
+      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

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

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

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)