Need help implementing a simple joint controller...

[GlenHenshaw]

The following code throws an error, and I don't understand why:

import numpy
import roboticstoolbox as rtb
robot = rtb.models.Panda()
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)

the error is:

  File "/usr/local/lib/python3.10/site-packages/roboticstoolbox/robot/Dynamics.py", line 326, in fdyn
    integrator = scipy_integrator(
  File "/usr/local/lib/python3.10/site-packages/scipy/integrate/_ivp/rk.py", line 94, in __init__
    self.f = self.fun(self.t, self.y)
  File "/usr/local/lib/python3.10/site-packages/scipy/integrate/_ivp/base.py", line 154, in fun
    return self.fun_single(t, y)
  File "/usr/local/lib/python3.10/site-packages/scipy/integrate/_ivp/base.py", line 23, in fun_wrapped
    return np.asarray(fun(t, y), dtype=dtype)
  File "/usr/local/lib/python3.10/site-packages/roboticstoolbox/robot/Dynamics.py", line 327, in <lambda>
    lambda t, y: self._fdyn(t, y, Q, Q_args),
  File "/usr/local/lib/python3.10/site-packages/roboticstoolbox/robot/Dynamics.py", line 422, in _fdyn
    qdd = self.accel(q, qd, tau)
  File "/usr/local/lib/python3.10/site-packages/roboticstoolbox/robot/Dynamics.py", line 500, in accel
    M = self.rne(qI, qdI, qddI, gravity=[0, 0, 0])
  File "/usr/local/lib/python3.10/site-packages/roboticstoolbox/robot/Robot.py", line 1794, 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```

[TEHREEMZUBAIR]

The error you are encountering seems to originate from the rne function within the roboticstoolbox library. Specifically, the issue arises due to an invalid index when accessing the v[jp] variable, where jp is None. This means that the parent link for the current joint is not being set correctly, leading to the TypeError when indexing.

In the Panda robot model, each joint should have a valid parent link. If jp is None, it implies that the model isn't correctly assigning parent-child relationships between joints. Here's how you can troubleshoot:

1. Check for Compatibility: Ensure that the roboticstoolbox version you're using is compatible with your robot model. The Panda robot model has 7 degrees of freedom, and the dynamic functions (like fdyn) depend on the robot's structure being correctly initialized. Updating the roboticstoolbox library might resolve any internal issues with how the model is loaded.

2. Controller Function Signature: Make sure the dumbcontroller function is correct in terms of its arguments and return type. It's expected to return a valid control input (tau), but currently, it returns all zeros, which might cause issues if the robot expects certain torques or forces to be applied. You could try returning small values or testing with different control signals.

   def dumbcontroller(robot, t, q, qd):
       # Apply small control values to avoid issues with zero torques
       return numpy.array([0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01])

3. Inspect Robot Model: Check if the Panda robot model is correctly loaded. You could inspect the robot’s joints and links to ensure there are no issues with the parent-child hierarchy:

   print(robot)

This error reminds me of how small setup details in complex projects, like yours with university enrollment analysis, can have cascading effects. Just like correctly normalizing enrollment data over the years helps avoid inconsistencies in trends, setting up the robot's structure correctly is crucial to smooth dynamics simulation.

If you're still stuck after trying the above, you could explore using a different robot model temporarily to see if the issue persists across models. Let me know how it goes!

[GlenHenshaw]

Thanks for the response. The problem turned out to be that the Panda model doesn't have dynamics parameters (mainly inertias, I think). It was a kinematics only model. The other robot arms I tried were also kinematics-only, so they all threw the same error. Temporarily switching to the Puma model, which does have dynamics parameters, fixed the problem.

[petercorke]

Hi @GlenHenshaw, thank you for your question and for posting the cause, and I'm sorry for the long delay: various life events got in the way, and since May I've been doing major work across the Robotics Toolbox, Swift and SpatialGeometry.

As you found, the default models such as rtb.models.Panda (and the ETS rtb.models.Puma560) are kinematics-only, with no inertial parameters, so dynamics functions such as fdyn and rne fail on them. The DH versions include dynamics, for example rtb.models.DH.Panda() and rtb.models.DH.Puma560(), and those work with fdyn.

I'm closing this as resolved. If you hit another problem, please post an Issue rather than a discussion item.

[petercorke]

Update: the unhelpful error from kinematics-only models is now tracked in #722.