[Rotation] Swing & Twist Decomposition

[fagg]

Adds two new functions to pytransform3d.rotations:

• swing_twist_decomposition - splits a unit quaternion into a twist (rotation about a given axis) and a swing (rotation about an axis orthogonal to it), such that q = q_swing . q_twist.
• swing_twist_composition - the inverse operation, for reconstruction of the original rotation from its swing and twist components.

This decomposition is useful for enforcing joint limits, separating roll about a link axis from the remaining rotation about a symmetry axis. It follows Dobrowolski (2015), "Swing-twist decomposition in Clifford algebra" - https://arxiv.org/abs/1506.05481

Algorithmic details:

• Twist is obtained by projecting the vector part of the quaternion onto the twist axis and renormalizing.
• The swing is recovered as q_swing = q . q_twist^-1
• Quaternions doubly cover SO(3) and this surfaces in two places. The first is that decomposition is sign invariant - as such the twist is canonicalized into the hemisphere with non-negative scalar part (w >= 0), so its rotation angle about the axis stays in [-pi, pi]. As a result, swing_twist_decomposition(q, axis) and swing_twist_decomposition(-q, axis) return the exact same twist quaternions and equivalent swings. Secondly, composition may cause a sign flip - you may get -q instead of q.

Implementation details:

• A zero twist axis will raise a ValueError
• The twist axis is rescaled by its largest-magnitude component before normalization, so subnormal axes don't underflow to zero (leading to a spurious zero-error vector) or lose precision.
• For a rotation of pi about an axis orthogonal to the twist axis, the twist is undefined (scalar part and axis projection both vanish). In this case we set the twist to identity and swing carries the full rotation.

All cases and constraints described above are covered by unit tests.

[AlexanderFabisch]

Hi @fagg ,

thank you for this contribution too!

A few comments:

1. Please run black + ruff check
2. Should describe it in the user guide? Maybe to Rotations -> Quaternions?
3. You say

This decomposition is useful, for example, to enforce joint limits, to separate the roll about a link axis from the remaining orientation, or to isolate the rotation about a symmetry axis.

It would be really interesting to have at least one example that demonstrates one of these use cases. I'd prefer the joint limit use case, since this is most interesting to me, but it does not have to be this one. Would it be possible to add one example?

4. The implementation of the decomposition adds conventions beyond the algorithm from the paper that should be clearly documented: (1) twist_norm < eps and (2) twist[0] < 0.

[fagg]

Hello @AlexanderFabisch,

1. Please run black + ruff check

Done.

2. Should describe it in the user guide? Maybe to Rotations -> Quaternions?

I added a draft set of changes.

3. 

It would be really interesting to have at least one example that demonstrates one of these use cases. I'd prefer the joint limit use case, since this is most interesting to me, but it does not have to be this one. Would it be possible to add one example?

I have added two.

One is simple plot - which shows the original rotation, it's decomposition into twist and swing, and it's composition from twist and swing.

The more complex of the two attempts to demonstrate axis redundancy using this decomposition.

What it shows is a robot end-effector holding a tool whose working axis is the body z-axis, where the tool is pointing towards a workpiece. It builds a nominal porientation (aim + arbitrary roll), decomposes it about the tool axis into swing (where the tool axis points) and twist (roll about the tool). We then visualize the sweep along the twist while holding the swing fixed. Effectively, what we generated is a set of end-effector orientations that keep the tool pointed at the workpiece - you could consider this a solution set for a task-space planner with twist being the only remaining free parameter.

4. The implementation of the decomposition adds conventions beyond the algorithm from the paper that should be clearly documented: (1) `twist_norm < eps` and (2) `twist[0] < 0`.

I already had tried to make that clear in the comments - it seems I failed. The latest version here has beefed up language around those assumptions in the doc string.

[AlexanderFabisch]

This looks really good. Thanks! I will test everything next week.

[AlexanderFabisch]

Minor detail, but these ===s should have the same length as the title.

[AlexanderFabisch]

Same here

[AlexanderFabisch]

~s should have same length as title

[AlexanderFabisch]

Could you use \text{swing} and \text{twist} here and in the docstrings?

[AlexanderFabisch]

Could you also use black-like formatting here?

[AlexanderFabisch]

Had time to review already now. It's very well documented. I just have some comments on little details. This is a really good contribution, thanks!