CREO2URDF – Implement recursion in the model tree navigation

[Nicogene]

Right now creo2urdf can export the urdf of assemblies that contains only parts, it fails if subassemblies are present.

cc @mfussi66 @pattacini

[Nicogene]

This task is much less trivial than I expected, mainly because when we compute the transform root_H_link we are using the ComponentPath assuming that all the piecies are mounted to the root assembly.

In the iCub asm for example we have

• root
  • lowerbody
    • root_link
    • belt
    • right_leg
      • hip
      • ...
    • left_leg
      • hip
      • ...
  • upperbody
    • ...
  • head
  • ...

When we are reaching a leaf of this graph with the component path we are able to compute the transform w.r.t the parent element in the model tree.

At some point we should concatenate all the transforms in order to have the root_H_link that for "flat" assemblies is straightforward.

This commit still does not work:

• b589bfa (#98)

But it has several improvements in the logic of the code.

In any case this change:

--- a/src/creo2urdf/src/Sensorizer.cpp
+++ b/src/creo2urdf/src/Sensorizer.cpp
@@ -40,6 +40,11 @@ void Sensorizer::readSensorsFromConfig(const YAML::Node & config)
         if (s["exportedFrameName"].IsDefined()) {
             exported_frame_name = s["exportedFrameName"].Scalar();
         }
+        std::vector<string> sensor_blobs;
+        if (s["sensorBlobs"].IsDefined())
+        {
+            sensor_blobs = s["sensorBlobs"].as<std::vector<std::string>>();
+        }

         try
         {
@@ -51,7 +56,7 @@ void Sensorizer::readSensorsFromConfig(const YAML::Node & config)
                                 export_frame,
                                 stringToEnum<SensorType>(sensor_type_map, s["sensorType"].Scalar()),
                                 update_rate,
-                                s["sensorBlobs"].as<std::vector<std::string>>() });
+                                sensor_blobs });

is not related but has to be added since sensorBlobs is not marked as mandatory, but if you do not specify it for all the sensors the urdf generation fails

cc @mfussi66 @traversaro

[Nicogene]

In this latest commit I tried to add the concatenation of transforms between the subassembly and the underlying items (either other subassemblies or parts):

• dfff7b0

Unfortunately

std::tie(ret, parentAsmCsys_H_asmCsys) = getTransformFromPart(component_handle, "ASM_CSYS", scale);

Is returning false (component_handle is the pointer to the items either is a part or a subassembly), I am getting this error:

Failed to get the transform from  ICUB_2-5_SIM_MODEL_LOWERBODY (the parent assembly) to SIM_SEA_2-5_L_LEG

I will investigate more in deep

[Nicogene]

Another piece of information in the debugging.

Failed to get the transform from ICUB_2-5_SIM_MODEL_LOWERBODY (the parent assembly) to SIM_SEA_2-5_L_LEG

Basically this is the only error I am getting because the the asm of left leg is missing any kind of csys, it doesn't have neither the ASM_CSYS

The problems I am getting now are related to the element tree, when we populate the joints map we are retrieving parent and child links from the element tree, and the first part of each subassembly result having a joint beteween the subassembly and the part, but there is not any kind of constraint, so probably the ElementTreeManager has to be revised/fixed.

[Nicogene]

After adding some guards while accessing map elements I managed to create an urdf, unfortunately, the model for some reason contains only those joints:

  Joints: 
    [0] r_leg_ft_sensor (dofs: 0) : r_hip_2<-->r_hip_3
    [1] torso_pitch (dofs: 1) : root_link<-->torso_1
    [2] r_wrist_pitch (dofs: 1) : r_forearm<-->r_wrist_1
    [3] r_hip_yaw (dofs: 1) : r_hip_3<-->r_upper_leg
    [4] l_wrist_pitch (dofs: 1) : l_forearm<-->l_wrist_1
    [5] r_knee (dofs: 1) : r_upper_leg<-->r_lower_leg
    [6] r_hip_roll (dofs: 1) : r_hip_1<-->r_hip_2
    [7] neck_yaw (dofs: 1) : neck_2<-->head

This instead is the list of joints retrieved from the element tree:

SIM_SEA_2-5_ROOT_LINK--SIM_SEA_2-5_LAP_BELT_1
SIM_SEA_2-5_LAP_BELT_1--SIM_SEA_2-5_LAP_BELT_2
SIM_SEA_2-5_R_HIP_1--SIM_SEA_2-5_R_HIP_2
SIM_SEA_2-5_R_HIP_2--SIM_SEA_2-5_R_UPPER_THIGH
SIM_SEA_2-5_R_UPPER_THIGH--SIM_SEA_2-5_R_THIGH
SIM_SEA_2-5_R_THIGH--SIM_SEA_2-5_R_SHANK
SIM_SEA_2-5_R_SHANK--SIM_SEA_2-5_R_ANKLE
SIM_SEA_2-5_R_ANKLE--SIM_SEA_2-5_R_FOOT
SIM_SEA_2-5_R_FOOT--SIM_SEA_2-5_R_SOLE
SIM_SEA_2-5_L_HIP_1--SIM_SEA_2-5_L_HIP_2
SIM_SEA_2-5_L_HIP_2--SIM_SEA_2-5_L_UPPER_THIGH
SIM_SEA_2-5_L_UPPER_THIGH--SIM_SEA_2-5_L_THIGH
SIM_SEA_2-5_L_THIGH--SIM_SEA_2-5_L_SHANK
SIM_SEA_2-5_L_SHANK--SIM_SEA_2-5_L_ANKLE
SIM_SEA_2-5_L_ANKLE--SIM_SEA_2-5_L_FOOT
SIM_SEA_2-5_L_FOOT--SIM_SEA_2-5_L_SOLE
ICUB_2-5_SIM_MODEL_UPPERBODY--SIM_SEA_2-5_R_SHOULDER_1
SIM_SEA_2-5_R_SHOULDER_1--SIM_SEA_2-5_R_SHOULDER_2
SIM_SEA_2-5_R_SHOULDER_2--SIM_SEA_2-5_R_UPPER_ARM
SIM_SEA_2-5_R_UPPER_ARM--SIM_SEA_2-5_R_ELBOW
SIM_SEA_2-5_R_ELBOW_PROSUP--SIM_SEA_2-5_R_FOREARM
SIM_SEA_2-5_R_FOREARM--SIM_SEA_2-5_R_WRIST_PITCH
SIM_SEA_2-5_R_WRIST_PITCH--SIM_SEA_2-5_R_HAND
SIM_SEA_2-5_CHEST_BB--SIM_SEA_2-5_L_SHOULDER_1
SIM_SEA_2-5_L_SHOULDER_1--SIM_SEA_2-5_L_SHOULDER_2
SIM_SEA_2-5_L_SHOULDER_2--SIM_SEA_2-5_L_UPPER_ARM
SIM_SEA_2-5_L_UPPER_ARM--SIM_SEA_2-5_L_ELBOW
SIM_SEA_2-5_L_ELBOW_PROSUP--SIM_SEA_2-5_L_FOREARM
SIM_SEA_2-5_L_FOREARM--SIM_SEA_2-5_L_WRIST_PITCH
SIM_SEA_2-5_L_WRIST_PITCH--SIM_SEA_2-5_L_HAND
SIM_SEA_2-5_NECK_1--SIM_SEA_2-5_NECK_2
SIM_SEA_2-5_NECK_2--SIM_SEA_2-5_HEAD

There are many more, but in any case, some links are missing from this list, this means that also some joints are missing, and the model seems to be disconnected at several points

The ergoCub asm continues to be exportable without problems, then the problem seems how I am managing the subassemblies

[Nicogene]

There are many more, but in any case, some links are missing from this list, this means that also some joints are missing, and the model seems to be disconnected at several points

For example r/l_hip_pitch are both missing, they should be defined as:

 SIM_SEA_2-5_ROOT_LINK--SIM_SEA_2-5_R_HIP_1
 SIM_SEA_2-5_ROOT_LINK--SIM_SEA_2-5_L_HIP_1

And if I check how it is mounted

The constraint is User defined, it is not pin, and that's why the ElementTreeManager is skipping it.

The constraint is not pin because SIM_SEA_2-5_R_HIP_1 belongs to a different assembly respect SIM_SEA_2-5_ROOT_LINK. (cc @fiorisi @Lawproto @salvi-mattia)

[Nicogene]

SIM_SEA_2-5_ROOT_LINK--SIM_SEA_2-5_R_HIP_1

Thanks to the help of @salvi-mattia, @mfussi66 and I understood why we are missing all the joints whose parts belong to different subassemblies.

The reason is that we are getting from the elementree of each part the joint information skipping the subassemblies since they does have to be insert in the urdf, but the joint that connects the last part of a subassembly to the first part of the childe subsubassembly (e.g. SIM_SEA_2-5_ROOT_LINK--SIM_SEA_2-5_R_HIP_1) is defined as a pin constraint between that part with the subsubassembly

This is because the first part of a (sub)assembly has to be fixed, and not constrainted to anything.

So a possible solution is that we analyze the elementree also of the subassembly, but we treat the subassembly as its first part.

cc @fiorisi @pattacini

[Nicogene]

A significant step forward, unfortunately we have still something to fix.

Yesterday we realized that from how a component is assembled into another, when we traverse the elementree of a part, if a joint is present, the first part returned is the parent and the second is the child.

Before for understanding which part was the parent and the child we did a check on the names, but when we retrieve the elementree of a subassembly the name of the owner is not a "link", so our method worked with flat assemblies, but the subassemblies broke it.

After this improvement I saw more joints in the resulting urdf, but still some were missing, this was due to the fact that the datum used for assemble had not univoque names (e.g. axis A_1), and since we were using the datum name as key of the joint map this lead to make some joints "hide" others.

Finally I decided to refactor the code in order to use as univoque key of the joint map <parent_name>--<child_name>, this was tough but voila we have a complete model

As you can see I still encountering issues on defining some transforms I don't know if:

• The concatenation of transforms between subassembly is wrong
• Many(almost all) the links does not have a reference frame defined, then we are using CSYS as fallback and this is problematic?

In any case a big step forward

cc @mfussi66 @pattacini @fiorisi @traversaro

[Nicogene]

In the latest commit, I managed to fix the transform between an assembly and child subassembly

I had to refactor a bit the code, the ergocub model is still exported as before.

The only thing I am missing is to fix the axis of the joints that are defined between two links belonging to different subassemblies, but we are close to the goal!

[Nicogene]

The only thing I am missing is to fix the axis of the joints that are defined between two links belonging to different subassemblies, but we are close to the goal!

Actually, the problems I am facing with the axis are not related to the recursion or subassemblies, are wrong all the axis where the child link (part) has not a linkFrame defined, but CSYS is used as reference frame, but usually the CSYS is outside the part.

For computing the axis vector we do:

axis_unit_vector = csys_H_link_frame.inverse() * axis_unit_vector; 

axis_unit_vector is originally get from creo and it is expressed in the CSYS frame, and usually when we have a link_frame, we manage to express the axis vector within the link, unfortunately when the link_frame is not defined CSYS is used insted and then csys_H_link_frame is an identity matrix.

This method does not work with CSYS, or I manage to find another way to retrieve the axis expressed in the link frame even if it is not defined, or we should keep it as limitation.

We are already printing a warning when CSYS is used saying that something could not work in the urdf exprotation

[Nicogene]

I elaborate a little bit on the problem above, maybe @traversaro on the math side or @fiorisi @Lawproto on the mech side have an idea of how to solve it because the Mathworks plugin manages to correctly export joints even if only CSYS is defined, so there should be a way to do it.

Right now we can handle a case like this

Instead, we cannot handle the CSYS case

To have an urdf well-formed as this one I have to create IDynTree::Joints using as transform parent_frame_H_child_frame for each joint.

If the frames used are user-defined (e.g. SCSYS_L_SHOULDER_2) in the way that the rotation axis passes through its center (see first picture), the transform I use in the IDynTree::Joint allows to both correctly place the link and define correctly the rotation axis.

On the other hand, when CSYS is used (second picture), the transformation we are using is referred to the default ref system that usually is not neither where the axis passes or "inside" the axis.

I would like to compute a sort of transform between CSYS and the axis, if I am not wrong the axis defines just a direction so that offset should be applied to the transform parent_frame_H_child_frame?

The axis in creo is represented as 2 3D points (start and end), I thought to compute the medium point and apply the offset to the transform, but it didn't give the expected results, since the meshes are exported using CSYS not centered on the joint axis.

[traversaro]

I would like to compute a sort of transform between CSYS and the axis, if I am not wrong the axis defines just a direction so that offset should be applied to the transform parent_frame_H_child_frame?

Actually in iDynTree a iDynTree::Axis is composed by a direction and a offset/position, see https://robotology.github.io/idyntree/classiDynTree_1_1Axis.html . By default the offset/position is zero, but you can set any kind of position you want if the axis is not passing through the origin of the frame.

[Nicogene]

I would like to compute a sort of transform between CSYS and the axis, if I am not wrong the axis defines just a direction so that offset should be applied to the transform parent_frame_H_child_frame?

Actually in iDynTree a iDynTree::Axis is composed by a direction and a offset/position, see robotology.github.io/idyntree/classiDynTree_1_1Axis.html . By default the offset/position is zero, but you can set any kind of position you want if the axis is not passing through the origin of the frame.

And as offset the medium point of the line could make sense ?

[traversaro]

And as offset the medium point of the line could make sense ?

Any point on the line would be fine. The important thing is that the line then passes through the origin of the child frame, to be compliant with URDF constraints when the iDynTree::Model is converted to URDF, see https://github.com/robotology/idyntree/blob/6ba29bd0fa41cac840186e2387f4a6a55beea7c2/src/model_io/codecs/src/URDFModelExport.cpp#L360-L368 . Actually reading the code in https://github.com/robotology/idyntree/blob/6ba29bd0fa41cac840186e2387f4a6a55beea7c2/src/model_io/codecs/src/URDFModelExport.cpp#L360-L368 there may be a bug in iDynTree, even if the offset is non-zero it should be ok as long as the offset is aligned with the axis (i.e. the axis passes through zero).

[Nicogene]

Right now we define the axis as follows

creo2urdf/src/creo2urdf/src/Creo2Urdf.cpp

Lines 239 to 242 in e1187ab

	if (joint_info.second.type == JointType::Revolute) {
	joint_sh_ptr = std::make_shared<iDynTree::RevoluteJoint>();
	dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition()));
	}

I changed it trying

dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, parent_H_child.getPosition() + axis_offset));

and

dynamic_cast<iDynTree::RevoluteJoint*>(joint_sh_ptr.get())->setAxis(iDynTree::Axis(axis, axis_offset));

where axis_offset is the medium point of the axis:

        axis_offset[0] = ((pend->get(0) + pstart->get(0)) / 2.0) * scale[0];
        axis_offset[1] = ((pend->get(1) + pstart->get(1)) / 2.0) * scale[1];
        axis_offset[2] = ((pend->get(2) + pstart->get(2)) / 2.0) * scale[2];

But in both cases I cannot export the urdf for errors like this one:

[ERROR] URDFModelExport: Impossible to convert joint torso_pitch to a URDF joint without moving the link frame of link torso_1 , because its origin is 1.90735e-10 -0.032 -0.0364

@traversaro this is due to the fact that the ref frame is still CSYS and we cannot move just the axis right?

[Lawproto]

I elaborate a little bit on the problem above, maybe @traversaro on the math side or @fiorisi @Lawproto on the mech side have an idea of how to solve it because the Mathworks plugin manages to correctly export joints even if only CSYS is defined, so there should be a way to do it.

@Nicogene I would gladly like to help you, unfortunately I am not sure I am understanding the problem. We can meet in person or via Teams if you want.

[traversaro]

@traversaro this is due to the fact that the ref frame is still CSYS and we cannot move just the axis right?

As I mentioned in the previous post, there was a bug in the check of the iDynTree's URDF exporter. This was fixed in robotology/idyntree#1188, if that is ok for you I can merge and do a new release of idyntree.

[traversaro]

The 12.3.1 idyntree version with the fix was updated in vcpkg: microsoft/vcpkg#39257 .

[Nicogene]

The 12.3.1 idyntree version with the fix was updated in vcpkg: microsoft/vcpkg#39257 .

Great thanks I will try now!

[Nicogene]

Unfortunately, the @traversaro fix did not solve the problems I am encountering, I applied oldChild_H_newChild both to the parent_h_child transform and to the child_H_geometry for correcly place the meshe, but unfortunately this is the current result

as you can see r_hand seems now correctly placed, but something broke along the kinematic chain.

Since this CSYS issue is not correlated to the make creo2urdf work with assemblies that have subassemblies, I opened a separated issue for that:

• CREO2URDF – Correctly handle the joint definition when using CSYS  #99

That issue happens also in the "flat" models, and we might debug it in a simpler assembly as 2bars rather than use iCub model.

I think that #98 can be merged because:

• We can manage now subassemblies
• The definition of the joints has been improved, now datums with repeated names can be safely used
• Several bugfixes + improvements of the quality of life has been added

cc @mfussi66