Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Appearance settings

Commit 1cf7d92

Browse filesBrowse files
jcao-bdaijcao-bdai
authored
[Issue-122] Pass shortest arg for interp; optionally enforce non-negative scalar … (bdaiinstitute#123)
1 parent bc6103a commit 1cf7d92
Copy full SHA for 1cf7d92

File tree

Expand file treeCollapse file tree

6 files changed

+46
-13
lines changed
Filter options
Expand file treeCollapse file tree

6 files changed

+46
-13
lines changed

‎spatialmath/base/quaternions.py

Copy file name to clipboardExpand all lines: spatialmath/base/quaternions.py
+6Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -549,6 +549,7 @@ def r2q(
549549
check: Optional[bool] = False,
550550
tol: float = 20,
551551
order: Optional[str] = "sxyz",
552+
shortest: bool = False,
552553
) -> UnitQuaternionArray:
553554
"""
554555
Convert SO(3) rotation matrix to unit-quaternion
@@ -562,6 +563,8 @@ def r2q(
562563
:param order: the order of the returned quaternion elements. Must be 'sxyz' or
563564
'xyzs'. Defaults to 'sxyz'.
564565
:type order: str
566+
:param shortest: ensures the quaternion has non-negative scalar part.
567+
:type shortest: bool, default to False
565568
:return: unit-quaternion as Euler parameters
566569
:rtype: ndarray(4)
567570
:raises ValueError: for non SO(3) argument
@@ -633,6 +636,9 @@ def r2q(
633636
e[1] = math.copysign(e[1], R[0, 2] + R[2, 0])
634637
e[2] = math.copysign(e[2], R[2, 1] + R[1, 2])
635638

639+
if shortest and e[0] < 0:
640+
e = -e
641+
636642
if order == "sxyz":
637643
return e
638644
elif order == "xyzs":

‎spatialmath/base/transforms2d.py

Copy file name to clipboardExpand all lines: spatialmath/base/transforms2d.py
+9-3Lines changed: 9 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -853,16 +853,16 @@ def tr2jac2(T: SE2Array) -> R3x3:
853853

854854

855855
@overload
856-
def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float) -> SO2Array:
856+
def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float, shortest: bool = True) -> SO2Array:
857857
...
858858

859859

860860
@overload
861-
def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float) -> SE2Array:
861+
def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float, shortest: bool = True) -> SE2Array:
862862
...
863863

864864

865-
def trinterp2(start, end, s):
865+
def trinterp2(start, end, s, shortest: bool = True):
866866
"""
867867
Interpolate SE(2) or SO(2) matrices
868868
@@ -872,6 +872,8 @@ def trinterp2(start, end, s):
872872
:type end: ndarray(3,3) or ndarray(2,2)
873873
:param s: interpolation coefficient, range 0 to 1
874874
:type s: float
875+
:param shortest: take the shortest path along the great circle for the rotation
876+
:type shortest: bool, default to True
875877
:return: interpolated SE(2) or SO(2) matrix value
876878
:rtype: ndarray(3,3) or ndarray(2,2)
877879
:raises ValueError: bad arguments
@@ -917,6 +919,8 @@ def trinterp2(start, end, s):
917919

918920
th0 = math.atan2(start[1, 0], start[0, 0])
919921
th1 = math.atan2(end[1, 0], end[0, 0])
922+
if shortest:
923+
th1 = th0 + smb.wrap_mpi_pi(th1 - th0)
920924

921925
th = th0 * (1 - s) + s * th1
922926

@@ -937,6 +941,8 @@ def trinterp2(start, end, s):
937941

938942
th0 = math.atan2(start[1, 0], start[0, 0])
939943
th1 = math.atan2(end[1, 0], end[0, 0])
944+
if shortest:
945+
th1 = th0 + smb.wrap_mpi_pi(th1 - th0)
940946

941947
p0 = transl2(start)
942948
p1 = transl2(end)

‎spatialmath/base/transforms3d.py

Copy file name to clipboardExpand all lines: spatialmath/base/transforms3d.py
+9-7Lines changed: 9 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1605,16 +1605,16 @@ def trnorm(T: SE3Array) -> SE3Array:
16051605

16061606

16071607
@overload
1608-
def trinterp(start: Optional[SO3Array], end: SO3Array, s: float) -> SO3Array:
1608+
def trinterp(start: Optional[SO3Array], end: SO3Array, s: float, shortest: bool = True) -> SO3Array:
16091609
...
16101610

16111611

16121612
@overload
1613-
def trinterp(start: Optional[SE3Array], end: SE3Array, s: float) -> SE3Array:
1613+
def trinterp(start: Optional[SE3Array], end: SE3Array, s: float, shortest: bool = True) -> SE3Array:
16141614
...
16151615

16161616

1617-
def trinterp(start, end, s):
1617+
def trinterp(start, end, s, shortest=True):
16181618
"""
16191619
Interpolate SE(3) matrices
16201620
@@ -1624,6 +1624,8 @@ def trinterp(start, end, s):
16241624
:type end: ndarray(4,4) or ndarray(3,3)
16251625
:param s: interpolation coefficient, range 0 to 1
16261626
:type s: float
1627+
:param shortest: take the shortest path along the great circle for the rotation
1628+
:type shortest: bool, default to True
16271629
:return: interpolated SE(3) or SO(3) matrix value
16281630
:rtype: ndarray(4,4) or ndarray(3,3)
16291631
:raises ValueError: bad arguments
@@ -1663,12 +1665,12 @@ def trinterp(start, end, s):
16631665
if start is None:
16641666
# TRINTERP(T, s)
16651667
q0 = r2q(end)
1666-
qr = qslerp(qeye(), q0, s)
1668+
qr = qslerp(qeye(), q0, s, shortest=shortest)
16671669
else:
16681670
# TRINTERP(T0, T1, s)
16691671
q0 = r2q(start)
16701672
q1 = r2q(end)
1671-
qr = qslerp(q0, q1, s)
1673+
qr = qslerp(q0, q1, s, shortest=shortest)
16721674

16731675
return q2r(qr)
16741676

@@ -1679,7 +1681,7 @@ def trinterp(start, end, s):
16791681
q0 = r2q(t2r(end))
16801682
p0 = transl(end)
16811683

1682-
qr = qslerp(qeye(), q0, s)
1684+
qr = qslerp(qeye(), q0, s, shortest=shortest)
16831685
pr = s * p0
16841686
else:
16851687
# TRINTERP(T0, T1, s)
@@ -1689,7 +1691,7 @@ def trinterp(start, end, s):
16891691
p0 = transl(start)
16901692
p1 = transl(end)
16911693

1692-
qr = qslerp(q0, q1, s)
1694+
qr = qslerp(q0, q1, s, shortest=shortest)
16931695
pr = p0 * (1 - s) + s * p1
16941696

16951697
return rt2tr(q2r(qr), pr)

‎spatialmath/baseposematrix.py

Copy file name to clipboardExpand all lines: spatialmath/baseposematrix.py
+5-3Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -377,14 +377,16 @@ def log(self, twist: Optional[bool] = False) -> Union[NDArray, List[NDArray]]:
377377
else:
378378
return log
379379

380-
def interp(self, end: Optional[bool] = None, s: Union[int, float] = None) -> Self:
380+
def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shortest: bool = True) -> Self:
381381
"""
382382
Interpolate between poses (superclass method)
383383
384384
:param end: final pose
385385
:type end: same as ``self``
386386
:param s: interpolation coefficient, range 0 to 1, or number of steps
387387
:type s: array_like or int
388+
:param shortest: take the shortest path along the great circle for the rotation
389+
:type shortest: bool, default to True
388390
:return: interpolated pose
389391
:rtype: same as ``self``
390392
@@ -432,13 +434,13 @@ def interp(self, end: Optional[bool] = None, s: Union[int, float] = None) -> Sel
432434
if self.N == 2:
433435
# SO(2) or SE(2)
434436
return self.__class__(
435-
[smb.trinterp2(start=self.A, end=end, s=_s) for _s in s]
437+
[smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest) for _s in s]
436438
)
437439

438440
elif self.N == 3:
439441
# SO(3) or SE(3)
440442
return self.__class__(
441-
[smb.trinterp(start=self.A, end=end, s=_s) for _s in s]
443+
[smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest) for _s in s]
442444
)
443445

444446
def interp1(self, s: float = None) -> Self:

‎tests/base/test_quaternions.py

Copy file name to clipboardExpand all lines: tests/base/test_quaternions.py
+7Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,13 @@ def test_rotation(self):
131131
)
132132
nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1])
133133

134+
large_rotation = math.pi + 0.01
135+
q1 = r2q(tr.rotx(large_rotation), shortest=False)
136+
q2 = r2q(tr.rotx(large_rotation), shortest=True)
137+
self.assertLess(q1[0], 0)
138+
self.assertGreater(q2[0], 0)
139+
self.assertTrue(qisequal(q1=q1, q2=q2, unitq=True))
140+
134141
def test_slerp(self):
135142
q1 = np.r_[0, 1, 0, 0]
136143
q2 = np.r_[0, 0, 1, 0]

‎tests/test_pose2d.py

Copy file name to clipboardExpand all lines: tests/test_pose2d.py
+10Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -456,6 +456,16 @@ def test_interp(self):
456456
array_compare(I.interp(TT, s=1), TT)
457457
array_compare(I.interp(TT, s=0.5), SE2(1, -2, 0.3))
458458

459+
R1 = SO2(math.pi - 0.1)
460+
R2 = SO2(-math.pi + 0.2)
461+
array_compare(R1.interp(R2, s=0.5, shortest=False), SO2(0.05))
462+
array_compare(R1.interp(R2, s=0.5, shortest=True), SO2(-math.pi + 0.05))
463+
464+
T1 = SE2(0, 0, math.pi - 0.1)
465+
T2 = SE2(0, 0, -math.pi + 0.2)
466+
array_compare(T1.interp(T2, s=0.5, shortest=False), SE2(0, 0, 0.05))
467+
array_compare(T1.interp(T2, s=0.5, shortest=True), SE2(0, 0, -math.pi + 0.05))
468+
459469
def test_miscellany(self):
460470
TT = SE2(1, 2, 0.3)
461471

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.