RNE Function on URDF Robot Models

[dbacher42]

I am trying to do dynamic simulations on robots using the URDF models. When I run a function that uses the rne() function, I run into errors.

When I run the following commands:

import roboticstoolbox as rtb
robot = rtb.models.URDF.UR10()
M = robot.inertia(robot.q)

I get the following error:

Traceback (most recent call last):

  File "C:\Users\dbacher\OneDrive - NASA\Documents\Python Scripts\UR10 URDF.py", line 18, in <module>
    M = robot.inertia(robot.q)

  File "C:\Users\dbacher\Anaconda3\envs\rrt-mpc\lib\site-packages\roboticstoolbox\robot\Dynamics.py", line 696, in inertia
    In[k, :, :] = self.rne(

  File "C:\Users\dbacher\Anaconda3\envs\rrt-mpc\lib\site-packages\roboticstoolbox\robot\ERobot.py", line 2110, in rne
    v[j] = Xup[j] * v[jp] + vJ

  File "C:\Users\dbacher\OneDrive - NASA\Documents\GitHub\spatialmath-python\spatialmath\spatialvector.py", line 148, in __getitem__
    return self.__class__(self.data[i])

TypeError: list indices must be integers or slices, not NoneType

If I run the same code on a Panda or another robot with a different number of joints, I get a different error:

import roboticstoolbox as rtb
robot = rtb.models.URDF.Panda()
M = robot.inertia(robot.q)

Traceback (most recent call last):

  File "C:\Users\dbacher\OneDrive - NASA\Documents\Python Scripts\UR10 URDF.py", line 18, in <module>
    M = robot.inertia(robot.q)

  File "C:\Users\dbacher\Anaconda3\envs\rrt-mpc\lib\site-packages\roboticstoolbox\robot\Dynamics.py", line 696, in inertia
    In[k, :, :] = self.rne(

  File "C:\Users\dbacher\Anaconda3\envs\rrt-mpc\lib\site-packages\roboticstoolbox\robot\ERobot.py", line 2100, in rne
    vJ = SpatialVelocity(s[j] * qd[j])

ValueError: operands could not be broadcast together with shapes (6,) (7,) 

It seems like there is some problem with between the rne() function and the URDF models. It works with the DH models, but for my application I have a custom end effector that can't be modeled as a point mass, and so we need to use the URDF description with the moments of inertia.

[petercorke]

I can reproduce the bug. There is a long standing issue, #236, for links with fixed transforms, of which this model has many. This code needs to be revisited, sorry for the inconvenience.

[thvhauwe]

I've experienced this issue and found a workaround by changing the way the inertia matrix is calculated.

In the function def inertia(self, q): in file ./roboticstoolbox/robot/Dynamics.py

Changed

        for k, qk in enumerate(q):
            In[k, :, :] = self.rne(
                (np.c_[qk] @ np.ones((1, self.n))).T,
                np.zeros((self.n, self.n)),
                np.eye(self.n),
                gravity=[0, 0, 0],
            )

To

        for k, qk in enumerate(q):
            for i in range(self.n):
                In[k, i, :] = self.rne(
                    np.array(qk),
                    np.zeros([self.n]),
                    np.eye(self.n)[i],
                    gravity=[0, 0, 0],
                )

Such that the inertia matrix gets calculated row by row using rne(). For my URDF model with fixed joints this does not throw an error

[niederha]

@petercorke I've run into the same bug... I am not super familiar with the math behind this, and this part of the code is a little opaque... could you confirm that the fix by @thvhauwe is correct? And if so could we maybe merge it on main and if possible update the pip package? It would be a big plus on my side for portability if I were able to get this working fix from pip. Let me know if I can assist in any way.

[niederha]

Ok, well I needed something that worked so I made a fix. PR #408 should fix the problem. Looking forward for feedback on that and fixing that bug!