Skip to content

Commit d7bebf3

Browse files
committed
change to interp, always has start and end,
added interp1 for one argument case arguments are reversed BREAKING CHANGE
1 parent b3d1509 commit d7bebf3

2 files changed

Lines changed: 72 additions & 20 deletions

File tree

‎spatialmath/super_pose.py‎

Lines changed: 68 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -361,27 +361,81 @@ def log(self, twist=False):
361361
else:
362362
return log
363363

364-
def interp(self, s=None, start=None):
364+
def interp(self, end=None, s=None):
365+
"""
366+
Interpolate between poses (superclass method)
367+
368+
:param end: final pose
369+
:type end: same as ``self``
370+
:param s: interpolation coefficient, range 0 to 1
371+
:type s: array_like
372+
:return: interpolated pose
373+
:rtype: SO2, SE2, SO3, SE3 instance
374+
375+
- ``X.interp(Y, s)`` interpolates pose between X between when s=0
376+
and Y when s=1.
377+
378+
Example:
379+
380+
.. runblock:: pycon
381+
382+
>>> x = SE3(-1, -2, 0) * SE3.Rx(-0.3)
383+
>>> y = SE3(1, 2, 0) * SE3.Rx(0.3)
384+
>>> x.interp(y, 0)
385+
>>> x.interp(y, 1)
386+
>>> x.interp(y, 0.5)
387+
>>> z = x.interp(y, np.linspace(0, 1, 11))
388+
>>> len(z)
389+
>>> y[0]
390+
>>> y[5]
391+
392+
.. note::
393+
394+
- For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
395+
396+
:seealso: :func:`interp1`, :func:`~spatialmath.base.transforms3d.trinterp`, :func:`~spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms2d.trinterp2`
397+
398+
:SymPy: not supported
399+
"""
400+
s = base.getvector(s)
401+
s = np.clip(s, 0, 1)
402+
403+
if len(self) > 1:
404+
raise ValueError('start pose must be a singleton')
405+
406+
if end is not None:
407+
if len(end) > 1:
408+
raise ValueError('end pose must be a singleton')
409+
end = end.A
410+
411+
if self.N == 2:
412+
# SO(2) or SE(2)
413+
return self.__class__([base.trinterp2(start=self.A, end=end, s=_s) for _s in s])
414+
415+
elif self.N == 3:
416+
# SO(3) or SE(3)
417+
return self.__class__([base.trinterp(start=self.A, end=end, s=_s) for _s in s])
418+
419+
def interp1(self, s=None):
365420
"""
366421
Interpolate pose (superclass method)
367422
368-
:param start: initial pose
369-
:type start: same as ``self``
423+
:param end: final pose
424+
:type end: same as ``self``
370425
:param s: interpolation coefficient, range 0 to 1
371426
:type s: array_like
372427
:return: interpolated pose
373428
:rtype: SO2, SE2, SO3, SE3 instance
374429
375-
- ``X.interp(s)`` interpolates the pose X between identity when s=0
376-
and X when s=1.
430+
- ``X.interp(s)`` interpolates pose between identity when s=0, and X when s=1.
377431
378-
====== ====== =========== ===============================
379-
len(X) len(s) len(result) Result
380-
====== ====== =========== ===============================
381-
1 1 1 Y = interp(identity, X, s)
382-
M 1 M Y[i] = interp(T0, X[i], s)
383-
1 M M Y[i] = interp(T0, X, s[i])
384-
====== ====== =========== ===============================
432+
====== ====== =========== ===============================
433+
len(X) len(s) len(result) Result
434+
====== ====== =========== ===============================
435+
1 1 1 Y = interp(X, s)
436+
M 1 M Y[i] = interp(X[i], s)
437+
1 M M Y[i] = interp(X, s[i])
438+
====== ====== =========== ===============================
385439
386440
Example::
387441
@@ -409,7 +463,7 @@ def interp(self, s=None, start=None):
409463
410464
#. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
411465
412-
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`, :func:`~spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms2d.trinterp2`
466+
:seealso: :func:`interp`, :func:`~spatialmath.base.transforms3d.trinterp`, :func:`~spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms2d.trinterp2`
413467
414468
:SymPy: not supported
415469
"""
@@ -434,7 +488,6 @@ def interp(self, s=None, start=None):
434488
return self.__class__([base.trinterp(start, self.A, s=_s) for _s in s])
435489
else:
436490
return self.__class__([base.trinterp(start, x, s=s[0]) for x in self.data])
437-
438491
def norm(self):
439492
"""
440493
Normalize pose (superclass method)
@@ -651,7 +704,7 @@ def _string_matrix(self):
651704
if self._ansiformatter is None:
652705
self._ansiformatter = ANSIMatrix(style='thick')
653706

654-
return self._ansiformatter.str(self.A)
707+
return "\n".join([self._ansiformatter.str(A) for A in self.data])
655708

656709
def _string_color(self, color=False):
657710
"""

‎tests/test_pose2d.py‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -468,13 +468,12 @@ def test_interp(self):
468468
TT = SE2(2, -4, 0.6)
469469
I = SE2()
470470

471-
z = I.interp(s=0)
471+
z = I.interp(TT, s=0)
472472
self.assertIsInstance(z, SE2)
473473

474-
array_compare(TT.interp(s=0), I)
475-
array_compare(TT.interp(s=1), TT)
476-
array_compare(TT.interp(s=0.5), SE2(1, -2, 0.3))
477-
array_compare(TT.interp(s=0.5,start=SE2(-2, 4, -0.6)), I)
474+
array_compare(I.interp(TT, s=0), I)
475+
array_compare(I.interp(TT, s=1), TT)
476+
array_compare(I.interp(TT, s=0.5), SE2(1, -2, 0.3))
478477

479478
def test_miscellany(self):
480479

0 commit comments

Comments
 (0)