Payload and rne computation

[williamledda]

It seems there is a bug computing the inverse dynamics with rne method.

Take a look at the following script that compute the inverse dynamics for a Puma560 (but this is the same for each model)

import numpy as np
import roboticstoolbox as rtb

if __name__ == '__main__':
    # load a robot model
    p560 = rtb.models.DH.Puma560()

    p560.q = [
        45 * np.pi / 180,
        70 * np.pi / 180,
        -100 * np.pi / 180,
        60 * np.pi / 180,
        25 * np.pi / 180,
        -140 * np.pi / 180,
    ]

    zero_v = np.zeros(p560.n)

    # p560.payload(m=0, p=np.zeros(3))
    rne = p560.rne(q=p560.q, qd=zero_v, qdd=zero_v)

    tau_str = ''

    for i in range(len(rne)):
        tau_str += '{:.3f} '.format(rne[i])

    print(tau_str)

If I run the script without setting any payload, the result that I have is the following

-0.000 16.368 4.595 -0.005 -0.004 0.000 

If now I now remove the comment to set the payload

p560.payload(m=0, p=np.zeros(3))

I expect to have the same numbers (payload and center of mass are set to zero) but I have the following result instead.

-0.000 16.024 4.381 0.000 0.000 0.000

I've reported a similar issue for the MATLAB toolbox, I think that the behaves the same.

[williamledda]

I think that the problem is in the payload inside the Dynamics class:

    def payload(self, m, p=np.zeros(3)):
        # ...
        lastlink.m = m
        lastlink.r = p

So when setting the payload, this will actually replace the mass of the last link in the DH model. Instead of replacing it, the mass should be added to. This would actually fix the issue. Not completely about what to do with the the center of mass. It should be added as well? Alternatively the payload should be considered as 'additional' link with its own mass and center of mass.

[petercorke]

This is on the list of things to fix, it was a quick hack in the MATLAB version and not really proper.