timor.utilities.spatial
=======================

.. py:module:: timor.utilities.spatial


Attributes
----------

.. autoapisummary::

   timor.utilities.spatial.NO_ROTATION
   timor.utilities.spatial.NO_TRANSLATION


Functions
---------

.. autoapisummary::

   timor.utilities.spatial.axis_angle2rot_mat
   timor.utilities.spatial.axis_angle_rotation
   timor.utilities.spatial.cartesian2cylindrical
   timor.utilities.spatial.cartesian2spherical
   timor.utilities.spatial.clone_collision_object
   timor.utilities.spatial.cylindrical2cartesian
   timor.utilities.spatial.dh_extended_to_homogeneous
   timor.utilities.spatial.euler2mat
   timor.utilities.spatial.frame2geom
   timor.utilities.spatial.homogeneous
   timor.utilities.spatial.inv_homogeneous
   timor.utilities.spatial.mat2euler
   timor.utilities.spatial.random_rotation
   timor.utilities.spatial.rot2D
   timor.utilities.spatial.rotX
   timor.utilities.spatial.rotY
   timor.utilities.spatial.rotZ
   timor.utilities.spatial.rot_mat2axis_angle
   timor.utilities.spatial.rotation_in_bounds
   timor.utilities.spatial.skew
   timor.utilities.spatial.spherical2cartesian


Module Contents
---------------

.. py:data:: NO_ROTATION

.. py:data:: NO_TRANSLATION

.. py:function:: axis_angle2rot_mat(axis_angle)

   Inverse to rot_mat2axis_angle.


.. py:function:: axis_angle_rotation(vec, axis_angle)

   Rotates a 3x1 vector by a given axis-angle representation.

   Equivalent to spatial.axis_angle2rot_mat(axis_angle) @ vec, but computationally more efficient.
   :param vec: The vector to be rotated
   :param axis_angle: The axis-angle representation of the rotation, either 4x1 or 3x1 array
   :source: https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation


.. py:function:: cartesian2cylindrical(cart)

   Takes a (3,) array in cartesian coordinates [x, y, z] and returns the according cylindrical coordinates [r, phi, z].

   :source: https://en.wikipedia.org/wiki/Polar_coordinate_system


.. py:function:: cartesian2spherical(cart)

   Takes a (3,) array in cartesian coordinates [x, y, z] and returns the according spherical coordinates.

   Convention: [r, theta, phi]; r, theta, phi ~ radial, azimuthal, polar.
   :source: https://en.wikipedia.org/wiki/Spherical_coordinate_system


.. py:function:: clone_collision_object(co)

   Deep copy of a hppfcl collision object


.. py:function:: cylindrical2cartesian(cyl)

   Inverse to cartesian2cylindrical


.. py:function:: dh_extended_to_homogeneous(a = 0, alpha = 0, n = 0, p = 0, delta = 0)

   Transform extended dh parameters into a homogeneous transformation matrix

   Describes a transformation similar to dh parameters (from Althoff et al., Sci. Robotics, 2019, Fig. 2)
   More precisely the transformation from a module input frame to the joint

   :param a: translate along x'_i [m]
   :param alpha: rotate around x_i [rad]
   :param delta: rotate around z_im1 [rad], gamma in Fig. 2
   :param p: offset of PJ_im1 to o'_i in z'_i direction [m]
   :param n: offset along z_i from o_i to PJ_i [m]


.. py:function:: euler2mat(rotation, seq)

   Wrapper for the scipy euler method

   :param rotation: Rotation angles in radian
   :param seq: Any ordering of {xyz}[intrinsic] or {XYZ}[extrinsic] axes.
   :return: 3x3 rotation matrix


.. py:function:: frame2geom(frame_id, geom_model)

   Returns all geometry objects where porent frame is frame_id


.. py:function:: homogeneous(translation = NO_TRANSLATION, rotation = NO_ROTATION)

   Returns a transformation matrix for translation, then rotation

   :param translation: Translation in the transformation matrix. 3x1
   :param rotation: 3x3 rotation matrix. Nested List works as well
   :return:


.. py:function:: inv_homogeneous(T)

   Efficiently inverses a homogeneous transformation


.. py:function:: mat2euler(R, seq = 'xyz')

   Inverse to euler2mat

   :param R: A 3x3 rotation matrix
   :param seq: Any ordering of {xyz}[intrinsic] or {XYZ}[extrinsic] axes. Defaults to roll-pitch-yaw
   :return: The rotations around the axes specified in seq that lead to R


.. py:function:: random_rotation(rng = None)

   Returns a random 3x3 rotation matrix.


.. py:function:: rot2D(angle)

   Returns the 2D rotation matrix for an angle in radian.

   :param angle: Angel in radian
   :return: 2D rotation


.. py:function:: rotX(alpha)

   Returns a transformation rotation matrix for a rotation of a vector around the x-axis


.. py:function:: rotY(beta)

   Returns a transformation rotation matrix for a rotation of a vector around the y-axis


.. py:function:: rotZ(gamma)

   Returns a transformation rotation matrix for a rotation of a vector around the z-axis


.. py:function:: rot_mat2axis_angle(rot_mat)

   Takes a rotation matrix and returns the corresponding axis-angle representation

   :param rot_mat: 3x3 rotation matrix
   :return: 4x1 array of axis-angle representation (n_x, n_y, n_z, theta_R), where n_x, n_y, n_z ~ unit vector and
       :math:`\theta_R` ~ rotation angle :math:`\in [0, \pi]`


.. py:function:: rotation_in_bounds(rot, bounds)

   Maps angles to an interval 2pi-agnostically.

   Takes an array of rotations in radian and maps them to bounds aka limits if possible by adding/subtracting multiples
   of 2 pi. If not possible, will return the original input.

   :param rot: A 1xn array of rotations, given in radian.
   :param bounds: A 2xn array of limits, lower limits in first, upper limits in second row of the array. Every column
       in bounds corresponds to a column in rot.
   :returns: A variant of rot, offset by multiples of 2pi to fit within bounds. If that is not possible, returns rot.


.. py:function:: skew(vec3)

   Returns the skew symmetric matrix of a 3D vector


.. py:function:: spherical2cartesian(spher)

   Inverse to cartesian2spherical