Center of Mass position for every link

[rshum19]

Hi,

I'm trying to visualize the center of mass of each link. However, it seems that this information is not available.

I'm aware that using the function centerOfMass() I can obtain the full body com and the com of subtrees. However, I'm looking for the center of mass of each link as defined in the URDF.

Is there a function or member function of DataTpl that I'm missing that holds this information?

[jcarpent]

The local inertial information are fixed quantities that are stored in model.inertias[joint_id] and the lever contains the quantity you are looking for expressed in the local frame of the joint.

[rshum19]

Great that is exactly what I was looking for.

However, I'm now confused about how centerOfMas() works. Is it correct that centerOfMass() ignores all the links masses of the model before the first non fixed joint?

[jcarpent]

In Pinocchio, we fusion fixed joint inertia with their closest movable parent joint. Indeed, it is not effective to keep a kinematic tree with fixed joints while they do not contribute to the whole kinematic motion.

As the fixed joint information is merged, it indeed contributes to the computation of the center of masses of all the subtrees, this is the role played by centerOfMass which computes the whole-body center of mass, plus all the subtrees' CoMs.

[rshum19]

Not sure if I'm doing something wrong here, but it seems that I'm not getting the

Consider the simple urdf model for an inverted pendulum below

<?xml version="1.0"?>
<robot name="InvertedPendulum" xmlns:xacro="http://ros.org/wiki/xacro">

  <link name="Base">
    <inertial>
      <origin xyz="0 0 0.05"/>
      <mass value="1.0"/>
      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
    </inertial>
  </link>
  <link name="Link1">
    <inertial>
      <origin xyz="0.0 0.0 0.2"/>
      <mass value="5.0"/>
      <inertia ixx="0.009055" ixy="0.0" ixz="0.0" iyy="0.00047" iyz="0.0" izz="0.00047"/>
    </inertial>
  </link>
  <joint name="Joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.05"/>
    <parent link="Base"/>
    <child link="Link1"/>
    <axis xyz="1 0 0"/>
    <limit effort="150.0" lower="-3.1416" upper="3.1416" velocity="12.0"/>
  </joint>
</robot>

if I run the

Eigen::VectorXd q(1);
q.setZero();
auto com = pinocchio::centerOfMass(*m_model, *m_data, q);

Then the output is com[0] = [0, 0, 0.25]. However, I would expect the output to be com[0] = [0, 0, 0.2166]. From the equation com_z = (0.05x1 + 0.25x5)/(1+5) = 0.2166.

So it seems like all the inertia information from the Base link is being ignored. All the kinematic information of the Base link is considered. Is this the intention?

[jcarpent]

But the base link inertia is not contributing to the robot dynamic equations because it is fixed wrt the inertial frame.

[jcarpent]

Why would you like to also account for the base inertia ?

[rshum19]

I agree in the example I gave the base does not contribute on the dynamics.

The reason we want to count is that the base is not fixed to the ground but is “floating” above the ground. Thus we want to know the COM of the entire system including the base. Adding a “floating” joint solved our issue.

Thank you for the clarification! As always great and rapid response from you!

[jcarpent]

By the way, if you really want it, you just need to append the base inertia (model.inertias[0]), to the composite inertia of the first joint (data.oMi[1]..act(data.Ycbr[1])) after calling pinocchio::crba

[rshum19]

Thanks for for that tip, I will try that too.