Compute the whole-body-inertia and the average-angular-velocity.

[wyqsnddd]

As I discussed in the mc_rtc issue 245, these commit provide additional computations.

[wyqsnddd]

In my other repo, I made a quick assertion to make sure the COM velocity is equal to the Average Linear Velocity. Thus I think the computation should be correct.

[gergondet]

In my other repo, I made a quick assertion to make sure the COM velocity is equal to the Average Linear Velocity. Thus I think the computation should be correct.

Hi Yuquan,

I added a test that test that but the test is failing. Could you have a look at it?

I also rebased your branch on master and fixed warnings on clang to get the build to run correctly, I had to force push to your branch so please be careful when you pull on your side

[wyqsnddd]

In my other repo, I made a quick assertion to make sure the COM velocity is equal to the Average Linear Velocity. Thus I think the computation should be correct.

Hi Yuquan,

I added a test that test that but the test is failing. Could you have a look at it?

I also rebased your branch on master and fixed warnings on clang to get the build to run correctly, I had to force push to your branch so please be careful when you pull on your side

Yes, sure! I'll look into the test.

[wyqsnddd]

@gergondet Hi Pierre,

I am revisiting this issue again. I found something fishy with the comVelocity. Basically, I failed the following:

    BOOST_CHECK_SMALL((av.tail(3) - comVelocity).norm(), TOL);

but passed:

    BOOST_CHECK_SMALL((ci*av - cm.vector()).norm(), TOL);

I computed the centroidal inertia ci here, which is mathematically correct according to the following equations. The computation of av should be fine too.

Reference equations (I can provide further reference on this point if necessary):

• Inertia transform from frame $F_i$ to $Fc$: $Adg^\top{i,c} Ii Adg{i,c}$, where $I_i$ denotes the inertia in frame $F_i$.
• Force / Momentum transform from $F_i$ to $Fc$: $Adg^\top{i,c} h_i$, where $h_i$ denotes the momentum in frame $F_i$.

Further, the linear momentum should be equal to the product of the COM velocity and the mass. But RBDyn can not pass such a test and the difference is huge if I do the following:

cmm.computeMatrix(mb, mbc, com);

    Eigen::MatrixXd cmmMatrix = cmm.matrix();

    Eigen::VectorXd alpha(mb.nrDof());
    double mass = 6.0;
    std::cout<<" Momemtum diff is: "<<((cmmMatrix * alpha).tail(3) - mass*comVelocity).transpose();

In the above example, I found the mass from other computations.

[gergondet]

Hi Yuquan,

cmm.computeMatrix(mb, mbc, com);

    Eigen::MatrixXd cmmMatrix = cmm.matrix();

    Eigen::VectorXd alpha(mb.nrDof());
    double mass = 6.0;
    std::cout<<" Momemtum diff is: "<<((cmmMatrix * alpha).tail(3) - mass*comVelocity).transpose();

This sample as such is not correct, your alpha vector is a vector of size mb.nrDof() of unitialized values not the velocity vector.

[wyqsnddd]

Hi Pierre,

Sorry for the mistake. It is a redundant comment. Now I have removed it.

Could you please run the example? The issue remains there.

[wyqsnddd]

Hi Pierre,

This is a kind reminder of the open issue. Maybe we can look at it together if you want.

Yuquan