Development branch, release 3.9.0

CITATION.cff

@@ -11,11 +11,12 @@ preferred-citation:
     authors:
     - family-names: Fabisch
       given-names: Alexander
+      orcid: https://orcid.org/0000-0003-2824-7956
     title: "pytransform3d: 3D Transformations for Python"
     doi: 10.21105/joss.01159
     journal: Journal of Open Source Software
     start: 1159
     issue: 33
     volume: 4
     month: 1
-    year: 2019
+    year: 2019

doc/source/api.rst

@@ -98,8 +98,10 @@ Logarithm of Rotation
    :toctree: _apidoc/
    :template: function.rst
 
+   ~check_rot_log
    ~check_skew_symmetric_matrix
 
+   ~rot_log_from_compact_axis_angle
    ~cross_product_matrix
 
 
@@ -178,6 +180,7 @@ Modified Rodrigues Parameters
    ~mrp_double
 
    ~concatenate_mrp
+   ~mrp_prod_vector
 
    ~mrp_from_axis_angle
    ~mrp_from_quaternion
@@ -295,7 +298,6 @@ Transformation Matrix
    ~vectors_to_points
    ~vector_to_direction
    ~vectors_to_directions
-   ~scale_transform
    ~adjoint_from_transform
 
    ~plot_transform
@@ -439,6 +441,15 @@ Jacobians
    ~left_jacobian_SE3_inv
    ~left_jacobian_SE3_inv_series
 
+Deprecated Functions
+--------------------
+
+.. autosummary::
+   :toctree: _apidoc/
+   :template: function.rst
+
+   ~scale_transform
+
 
 :mod:`pytransform3d.batch_rotations`
 ====================================
@@ -468,7 +479,9 @@ Axis-Angle Representation
    :toctree: _apidoc/
    :template: function.rst
 
+   ~norm_axis_angles
    ~axis_angles_from_matrices
+   ~axis_angles_from_quaternions
 
    ~cross_product_matrices
 
@@ -516,6 +529,8 @@ Transformation Matrices
    ~invert_transforms
    ~concat_one_to_many
    ~concat_many_to_one
+   ~concat_many_to_many
+   ~concat_dynamic
 
    ~transforms_from_pqs
    ~transforms_from_exponential_coordinates
@@ -533,6 +548,13 @@ Positions and Quaternions
    ~pqs_from_transforms
    ~pqs_from_dual_quaternions
 
+Screw Parameters
+----------------
+.. autosummary::
+   :toctree: _apidoc/
+   :template: function.rst
+
+   ~screw_parameters_from_dual_quaternions
 
 Exponential Coordinates
 -----------------------
@@ -553,10 +575,13 @@ Dual Quaternions
    :template: function.rst
 
    ~batch_dq_conj
+   ~batch_dq_q_conj
    ~batch_concatenate_dual_quaternions
    ~batch_dq_prod_vector
-
    ~dual_quaternions_from_pqs
+   ~dual_quaternions_power
+   ~dual_quaternions_sclerp
+   ~dual_quaternions_from_screw_parameters
 
 
 :mod:`pytransform3d.uncertainty`

doc/source/index.rst

@@ -51,6 +51,11 @@ pytransform3d offers...
 * a matplotlib-like interface to Open3D's visualizer to display
   geometries and transformations
 
+The design philosophy of pytransform3d is to not hide anything; there is no
+magic: no operator overloading, no classes to store data (rotations or
+transformations) other than NumPy arrays, and no layers of abstraction.
+It is as transparent to the user as possible and its interface is mainly
+functional.
 
 --------
 Citation

doc/source/user_guide/uncertainty.rst

@@ -50,7 +50,7 @@ A typical visual representation of Gaussian distributions in 3D coordinates
 are equiprobable ellipsoids (obtained with
 :func:`~pytransform3d.uncertainty.to_ellipsoid`). This is equivalent to showing
 the :math:`k\sigma, k \in \mathbb{R}` intervals of a 1D Gaussian distribution.
-However, for transformations are also interactions between rotation and
+However, for transformations there are also interactions between rotation and
 translation components so that an ellipsoid is not an appropriate
 representation to visualize the distribution of transformations in 3D. We have
 to project a 6D hyper-ellipsoid to 3D (for which we can use

examples/plots/plot_interpolation_for_transform_manager.py

@@ -60,7 +60,7 @@ def create_sinusoidal_movement(
 tm.add_transform("A", "world", transform_WA)
 tm.add_transform("B", "world", transform_WB)
 
-query_time = 4.9  # [s]
+query_time = 4.9  # [s] or an array of times np.array([4.9, 5.2])
 A2B_at_query_time = tm.get_transform_at_time("A", "B", query_time)
 
 # transform the origin of A in A (x=0, y=0, z=0) to B

pytransform3d/__init__.py

@@ -1,4 +1,4 @@
 """3D transformations for Python."""
 
 
-__version__ = "3.8.0"
+__version__ = "3.9.0"

pytransform3d/batch_rotations.py

@@ -8,7 +8,7 @@
 """
 import numpy as np
 from .rotations import (
-    angle_between_vectors, slerp_weights, pick_closest_quaternion)
+    angle_between_vectors, slerp_weights, pick_closest_quaternion, norm_angle)
 
 
 def norm_vectors(V, out=None):
@@ -38,6 +38,52 @@ def norm_vectors(V, out=None):
     return out
 
 
+def norm_axis_angles(a):
+    """Normalize axis-angle representation.
+
+    Parameters
+    ----------
+    a : array-like, shape (..., 4)
+        Axis of rotation and rotation angle: (x, y, z, angle)
+
+    Returns
+    -------
+    a : array, shape (..., 4)
+        Axis of rotation and rotation angle: (x, y, z, angle). The length
+        of the axis vector is 1 and the angle is in [0, pi). No rotation
+        is represented by [1, 0, 0, 0].
+    """
+    a = np.asarray(a)
+
+    # Handle the case of only one axis-angle instance
+    only_one = a.ndim == 1
+    a = np.atleast_2d(a)
+
+    angles = a[..., 3]
+    norm = np.linalg.norm(a[..., :3], axis=-1)
+
+    no_rot_mask = (angles == 0.0) | (norm == 0.0)
+    rot_mask = ~no_rot_mask
+
+    res = np.empty_like(a)
+    res[no_rot_mask, :] = np.array([1.0, 0.0, 0.0, 0.0])
+    res[rot_mask, :3] = (
+        a[rot_mask, :3] / norm[rot_mask, np.newaxis]
+    )
+
+    angle_normalized = norm_angle(angles)
+
+    negative_angle_mask = angle_normalized < 0.0
+    res[negative_angle_mask, :3] *= -1.0
+    angle_normalized[negative_angle_mask] *= -1.0
+
+    res[rot_mask, 3] = angle_normalized[rot_mask]
+
+    if only_one:
+        res = res[0]
+    return res
+
+
 def angles_between_vectors(A, B):
     """Compute angle between two vectors.
 
@@ -338,6 +384,45 @@ def axis_angles_from_matrices(Rs, traces=None, out=None):
     return out
 
 
+def axis_angles_from_quaternions(qs):
+    """Compute axis-angle from quaternion.
+
+    This operation is called logarithmic map.
+
+    We usually assume active rotations.
+
+    Parameters
+    ----------
+    qs : array-like, shape (..., 4)
+        Unit quaternion to represent rotation: (w, x, y, z)
+
+    Returns
+    -------
+    as : array, shape (..., 4)
+        Axis of rotation and rotation angle: (x, y, z, angle). The angle is
+        constrained to [0, pi) so that the mapping is unique.
+    """
+    quaternion_vector_part = qs[..., 1:]
+    qvec_norm = np.linalg.norm(quaternion_vector_part, axis=-1)
+
+    # Vectorized branches bases on norm of the vector part
+    small_p_norm_mask = qvec_norm < np.finfo(float).eps
+    non_zero_mask = ~small_p_norm_mask
+
+    axes = (quaternion_vector_part[non_zero_mask]
+            / qvec_norm[non_zero_mask, np.newaxis])
+
+    w_clamped = np.clip(qs[non_zero_mask, 0], -1.0, 1.0)
+    angles = 2.0 * np.arccos(w_clamped)
+
+    result = np.empty_like(qs)
+    result[non_zero_mask] = norm_axis_angles(
+        np.concatenate((axes, angles[..., np.newaxis]), axis=-1))
+    result[small_p_norm_mask] = np.array([1.0, 0.0, 0.0, 0.0])
+
+    return result
+
+
 def cross_product_matrices(V):
     """Generate the cross-product matrices of vectors.
 

pytransform3d/batch_rotations.pyi

@@ -8,6 +8,9 @@ def norm_vectors(
         out: Union[npt.ArrayLike, None] = ...) -> np.ndarray: ...
 
 
+def norm_axis_angles(a: npt.ArrayLike) -> np.ndarray: ...
+
+
 def angles_between_vectors(A: npt.ArrayLike, B: npt.ArrayLike) -> np.ndarray: ...
 
 
@@ -38,6 +41,9 @@ def axis_angles_from_matrices(
         out: Union[np.ndarray, None] = ...) -> np.ndarray: ...
 
 
+def axis_angles_from_quaternions(qs: npt.ArrayLike) -> np.ndarray: ...
+
+
 def cross_product_matrices(V: npt.ArrayLike) -> np.ndarray: ...
 
 

pytransform3d/rotations/__init__.py

@@ -11,7 +11,7 @@
     matrix_requires_renormalization, norm_matrix,
     quaternion_requires_renormalization,
     plane_basis_from_normal,
-    check_skew_symmetric_matrix, check_matrix, check_quaternion,
+    check_skew_symmetric_matrix, check_rot_log, check_matrix, check_quaternion,
     check_quaternions, check_axis_angle, check_compact_axis_angle,
     check_rotor, check_mrp)
 from ._random import (
@@ -85,6 +85,7 @@
     euler_from_quaternion,
     quaternion_from_angle,
     cross_product_matrix,
+    rot_log_from_compact_axis_angle,
     mrp_from_quaternion,
     quaternion_from_mrp,
     mrp_from_axis_angle,
@@ -93,7 +94,9 @@
     quaternion_double, quaternion_integrate, quaternion_gradient,
     concatenate_quaternions, q_conj, q_prod_vector, quaternion_diff,
     quaternion_dist, quaternion_from_euler)
-from ._mrp import mrp_near_singularity, norm_mrp, mrp_double, concatenate_mrp
+from ._mrp import (
+    mrp_near_singularity, norm_mrp, mrp_double, concatenate_mrp,
+    mrp_prod_vector)
 from ._slerp import (slerp_weights, pick_closest_quaternion, quaternion_slerp,
                      axis_angle_slerp, rotor_slerp)
 from ._testing import (
@@ -137,6 +140,7 @@
     "random_compact_axis_angle",
     "random_quaternion",
     "check_skew_symmetric_matrix",
+    "check_rot_log",
     "check_matrix",
     "check_quaternion",
     "check_quaternions",
@@ -223,7 +227,9 @@
     "norm_mrp",
     "mrp_double",
     "concatenate_mrp",
+    "mrp_prod_vector",
     "cross_product_matrix",
+    "rot_log_from_compact_axis_angle",
     "mrp_from_quaternion",
     "quaternion_from_mrp",
     "mrp_from_axis_angle",

pytransform3d/rotations/_conversions.py

@@ -38,21 +38,27 @@ def cross_product_matrix(v):
         -v_2 & v_1 & 0
         \end{array}\right).
 
+    The function can also be used to compute the logarithm of rotation from
+    a compact axis-angle representation.
+
     Parameters
     ----------
     v : array-like, shape (3,)
         3d vector
 
     Returns
     -------
-    V : array-like, shape (3, 3)
+    V : array, shape (3, 3)
         Cross-product matrix
     """
     return np.array([[0.0, -v[2], v[1]],
                      [v[2], 0.0, -v[0]],
                      [-v[1], v[0], 0.0]])
 
 
+rot_log_from_compact_axis_angle = cross_product_matrix
+
+
 def matrix_from_two_vectors(a, b):
     """Compute rotation matrix from two vectors.
 
@@ -1964,12 +1970,13 @@ def axis_angle_from_two_directions(a, b):
 def compact_axis_angle(a):
     r"""Compute 3-dimensional axis-angle from a 4-dimensional one.
 
-    In a 3-dimensional axis-angle, the 4th dimension (the rotation) is
-    represented by the norm of the rotation axis vector, which means we
-    transform :math:`\left( \boldsymbol{\hat{e}}, \theta \right)` to
-    :math:`\theta \boldsymbol{\hat{e}}`.
+    In the 3-dimensional axis-angle representation, the 4th dimension (the
+    rotation) is represented by the norm of the rotation axis vector, which
+    means we map :math:`\left( \hat{\boldsymbol{\omega}}, \theta \right)` to
+    :math:`\boldsymbol{\omega} = \theta \hat{\boldsymbol{\omega}}`.
 
-    We usually assume active rotations.
+    This representation is also called rotation vector or exponential
+    coordinates of rotation.
 
     Parameters
     ----------

pytransform3d/rotations/_conversions.pyi

@@ -5,6 +5,9 @@ import numpy.typing as npt
 def cross_product_matrix(v: npt.ArrayLike) -> np.ndarray: ...
 
 
+def rot_log_from_compact_axis_angle(v: npt.ArrayLike) -> np.ndarray: ...
+
+
 def matrix_from_two_vectors(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ...