Merge pull request #70 from rtyh12/urdf-parser

URDF parser

.gitignore

@@ -1,4 +1,5 @@
 src/Trip.egg-info
+src/trip_kinematics.egg-info
 docs/build/
 build/
 src/dist

docs/source/from_urdf_tutorial.rst

@@ -0,0 +1,21 @@
+How to use the urdf parser
+**************************
+The URDF parser allows the usage of URDF files with TriP by converting the described robot into a list of TriP Transformations.
+
+It can be used directly after importing the TriP library by calling the function trip_kinematics.from_urdf with the path to the URDF file as the argument. Note that the function returns a list of Transformations, which you probably want to create a Robot from in most cases:
+
+.. code-block:: python
+
+    transformations_list = trip_kinematics.urdf_parser.from_urdf(urdf_path)
+    robot = Robot(transformations_list)
+
+
+This means you can also add other Transformations manually on top of those specified in the URDF file, if required.
+
+Also note that the parser includes defaults for certain values if the corresponding URDF tag is missing, specifically:
+
+* <origin> defaults to <origin xyz="0 0 0" rpy="0 0 0" />.
+
+  * (The same also applies if only one of xyz and rpy is specified, with the omitted value defaulting to "0 0 0")
+
+* <axis> defaults to <axis  xyz="0 0 1" />

src/setup.py

@@ -7,6 +7,6 @@
     author='Torben Miller, Jan Baumgärtner',
     license='MIT',
     description='...',
-    install_requires=['casadi>=3.5.5', 'numpy>=1.17.4, < 1.20.0'],
+    install_requires=['casadi>=3.5.5', 'numpy>=1.17.4, < 1.20.0', 'defusedxml>=0.5'],
     packages=['trip_kinematics', 'trip_robots']
 )

src/trip_kinematics/Utility.py

@@ -1,14 +1,149 @@
-from numpy import array
+from importlib_metadata import re
+import numpy as np
 from casadi import cos, sin
 
 
+class Rotation:
+    """Represents a 3D rotation.
+
+    Can be initialized from quaternions, rotation matrices, or Euler angles, and can be represented
+    as quaternions. Reimplements a (small) part of the scipy.spatial.transform.Rotation API and is
+    meant to be used for converting between rotation representations to avoid depending on SciPy.
+    This class is not meant to be instantiated directly using __init__; use the methods
+    from_[representation] instead.
+
+    """
+
+    def __init__(self, quat):
+        quat /= np.linalg.norm(quat)
+
+        self._xyzw = quat
+
+    @classmethod
+    def from_quat(cls, xyzw, scalar_first=True):
+        """Creates a :py:class`Rotation` object from a quaternion.
+
+        Args:
+            xyzw (np.array): Quaternion.
+            scalar_first (bool, optional): Whether the quaternion is in scalar-first (w, x, y, z) or
+                                           scalar-last (x, y, z, w) format. Defaults to True.
+
+        Returns:
+            Rotation: Rotation object.
+
+        """
+        if scalar_first:
+            return cls(xyzw[[1, 2, 3, 0]])
+        return cls(xyzw)
+
+    @classmethod
+    def from_euler(cls, seq, rpy, degrees=False):
+        """Creates a :py:class`Rotation` object from Euler angles.
+
+        Args:
+            seq (str): Included for compatibility with the SciPy API. Required to be set to 'xyz'.
+            rpy (np.array): Euler angles.
+            degrees (bool, optional): True for degrees and False for radians. Defaults to False.
+
+        Returns:
+            Rotation: Rotation object.
+
+        """
+        # pylint: disable=invalid-name
+
+        assert seq == 'xyz'
+        rpy = np.array(rpy)
+
+        if degrees:
+            rpy = rpy * (np.pi / 180)
+
+        sin_r = np.sin(0.5 * rpy[0])
+        cos_r = np.cos(0.5 * rpy[0])
+        sin_p = np.sin(0.5 * rpy[1])
+        cos_p = np.cos(0.5 * rpy[1])
+        sin_y = np.sin(0.5 * rpy[2])
+        cos_y = np.cos(0.5 * rpy[2])
+
+        x = sin_r * cos_p * cos_y - cos_r * sin_p * sin_y
+        y = cos_r * sin_p * cos_y + sin_r * cos_p * sin_y
+        z = cos_r * cos_p * sin_y - sin_r * sin_p * cos_y
+        w = cos_r * cos_p * cos_y + sin_r * sin_p * sin_y
+
+        return cls(np.array([x, y, z, w]))
+
+    @classmethod
+    def from_matrix(cls, matrix):
+        """Creates a :py:class`Rotation` object from a rotation matrix.
+
+        Uses a very similar algorithm to scipy.spatial.transform.Rotation.from_matrix().
+        See https://github.com/scipy/scipy/blob/22f66bbd83867459f1491abf01b860b5ef4e026e/
+        scipy/spatial/transform/_rotation.pyx
+
+        Args:
+            matrix (np.array): Rotation matrix.
+
+        Returns:
+            Rotation: Rotation object.
+
+        """
+        diag1 = matrix[0, 0]
+        diag2 = matrix[1, 1]
+        diag3 = matrix[2, 2]
+        thing = diag1 + diag2 + diag3
+        diag = np.array([diag1, diag2, diag3, thing])
+        argmax = np.argmax(diag)
+        quat = np.zeros(4)
+
+        if argmax != 3:
+            i = argmax
+            j = (i + 1) % 3
+            k = (j + 1) % 3
+
+            quat[i] = 1 - diag[3] + 2 * matrix[i, i]
+            quat[j] = matrix[j, i] + matrix[i, j]
+            quat[k] = matrix[k, i] + matrix[i, k]
+            quat[3] = matrix[k, j] - matrix[j, k]
+
+        else:
+            quat[0] = matrix[2, 1] - matrix[1, 2]
+            quat[1] = matrix[0, 2] - matrix[2, 0]
+            quat[2] = matrix[1, 0] - matrix[0, 1]
+            quat[3] = 1 + diag[3]
+
+        return cls(quat)
+
+    def as_quat(self, scalar_first=True):
+        """Represents the object as a quaternion.
+
+        Args:
+            scalar_first (bool, optional): Represent the quaternion in scalar-first (w, x, y, z)
+                                           or scalar-last (x, y, z, w) format. Defaults to True.
+
+        Returns:
+            np.array: Quaternion.
+
+        """
+        if scalar_first:
+            return self._xyzw[[3, 0, 1, 2]]
+        return self._xyzw
+
+    def __str__(self):
+        return f'''Rotation xyzw={self._xyzw}'''
+
+    def __repr__(self):
+        return f'''Rotation xyzw={self._xyzw}'''
+
+
+print(1)
+
+
 def identity_transformation():
     """Returns a 4x4 identity matix
 
     Returns:
-        numy.array: a 4x4 identity matrix
+        numpy.array: a 4x4 identity matrix
     """
-    return array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=object)
+    return np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype=object)
 
 
 def hom_translation_matrix(t_x=0, t_y=0, t_z=0):
@@ -22,7 +157,7 @@ def hom_translation_matrix(t_x=0, t_y=0, t_z=0):
     Returns:
         numpy.array: A 4x4 homogenous translation matrix
     """
-    return array(
+    return np.array(
         [[1, 0, 0, t_x], [0, 1, 0, t_y], [0, 0, 1, t_z], [0., 0., 0., 1.]], dtype=object)
 
 
@@ -40,7 +175,7 @@ def hom_rotation(rotation_matrix):
     return matrix
 
 
-def quat_rotation_matrix(q_w, q_x, q_y, q_z) -> array:
+def quat_rotation_matrix(q_w, q_x, q_y, q_z) -> np.array:
     """Generates a 3x3 rotation matrix from q quaternion
 
     Args:
@@ -52,10 +187,10 @@ def quat_rotation_matrix(q_w, q_x, q_y, q_z) -> array:
     Returns:
         numpy.array: A 3x3 rotation matrix
     """
-    return array([[1-2*(q_y**2+q_z**2), 2*(q_x*q_y-q_z*q_w), 2*(q_x*q_z + q_y*q_w)],
-                 [2*(q_x*q_y + q_z*q_w), 1-2 *
+    return np.array([[1-2*(q_y**2+q_z**2), 2*(q_x*q_y-q_z*q_w), 2*(q_x*q_z + q_y*q_w)],
+                     [2*(q_x*q_y + q_z*q_w), 1-2 *
                      (q_x**2+q_z**2), 2*(q_y*q_z - q_x*q_w)],
-                 [2*(q_x*q_z-q_y*q_w), 2*(q_y*q_z+q_x*q_w), 1-2*(q_x**2+q_y**2)]], dtype=object)
+                     [2*(q_x*q_z-q_y*q_w), 2*(q_y*q_z+q_x*q_w), 1-2*(q_x**2+q_y**2)]], dtype=object)
 
 
 def x_axis_rotation_matrix(theta):
@@ -67,9 +202,9 @@ def x_axis_rotation_matrix(theta):
     Returns:
         numpy.array: A 3x3 rotation matrix
     """
-    return array([[1, 0, 0],
-                  [0, cos(theta), -sin(theta)],
-                  [0, sin(theta), cos(theta)]], dtype=object)
+    return np.array([[1, 0, 0],
+                     [0, cos(theta), -sin(theta)],
+                     [0, sin(theta), cos(theta)]], dtype=object)
 
 
 def y_axis_rotation_matrix(theta):
@@ -81,9 +216,9 @@ def y_axis_rotation_matrix(theta):
     Returns:
         numpy.array: A 3x3 rotation matrix
     """
-    return array([[cos(theta), 0, sin(theta)],
-                  [0, 1, 0],
-                  [-sin(theta), 0, cos(theta)]], dtype=object)
+    return np.array([[cos(theta), 0, sin(theta)],
+                     [0, 1, 0],
+                     [-sin(theta), 0, cos(theta)]], dtype=object)
 
 
 def z_axis_rotation_matrix(theta):
@@ -95,9 +230,9 @@ def z_axis_rotation_matrix(theta):
     Returns:
         numpy.array: A 3x3 rotation matrix
     """
-    return array([[cos(theta), -sin(theta), 0],
-                  [sin(theta), cos(theta), 0],
-                  [0, 0, 1]], dtype=object)
+    return np.array([[cos(theta), -sin(theta), 0],
+                     [sin(theta), cos(theta), 0],
+                     [0, 0, 1]], dtype=object)
 
 
 def get_rotation(matrix):