Jacobian of COM of a subtree

Created by: jmirabel

I was able to get the COM of a subtree but I am not able to get it's Jacobian. Did I miss something or do I have to implement it myself ?

Most of the time, the subtree I'm interested in is made of one or two subtrees so it would not be so costly to compute them separately.

Created by: jmirabel

I had a look at the code of computeJacobianCenterOfMass and it seems pretty easy to compute the jacobian of a subtree from the existing code. I see two options:

• add an argument in Pinocchio (the root index of the subtree, 0 by default),
• do my own algorithm in hpp-pinocchio.

Are the Pinocchio developpers ( @nmansard @jcarpent ) interested in the option 1 ?

I do not think that it is trivial. Just because the jacobian of the com subtree starting at j_id is influenced both by its parents and its children.

I'm adding the subtree information in model to support it. Do you agree?

Created by: jmirabel

Great, thanks !

About the math: I'm probably wrong but isn't it a weighed sum of the derivative of joint placements ? How can it be influenced by its parents ?

The velocity of the com of a certain joint is influences by the velocity of its supporting joints, like the jacobians.

But, in the case of the com subtree, it also influenced by the CoM of its children, by definition. I'm not sure if I'm clear. You may have look to the algo centerOfMass(model,data,q,v) which computes both the CoMs and their velocities with a forward and a backward steps.

Created by: jmirabel

Got it, thanks !

Created by: jmirabel

I came up with that: https://github.com/humanoid-path-planner/hpp-pinocchio/blob/master/src/center-of-mass-computation/algorithm.cc

Except for some problems related to https://github.com/stack-of-tasks/pinocchio/pull/250, it was actually pretty easy. The comparison with HPP works fine.

Again, the velocity of the CoM does depends on both the DoFs of the subtree and also the DoFs supporting the subtree.

This means that vcom[subtree] =Jcom_subtree * q_dot

Created by: jmirabel

Yes, I realized that today. My unit test was working because I was using the root joint as root of the subtree. Not very smart...

I think I know how to modify it without too much effort to get the right behavior. We can discuss it tomorrow.

I implemented the correct version if you want https://github.com/jcarpent/pinocchio/tree/topic/subtree-com I think it is interested to move it inside Pinocchio directly. I can also add a method which uses the result of jacobianCenterOfMass without further computations. Are you interested?

Related to #28 This PR should close the issue if accepted.