Interp function - invalid pose/transform from two valid poses/transforms

[jmount1992]

I attempted to interpolate between two spatialmath.pose3d.SE3 objects and the returned result was also a spatialmath.pose3d.SE3 object but, the A matrix was of type None rather than a ndarray(4,4) as expected. I am trying to interpolate from transform 1 to transform 2 a certain amount, so s in the function is set in the interval 0 to 1. Both transform 1 and transform 2 are valid, yet the interpolated transform matrix (i.e. the NumPy array) returned from line 1504 in spatialmath.base.transforms3d.py (trinterp function), is not valid. I am curious as to how interpolating between two valid poses could result in an invalid pose. Any thoughts?

I might be using the isvalid function incorrectly.
Side note: why have you made the isvalid function static rather than a class member function? Seems odd to have to do se3_object.isvalid(se3_object.A) rather than se3_object.isvalid().

Code and a pickle data to recreate is provided via Google Drive.

[petercorke]

Re. your last question, it's because the argument isn't an object yet. The class is testing whether the argument is the right kind of matrix for the class.

From a readability perspective
SE3.isvalid(M)
is saying does M have what it takes to join the class?

An important assumption in SMTB is that the matrix values encapsulated by a class are always valid. Checking at constructor time means we don't have to check in every method.

[petercorke]

Re. the first part, I see the behaviour that I'd expect:

>>> a = SE3.Rand()
>>> b = SE3.Rand()
>>> a.interp(b, 0)
   0.5829    0.2942    0.7574   -0.7834    
  -0.6052    0.7792    0.1631   -0.8598    
  -0.5422   -0.5534    0.6323   -0.4823    
   0         0         0         1         
>>> a.interp(b, 1)
  -0.6282    0.6877    0.3639    0.294     
  -0.6738   -0.7147    0.1875   -0.6723    
   0.389    -0.1274    0.9124    0.3263    
   0         0         0         1         
>>> z = a.interp(b, 0.5)
>>> z.A
array([[ 0.07437,   0.8102,   0.5814,  -0.2447],
       [  -0.997,  0.04683,  0.06227,   -0.766],
       [ 0.02323,  -0.5842,   0.8113, -0.07802],
       [       0,        0,        0,        1]])

I can only assume that the transforms are not proper. Will have a deeper look, but am a bit loathe to import a pickle file I didn't create, see https://www.benfrederickson.com/dont-pickle-your-data

[petercorke]

Doing this in bursts...

Deliberately creating an invalid SE(3) matrix and object

>>> bad = SE3(np.diag([2,1,1,1]), check=False)
>>> bad
Out[10]: 
   2         0         0         0         
   0         1         0         0         
   0         0         1         0         
   0         0         0         1         
>>> bad.interp(a, 0.5)
Out[11]: ---------------------------------------------------------------------------
TypeError                                 Traceback (most recent call last)
~/opt/miniconda3/envs/dev/lib/python3.8/site-packages/IPython/core/formatters.py in __call__(self, obj)
    700                 type_pprinters=self.type_printers,
    701                 deferred_pprinters=self.deferred_printers)
--> 702             printer.pretty(obj)
    703             printer.flush()
    704             return stream.getvalue()

~/opt/miniconda3/envs/dev/lib/python3.8/site-packages/IPython/lib/pretty.py in pretty(self, obj)
    389                             meth = cls._repr_pretty_
    390                             if callable(meth):
--> 391                                 return meth(obj, self, cycle)
    392                         if cls is not object \
    393                                 and callable(cls.__dict__.get('__repr__')):

~/Dropbox/code/spatialmath-python/spatialmath/baseposematrix.py in _repr_pretty_(self, p, cycle)
    751 
    752         if len(self) == 1:
--> 753             p.text(str(self))
    754         else:
    755             for i, x in enumerate(self):

~/Dropbox/code/spatialmath-python/spatialmath/baseposematrix.py in __str__(self)
    786             return self._string_matrix()
    787         else:
--> 788             return self._string_color(color=True)
    789 
    790     def _string_matrix(self):

~/Dropbox/code/spatialmath-python/spatialmath/baseposematrix.py in _string_color(self, color)
    884         elif len(self.data) == 1:
    885             # single matrix case
--> 886             output_str = mformat(self, self.A)
    887         else:
    888             # sequence case

~/Dropbox/code/spatialmath-python/spatialmath/baseposematrix.py in mformat(self, X)
    850             out = ""
    851             n = self.N  # dimension of rotation submatrix
--> 852             for rownum, row in enumerate(X):
    853                 rowstr = " "
    854                 # format the columns

TypeError: 'NoneType' object is not iterable

Is this the error you get?

[colormotor]

Hi @petercorke, @jmount1992
I encountered a similar issue, where the two matrices used to create poses are detected as valid. The following should reproduce the problem:

from spatialmath import SE3
import numpy as np
from spatialmath.base import trnorm

A = np.array(
[[-0.11483364,-0.99315865,-0.02119266, 0.0 ],
 [-0.99294008, 0.11539436,-0.0274615 , 0.0],
 [ 0.02971914, 0.01788954,-0.99939819, 0.0],
 [ 0.        , 0.        , 0.        , 1.  ]])
B = np.array(
[[-0.        ,-1.        , 0.        , 0.0 ],
 [-1.        , 0.        , 0.        , 0.0],
 [-0.        , 0.        ,-1.        , 0.0],
 [ 0.        , 0.        , 0.        , 1.  ]])
A = SE3(trnorm(A)) 
B = SE3(trnorm(B)) 
print(SE3.isvalid(np.array(A)))
print(SE3.isvalid(np.array(B)))
print(A.interp(B, 0.0)) # this works
print(A.interp(B, 1.0)) # this works
print(A.interp(B, 0.3)) # TypeError