[Rotation] Swing & Twist Decomposition

doc/source/api.rst

@@ -136,6 +136,8 @@ Quaternion
    ~quaternion_slerp
    ~quaternion_dist
    ~quaternion_diff
+   ~swing_twist_decomposition
+   ~swing_twist_composition
    ~quaternion_gradient
    ~quaternion_integrate
    ~quaternion_from_angle

doc/source/user_guide/rotations.rst

@@ -398,6 +398,59 @@ with the quaternion product (:func:`~pytransform3d.rotations.q_prod_vector`).
 * Interpretation: not straightforward.
 * Ambiguities: double cover.
 
+Swing-Twist Decomposition
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Any rotation can be split into a *twist* about a chosen axis
+:math:`\boldsymbol{e}` and a *swing* that carries the remaining orientation,
+such that
+
+.. math::
+
+    \boldsymbol{q} = \boldsymbol{q}_{swing} \boldsymbol{q}_{twist},
+
+where the twist is applied first. The twist
+:math:`\boldsymbol{q}_{twist}` is a rotation about :math:`\boldsymbol{e}`
+and the swing :math:`\boldsymbol{q}_{swing}` is a rotation about an axis that
+is orthogonal to :math:`\boldsymbol{e}` [9]_. The twist is obtained by
+projecting the vector part of the quaternion onto the twist axis and
+renormalizing; the swing is then recovered as
+:math:`\boldsymbol{q}_{swing} = \boldsymbol{q} \boldsymbol{q}_{twist}^{-1}`.
+
+This is useful to separate the roll about a link axis from the rest of an
+orientation, to enforce joint limits, or to isolate the rotation about a
+symmetry axis. pytransform3d provides
+:func:`~pytransform3d.rotations.swing_twist_decomposition` and the inverse
+:func:`~pytransform3d.rotations.swing_twist_composition`, which reconstructs
+the original rotation from swing and twist.
+
+.. plot::
+    :include-source:
+
+    import numpy as np
+    from pytransform3d.rotations import (
+        swing_twist_decomposition, quaternion_from_axis_angle,
+        matrix_from_quaternion, plot_basis)
+
+    axis = np.array([0.0, 0.0, 1.0])
+    q = quaternion_from_axis_angle([0.3, 0.7, 0.2, 1.2])
+    swing, twist = swing_twist_decomposition(q, axis)
+
+    ax = plot_basis(R=matrix_from_quaternion(q), p=[0, 0, 0])
+    plot_basis(ax=ax, R=matrix_from_quaternion(twist), p=[3, 0, 0])
+    plot_basis(ax=ax, R=matrix_from_quaternion(swing), p=[6, 0, 0])
+
+The left frame shows the full rotation, the middle frame the twist (a pure
+rotation about the z-axis), and the right frame the swing.
+
+.. warning::
+
+    The twist is undefined for a rotation by :math:`\pi` about an axis
+    orthogonal to the twist axis: both the scalar part and the projection onto
+    the twist axis vanish. In this degenerate case
+    :func:`~pytransform3d.rotations.swing_twist_decomposition` returns the
+    identity as the twist and lets the swing carry the full rotation.
+
 ------------
 Euler Angles
 ------------
@@ -565,3 +618,5 @@ References
 .. [8] Shuster, M. D. (1993). A Survey of Attitude Representations.
    Journal of the Astronautical Sciences, 41, 439-517.
    http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf
+.. [9] Dobrowolski, P. (2015). Swing-twist decomposition in Clifford algebra.
+   https://arxiv.org/abs/1506.05481

examples/plots/plot_swing_twist.py

@@ -0,0 +1,78 @@
+"""
+==========================
+Swing-Twist Decomposition
+==========================
+
+Any rotation can be split into a *twist* about a chosen axis and a *swing*
+that rotates the axis itself, such that ``q = swing * twist`` (the twist is
+applied first). The twist is a rotation about the given axis and the swing is a
+rotation about an axis orthogonal to it. This is convenient, for example, to
+separate the roll about a link axis from the remaining orientation.
+
+Here the twist axis is the body z-axis. We take an arbitrary rotation,
+decompose it into its swing and twist components, and recompose them to recover
+the original orientation.
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from pytransform3d.rotations import (
+    axis_angle_from_quaternion,
+    matrix_from_quaternion,
+    plot_axis_angle,
+    plot_basis,
+    quaternion_from_axis_angle,
+    swing_twist_composition,
+    swing_twist_decomposition,
+)
+
+# %%
+# We pick an arbitrary rotation and decompose it about the body z-axis. The
+# decomposition returns a swing (a rotation about an axis orthogonal to z) and
+# a twist (a rotation about z).
+axis = np.array([0.0, 0.0, 1.0])
+q = quaternion_from_axis_angle([0.6, 0.3, 0.8, np.deg2rad(100.0)])
+
+swing, twist = swing_twist_decomposition(q, axis)
+
+# %%
+# Recomposing the swing and twist recovers the original rotation. The
+# reconstruction may differ in sign, since a quaternion and its negation
+# represent the same rotation, so we compare rotation matrices.
+q_recomposed = swing_twist_composition(swing, twist)
+print(
+    "Recomposition matches original:",
+    np.allclose(
+        matrix_from_quaternion(q), matrix_from_quaternion(q_recomposed)
+    ),
+)
+
+# %%
+# We visualize the pieces as a sequence of frames along the x-axis. From left
+# to right: the original rotation, the isolated swing, the isolated twist, and
+# the recomposed rotation (swing * twist), which coincides with the original.
+# The black arrow marks the twist axis and its arc shows the twist angle.
+frames = [
+    (q, "original", 0.0),
+    (swing, "swing", 2.5),
+    (twist, "twist", 5.0),
+    (q_recomposed, "swing * twist", 7.5),
+]
+
+ax = None
+for q_i, label, offset in frames:
+    p = np.array([offset, 0.0, 0.0])
+    ax = plot_basis(ax=ax, R=matrix_from_quaternion(q_i), p=p, ax_s=4, lw=3)
+    ax.text(offset, 0.0, -2.0, label, ha="center")
+
+# Draw the twist axis and angle on the twist frame.
+plot_axis_angle(ax, axis_angle_from_quaternion(twist), p=[5.0, 0.0, 0.0])
+
+ax.view_init(elev=25, azim=-60)
+ax.set_xlim((-1.5, 9.0))
+ax.set_ylim((-3.0, 3.0))
+ax.set_zlim((-3.0, 3.0))
+ax.set_box_aspect((10.5, 6.0, 6.0))
+plt.subplots_adjust(left=0, right=1, bottom=0, top=1)
+plt.show()

examples/plots/plot_swing_twist_tool_axis.py

@@ -0,0 +1,116 @@
+"""
+=========================================
+Tool-Axis Redundancy via Swing and Twist
+=========================================
+
+Many robot tasks only constrain the *direction* a tool points, not the roll
+about the tool's own axis. Drilling, deburring, spraying, or spot welding all
+leave the rotation about the tool axis free, because the tool is symmetric
+about it. Swing-twist decomposition isolates exactly this redundant degree of
+freedom.
+
+Choosing the tool axis as the twist axis, an end-effector orientation splits
+into a *swing* that aims the tool axis (the task-relevant approach direction)
+and a *twist* that rolls the tool about that axis (the redundant part). We take
+a nominal end-effector orientation, recover its swing and twist, and then sweep
+the twist to generate a whole family of orientations that all keep the tool
+pointing the same way.
+"""
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+from pytransform3d.rotations import (
+    axis_angle_from_two_directions,
+    matrix_from_quaternion,
+    plot_basis,
+    quaternion_from_axis_angle,
+    swing_twist_composition,
+    swing_twist_decomposition,
+    unitz,
+)
+
+# %%
+# The tool axis is the end-effector's body z-axis. We build a nominal
+# orientation that aims the tool along a desired approach direction and adds
+# some arbitrary roll, then decompose it about the tool axis. The swing
+# encodes where the tool axis points; the twist is the roll about it.
+tool_axis = unitz
+approach = np.array([1.0, -0.7, -0.6])
+approach = approach / np.linalg.norm(approach)
+
+aim = quaternion_from_axis_angle(
+    axis_angle_from_two_directions(unitz, approach)
+)
+roll = quaternion_from_axis_angle(np.hstack((tool_axis, np.deg2rad(40.0))))
+q_nominal = swing_twist_composition(aim, roll)
+
+swing, twist = swing_twist_decomposition(q_nominal, tool_axis)
+
+# %%
+# Sweeping the twist while holding the swing fixed produces end-effector
+# orientations that all share the same tool-axis direction -- only the roll
+# about the tool changes. This is the family of solutions a task-space planner
+# is free to pick from, e.g. to dodge a joint limit or an obstacle.
+twist_angles = np.linspace(0.0, 2.0 * np.pi, 12, endpoint=False)
+family = [
+    swing_twist_composition(
+        swing, quaternion_from_axis_angle(np.hstack((tool_axis, angle)))
+    )
+    for angle in twist_angles
+]
+
+# The tool tip sits at a workpiece; the end-effector is set back along the
+# approach direction so the tool axis points at the workpiece.
+workpiece = np.zeros(3)
+reach = 1.4
+tcp = workpiece - reach * approach
+
+# %%
+# The nominal end-effector orientation is drawn as a full frame, with its blue
+# tool axis running down the dashed shaft to the workpiece. The rest of the
+# family is shown as a pinwheel: each spoke is a reference mark painted on the
+# tool (its x-axis) at one twist angle, colored from dark to bright as the roll
+# advances. Every spoke shares the same tool axis; only the roll differs.
+radius = 0.5
+ax = plot_basis(
+    R=matrix_from_quaternion(q_nominal), p=tcp, s=radius, ax_s=1.5, lw=4
+)
+
+# Tool shaft from the end-effector to the workpiece and the workpiece itself.
+ax.plot(*np.column_stack((tcp, workpiece)), "--", color="k", lw=2)
+ax.scatter(*workpiece, color="k", s=60)
+ax.text(*(workpiece + [0.05, 0.05, 0.1]), "workpiece")
+
+# Circle traced by the tool's x-axis to emphasize the free roll.
+fine = np.linspace(0.0, 2.0 * np.pi, 60)
+ring = np.array(
+    [
+        tcp
+        + radius
+        * matrix_from_quaternion(
+            swing_twist_composition(
+                swing, quaternion_from_axis_angle(np.hstack((tool_axis, a)))
+            )
+        )[:, 0]
+        for a in fine
+    ]
+)
+ax.plot(ring[:, 0], ring[:, 1], ring[:, 2], color="gray", lw=1)
+
+# Pinwheel spokes: the tool's x-axis reference mark at each twist angle.
+colors = plt.cm.viridis(np.linspace(0.0, 1.0, len(family), endpoint=False))
+for q, color in zip(family, colors):
+    tip = tcp + radius * matrix_from_quaternion(q)[:, 0]
+    ax.plot(*np.column_stack((tcp, tip)), color=color, lw=2)
+    ax.scatter(*tip, color=color, s=25)
+
+ax.view_init(elev=28, azim=-55)
+lo = np.minimum(tcp, workpiece) - 0.6
+hi = np.maximum(tcp, workpiece) + 0.6
+ax.set_xlim((lo[0], hi[0]))
+ax.set_ylim((lo[1], hi[1]))
+ax.set_zlim((lo[2], hi[2]))
+ax.set_box_aspect(hi - lo)
+plt.subplots_adjust(left=0, right=1, bottom=0, top=1)
+plt.show()

pytransform3d/rotations/__init__.py

@@ -138,6 +138,7 @@
     left_jacobian_SO3_inv_series,
 )
 from ._polar_decomp import robust_polar_decomposition
+from ._swing_twist import swing_twist_decomposition, swing_twist_composition
 from ._plot import (
     plot_basis,
     plot_axis_angle,
@@ -361,4 +362,6 @@
     "left_jacobian_SO3_inv",
     "left_jacobian_SO3_inv_series",
     "robust_polar_decomposition",
+    "swing_twist_decomposition",
+    "swing_twist_composition",
 ]