Skip to content

Commit 9604839

Browse files
committed
new: create SO3/SE3 from exponential coordinates
1 parent c2553fa commit 9604839

3 files changed

Lines changed: 116 additions & 0 deletions

File tree

‎spatialmath/base/__init__.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,8 @@
7979
'eul2tr',
8080
'angvec2r',
8181
'angvec2tr',
82+
'exp2r',
83+
'exp2tr',
8284
'oa2r',
8385
'oa2tr',
8486
'tr2angvec',

‎spatialmath/base/transforms3d.py‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -658,7 +658,89 @@ def angvec2tr(theta, v, unit='rad'):
658658
"""
659659
return base.r2t(angvec2r(theta, v, unit=unit))
660660

661+
# ---------------------------------------------------------------------------------------#
662+
663+
def exp2r(w):
664+
"""
665+
Create an SO(3) rotation matrix from exponential coordinates
666+
667+
:param w: exponential coordinate vector
668+
:type w: array_like(3)
669+
:return: SO(3) rotation matrix
670+
:rtype: ndarray(3,3)
671+
:raises ValueError: bad arguments
672+
673+
``exp2r(w)`` is an SO(3) orthonormal rotation matrix
674+
equivalent to a rotation of :math:`\| w \|` about the vector :math:`\hat{w}`.
675+
676+
If ``w`` is zero then result is the identity matrix.
677+
678+
.. runblock:: pycon
679+
680+
>>> from spatialmath.base import *
681+
>>> eulervec2r([0.3, 0, 0]) # rotx(0.3)
682+
>>> angvec2r([0, 0, 0]) # rotx(0)
683+
684+
.. note:: Exponential coordinates are also known as an Euler vector
661685
686+
:seealso: :func:`~angvec2r`, :func:`~tr2angvec`
687+
688+
:SymPy: not supported
689+
"""
690+
if not base.isvector(w, 3):
691+
raise ValueError("Arguments must be a 3-vector")
692+
693+
v, theta = base.unitvec_norm(w)
694+
695+
if theta is None:
696+
return np.eye(3)
697+
698+
# Rodrigue's equation
699+
700+
sk = base.skew(v)
701+
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
702+
return R
703+
704+
def exp2tr(w):
705+
"""
706+
Create an SE(3) pure rotation matrix from exponential coordinates
707+
708+
:param w: exponential coordinate vector
709+
:type w: array_like(3)
710+
:return: SO(3) rotation matrix
711+
:rtype: ndarray(3,3)
712+
:raises ValueError: bad arguments
713+
714+
``exp2r(w)`` is an SO(3) orthonormal rotation matrix
715+
equivalent to a rotation of :math:`\| w \|` about the vector :math:`\hat{w}`.
716+
717+
If ``w`` is zero then result is the identity matrix.
718+
719+
.. runblock:: pycon
720+
721+
>>> from spatialmath.base import *
722+
>>> eulervec2r([0.3, 0, 0]) # rotx(0.3)
723+
>>> angvec2r([0, 0, 0]) # rotx(0)
724+
725+
.. note:: Exponential coordinates are also known as an Euler vector
726+
727+
:seealso: :func:`~angvec2r`, :func:`~tr2angvec`
728+
729+
:SymPy: not supported
730+
"""
731+
if not base.isvector(w, 3):
732+
raise ValueError("Arguments must be a 3-vector")
733+
734+
v, theta = base.unitvec_norm(w)
735+
736+
if theta is None:
737+
return np.eye(4)
738+
739+
# Rodrigue's equation
740+
741+
sk = base.skew(v)
742+
R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
743+
return base.r2t(R)
662744
# ---------------------------------------------------------------------------------------#
663745
def oa2r(o, a=None):
664746
"""

‎tests/base/test_transforms3d.py‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -228,6 +228,38 @@ def test_eul2tr(self):
228228
nt.assert_array_almost_equal(eul2tr(0.1 * r2d, 0.2 * r2d, 0.3 * r2d, unit='deg'), T)
229229
nt.assert_array_almost_equal(eul2tr([0.1 * r2d, 0.2 * r2d, 0.3 * r2d], unit='deg'), T)
230230

231+
def test_exp2r(self):
232+
233+
r2d = 180 / pi
234+
235+
nt.assert_array_almost_equal(exp2r([0, 0, 0]), rotx(0))
236+
nt.assert_array_almost_equal(exp2r([pi/4, 0, 0]), rotx(pi/4))
237+
nt.assert_array_almost_equal(exp2r([-pi/4, 0, 0]), rotx(-pi/4))
238+
239+
nt.assert_array_almost_equal(exp2r([0, 0, 0]), roty(0))
240+
nt.assert_array_almost_equal(exp2r([0, pi/4, 0]), roty(pi/4))
241+
nt.assert_array_almost_equal(exp2r([0, -pi/4, 0]), roty(-pi/4))
242+
243+
nt.assert_array_almost_equal(exp2r([0, 0, 0]), rotz(0))
244+
nt.assert_array_almost_equal(exp2r([0, 0, pi/4]), rotz(pi/4))
245+
nt.assert_array_almost_equal(exp2r([0, 0, -pi/4]), rotz(-pi/4))
246+
247+
def test_exp2tr(self):
248+
249+
r2d = 180 / pi
250+
251+
nt.assert_array_almost_equal(exp2tr([0, 0, 0]), trotx(0))
252+
nt.assert_array_almost_equal(exp2tr([pi/4, 0, 0]), trotx(pi/4))
253+
nt.assert_array_almost_equal(exp2tr([-pi/4, 0, 0]), trotx(-pi/4))
254+
255+
nt.assert_array_almost_equal(exp2tr([0, 0, 0]), troty(0))
256+
nt.assert_array_almost_equal(exp2tr([0, pi/4, 0]), troty(pi/4))
257+
nt.assert_array_almost_equal(exp2tr([0, -pi/4, 0]), troty(-pi/4))
258+
259+
nt.assert_array_almost_equal(exp2tr([0, 0, 0]), trotz(0))
260+
nt.assert_array_almost_equal(exp2tr([0, 0, pi/4]), trotz(pi/4))
261+
nt.assert_array_almost_equal(exp2tr([0, 0, -pi/4]), trotz(-pi/4))
262+
231263
def test_tr2rpy(self):
232264
rpy = np.r_[0.1, 0.2, 0.3]
233265
R = rpy2r(rpy)

0 commit comments

Comments
 (0)