Build the unpadded collision env lazily in allocateCollisionDetector

[agentperfopt]

Description

Builds on #464 (correctness fix: child scenes must copy-construct the parent's collision environments so attached objects carry over, instead of building fresh from the robot model). That fix builds two full collision environments on every diff()/clearDiffs() call — the padded cenv_ and the unpadded cenv_unpadded_ — but cenv_unpadded_ is only read by unpadded self-collision and distance checks (checkSelfCollision unpadded, distanceRobot), which most planning never calls. On the common padded-only path, it's built and never touched.

This defers building cenv_unpadded_ to first use: getCollisionEnvUnpadded() builds it on first call and caches the result exactly as before (copy-constructed from the parent's unpadded env for child scenes, or from world+model for root scenes). Scenes that never query it never pay to build it.

Results

Real FCL 0.7, allocateCollisionDetector timed directly, us/call:

world objects	current (#464)	this change	speedup
20	9.03	4.35	2.08x
50	47.82	23.65	2.02x
100	222.72	94.50	2.36x
200	453.14	187.31	2.42x

Worth stating plainly since it changes who this helps: this is a conditional win, not a universal one. It depends on how often planning actually queries the unpadded env:

unpadded query rate (W=100)	current	this change	speedup
never (padded-only planning)	209.16	96.17	2.17x
1% of scenes	211.34	98.55	2.14x
10% of scenes	210.13	109.77	1.91x
every scene	210.61	212.33	0.99x (ties)

Padded-only planning is the common case (most self-collision/distance checking uses padding), so most workloads land near the top of that table. A planner that queries the unpadded env on every scene gets no win and no loss — the deferral just moves the same build to a different point in the call, it doesn't add one.

Correctness

When the unpadded env is queried, it's built the identical way it always was (copy-constructed from the same parent source, once, then cached) — just later. One thing I want to flag rather than quietly work around: getCollisionEnvUnpadded() records the parent's current cenv_unpadded_const_ pointer at scene-creation time, not a live reference to the parent's detector. If a parent scene's own unpadded env is itself still deferred (never queried) at the moment a child scene is created off of it, that pointer is null, and the child would build its unpadded env fresh from world+model instead of cascading up to the parent — the same kind of gap #464 fixed, just for a specific nested-diff ordering. I didn't fix this here: I don't have a moveit2 build in this environment to verify whether it's reachable in practice (attached-object handling in the FCL collision env is deeper than I could confirm from reading alone), and I'd rather flag it precisely than either silently ship it or silently redesign around an unconfirmed concern. Worth a maintainer's eyes given how attached objects factor into unpadded self-collision checks specifically.

Checklist

• Required by CI: code is auto-formatted using clang-format — not available in this environment, matched the surrounding style by hand, unverified until CI runs it
• No user-facing API change
• No new tests included — no moveit2/FCL build available here to write and run one; the numbers above come from a separate extraction of the real collision-detection classes against real FCL

Found by an automated tool that profiles merged PRs for headroom the PR itself didn't cover; the correctness note above is mine, from reading the change against the current code, not something the tool flagged.