rne_python in mdh module

[hanm2019]

I find the result is different between rne and rne_python function when I use a MDH module Robot, After review the code, I find two bugs in there, and After update the code, it finally get the same result.

1. In rne forward, the Rt should dot with all three types of vd corrent code:

vd_ = Rt @ _cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd

should update to:

vd_ = Rt @ (_cross(wd, pstar) + _cross(w, _cross(w, pstar)) + vd)

2. In rne backward , the Fm should cross with the center of mass , not the pstar corrent code:

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

should update to:

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

[hanm2019]

#266