@@ -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
0 commit comments