spatialmath-python
spatialmath-python copied to clipboard
Interp function - invalid pose/transform from two valid poses/transforms
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.
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.
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
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?
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