metapod requires a single root link

[sbarthelemy]

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?

[aelkhour]

Hello Sébastien,

I think that adding support for fixed joints is a lightweight and elegant solution to this problem. It is a formalism I've seen and/or used in other mainstream libraries that handle generic kinematic trees [1,2,3,4].

[1] http://ros.org/doc/fuerte/api/urdf_interface/html/classurdf_1_1Joint.html [2] http://moveit.ros.org/classJointModel.html [3] https://github.com/laas/hpp-model/tree/master/src [4] http://rbdl.bitbucket.org/dd/d16/namespace_rigid_body_dynamics.html#aaf742f484e2505a1a045b456a58dbb3a

Best, Antonio

[sbarthelemy]

Hello Antonio,

I thought like you and gave it a try. Alas, Eigen seems to forbid taking sub-matrices (segment) of size zero. So adding a 0-dof joint is more complicated than I first hoped.

You can have a look at my attempt here https://github.com/sbarthelemy/metapod/tree/add_fixed_joint

It builds in debug, but fails at runtime with

Assertion `startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= xpr.rows() && startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= xpr.cols()' failed.

Too bad. Any workaround welcome!

[aelkhour]

The only workaround I see in order to add fixed joint support is to hide the configuration sub-blocks qi, dqi, ddqi from algorithms such as rnea and crba.

in jcalc.hh they appear in:

Node::Joint::jcalc(
        q.template segment< Node::Joint::NBDOF >(Node::Joint::positionInConf),
        dq.template segment< Node::Joint::NBDOF >(Node::Joint::positionInConf)
      );

In rnea.hh they appear in:

  // Extract subvector corresponding to current Node
  Eigen::Matrix< FloatType, Node::Joint::NBDOF, 1 > ddqi =
    ddq.template segment<Node::Joint::NBDOF>(Node::Joint::positionInConf);

and

 Node::Body::ai = sum(Node::Joint::sXp*Node::Body::Parent::ai,
                       Spatial::Motion(Node::Joint::S * ddqi),
                       Node::Joint::cj,
                       (Node::Body::vi^Node::Joint::vj));

You could,for instance, define for each joint a method which:

• takes the full q, dq, ddq,
• extracts sub-blocks qi, dqi, ddqi if necessary,

The function could then also (although not sure about this part)

• call jcalc if necessary,
• compute additional quantities such as S * ddqi if necessary and store them.

Antonio

[olivier-stasse]

Hi Guys, Since commit d810 I introduced a specific class for constraint motion which explicitly handle the S matrix. This should allow us to start the fixed joint implementation and probable address some of the following issues concerning the joint classes. Could you review the current head master ? Sorry for the mess up betweeen commit 424fc and d810. I tested properly d810.

[aelkhour]

Branch topic/operators up to commit 7bff6782f189a23bdec95459522fde87121e0a02 now offers (despite its name, sorry) support for fixed joints for all implemented algorithms.

As explained in the commits, the trick is to consider fixed joints as having 0 dofs, and testing this value in the algorithms. The tests are evaluated at compile time, so I did not notice any performance loss.

I also defined macros for fixed joint type.

I think we need models that contain fixed joints so that we can run tests on them. We must be able to verify for those models that all algorithms are correct, and that no performance loss is induced.

We could have:

• A model similar to simple_arm but with an additional fixed joint at the root.
• A model similar to simple_humanoid with additional fixed joints inside the tree.

This means that an additional overhead will be induced by compiling two more models. We could hence, once algorithms are validated, replace the old models by the new more complete ones.

Do you agree?

[aelkhour]

Thank you Olivier for pointing out the shortcomings of my fixed joint proposal for all algorithms and fixing them in commit 96e7eae768dc811e131ef634a2040f84e61e4db5.

I propose in the branch topic/fixed-joint-test in commit c54145ba45e7d774b2e3417a1c18ae5d1ed6feb1 an alternative way which is in my opinion a bit more elegant:

• Example shown for crba.
• Add Joint::NBDOF as template argument.
• This allows to "catch" at compile-time fixed joints and avoids defining fictitious attributes and methods for fixed joint class which do not hold any physical or mathematical meaning, such as a 6x0 motion subspace matrix.

Comments are welcome.

Sébastien, does adding fixed joint support affect (for the better or worse) your current work? I would like your input on this as you were the one who originally opened this issue.

[olivier-stasse]

Hi Antonio, I put your commit in the crba of topic/operators, because I found your solution very elegant. However in commit c54145b, I had a problem with NP::Iic which was corrected using the same approach in commit 5fea311. Just hope that there is no further wired case... RNEA has also to be modified before removing cm-fixed.hh Could you also base your next modifications on topic/operators ? I removed all the warnings and it makes the debugging easier. Thx.

[aelkhour]

Again thanks Olivier for the fix.

Sébastien, with respect to issue #27, do you think there is a way of using visitors to handle if_parent, and if_fixed_joint cases in a generic way? I haven't given much thought to this, but I agree with Olivier that it's becoming cumbersome to add these recurring cases (maybe there will be others) for all algorithms.

Ok for pursuing on topic/opertors. Unless urgently needed, I will not be working on metapod for the next two days.

[olivier-stasse]

With respect to your implementation of crba it is probably possible to use the visitor pattern inside the crba_forward_propagation template.

[sbarthelemy]

Hello,

I had a quick look at the branches.

• I like very much the way the S matrix is handled.
• maybe there is confusion regarding FixedJoint: there are two distinct issues here. First metapod only handles one root joint. This is a severe limitation. Second metapod does not support 0-dof joints. Such joint might be useful sometimes, but I see this feature as much lower priority. Such FixedJoints should be supported anywhere on the tree to be useful, not just at the root.
• regarding the "purity" of the if_fixed_joint/if (nbdof>0) tests, I think there are two problems here: adding support for fixed joint, and working around the lack of support for zero-sized blocks from Eigen. I'm ok with the "if (nbdof>0)" tests for the latter reason, not much for the former: support for a joint type should not leak into the algorithms. Currently the joint classes do to much work: ideally, they should not deal with sXp for instance.

What do you think?

[aelkhour]

Such FixedJoints should be supported anywhere on the tree to be useful, not just at the root.

This is what we had in mind too, and the current algorithm implementation on branch topic/operators is such that fixed joints are supported anywhere on the tree, even if for now the only test we have is for the fixed root joint of simple_arm.

[sbarthelemy]

sorry, I was updating my comment while you responded.

[aelkhour]

regarding the "purity" of the if_fixed_joint/if (nbdof>0) tests, I think there are two problems here: adding support for fixed joint, and working around the lack of support for zero-sized blocks from Eigen. I'm ok with the "if (nbdof>0)" tests for the latter reason, not much for the former: support for a joint type should not leak into the algorithms.

Even though I've done differently, I must admit I agree with you. I will try to give further thought about this.

Currently the joint classes do to much work: ideally, they should not deal with sXp for instance.

I agree, sXp is actually computed outside jcalc in Fearherstone's rnea.

[sbarthelemy]

regarding if_parent, I think we can avoid storing it in the Node, with some helper struct like this

template <typename Node > struct is_no_parent
{
   bool value = false;
}
template <> struct is_no_parent<NP>
{
   bool value = true;
}

template <typename Node > struct has_parent
{
   bool value = !is_no_parent<typename Node::Body::Parent>::value;
}

we could then use it

if (has_parent<Node>::value)
{
  ...
}

[olivier-stasse]

regarding the "purity" of the if_fixed_joint/if (nbdof>0) tests, I think there are two problems here: adding support for >fixed joint, and working around the lack of support for zero-sized blocks from Eigen. I'm ok with the "if (nbdof>0)" >tests for the latter reason, not much for the former: support for a joint type should not leak into the algorithms.

Even though I've done differently, I must admit I agree with you. I will try to give further thought about this.

Currently the joint classes do to much work: ideally, they should not deal with sXp for instance.

I agree, sXp is actually computed outside jcalc in Fearherstone's rnea.

We probably should address this issue in the separation of the quantities depending on the state and the ones related to the model. Maybe the introduction of the visiting design pattern is a good occasion to do so.

[olivier-stasse]

regarding if_parent, I think we can avoid storing it in the Node, with some helper struct like this

template struct is_no_parent { bool value = false; } template <> struct is_no_parent { bool value = true; }

template struct has_parent { bool value = !is_no_parent::value; }

we could then use it

if (has_parent::value) { ... }

I am not sure I understand this comment, the parent relationship is already embedded inside the tree structure. I like the way it is handle through the NC (no child) NP (no parent) classes. It allows specialization for the algorithms. If you are talking about the RNEA formulation, I agree with you that the has_parent parameter is useless, we could directly pass the Parent type as it is done in CRBA.

[olivier-stasse]

regarding the "purity" of the if_fixed_joint/if (nbdof>0) tests, I think there are two problems here: adding support for >fixed joint, and working around the lack of support for zero-sized blocks from Eigen. I'm ok with the "if (nbdof>0)" >tests for the latter reason, not much for the former: support for a joint type should not leak into the algorithms.

Even though I've done differently, I must admit I agree with you. I will try to give further thought about this.

Currently the joint classes do to much work: ideally, they should not deal with sXp for instance.

I agree, sXp is actually computed outside jcalc in Fearherstone's rnea.

Agree with both of the comments, we could try to fix the matter with the visitor pattern implementation.

Just to make things straight, can I push in the master, and close the issue because fixed joints are now supported ? For the sake of completness I would suggest to do it after modifying the humanoid sample model with the galilean frame and a fixed joint along of the chain. Looking for comments.

[sbarthelemy]

regarding "struct has_parent":

the parent relationship is currently embedded both through the NP class and in every Boby class through the HAS_PARENT enum. The trick I propose would allow us to remove the latter and deduce this information from the former.

Regarding closing this issue: the issue is not about Fixed Joints but about multiple root joint support. This support can be achieved with a FixedJoint but this is not the only way. I'd prefer to deal with it without adding a joint. This will be easy once the visitors are merged.

Regarding a push to master. I prefer you don't push in the current state.

• First, the tests do not pass.
• Second the history is quite messy.
  • There are useless visitor commits integrated.
  • I have some detail comments about the code that I have not done by lack of time, and being lost in the history.
• Third I'm not very happy with the way FixedJoint are handled, but I had very quick look, I probably got it wrong.
• Eventually, I think the single root joint problem would be better solved after the visitors merge.

What about cleaning the history, and making two feature branches out of topic/operator: one about the S matrix the other about fixed joints. If you which I can do it (severe rebase -i use needed).

Regarding my own planning: I'd like to get the visitors finished and merged on master by tomorrow, to base the fusion branch on it. It would be great if we can merge the S matrix and maybe the Fixed Joint too.

[olivier-stasse]

Hi guys, Do we agree that we should:

• NOT include the visitor design pattern (also it does not change anything)
• include the FixedJoint (despite some reserve about it) with the current implementation of rnea and crba inside topic/operators.

If you say yes I push it to the master and release. Of course after updating the tests. And yes I will clean up the history by cherry-picking.

[sbarthelemy]

If you say yes I push it to the master and release. Of course after updating the tests. And yes I will clean up the history by cherry-picking.

Olivier, what about cleaning up history and cherry-picking in a new branch (say topic/operator2) and letting us review it?

That is the way we work here: we rebase -i our feature branch so that history is clean (each commit is a step, it does not go forward then backward etc.), then we push it for review (reviewers do not need to see our trials and errors). Then we do it again to take their comments into account. This workflow is encouraged (and helped) by our tools (http://code.google.com/p/gerrit/).

That being said, this is your repository, you're the upstream, I'll adapt.

[olivier-stasse]

That probably the best way, what about to do a release-candidate branch ? I will call it topic/v1.0.5-rc

I will try gerrit.

[sbarthelemy]

That probably the best way, what about to do a release-candidate branch ?

I prefer feature branches, with one feature per branch. it helps working on them and reviewing them independently.

By the way I never heard about rc branches, but more about release/stable branches, with rc version tags.

[olivier-stasse]

Release candidate is usual in linux kernel

[olivier-stasse]

Feature is when you develop one specific features. At some point you have to merge different features and release-candidate is a good choice. What is missing currently, is to have milestones and specifying what kind of features we want to integrate in one release. For the next release, I would like to have a working fixed point implementation. For v1.0.6, I am thinking about the following features:

• Visitor pattern
• Separation between model quantities and state related quantities
• Implementing other Joint models (the ones in the Rigid Body Mechanics)

Your multiple roots feature looks to me as a way to extract tree from a graph representation of the robot. Which is very interesting for closed loop computation. This could be a good candidate for release v1.0.7

[sbarthelemy]

Release candidate is usual in linux kernel

Do you have any link which describes a release candidate branch is?

Feature is when you develop one specific features.

this is what we do. We develop constraint motion operators for faster algorithms and FixedJoints. Alas these developments are mixed up in a single branch, with messy history, and commit cherry-picked from yet a third feature (visitors).

At some point you have to merge different features

agreed. I think that point is not reached yet.

and release-candidate is a good choice.

to my understanding, a release-candidate is a version (a git tag), not a branch.

Your multiple roots feature looks to me as a way to extract tree from a graph representation of the robot.

I was not clear, sorry. By multiple roots, I means multiple "root joints", or multiple robots in a single model. This is what this issue (issue #14) is all about.

[olivier-stasse]

Release candidate is usual in linux kernel Do you have any link which describes a release candidate branch is?

This is called usually the "vanilla" branch here: http://en.wikipedia.org/wiki/Linux_kernel But I am not a big fan of the name

Feature is when you develop one specific features.

this is what we do. We develop constraint motion operators for faster algorithms and FixedJoints. Alas these >developments are mixed up in a single branch, with messy history, and commit cherry-picked from yet a third >feature (visitors).

Sure and I totally take up on me the responsability, because I wanted to check that there is no problem of compatibility with the other development. I probably should have create yet another branch. But I don't like the inflation of branches. My experiences is that people rarely clean up their branch (see for instance topic/fixed-joint-test, ...) and that it better once a test is done to integrate or leave it from the "vanilla" branch.

Here specifically, I propose to cherry pick the commit and put them on the master or on a vanilla branch once it is reviewed and agreed upon,

At some point you have to merge different features agreed. I think that point is not reached yet.

Technically could you specify what is wrong with the current fixed joint implementation ?

and release-candidate is a good choice. to my understanding, a release-candidate is a version (a git tag), not a branch.

If you prefer call the branch "vanilla" no problem...

Your multiple roots feature looks to me as a way to extract tree from a graph representation of the robot. I was not clear, sorry. By multiple roots, I means multiple "root joints", or multiple robots in a single model. This is what this issue (issue #14) is all about.

Sorry I don't get why you are not handling several models, real robots usually have various sample rate, and therefore it is better to separate them. But I still believe that walking through a robot model from various root points is a good idea.

[olivier-stasse]

Could you review branch topic/rc-vanilla at commit d33cf18? It is based upon the master at a777665. Unless you have further bug fixing to make, I will release it as the v1.0.5

[sbarthelemy]

I added comments to several commits in this branch.

The bottom line is that I'd prefer no FixedJoint support rather than this support leaking in the algorithms as it is currently done in the various variants. I also fear that other joints with 0 dof might come up (for coupled joints, sharing a single dof for instance).

On the other hand, I'm very happy with the the improvements regarding the S matrix. If you wish I'm willing to take the time to extract this single feature from your branch and propose it to you for a merge.

[olivier-stasse]

Ok with me.

[olivier-stasse]

BTW Thank you very much for the thorough review.

[sbarthelemy]

FIxed in laas/master