CREO2URDF – Validation of MVP-1

[Nicogene]

Task description

This is a follow-up of #13.

• [x] Validate the urdf obtained in #13
  • [x] Visualize the model with idyntree-visualizer
  • [x] Check if the visual/collision are correctly exported
  • [x] Check that kinematics info is correct
  • [x] As @traversaro suggested: https://github.com/icub-tech-iit/creo2urdf/issues/13#issuecomment-1506681619
  • [x] Compare total inertial properties get from idyntree and from Creo.

Optional:

• [ ] Get the rest position from Creo and set it in the iDynTree::RevoluteJoint
• [ ] Get the joints limits from Creo and set it in the iDynTree::RevoluteJoint
• [ ] Align stl path as we manage in other urdf with relative/package path
• [ ] Add a gazebo control board in the resulting urdf
• [ ] Generate protk.dat via cmake with the correct paths

Definition of done

The above list is completed

cc @pattacini

[traversaro]

A possible additional validation: the idyntree-visualizer permit to check if the visual/collision are correctly exported, and the relative transform permit to check that kinematics info is correct. The last information that needs to be check for correctnes is the one required to compute the dynamics, i.e. inertial information. A check that probably can be done easily is to compute both via Creo and via iDynTree the total inertial properties (i.e. mass, center of mass, 3D inertia matrix) for the complete model (as opposed to the inertial properties of each link that are the one exported by Creo). In iDynTree, you can compute this quantity for a given model in its base frame with the iDynTree::KinDynComputations::KinDynComputationsPrivateAttributes::getRobotLockedInertia() method. The method is currently private, but we can easily make it public.

[mfussi66]

At the moment I'm struggling with visualizing the urdf with idyntree-model-view: the terminal shows no errors and it is printed out that the two .stl files are correctly loaded. But just for a double check I used the following visualizer: https://gkjohnson.github.io/urdf-loaders/javascript/example/bundle/index.html

And the model seems to work as expected. According to the viewer, we also can see that the collision matches the solid.

https://user-images.githubusercontent.com/38140169/232764173-18750bf3-a445-4667-88bc-2998d512b185.mp4

Of course we need to make it work with our tools, but it's one small step towards validation.

[traversaro]

At the moment I'm struggling with visualizing the urdf with idyntree-model-view: the terminal shows no errors and it is printed out that the two .stl files are correctly loaded.

So if the terminal shows no error what is not working? Nothing is visualized?

[mfussi66]

@traversaro Exactly, no mesh is visualized in the viewer, only a coordinate system. I thought it could just be that the model is too small or somewhere else in the world but it does not seem the case.

https://user-images.githubusercontent.com/38140169/232773966-fb3d498d-6a0b-4408-881d-79238c2c420f.mp4

[traversaro]

As I mentioned to @Nicogene in chat, this could also be related to how you are specifying the location of meshes. Unfortunatly the URDF specification does not clearly support the use of relative paths to specify meshes, see https://github.com/robotology/idyntree/issues/291 or https://discourse.ros.org/t/urdf-improvements/30520/21?u=traversaro . However, perhaps this is not the issue, another source of problems could be the use of binary or ascii STLs. Which kind of STLs are you using at the moment?

[traversaro]

Another thing that could influence what we see is if that there is a bug in the irrlicht version used by iDynTree, which iDynTree are you using? The one with dependencies (including irrlicht) installed by vcpkg or the one with dependencies installed by conda-forge?

[mfussi66]

However, perhaps this is not the issue, another source of problems could be the use of binary or ascii STLs. Which kind of STLs are you using at the moment?

Binary!

Another thing that could influence what we see is if that there is a bug in the irrlicht version used by iDynTree, which iDynTree are you using? The one with dependencies (including irrlicht) installed by vcpkg or the one with dependencies installed by conda-forge?

I tried with the one installed with conda-forge, but I can double check with the vcpkg version.

[traversaro]

However, perhaps this is not the issue, another source of problems could be the use of binary or ascii STLs. Which kind of STLs are you using at the moment?

Binary!

Just to understand if that is the issue you can also try to convert to ascii, just to try: for example using https://github.com/cmpolis/convertSTL or similar.

[traversaro]

By the way, which URDF are you trying?

[mfussi66]

By the way, which URDF are you trying?

model.urdf is:

<?xml version="1.0"?>
<robot name="2BARS">
  <link name="BAR">
    <inertial>
      <mass value="0.00741938044781609"/>
      <origin xyz="0 -0.015 0.0035" rpy="0 -0 0"/>
      <inertia ixx="-8.349706673190367e-7" ixy="0" ixz="0" iyy="-1.970110919505906e-9" iyz="-3.895174735103447e-7" izz="-7.157575641017976e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="bar.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="bar.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
  </link>
  <joint name="BAR--BARLONGER" type="revolute">
    <origin xyz="0 0 6.811996306092723e-18" rpy="0 -0 -0.7060878385670463"/>
    <axis xyz="0 0 1"/>
    <parent link="BAR"/>
    <child link="BARLONGER"/>
    <limit lower="0" upper="3.141592653589793" effort="1e+9" velocity="1e+9"/>
  </joint>
  <link name="BARLONGER">
    <inertial>
      <mass value="0.010359380471568048"/>
      <origin xyz="0 -0.0225 0.0035" rpy="0 -0 0"/>
      <inertia ixx="-2.9047306372231453e-6" ixy="-2.249810035331474e-12" ixz="0" iyy="-1.4799217693475475e-9" iyz="-8.158012121359838e-7" izz="-2.7370073443897514e-6"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="barlonger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="barlonger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
  </link>
</robot>

For completeness, I'm uploading both the urdf and the stl in binary format: bars_urdf.zip

[mfussi66]

Just to understand if that is the issue you can also try to convert to ascii, just to try: for example using cmpolis/convertSTL or similar.

Sadly the ruby script in the repo wrongly detects the binary .stl that I have as an ascii one, so the conversion gets all messed up. I'll download Paraview to make the conversion and try again. Additionally, I tried with the absolute path of the .stl files, but with no success.

[traversaro]

I converted the stl in ascii with MeshLab, and it is opening the model even with relative path, but note that the model is quite big:

[mfussi66]

That's because in that file I removed the scaling from meters to millimeters to make sure the model was well visible. Thanks @traversaro !

[traversaro]

By re-exporting the ascii stl to binary (again with MeshLab) I am able to visualize also the model with binary stl. So I guess there is something in the STL as exported by Creo that triggers a bug in Irrlicht. We would need to understand if it is the STL that is malformed or the irrlicht parser of STL that has a bug, but this is kind of not due to the creo2urdf code itself.

[traversaro]

For reference, this are the fixed files: bars_urdf_fixed_with_meshlab.zip . model_ml.urdf is the ASCII one, while model.urdf is the one with binary .stl but "sanitized" by MeshLab.

[mfussi66]

Maybe the prova del nove could be opening it in Gazebo? To see if it is actually the fault of Irrlicht.

[traversaro]

Anyhow, to minimize the possibility of other such bugs and solve this specific problem with idyntree-model-view we can just import the STL in assimp and export it, idyntree already depends on assimp so it should not a problem for creo2urdf to depend on assimp (hoping that assimp can load Creo's stl file fine).

[traversaro]

Just as a refernce, the only know stl-related bugs for irrlicht are quite old and fixed https://sourceforge.net/p/irrlicht/bugs/search/?q=stl .

[Nicogene]

The idea for the validation was:

• Export the urdf with a known transform between links
• Load the urdf with idyntree and compare the known transform with the one obtained by iDynTree::KinDynComputations::getRelativeTransform.
• Check on the inertia properties reading them from idyntree and creo.
• Modify the asm changing the relative position of the links and repeat the first two steps.

Sounds good @traversaro?

This validator function would be nice to have as unit test, but I think is quite complicated create unit tests with creo.

[traversaro]

Sounds good @traversaro?

Yes!

[mfussi66]

Maybe the prova del nove could be opening it in Gazebo? To see if it is actually the fault of Irrlicht.

Just FYI, we can indeed display it in Gazebo:

[traversaro]

I think the bug/problem is on the Creo side. I just checked for curiosity, and the binary STL generated by Creo start with the first 5 bytes that are solid in ASCII. If you change them with an hex editor to anything else (as long as they are still 5 bytes), Irrlicht can load it successfully. According to Wikipedia page on STL (see https://en.wikipedia.org/wiki/STL_(file_format):

A binary STL file has an 80-character header which is generally ignored, but should never begin with the ASCII representation of the string solid, as that may lead some software to confuse it with an ASCII STL file.

It is not clear if they mean STL binary file starting with solid is a violation of the STL Binary specification, as they use the word "should" while typically a violation of the specification is indicated with MUST (see IETF's RFC 2119 for an example https://www.ietf.org/rfc/rfc2119.txt).

However, the original specification(s) according to Wikipedia appears to be "StereoLithography Interface Specification, 3D Systems, Inc., July 1988" and "StereoLithography Interface Specification, 3D Systems, Inc., October 1989" , but I can't find any copy of those online.

Anyhow, being pragmatic it probably could make sense to sanitize binary STL generated by Creo changing the initial solid to any other 5 bytes.

[traversaro]

Apparently the fact that STL binary files created by Creo start with solid is well known, see https://www.ptc.com/en/support/article/CS372211 (I do not have access).

[mfussi66]

Apparently the fact that STL binary files created by Creo start with solid is well known, see ptc.com/en/support/article/CS372211 (I do not have access).

Sadly not much is said in the reply:

[traversaro]

Apparently not only creo does that, also Solidworks: https://github.com/nschloe/meshio/issues/530 . All STL reader that work have hacks to deal with this, see https://github.com/assimp/assimp/blob/b1afa41047de5e81f3c6b8b083a1ba8c169adce6/code/AssetLib/STL/STLLoader.cpp#L111-L119, https://github.com/nschloe/meshio/pull/534/files or https://github.com/gazebosim/gz-common/blob/6f5ee941536e5c781bd826f6ce35360347d581ba/graphics/src/STLLoader.cc#L62 .

[mfussi66]

Latest commit (https://github.com/icub-tech-iit/creo2urdf/commit/e3fc531a837b767eb9b236f030d46e6f6f72892e) implements a simple validation mechanism for the kinematics:

• The relative transform between the CSYS of BAR and BARLONGER is computed as a rotation matrix, both using Creo and iDynTree after re-loading the .urdf file that was just exported.
• Then, the difference between the two transforms is computed from rpy angles, here is a test with different poses:

Note that the angle corresponds to the "Current Position":

[mfussi66]

Compare total inertial properties get from idyntree and from Creo.

I think that point can be split in 3:

• Correctness of the mass
• Correctness of the center of gravity
• Correctness of the inertia matrix wrt the center of gravity

I think the first two are correct, because we can see in the urdf:

  <link name="BAR">
    <inertial>
      <mass value="0.00741938044781609"/>
      <origin xyz="0 -0.015 0.0035" rpy="0 -0 0"/>

...
  <link name="BARLONGER">
    <inertial>
      <mass value="0.010359380471568048"/>
      <origin xyz="0 -0.0225 0.0035" rpy="0 -0 0"/>

In Creo:

BAR

MASS =  7.4193804e-03 KILOGRAM 
CENTER OF GRAVITY with respect to _BAR coordinate frame:
X   Y   Z     0.0000000e+00 -1.5000000e+01  3.5000000e+00  MM
...
BARLONGER

MASS =  1.0359380e-02 KILOGRAM 
CENTER OF GRAVITY with respect to _BARLONGER coordinate frame:
X   Y   Z     0.0000000e+00 -2.2500000e+01  3.5000000e+00  MM

However, the inertia does not match:

URDF:

  <link name="BAR">
      <inertia ixx="-8.349706673190367e-7" ixy="0" ixz="0" iyy="-1.970110919505906e-9" 
iyz="-3.895174735103447e-7" izz="-7.157575641017976e-7"/>

  <link name="BARLONGER">
  <inertia ixx="-2.9047306372231453e-6" ixy="-2.249810035331474e-12" ixz="0" iyy="-1.4799217693475475e-9"
 iyz="-8.158012121359838e-7" izz="-2.7370073443897514e-6"/>

CREO:

BAR

INERTIA at CENTER OF GRAVITY with respect to _BAR coordinate frame:  (KILOGRAM * MM^2)

INERTIA TENSOR:
Ixx Ixy Ixz  9.2527734e-01 -1.3694778e-06  0.0000000e+00
Iyx Iyy Iyz -1.3694778e-06  8.8917300e-02  0.0000000e+00
Izx Izy Izz  0.0000000e+00  0.0000000e+00  9.5360304e-01

...
BARLONGER

INERTIA at CENTER OF GRAVITY with respect to _BARLONGER coordinate frame:  (KILOGRAM * MM^2)

INERTIA TENSOR:
Ixx Ixy Ixz  2.4666081e+00 -2.2498100e-06  0.0000000e+00
Iyx Iyy Iyz -2.2498100e-06  1.2542249e-01  0.0000000e+00
Izx Izy Izz  0.0000000e+00  0.0000000e+00  2.5074290e+00

[traversaro]

Note that you can also check a-priori if the inertia that you are passing to the model make sense by calling the isPhysicallyConsistent method (see https://robotology.github.io/idyntree/classiDynTree_1_1SpatialInertia.html#a6b493d81ed9bcedb37f81d0125d1ab04). This will check that the inertia does not have non-sensical value, such as negative values for the diagonal terms, that do not have physical sense. If you are curious about what this checks are in the mathematical sense, you can see pages 2 and 3 of https://arxiv.org/pdf/1610.08703.pdf .

[mfussi66]

Note that you can also check a-priori if the inertia that you are passing to the model make sense by calling the isPhysicallyConsistent method

Just ran it before the urdf generation, and it does return false for both links. I will inspect how the iDynTree::SpatialInertia object is filled.

[mfussi66]

I found what I think was the error:

1. We retrieve from Creo the inertia matrix wrt the center of mass, with GetCenterGravityInertiaTensor()
2. we use that matrix to instantiate the spatial inertia object,
   • We pass its mass
   • We pass the coordinates of the center of mass wrt to the link coordinate system
   • We also pass the inertia matrix computed in point 1

If we print the spatial inertia object info with getRotationalInertiaWrtCenterOfMass() we get the crazy values with the negative diagonal elements, otherwise with getRotationalInertiaWrtFrameOrigin() we get the inertia matrix found out in point 1.

Therefore, we might need an extra transformation in the code (maybe fromRotationalInertiaWrtCenterOfMass ?)

However, at the moment (depending on the requirements for the PI) we could use the Creo function GetCoordSysInertiaTensor(), which conveniently for our 2BARS model uses the csys corresponding to the revolute joint.

And we get the correct inertia (converted from mm2 to m2):

  <link name="BAR">
    <inertial>
      <mass value="0.00741938044781609"/>
      <origin xyz="0 -0.015 0.0035" rpy="0 -0 0"/>
      <inertia ixx="9.252773439253307e-7" ixy="0" ixz="0" iyy="8.891729956624118e-8" iyz="1.0587911840678754e-22" izz="9.536030366568225e-7"/>
    </inertial>

  <link name="BARLONGER">
    <inertial>
      <mass value="0.010359380471568048"/>
      <origin xyz="0 -0.0225 0.0035" rpy="0 -0 0"/>
      <inertia ixx="2.4666081372848873e-6" ixy="-2.249810035660938e-12" ixz="0" iyy="1.2542248900736103e-7" iyz="0" izz="2.507429019341573e-6"/>
    </inertial>

[mfussi66]

Therefore, we might need an extra transformation in the code (maybe fromRotationalInertiaWrtCenterOfMass ?)

And yep, we now have a valid URDF:

<?xml version="1.0"?>
<robot name="2BARS">
  <link name="BAR">
    <inertial>
      <mass value="0.00741938044781609"/>
      <origin xyz="0 -0.015 0.0035" rpy="0 -0 0"/>
      <inertia ixx="9.252773439253307e-7" ixy="0" ixz="0" iyy="8.891729956624118e-8" iyz="0" izz="9.536030366568229e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="BAR.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="BAR.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
  </link>
  <joint name="BAR--BARLONGER" type="revolute">
    <origin xyz="0 0 6.811996306092723e-18" rpy="0 -0 -0.7060878385670463"/>
    <axis xyz="0 0 1"/>
    <parent link="BAR"/>
    <child link="BARLONGER"/>
    <limit lower="0" upper="3.141592653589793" effort="1e+9" velocity="1e+9"/>
  </joint>
  <link name="BARLONGER">
    <inertial>
      <mass value="0.010359380471568048"/>
      <origin xyz="0 -0.0225 0.0035" rpy="0 -0 0"/>
      <inertia ixx="2.4666081372848873e-6" ixy="-2.249810035331474e-12" ixz="0" iyy="1.2542248900736103e-7" iyz="0" izz="2.507429019341573e-6"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="BARLONGER.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 -0 0"/>
      <geometry>
        <mesh filename="BARLONGER.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </collision>
  </link>
</robot>

[traversaro]

Thanks, this seems indeed the correct fix!

[mfussi66]

The latest commit (https://github.com/icub-tech-iit/creo2urdf/commit/9e4ecb7fe1c6b8be6efeb9afd861d4c58aeb3b99) also sanitizes the STL 👨‍⚕️ . I replaced "solid" with "iCubT" and it works fine:

[pattacini]

Well done! Optional points moved to #18.