fix: improve orca reliability, less deadlocks

dotbot/examples/charging_station.py

@@ -24,7 +24,7 @@
 
 # TODO: Measure these values for real dotbots
 BOT_RADIUS = 0.03  # Physical radius of a DotBot (unit), used for collision avoidance
-MAX_SPEED = 0.75  # Maximum allowed linear speed of a bot
+MAX_SPEED = 0.075  # Maximum allowed linear speed of a bot
 
 (QUEUE_HEAD_X, QUEUE_HEAD_Y) = (
     0.1,
@@ -137,7 +137,6 @@ async def send_to_goal(
                     position=Vec2(x=bot.lh2_position.x, y=bot.lh2_position.y),
                     velocity=Vec2(x=0, y=0),
                     radius=BOT_RADIUS,
-                    direction=bot.direction,
                     max_speed=MAX_SPEED,
                     preferred_velocity=preferred_vel(
                         dotbot=bot, goal=goals.get(bot.address)
@@ -156,8 +155,7 @@ async def send_to_goal(
             orca_vel = await compute_orca_velocity(
                 agent, neighbors=neighbors, params=params
             )
-            STEP_SCALE = 0.1
-            step = Vec2(x=orca_vel.x * STEP_SCALE, y=orca_vel.y * STEP_SCALE)
+            step = Vec2(x=orca_vel.x, y=orca_vel.y)
 
             # ---- CLAMP STEP TO GOAL DISTANCE ----
             goal = goals.get(agent.id)
@@ -297,7 +295,7 @@ async def compute_orca_velocity(
 
 
 async def main() -> None:
-    params = OrcaParams(time_horizon=DT)
+    params = OrcaParams(time_horizon=5 * DT, time_step=DT)
     url = os.getenv("DOTBOT_CONTROLLER_URL", "localhost")
     port = os.getenv("DOTBOT_CONTROLLER_PORT", "8000")
     use_https = os.getenv("DOTBOT_CONTROLLER_USE_HTTPS", False)

dotbot/examples/orca.py

@@ -36,7 +36,6 @@ class Agent:
     id: str
     position: Vec2
     velocity: Vec2
-    direction: float
     radius: float
     max_speed: float
     preferred_velocity: Vec2
@@ -45,6 +44,7 @@ class Agent:
 @dataclass
 class OrcaParams:
     time_horizon: float
+    time_step: float
 
 
 def cross(a: Vec2, b: Vec2) -> float:
@@ -92,8 +92,6 @@ def compute_orca_line_pair(A: Agent, B: Agent, params: OrcaParams) -> OrcaLine:
             line.direction = perp(line.normal)
 
         else:
-            dist = math.sqrt(dist_sq)
-            rel_unit = mul(rel_pos, 1.0 / dist)
             leg = math.sqrt(dist_sq - combined_radius_sq)
 
             left_leg = Vec2(
@@ -106,9 +104,9 @@ def compute_orca_line_pair(A: Agent, B: Agent, params: OrcaParams) -> OrcaLine:
                 (-rel_pos.x * combined_radius + rel_pos.y * leg) / dist_sq,
             )
 
-            side = math.copysign(1, cross(rel_vel, rel_unit))
+            side = cross(rel_pos, w)
 
-            if side >= 0:
+            if side > 0:
                 leg_dir = left_leg
             else:
                 leg_dir = right_leg
@@ -123,15 +121,16 @@ def compute_orca_line_pair(A: Agent, B: Agent, params: OrcaParams) -> OrcaLine:
 
     else:
         # CASE 2: Already colliding
-        dist = math.sqrt(dist_sq)
-        rel_unit = mul(rel_pos, 1.0 / dist) if dist > 0 else vec(1, 0)
+        inv_dt = 0.5 / params.time_step
+        w = sub(rel_vel, mul(rel_pos, inv_dt))
+        w_len = vec2_length(w)
+        unit_w = mul(w, 1.0 / w_len) if w_len > 0 else vec(1, 0)
 
-        penetration = combined_radius - dist
-        u = mul(rel_unit, penetration)
+        u = mul(unit_w, combined_radius * inv_dt - w_len)
 
         line.point = add(A.velocity, mul(u, 0.5))
-        line.normal = rel_unit
-        line.direction = perp(line.normal)
+        line.normal = unit_w
+        line.direction = perp(unit_w)
 
     line.direction = normalize(line.direction)
     line.normal = normalize(line.normal)

dotbot/tests/test_experiment_charging_station.py

@@ -151,7 +151,7 @@ async def test_queue_robots_converges_to_queue_positions(_):
     ]
 
     client = FakeRestClient(bots)
-    params = OrcaParams(time_horizon=DT)
+    params = OrcaParams(time_horizon=5 * DT, time_step=DT)
 
     await queue_robots(client, bots, params)
 
@@ -184,7 +184,7 @@ async def test_charge_robots_moves_all_bots_to_parking(_):
     ]
 
     client = FakeRestClient(bots)
-    params = OrcaParams(time_horizon=DT)
+    params = OrcaParams(time_horizon=5 * DT, time_step=DT)
 
     await charge_robots(client, params)