AOXRRTConnect: fix path starting at the goal, and unbounded growTree loop - #1471

Fixes #1420.

The reported symptom

AORRTC on a RealVectorStateSpace returns a zero-length path whose states are all
the goal, and logs Zero-length path found. May have a common start/goal.
repro_1420.cpp (attached, OMPL-only) reproduces it deterministically:

dim=2  goals=1  obstacle=no
  RRTConnect (reference)   solved=1   0 ms  states=6  len=2.8203  |path[0]-start|=0.0000
  AORRTC                   solved=1   0 ms  states=2  len=0.0000  |path[0]-start|=2.5456   <-- wrong

The trigger is start and goal being directly connectable — with an obstacle between
them AORRTC behaves correctly. Reproduced at dim 2 and 6, with 1 and 3 goal states.

Root cause (B): tgi.xmotion is used for two different things

AOXRRTConnect::solve() presets gs = REACHED in the "straight line check" so the
first growTree on the start tree is skipped. But on that same pass tgi.xmotion was
assigned the goal tree root when tGoal_ was seeded:

if (tGoal_->size() == 0) { tgi.xmotion = motion; }   // motion is the *goal* root
...
Motion *addedMotion = tgi.xmotion;                    // expected: node just added to `tree`
...
Motion *startMotion = tgi.start ? tgi.xmotion : addedMotion;

tgi.start has just been flipped to false, so startMotion = addedMotion = the goal
root (parent == nullptr, root == goal). Both reconstructed chains then live inside
the goal tree, and isStartGoalPairValid(goal_root, goal_root) still returns true, so
the planner reports EXACT_SOLUTION.

The fix remembers the motion created from pis_.nextStart() and uses it as
addedMotion on that pass.

It also guards the "step back to avoid a duplicate state" branch: when both
startMotion and goalMotion are roots there is no duplicate to remove, and the
current code sets goalMotion = nullptr, which the following
connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state) dereferences.

Root cause (A): while (gsc == ADVANCED) has no termination check

while (gsc == ADVANCED)
    gsc = growTree(otherTree, tgi, cmotion);

No PlannerTerminationCondition, no iteration bound. A single solve() call can run
for minutes under a sub-second budget.

⚠ The two are coupled — fixing (B) alone makes things worse

AORRTC::solve()'s outer do { ... } while (!ptc) has exactly one early exit:

if (bestCost_.value() == 0) { OMPL_ERROR("Zero-length path found..."); return INVALID_GOAL; }

The zero-length path produced by (B) is therefore the only thing that currently stops
(A). We first fixed (B) alone, and separately tried disabling AORRTC's internal
simplifySolution (where #1420 currently suspects the problem) — both turned "wrong
path" into "hangs indefinitely" on a 0.4 s budget. That is why this PR fixes both.

The coupling is visible in the repro output: before the fix the obstacle=no rows
return in 0 ms (hitting the zero-length early return); after the fix they take the
full 400 ms budget, as an anytime planner should. That 400 ms is exactly the work
that the early return was skipping.

Validation

• repro_1420.cpp: 8/8 configurations correct after the patch (|path[0]-start| = 0,
  and obstacle=no now returns the straight line, len = the start–goal distance).
• On a 6-DoF manipulator benchmark (289 collision spheres, 1332–3744 obstacle spheres,
  500 ms budget, 4 parallel AORRTC instances): 160/160 queries solved, path length
  p50 2.037 rad, smoothness p50 0.4825 — unchanged from the pre-fix quality on the
  queries that did succeed, so the patch does not trade quality for correctness.
• Multi-goal (GoalStates with 2-8 states, goals from IK branches): 7/9 -> 9/9, and a
  larger run of 96 such queries: 96/96.

All of the above was run on main (68614a0) with this patch applied, and the repro was
additionally checked against 2.0.0 with and without the patch.

Note: I could not run clang-format (not available on this machine), so the new lines
follow the surrounding style by hand -- please reformat if it does not match.

⚠ Noticed but not fixed here

pdef_ accumulates three solution paths per outer round (one from
AOXRRTConnect::solve, two from AORRTC::solve) and is never cleared — we observed
three identical entries (9.968 9.968 9.968) for one query. That is unbounded growth
plus a quadratic sorted insert as the round count rises. Left alone because it does not
affect correctness and fixing it means touching AORRTC's solution bookkeeping.

repro_1420.cpp