RNEA method

[Tianhuanyu]

Description

The RNEA method is added to model module of optas for computing inverseDynamics. Given q, qd, qdd, the function could return a external force vector.

The RNEA is tested with test_rnea.py

The current version of RNEA only supports the robots with:

1. resolute joints,
2. the first joint of robot fixed,
3. no fixed joints except the first joint and the last joint.

Todo

• [x] Completed implementation of new feature.
• [x] Add unit-test for the new feature.
• [ ] Add documentation for the new feature.
• [ ] Ensure all unit tests pass.
• [ ] Address all issues raised by unit-tests.
• [ ] Consider adding an example that demonstrates the use of the new feature (not always necessary).
• [ ] Added code reviewer.

[cmower]

Please also address the todo list in the PR description above.

[cmower]

Could you also make sure to add a unit test in tests/test_models.py. It would be great if you could generate random q, dq, ddq and check the output of your function against the output of PyBullet RNEA method to ensure it is at-least numerically equivalent.

[Tianhuanyu]

Thanks @Tianhuanyu! This is great! I have some feedback (please see comments). Also, you should run black on the files you modify - this will ensure the code formatting is correct.

Thanks, @cmower . I try black for reformatting my code. It's fantastic. All the following parts are done within this update.

[Tianhuanyu]

Could you also make sure to add a unit test in tests/test_models.py. It would be great if you could generate random q, dq, ddq and check the output of your function against the output of PyBullet RNEA method to ensure it is at-least numerically equivalent.

Sure, but I find there is always a small error between PyBullet output and our RNEA. (around 0.5% or less). Maybe Pybullet uses a different method for computing inverse dynamics or it may consider friction. Currently, the error doesn't have much effect on my application. It maybe a long-term work to find out why the numbers are slightly different.

[Tianhuanyu]

Hi @Tianhuanyu, thanks for the updates. Just a couple more changes.

Also, could you please add a unit test (that compares RobotModel.rnea to PyBullet) into tests/test_models.py. The small error you see is fine. As you mention, PyBullet probably takes into account friction or maybe other things. The test is good to have though because it verifies i) you're implementation is working (i.e. the code runs), and ii) the values it outputs are "pretty much" what we expect. Take a look here for more details about the implementation of RNEA in PyBullet.

Thanks. @cmower I've submitted the modification for the two minor suggestion. I will submit the testing part as soon as possible.

[cmower]

Great, thanks @Tianhuanyu.

[cmower]

@Tianhuanyu, please could you rebase this branch. There are new commits to the master branch.