From bd93cfa0f82819fb832d48b5fa6082b901ddadc4 Mon Sep 17 00:00:00 2001
From: codeFighter2022 <116827556+codeFighter2022@users.noreply.github.com>
Date: Wed, 19 Mar 2025 14:56:41 +0100
Subject: [PATCH] Update Robot.py

Inverse dynamics with recursive Newton-Euler does not work for URDF imports and also not for the Panda model:

import roboticstoolbox as rtb
robot = rtb.models.Panda()
q = np.random.rand(robot.n)
qD = np.random.rand(robot.n)
qDD = np.random.rand(robot.n)
tau = robot.rne(q, qD, qDD)

The origin of the error is
jp = self.links[j].parent.jindex
which is empty for some types.

I implemented a another search algorithm of the parent algorithm via:
jp = next(i for i, link in enumerate(self.links) if link.name == parent_name)
---
 roboticstoolbox/robot/Robot.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py
index eadab10d..2ad479e3 100644
--- a/roboticstoolbox/robot/Robot.py
+++ b/roboticstoolbox/robot/Robot.py
@@ -1790,7 +1790,8 @@ def rne(
                     v[j] = vJ
                     a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[j])
                 else:
-                    jp = self.links[j].parent.jindex  # type: ignore
+                    parent_name = self.links[j].parent.name
+                    jp = next(i for i, link in enumerate(self.links) if link.name == parent_name)
                     v[j] = Xup[j] * v[jp] + vJ
                     a[j] = (
                         Xup[j] * a[jp] + SpatialAcceleration(s[j] * qddk[j]) + v[j] @ vJ
@@ -1804,7 +1805,8 @@ def rne(
                 Q[k, j] = sum(f[j].A * s[j])
 
                 if self.links[j].parent is not None:
-                    jp = self.links[j].parent.jindex  # type: ignore
+                    parent_name = self.links[j].parent.name
+                    jp = next(i for i, link in enumerate(self.links) if link.name == parent_name)
                     f[jp] = f[jp] + Xup[j] * f[j]
 
         if l == 1: