10000 polish quaternion log/exp · StephLin/spatialmath-python@9e114d6 · GitHub
[go: up one dir, main page]
Skip to content

Commit 9e114d6

Browse files
petercorkepetercorke
committed
polish quaternion log/exp
1 parent 71bebd4 commit 9e114d6

File tree

1 file changed

+53
-30
lines changed

1 file changed

+53
-30
lines changed

‎spatialmath/quaternion.py

Lines changed: 53 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,8 @@
2222
from spatialmath.pose3d import SO3, SE3
2323
from spatialmath.smuserlist import SMUserList
2424

25+
_eps = np.finfo(np.float64).eps
26+
2527
class Quaternion(SMUserList):
2628
r"""
2729
Quaternion class
@@ -106,7 +108,7 @@ def Pure(cls, v):
106108
.. runblock:: pycon
107109
108110
>>> from spatialmath import Quaternion
109-
>>> print(Quaternion.pure([1,2,3]))
111+
>>> print(Quaternion.Pure([1,2,3]))
110112
"""
111113
return cls(s=0, v=base.getvector(v, 3))
112114

@@ -267,7 +269,7 @@ def conj(self):
267269
.. runblock:: pycon
268270
269271
>>> from spatialmath import Quaternion
270-
>>> print(Quaternion.pure([1,2,3]).conj())
272+
>>> print(Quaternion.Pure([1,2,3]).conj())
271273
272274
:seealso: :func:`~spatialmath.base.quaternions.conj`
273275
"""
@@ -335,19 +337,26 @@ def log(self):
335337
336338
.. math::
337339
338-
\ln \| q \| \langle \frac{\mathb{v}}{\| \mathbf{v} \|} \acos \frac{s}{\| q \|} \rangle
340+
\ln \| q \|, \langle \frac{\vec{v}}{\| \vec{v} \|} \cos^{-1} \frac{s}{\| q \|} \rangle
341+
342+
For a ``UnitQuaternion`` the logarithm is a pure quaternion whose vector
343+
part :math:`\vec{v}` and :math:`\vec{v}/2` is a Euler vector: parallel
344+
to the axis of rotation and whose norm is the magnitude of rotation.
339345
340346
Example:
341347
342348
.. runblock:: pycon
343349
344-
>>> from spatialmath import Quaternion
350+
>>> from spatialmath import Quaternion, UnitQuaternion
351+
>>> from math import pi
345352
>>> q = Quaternion([1, 2, 3, 4])
346353
>>> print(q.log())
354+
>>> q = UnitQuaternion.Rx(pi / 2)
355+
>>> print(q.log())
347356
348357
:reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
349358
350-
:seealso: :func:`~spatialmath.quaternion.UnitQuaternion.log`, `~spatialmath.quaternion.Quaternion.exp`
359+
:seealso: :func:`~spatialmath.quaternion.Quaternion.exp`, :func:`~spatialmath.quaternion.Quaternion.log`, :func:`~spatialmath.quaternion.UnitQuaternion.angvec`,
351360
"""
352361
norm = self.norm()
353362
s = math.log(norm)
@@ -364,24 +373,38 @@ def exp(self):
364373
365374
.. math::
366375
367-
e^s \cos \| v \| \langle e^s \frac{\mathb{v}}{\| \mathbf{v} \|} \sin \| \mathbf{v} \| \rangle
376+
e^s \cos \| v \|, \langle e^s \frac{\vec{v}}{\| \vec{v} \|} \sin \| \vec{v} \| \rangle
377+
378+
For a pure quaternion with vector value :math:`\vec{v}` the the result
379+
is a unit quaternion equivalent to a rotation defined by
380+
:math:`2\vec{v}` intepretted as an Euler vector, that is, parallel to
381+
the axis of rotation and whose norm is the magnitude of rotation.
368382
369383
Example:
370384
371385
.. runblock:: pycon
372386
373387
>>> from spatialmath import Quaternion
388+
>>> from math import pi
374389
>>> q = Quaternion([1, 2, 3, 4])
375390
>>> print(q.exp())
391+
>>> q = Quaternion.Pure([pi / 4, 0, 0])
392+
>>> print(q.exp()) # result is a UnitQuaternion
393+
>>> print(q.exp().angvec())
376394
377395
:reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
378396
379-
:seealso: :func:`~spatialmath.quaternion.UnitQuaternion.log`, `~spatialmath.quaternion.Quaternion.log`
397+
:seealso: :func:`~spatialmath.quaternion.Quaternion.log`, :func:`~spatialmath.quaternion.UnitQuaternion.log`, :func:`~spatialmath.quaternion.UnitQuaternion.AngVec`, :func:`~spatialmath.quaternion.UnitQuaternion.EulerVec`
380398
"""
381-
es = math.exp(self.s)
382-
s = es * math.cos(base.norm(self.v))
383-
v = es * base.unitvec(self.v) * math.sin(base.norm(self.v))
384-
return Quaternion(s=s, v=v)
399+
exp_s = math.exp(self.s)
400+
norm_v = base.norm(self.v)
401+
s = exp_s * math.cos(norm_v)
402+
v = exp_s * self.v / norm_v * math.sin(norm_v)
403+
if abs(self.s) < 100 * _eps:
404+
# result will be a unit quaternion
405+
return UnitQuaternion(s=s, v=v)
406+
else:
407+
return Quaternion(s=s, v=v)
385408

386409

387410
def inner(self, other):
@@ -936,7 +959,7 @@ def __init__(self, s: Any = None, v=None, norm=True, check=True):
936959
937960
.. runblock:: pycon
938961
939-
>>> from spatialmath import UnitQuaternion as UQ as UQ
962+
>>> from spatialmath import UnitQuaternion as UQ
940963
>>> q = UQ()
941964
>>> q # repr()
942965
>>> print(q) # str()
@@ -1315,7 +1338,7 @@ def AngVec(cls, theta, v, *, unit='rad'):
13151338
.. note:: :math:`\theta = 0` the result in an identity quaternion, otherwise
13161339
``V`` must have a finite length, ie. :math:`|V| > 0`.
13171340
1318-
:seealso: :func:`~spatialmath.UnitQuaternion.angvec`, :func:`~spatialmath.base.transforms3d.angvec2r`
1341+
:seealso: :func:`~spatialmath.UnitQuaternion.angvec`, :func:`~spatialmath.quaternion.UnitQuaternion.exp`, :func:`~spatialmath.base.transforms3d.angvec2r`
13191342
"""
13201343
v = base.getvector(v, 3)
13211344
base.isscalar(theta)
@@ -1932,35 +1955,35 @@ def angvec(self, unit='rad'):
19321955
>>> UQ.Rz(0.3).angvec()
19331956
(0.3, array([0., 0., 1.]))
19341957
1935-
:seealso: :func:`~spatialmath.quaternion.AngVec`, :func:`~angvec2r`
1958+
:seealso: :func:`~spatialmath.quaternion.AngVec`, :func:`~spatialmath.quaternion.UnitQuaternion.log`, :func:`~angvec2r`
19361959
"""
19371960
return base.tr2angvec(self.R, unit=unit)
19381961

1939-
def log(self):
1940-
r"""
1941-
Logarithm of unit quaternion
1962+
# def log(self):
1963+
# r"""
1964+
# Logarithm of unit quaternion
19421965

1943-
:rtype: Quaternion instance
1966+
# :rtype: Quaternion instance
19441967

1945-
``q.log()`` is the logarithm of the unit quaternion ``q``, ie.
1968+
# ``q.log()`` is the logarithm of the unit quaternion ``q``, ie.
19461969

1947-
.. math::
1970+
# .. math::
19481971

1949-
0 \langle \frac{\mathb{v}}{\| \mathbf{v} \|} \acos s \rangle
1972+
# 0 \langle \frac{\mathb{v}}{\| \mathbf{v} \|} \acos s \rangle
19501973

1951-
Example:
1974+
# Example:
19521975

1953-
.. runblock:: pycon
1976+
# .. runblock:: pycon
19541977

1955-
>>> from spatialmath import UnitQuaternion
1956-
>>> q = UnitQuaternion.Rx(0.3)
1957-
>>> print(q.log())
1978+
# >>> from spatialmath import UnitQuaternion
1979+
# >>> q = UnitQuaternion.Rx(0.3)
1980+
# >>> print(q.log())
19581981

1959-
:reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
1982+
# :reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
19601983

1961-
:seealso: :func:`~spatialmath.quaternion.Quaternion.log`, `~spatialmath.quaternion.Quaternion.exp`
1962-
"""
1963-
return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
1984+
# :seealso: :func:`~spatialmath.quaternion.Quaternion.log`, `~spatialmath.quaternion.Quaternion.exp`
1985+
# """
1986+
# return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
19641987

19651988
def angle(self, other):
19661989
"""

0 commit comments

Comments
 (0)
0