why it doesn't match with the equations of Brian Mirtich in his PhD thesis?

[Steven89Liu]

these days i again read the function of computeAccelerationsArticulatedBodyAlgorithmMultiDof() and try to derive the result according to the process of this function, but find it doesn't match with the equations of Mirtich in his PhD thesis. i don't know if i made some mistake or the engine has its own other consideration that i don't know. hope someone can give me some explanation. thanks in advance.

Take the revolute for example, this is some code snippet.
`fromParent.transform(spatVel[parent + 1], spatVel[i + 1]); //step 1

spatJointVel.setZero()
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof] //step 2

spatVel[i+1] += spatJointVel //step 3
spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]) //step 4

zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())) //step 5`
derive these variables with this below

In my understanding, the Rw x (Rw x r) is the centripetal force and the 2Rw x (u x d)q' is the coriolis force(refer to the Definition 10 of Brian Mirtich's thesis). i have these several questions.

1. why the coriolis force is split into tow parts. one in the SpatCoriolisAcc and the other is in the linear part of zeroAccSpatFrc?
2. what does the (Rv + Rw x r) x uq' mean in the SpatCoriolisAcc?
3. Rw x Rv in the ZeroAccSpatFrc.addLinear?

[erwincoumans]

It is complicated indeed. Could you have a look at this PhD thesis, search for Coriolis and check out chapter 8.3.1B: http://delibra.bg.polsl.pl/Content/12190/J.Stepien-rozprawa-dr.pdf?handler=pdf

The original implementation is here:
f02dd51#diff-c7da1a237b2b0f6a5d7a28f6666aa2a8R394

It would be good to have a more clear/clean implementation of this indeed.

[erwincoumans]

Also, the implementation changed from original Mirtich PhD thesis towards Featherstone 2008 book by Jakub Stephien. So checking up that chapter 8.3.1 that I pointed you to will be helpful anyway.

We do have a unit test to round-trip Bullet inverse dynamics and forward dynamics, and they match well. See https://github.com/bulletphysics/bullet3/blob/master/test/InverseDynamics/test_invdyn_bullet.cpp

/// create a bullet btMultiBody model of a tree structured multibody system,
/// convert that model to a MultiBodyTree model.
/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces
///      - run forward dynamics on (q,u, forces) to get accelerations
///      - compare input accelerations to inverse dynamics to output from forward dynamics

It would be very useful to re-implement it again, or hook up RBDL library and compare the input and ouput carefully for this forward dynamics ABA implementation.

Someone did such comparison for the inverse dynamics implementation, and the difference is very small (although not totally zero, close to 1e-6 for 1 step). See #2246

Are you interested in helping with this?

[Steven89Liu]

thanks, it would be honour for me to accept this task.
i want to make your question more clear, do you want to implement a test similar to test_invdyn_bullet.cpp but comparing the result between the RBDL's and bullet's forward dynamics ABA implementation ?
- run forward dynamics on (q,u, forces) to get accelerations with bullet
- run forward dynamics on (q,u, forces) to get accelerations with RBDL
- compare the result between them.

[Steven89Liu]

i have finished the code, including:

1. port the rbdl to the Extras/rbdl, and only change the CMakeLists.txt to make it built together with the bullet.
2. add the pantilt.urdf to the data/
3. add a test named test_dynABA_bulletVSrbdl.cpp in the test/InverseDynamics/.
   this function implements,
   - run forward dynamics on (q,u, forces) to get accelerations with RBDL
   - run forward dynamics on (q,u, forces) to get accelerations with bullet
   - compare the result between them.
   now i'm debugging this test and the difference between them is too big, maybe i made some mistake somewhere. i will continue to debug it later.

[erwincoumans]

Thanks for looking into this. If/when you got the comparison right, it would be nice to also re-implement only the computeAccelerationsArticulatedBodyAlgorithmMultiDof call using rbdl. It could be that the URDF loader or other parts of RBDL are different. It would involve some conversion between Bullet and RBDL/Eigen data structures.

[Steven89Liu]

i have finished this part and meanwhile also find the rootcause of the #2246, now the difference between them is less than the 1e-11, i will submit the codereview. after this, i will try to re-implement the computeAccelerationsArticulatedBodyAlgorithmMultiDof call using rbdl.

[erwincoumans]

Were you able to map and/or derive computeAccelerationsArticulatedBodyAlgorithmMultiDof to the formulation described in Featherstone's book/Jakub's PhD thesis?

[Steven89Liu]

yes. i mainly refer to the Featherstone's book, the Table 7.1 for details of the ABA. the RBDL tightly follows the procedure of this book, so the code is more easily to be understood.
the main difference between the rbdl's and bullet's implementation includes,

1. rbdl:  the origin of the coordiante of the formula is located in the joint
   

   bullet: the origin of the coordinate of the formula is in the certer of the mass.
   this will cause the difference of the spacial Axis (S) and spacial Inertia (I) but the intermediate value of I'SI is the same. (starting from this point, i find the rootcause of the issue Numerical difference between pybullet and RBDL/KDL #2246.)
2. rbdl: the gravity is applied to the initial acceleration (a)
   copied from the page 94 of Featherstone's book: (If this trick is used, then the calculated values of ai and fBi are no longer the true acceleration and net force for body i, but are offset by the gravitational acceleration and force vectors)
   bullet: the gravity is applied to the bias force (p) (called spatial articulated zero-acceleration in Mirtich's)
3. cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl; (line 246 in the urdfreader.cc.)
   currentlly the rbdl test has this limitation. i'm not sure if the rbdl library itself also has this limitation.

So do you still think it is necessary or important to re-implement the computeAccelerationsArticulatedBodyAlgorithmMultiDof? if not, i can help to investigate other issues.

[erwincoumans]

It would be great to have some document going over the implementation line by line to compare RBDL and Bullet. Do you have time to work on such doc? Some sketches on paper helps too (like your previous writing in the beginning of this thread).

Yes, it would still be helpful to re-implement the computeAccelerationsArticulatedBodyAlgorithmMultiDof in a much more readable way, similar to RBDL and Featherstone book. Preferably without requiring the RBDL library dependency.

Thanks a lot for looking into all this!

[Steven89Liu]

OK, i will think about a easy and clear way to compare these two methods.

[Steven89Liu]

[Steven89Liu]

attach the original excel also.
compareABA.xlsx