function rne_park_forward issue

[zhengf0509]

In this function, dV[i] = ifunc(geom.S[i] * rbtdef.ddq[i]) +
ifunc(Adj(geom.Tdh_inv[i], dV[i - 1])) +
ifunc(adj(ifunc(Adj(geom.Tdh_inv[i], V[i - 1])), ifunc(geom.S[i] * rbtdef.dq[i])))

but I think dV[i] = ifunc(geom.S[i] * rbtdef.ddq[i]) +
ifunc(Adj(geom.Tdh_inv[i], dV[i - 1])) +
ifunc(adj(ifunc(Adj(geom.Tdh_inv[i], V[i - 1]) +ifunc(geom.S[i] * rbtdef.dq[i])), ifunc(geom.S[i] * rbtdef.dq[i])))

beccause the bold font content should be the adjoint matrix of twist V[i]。

[cdsousa]

Please, make a Pull Request. It will be easier to review the change.