fix collision between agent and bubble agent vehicle (#485)

* Bubble manager will choose BoxChassis for hijacking vehicles with non-dynamic actions

* added warnings when vehicles with AckermanChassis call set_pose after step

* Fixed vehicle generation to use boxchassis if action is not in smarts.dynamic_actions

scenarios/intersections/4lane_t/scenario.py

@@ -35,7 +35,8 @@
         zone=t.PositionalZone(pos=(100, 100), size=(20, 20)),
         margin=2,
         actor=t.SocialAgentActor(
-            name="keep-lane-agent", agent_locator=f"{agent_prefabs}:zoo-agent-v0",
+            name="motion-planner-agent",
+            agent_locator=f"{agent_prefabs}:motion-planner-agent-v0",
         ),
     ),
 ]

smarts/core/bubble_manager.py

@@ -443,13 +443,15 @@ def _hijack_social_vehicle_with_social_agent(
         else:
             agent_id = BubbleManager._make_social_agent_id(vehicle_id)
 
+        agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id)
         vehicle = sim.vehicle_index.switch_control_to_agent(
             sim,
             vehicle_id,
             agent_id,
             boid=bubble.is_boid,
             hijacking=True,
             recreate=False,
+            agent_interface=agent_interface,
         )
 
         for provider in sim.providers:

smarts/core/chassis.py

@@ -152,6 +152,7 @@ def __init__(
             self._bullet_body._bullet_id, -1, 0x0, 0x0
         )
         self.control(pose, speed)
+        self._client = bullet_client
 
     def control(self, pose: Pose, speed: float):
         self._pose = pose
@@ -164,9 +165,7 @@ def dimensions(self) -> BoundingBox:
 
     @property
     def contact_points(self) -> Sequence:
-        contact_points = _query_bullet_contact_points(
-            self._client, self._bullet_body, -1
-        )
+        contact_points = _query_bullet_contact_points(self._client, self.bullet_id, -1)
         return [
             ContactPoint(bullet_id=p[2], contact_point=p[5], contact_point_other=p[6])
             for p in contact_points

smarts/core/smarts.py

@@ -50,6 +50,7 @@
 from .sumo_road_network import SumoRoadNetwork
 from .sumo_traffic_simulation import SumoTrafficSimulation
 from .traffic_history_provider import TrafficHistoryProvider
+from smarts.core.chassis import AckermannChassis, BoxChassis
 from .trap_manager import TrapManager
 from .utils import pybullet
 from .utils.pybullet import bullet_client as bc
@@ -325,7 +326,6 @@ def reset(self, scenario: Scenario):
 
         # Visualization
         self._try_emit_visdom_obs(observations)
-
         if len(self._agent_manager.ego_agent_ids):
             while len(observations_for_ego) < 1:
                 observations_for_ego, _, _, _ = self.step({})
@@ -482,6 +482,10 @@ def observe_from(self, vehicle_ids):
     def road_stiffness(self):
         return self._bullet_client.getDynamicsInfo(self._ground_bullet_id, -1)[9]
 
+    @property
+    def dynamic_action_spaces(self):
+        return self._dynamic_action_spaces
+
     @property
     def traffic_sim(self) -> SumoTrafficSimulation:
         return self._traffic_sim
@@ -574,17 +578,13 @@ def _pybullet_provider_sync(self, provider_state: ProviderState):
                     agent_id
                 )
                 agent_action_space = agent_interface.action_space
-                if agent_action_space in self._dynamic_action_spaces:
-                    # This is a pybullet agent, we were the source of this vehicle state.
-                    # No need to make any changes
-                    continue
-                else:
+                if agent_action_space not in self._dynamic_action_spaces:
                     # This is not a pybullet agent, but it has an avatar in this world
                     # to make it's observations. Update the avatar to match the new
                     # state of this vehicle
                     pybullet_vehicle = self._vehicle_index.vehicle_by_id(vehicle_id)
-                    pybullet_vehicle.set_pose(vehicle.pose)
-                    pybullet_vehicle.set_speed(vehicle.speed)
+                    assert isinstance(pybullet_vehicle.chassis, BoxChassis)
+                    pybullet_vehicle.control(pose=vehicle.pose, speed=vehicle.speed)
             else:
                 # This vehicle is a social vehicle
                 if vehicle_id in self._vehicle_index.social_vehicle_ids():

smarts/core/trap_manager.py

@@ -255,8 +255,14 @@ def _hijack_vehicle(sim, vehicle_id, agent_id, mission):
         sim.vehicle_index.start_agent_observation(
             sim, vehicle_id, agent_id, agent_interface, planner
         )
+        agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id)
         vehicle = sim.vehicle_index.switch_control_to_agent(
-            sim, vehicle_id, agent_id, recreate=True, hijacking=False
+            sim,
+            vehicle_id,
+            agent_id,
+            recreate=True,
+            hijacking=False,
+            agent_interface=agent_interface,
         )
         return vehicle
 

smarts/core/vehicle.py

@@ -45,6 +45,7 @@
     WaypointsSensor,
 )
 from .utils.math import rotate_around_point
+from functools import lru_cache
 
 
 @dataclass(frozen=True)
@@ -148,6 +149,7 @@ def __init__(
         # TODO: Move this into the VehicleGeometry class
         self._np = self._build_model(pose, config, showbase)
         self._initialized = True
+        self._has_stepped = False
 
     def _assert_initialized(self):
         assert self._initialized, f"Vehicle({self.id}) is not initialized"
@@ -342,19 +344,31 @@ def build_agent_vehicle(
                 agent_interface.vehicle_type
             ]
 
-        vehicle = Vehicle(
-            id=vehicle_id,
-            pose=start_pose,
-            showbase=sim,
-            chassis=AckermannChassis(
+        chassis = None
+        # change this to dynamic_action_spaces later when pr merged
+        if agent_interface and agent_interface.action in sim.dynamic_action_spaces:
+            chassis = AckermannChassis(
                 pose=start_pose,
                 bullet_client=sim.bc,
                 vehicle_filepath=vehicle_filepath,
                 tire_parameters_filepath=tire_filepath,
                 friction_map=surface_patches,
                 controller_parameters=controller_parameters,
                 initial_speed=initial_speed,
-            ),
+            )
+        else:
+            chassis = BoxChassis(
+                pose=start_pose,
+                speed=initial_speed,
+                dimensions=chassis_dims,
+                bullet_client=sim.bc,
+            )
+
+        vehicle = Vehicle(
+            id=vehicle_id,
+            pose=start_pose,
+            showbase=sim,
+            chassis=chassis,
             color=vehicle_color,
         )
 
@@ -474,6 +488,7 @@ def attach_sensors_to_vehicle(sim, vehicle, agent_interface, mission_planner):
         )
 
     def step(self, current_simulation_time):
+        self._has_stepped = True
         self._chassis.step(current_simulation_time)
 
     def control(self, *args, **kwargs):
@@ -483,8 +498,17 @@ def sync_to_panda3d(self):
         pos, heading = self._chassis.pose.as_panda3d()
         self._np.setPosHpr(*pos, heading, 0, 0)
 
+    @lru_cache(maxsize=1)
+    def _warn_AckermannChassis_set_pose(self):
+        if self._has_stepped and isinstance(self._chassis, AckermannChassis):
+            logging.warning(
+                f"Agent `{self._id}` has called set pose after step."
+                "This may cause collision problems"
+            )
+
     # TODO: Merge this w/ speed setter as a set GCD call?
     def set_pose(self, pose: Pose):
+        self._warn_AckermannChassis_set_pose()
         self._chassis.set_pose(pose)
 
     def swap_chassis(self, chassis: Chassis):

smarts/core/vehicle_index.py

@@ -354,7 +354,14 @@ def start_agent_observation(
 
     @clear_cache
     def switch_control_to_agent(
-        self, sim, vehicle_id, agent_id, boid=False, hijacking=False, recreate=False
+        self,
+        sim,
+        vehicle_id,
+        agent_id,
+        boid=False,
+        hijacking=False,
+        recreate=False,
+        agent_interface=None,
     ):
         self._log.debug(f"Switching control of {agent_id} to {vehicle_id}")
 
@@ -368,8 +375,19 @@ def switch_control_to_agent(
             )
 
         vehicle = self._vehicles[vehicle_id]
-        ackermann_chassis = AckermannChassis(pose=vehicle.pose, bullet_client=sim.bc)
-        vehicle.swap_chassis(ackermann_chassis)
+        chassis = None
+        # change this to dynamic_action_spaces later when pr merged
+        if agent_interface and agent_interface.action in sim.dynamic_action_spaces:
+            chassis = AckermannChassis(pose=vehicle.pose, bullet_client=sim.bc)
+        else:
+            chassis = BoxChassis(
+                pose=vehicle.pose,
+                speed=vehicle.speed,
+                dimensions=vehicle.state.dimensions,
+                bullet_client=sim.bc,
+            )
+
+        vehicle.swap_chassis(chassis)
 
         v_index = self._controlled_by["vehicle_id"] == vehicle_id
         entity = _ControlEntity(*self._controlled_by[v_index][0])