Include the final goal edge in arm RRT* path selection

[Afloat16]

Reference issue

No matching issue or PR found in searches for arm goal costs and safe_goal_costs.

What does this implement/fix?

Arm RRT* goal selection minimizes only the stored cost of each nearby vertex, omitting the final connection to the goal. A cheaper partial path can therefore produce a more expensive complete path than another collision-free candidate.

Rank safe candidates by calc_new_cost(vertex, goal_node), which adds the Euclidean joint-space length of the final edge. This matches the complete-path objective already used by the two-dimensional RRT* implementation. Retain collision checks, connection-radius filtering and first-candidate handling for equal total costs.

Additional information

Python 3.13.15 with project-pinned dependencies: 7 tests pass, including the existing seven-joint arm planning test. Six new cases build actual parent branches with costs calculated from their edges, compare complete costs with the extracted course length, and cover native-DH collision filtering, no safe candidate, out-of-radius vertices and equal complete costs. Two competing-branch regressions fail on upstream. An independent reviewer reran all seven tests successfully. Repository Ruff and patch whitespace checks pass; the complete project suite was not run.

CheckList

• Did you add an unittest for your new example or defect fix?
• Did you add documents for your new example? (No new algorithm example.)
• All CIs are green? (Pending upstream CI.)