Remove padding agents using interface entities (#103)

* Init remove padding agents

* Testing

* Refactor out the controlled state

* Fix the tests

* Use by reference

* Temp fix for map export

* cleanup

* Fix merge issues

* Fix merge issues

* make consts 6000 to pass tests

* Use init counts for BVH

* Remove unused function

* Pass all tests

* Set controlledstate

* Zero out things

* Zero padded agents out

* Add a test.py for easy debugging

* Better check for map drawing.

* Set agents to 128

* Cycle through files in test.py

* Rename InterfaceEntity to AgentInterfaceEntity

* Only print dones

* Remove debug checks

* Separete out the observation systems

* Sort interfaces

* remove debug script

* Remove debug statements

* Minor improvements

* Info tensor interface matches main

* Correctly export response type and trajectory

pygpudrive/env/viz.py

@@ -289,7 +289,7 @@ def draw_map(self, surf, map_info, world_render_idx=0):
         """Draw static map elements."""
         for idx, map_obj in enumerate(map_info):
 
-            if map_obj[-1] == float(gpudrive.EntityType.Padding):
+            if map_obj[-1] == float(gpudrive.EntityType.Padding) or map_obj[-1] == float(gpudrive.EntityType._None):
                 continue
 
             elif map_obj[-1] <= float(gpudrive.EntityType.RoadLane):

src/consts.hpp

@@ -3,7 +3,7 @@
 #include <limits>
 #include <madrona/math.hpp>
 #include <madrona/types.hpp>
-
+ 
 namespace gpudrive {
 
 namespace consts {

src/dynamics.hpp

@@ -8,7 +8,7 @@ using namespace madrona::math;
 namespace gpudrive
 {
 
-    inline void forwardKinematics(Action &action, VehicleSize &size, Rotation &rotation, Position &position, Velocity &velocity)
+    inline void forwardKinematics(const Action &action, VehicleSize &size, Rotation &rotation, Position &position, Velocity &velocity)
     {
         const float maxSpeed{std::numeric_limits<float>::max()};
         const float dt{0.1};

src/headless.cpp

@@ -97,17 +97,17 @@ int main(int argc, char *argv[])
         // printf("Reward\n");
         // rewardPrinter.print();
 
-        // printf("Done\n");
-        // donePrinter.print();
+        printf("Done\n");
+        donePrinter.print();
 
         // printf("Controlled State\n");
         // controlledStatePrinter.print();
 
-        printf("Agent Map Obs\n");
-        agent_map_obs_printer.print();
+        // printf("Agent Map Obs\n");
+        // agent_map_obs_printer.print();
 
-        printf("Info\n");
-        info_printer.print();
+        // printf("Info\n");
+        // info_printer.print();
     };
 
     auto worldToShape =
@@ -133,6 +133,7 @@ int main(int argc, char *argv[])
     }
     const auto end = std::chrono::steady_clock::now();
     const std::chrono::duration<double> elapsed = end - start;
+    printObs();
 
     float fps = (double)num_steps * (double)num_worlds / elapsed.count();
     printf("FPS %f\n", fps);

src/knn.hpp

@@ -60,8 +60,9 @@ bool isObservationsValid(gpudrive::Engine &ctx,
   sortedObservations.reserve(roadCount);
 
   for (madrona::CountT roadIdx = 0; roadIdx < roadCount; ++roadIdx) {
+    auto &road_iface = ctx.get<gpudrive::RoadInterfaceEntity>(ctx.data().roads[roadIdx]).e;
     const auto &currentObservation =
-        ctx.get<gpudrive::MapObservation>(ctx.data().roads[roadIdx]);
+        ctx.get<gpudrive::MapObservation>(road_iface);
     sortedObservations.emplace_back(relativeObservation(
         currentObservation, referenceRotation, referencePosition));
   }

src/mgr.cpp

@@ -676,7 +676,7 @@ Tensor Manager::agentMapObservationsTensor() const
                                TensorElementType::Float32,
                                {
                                    impl_->numWorlds,
-				   consts::kMaxAgentCount,
+				                    consts::kMaxAgentCount,
                                    consts::kMaxAgentMapObservationsCount,
                                    AgentMapObservationExportSize,
                                });