robot-sim
diff --git a/‎spatialmath/base/__init__.py
Copy file name to clipboardExpand all lines: spatialmath/base/__init__.py
+2Lines changed: 2 additions & 0 deletions b/‎spatialmath/base/__init__.py
Copy file name to clipboardExpand all lines: spatialmath/base/__init__.py
+2Lines changed: 2 additions & 0 deletions
diff --git a/‎spatialmath/base/transforms3d.py
Copy file name to clipboardExpand all lines: spatialmath/base/transforms3d.py
+82Lines changed: 82 additions & 0 deletions b/‎spatialmath/base/transforms3d.py
Copy file name to clipboardExpand all lines: spatialmath/base/transforms3d.py
+82Lines changed: 82 additions & 0 deletions
diff --git a/‎tests/base/test_transforms3d.py
Copy file name to clipboardExpand all lines: tests/base/test_transforms3d.py
+32Lines changed: 32 additions & 0 deletions b/‎tests/base/test_transforms3d.py
Copy file name to clipboardExpand all lines: tests/base/test_transforms3d.py
+32Lines changed: 32 additions & 0 deletions
@@ -79,6 +79,8 @@
     'eul2tr',
     'angvec2r',
     'angvec2tr',
+    'exp2r',
+    'exp2tr',
     'oa2r',
     'oa2tr',
     'tr2angvec',
 
@@ -658,7 +658,89 @@ def angvec2tr(theta, v, unit='rad'):
     """
     return base.r2t(angvec2r(theta, v, unit=unit))
 
+# ---------------------------------------------------------------------------------------#
+
+def exp2r(w):
+    """
+    Create an SO(3) rotation matrix from exponential coordinates
+
+    :param w: exponential coordinate vector
+    :type w: array_like(3)
+    :return: SO(3) rotation matrix
+    :rtype: ndarray(3,3)
+    :raises ValueError: bad arguments
+
+    ``exp2r(w)`` is an SO(3) orthonormal rotation matrix
+    equivalent to a rotation of :math:`\| w \|` about the vector :math:`\hat{w}`.
+
+    If ``w`` is zero then result is the identity matrix.
+
+    .. runblock:: pycon
+
+        >>> from spatialmath.base import *
+        >>> eulervec2r([0.3, 0, 0])  # rotx(0.3)
+        >>> angvec2r([0, 0, 0])      # rotx(0)
+
+    .. note:: Exponential coordinates are also known as an Euler vector
 
+    :seealso: :func:`~angvec2r`, :func:`~tr2angvec`
+
+    :SymPy: not supported
+    """
+    if not base.isvector(w, 3):
+        raise ValueError("Arguments must be a 3-vector")
+
+    v, theta = base.unitvec_norm(w)
+
+    if theta is None:
+        return np.eye(3)
+
+    # Rodrigue's equation
+
+    sk = base.skew(v)
+    R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
+    return R
+
+def exp2tr(w):
+    """
+    Create an SE(3) pure rotation matrix from exponential coordinates
+
+    :param w: exponential coordinate vector
+    :type w: array_like(3)
+    :return: SO(3) rotation matrix
+    :rtype: ndarray(3,3)
+    :raises ValueError: bad arguments
+
+    ``exp2r(w)`` is an SO(3) orthonormal rotation matrix
+    equivalent to a rotation of :math:`\| w \|` about the vector :math:`\hat{w}`.
+
+    If ``w`` is zero then result is the identity matrix.
+
+    .. runblock:: pycon
+
+        >>> from spatialmath.base import *
+        >>> eulervec2r([0.3, 0, 0])  # rotx(0.3)
+        >>> angvec2r([0, 0, 0])      # rotx(0)
+
+    .. note:: Exponential coordinates are also known as an Euler vector
+
+    :seealso: :func:`~angvec2r`, :func:`~tr2angvec`
+
+    :SymPy: not supported
+    """
+    if not base.isvector(w, 3):
+        raise ValueError("Arguments must be a 3-vector")
+
+    v, theta = base.unitvec_norm(w)
+
+    if theta is None:
+        return np.eye(4)
+
+    # Rodrigue's equation
+
+    sk = base.skew(v)
+    R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
+    return base.r2t(R)
 # ---------------------------------------------------------------------------------------#
 def oa2r(o, a=None):
     """
 
@@ -228,6 +228,38 @@ def test_eul2tr(self):
         nt.assert_array_almost_equal(eul2tr(0.1 * r2d, 0.2 * r2d, 0.3 * r2d, unit='deg'), T)
         nt.assert_array_almost_equal(eul2tr([0.1 * r2d, 0.2 * r2d, 0.3 * r2d], unit='deg'), T)
 
+    def test_exp2r(self):
+
+        r2d = 180 / pi
+
+        nt.assert_array_almost_equal(exp2r([0, 0, 0]), rotx(0))
+        nt.assert_array_almost_equal(exp2r([pi/4, 0, 0]), rotx(pi/4))
+        nt.assert_array_almost_equal(exp2r([-pi/4, 0, 0]), rotx(-pi/4))
+
+        nt.assert_array_almost_equal(exp2r([0, 0, 0]), roty(0))
+        nt.assert_array_almost_equal(exp2r([0, pi/4, 0]), roty(pi/4))
+        nt.assert_array_almost_equal(exp2r([0, -pi/4, 0]), roty(-pi/4))
+
+        nt.assert_array_almost_equal(exp2r([0, 0, 0]), rotz(0))
+        nt.assert_array_almost_equal(exp2r([0, 0, pi/4]), rotz(pi/4))
+        nt.assert_array_almost_equal(exp2r([0, 0, -pi/4]), rotz(-pi/4))
+
+    def test_exp2tr(self):
+
+        r2d = 180 / pi
+
+        nt.assert_array_almost_equal(exp2tr([0, 0, 0]), trotx(0))
+        nt.assert_array_almost_equal(exp2tr([pi/4, 0, 0]), trotx(pi/4))
+        nt.assert_array_almost_equal(exp2tr([-pi/4, 0, 0]), trotx(-pi/4))
+
+        nt.assert_array_almost_equal(exp2tr([0, 0, 0]), troty(0))
+        nt.assert_array_almost_equal(exp2tr([0, pi/4, 0]), troty(pi/4))
+        nt.assert_array_almost_equal(exp2tr([0, -pi/4, 0]), troty(-pi/4))
+
+        nt.assert_array_almost_equal(exp2tr([0, 0, 0]), trotz(0))
+        nt.assert_array_almost_equal(exp2tr([0, 0, pi/4]), trotz(pi/4))
+        nt.assert_array_almost_equal(exp2tr([0, 0, -pi/4]), trotz(-pi/4))
+
     def test_tr2rpy(self):
         rpy = np.r_[0.1, 0.2, 0.3]
         R = rpy2r(rpy)