fix:fixes double dummy and collision checks. Addresses #44

tud-airlab · Jun 8, 2023 · 1bfbafa · 1bfbafa
1 parent 3d5c250
commit 1bfbafa
Show file tree

Hide file tree

Showing 6 changed files with 523 additions and 19 deletions.
diff --git a/examples/boxer_push_client.py b/examples/boxer_push_client.py
@@ -69,7 +69,7 @@ def compute_cost(self, sim):
 
         # Collision avoidance
         xy_contatcs = torch.sum(torch.abs(torch.cat((sim.net_cf[:, 0].unsqueeze(1), sim.net_cf[:, 1].unsqueeze(1)), 1)),1)
-        coll = torch.sum(xy_contatcs.reshape([sim.num_envs, int(xy_contatcs.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - self.obst_number):sim.num_bodies], 1)
+        coll = torch.sum(xy_contatcs.reshape([sim.num_envs, int(xy_contatcs.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - 1 - self.obst_number):sim.num_bodies], 1)
 
         total_cost = (
             self.w_robot_to_block_pos * robot_to_block_dist

diff --git a/examples/heijn_push_client.py b/examples/heijn_push_client.py
@@ -81,7 +81,7 @@ def compute_cost(self, sim):
 
         # Collision avoidance
         xy_contatcs = torch.sum(torch.abs(torch.cat((sim.net_cf[:, 0].unsqueeze(1), sim.net_cf[:, 1].unsqueeze(1)), 1)),1)
-        coll = torch.sum(xy_contatcs.reshape([sim.num_envs, int(xy_contatcs.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - self.obst_number):sim.num_bodies], 1)
+        coll = torch.sum(xy_contatcs.reshape([sim.num_envs, int(xy_contatcs.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - 1 - self.obst_number):sim.num_bodies], 1)
 
         total_cost = (
             self.w_robot_to_block_pos * robot_to_block_dist

diff --git a/examples/omnipanda_isaacgym_client.py b/examples/omnipanda_isaacgym_client.py
@@ -58,7 +58,7 @@ def compute_cost(self, sim):
 
          # Collision avoidance
         contact_f = torch.sum(torch.abs(torch.cat((sim.net_cf[:, 0].unsqueeze(1), sim.net_cf[:, 1].unsqueeze(1), sim.net_cf[:, 2].unsqueeze(1)), 1)),1)
-        coll = torch.sum(contact_f.reshape([sim.num_envs, int(contact_f.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - self.obst_number):sim.num_bodies], 1)
+        coll = torch.sum(contact_f.reshape([sim.num_envs, int(contact_f.size(dim=0)/sim.num_envs)])[:, (sim.num_bodies - 1 - self.obst_number):sim.num_bodies], 1) # Consider obstacles
 
         total_cost = (
             self.w_robot_to_block_pos * robot_to_block_dist

diff --git a/mppiisaac/planner/isaacgym_wrapper.py b/mppiisaac/planner/isaacgym_wrapper.py
@@ -103,7 +103,7 @@ def __init__(
         if viewer:
             self.cfg.viewer = viewer
         self.num_envs = num_envs
-
+        self.restarted = 1
         self.start_sim()
 
     def start_sim(self):
@@ -122,20 +122,23 @@ def start_sim(self):
         self.add_ground_plane()
 
         # Always add dummy obst at the end
-        self.env_cfg.append(
-           ActorWrapper(
-               **{
-                   "type": "sphere",
-                   "name": "dummy",
-                   "handle": None,
-                   "size": [0.1],
-                   "fixed": True,
-                   "init_pos": [0, 0, -10],
-                   "collision": False
-               }
-           )
-        )
-
+        if self.restarted == 2:
+            self.env_cfg.append(
+            ActorWrapper(
+                **{
+                    "type": "sphere",
+                    "name": "dummy",
+                    "handle": None,
+                    "size": [0.1],
+                    "fixed": True,
+                    "init_pos": [0, 0, -10],
+                    "collision": False
+                }
+            )
+            )
+            self.restarted += 1
+        else:
+            self.restarted += 1
 
         # Load / create assets for all actors in the envs
         self.env_actor_assets = []
@@ -220,6 +223,8 @@ def robot_velocities(self):
     def stop_sim(self):
         if self.viewer:
             self.gym.destroy_viewer(self.viewer)
+        for env_idx in range(self.num_envs):
+            self.gym.destroy_env(self.envs[env_idx])
         self.gym.destroy_sim(self.sim)
 
     def add_to_envs(self, additions):