Add legs FT IMU frames - address review

[prashanthr05]

This PR incorporates the changes to suggested in PR #111 and removes the gazebo_imu plugins to be compatible with the robot.

cc @traversaro @fiorisi

[prashanthr05]

I have just edited the yaml files. I hope that'd be enough? or should I compile/generate a few files after the changes and push it as well?

[traversaro]

If you can rebase your commits with @fiorisi it will be great, otherwise I will merge & squash, thanks!

[prashanthr05]

If you can rebase your commits with @fiorisi it will be great, otherwise I will merge & squash, thanks!

I had pushed on top of @fiorisi commits. In that case, would a rebase be necessary? The history of his commits can also be seen?

[prashanthr05]

The error in Travis persists. I will check it.

[traversaro]

In that case, would a rebase be necessary?

Given that one commit is "adding YARP conf files" and another is approximately "removing YARP conf files", for keep a simple history it would be nice to merge the two last commits, but again I can just merge all the commits of the PR and that will also result in a cleanish git history.

[traversaro]

If I am not mistaken, we should also fix/workaround robotology/simmechanics-to-urdf#36 .

[prashanthr05]

In that case, would a rebase be necessary?

Given that one commit is "adding YARP conf files" and another is approximately "removing YARP conf files", for keep a simple history it would be nice to merge the two last commits, but again I can just merge all the commits of the PR and that will also result in a cleanish git history.

Ah okay, I can squash those commits.

[prashanthr05]

The error in Travis persists. I will check it.

Ok, after reading through the code, I understand this error is because I turned exportFrameInURDF to Yes for the sensorName: default but not for the newly added sensors.

@traversaro could you elaborate on this comment a bit more, #113 (comment)

and should we address it in this PR?

[traversaro]

@traversaro could you elaborate on this comment a bit more, #113 (comment)

and should we address it in this PR?

If I understood correctly robotology/simmechanics-to-urdf#36, using the latest urdf_parser_py (as done in the Travis script, see https://github.com/robotology/icub-model-generator/blob/master/.travis.yml#L49) will result in the visual and collision element not to be generated.

[prashanthr05]

cc @nunoguedelha I think we will need your review for this PR

[nunoguedelha]

As far as I remember, sensors don't need an exported fixed URDF frames, i.e. fake link with zero mass connected to a link with a fixed joint. this is automatically handled by iDyntree, the exportFrameInURDF parameter is not mandatory (refer to https://github.com/robotology/simmechanics-to-urdf/blob/master/README.md).

[nunoguedelha]

exportFrameInURDF -> No

As mentioned in my previous comment aboce, it is not mandatory to export a fake link frame in the URDF. So we can leave exportFrameInURDF to the default value "No" (I explain further how the default is set).

sensorName

I would use all lower case, as for all other sensor names used in the URDF.

sensorType

For each IMU sensor, we shold add one accelerometer and one gyroscope. For instance, for SCSYS_L_UPPER_LEG_FT-IMU_3B12:

  - frameName: SCSYS_L_UPPER_LEG_FT-IMU_3B12
    linkName: l_hip_2
    sensorName: l_upper_leg_ft_acc_3b12
    sensorType: "accelerometer"
  - frameName: SCSYS_L_UPPER_LEG_FT-IMU_3B12
    linkName: l_hip_2
    sensorName: l_upper_leg_ft_gyro_3b12
    sensorType: "gyroscope"

The default block is identified by the parser through the sensorType...
accelerometer:
https://github.com/robotology/icub-model-generator/blob/44e99c620cbbc237a84929023b80aab220ab0428/simmechanics/data/icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in#L652-L658
gyroscope:
https://github.com/robotology/icub-model-generator/blob/44e99c620cbbc237a84929023b80aab220ab0428/simmechanics/data/icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in#L652-L658

The purpose of these default blocks was to avoid to fill the same information for the large number of MTB accelerometers.
the default sensor blob:
https://github.com/robotology/icub-model-generator/blob/44e99c620cbbc237a84929023b80aab220ab0428/simmechanics/data/icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in#L656-L658
will cause a problem. A workaround would be to set an empty sensorBlobs: field, which requires a small change in the parser. I can help on that.

linkName <-- POTENTIAL ISSUE

Is the transform of the IMU sensor frame expressed with respect to the FT parent link, or with respect to the FT sensor frame as illustrated here?

[nunoguedelha]

linkName <-- POTENTIAL ISSUE

Is the transform of the IMU sensor frame expressed with respect to the FT parent link, or with respect to the FT sensor frame as illustrated here?

After discussing with @prashanthr05 and going through the Simmechanics output file, we realized that the relative transform that will appear in the generated URDF model file, under the tag <sensor><origin> is computed from the composition of the absolute pose of the IMU sensor frame (defined in the simmechanics output ICUB_2-5_BB_SIM_MODEL.xml with respect to the same WORLD frame) and the linkName frame pose with respect to the WORLD. Since l_hip_2 and r_hip_2 are respectively parents of the FT sensor fixed frames l_leg_ft_sensor and r_leg_ft_sensor, it is ok to use them as the FT-IMU parent links.

[prashanthr05]

While compiling, I get this error:

[  3%] Generating iCubGazeboV2_5_plus.urdf
Traceback (most recent call last):
  File "/usr/local/bin/simmechanics_to_urdf", line 11, in <module>
    load_entry_point('simmechanics-to-urdf==0.2', 'console_scripts', 'simmechanics_to_urdf')()
  File "build/bdist.linux-x86_64/egg/simmechanics_to_urdf/firstgen.py", line 1871, in main
  File "build/bdist.linux-x86_64/egg/simmechanics_to_urdf/firstgen.py", line 177, in convert
  File "build/bdist.linux-x86_64/egg/simmechanics_to_urdf/firstgen.py", line 249, in addSensors
KeyError: ('r_hip_2', 'SCSYS_R_UPPER_LEG_FT-IMU_3B11')
simmechanics/CMakeFiles/generate-models-simmechanics.dir/build.make:92: recipe for target 'simmechanics/iCubGazeboV2_5_plus.urdf' failed
make[2]: *** [simmechanics/iCubGazeboV2_5_plus.urdf] Error 1
CMakeFiles/Makefile2:1202: recipe for target 'simmechanics/CMakeFiles/generate-models-simmechanics.dir/all' failed
make[1]: *** [simmechanics/CMakeFiles/generate-models-simmechanics.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

REASON

The IMU frames have not been generated in the icub2_5/ICUB_2-5_plus_BB_SIM_MODEL.xml and in the options file icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in, we are asking the firstgen.py parser to add the IMU sensors by accessing the relevant frame names in the generated sim mechanics file.

WORKAROUND

• To test if the changes in the PR are passing, I commented out the macro in generate_icub_simmechanics that uses the icub2_5/ICUB_2-5_plus_BB_SIM_MODEL.xml. With this change, the compilation succeeds.

• Maybe a proper fix could be to maintain two yaml option files, one for ICUB_2-5_plus_BB_SIM_MODEL.xml and another for ICUB_2-5_BB_SIM_MODEL.xml.

Any suggestions @nunoguedelha @fiorisi ?

[traversaro]

To workaround robotology/simmechanics-to-urdf#36 we can also just checkout a commit of urdf_parser_py becore the merging of ros/urdf_parser_py#26 .

[nunoguedelha]

Pushed fix on CMakeLists.txt regarding #113 (comment) and #114.

[prashanthr05]

Somehow, an additional indentation is being added in the generated yaml files with the current changes and the parser fails for this reason.

[prashanthr05]

Somehow, an additional indentation is being added in the generated yaml files with the current changes and the parser fails for this reason.

Fixed in 933d1cc

We can merge this PR if #113 (comment) is addressed.

[prashanthr05]

After testing the backward compatibility PR of urdf_parser_py following the steps suggested in robotology/simmechanics-to-urdf#38 (comment), the visual and collision components of every link was added and I could visualize the frames added in this PR in RViz successfully.

Showing only 4 out of the 8 frames added here to have a degree of readability.

[traversaro]

I am not sure why I can't directly edit your PR, but I think adding the line:

git checkout 31474b9baaf7c3845b40e5a9aa87d5900a2282c3

after the line cd urdf_parser_py ( https://github.com/robotology/icub-model-generator/blob/master/.travis.yml#L50 ) should be a valid workaround for #113 (comment) .

Other info for the change:

Commit message: Use latest confirmed working version of urdf_parser_py

Using urdf_parser_py's commit 31474b9baaf7c3845b40e5a9aa87d5900a2282c3, that precedes the regression introduced in ros/urdf_parser_py#26 and discussed in robotology/simmechanics-to-urdf#36 .
The commit used is based from the info contained in this icub-models commit: robotology/icub-models@b69b989 .

[prashanthr05]

@nunoguedelha I do not understand why the Travis is failing now. Could you please check this?

[traversaro]

I am unable to reproduce locally, so I opened #116 to migrate Travis to Bionic and hopefully align the two systems.

[traversaro]

Can you rebase against the latest master?

[prashanthr05]

@traversaro done. Let's see what Travis says!

[prashanthr05]

@traversaro friendly ping..!

[traversaro]

I have no idea about why Travis is failing, probably there is strange interaction with the branch problems you were mentioning f2f. I pushed the branch in this repo, and I think we can open a new PR from that. Note that you have access to the repo, so you can easily manage the PR also from this new branch.