rewriting algorithms with visitors

[sbarthelemy]

Currently all the algorithms walk the tree on their own. We could factor this in a few visiting algorithms. Other algorithms (crba, etc.) would be implemented as visitors.

here are some avantages:

• no duplication
• easier extendability
• (some) abstraction from the data structure (that would help when refactoring metapod)
• support for "several root joints"

A first try is in the following branch https://github.com/sbarthelemy/metapod/tree/add_visitor could you review it?

I also propose that all algorithms take Robot as a template parameter instead of Robot::Tree. I see Robot::Tree as an implementation detail.

There are some open questions:

• the visiting algorithm itself has lot of duplication because it has to support overloads. Is there a better way?
• Should we support const ref overloads too?
• Better naming than visit()? Maybe better to wait until other visiting algorithms a written.
• boost::graph make a distinction between visiting a vertex and an edge. I think it is overkill for trees. Agreed?

[aelkhour]

A first try is in the following branch https://github.com/sbarthelemy/metapod/tree/add_visitor could you review it?

Great! I think it is a good idea too. Do you think there might be tree traversal methods that cannot be done with visitors?

I also propose that all algorithms take Robot as a template parameter instead of Robot::Tree. I see Robot::Tree as an implementation detail.

Agreed.

• the visiting algorithm itself has lot of duplication because it has to support overloads. Is there a better way?
• Should we support const ref overloads too?

Just to check that I understood, do we need overloads because some algorithms might for instance take q as arguments and others q, dq and ddq?

• Better naming than visit()? Maybe better to wait until other visiting algorithms a written.

Yes we can always chose the name later.

• boost::graph make a distinction between visiting a vertex and an edge. I think it is overkill for trees. Agreed?

Agreed.

[sbarthelemy]

Do you think there might be tree traversal methods that cannot be done with visitors?

No, I don't think. But if a single visitor uses the traversal method, I'm not sure it is worth the trouble.

I also need to think a little bit more to the possibility to do some traversals at runtime too.

the visiting algorithm itself has lot of duplication because it has to support overloads. Is there a better way? Should we support const ref overloads too?

Just to check that I understood, do we need overloads because some algorithms might for instance take q as arguments and others q, dq and ddq?

yes. In my "fusion" branch I might end up storing this vector in Robot, which would kind of solve it. But I'm not sure it would be a good idea.

And there are other cases, such as printConf(stream, q);

Better naming than visit()? Maybe better to wait until other visiting algorithms a written. Yes we can always chose the name later. boost::graph make a distinction between visiting a vertex and an edge. I think it is overkill for trees. Agreed? Agreed.

Well I'll try it. But it comes to my mind that we might want to distinguish between visiting the tree while walking it up vs. down.

For instance it would help when implementing

i_X_j = getTransform(i, j);

So maybe: crossUp, crossDown and visit would be a better API (I made up the names, there might be better ones in boost::graph).

[sbarthelemy]

And by the way, thank you for the review and the enthusiasm!

[olivier-stasse]

Hi Sebastien, I integrated your visitor implementation in topic/operators.

[sbarthelemy]

Hello,

I force-pushed a new version to

https://github.com/sbarthelemy/metapod/tree/add_visitor

here are the changes:

• depth_first_traversal now has two hooks: discover() and finish() instead of visit() the names are inspired from discover_vertex and finish_vertex in boost::graph
• I added a backward_traversal algorithm which has the two same hooks
• I implemented crba and rnea with visitors.

This branch is based on the same commit as the previous one. Can you please review it?

I'll now review your work, then try to rebase this branch on top of yours.

[sbarthelemy]

Hello, I pushed a new version to

https://github.com/sbarthelemy/metapod/tree/add_visitor3

The branch starts with the same misc. cleanup commits as https://github.com/laas/metapod/pull/28 but based earlier in the history, because rebasing everything on master in its current state would be a lot of work and would further complicate the merge of Olivier's vanilla-rc branch.

Here are the changes:

• I simplified a bit the rnea implementation
• I refactored the jacpoint* and implemented them using backward visitors
• To this purpose, the backward traversal gained an EndNode parameter
• I also kind of duplicated the backward_traversal algorithm, which comes in two flavours : one with a single "Node" parameter (nice for jac_point) and the other with two "Node" and "PrevNode" parameters (nice for crba).

We could do both with two parameters flavour if we let it visit the NP node. Crba won't compile in this case, except if we specialize the visitor for the NP case. Maybe it is a better solution?

[olivier-stasse]

Hi Sebastien, I pulled your branch on top of v1.0.5 in topic/add_visitor3. I'll try to get back to you as soon as possible concerning your proposal.

[sbarthelemy]

Hello Olivier

I pulled your branch on top of v1.0.5 in topic/add_visitor3.

if you prefer, now that the FixedJoint is out of the way, I can rebase the branch on top of v1.0.5.

[sbarthelemy]

Hello,

I pushed branch https://github.com/sbarthelemy/metapod/tree/add_visitor4.

While add_visitor3 was based on v1.0.4, this version is based on v1.0.5. As such, it include the performance improvements brought by the S matrix modifications. There is no other change.

The performance are similar to the v1.0.5, except for rnea, which is 20% slower on the simple_humanoid model. Note that on other models the performance is unchanged.

Note also that the performance gets back to the v1.0.5 level if one comments out the others algorithms from the benchmark program.

I tried commenting them on the v1.0.5 version, it has no impact.

This is definitely weird.

Maybe the number of siblings as an impact? I think the other models are binary trees. Yet, it would not explain why the performance changes depending on the tested algorithms.

Maybe the compiler inlines differently depending on the program size?

Or has the symbols table size an impact?

I threw callgrind at it but found nothing relevant. I just noticed that some Eigen mangled functions are reported as belonging to the lib in the visitor version and from the executable in the v1.0.5 version.

Thus I tried to build the models as static libraries, but it fails because of multiple definitions of metapod::Spatial::OperatorMul.

I'm kind of stuck.

make[1]: *** [tests/use_simple_humanoid/CMakeFiles/test_bcalc_simple_humanoid.dir/all] Error 2
Linking CXX executable test_crba_simple_humanoid
cd /home/sbarthelemy/ar/m/lib/metapod/build-release/tests/use_simple_humanoid && /usr/bin/cmake -E cmake_link_script CMakeFiles/test_crba_simple_humanoid.dir/link.txt --verbose=1
/usr/bin/c++     -pedantic -Wno-long-long -Wall -Wextra -Wcast-align -Wcast-qual -Wformat -Wwrite-strings -Wconversion -O3 -DNDEBUG    CMakeFiles/test_crba_simple_humanoid.dir/test_crba.cc.o  -o test_crba_simple_humanoid -rdynamic -lboost_filesystem-mt -lboost_system-mt -lboost_thread-mt -lpthread -lboost_program_options-mt -lboost_unit_test_framework-mt ../libsimple_humanoid.a ../libsimple_arm.a 
../libsimple_humanoid.a(simple_humanoid.cc.o): In function `metapod::Spatial::OperatorMul<metapod::Spatial::Inertia, metapod::Spatial::Inertia, double>::mul(metapod::Spatial::Inertia const&, double const&) const':
simple_humanoid.cc:(.text+0x1210): multiple definition of `metapod::Spatial::OperatorMul<metapod::Spatial::Inertia, metapod::Spatial::Inertia, double>::mul(metapod::Spatial::Inertia const&, double const&) const'
CMakeFiles/test_crba_simple_humanoid.dir/test_crba.cc.o:test_crba.cc:(.text+0x540): first defined here
../libsimple_humanoid.a(simple_humanoid.cc.o): In function `metapod::Spatial::operator*(metapod::Spatial::Inertia const&, double const&)':
simple_humanoid.cc:(.text+0x12e0): multiple definition of `metapod::Spatial::operator*(metapod::Spatial::Inertia const&, double const&)'
CMakeFiles/test_crba_simple_humanoid.dir/test_crba.cc.o:test_crba.cc:(.text+0x610): first defined here
../libsimple_humanoid.a(simple_humanoid.cc.o): In function `metapod::Spatial::OperatorMul<metapod::Spatial::Force, metapod::Spatial::Inertia, metapod::Spatial::Motion>::mul(metapod::Spatial::Inertia const&, metapod::Spatial::Motion const&) const':

[olivier-stasse]

Fix by merging the commits proposed by Sebastien (at HEAD commit 4daf5)