Open Adineh94 opened 3 years ago
That URDF model has no dynamic parameters set
(rtb) >>> panda.dynamics()
┌────────────┬────┬────────────┬────────────────────────┬────┬────┬────────┬────┐
│ j │ m │ r │ I │ Jm │ B │ Tc │ G │
├────────────┼────┼────────────┼────────────────────────┼────┼────┼────────┼────┤
│panda_link0 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link1 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link2 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link3 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link4 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link5 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link6 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link7 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
│panda_link8 │ 0 │ 0, 0, 0 │ 0, 0, 0, 0, 0, 0 │ 0 │ 0 │ 0, 0 │ 0 │
└────────────┴────┴────────────┴────────────────────────┴────┴────┴────────┴────┘
but that shouldn't lead to the error you report (which I also get). None of our shipped Panda models have dynamic parameters at the moment, but the MATLAB DH model does include inertial parameters.
I'll log this a bug.
On a quick look this is due to static joints, ie. two links with a constant transform between them. It's not an issue with URDF, it's just that URDF allows us to express this kind of robot structure.
I will need to dig into the originals papers again to sort this out, but that's not likely to happen soon.
starting from Jesse's fix, here is my attempt at modifying the rne function. It now runs through without any errors for a number of URDF files I tested, but the resulting torque is quite different from an equivalent model using a DH definition. Hope that helps somebody a bit to finally get this working.
# inverse dynamics (recursive Newton-Euler) using spatial vector notation
def rne(robot, q, qd, qdd, symbolic=False, gravity=None):
n = robot.n
# allocate intermediate variables
Xup = SE3.Alloc(n)
Xtree = SE3.Alloc(n)
v = SpatialVelocity.Alloc(n)
a = SpatialAcceleration.Alloc(n)
f = SpatialForce.Alloc(n)
I = SpatialInertia.Alloc(n) # noqa
s = [] # joint motion subspace
if symbolic:
Q = np.empty((n,), dtype="O") # joint torque/force
else:
Q = np.empty((n,)) # joint torque/force
# A temp variable to handle static joints
Ts = SE3(np.eye(4, dtype="O"), check=False)
# A counter through joints
j = 0
# initialize intermediate variables
for link in robot.links:
if link.isjoint:
I[j] = SpatialInertia(m=link.m, r=link.r)
if symbolic and link.Ts is None:
Xtree[j] = SE3(np.eye(4, dtype="O"), check=False)
else:
Xtree[j] = Ts * SE3(link.Ts, check=False)
if link.v is not None:
s.append(link.v.s)
# Increment the joint counter
j += 1
# Reset the Ts tracker
Ts = SE3(np.eye(4, dtype="O"), check=False)
else:
Ts *= SE3(link.Ts, check=False)
if gravity is None:
a_grav = -SpatialAcceleration(robot.gravity)
else:
a_grav = -SpatialAcceleration(gravity)
# forward recursion
# A counter through joints
j = 0
for link in robot.links:
if link.isjoint:
vJ = SpatialVelocity(s[j] * qd[j])
# transform from parent(j) to j
Xup[j] = link.A(q[j]).inv()
jointParent = link.parent
while not (jointParent is None or jointParent.isjoint):
jointParent = jointParent.parent
if jointParent is None:
v[j] = vJ
a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j])
else:
jp = jointParent.jindex
v[j] = Xup[j] * v[jp] + vJ
a[j] = Xup[j] * a[jp] + SpatialAcceleration(s[j] * qdd[j]) + v[j] @ vJ
f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
# Increment the joint counter
j += 1
# backward recursion
# A counter through joints
j = n-1
for link in reversed(robot.links):
if link.isjoint:
# next line could be np.dot(), but fails for symbolic arguments
Q[j] = np.sum(f[j].A * s[j])
jointParent = link.parent
while not (jointParent is None or jointParent.isjoint):
jointParent = jointParent.parent
if jointParent is not None:
jp = link.jindex
f[jp] = f[jp] + Xup[j] * f[j]
# Decrement the joint counter
j -= 1
return Q
Hi, I tried to obtain the inertial matrix of the URDF of the Puma560 robot but I did not succeed. I implement the proposed solution of jenskober in the last version of the master (v1.0.2) but I could not make it possible to extract the inertial matrix of the URDF of Puma560. The following error is with and without the proposed solution.
Traceback (most recent call last):
File "/robotics-toolbox-python/roboticstoolbox/robot/ET.py", line 505, in A
return ET_T(self.__fknm, q)
TypeError: Symbolic value
During handling of the above exception, another exception occurred:
Traceback (most recent call last):
File "/URDF/Stablity.py", line 21, in <module>
M = Puma.inertia(q)
File "/robotics-toolbox-python/roboticstoolbox/robot/Dynamics.py", line 750, in inertia
In[k, :, :] = self.rne(
File "/robotics-toolbox-python/roboticstoolbox/robot/ERobot.py", line 2102, in rne
Xup[j] = link.A(q[j]).inv()
File "/robotics-toolbox-python/roboticstoolbox/robot/Link.py", line 1431, in A
return SE3(self._Ts @ self._ets[-1].A(q), check=False)
File "/robotics-toolbox-python/roboticstoolbox/robot/ET.py", line 513, in A
return self.axis_func(q)
File "/spatialmath-python/spatialmath/base/transforms3d.py", line 203, in troty
T = smb.r2t(roty(theta, unit))
File "/spatialmath-python/spatialmath/base/transforms3d.py", line 95, in roty
ct = smb.sym.cos(theta)
File "/spatialmath-python/spatialmath/base/symbolic.py", line 134, in cos
return math.cos(theta)
TypeError: only size-1 arrays can be converted to Python scalars
Hello! I have had this problem, and made a PR to fix it: #408 I'm hoping to get it looked at so I thought I would ask here. Or at least the next person looking at this issue might have an idea how to circumvent the problem,
Hi, I have created two robots from build-in functions, as following. However, I am not able to get "inertia" matrix for robot which has been created from URDF, and I have received this error:
File "C:\Users\moham\miniconda3\envs\rtb\lib\site-packages\roboticstoolbox\robot\ERobot.py", line 2102, in rne s[j] = link.v.s AttributeError: 'NoneType' object has no attribute 's' #############Code######################### import roboticstoolbox as rtb panda = rtb.models.URDF.Panda() puma = rtb.models.DH.Puma560()
print(puma.inertia(puma.qr)) print(panda.inertia(panda.qr))