can't initialize the SE3 OR SO3 object with np.array input

[huiwenzhang]

Hi,

I want to create a SE3 object with a 4x4 np.array argument, the document says I can do it, but it seems that the implemented code doesn't accept np.array input when initialization. What's more, I can't find an easy way to work around this, e.g. create the SE3 by a SO3 and a translation.

Please check the code below

https://github.com/petercorke/spatialmath-python/blob/a3116021b8bd95b4f6015b180053941599ebd6cc/spatialmath/pose3d.py#L731-L785

THX

[petercorke]

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         

[Daniella1]

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.

[petercorke]

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

[petercorke]

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.

[Daniella1]

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.

[petercorke]

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.