Allow full 6D wrench computation in LOCAL and WORLD frames

[skleff1994]

Description

So far only linear forces are computed (in WORLD) . This PR adds angular force computation + LOCAL and WORLD wrench expressions (LOCAL is by default)

How I Tested

Do not merge before

• [x] link to dependent pull request

I fulfilled the following requirements

• [x] All new code is formatted according to our style guide (for C++ run clang-format, for Python, run flake8 and fix all warnings).
• [x] All new functions/classes are documented and existing documentation is updated according to changes.
• [x] No commented code from testing/debugging is kept (unless there is a good reason to keep it).