tech-debt: vision_collision_damper breaks when SMTB angvec2r rejects a zero axis (parallel/antiparallel vectors) #843

Summary

Robot.vision_collision_damper builds the line-of-sight cylinder orientation with a local helper, rotation_between_vectors, that calls SE3.AngleAxis(angle, axis) with axis = cross(a, b). When a and b are parallel or antiparallel the cross product is the zero vector. This code already misbehaves today, and an upcoming SMTB change will turn the silent misbehaviour into a ValueError, so it should be fixed in RTB before that SMTB release ships.

This is not yet a live crash. It becomes one when a SMTB release containing the change described below is published and RTB picks it up (RTB pins spatialmath-python>=1.1.18, so the newest SMTB is installed for new users automatically).

Where

src/roboticstoolbox/robot/Robot.py, Robot.vision_collision_damper (def at line ~1595 on feat/symbolic-ik; the helper is at ~1638-1645, the use at ~1658):

def rotation_between_vectors(a: Any, b: Any) -> Any:
    a = a / np.linalg.norm(a)
    b = b / np.linalg.norm(b)

    angle = np.arccos(np.dot(a, b))
    axis = np.cross(a, b)

    return SE3.AngleAxis(angle, axis)
...
los_orientation = rotation_between_vectors(
    np.array([0.0, 0.0, 1.0]),
    wTcp - wTtp,          # camera position minus target position
)

It orients a thin Cylinder (the "line of sight" between camera and target) along wTcp - wTtp. Degenerate whenever the camera-to-target direction is exactly (anti)parallel to world +z, e.g. a camera directly above or below the target, which is a natural configuration for a visual-servoing / collision-avoidance example (see examples/fetch_vision.py).

Background: the SMTB change

SMTB PR (pending; branch fix/angvec2r-zero-axis in spatialmath-python, will be linked here when opened), following SMTB PR #266 (UnitQuaternion.AngVec did not normalize its axis).

Before: smb.angvec2r(theta, v) returned the identity whenever |v| was ~0, regardless of theta. This is what SO3/SE3.AngleAxis, SO3/SE3.AngVec and angvec2tr all rely on.

After: angvec2r returns the identity only when theta is ~0 (within tol eps, default 20 eps ~ 4.4e-15). For any other theta the axis is normalized with unitvec, which raises ValueError("zero norm vector") for a zero-length axis. A non-zero rotation about no axis is meaningless, and the docstring already said "if θ ~= 0 then V must have a finite length"; the code simply did not enforce it.

Behaviour of rotation_between_vectors (verified, 2026-10-19)

Run against SMTB 1.1.18 (today) vs the branch above. a = [0,0,1] in each case.

Two additional pre-existing problems in the same helper, independent of the SMTB change:

1. np.arccos domain error. After normalizing, np.dot(a, b) can be 1.0000000000000002, giving arccos(...) = nan plus a RuntimeWarning. Needs np.clip(..., -1, 1) if the arccos approach is kept.
2. Antiparallel is silently wrong today (identity instead of 180 degrees), so the line-of-sight cylinder is drawn pointing the wrong way when the camera is directly below the target.

Other RTB call sites (audited, feat/symbolic-ik @ 61d1ddc)

Searched src, tests, examples, docs for angvec2r, angvec2tr, .AngVec(, .AngleAxis(. Only these use the affected functions:

No other sibling toolbox is affected: machinevision-toolbox-python, bdsim, swift and spatialgeometry have no non-test uses of these functions beyond a scratch notebook script in MVTB (docs/notebooks/test_angvec2r.py, unit axis).

vision_collision_damper has no test in tests/, which is how this has gone unnoticed.

Suggested fix

Replace the helper with SE3(SO3.RotatedVector(a, b)). SO3.RotatedVector(v1, v2) is SMTB's purpose-built "rotation taking vector v1 to v2". It accepts non-unit vectors, returns the identity for parallel vectors, and for antiparallel vectors uses a closed-form half-turn (2uu^T - I) about a perpendicular axis. It does not use arccos, so there is no nan problem either.

It is already in the released SMTB 1.1.18 (SMTB #201, "SO3.RotatedVector returned identity for antipodal vectors", is an ancestor of the 1.1.18 tag), so no pin change is needed: RTB already requires spatialmath-python>=1.1.18. I checked the released 1.1.18 wheel from PyPI directly (not the dev checkout): SE3(SO3.RotatedVector(a, b)) maps a to b with a proper orthonormal R for parallel, near-parallel (angle ~1e-8), antiparallel and 500 random pairs, 0 failures of 507.

los_orientation = SE3(SO3.RotatedVector(np.array([0.0, 0.0, 1.0]), wTcp - wTtp))

and delete rotation_between_vectors. (Check that SO3 is imported in Robot.py.)

Because the fix is valid on today's released SMTB, it can, and should, land before the SMTB release containing the angvec2r change.

Acceptance criteria

• The line-of-sight orientation returns the identity for parallel a, b (including parallel up to rounding, e.g. angle ~1e-8), a proper 180-degree rotation for antiparallel a, b, and the usual rotation otherwise. Never nan, never ValueError.
• New unit tests for vision_collision_damper's orientation helper covering parallel, near-parallel (rounding), antiparallel and generic vectors. Factor the orientation computation out into a small module-level private function so it can be tested without building a robot and a Cylinder.
• Tests pass against both SMTB 1.1.18 (released) and SMTB master (with the angvec2r change), e.g. pip install git+https://github.com/rai-opensource/spatialmath-python@master in a throwaway venv (confirm with sys.prefix / the venv's own bin/pip that it is isolated before installing).
• examples/fetch_vision.py still runs for a camera directly above the target.

Notes for whoever picks this up

• Do this before the SMTB release containing the angvec2r change; it is safe to land first because the fix is valid on the already-released 1.1.18.
• Branch per the usual one-change-set rule; this is a separate concern from feat/symbolic-ik.
• RTB conventions: NDArray/ArrayLike from roboticstoolbox.tools.types, modern type hints, reST docstrings, Australian/"color" spelling.
• Context: SMTB is RAI-owned (rai-opensource/spatialmath-python), merges are RAI's; Peter reviews/approves PRs there.