diff --git a/dotbot/examples/charging_station.py b/dotbot/examples/charging_station.py index a8a4306d..da9acf56 100644 --- a/dotbot/examples/charging_station.py +++ b/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) diff --git a/dotbot/examples/orca.py b/dotbot/examples/orca.py index 0528b6df..8c176526 100644 --- a/dotbot/examples/orca.py +++ b/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) diff --git a/dotbot/tests/test_experiment_charging_station.py b/dotbot/tests/test_experiment_charging_station.py index f00e2ce1..ee500320 100644 --- a/dotbot/tests/test_experiment_charging_station.py +++ b/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)