Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
traversaro opened this issue Jun 24, 2022 · 1 comment

Comments

@traversaro
Copy link

traversaro commented Jun 24, 2022

The inertia value given in:

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 &lt; 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
Copy link
Collaborator

@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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants