Skip to content

Commit ec00d13

Browse files
committed
Allow Eul RPY constructor to accept positional args as well as vectors
1 parent 07b9b26 commit ec00d13

3 files changed

Lines changed: 172 additions & 64 deletions

File tree

‎spatialmath/pose3d.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -184,7 +184,7 @@ def eul(self, unit='rad', flip=False):
184184
:param unit: angular units: 'rad' [default], or 'deg'
185185
:type unit: str
186186
:return: 3-vector of Euler angles
187-
:rtype: numpy.ndarray, shape=(3,)
187+
:rtype: ndarray(3,), ndarray(n,3)
188188
189189
``x.eul`` is the Euler angle representation of the rotation. Euler angles are
190190
a 3-vector :math:`(\phi, \theta, \psi)` which correspond to consecutive
@@ -193,15 +193,15 @@ def eul(self, unit='rad', flip=False):
193193
If ``len(x)`` is:
194194
195195
- 1, return an ndarray with shape=(3,)
196-
- N>1, return ndarray with shape=(N,3)
196+
- N>1, return ndarray with shape=(3,N)
197197
198198
:seealso: :func:`~spatialmath.pose3d.SE3.Eul`, :func:`~spatialmath.base.transforms3d.tr2eul`
199199
:SymPy: not supported
200200
"""
201201
if len(self) == 1:
202202
return base.tr2eul(self.A, unit=unit)
203203
else:
204-
return np.array([base.tr2eul(x, unit=unit) for x in self.A]).T
204+
return np.array([base.tr2eul(x, unit=unit) for x in self.A])
205205

206206
def rpy(self, unit='rad', order='zyx'):
207207
"""
@@ -212,7 +212,7 @@ def rpy(self, unit='rad', order='zyx'):
212212
:param unit: angular units: 'rad' [default], or 'deg'
213213
:type unit: str
214214
:return: 3-vector of roll-pitch-yaw angles
215-
:rtype: numpy.ndarray, shape=(3,)
215+
:rtype: ndarray(3,), ndarray(n,3)
216216
217217
``x.rpy`` is the roll-pitch-yaw angle representation of the rotation. The angles are
218218
a 3-vector :math:`(r, p, y)` which correspond to successive rotations about the axes
@@ -231,15 +231,15 @@ def rpy(self, unit='rad', order='zyx'):
231231
If `len(x)` is:
232232
233233
- 1, return an ndarray with shape=(3,)
234-
- N>1, return ndarray with shape=(N,3)
234+
- N>1, return ndarray with shape=(3,N)
235235
236236
:seealso: :func:`~spatialmath.pose3d.SE3.RPY`, :func:`~spatialmath.base.transforms3d.tr2rpy`
237237
:SymPy: not supported
238238
"""
239239
if len(self) == 1:
240240
return base.tr2rpy(self.A, unit=unit, order=order)
241241
else:
242-
return np.array([base.tr2rpy(x, unit=unit, order=order) for x in self.A]).T
242+
return np.array([base.tr2rpy(x, unit=unit, order=order) for x in self.A])
243243

244244
def angvec(self, unit='rad'):
245245
r"""

‎spatialmath/quaternion.py‎

Lines changed: 163 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -1222,9 +1222,13 @@ def Eul(cls, angles, *, unit='rad'):
12221222
:return: unit-quaternion
12231223
:rtype: UnitQuaternion instance
12241224
1225-
``UnitQuaternion.Eul(𝚪)`` is a unit quaternion that describes the 3D
1226-
rotation defined by a 3-vector of Euler angles :math:`\Gamma = (\phi, \theta, \psi)` which correspond to consecutive rotations about the Z, Y, Z axes
1227-
respectively.
1225+
- ``UnitQuaternion.Eul(𝚪)`` is a unit quaternion that describes the 3D
1226+
rotation defined by a 3-vector of Euler angles :math:`\Gamma = (\phi,
1227+
\theta, \psi)` which correspond to consecutive rotations about the Z,
1228+
Y, Z axes respectively.
1229+
1230+
- ``UnitQuaternion.Eul(φ, θ, ψ)`` as above but the angles are provided
1231+
as three scalars.
12281232
12291233
Example:
12301234
@@ -1235,6 +1239,9 @@ def Eul(cls, angles, *, unit='rad'):
12351239
12361240
:seealso: :func:`~spatialmath.quaternion.UnitQuaternion.RPY`, :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`~spatialmath.base.transforms3d.eul2r`
12371241
"""
1242+
if len(angles) == 1:
1243+
angles = angles[0]
1244+
12381245
return cls(base.r2q(base.eul2r(angles, unit=unit)), check=False)
12391246

12401247
@classmethod
@@ -1251,18 +1258,27 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
12511258
:return: unit-quaternion
12521259
:rtype: UnitQuaternion instance
12531260
1254-
``UnitQuaternion.RPY(𝚪)`` is a unit quaternion that describes the 3D rotation defined by a 3-vector of roll, pitch, yaw angles :math:`\Gamma = (r, p, y)`
1255-
which correspond to successive rotations about the axes specified by ``order``:
1261+
- ``UnitQuaternion.RPY(𝚪)`` is a unit quaternion that describes the 3D
1262+
rotation defined by a 3-vector of roll, pitch, yaw angles
1263+
:math:`\Gamma = (r, p, y)` which correspond to successive rotations
1264+
about the axes specified by ``order``:
12561265
1257-
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
1258-
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
1259-
and y-axis sideways.
1260-
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
1261-
then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
1262-
and y-axis between the gripper fingers.
1263-
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
1264-
then by roll about the new z-axis. Convention for a camera with z-axis parallel
1265-
to the optic axis and x-axis parallel to the pixel rows.
1266+
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch
1267+
about the new y-axis, then by roll about the new x-axis.
1268+
Convention for a mobile robot with x-axis forward and y-axis
1269+
sideways.
1270+
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the
1271+
new y-axis, then by roll about the new z-axis. Convention for a
1272+
robot gripper with z-axis forward and y-axis between the gripper
1273+
fingers.
1274+
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the
1275+
new x-axis, then by roll about the new z-axis. Convention for a
1276+
camera with z-axis parallel to the optic axis and x-axis parallel
1277+
to the pixel rows.
1278+
1279+
1280+
- ``UnitQuaternion.RPY(⍺, β, 𝛾)`` as above but the angles are provided
1281+
as three scalars.
12661282
12671283
Example:
12681284
@@ -1273,6 +1289,9 @@ def RPY(cls, angles, *, order='zyx', unit='rad'):
12731289
12741290
:seealso: :func:`~spatialmath.quaternion.UnitQuaternion.Eul`, :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`~spatialmath.base.transforms3d.rpy2r`
12751291
"""
1292+
if len(angles) == 1:
1293+
angles = angles[0]
1294+
12761295
return cls(base.r2q(base.rpy2r(angles, unit=unit, order=order)), check=False)
12771296

12781297
@classmethod
@@ -1725,26 +1744,41 @@ def __ne__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argu
17251744
return left.binop(right, lambda x, y: not base.isequal(x, y, unitq=True), list1=False)
17261745

17271746
def __matmul__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
1728-
print('normalizing product')
1729-
return left.__mul__(right)
1747+
"""
1748+
Overloaded @ operator
1749+
1750+
:return: product :rtype: UnitQuaternion
1751+
1752+
- ``q1 @ q2`` is the Hamilton product of ``q1`` and ``q2``, both unit
1753+
quaternions, followed by explicit normalization.
1754+
1755+
- `` q1 @= q2`` as above.
17301756
1731-
def interp(self, s=0, start=None, shortest=False):
1757+
.. note:: This operator is functionally equivalent to ``*`` but is more
1758+
costly. It is useful for cases where a pose is incrementally update
1759+
over many cycles.
1760+
"""
1761+
return left.__class__(left.binop(right, lambda x, y: base.unit(base.qqmul(x, y))))
1762+
1763+
def interp(self, end, s=0, shortest=False):
17321764
"""
17331765
Interpolate between two unit quaternions
17341766
1735-
:param start: initial unit quaternion
1736-
:type start: UnitQuaternion
1767+
:param end: final unit quaternion
1768+
:type end: UnitQuaternion
17371769
:param shortest: Take the shortest path along the great circle
1738-
:param s: interpolation in range [0,1]
1739-
:type s: array_like
1770+
:param s: interpolation coefficient, range 0 to 1, or number of steps
1771+
:type s: array_like or int
17401772
:return: interpolated unit quaternion
17411773
:rtype: UnitQuaternion instance
17421774
1743-
- ``q.interp(s)`` is a unit quaternion that is interpolated between
1744-
the identity quaternion and ``q``. Spherical linear interpolation (slerp) is used.
1775+
- ``q0.interp(q1, s)`` is a unit quaternion that is interpolated between
1776+
``q0`` when s=0 and ``q1`` when s=1. Spherical linear interpolation
1777+
(slerp) is used. If ``s`` is an ndarray(n) then the result will be
1778+
a UnitQuaternion with n values.
17451779
1746-
- ``q1.interp(s, start=q0)`` as above but interpolated between
1747-
``q0`` and ``q1``.
1780+
- ``q0.interp(q1, N)`` interpolate between ``q0`` and ``q1`` in ``N``
1781+
steps.
17481782
17491783
Example:
17501784
@@ -1754,41 +1788,40 @@ def interp(self, s=0, start=None, shortest=False):
17541788
>>> q1 = UQ.Rx(0.3); q2 = UQ.Rz(-0.4)
17551789
>>> print(q1)
17561790
>>> print(q2)
1757-
>>> print(q2.interp(0, start=q1)) # this is q1
1758-
>>> print(q2.interp(1, start=q1)) # this is q2
1759-
>>> print(q2.interp(0.5, start=q1)) # this is in between
1791+
>>> q1.interp(q2, 0) # this is q1
1792+
>>> q1.interp(q2, 1,) # this is q2
1793+
>>> q1.interp(q2, 0.5) # this is in between
1794+
>>> q = q1.interp(q2, 11) # in 11 steps
1795+
>>> len(q)
1796+
>>> q[0] # this is q1
1797+
>>> q[5] # this is in between
17601798
1761-
.. note:: values of ``s`` are clipped to the range [0, 1]
1799+
.. note:: values of ``s`` are silently clipped to the range [0, 1]
17621800
17631801
:seealso: :func:`~spatialmath.base.quaternions.slerp`
17641802
"""
17651803
# TODO allow self to have len() > 1
17661804

1767-
s = base.getvector(s)
1768-
s = np.clip(s, 0, 1) # enforce valid values
1769-
1770-
if start is not None:
1771-
# 2 quaternion form
1772-
if not isinstance(start, UnitQuaternion):
1773-
raise TypeError('start argument must be a UnitQuaternion')
1774-
q1 = start.vec
1775-
q2 = self.vec
1776-
dot = base.inner(q1, q2)
1777-
1778-
# If the dot product is negative, the quaternions
1779-
# have opposite handed-ness and slerp won't take
1780-
# the shorter path. Fix by reversing one quaternion.
1781-
if shortest:
1782-
if dot < 0:
1783-
q1 = - q1
1784-
dot = -dot
1785-
1805+
if isinstance(s, int) and s > 1:
1806+
s = np.linspace(0, 1, s)
17861807
else:
1787-
# 1 quaternion form
1788-
# s will always be positive
1789-
q1 = base.eye()
1790-
q2 = self.vec
1791-
dot = q2[0] # if q1 = (1, 0,0,0)
1808+
s = base.getvector(s)
1809+
s = np.clip(s, 0, 1) # enforce valid values
1810+
1811+
# 2 quaternion form
1812+
if not isinstance(end, UnitQuaternion):
1813+
raise TypeError('end argument must be a UnitQuaternion')
1814+
q1 = self.vec
1815+
q2 = end.vec
1816+
dot = base.inner(q1, q2)
1817+
1818+
# If the dot product is negative, the quaternions
1819+
# have opposite handed-ness and slerp won't take
1820+
# the shorter path. Fix by reversing one quaternion.
1821+
if shortest:
1822+
if dot < 0:
1823+
q1 = - q1
1824+
dot = -dot
17921825

17931826
# shouldn't be needed by handle numerical errors: -eps, 1+eps cases
17941827
dot = np.clip(dot, -1, 1) # Clip within domain of acos()
@@ -1806,6 +1839,78 @@ def interp(self, s=0, start=None, shortest=False):
18061839

18071840
return UnitQuaternion(qi)
18081841

1842+
def interp1(self, s=0, shortest=False):
1843+
"""
1844+
Interpolate a unit quaternions
1845+
1846+
:param shortest: Take the shortest path along the great circle
1847+
:param s: interpolation coefficient, range 0 to 1, or number of steps
1848+
:type s: array_like or int
1849+
:return: interpolated unit quaternion
1850+
:rtype: UnitQuaternion instance
1851+
1852+
- ``q.interp1(s)`` is a unit quaternion that is interpolated between
1853+
identity when s=0 and ``q`` when s=1. Spherical linear interpolation
1854+
(slerp) is used. If ``s`` is an ndarray(n) then the result will be
1855+
a UnitQuaternion with n values.
1856+
1857+
- ``q.interp1(N)`` interpolate between identity and ``q1`` in ``N``
1858+
steps.
1859+
1860+
Example:
1861+
1862+
.. runblock:: pycon
1863+
1864+
>>> from spatialmath import UnitQuaternion as UQ
1865+
>>> q = UQ.Rx(0.3)
1866+
>>> print(q)
1867+
>>> q.interp1(0) # this is identity
1868+
>>> q.interp1(1) # this is q
1869+
>>> q.interp1(0.5) # this is in between
1870+
>>> qi = q.interp1(q2, 11) # in 11 steps
1871+
>>> len(qi)
1872+
>>> qi[0] # this is q1
1873+
>>> qi[5] # this is in between
1874+
1875+
.. note:: values of ``s`` are silently clipped to the range [0, 1]
1876+
1877+
:seealso: :func:`~spatialmath.base.quaternions.slerp`
1878+
"""
1879+
# TODO allow self to have len() > 1
1880+
1881+
if isinstance(s, int) and s > 1:
1882+
s = np.linspace(0, 1, s)
1883+
else:
1884+
s = base.getvector(s)
1885+
s = np.clip(s, 0, 1) # enforce valid values
1886+
1887+
q = self.vec
1888+
dot = q[0] # s
1889+
1890+
# If the dot product is negative, the quaternions
1891+
# have opposite handed-ness and slerp won't take
1892+
# the shorter path. Fix by reversing one quaternion.
1893+
if shortest:
1894+
if dot < 0:
1895+
q = - q
1896+
dot = -dot
1897+
1898+
# shouldn't be needed by handle numerical errors: -eps, 1+eps cases
1899+
dot = np.clip(dot, -1, 1) # Clip within domain of acos()
1900+
1901+
theta_0 = math.acos(dot) # theta_0 = angle between input vectors
1902+
1903+
qi = []
1904+
for sk in s:
1905+
theta = theta_0 * sk # theta = angle between v0 and result
1906+
1907+
s1 = float(math.cos(theta) - dot * math.sin(theta) / math.sin(theta_0))
1908+
s2 = math.sin(theta) / math.sin(theta_0)
1909+
out = np.r_[s1, 0, 0, 0] + (q * s2)
1910+
qi.append(out)
1911+
1912+
return UnitQuaternion(qi)
1913+
18091914
def increment(self, w, normalize=False):
18101915
"""
18111916
Quaternion incremental update
@@ -1874,6 +1979,10 @@ def animate(self, *args, **kwargs):
18741979
18751980
:see :func:`~spatialmath.base.transforms3d.tranimate`, :func:`~spatialmath.base.transforms3d.trplot`
18761981
"""
1982+
if len(self) > 1:
1983+
base.tranimate([base.q2r(q) for q in self.data], *args, **kwargs)
1984+
else:
1985+
base.tranimate(base.q2r(self._A), *args, **kwargs)
18771986

18781987
def rpy(self, unit='rad', order='zyx'):
18791988
"""
@@ -2128,10 +2237,6 @@ def SE3(self):
21282237

21292238
import pathlib
21302239

2131-
q=UnitQuaternion.Rand()
2132-
a = q.log()
2133-
print(a, a.norm())
2134-
a = q.exp()
2135-
print(a, a.norm())
2240+
a = UnitQuaternion([0, 1, 0, 0])
21362241

21372242
exec(open(pathlib.Path(__file__).parent.parent.absolute() / "tests" / "test_quaternion.py").read()) # pylint: disable=exec-used

‎tests/test_quaternion.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -414,8 +414,11 @@ def test_conversions(self):
414414

415415
# , 3 angle
416416
qcompare(UnitQuaternion.RPY([0.1, 0.2, 0.3]).rpy(), [0.1, 0.2, 0.3])
417+
qcompare(UnitQuaternion.RPY(0.1, 0.2, 0.3).rpy(), [0.1, 0.2, 0.3])
417418

418419
qcompare(UnitQuaternion.Eul([0.1, 0.2, 0.3]).eul(), [0.1, 0.2, 0.3])
420+
qcompare(UnitQuaternion.Eul(0.1, 0.2, 0.3).eul(), [0.1, 0.2, 0.3])
421+
419422

420423
qcompare(UnitQuaternion.RPY([10, 20, 30], unit='deg').R, rpy2r(10, 20, 30, unit='deg'))
421424

0 commit comments

Comments
 (0)