Fix out-of-bounds SVD indexing in Servo singularity check for robots with fewer than 6 joints

[EmirataG]

Description

velocityScalingFactorForSingularity() in moveit_servo indexes the SVD of the robot Jacobian with the Cartesian task dimension instead of the number of singular values. The Jacobian is 6 x num_joints, and the thin SVD (ComputeThinU) therefore has min(6, num_joints) singular values and U columns, but the code reads singularValues()(dims - 1) and matrixU().col(dims - 1) with dims = 6 always. For any move group with fewer than 6 active joints, both accesses are out of bounds. Because release builds define NDEBUG, Eigen's bounds assertions are compiled out and the reads silently return garbage memory: the computed "condition number" is sigma_max divided by an arbitrary value, which is almost always far above hard_stop_singularity_threshold, so Servo emits HALT_FOR_SINGULARITY ("Very close to a singularity, emergency stop") on effectively every TWIST/POSE command, independent of the configured thresholds. The corrupted last-U-column also breaks the moving-toward/away-from-singularity classification that selects the deceleration band.

This explains several reports that were closed without the root cause being found: #3411 (4-DOF arm, constant emergency stops; the reporter worked around it by adding two fake virtual joints to the URDF, which "fixes" the issue only by making the out-of-bounds index legal), #2760, and ROBOTIS-GIT/open_manipulator#192. It is related to but independent of the drift-dimensions discussion in moveit/moveit_msgs#185: even with structurally large condition numbers, users of low-DOF arms can tune thresholds — but only if the condition number is actually computed from their robot's geometry rather than from undefined behavior.

How this was found: a 4-DOF robot emergency-stopped on every POSE/TWIST command under Jazzy. The decisive symptom was that changing lower_singularity_threshold / hard_stop_singularity_threshold had no effect whatsoever on the behavior, which is impossible if the halting decision compares a geometry-derived condition number against those thresholds, and pointed to the comparison itself being broken. Reading velocityScalingFactorForSingularity() with the robot's 6x4 Jacobian in mind showed the thin SVD is indexed by the task dimension (6) rather than the number of singular values (4). With the index fixed, the computed condition numbers match an independent numerical model of the arm to four significant figures.

The fix indexes by singularValues().size() - 1, which is the position of sigma_min for any number of joints (singular values are sorted in decreasing order), and uses the corresponding last column of thin U as the least-responsive direction. For robots with 6 or more joints in the group this evaluates to the same index 5 as before, so behavior is unchanged for existing 6/7-DOF users.

A unit test is added that builds a 4-DOF chain with RobotModelBuilder and calls velocityScalingFactorForSingularity() directly, asserting both branches: a well-conditioned bent posture (condition number ~33, verified against an independent numerical model of the same kinematics) must return NO_WARNING with scale 1.0, and a near-extended posture (condition number ~979) must return HALT_FOR_SINGULARITY with scale 0.0, with at least 2x margin between every condition number and its threshold. The test was verified to fail against the stock, unpatched 2.12.4 release library (it spuriously halts at the well-conditioned posture) — no sanitizer needed; additionally, ASan and debug builds (eigen_assert) catch the out-of-bounds read itself deterministically. The existing Panda-based singularity tests pass unchanged.

Verified on real hardware: the same 4-DOF arm that previously emergency-stopped on every pose command now servos correctly with thresholds tuned to its structural condition-number range.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference (not applicable: bug fix, no API or behavior change for supported
  configurations)
• Document API changes relevant to the user in the MIGRATION.md notes (not applicable: no API change)
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI (not applicable)
• While waiting for someone to review your request, please help review [tps://github.com/moveit/moveit2/pulls) to support the maintainers