Skip to content

[Issue-122] Pass shortest arg for interp; optionally enforce non-negative scalar … #123

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
myeatman-bdai merged 3 commits into from
May 13, 2024
+46 −13
Merged

[Issue-122] Pass shortest arg for interp; optionally enforce non-negative scalar … #123

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
13321ff
pass shortest arg for interp; optionally enforce non-negative scalar …
jcao-bdai May 5, 2024
4def39f
add tests
jcao-bdai May 5, 2024
55a650c
use existing angle wrap func
jcao-bdai May 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
add tests
  • Loading branch information
@jcao-bdai
jcao-bdai committed May 13, 2024
commit 4def39fb44430d05435a708861adaa118c5b362f
4 changes: 2 additions & 2 deletions spatialmath/base/quaternions.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
4 changes: 4 additions & 0 deletions spatialmath/base/transforms2d.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

@myeatman-bdai myeatman-bdai May 13, 2024

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

lets make this a function if the code is dupllicated twice

@jcao-bdai jcao-bdai May 13, 2024 •
edited
Loading

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

found an existing function - updates are pushed in 55a650c

delta = delta + math.pi * 2 if delta < -math.pi else delta
th1 = th0 + delta

p0 = transl2(start)
p1 = transl2(end)
Expand Down
2 changes: 1 addition & 1 deletion spatialmath/baseposematrix.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
7 changes: 7 additions & 0 deletions tests/base/test_quaternions.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
10 changes: 10 additions & 0 deletions tests/test_pose2d.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down