Comments (6)
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.
Related Issues (20)
- T[-1] not working for SE3 objects / fkine() HOT 4
- Adding inertias yields: AttributeError: 'SpatialInertia' object has no attribute 'I' HOT 2
- typing glitches? HOT 2
- Initialize a SE3/SO3 instance using the numpy array HOT 2
- Fall through error in the binary operator for BasePoseMatrix HOT 4
- ImportError: cannot import name 'plotvol3' from 'spatialmath.base.graphics' HOT 5
- Discrepancies in SE3.interp with respect to initial rotations and comparison to SciPy's SLERP implementation HOT 4
- "nan" returned for the angle when callin SE3.angvec() HOT 1
- Publish new version and update GitHub HOT 10
- Printing SE3 with symbolic equations HOT 1
- Line3 accepts arbitrary moment and direction vector arguments HOT 8
- Line3 multiplication with SE3 not working properly HOT 1
- Bug in base.r2q HOT 5
- AttributeError: module 'spatialmath' has no attribute 'base' HOT 3
- Matrix multiplication of numpy array and SO/SE object HOT 5
- Question on quaternion to rotation conversion and back HOT 1
- Get SO3 from SE3 HOT 2
- spatialmath import error on Ubuntu 22.04 HOT 2
- spatialmath import error HOT 1
- angdiff returns a numpy array when the function arguments are 2 scalars. HOT 3
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from spatialmath-python.