Description
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
>>> 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-06In 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.