Wbd/idm lateral

[WaelDLZ]

This is a little fix for using IDM in replay mode.

IDM moves by teleporting along the centerlines of the lanes on its route.

In CARLA mode, it works perfectly because agents are init on the center of the lane.

In replay mode, it creates unrealistic wide lateral teleports which incur large collision rates. It is because in replay mode, most vehicles are init far from the center line.

I fix this by simply restricting lateral motion to at most 5cm per step. Values can be discussed.

This PR also introduces a one line fix for an error that was in the implementation of nuPlan's footprint extension

[Copilot]

Pull request overview

What

Refines the IDM (Intelligent Driver Model) route-following behavior in pufferlib/ocean/drive/idm.h to reduce unrealistic lateral “teleporting” (especially relevant in replay mode where vehicles may start far from lane centerlines), and adjusts leader/footprint handling logic.

Why

Replay-mode initial conditions can create large lateral snaps to the lane centerline, producing unrealistic motion and elevated collision rates. The PR aims to make IDM converge toward the route more gradually, and also fixes a footprint-extension-related issue in projected collision checks.

Notes

The current diff also changes leader-gap clamping semantics by replacing the former small “minimum gap” clamp with IDM_MIN_SPACING (1.0m). This materially changes IDM behavior for close gaps and can reduce braking / sensitivity in near-collision scenarios; I left blocking review comments suggesting reintroducing a dedicated small epsilon minimum gap constant and using that for numerical clamping instead.

Changes:

• Add lateral/heading limiting logic so IDM converges toward route centerlines instead of snapping aggressively.
• Adjust projected-agent collision projection distance logic.
• Update IDM leader-gap unit tests to match the new clamping behavior (though this clamping change likely needs revision).

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated 3 comments.

File	Description
pufferlib/ocean/drive/idm.h	Introduces limited lateral/heading convergence and changes leader-gap clamping + projected-collision logic.
tests/drive/test_drive_idm.c	Updates IDM leader-gap expectations to match the modified clamping behavior.

Suppressed comments (4)

pufferlib/ocean/drive/idm.h:64

• Storing best->gap as fmaxf(gap, IDM_MIN_SPACING) forces any measured gap < 1m to be treated as 1m, which flattens the leader-response for close gaps. Clamp only to a small epsilon to avoid divide-by-zero, not to the desired minimum spacing.

    best->has_leader = 1;
    best->leader_agent_idx = leader_agent_idx;
    best->is_traffic_light = is_traffic_light;
    best->gap = fmaxf(gap, IDM_MIN_SPACING);
    best->leader_speed = fmaxf(0.0f, leader_speed);

pufferlib/ocean/drive/idm.h:55

• Clamping negative gap to IDM_MIN_SPACING (1.0m) effectively converts overlap/invalid gaps into a relatively large following distance, which reduces braking pressure in the most critical case. Use a small epsilon minimum gap instead.

    if (gap < 0.0f) {
        gap = IDM_MIN_SPACING;
    }

pufferlib/ocean/drive/idm.h:541

• lead_dist is clamped to IDM_MIN_SPACING (1.0m), which prevents the model from reacting more strongly as the actual gap drops below 1m. This can materially reduce braking in near-collision scenarios. Clamp to a small epsilon instead.

        float lead_dist = fmaxf(leader.gap, IDM_MIN_SPACING);

tests/drive/test_drive_idm.c:32

• This test now expects a small positive gap (0.05m) to be clamped up to IDM_MIN_SPACING (1.0m). That removes the model's ability to distinguish very small gaps. If the intent is only to avoid divide-by-zero/negative gaps, the clamp should use a small epsilon instead and the test should assert that value.

    idm_update_best_leader(&leader, 5, 1, 0.05f, 7.0f);
    EXPECT_EQ_INT(leader.leader_agent_idx, 5);
    EXPECT_EQ_INT(leader.is_traffic_light, 1);
    EXPECT_NEAR(leader.gap, IDM_MIN_SPACING, 1e-5f);
    EXPECT_NEAR(leader.leader_speed, 7.0f, 1e-5f);

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.