From 13321ff3b4a2092e688f8e1d9d7fe16d6913c08c Mon Sep 17 00:00:00 2001 From: Jien Cao Date: Sun, 5 May 2024 10:28:01 -0400 Subject: [PATCH 1/3] pass shortest arg for interp; optionally enforce non-negative scalar for r2q --- spatialmath/base/quaternions.py | 6 ++++++ spatialmath/base/transforms2d.py | 12 +++++++++--- spatialmath/base/transforms3d.py | 16 +++++++++------- spatialmath/baseposematrix.py | 6 ++++-- 4 files changed, 28 insertions(+), 12 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 77efe640..516112fe 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -549,6 +549,7 @@ def r2q( check: Optional[bool] = False, tol: float = 20, order: Optional[str] = "sxyz", + shortest: bool = True, ) -> UnitQuaternionArray: """ Convert SO(3) rotation matrix to unit-quaternion @@ -562,6 +563,8 @@ def r2q( :param order: the order of the returned quaternion elements. Must be 'sxyz' or 'xyzs'. Defaults to 'sxyz'. :type order: str + :param shortest: ensures the quaternion has non-negative scalar part. + :type shortest: bool, default to True :return: unit-quaternion as Euler parameters :rtype: ndarray(4) :raises ValueError: for non SO(3) argument @@ -633,6 +636,9 @@ def r2q( e[1] = math.copysign(e[1], R[0, 2] + R[2, 0]) e[2] = math.copysign(e[2], R[2, 1] + R[1, 2]) + if shortest and e[0] < 0: + e = -e + if order == "sxyz": return e elif order == "xyzs": diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 669c8fdd..0d02807f 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -853,16 +853,16 @@ def tr2jac2(T: SE2Array) -> R3x3: @overload -def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float) -> SO2Array: +def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float, shortest: bool = True) -> SO2Array: ... @overload -def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float) -> SE2Array: +def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float, shortest: bool = True) -> SE2Array: ... -def trinterp2(start, end, s): +def trinterp2(start, end, s, shortest: bool = True): """ Interpolate SE(2) or SO(2) matrices @@ -872,6 +872,8 @@ def trinterp2(start, end, s): :type end: ndarray(3,3) or ndarray(2,2) :param s: interpolation coefficient, range 0 to 1 :type s: float + :param shortest: take the shortest path along the great circle for the rotation + :type shortest: bool, default to True :return: interpolated SE(2) or SO(2) matrix value :rtype: ndarray(3,3) or ndarray(2,2) :raises ValueError: bad arguments @@ -917,6 +919,10 @@ def trinterp2(start, end, s): th0 = math.atan2(start[1, 0], start[0, 0]) th1 = math.atan2(end[1, 0], end[0, 0]) + if shortest: + delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi + delta = delta + math.pi * 2 if delta < -math.pi else delta + th1 = th0 + delta th = th0 * (1 - s) + s * th1 diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 376af9ff..ceff8732 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -1605,16 +1605,16 @@ def trnorm(T: SE3Array) -> SE3Array: @overload -def trinterp(start: Optional[SO3Array], end: SO3Array, s: float) -> SO3Array: +def trinterp(start: Optional[SO3Array], end: SO3Array, s: float, shortest: bool = True) -> SO3Array: ... @overload -def trinterp(start: Optional[SE3Array], end: SE3Array, s: float) -> SE3Array: +def trinterp(start: Optional[SE3Array], end: SE3Array, s: float, shortest: bool = True) -> SE3Array: ... -def trinterp(start, end, s): +def trinterp(start, end, s, shortest=True): """ Interpolate SE(3) matrices @@ -1624,6 +1624,8 @@ def trinterp(start, end, s): :type end: ndarray(4,4) or ndarray(3,3) :param s: interpolation coefficient, range 0 to 1 :type s: float + :param shortest: take the shortest path along the great circle for the rotation + :type shortest: bool, default to True :return: interpolated SE(3) or SO(3) matrix value :rtype: ndarray(4,4) or ndarray(3,3) :raises ValueError: bad arguments @@ -1663,12 +1665,12 @@ def trinterp(start, end, s): if start is None: # TRINTERP(T, s) q0 = r2q(end) - qr = qslerp(qeye(), q0, s) + qr = qslerp(qeye(), q0, s, shortest=shortest) else: # TRINTERP(T0, T1, s) q0 = r2q(start) q1 = r2q(end) - qr = qslerp(q0, q1, s) + qr = qslerp(q0, q1, s, shortest=shortest) return q2r(qr) @@ -1679,7 +1681,7 @@ def trinterp(start, end, s): q0 = r2q(t2r(end)) p0 = transl(end) - qr = qslerp(qeye(), q0, s) + qr = qslerp(qeye(), q0, s, shortest=shortest) pr = s * p0 else: # TRINTERP(T0, T1, s) @@ -1689,7 +1691,7 @@ def trinterp(start, end, s): p0 = transl(start) p1 = transl(end) - qr = qslerp(q0, q1, s) + qr = qslerp(q0, q1, s, shortest=shortest) pr = p0 * (1 - s) + s * p1 return rt2tr(q2r(qr), pr) diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index 379a5439..7b279b59 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -377,7 +377,7 @@ def log(self, twist: Optional[bool] = False) -> Union[NDArray, List[NDArray]]: else: return log - def interp(self, end: Optional[bool] = None, s: Union[int, float] = None) -> Self: + def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shortest: bool = True) -> Self: """ Interpolate between poses (superclass method) @@ -385,6 +385,8 @@ def interp(self, end: Optional[bool] = None, s: Union[int, float] = None) -> Sel :type end: same as ``self`` :param s: interpolation coefficient, range 0 to 1, or number of steps :type s: array_like or int + :param shortest: take the shortest path along the great circle for the rotation + :type shortest: bool, default to True :return: interpolated pose :rtype: same as ``self`` @@ -438,7 +440,7 @@ def interp(self, end: Optional[bool] = None, s: Union[int, float] = None) -> Sel elif self.N == 3: # SO(3) or SE(3) return self.__class__( - [smb.trinterp(start=self.A, end=end, s=_s) for _s in s] + [smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest) for _s in s] ) def interp1(self, s: float = None) -> Self: From 4def39fb44430d05435a708861adaa118c5b362f Mon Sep 17 00:00:00 2001 From: Jien Cao Date: Sun, 5 May 2024 12:26:03 -0400 Subject: [PATCH 2/3] add tests --- spatialmath/base/quaternions.py | 4 ++-- spatialmath/base/transforms2d.py | 4 ++++ spatialmath/baseposematrix.py | 2 +- tests/base/test_quaternions.py | 7 +++++++ tests/test_pose2d.py | 10 ++++++++++ 5 files changed, 24 insertions(+), 3 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 516112fe..0adf77ca 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -549,7 +549,7 @@ def r2q( check: Optional[bool] = False, tol: float = 20, order: Optional[str] = "sxyz", - shortest: bool = True, + shortest: bool = False, ) -> UnitQuaternionArray: """ Convert SO(3) rotation matrix to unit-quaternion @@ -564,7 +564,7 @@ def r2q( 'xyzs'. Defaults to 'sxyz'. :type order: str :param shortest: ensures the quaternion has non-negative scalar part. - :type shortest: bool, default to True + :type shortest: bool, default to False :return: unit-quaternion as Euler parameters :rtype: ndarray(4) :raises ValueError: for non SO(3) argument diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 0d02807f..342e270a 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -943,6 +943,10 @@ def trinterp2(start, end, s, shortest: bool = True): th0 = math.atan2(start[1, 0], start[0, 0]) th1 = math.atan2(end[1, 0], end[0, 0]) + if shortest: + delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi + delta = delta + math.pi * 2 if delta < -math.pi else delta + th1 = th0 + delta p0 = transl2(start) p1 = transl2(end) diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index 7b279b59..1a850600 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -434,7 +434,7 @@ def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shorte if self.N == 2: # SO(2) or SE(2) return self.__class__( - [smb.trinterp2(start=self.A, end=end, s=_s) for _s in s] + [smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest) for _s in s] ) elif self.N == 3: diff --git a/tests/base/test_quaternions.py b/tests/base/test_quaternions.py index 54977c20..c512c6d2 100644 --- a/tests/base/test_quaternions.py +++ b/tests/base/test_quaternions.py @@ -131,6 +131,13 @@ def test_rotation(self): ) nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1]) + large_rotation = math.pi + 0.01 + q1 = r2q(tr.rotx(large_rotation), shortest=False) + q2 = r2q(tr.rotx(large_rotation), shortest=True) + self.assertLess(q1[0], 0) + self.assertGreater(q2[0], 0) + self.assertTrue(qisequal(q1=q1, q2=q2, unitq=True)) + def test_slerp(self): q1 = np.r_[0, 1, 0, 0] q2 = np.r_[0, 0, 1, 0] diff --git a/tests/test_pose2d.py b/tests/test_pose2d.py index e45cc919..d6d96813 100755 --- a/tests/test_pose2d.py +++ b/tests/test_pose2d.py @@ -456,6 +456,16 @@ def test_interp(self): array_compare(I.interp(TT, s=1), TT) array_compare(I.interp(TT, s=0.5), SE2(1, -2, 0.3)) + R1 = SO2(math.pi - 0.1) + R2 = SO2(-math.pi + 0.2) + array_compare(R1.interp(R2, s=0.5, shortest=False), SO2(0.05)) + array_compare(R1.interp(R2, s=0.5, shortest=True), SO2(-math.pi + 0.05)) + + T1 = SE2(0, 0, math.pi - 0.1) + T2 = SE2(0, 0, -math.pi + 0.2) + array_compare(T1.interp(T2, s=0.5, shortest=False), SE2(0, 0, 0.05)) + array_compare(T1.interp(T2, s=0.5, shortest=True), SE2(0, 0, -math.pi + 0.05)) + def test_miscellany(self): TT = SE2(1, 2, 0.3) From 55a650c5e7fb36ffb18109a2dd37fad4787b0fd1 Mon Sep 17 00:00:00 2001 From: Jien Cao Date: Mon, 13 May 2024 09:02:45 -0400 Subject: [PATCH 3/3] use existing angle wrap func --- spatialmath/base/transforms2d.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 342e270a..c64ed036 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -920,9 +920,7 @@ def trinterp2(start, end, s, shortest: bool = True): th0 = math.atan2(start[1, 0], start[0, 0]) th1 = math.atan2(end[1, 0], end[0, 0]) if shortest: - delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi - delta = delta + math.pi * 2 if delta < -math.pi else delta - th1 = th0 + delta + th1 = th0 + smb.wrap_mpi_pi(th1 - th0) th = th0 * (1 - s) + s * th1 @@ -944,9 +942,7 @@ def trinterp2(start, end, s, shortest: bool = True): th0 = math.atan2(start[1, 0], start[0, 0]) th1 = math.atan2(end[1, 0], end[0, 0]) if shortest: - delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi - delta = delta + math.pi * 2 if delta < -math.pi else delta - th1 = th0 + delta + th1 = th0 + smb.wrap_mpi_pi(th1 - th0) p0 = transl2(start) p1 = transl2(end)