Centroidal Momentum Matrix, Centroidal Composite Rigid Body Inertia, Centroidal Momentum

[jackpittenger]

Hello,

I am a student trying to get three values I am currently computing from Pinocchio,

1. Centroidal Momentum Matrix
2. Centroidal Composite Rigid Body Inertia
3. Centroidal Momentum Quantity

Is there a recommended way to compute these three values with Mujoco? I've attempted to calculate them but can't seem to find methods that give me what I'm after.

[yuvaltassa]

1. IIUC for linear momentum this is just the CoM Jacobian (see below) scaled by the mass? I don't think this exists for angular momentum (not a linear operator?), but I might be wrong.
2. I think you want to sandwich the full inertia with the CoM Jacobian, right? So if J is the 3 x nv Jacobian for the CoM of your mechanism, you want J @ M @ J'. CoM Jacobians are computed with mj_jacSubtreeCom. You can get the dense inertia matrix with mj_fullM or use three calls to mj_mulM.
3. Use the subtreelinvel sensor for CoM velocities (multiply by subtree_mass to get momentum) and subtreeangmom for angular momentum.

[v-r-a]

Hello, I'm a student using MuJoCo for research.

The following is regarding point 1:

Centroidal Momentum Matrix

and

IIUC for linear momentum this is just the CoM Jacobian (see below) scaled by the mass? I don't think this exists for angular momentum (not a linear operator?), but I might be wrong.

For the following discussion, I am assuming that you have only one robot with a floating base in your model (for simplifying things at the moment).

You may find section 3.1 from this article (open access) useful: https://doi.org/10.1080/01691864.2020.1778524

I have verified that if you extract $H_L$ from mj_fullM, then indeed $\frac{H_L}{m}$ (m= total mass) equals COM Jacobian computed from (a more general function) mj_jacSubtreeCom (tree starting at body 1) However, if you extract the Angular momentum matrix from the mass matrix as per section 3.1, its frame of reference is not clear as the torques corresponding to the floating base are computed in the local body frame in MuJoCo (see issue #691 .

You will have to build the Angular (Jacobian) momentum matrix yourself (I too have to implement this someday!) as explained in article or numerous other places. Pardon me, I'm not able to find an open access link right now.

Basically (K = angular momentum of the robot in the ground frame about the global origin) $K = \sum{K_i}$, where $K_i$ is the angular momentum of a single link in the ground frame about the global origin. $K_i = I_i \omegai + r{com} \times L_i$, where $I_i$ is the inertia matrix of body $i$ about its COM in the ground frame, $wi$ is the absolute angular velocity of the body in the ground frame, $r{com}$ is the location of the body COM in the ground frame, and $L_i$ is the angular momentum of the body in the ground frame. All these quantities, and their mapping matrices (Jacobians) are available in MuJoCo. So, I am looking forward to building the angular momentum matrix someday soon!

I am also pretty sure that these calculations are already happening inside the CRB algorithm, so one may not do them all over again. @yuvaltassa can point to the source code where these computations happen.

[yuvaltassa]

I think you are looking for something else, it is not happening in CRB.

Look at the code which computes subtree_angmom. subtree_angmom is the centroidal angular momentum w.r.t some subtree, e.g. the base of your floating robot. This is the quantity reported by the subtreeangmom sensor I mentioned above.

If I understand correctly, you are looking for some $q$-dependent matrix $H_A(q)$ of shape 3 x nv such that $H_A \cdot \dot q$ equals the 3-vector computed by this function. Yes?

If I was wanting to write the code that computes it, I would start by finite-differencing subtree_angmom w.r.t to $\dot q$ (aka qvel), so that

1. I have a reference to compare the analytical code to and
2. Verify that the result is indeed independent of the value of $\dot q$, i.e. it is linear (which I still find unintuitive).

Then, using the subtree_angmom code I linked to above as reference, I would write the function that computes the matrix you want. If you get around to it, please do submit a PR! We would love to include it.

[yuvaltassa]

PS if you do try this, note that subtree_angmom is not computed automatically (item 16 here), you need at least one angmom sensor in your model for this to be computed (or just call it manually)

[v-r-a]

If I understand correctly, .... equals the 3-vector computed by this function. Yes?

Right.

I forgot to mention that I'm also looking at subtree_angmom code. Thanks a lot! I will try this out soon.

[v-r-a]

Hi @yuvaltassa

I have written a function mj_subtreeAngMomMat() and made a sample test script (which prints out required details) based on basic.cc. I have tweaked the Robotis-OP3 model a bit so that the robot can move its hands freely to generate angular momentum. The results agree with the subtree_angmom values.

angmomtest.zip

I am yet to do the following, looking forward to doing it soon:

Verify that the result is indeed independent of the value of $\dot{q}$, i.e. it is linear (which I still find unintuitive).

If the code looks fine, could you guide me on submitting a PR? I have seen the function mj_jacSubtreeCom() in the source engine_support.c. I can reformat my code on those lines.

[yuvaltassa]

Very cool!!

Read the contributor's guide and style guide but basically ya, just closely follow the existing patterns in the code.

Once you submit a PR we can also iterate from there in code review. See if you can also write a test in engine_support_test.cc

The check I wrote above and the check you already did should both be converted into proper tests.

[yuvaltassa]

I hope to take a closer look at the zip you posted tomorrow, and will write my thoughts here.

[yuvaltassa]

Yup this looks like a good start. Let me know if you're convinced that the right thing is being computed (and you have some tests for it) and if you like we can do a PR. Can you write down the math please? You wrote down the quantities above, but not the actual formula 🙂

[v-r-a]

1. I will share the formula with derivation (see the attachment)ang_mom_mat.pdf. The angular momentum matrix/centroidal momentum matrix has been used in literature for balance control objectives for biped robots. It would be good to have that quantity along with the mj_jacSubtreeCom.
2. I went through the engine_support_test.cc. I believe I should use a simpler model for the test. Which is better: a two/three link serial robot or the humanoid model?
3. I will soon try out the finite differencing test.

[yuvaltassa]

Definitely a simple abstract mechanism with few joints that you can inline in the test.

[v-r-a]

1. I will share the formula with derivation (see the attachment)ang_mom_mat.pdf. The angular momentum matrix/centroidal momentum matrix has been used in literature for balance control objectives for biped robots. It would be good to have that quantity along with the mj_jacSubtreeCom.
2. I went through the engine_support_test.cc. I believe I should use a simpler model for the test. Which is better: a two/three link serial robot or the humanoid model?
3. I will soon try out the finite differencing test.

The angmommat_doc.zip contains the pdf and source. (cannot attach a .tex file). Sorry, the source has too many customised commands! Let me know if I should make it easier to use.

[yuvaltassa]

Okay this is now live, see item 3 (as of writing this message) in the Latest changelog.

I thought I'd put some of your derivation in the function docs, but in the end decided against it, TMI.

It's a good PR, you should be happy with it. Note the edits I made in 6b7d71429157e420dff297de30c6920097a0e06a, most of them cosmetic. I also managed to tighten the test tolerances quite a bit, which is a more substantive change.

[v-r-a]

Thank you, @yuvaltassa, for encouraging and guiding me through this.

I hope the OP uses the function.

Yeah, the results show a much better match now. I should have thought of giving an initial qvel to keep the test succinct and accurate!