robot.rne function does not work for robots with static joints #236
Comments
|
That URDF model has no dynamic parameters set 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. |
petercorke
changed the title
petercorke
changed the title
|
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, |
|
Hello! |
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))
The text was updated successfully, but these errors were encountered: