adam.core.spatial_math
======================

.. py:module:: adam.core.spatial_math


Classes
-------

.. autoapisummary::

   adam.core.spatial_math.ArrayLike
   adam.core.spatial_math.ArrayLikeFactory
   adam.core.spatial_math.SpatialMath


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

.. py:class:: ArrayLike

   Bases: :py:obj:`abc.ABC`


   Abstract class for a generic Array wrapper. Every method should be implemented for every data type.


   .. py:method:: __add__(other)
      :abstractmethod:



   .. py:method:: __radd__(other)
      :abstractmethod:



   .. py:method:: __sub__(other)
      :abstractmethod:



   .. py:method:: __rsub__(other)
      :abstractmethod:



   .. py:method:: __mul__(other)
      :abstractmethod:



   .. py:method:: __rmul__(other)
      :abstractmethod:



   .. py:method:: __matmul__(other)
      :abstractmethod:



   .. py:method:: __rmatmul__(other)
      :abstractmethod:



   .. py:method:: __neg__()
      :abstractmethod:



   .. py:method:: __getitem__(item)
      :abstractmethod:



   .. py:method:: __truediv__(other)
      :abstractmethod:



   .. py:property:: T
      :abstractmethod:


      Transpose of the array

      :type: Returns


   .. py:method:: __len__()


   .. py:method:: __repr__()


   .. py:method:: as_list()


.. py:class:: ArrayLikeFactory

   Bases: :py:obj:`abc.ABC`


   Abstract class for a generic Array wrapper. Every method should be implemented for every data type.


   .. py:method:: zeros(*x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: matrix dimension
      :type x: npt.ArrayLike

      :returns: zero matrix of dimension x
      :rtype: npt.ArrayLike



   .. py:method:: eye(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: matrix dimension
      :type x: npt.ArrayLike

      :returns: identity matrix of dimension x
      :rtype: npt.ArrayLike



   .. py:method:: asarray(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: array
      :type x: npt.ArrayLike

      :returns: array
      :rtype: npt.ArrayLike



   .. py:method:: ones_like(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: array
      :type x: npt.ArrayLike

      :returns: one array with the same shape as x
      :rtype: npt.ArrayLike



   .. py:method:: zeros_like(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: array
      :type x: npt.ArrayLike

      :returns: one array with the same shape as x
      :rtype: npt.ArrayLike



   .. py:method:: tile(x: numpy.typing.ArrayLike, reps: tuple) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: input array
      :type x: npt.ArrayLike
      :param reps: repetition factors for each dimension
      :type reps: tuple

      :returns: tiled array
      :rtype: npt.ArrayLike



.. py:class:: SpatialMath(factory: ArrayLikeFactory)

   Class implementing the main geometric functions used for computing rigid-body algorithm

   :param ArrayLike: abstract class describing a generic Array wrapper. It needs to be implemented for every data type


   .. py:attribute:: _factory


   .. py:property:: factory
      :type: ArrayLikeFactory



   .. py:method:: vertcat(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: elements
      :type x: npt.ArrayLike

      :returns: vertical concatenation of elements x
      :rtype: npt.ArrayLike



   .. py:method:: horzcat(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: elements
      :type x: npt.ArrayLike

      :returns: horizontal concatenation of elements x
      :rtype: npt.ArrayLike



   .. py:method:: concatenate(x: numpy.typing.ArrayLike, axis: int) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: elements
      :type x: npt.ArrayLike
      :param axis: axis along which to concatenate
      :type axis: int

      :returns: concatenation of elements x along axis
      :rtype: npt.ArrayLike



   .. py:method:: stack(x: numpy.typing.ArrayLike, axis: int) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: elements
      :type x: npt.ArrayLike
      :param axis: axis along which to stack
      :type axis: int

      :returns: stacked elements x along axis
      :rtype: npt.ArrayLike



   .. py:method:: tile(x: numpy.typing.ArrayLike, reps: tuple) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: input array
      :type x: npt.ArrayLike
      :param reps: repetition factors for each dimension
      :type reps: tuple

      :returns: tiled array
      :rtype: npt.ArrayLike



   .. py:method:: transpose(x: numpy.typing.ArrayLike, dims: tuple) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: input array
      :type x: npt.ArrayLike
      :param dims: permutation of dimensions
      :type dims: tuple

      :returns: transposed array
      :rtype: npt.ArrayLike



   .. py:method:: inv(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: input array
      :type x: npt.ArrayLike

      :returns: inverse of the array
      :rtype: npt.ArrayLike



   .. py:method:: mtimes(x: numpy.typing.ArrayLike, y: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:



   .. py:method:: sin(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: angle value
      :type x: npt.ArrayLike

      :returns: sin value of x
      :rtype: npt.ArrayLike



   .. py:method:: cos(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike
      :abstractmethod:


      :param x: angle value
      :type x: npt.ArrayLike

      :returns: cos value of angle x
      :rtype: npt.ArrayLike



   .. py:method:: skew(x)
      :abstractmethod:



   .. py:method:: R_from_axis_angle(axis, q)

      axis: (...,3) - normalized axis vector, batched if q is batched
      q   : (...,)  - rotation angle, batched or scalar
      returns: (...,3,3) - rotation matrix



   .. py:method:: sxm(s: numpy.typing.ArrayLike, m: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      Computes scalar multiplication with a matrix

      :param s: scalar value
      :type s: npt.ArrayLike
      :param m: matrix to be multiplied
      :type m: npt.ArrayLike

      :returns: result of the multiplication
      :rtype: npt.ArrayLike



   .. py:method:: Rx(q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param q: angle value
      :type q: npt.ArrayLike

      :returns: rotation matrix around x axis
      :rtype: npt.ArrayLike



   .. py:method:: Ry(q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param q: angle value
      :type q: npt.ArrayLike

      :returns: rotation matrix around y axis
      :rtype: npt.ArrayLike



   .. py:method:: Rz(q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param q: angle value
      :type q: npt.ArrayLike

      :returns: rotation matrix around z axis
      :rtype: npt.ArrayLike



   .. py:method:: H_revolute_joint(xyz: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike, axis: numpy.typing.ArrayLike, q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param xyz: joint origin in the urdf
      :type xyz: npt.ArrayLike
      :param rpy: joint orientation in the urdf
      :type rpy: npt.ArrayLike
      :param axis: joint axis in the urdf
      :type axis: npt.ArrayLike
      :param q: joint angle value
      :type q: npt.ArrayLike

      :returns: Homogeneous transform
      :rtype: npt.ArrayLike



   .. py:method:: homogeneous(R, p)


   .. py:method:: H_prismatic_joint(xyz: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike, axis: numpy.typing.ArrayLike, q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param xyz: joint origin in the urdf
      :type xyz: npt.ArrayLike
      :param rpy: joint orientation in the urdf
      :type rpy: npt.ArrayLike
      :param axis: joint axis in the urdf
      :type axis: npt.ArrayLike
      :param q: joint angle value
      :type q: npt.ArrayLike

      :returns: Homogeneous transform
      :rtype: npt.ArrayLike



   .. py:method:: H_from_Pos_RPY(xyz: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param xyz: translation vector
      :type xyz: npt.ArrayLike
      :param rpy: rotation as rpy angles
      :type rpy: npt.ArrayLike

      :returns: Homegeneous transform
      :rtype: npt.ArrayLike



   .. py:method:: R_from_RPY(rpy: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param rpy: rotation as rpy angles
      :type rpy: npt.ArrayLike

      :returns: Rotation matrix
      :rtype: npt.ArrayLike



   .. py:method:: X_revolute_joint(xyz: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike, axis: numpy.typing.ArrayLike, q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param xyz: joint origin in the urdf
      :type xyz: npt.ArrayLike
      :param rpy: joint orientation in the urdf
      :type rpy: npt.ArrayLike
      :param axis: joint axis in the urdf
      :type axis: npt.ArrayLike
      :param q: joint angle value
      :type q: npt.ArrayLike

      :returns: Spatial transform of a revolute joint given its rotation angle
      :rtype: npt.ArrayLike



   .. py:method:: X_prismatic_joint(xyz: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike, axis: numpy.typing.ArrayLike, q: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param xyz: joint origin in the urdf
      :type xyz: npt.ArrayLike
      :param rpy: joint orientation in the urdf
      :type rpy: npt.ArrayLike
      :param axis: joint axis in the urdf
      :type axis: npt.ArrayLike
      :param q: joint angle value
      :type q: npt.ArrayLike

      :returns: Spatial transform of a prismatic joint given its increment
      :rtype: npt.ArrayLike



   .. py:method:: X_fixed_joint(xyz: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param xyz: joint origin in the urdf
      :type xyz: npt.ArrayLike
      :param rpy: joint orientation in the urdf
      :type rpy: npt.ArrayLike

      :returns: Spatial transform of a fixed joint
      :rtype: npt.ArrayLike



   .. py:method:: _X_from_H(T)


   .. py:method:: spatial_transform(R: numpy.typing.ArrayLike, p: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param R: Rotation matrix
      :type R: npt.ArrayLike
      :param p: translation vector
      :type p: npt.ArrayLike

      :returns: spatial transform
      :rtype: npt.ArrayLike



   .. py:method:: spatial_inertia(inertia_matrix: numpy.typing.ArrayLike, mass: numpy.typing.ArrayLike, c: numpy.typing.ArrayLike, rpy: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param inertia_matrix: inertia values from urdf
      :type inertia_matrix: npt.ArrayLike
      :param mass: mass value from urdf
      :type mass: npt.ArrayLike
      :param c: origin of the link from urdf
      :type c: npt.ArrayLike
      :param rpy: orientation of the link from the urdf
      :type rpy: npt.ArrayLike

      :returns: the 6x6 inertia matrix expressed at the origin of the link (with rotation)
      :rtype: npt.ArrayLike



   .. py:method:: spatial_inertia_with_parameters(inertia_matrix, mass, c, rpy)

      :param I: inertia values parametric
      :type I: npt.ArrayLike
      :param mass: mass value parametric
      :type mass: npt.ArrayLike
      :param c: origin of the link parametric
      :type c: npt.ArrayLike
      :param rpy: orientation of the link from urdf
      :type rpy: npt.ArrayLike

      :returns: the 6x6 inertia matrix parametric expressed at the origin of the link (with rotation)
      :rtype: npt.ArrayLike



   .. py:method:: spatial_skew(v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param v: 6D vector
      :type v: npt.ArrayLike

      :returns: spatial skew matrix
      :rtype: npt.ArrayLike



   .. py:method:: spatial_skew_star(v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param v: 6D vector
      :type v: npt.ArrayLike

      :returns: negative spatial skew matrix traspose
      :rtype: npt.ArrayLike



   .. py:method:: adjoint(H: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike

      :returns: adjoint matrix
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_derivative(H: numpy.typing.ArrayLike, v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike
      :param v: 6D twist
      :type v: npt.ArrayLike

      :returns: adjoint matrix derivative
      :rtype: npt.ArrayLike



   .. py:method:: mxv(m: numpy.typing.ArrayLike, v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param m: Matrix
      :type m: npt.ArrayLike
      :param v: Vector
      :type v: npt.ArrayLike

      :returns: Result of matrix-vector multiplication
      :rtype: npt.ArrayLike



   .. py:method:: vxs(v: numpy.typing.ArrayLike, s: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param v: Vector
      :type v: npt.ArrayLike
      :param s: Scalar
      :type s: npt.ArrayLike

      :returns: Result of vector cross product with scalar multiplication
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_inverse(H: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike

      :returns: adjoint matrix
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_inverse_derivative(H: numpy.typing.ArrayLike, v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike
      :param v: 6D twist
      :type v: npt.ArrayLike

      :returns: adjoint matrix derivative
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_mixed(H: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike

      :returns: adjoint matrix
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_mixed_inverse(H: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike

      :returns: adjoint matrix
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_mixed_derivative(H: numpy.typing.ArrayLike, v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike
      :param v: 6D twist
      :type v: npt.ArrayLike

      :returns: adjoint matrix derivative
      :rtype: npt.ArrayLike



   .. py:method:: adjoint_mixed_inverse_derivative(H: numpy.typing.ArrayLike, v: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike
      :param v: 6D twist
      :type v: npt.ArrayLike

      :returns: adjoint matrix derivative
      :rtype: npt.ArrayLike



   .. py:method:: homogeneous_inverse(H: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param H: Homogeneous transform
      :type H: npt.ArrayLike

      :returns: inverse of the homogeneous transform
      :rtype: npt.ArrayLike



   .. py:method:: zeros(*x: int) -> numpy.typing.ArrayLike

      :param x: dimension
      :type x: int

      :returns: zero matrix of dimension x
      :rtype: npt.ArrayLike



   .. py:method:: eye(x: int) -> numpy.typing.ArrayLike

      :param x: dimension
      :type x: int

      :returns: identity matrix of dimension x
      :rtype: npt.ArrayLike



   .. py:method:: asarray(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param x: array
      :type x: npt.ArrayLike

      :returns: array
      :rtype: npt.ArrayLike



   .. py:method:: zeros_like(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param x: array
      :type x: npt.ArrayLike

      :returns: zero array with the same shape as x
      :rtype: npt.ArrayLike



   .. py:method:: ones_like(x: numpy.typing.ArrayLike) -> numpy.typing.ArrayLike

      :param x: array
      :type x: npt.ArrayLike

      :returns: one array with the same shape as x
      :rtype: npt.ArrayLike