Acceleration Calculation of the RNE in modified DH

[hanm2019]

Describe the bug
the vd is code vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd may be have some problem.

Version information
master

Expected behavior
when the joint is revolute, I think the vd = Rt@(vd + _cross(wd,pstar) + _cross(w, _cross(w, pstar)))

since,

then, We add the RT trans at each side.

[hanm2019]

I read the ne.c code, It also show that, after calculate the sum of a_i, cross(wd,pstar), cross(w,cross(w,pstar)) (in line 173-177), the sum is then mult a ROT to transform the reference axis

[hanm2019]

I write a test:

import numpy.testing as nt
import numpy as np
import roboticstoolbox as rp
import spatialmath as sm
import unittest
import math

panda = rp.models.DH.Panda()
print(panda.mdh)

dq = np.array([1,1,1,1,1,1,1])
ddq = np.array([1,2,3,4,5,6,7])
t0 = panda.rne(panda.qr, dq, ddq)
tp0 = panda.rne_python(panda.qz, dq, ddq, debug=True)
print('t0:',t0)
print('tp0:',tp0)

but get two difference result.
since the Panda Robot'm is unset, default is 0, so, I change the Link.py:143 to dynchange += dynpar(self, "m", m, 1.0) so, each Link is 1 kg

the testcase get two result:

t0: [  3.69972741 -12.09275043   4.30749236  13.13058012   0.22469786   1.01257754   0.        ]
tp0: [ 1.09406   -0.20341    1.09406    0.9699845  1.04192    0.864377   0.       ]

so, I think the rne and rne_python may have some bug in it.

Even I update the vd_ in Line 1468, the result also not same.

After read the rne_python code, I think the Line1539 maybe have some problem.

nn_ = (
                       R @ nn
                       + _cross(pstar, R @ f)
                       + _cross(pstar, Fm[:, j])
                       + Nm[:, j]
                   )

nn should be the sum of 4 part of vector, the third part which should be the link.r x Fm , not pstar x Fm , since the Fm is in the center of mess, not at the endpoint.

[petercorke]

Quick question, it's been ages since I looked at this code, the vd fix is for an inconsistency between the C version, the Python version. That's not good! What reference or text are you using for truth?