can't initialize the SE3 OR SO3 object with np.array input about spatialmath-python HOT 6 CLOSED

Remember that an SE(3) matrix has a special structure, the top left 3x3 must be an SO(3) matrix which has a determinant of 1, otherwise bad things will happen. For your example

>>> np.linalg.det(T[:3,:3])
Out[4]: 0.9999808199999998

and that's because you've created the rotation matrix using numbers good to just 4 significant figures.

Two options. Make the matrix proper (orthogonalizing) by tweaking the columns of the rotation sub matrix:

>>> from spatialmath.base import trnorm

(rtb) >>> SE3(trnorm(T))
Out[11]: 
   0.5      -0.7071    0.5       0.2805    
   0.5       0.7071    0.5       0.2805    
  -0.7071    0         0.7071    1.137     
   0         0         0         1         

or

>>> SE3(T, check=False)
Out[7]: 
   0.5      -0.7071    0.5       0.2805    
   0.5       0.7071    0.5       0.2805    
  -0.7071    0         0.7071    1.137     
   0         0         0         1        

but I would strongly recommend against that one since it will cause numerical problems down the track.

from spatialmath-python.

Not sure what's happening for you

>>> SE3(np.eye(4))
Out[11]: 
   1         0         0         0         
   0         1         0         0         
   0         0         1         0         
   0         0         0         1    

If the value is not a proper SE(3) matrix it won't be allowed. You can add check=False to disable that check at constructor time.

From R,t components use

(rtb) >>> SE3.Rt(np.eye(3), [0,0,0])
Out[12]: 
   1         0         0         0         
   0         1         0         0         
   0         0         1         0         
   0         0         0         1         

from spatialmath-python.

I also get the same error as @huiwenzhang when using a 4x4 numpy array, but the problem doesn't occur when using a np.eye(4), which is a bit weird.
Looking into the code, it seems the issue may be in pose3d.py at Line 776:
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
A check is performed that the shape of the array is 3, but it should be 4, as the shape of the numpy array should be 4x4, and not 3x3.

from spatialmath-python.

Give me a day or do to check this out. It is supposed to init from a 4x4 array, on a quick look I don't understand what that line is trying to do. Thanks

from spatialmath-python.

OK, the line you are looking at is a distraction. The code has gotten here because the argument was not a valid ndarray, ie. it was a 4x4 array but not a valid SE(3) matrix. That's why your np.eye(4) example worked.

You can turn off this input checking by SE3(myarray, check=False) but this is dangerous and will lead to bad results down the track. The big question is how are you computing the matrix you pass in?

There's clearly a problem with the SE3 constructor not giving a helpful error message in this situation.

from spatialmath-python.

Thanks for taking a look at it, and so quickly.

What I did before, was, I defined the robot using the DH parameters and the roboticstoolbox, then used the forward kinematics function fkine to generate the transformation matrix of the robot, then initialised a numpy array with the same values as the transformation matrix. I assume if I used the fkine function that the generated transformation matrix can be used for creating an SE3 matrix.

The ndarray functionality isn't necessary for me, I was just playing around with the roboticstoolbox and inverse kinematics of a robot, but here is the code and the output:

import roboticstoolbox as rtb
from numpy import pi, array, round
from spatialmath import SE3

class KukaLBR(rtb.DHRobot):
    def __init__(self):
        L1 = rtb.RevoluteMDH(d=0.34, a=0, alpha=0)
        L2 = rtb.RevoluteMDH(d=0, a=0, alpha=-pi/2)
        L3 = rtb.RevoluteMDH(d=0.4, a=0, alpha=pi/2)
        L4 = rtb.RevoluteMDH(d=0, a=0, alpha=-pi/2)
        L5 = rtb.RevoluteMDH(d=0.4, a=0, alpha=pi/2)
        L6 = rtb.RevoluteMDH(d=0, a=0, alpha=-pi/2)
        L7 = rtb.RevoluteMDH(d=0.161, a=0, alpha=pi/2)
        super().__init__([L1, L2, L3, L4, L5, L6, L7],name="Kuka Lbr iiwa")

kuka = KukaLBR()
q = [pi/4,0,0,pi/4,0,0,0]
print(kuka.fkine(q))
# kuka.plot(q, block=True)

x = 0.2805
y = 0.2805
z = 1.137
# T = kuka.fkine(q)
T = array([[0.5,-0.7071,0.5,x],[0.5,0.7071,0.5,y],[-0.7071,0,0.7071,z],[0,0,0,1]])
print(T)
T = SE3(T)
print(T)
sol = kuka.ikine_LM(T)
print(sol.success)
print(round(sol.q,2))

Output:

   0.5      -0.7071    0.5       0.2805    
   0.5       0.7071    0.5       0.2805
  -0.7071    0         0.7071    1.137
   0         0         0         1

[[ 0.5    -0.7071  0.5     0.2805]
 [ 0.5     0.7071  0.5     0.2805]
 [-0.7071  0.      0.7071  1.137 ]
 [ 0.      0.      0.      1.    ]]
Traceback (most recent call last):
  File "rtb_kuka.py", line 28, in <module>
    T = SE3(T)
  File "C:\python38\lib\site-packages\spatialmath\pose3d.py", line 781, in __init__
    raise ValueError('bad argument to constructor')
ValueError: bad argument to constructor

When I tried debugging the code, I could see that the line I mentioned earlier (pose3d.py 776) indicated that the shape of the array was not equal to 3 and therefore raised the error.

from spatialmath-python.