CREO2URDF – MVP-2 kick-off

[Nicogene]

Task description

As preliminary steps of MVP-2 we would like:

• Produce the simulation asm of neck+head of iCub and commit to the repo
• Get the datums by processing the copy geometries

Definition of done

The above list is completed

[Mick3Lozzo]

Created assembly SIM_ERGOCUB_1-0_HEAD inside repository \\cad-mechanics\projects\simulation_model\ergocub.

@Nicogene @mfussi66

[Nicogene]

Get the datums by processing the copy geometries

Today w/ @mfussi66 noticed that the features inside the copy geometry can be taken transparently (e.g. as axis or as csys), so we didn't need to do anything in this sense.

We started then modifying creo2urdf in order to handle the new assembly and right now we have to find a way to correctly express the transform prev_link_H_link.

See 4932222

We noticed that the root_H_link is fine, but then composing them we get strange transforms (some w/ nan). Probably we are using the idyntree API in the wrong way. But we will see better in the next days.

cc @pattacini @traversaro

[traversaro]

We noticed that the root_H_link is fine, but then composing them we get strange transforms (some w/ nan). Probably we are using the idyntree API in the wrong way. But we will see better in the next days.

cc @pattacini @traversaro

See 4932222#r114447105 .

[Nicogene]

Alongside @mfussi66 we fixed some bugs in axis definitions (see ef64224).
Since each joint axis is contained both on the child and parent links, each link has de-facto 2 axis, but only one is correct for defining the joint. Then for now we hardcoded which axis we have to take given a child link since we don't have the mechanism representation from the API.
We don't know if we can overcome this, maybe it will be part of the information passed via file.

We noticed that we are missing the SCSYS for the first link (NECK_1), this is required for us to correctly define the transforms from links. We are assuming that the first one is an identity but it is not what we actually have.
@Mick3Lozzo could you add it to the asm?

We are still getting errors in the export of the urdf, maybe it is due to the fact we have the wrong first transform, or maybe we have some bug around.
Unfortunately, we don't have either the cout and cerr output, @traversaro said that in the export of the urdf a check is performed on the axis of the joint if it is close enough to the reference frame of the child link (or parent?), probably it is the error we are getting.
Right now the priority is to get the prints from idyntree, redirecting the cerr/cout to string/file.

cc @pattacini @fiorisi

[traversaro]

Right now the priority is to get the prints from idyntree, redirecting the cerr/cout to string/file.

See robotology/idyntree#1064 . As a first test we can try https://stackoverflow.com/questions/10150468/how-to-redirect-cin-and-cout-to-files , even if it is probably not clean as it interferes with the global process state.

[traversaro]

said that in the export of the urdf a check is performed on the axis of the joint if it is close enough to the reference frame of the child link (or parent?), probably it is the error we are getting.

See https://github.com/robotology/idyntree/blob/v9.0.0/src/model_io/codecs/src/URDFModelExport.cpp#L365 . See also other [ERROR] messages to get an idea of the error we could be getting.

[mfussi66]

And indeed we have the error on the exporter, related to the first joint:

[ERROR] URDFModelExport: Impossible to convert joint SIM_ECUB_HEAD_NECK_1--SIM_ECUB_HEAD_NECK_2 to a URDF joint 
without moving the link frame of link SIM_ECUB_HEAD_NECK_2 , because its origin is 0.00486151 4.13255e-16 -0.28977

[mfussi66]

By defining the joint origin as the position of the child coordinate system we were able to fix the error. Initially, we put (0 0 0) assuming that the origin would be taken from the required transform, see here . I'm not 100% sure that it is the intended usage, but the urdf is now valid.

However, we still need to correct the wrong pose of some of the links and joints. Since we loop through the kinematic chain, it might be just a matter of checking if all the correct steps are taken, and apply the fix from @Mick3Lozzo .

[Mick3Lozzo]

We noticed that we are missing the SCSYS for the first link (NECK_1), this is required for us to correctly define the transforms from links. We are assuming that the first one is an identity but it is not what we actually have. @Mick3Lozzo could you add it to the asm?

Added SCSYS_NECK_1.

@mfussi66 @Nicogene

[mfussi66]

Joint axes fixed, the (simple) issue was that they are expressed by default in the root frame:

[mfussi66]

So in the urdf in the previous post there was a problem in the link transforms. Basically, whenever we retrieved them, they had an unexpected rotation around the Z axis of 180deg, compared to the measurement by hand inside Creo.

So after a lot of checks and verifications, I think we have a potentially working URDF of the head, with all the links and joints in correct order and placement:

BUT

What fixed the problem?

Since we need the transforms as iDynTree structures, we made a deep copy to convert from pfcTransform3D to iDyntree::Transform, assigning each element of the first to the constructor of the second (see https://github.com/icub-tech-iit/creo2urdf/blob/master/src/creo2urdf/src/Creo2Urdf.cpp#L127-L130)
I was afraid that Creo might be crazy enough to index matrix elements as (col, row) instead of (row, col) (basically transposing the matrix).
After applying the switch, we have the correct model 👀

I'm fairly sure that there are no other errors in the code that compensate for this transposition, but it might be useful to make a test in an environment with only two coordinate systems, and see what the transform is when printed from the toolkit in (row, col).
The documentation feels a bit opaque regarding it:

[traversaro]

Probably it is not a col/row problem, just a matter of ${}^{parent} R_{child}$ used instead of ${}^{child} R_{parent}$ (or viceversa)

[mfussi66]

I suspected that too, but what worried me was the fact that a simple retrieval of a transform would return only the rows and columns of the rotation matrix switched, compared to the program: