see the test added in the "clean_tests" branch to reproduce the problem.

Olivier, if you don't mind, I prefer you deal with this one.

Out of box:

pbeeson@tardis:$ git clone https://github.com/laas/metapod.git
Cloning into 'metapod'...
remote: Counting objects: 4627, done.
remote: Compressing objects: 100% (1463/1463), done.
remote: Total 4627 (delta 2713), reused 4550 (delta 2672)
Receiving objects: 100% (4627/4627), 10.91 MiB | 564 KiB/s, done.
Resolving deltas: 100% (2713/2713), done.
pbeeson@tardis:$ cd metapod/
pbeeson@tardis:/metapod$ mkdir _build
pbeeson@tardis:/metapod$ cd _build/
pbeeson@tardis:~/metapod/_build$ cmake ..
-- The C compiler identification is GNU
-- The CXX compiler identification is GNU
-- Check for working C compiler: /usr/bin/gcc
-- Check for working C compiler: /usr/bin/gcc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
CMake Error at CMakeLists.txt:24 (INCLUDE):
include could not find load file:

cmake/base.cmake

CMake Error at CMakeLists.txt:25 (INCLUDE):
include could not find load file:

cmake/cpack.cmake

CMake Error at CMakeLists.txt:26 (INCLUDE):
include could not find load file:

cmake/boost.cmake

CMake Error at CMakeLists.txt:27 (INCLUDE):
include could not find load file:

cmake/ros.cmake

CMake Error at CMakeLists.txt:82 (SETUP_PROJECT):
Unknown CMake command "SETUP_PROJECT".

-- Configuring incomplete, errors occurred!

we should avoid duplication and use consistent names.

See also the comment on this commit
e47cbb4

I cannot find the following header file:
#include <metapod/models/simple_humanoid/simple_humanoid.hh>

Please let me know how can I get it?

Can I drop fuerte support and only support Groovy?

I was wondering if there has been attempt to add ZMP computation in metapod or if these is any plan to add ZMP computation support in the future?

Thanks!

Hello, I'm trying to install metapod on macosx. I've succeeded in installing all dependencies with homebrew, now I'm compiling and the building fails but I have no idea why. Cmake gives no errors, here's the output of the make (the part giving errors):

ternal::backward_traversal_prev_internal<Visitor, Robot,...
^
/Users/sere/software/src/metapod/tests/use_model/test_backward_traversal_prev.cc:67:3: note:
in instantiation of function template specialization
'metapod::backward_traversal_prev<PrintBwdTraversalVisitor,
metapod::simple_humanoid, 10, -1>::runstd::basic_ofstream<char, int>'
requested here
backward_traversal_prev<PrintBwdTraversalVisitor, CURRENT_MODEL_ROBOT,
^
24 warnings generated.
Linking CXX executable test_backward_traversal_prev_simple_humanoid
cd /Users/sere/software/src/metapod/build/tests/use_simple_humanoid && /usr/local/Cellar/cmake/2.8.11.1/bin/cmake -E cmake_link_script CMakeFiles/test_backward_traversal_prev_simple_humanoid.dir/link.txt --verbose=1
/usr/bin/c++ -pedantic -Wno-long-long -Wall -Wextra -Wcast-align -Wcast-qual -Wformat -Wwrite-strings -Wconversion -Wl,-search_paths_first -Wl,-headerpad_max_install_names CMakeFiles/test_backward_traversal_prev_simple_humanoid.dir/test_backward_traversal_prev.cc.o -o test_backward_traversal_prev_simple_humanoid /usr/local/lib/libboost_filesystem-mt.dylib /usr/local/lib/libboost_system-mt.dylib /usr/local/lib/libboost_unit_test_framework-mt.dylib /usr/local/lib/libboost_program_options-mt.dylib /usr/local/lib/libboost_regex-mt.dylib /usr/local/lib/libboost_timer-mt.dylib /usr/local/lib/libboost_chrono-mt.dylib /usr/local/lib/libboost_system-mt.dylib ../../libmetapod_simple_humanoid.dylib /usr/local/lib/libboost_unit_test_framework-mt.dylib /usr/local/lib/libboost_program_options-mt.dylib /usr/local/lib/libboost_regex-mt.dylib /usr/local/lib/libboost_timer-mt.dylib /usr/local/lib/libboost_chrono-mt.dylib
Undefined symbols for architecture x86_64:
"_main", referenced from:
implicit entry/start for main executable
ld: symbol(s) not found for architecture x86_64
clang: error: linker command failed with exit code 1 (use -v to see invocation)
make[2]: *** [tests/use_simple_humanoid/test_backward_traversal_prev_simple_humanoid] Error 1
make[1]: *** [tests/use_simple_humanoid/CMakeFiles/test_backward_traversal_prev_simple_humanoid.dir/all] Error 2
make: *** [all] Error 2

Dear all,
I tried to use metapodfromurdf with the HRP-4 model and so far it works quite well.

One slight addition that would have helped me is the possibility to add the root free-floating joint automatically.

I see two main reasons to this:

1. I prefer to not have the free-floating joint in my URDF models. Mainly because I would like to have a CAO-like model, close to the mechanical design without any computation artefacts such as the free-floating joint inside. Then the software loading/parsing the model can do the appropriate modification to obtain a workable computation model.
2. Free-floating joints are unfortunately deprecated in URDF. They probably will not go away anywhere soon but still it does not harm to be future-proof. Especially because in our use case, the free-floating joint has only sense as a root joint.

Sorry, I am a bit lacking time to directly open a pull request right now, but something like an additional optional argument called --add-root-floating-joint to metapodfromurdf would be IMHO a nice addition.

On Travis, metapodfromurdf leads to a segmentation fault when using clang 3.4:

[ 18%] Generating include/metapod/models/simple_arm/config.hh, include/metapod/models/simple_arm/simple_arm.hh, include/metapod/models/simple_arm/simple_arm.cc

metapodfromurdf/metapodfromurdf --name simple_arm --libname metapod_simple_arm --directory /home/travis/build/laas/metapod/_travis/build/include/metapod/models/simple_arm --config-file /home/travis/build/laas/metapod/data/simple_arm.config --license-file /home/travis/build/laas/metapod/data/metapod_license_file.txt /home/travis/build/laas/metapod/data/simple_arm.urdf

make[2]: *** [include/metapod/models/simple_arm/config.hh] Segmentation fault

Also, locally, when using clang 3.5.0, I get a bunch of elaborated type refers to a typedef errors:

In file included from .../metapod/build_clang/include/metapod/models/simple_arm/simple_arm.cc:31:
.../metapod/build_clang/include/metapod/models/simple_arm/simple_arm.hh:58:3: error: elaborated type refers to a typedef
  METAPOD_TYPEDEFS;
  ^
.../metapod/include/metapod/tools/joint.hh:32:3: note: expanded from macro 'METAPOD_TYPEDEFS'
  EIGEN_METAPOD_TYPEDEFS; \
  ^
.../metapod/include/metapod/tools/fwd.hh:62:42: note: expanded from macro 'EIGEN_METAPOD_TYPEDEFS'
  typedef struct Vector1dTpl<FloatType>::Type Vector1d;      \
                                         ^
.../metapod/build_clang/include/metapod/models/simple_arm/simple_arm.cc:36:31: note: in instantiation of template class 'metapod::simple_arm<double>' requested here
template <> const std::string simple_arm<FloatType>::Node0::joint_name = std::string("SHOULDER");
                              ^
.../metapod/include/metapod/tools/fwd.hh:32:44: note: declared here
  { typedef Eigen::Matrix<FloatType, 1, 1> Type; };
                                           ^

in metapod you can define a simple (head-less) humanoid like this:

Galilean frame
 | floating joint
torso-----------------+-------------------+-------------------+
 | revolute joint     | revolute joint    | revolute joint    | revolute joint
left arm             right arm           left leg            right leg

The C++ code would be

typedef
Node< TorsoLink,
  Torso,
  Node< LeftArmLink,
        LeftArm
  >,
  Node< RightArmLink,
        RightArm
  >,
  Node< LeftLegLink,
        LeftLeg
  >,
  Node< LeftLegLink,
        LeftLeg
  >
> Tree;

Note that the Galilean frame does not appear in the C++: its existence is implicit. It follows from that design choice that the "Tree" cannot describe systems that miss a single root link. For instance, one cannot model the previous example with the Torso fixed to the ground (à la iCub). One cannot describes a compound system composed of several robots (each with its floating root). One cannot describe industrial robot fixed to the ground with two serial arms.

A solution would be to change so that "Tree" starts with the Galilean frame.
A possible workaround is to add support for fixed joint.

Thoughts?

In some cases, some fixed-size vectorizable Eigen matrices are not properly aligned and an eigen assert fails.

This showed up with on win32 with boost 1.50 and linux32 with boost 1.48, but not on linux32 with boost 1.50. (And not on linux46 not mac64 with boost 1.50).

This does not happen on any metapod test but only on some of our models here.

Maybe the fusion vector tries to pack things?

I'll try to brings a small test that shows up the problem.

As discussed, I tried to integrate the urdf parser in the metapod project.

I dis it in two branches, one for jrl-cmake, one for qibuild

the jrl-cmake one: https://github.com/sbarthelemy/metapod/tree/add_metapodfromurdf
it does not link, I do not know why. If you, jrl-cmakers, can have a quick look to check I'm using ros.cmake properly, I'd be glad.

The qibuild one:
https://github.com/sbarthelemy/metapod/tree/qadd_metapodfromurdf
this one works, and generates models exactly identical to the ones currently in master.

Still to do :

• fix it for jrl-cmake
• check it works with boost 1.40
• remove the sample models from the tree

While trying to test metapod on a dummy URDF model that I use for my unit tests, I was surprised by the disappearance of the root link. I know that several URDF models use a dummy link for the free-flyer (base_link for HRP-2, NP for metapod's examples etc.), e.g.:

  <link name="NP"/>

However, this does not seem to be part of the standard, and my test model has a "real" root link (with a mass, an inertia etc.). I have used it with ROS and I haven't had any problem so far. Is this "dummy root link" a known requirement of metapod? (possibly related to #65) If it is, it may be good to add this information somewhere in the README.md since some URDF models may not comply (e.g. non REP 120 models). Else, please enlighten me :-)

Note: I noticed a similar problem when checking rbdl's URDF loader.

If we compute the torques using RNEA, Tau=ID(q,dq,ddq), we get the expected reference torques (rnea.ref).
If we use the equation of motion, Tau=H*ddq+C, we get wrong torques. The issue is introduced by the use of optional parameter "prefer-fixed-axis=true" in simple_humanoid.config file . Refer to following comments for more details.

Eigen constructors do not initialize the matrix, even the default one. [1]

As a consequence, spatial types, which do not initialize their eigen data members, are constructed uninitialized too.

The computations on the master branch are still sound because all these objects are used in static data members and thus are zero initialized. [2]

In the fusion0 branch, the objects are no more static and we need a proper initialization.

I'd like to fix it, but we need to choose a policy:

• either the spatial types provide a static initilizer (say Spatial::Force::Zero) and the user has to manually call them
• or the default constructor does the job.

I'd tend to prefer the first option, for the sake of consistency with eigen. What do you think?

[1] http://eigen.tuxfamily.org/dox/TutorialMatrixClass.html#TutorialMatrixConstructors
[2] http://stackoverflow.com/a/58804

From recent comments and issues open previously on github, it appears clearly that a documentation is needed, at least at the user level. The recent merge with the fusion merge proposed a very different way of specifying a robot model. The integration of the appendix A of R. Featherstone is also calling for some explanation for possible extensions,
aka being able to handle joints with various representations (for instance Free flyer with rpy, matrix, or quaternion representations with various orders X-Y-Z, Z-Y-X Euler angles).

There is no real user-level documentation for this code, so I'm left examining the code in detail to figure it out. In doing this I either found an inconsistency, or I'm confused.

It seems that the position inputs for a floating base are x,y,z,roll,pitch,yaw (see joint-freeflyer.hh bcalc to see that the first triple are translation and second triple are rotations).

But, in the velocity inputs, examined by freeflyer jcalc (just under the call to bcalc), the velocity vector goes through Spatial::Motion, which defines the first triple as w (omega, usually rotational velocity) and the second as v (usually translational velocity). This seems to be the case with accelerations too.

Please confirm if I am correct.

Hi,

I think there is an inconsistency between the Transform.apply overloads. See the following excerpt

class Transform
{
  public:
    // Transformations
    Motion apply(const Motion & mv) const
    {
      return Motion(m_E * mv.w(),
                    m_E * (mv.v() - m_r.cross(mv.w())));
    }

    Motion applyInv(const Motion & mv) const
    {
      vector3d ET_w = m_E.transpose()*mv.w();
      return Motion(ET_w, m_E.transpose()*mv.v() + m_r.cross(ET_w));
    }
    vector3d apply(const vector3d & v) const
    {
      return vector3d(m_E.transpose()*v + m_r);
    }
};

I would prefer having

    vector3d apply(const vector3d & v) const
    {
      return vector3d(m_E*(v - m_r));
   }

   vector3d applyInv(const vector3d & v) const
    {
      return vector3d(m_E.transpose()*v + m_r);
    }

or just removing the vector3d overload, it does not seem to be used anywhere in the project

I was in the process of packaging metapod for Arch Linux, and the last release (1.0.7) fails because of some Boost errors. However, everything seems to work just fine when using master (tested with Boost 1.55, Eigen 3.2).

Is there a release planned for the near future?

Thanks!

I'd like to let the user change the floating point precision of algorithms
properly.

Currently this parameter is a typedef in metapod namespace. Changing it
requires editing the file.

On could add a preprocessor option to avoid editing the file. This might be
handy (especially for testing both precisions), but can led to problems at
link time:

• if one build with the USE_SINGLE_PRECISION flag and does not define the flag
  prior to including the headers.
• worst, it prevents us to use both precisions at the same time, because of
  symbols collisions.

I think a proper solution would be to move FloatType as a template parameter to
both Robot and Spatial::* classes.

I haven't thought about that much though. Any comment?

I would also jump on that occasion to rename linear algebra types in metapod.
I think we should closely follow the Eigen conventions, and keep the first letter capital.

So the current metapod::matrix3d would become metapod::Matrix3
(we could also define metapod::Matrix3d and metapod::Matrix3f).

Any comment?

There are a few things that annoy me in the current metapod structure:

1. model and storage mixup: we could separate the kinematic model (tree of
   joints), the state-independant parameters (masses and lengths) and the
   stuff which depends on the state.
2. joint macros: I see no point in duplicating macros in each joint. Each
   joint type could be a static class (once the separation mentioned above is
   effective).
3. tree structure:
   • the children can be accessed from the Node tree, yet the parent is accessed
     from Body::Parent.
   • The joint is accessed from both Node::Joint and Node::Body::Joint.

Proposed solution

1. Split data:

   • Put all the state dependant data in a storage array (members to be
     defined). As a bonus, I think this would make algorithms more similar to
     Featherstone book.
   • let the constant parameters be initialized at run time. (I do not expect
     this would impact performance. Am I overlooking something ?)

2. Put each joint type in a static class (say, FloatingJoint, RevoluteJoint...)

3. I like the Node tree, but have not succeeded yet to add a "parent" typedef
   to the tree.
   So I plan to drop it and use a flat set of classes which would merge the
   data from the current Node and Body classes.
   These classes would not be templated, but macro-generated.
   For instance, for a 6+3 dof robot, we would get:

   class NodeTorso {
   typedef NoParent parent;
   typedef FloatingJoint joint;
   typedef NodeLShoulder child0;
   typedef NodeHead child1;
   static const idx = 0;
   }
   class NodeLShoulder {
   typedef NodeTorso parent;
   typedef RevoluteJoint joint;
   typedef NodeLArm child0;
   typedef NoChild child1;
   static const idx = 1;
   }
   class NodeLArm {
   typedef NoLShoulder parent;
   typedef RevoluteJoint joint;
   typedef NoChild child0;
   typedef NoChild child1;
   static const idx = 2;
   }
   class NodeHead {
   typedef NodeTorso parent;
   typedef RevoluteJoint joint;
   typedef NoChild child0;
   typedef NoChild child1;
   static const idx = 3;
   }

   I'll also probably add a "NodeRoot" on top of that hierachy to fix issue #14.
   Alternatively, on could generate a single class templated by the index, but
   I do not see what we would gain or loose.

Comments ?

(it is more an enhancement than an issue.)

Currently, the floating point scalar type is given by the FloatType typedef.
It would be much better to define FloatType as a template argument to the robot class, and to all spatial algebra type.

I would also rename FloatType into Scalar, to follow the Eigen convention.

Metapod #defines EIGEN_DEFAULT_TO_ROW_MAJOR

It is a bad idea, see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422

Any idea why it was needed?

Was it for performance reasons? Or maybe to have compatibility with another library?

Dear all,
I have the following code which is sub-optimal:

  robot_t::confVector torques;
  for (unsigned frameId = 1; frameId < animatedMesh_->numFrames () - 1;
   ++frameId)
{
  metapod::rnea<robot_t, true>::run
    (robot, q[frameId], dq[frameId], ddq[frameId]);
  metapod::getTorques (robot, torques);

  result.segment
    (frameId * robot_t::NBDOF, robot_t::NBDOF) = torques;
}

...the following version would avoid a copy but is not possible right now:

  metapod::getTorques (robot, result.segment
    (frameId * robot_t::NBDOF, robot_t::NBDOF));

Would it be possible to make sure that algorithms and functions take a EigenBase<T>, MatrixBase<T> or any other adequate abstract type instead of concrete type so that metapod can avoid enforcing its own matrix type confVector to everyone.

A side effect would be to let the user choose its own scalar type, matrix implementation, etc. which could lead to many potential optimization on the end-user side.

Thanks!

it is more an enhancement than an issue.

bcalc is poorly chosen, and there are redundancies between bcalc, jcalc and rnea.

This was already discussed in comments to this commit
a1a2644#commitcomment-2144685

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?

Bonjour et bonne année!

I have been playing around with the benchmark and possibly ran into a bug. The benchmark runs two loops: the outer loop generates random robot configuration variables and the inner loop then calls the function that is being benchmarked. By default there are 100 outer and 1000 inner loops.

When changing to 1 outer and 100000 inner loops I get very different results for the individual function call durations even though the overall runtime is similar.

I ran 'time benchmark' on both variants. Here is the output of the last benchmark model and the time command:

Default benchmark (outer 100, inner 1000):

*************
Model NBDOF : 31
  average execution time :
jcalc: 1.82543µs
bcalc: 2.40218µs
rnea: 7.12526µs
rnea (without jcalc): 5.21813µs
crba: 7.08987µs
crba (without jcalc): 5.18196µs
jac (without jcalc): 4.82069µs
jac_point_robot (without bcalc): 7.57695µs
./benchmark/benchmark  11.62s user 0.00s system 99% cpu 11.635 total

Modified benchmark (outer 1, inner 100000)

*************
Model NBDOF : 31
  average execution time :
jcalc: 1.81856µs
bcalc: 4.20191µs
rnea: 11.1085µs
rnea (without jcalc): 15.9698µs
crba: 22.8785µs
crba (without jcalc): 27.8271µs
jac (without jcalc): 32.2924µs
jac_point_robot (without bcalc): 39.6024µs
./benchmark/benchmark  11.50s user 0.00s system 99% cpu 11.505 total

Any ideas?

Regards,
Martin

which are hard to meet requirement when caring about portability.

There is a scoped enum scoped and default template arguments:

metapod/include/metapod/tools/spatial/rm-chgaxis.hh:192:56: error: default template arguments may not be used in function templates without -std=c++0x or -std=gnu++0x

sorry, I missed these last ones during the review.