Add Featherstone constraint solver for articulated systems

[GoogleCodeExporter]

For NaturalMotion forward dynamics, powering ragdoll joints using strong 
motors, it is useful to have a Featherstone implementation available.

- including motors
- handling contacts

See Vangelis Kokkevis comments/paper on Bullet forums:
http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?
f=4&t=148&hilit=vangelis

We received some Featherstone implementations that need cleanup.

Original issue reported on code.google.com by erwin.coumans on 1 Mar 2008 at 7:51

[GoogleCodeExporter]

Hi Erwin,
  Do you already have code for this?  It's still on my to-do list to get Featherstone 
to work with Bullet. 

Cheers,
Vangelis

Original comment by vkokke...@gmail.com on 4 Mar 2009 at 11:43

[GoogleCodeExporter]

[deleted comment]

[GoogleCodeExporter]

Hi Vangelis,

Good to hear from you again. No progress on this really.

It would be great if you can contribute something. I'm happy to help merging 
any code 
you have. Do you consider making it available under the Zlib license?
Thanks!
Erwin

Original comment by erwin.coumans on 5 Mar 2009 at 4:39

[GoogleCodeExporter]

Hi Erwin,

I hope to find some time to do this soon. No problems with the Zlib license.  
I'll 
let you know how it goes. 

Cheers,
Vangelis

Original comment by vkokke...@gmail.com on 5 Mar 2009 at 4:07

[GoogleCodeExporter]

If you lack time, perhaps you could try to get a Google Summer of Code project 
for 
integrating your Featherstone implementation into Bullet?

Although someone would need to take the time to mentor the student.

Original comment by erwin.coumans on 22 Mar 2009 at 5:42

[GoogleCodeExporter]

Someone else who has/had interest in Featherstone:
http://bit.ly/aEjeLm

Original comment by erwin.coumans on 17 May 2010 at 5:51

[GoogleCodeExporter]

[deleted comment]

[GoogleCodeExporter]

Original comment by erwin.coumans on 27 Sep 2012 at 3:32

• Changed state: Accepted

[GoogleCodeExporter]

Here is a zlib licensed Featherstone implementation, thanks to Roy Featherstone 
for the link: https://bitbucket.org/rbdl/rbdl / http://rbdl.bitbucket.org by 
Martin Felis

Original comment by erwin.coumans on 28 Sep 2012 at 5:18

[GoogleCodeExporter]

If there is any help needed I would love to help out.

The RBDL also contains an implementation of the contact force computation 
described in the paper by Vangelis Kokkevis 
http://research.scea.com/research/pdfs/VangelisK_GDC2004.pdf .

Original comment by martin.f...@gmail.com on 5 Oct 2012 at 4:03

[GoogleCodeExporter]

Hi Martin, that sounds great. 

Vangelis was my previous colleague and he posted on comment #4, but I guess his 
work at Google is keeping him too busy. I wonder what he is doing during his 
"20% time" there.

Anyhow, I hope to find time for your library soon.

Original comment by erwin.coumans on 5 Oct 2012 at 4:19

[GoogleCodeExporter]

Martin, glad to hear that some of the work from the paper found its way to 
opensource code.  I've been meaning to do that for a really long time!

Original comment by vkokke...@gmail.com on 5 Oct 2012 at 4:19

[GoogleCodeExporter]

@Vangelis: yeah I've been wanting to implement your method it since I found the 
GDC paper somewhere around 2009 and am now quite satisfied with the library. So 
thanks for writing it up!

@Erwin: I wrote the code for my research and stuck very close to Featherstones 
book (which I think is great). Please do not hesitate to contact me if you have 
questions.

Original comment by martin.f...@gmail.com on 5 Oct 2012 at 9:21

[GoogleCodeExporter]

Martin, your library is under a permissive license ,but it depends on Eigen 
library which is not under a permissive license. That is a showstopper, so we 
need to write a replacement math library and add it to Bullet linear algebra 
math classes.

What math properties do you use from Eigen ?

Original comment by erwin.coumans on 7 Oct 2012 at 9:07

[GoogleCodeExporter]

I took care to have only few dependencies on the math library. So replacing 
Eigen should be feasible. 

A replacement for Eigen already exists that can make RBDL work completely Eigen 
free. To do so compile RBDL using SimpleMath by setting RBDL_USE_SIMPLE_MATH to 
"ON". I wrote SimpleMath as a fast compiling substitute to Eigen and it 
contains everything to make it work but it does not match Eigen's performance. 
You can use SimpleMath for whatever you want. If required I can also publish it 
formally under the zlib license.

The most advanced features I use from Eigen are the matrix decompositions (QR 
and LU) to solve linear systems (e.g. contact forces for Vangelis' method but 
also in ForwardDynamicsLagrangian). In the SimpleMath library I have a LU 
solver which should work.

Apart from that I use the Eigen's block syntax when accessing parts of 6-D 
elements. They are of the form matrix.block<nrows, ncols>(row_index, 
col_index). But this is mostly done in SpatialAlgebraOperators.h and can be 
replaced with something else.

Btw. Eigen recently changed their license to MPL. Maybe this makes it usable in 
bullet? 

Original comment by martin.f...@gmail.com on 7 Oct 2012 at 11:02

[GoogleCodeExporter]

Thanks for your fast reply.

MPL is still not acceptable, unfortunately some of the Eigen developers still 
can't accept the BSD style license so it is a showstopper for that library.

It is good to know that the SimpleMath provides an unoptimized alternative. It 
should be doable to reimplement the functionality and improve the performance 
compared to the Eigen version.

It is like that other changes need to be made to the RBDL before merging it 
into Bullet, but that's OK.  I have not looked into RBDL details yet, but STL 
and BOOST or any other external dependency are not acceptable: all Bullet code 
needs to be self contained.

Original comment by erwin.coumans on 8 Oct 2012 at 2:34

[GoogleCodeExporter]

You mean STL (Standard Template Library)? I also do use it. More specifically 
std::string, std::vector, std::map, std::sstream. But I guess they should be 
relatively easy mapped to C strings, btAlignedObjectArray and btHashMaps. 
std::sstream is mostly used for logging which could be ignored.

But there are is one major difference between RBDL and Bullet that is probably 
more significant:

Models in RBDL store each type of value (Body, Joint, Transformations, spatial 
velocity, spatial acceleration, spatial bias forces, ...) in separate 
std::vectors. The Body class only contains its mass properties (mass, center of 
mass, inertia).

In Bullet all values related to a single body are stored within btRigidBody.

How to transfer this is probably the most difficult part on integration, 
especially since the nicely readable Articulated Body Algorithm (ABA) pseudo 
code uses temporary variables (i.e. pA, U, d, u) that would only be required 
for bodies within an articulated model.

Furthermore when using Vangelis' method (ForwardDynamicsContacts) there are a 
few more variables that are only needed when using this method. In RBDL they 
are stored in the ConstraintSet. Maybe a similar approach of using a additional 
data structure to store method related variables would be viable for Bullet.

Original comment by martin.f...@gmail.com on 8 Oct 2012 at 12:28

[GoogleCodeExporter]

Indeed, std::vector and std::map can be replaced. Why are you using 
std::string? Is that just for debugging?

One way to deal with the data is to create some special object, say a 
btFeatherstoneHierarchy that contains an array of pointers to btRigidBody, and 
it keeps all its own data structures internally, including the links and data 
needed for the articulated body method (ABM).

Original comment by erwin.coumans on 9 Oct 2012 at 3:57

[GoogleCodeExporter]

std::strings are only used for debugging and utility purposes such as finding 
the body id for a given body name and vice versa.

Yes using a additional object to store the data sounds reasonable to me. Then 
the approach and code implemented in RBDL is much easier to add to Bullet.

I personally find the naming btArticulatedHierarchy better fitting. Also 
calling the method ABM or ABA instead of Featherstone method/solver. I do have 
great respect for him and also this particular method but he invented a great 
deal of other methods and therefore find calling it Featherstone method a bit 
vague. (But maybe I am too much of a mathematician here ;-D)

Original comment by martin.f...@gmail.com on 10 Oct 2012 at 10:04

[GoogleCodeExporter]

Does your library handle closed loops? If so, can you explain what approach?

Also, do you have any videos and/or graphical demos, showing your library in 
action?
(I suppose we will have that once the integration is done, but I'm just curious)

Original comment by erwin.coumans on 11 Oct 2012 at 1:24

[GoogleCodeExporter]

RBDL does not yet handle closed loops. However as described in Vangelis' Paper 
"Efficient Dynamic Constraints for Animating Articulated Figures" published in 
Multibody System Dynamics, 1998, section 5.1 this should be possible through a 
small modification. I have not yet implemented it as I had no requirement for 
it.

You can find a video here: 
http://www.fysx.org/2011/01/07/articulated-body-algorithm-first-video/. If you 
watch carefully you can see that the system gains energy, but this is entirely 
due to the simple Euler integrator I used here.

I will also send you via email a link for some research results that I will 
soon present at the Motion In Games conference.

Original comment by martin.f...@gmail.com on 11 Oct 2012 at 5:35

[GoogleCodeExporter]

Thanks for your videos!

I had a look at that paper, chapter 5.1 and it looks like contact constraints, 
limits and a point to point constraint (for loops) are basically all handled 
the same:

1) compute the effect of a unit force (or impulse) (effectively measuring the 
Jacobian)
2) solve the constraint externally using any method, such as LCP etc
3) apply the forces or impulses to the links/bodies in the hierarchy

Original comment by erwin.coumans on 12 Oct 2012 at 12:54

[GoogleCodeExporter]

Hi guys.

Intro: I've contacted Erwin recently and have been preliminarily invited to 
help with the integration.

Martin: after you have built the constrained system equation of motion (either 
by Kokkevis' mtd or explicitly) you seem to be solving it using SOLE methods - 
does it mean you treat contact constraints as bilateral?

As far as loops closure constraints are considered - I agree that it will not 
require any heavy alterations as far as the methodology itself is considered.

-
Kuba

Original comment by jakub.b....@gmail.com on 7 Nov 2012 at 3:24

[GoogleCodeExporter]

Hi Kuba,

good stuff! Let me know if you run into things that are puzzling.

yes the contacts are treated as being bilateral. For unilateral an additional 
LCP problem has to be set up and solved.

Martin

Original comment by martin.f...@gmail.com on 8 Nov 2012 at 11:01

[GoogleCodeExporter]

Martin - okay, thanks, then now I think I know your general approach in RBDL.

There is one circumstance that I found very favorable in the overall context - 
I will be also attending this year's MIG, so we'll have a perfect opportunity 
to talk this thing out. See you in Rennes :).

-
Kuba

Original comment by jakub.b....@gmail.com on 8 Nov 2012 at 4:54

[GoogleCodeExporter]

Haha, that's cool! Looking forward to meet you there!

Original comment by martin.f...@gmail.com on 9 Nov 2012 at 11:50

[GoogleCodeExporter]

As planned, we've had a pretty thorough conversation on this with Martin. 
Unfortunately, I am quite busy these days (yeah, who isn't), but have managed 
to start digging into Bullet itself in order to try to find the best 
possible/most natural ways of introducing Featherstone's formalism into it. 
Once this is done, I plan to verify if RBDL fits this (which it probably won't) 
and then we can start discussing the details. This way we won't be biased with 
RBDL internal organization but will still be able to extract its key elements 
and possibly extend them as needed. Doing it the other way round would probably 
mean hacking RBDL into Bullet since Martin doesn't want to modify his lib 
(which is perfectly understandable).

Erwin - is there any date-deadline for this thing?

-
Kuba

Original comment by jakub.b....@gmail.com on 6 Dec 2012 at 12:20

[GoogleCodeExporter]

Sounds like a good plan!

Just wanted to clarify the comments concerning the modification of RBDL. I do 
not plan to change (and maintain) the RBDL substantially to make it compatible 
with Bullet. 

However I am happy to help creating a heavily modified version that can be 
integrated more easily into Bullet.

Cheers,
Martin

Original comment by martin.f...@gmail.com on 6 Dec 2012 at 8:47

[GoogleCodeExporter]

Before anything else, please create a sample using RBDS that updates the world 
transform (add some integrator) and add simple collision detection. There 
should be no need to use Bullet for this.

For example, add a hierarchy of 2 connected spheres, falling on an infinite 
plane under gravity. The collision detection for that should be trivial.

Once such sample is there, it makes it much easier to integrate RBDS with 
Bullet. Right now, there is too much guessing how to add collision detection 
and integration to RBDS, the API is not well documented. Even trivial things 
such as where is the world transform (position, quaternion) for bodies, and 
gettingn the linear/angular world velocity requires digging into the RBDS 
source code.

Thanks!
Erwin

Original comment by erwin.coumans on 7 Dec 2012 at 6:27

[GoogleCodeExporter]

For starters - I cannot say I have done anything worth reporting (shame), I 
guess I overestimated my spare time (PhD deadline is pressing). I am still very 
much interested in accomplishing this, but at the same time afraid that my 
contribution will need to be postponed. On the other hand, I am considering 
launching a related MSc project that could propel the work, but I am not sure I 
have an enough-physics-interested student at hand ;).

Nevertheless, I have recently skimmed Martin's code and, although it seems to 
be generally clear what's happening there, I have several questions about 
certain details that made me doubt my full comprehension:
[1] model.S[joint_id] and model.mJoints[joint_id].mJointAxes[0]: aren't these 
the same thing? (since the number of bodies = number of joints and all joints 
are 1DoF)
[2] in jcalc -> v_J = model.S.at(joint_id): I find the line unclear due to what 
it says and even more so as it is overwritten soon after (with what one could 
expect, i.e. Sq')
[3] fixed joints? are these joints used to create compound/composite rigid 
bodies? is it a working feature? isn't it wasteful (summing their inertias 
would be enough)
[4] null bodies are treated as any other kind of bodies - isn't it wasteful?
[5] why all the mulidof joint ctors? they don't really seem to fit into the 
current state of the lib - are they there for some prospective development?

One general note is that RBDL seems pretty austere (it was Martin's deliberate 
decision to build it like that) which has its pros and cons, the main con being 
that building a fully-featured simulator basing on such a bare-bone design can 
turned out be a challenge. Or maybe it is the perfect opposite, what do you 
guys think?

-
Kuba

Original comment by jakub.b....@gmail.com on 1 Feb 2013 at 8:58

[GoogleCodeExporter]

Hi Kuba,

I can relate to what you wrote about 'spare time' very well!

Concerning your questions:
[1] Yes, model.S[joint_id] and model.mJoints[joint_id].mJointAxes[0] are the 
same thing. Initially I planned to implement multi-dof joint models using 
dynamic sizes for model.S[], model.U[], and model.d[], but the implementation 
was measurably slower than emulating multi-dof joints using multiple 1-dof 
joints.
[2] Yes, this line can be removed. Thanks btw. I just committed the change.
[3] The fixed joints is a working feature since June 2012 and it does sum up 
the inertia of the bodies, so there is no waste here. See 
http://rbdl.bitbucket.org/d1/d76/group__model__group.html#joint_models_fixed
[4] Can you elaborate on this? I cannot find any special treatment of null 
bodies except for the emulation of multi-dof joints. And here they are inserted 
so that in the actual algorithm no special treatment is required.
[5] The multi-dof joint constructors are working and depending on how many 
degrees of freedom you have there is a different constructor. See 
http://rbdl.bitbucket.org/d1/d76/group__model__group.html#joint_models

My aim was to create a library that has little external dependencies and that 
focuses on one thing: the dynamics computation. But I also tried to make it 
easy to use so that could be used by fully-featured simulators. Maybe I lost 
sight of the integration perspective. Nevertheless I would be interested what 
could be improved.

Feel free to contact me directly if you have more questions or suggestions 
concerning RBDL. If you want to have a public discussion, you can also post 
questions to 
http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=6&t=8011.

Cheers,
Martin

Original comment by martin.f...@gmail.com on 1 Feb 2013 at 10:13

[GoogleCodeExporter]

Okay, I will use email not litter this thread with such details.

Original comment by jakub.b....@gmail.com on 1 Feb 2013 at 11:15

[GoogleCodeExporter]

I created a small real-time demo application with some spheres and hierarchies 
colliding with the ground. The source can be found at 
https://bitbucket.org/MartinFelis/rbdl-demo.

A video can be found here: http://www.youtube.com/watch?v=D3ya5zdkoK4

Original comment by martin.f...@gmail.com on 22 Feb 2013 at 6:26

[GoogleCodeExporter]

The video is private, can you adjust the privacy settings?

Thanks for sharing, I look forward to try it out!

Original comment by erwin.coumans on 22 Feb 2013 at 6:37

[GoogleCodeExporter]

[deleted comment]

[GoogleCodeExporter]

I added a new Featherstone multi body implementation to Bullet.
https://code.google.com/p/bullet/source/detail?r=2675
Stephen Thompson wrote the original implementation, his work is all in a one 
single concise file, called btMultiBody.cpp. Thanks Stephen for this great 
contribution!
The original version used Eigen math library, so I replaced that with Bullet 
LinearMath, and implemented a dedicated 6x6 linear solver.

The Bullet Featherstone implementation has proper support for floating base, 
integration with the Bullet constraint solver for two-way interaction with 
btRigidBody and de-activation/sleeping.

See Bullet/Demos/FeatherstoneMultiBodyDemo for sample use.

Currently there are only collision/contact constraints for btMultiBody. Next on 
the list will be adding a btMultiBody joint limit constraint.
The implementation is preliminary and expect the quality to improve during the 
upcoming Bullet 2.x releases.

Original comment by erwin.coumans on 1 Oct 2013 at 10:58

[GoogleCodeExporter]

Then it is finally done. Good news :). I didn't know someone (Stephen Thompson) 
was working on it.

I've preliminarily scanned the implementation (I can see that Featherstone's 
portions are based on Mirtich's thesis) and will look into it in more detail 
today. Just in case you need assistance refining this, I'm done with the PhD 
(waiting for reviews) = I have more spare time and will be glad to finally 
really help with this issue, e.g. with joint limits, optimization or making the 
API more user-friendly. By the commits I can see you're actively working on it. 
Let me know.

--
A sidenote: in the course of the last months I've created a similar fusion of a 
reduced-coordinate dynamics algo with Bullet-based SI and the approach seems 
pretty much the same as the one Erwin has chosen for Bullet integration. The 
main difference is that I ended up using CRBA rather than ABA (tests proved 
CRBA less CPU-time hungry in scenarios I was interested in). Implementing both 
of these approaches under the common hood seems a reasonable perspective but I 
guess finalizing the current version is more important for now.

Original comment by jakub.b....@gmail.com on 2 Oct 2013 at 12:22

[GoogleCodeExporter]

The Featherstone ABA integration is working really well now and ready for 
Bullet 2.82 release. The 1-DOF joints can be extended with multi-DOF joints in 
a future Bullet release. The current btMultiBody class can also be used as an 
example of integrating other multi-body libraries, such as RBDL.

I just fixed an instability issue, when picking a btMultiBody: the 
self-collisions between links in the same body returns the 'wrong' collision 
normals that cannot be resolved (due to prismatic joint), so it would keep on 
adding energy. We need some special collision detection for the 
self-collisions, to avoid this, or use the btMultiBodyJointLimitConstraint.

Thanks for the help/discussion.

Original comment by erwin.coumans on 23 Oct 2013 at 3:58

• Changed state: Fixed