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