Doubtful opposite definition for 3d rotation in BulletInverseDynamics module

[Rullec]

Hi all, Thanks for your time in this issue.

Background

Recently I'm using ID module in bullet for a robot control problem. I test it in fixed base & revolute-only case then it works well, quite nice.:) But when I try to adapt it to spherical joints & floating base, I can not get the correct answer. For locating this problem, I dig into the source code in this Inverse Dynamics module, especially for the MultiBodyTree::MultiBodyImpl::CalculateInverseDynamics

the definition of euler angles are quite "unusual"

According to my understanding (if I am wrong please let me know, thanks a lot), the input of https://github.com/bulletphysics/bullet3/blob/61928fdde3b7910b280c79d995c356c6166c654e/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp#L278 this function are literally generalized coordinate & velocity & accel, company with a pointer to the desired output "joint_forces", which is explicitly revealed in MultiBodyTree.hpp.

And the first step of it, is to use these generalized vars to update the status of ID tree, which refers to https://github.com/bulletphysics/bullet3/blob/61928fdde3b7910b280c79d995c356c6166c654e/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp#L443 But what gets me confused are these 2 points:

1. the definition of basic euler rot is unusual, I mean normally generalized coordinate is just a collection of euler angles(with no primistic parts), specified in a pre-fixed rotation order, whatever XYZ or ZYX. But according to my opinion, most of the euler angles in generalized systems, they were constructed like this formula https://en.wikipedia.org/wiki/Rotation_matrix

But not what I saw in bullet ID: https://github.com/bulletphysics/bullet3/blob/6b2cae1b1d63056ef48c64b39c8db6027e897663/src/BulletInverseDynamics/IDMath.cpp#L168 They are excatly opposite to each other, which means these 2 mats are another's transpose. It will make the transform matrix, or simply rotation matrix, has an opposite rot effect...

btw, I guess the effect of a key matrix "body.m_body_T_parent"(refered to as Trans) is converting a vector in parent body-fixed frame(refered to as p) to myself body-fixed frame(p'), but not "body to parent", we can get: p = Trans * p'.

2. the rotation order of sphericals and floatings parts are different: for spherical https://github.com/bulletphysics/bullet3/blob/6b2cae1b1d63056ef48c64b39c8db6027e897663/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp#L368 and floating https://github.com/bulletphysics/bullet3/blob/b57557c6cf98048a2711cf8f23b00a99a89398b0/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp#L484

I don't know whether they are mistakes or my misunderstanding them ...

Apologize first...I'm fully a newbie in github and bullet, not quite familared with speaking in an issue and posting. Thank you for any tips :)

[erwincoumans]

The Bullet3/src/BulletInverseDynamics doesn't support spherical joints natively. For spherical joints we use a different code path, as used in deep_mimic, and you need to pass the flags=1 argument:

c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)

If you can fix the Bullet3/src/BulletInverseDynamics spherical joints, happy to accept a pull request (I'm not very familiar with that part of the code and the original author didn't have time to help out adding spherical joints).

[erwincoumans]

You can try out deep_mimic as part of PyBullet like this:

pip3 install pybullet --user python3 -m pybullet_envs.deep_mimic.testrl --arg_file run_humanoid3d_backflip_args.txt See the code here: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_envs/deep_mimic

[erwincoumans]

Note that the issue is only for spherical joints. Floating base should be fine, since the floating coordinates are not controlled. There is a C++ example of inverse dynamics using a floating base in the BulletExampleBrowser.

[dvedova]

Where is BulletExampleBrowser? I can't seem to find this folder anywhere

[Steven89Liu]

bullet3/examples/ExampleBrowser/, FYI

[dvedova]

Great, thank you. Another question: Where is the C++ example of inverse dynamics using a floating base? Above, it was mentioned that "There is a C++ example of inverse dynamics using a floating base in the BulletExampleBrowser."

Thank you for the quick response!

[Rullec]

Note that the issue is only for spherical joints. Floating base should be fine, since the floating coordinates are not controlled. There is a C++ example of inverse dynamics using a floating base in the BulletExampleBrowser.

Oh my god... It's totaly my fault, Sorry erwin! I didn't manage to recevie your message until today TAT, really really sorry.

A good news is that I have solved the problem I encountered in bullet Inverse dynamic module. But unfortunately, I do a BIG revision based on my own understanding of the Newton-euler equation ;) Still very grateful for your constructive help! XD

Great, thank you. Another question: Where is the C++ example of inverse dynamics using a floating base? Above, it was mentioned that "There is a C++ example of inverse dynamics using a floating base in the BulletExampleBrowser."

Thank you for the quick response!

Great, thank you. Another question: Where is the C++ example of inverse dynamics using a floating base? Above, it was mentioned that "There is a C++ example of inverse dynamics using a floating base in the BulletExampleBrowser."

Hi Danny! As you seen I'm the questioner of this issue. I met with this ID problem in the beginning of this year, and after a big struggle & digging in the NE equation, I managed to make the inverse dynamic module work for me. These code snippets now are in my own version of bullet.

If other experts (erwin or other powerful guys) felt hard to offer a timely help, maybe you can try my code. I solved ID successfully with it for an 50-DOF robot, with floating base, spherical, revolute and fixed joints. I am also glad to offer my help on this topic.

Bests, Xudong

[Rullec]

The Bullet3/src/BulletInverseDynamics doesn't support spherical joints natively. For spherical joints we use a different code path, as used in deep_mimic, and you need to pass the flags=1 argument:

c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)

If you can fix the Bullet3/src/BulletInverseDynamics spherical joints, happy to accept a pull request (I'm not very familiar with that part of the code and the original author didn't have time to help out adding spherical joints).

Hi erwin, In feburary I spend a whole month and solved this problem of spherical joints in Inverse Dynamic module (as I claimed, it passed the test on a 50-DOF robot without primstic joints). Id it still necessary for bullet to have this PR? If so, I am also glad to offer my help.

[Rullec]

You can try out deep_mimic as part of PyBullet like this:

pip3 install pybullet --user python3 -m pybullet_envs.deep_mimic.testrl --arg_file run_humanoid3d_backflip_args.txt See the code here: https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_envs/deep_mimic

Also, I worked with the C++ version of bullet ;-) so my debugging work also stayed in the C++ part...

[dvedova]

Xudong, thanks for the response! I sent an email to the gmail account you have listed on your github page.

[erwincoumans]

Id it still necessary for bullet to have this PR?

Thanks, a PR would be appreciated!

I haven't had time to digg into this, there are some other projects, including a new tiny differentiable physics engine (see https://github.com/google-research/tiny-differentiable-simulator).

[Rullec]

Id it still necessary for bullet to have this PR?

Thanks, a PR would be appreciated!

I haven't had time to digg into this, there are some other projects, including a new tiny differentiable physics engine (see https://github.com/google-research/tiny-differentiable-simulator).

Thanks for your recommendation! Differentiable physics is quite interesting. ;-) I will try to learn from your new project.

And I will clean my code, write some helpful comment, then create a PR about this module.

I was also a contributor in the Taichi programming language, it also has a differentiable feature. I just think it maybe helpful to you, hope this is not a bother!

[Rullec]

@erwincoumans hi erwin, would it be proper for us to reopen this issue? I would like to track my working progress in this issue, so that you & other people can know my work plan.

[Rullec]

Hi @erwincoumans , till now I have nearly finished cleaning my code, but I still need to do some verification work before a PR. I saw that there are literally two examples in the ExampleBrowser which are responsible for the InverseDynamics. They try to use the Inverse model to have a more stable PD control.

It's quite a good way to illurstrate the usage of InverseDynamic module. But it cannot teach other guys how to integrate the external contact force (or other cartesian forces) into the Inverse Model, which is also what I want to tell (It troubles me a lot in the begining too ;-)). My plan is to add a new example entry, in the subdirectory of InverseDynamic, which can solve the activate control force of a multibody accurately when it is even effected by other contact forces/ perturbation. HDYT?

Bests, Xudong

[Rullec]

Another question is about the test and some other fit. ;-) Do I need to do some more test and then recorded them in a particular pipeline? How & What can I tried to do the test in pybullet?

Very eager to get you guidence! XD

[erwincoumans]

There is a Python example that uses spherical joints and inverse dynamics, see https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/humanoidMotionCapture.py It would be great if you can make your inverse dynamics spherical joint work with that example.

You can add unit tests in the Bullet/tests folder and run them using

mkdir build
cd build
cmake ..
ctest

There are also Python tests, in the https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/unittests directory. You can run those for example using

python3 examples/pybullet/unittests/unittests.py --verbose

See also the travis CI script: https://github.com/bulletphysics/bullet3/blob/master/.ci/script.sh

[Rullec]

Thanks a lot for your detailed reply. I will read them carefully!