Talos's gripper_left_motor_single_link and gripper_right_motor_single_link links inertia do not respect triangle inequalities

[traversaro]

The inertia value given in:

• https://github.com/Gepetto/example-robot-data/blob/v4.0.1/robots/talos_data/robots/talos_reduced.urdf#L1448
• https://github.com/Gepetto/example-robot-data/blob/v4.0.1/robots/talos_data/robots/talos_reduced.urdf#L1803

are not consisten with the physical constraints of inertia. In particular, they do not respect the triangle inequalities of inertia (see for example Eq. (16) in https://arxiv.org/pdf/1610.08703.pdf).

Checking this is quite simple. The <inertia> element of URDF gives you the inertia expressed at the center of mass of the link (see http://wiki.ros.org/urdf/XML/link). If you extract the principal moment of inertia, you can see that $J_y + J_z - J_x < 0$, while it should be $J_y + J_z - J_x \geq 0$ :

>>> inertia = np.array([[0.000115, 0.000052, 0.000025], [0.000052, 0.000153, 0.000034], [0.000025, 0.000034, 0.00019]])
>>> inertia
array([[1.15e-04, 5.20e-05, 2.50e-05],
       [5.20e-05, 1.53e-04, 3.40e-05],
       [2.50e-05, 3.40e-05, 1.90e-04]])
>>> u, s, vh = np.linalg.svd(inertia)
>>> s
array([2.31875699e-04, 1.47497311e-04, 7.86269902e-05])
0.0002261243014149173
>>> s[1]+s[2]-s[0]
-5.7513971701654825e-06

In many case this is not a problem, but some libraries (such as drake) throw an error if a non-physical consistent model is loaded. Indeed, this is how @akhilsathuluri found the problem, as trying to load a Talos model from this repo resulted in:

meshcat.Delete()
meshcat.DeleteAddedControls()
urdf_path = './talos_reduced_iso.urdf' 
builder = DiagramBuilder()
time_step = 1e-4
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
model = Parser(plant, scene_graph).AddModelFromFile(urdf_path, 'talos-robot')
# Transform spawn location
# X_R = RigidTransform(RotationMatrix.MakeZRotation(np.pi), np.array([0, 0, 0.63]))
# plant.WeldFrames(plant.world_frame(), plant.get_body(plant.GetBodyIndices(model)[0]).body_frame(), X_R)
plant.Finalize()
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)

diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)

# plant.SetPositions(plant_context, np.concatenate((iCub_base_pose_list, iCub_joint_pos_list)))
diagram.Publish(context)

---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
Input In [5], in <cell line: 7>()
      5 time_step = 1e-4
      6 plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
----> 7 model = Parser(plant, scene_graph).AddModelFromFile(urdf_path, 'talos-robot')
      8 # Transform spawn location
      9 # X_R = RigidTransform(RotationMatrix.MakeZRotation(np.pi), np.array([0, 0, 0.63]))
     10 # plant.WeldFrames(plant.world_frame(), plant.get_body(plant.GetBodyIndices(model)[0]).body_frame(), X_R)
     11 plant.Finalize()

RuntimeError: Spatial inertia fails SpatialInertia::IsPhysicallyValid().
 mass = 0.14765
 Center of mass = [0.02589  -0.01284  -0.0064]
 Inertia about point P, I_BP =
[ 0.00014539  0.000101083   4.9465e-05]
[0.000101083  0.000258016  2.18667e-05]
[ 4.9465e-05  2.18667e-05  0.000313311]
 Inertia about center of mass, I_BBcm =
[0.000115   5.2e-05   2.5e-05]
[ 5.2e-05  0.000153   3.4e-05]
[ 2.5e-05   3.4e-05   0.00019]
 Principal moments of inertia about Bcm (center of mass) =
[7.86269901844723e-05  0.00014749731123044508  0.00023187569858508283]

I checked if the problem was also in the Talos models in https://github.com/pal-robotics/talos_robot, but the only instances of gripper_left_motor_single_link I found there (https://github.com/pal-robotics/talos_robot/blob/452fd0727ad09b616e000901ff85c3d7629a9d28/talos_description/test/calibration/displaced.urdf#L1416) had different inertial parameters.

[cmastalli]

@traversaro -- thank you so much for reporting this issue with plenty of details!!!

@olivier-stasse -- could you tell us why are the right values that we should use here?

@jcarpent -- perhaps, it makes sense to have such kind of "checks" in Pinocchio as Drake does.