Dynamics broken for some models (?)

Describe the bug

Some URDF-based robot models that include dynamics information are either not handled correctly by BaseRobot._sort_links() or Robot.rne() (not sure which). This causes rne() to halt with an error.

Version information

Did you install from PyPI or GitHub?

PyPI

If PyPI what version number?

1.1.1

To Reproduce

Executing something like the following:

import numpy
import roboticstoolbox as rtb
robot = rtb.models.KinovaGen3()
q0 = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

def dumbcontroller(robot, t, q, qd):
    return numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

q = robot.fdyn(1, q0, dumbcontroller)

results in the following error:

  File "/usr/local/lib/python3.10/site-packages/roboticstoolbox/robot/Robot.py", line 1791, in rne
    v[j] = Xup[j] * v[jp] + vJ
  File "/usr/local/lib/python3.10/site-packages/spatialmath/spatialvector.py", line 148, in __getitem__
    return self.__class__(self.data[i])
TypeError: list indices must be integers or slices, not NoneType```

Expected behavior I expect rne() to execute without errors.

As best I can tell, this is present in URDF robots, but of course only manifests in URDF robots that have embedded dynamics information in their URDF files; however, the the same error is latent in for instance the URDF version of Panda(), but not the DH version. In the URDF version, the parent frame of the first actuated frame is the base frame, but the base frame does not have a jindex assigned. rne() incorrectly assumes that when it encounters the first frame with a parent, that parent will have a jindex and should be included in the rne calculations. As a consequence, the following lines in Robot.rne() (lines 1790-1791) fail:

jp = self.links[j].parent.jindex  # type: ignore
v[j] = Xup[j] * v[jp] + vJ

because jp is None (not a valid index) when j=1:

>>> robot = rtb.models.KinovaGen3()
>>> robot2 = rtb.models.Panda()
>>> robot3 = rtb.models.DH.Panda()
>>> for l in robot.links:
...     print(l.jindex)
...
None
0
1
2
3
4
5
6
None
None
>>> for l in robot2.links:
...     print(l.jindex)
...
None
0
1
2
3
4
5
6
None
>>> for l in robot3.links:
...     print(l.jindex)
...
0
1
2
3
4
5
6

Note that unlike the URDF version of Panda(), KinovaGen3() is a URDF-based model which does include dynamics information, which can be seen once the model is instantiated,. and should therefore be properly handled by the dynamics routines.

Environment (please complete the following information): macOS 15.0.1 python 3.10.8

petercorke / robotics-toolbox-python

Dynamics broken for some models (?) #463