fix[urdfenvs]: Assumes sphere type for obstacles, as the type cannot …

…be wrapped as a observation. Wait for fix in urdfenvs.
tud-airlab · May 10, 2023 · 5e603c5 · 5e603c5
1 parent 2b31076
commit 5e603c5
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 14 deletions.
diff --git a/examples/panda_robot_with_obstacles.py b/examples/panda_robot_with_obstacles.py
@@ -98,7 +98,7 @@ def initalize_environment(cfg):
     # sense both
     sensor = FullSensor(
         goal_mask=["position"],
-        obstacle_mask=["position", "velocity", "type", "size"],
+        obstacle_mask=["position", "velocity", "size"],
         variance=0.0,
     )
     env.add_sensor(sensor, [0])

diff --git a/examples/point_robot_with_obstacle.py b/examples/point_robot_with_obstacle.py
@@ -81,17 +81,18 @@ def initalize_environment(cfg) -> UrdfEnv:
     sphereObst = SphereObstacle(name="simpleSphere", content_dict=obst1Dict)
     env.add_obstacle(sphereObst)
 
-    obst2Dict = {
-        "type": "box",
-        "geometry": {
-            "position": [1.0, 2.0, 0.0],
-            "width": 0.3,
-            "height": 0.2,
-            "length": 1.0,
-        },
-    }
-    boxObst = BoxObstacle(name="simpleBox", content_dict=obst2Dict)
-    env.add_obstacle(boxObst)
+    # TODO: Allow for non-sphere obstacles. Wait for update of urdfenvs.
+    # obst2Dict = {
+        # "type": "box",
+        # "geometry": {
+            # "position": [1.0, 2.0, 0.0],
+            # "width": 0.3,
+            # "height": 0.2,
+            # "length": 1.0,
+        # },
+    # }
+    # boxObst = BoxObstacle(name="simpleBox", content_dict=obst2Dict)
+    # env.add_obstacle(boxObst)
     goal_dict = {
         "weight": 1.0,
         "is_primary_goal": True,
@@ -108,7 +109,7 @@ def initalize_environment(cfg) -> UrdfEnv:
     # sense both
     sensor = FullSensor(
         goal_mask=["position"],
-        obstacle_mask=["position", "velocity", "type", "size"],
+        obstacle_mask=["position", "velocity", "size"],
         variance=0.0,
     )
     env.add_sensor(sensor, [0])

diff --git a/mppiisaac/planner/isaacgym_wrapper.py b/mppiisaac/planner/isaacgym_wrapper.py
@@ -488,7 +488,7 @@ def update_root_state_tensor_by_obstacles(self, obstacles):
         for i, obst in enumerate(list(obstacles.values())):
             pos = obst['position']
             vel = obst['velocity']
-            o_type = obst['type']
+            o_type = 'sphere'
             o_size = obst['size']
             name = f"{o_type}{i}"
             try: