[skinDynLib] Add linkName and frameName to dynContact and skinContact, and forceTorqueEstimateConfidence to skinContact

[traversaro]

Quoting from the release notes:

Add linkName and frameName attributes to iCub::skinDynLib::dynContact and iCub::skinDynLib::skinContact, to simplify interfacing with software in which link and frames are identified by names, such as software that load the robot description from file formats like URDF and SDF. This attributes have been also added to the on-wire representation of this class, creating an incompatibility between versions of iCub before 1.8 and version after 1.10 , meaning that a module compiled with iCub 1.8 will not be able to communicate with a module compiled with iCub 1.10 . Recompiling all your software with iCub 1.10 (without any modification to the source code) should be sufficient to ensure that everything works fine.

The main goal of this PR is to simplify interfacing the contact information published by the skinManager and by wholeBodyDynamics with software that loads its robot representation from URDF files. For example, it would be useful to write a RViz plugins that visualize contact forces. Furthermore, it is a first step towards make skinDynLib useful also for non-iCub robots, as R1 .
It will also help to get rid of the horrible IDYNTREE_SKINDYNLIB_LINKS (see https://github.com/robotology/codyco-modules/blob/7b92d1d915b7c5497ff516be02d5f5ec54281f72/src/devices/wholeBodyDynamics/app/wholebodydynamics-icub-eth-six-fts.xml#L15) configuration block, by moving this information directly in the skinManager, to have a single source of truth ( https://en.wikipedia.org/wiki/Single_source_of_truth ) .

Quick benchmarks on my iMac show that the performance hit using an example linkName = r_upper_forearm and frameName = r_upper_forearm_dh_frame (worse w.r.t. string length than the current worse scenario for the iCub model) show a performance hit of ~1 us for each skinContact deserialized from a ConnectionReader. This performance hit is not negligible, but I think it is worth the benefit (the amount of additional data trasmitted on write in this "bad" case is ~50 bytes over ~200 bytes of the current skinContact message). Furthermore, I think this hit can be reduced by improving the implementation of the ConnectionReader, but it is not currently in my scope.

[traversaro]

Added support for publishing such information in the skinManager : 410705d .

[traversaro]

cc @joankangro

[traversaro]

Hint: the PR is much more comprehensible if you read it commit by commit.

[diegoferigo]

I see this very often, please append always a new line at the and of the files.

[traversaro]

Fixed. If you see it often, I think this is due to the lack of proper tooling to insert the final new line/check the missing new line in files. Any suggestions?

[diegoferigo]

It's editor specific, so it depends what people use.

[alecive]

Every editor has that option (even gedit I think).

[diegoferigo]

Why not using chrono? It should be the preferred way in C++.

@traversaro I tag you because I commented on an outdated commit and github hides it, but it is still valid

[traversaro]

Correct me if I am wrong, but I was unable to find an equivalent to std::clock in chrono.

[traversaro]

@diegoferigo

[diegoferigo]

Am I wrong or going to a newline in markdown has no effect? I think that writing paragraphs on a single line is the best choice, and new line behavior should be handled by the soft wrap configuration of the editor the user uses.

[traversaro]

There are several reasons why I prefer to break paragraph in raw Markdown, even if this has no effect on the rendered document:

• Small lines simplify the usage of git, as merging, diffing, conflicts, etc etc is all managed at line (in the raw document) level
• One of the main features of Markdown is to be quite readable even when it not rendered, and (at least in my case) I typically edit Markdown parallel with source code, meaning that no soft wrap is enabled in the editor.

[fjandrad]

Correct me if I'm wrong link will be the one that matches the urdf so also match idyntree, while frame will remain the one from skindynlib.
So even if the we can get rid off "idyntreee_skindynlib_links" skindynlibConversions will remain since the transform from the frame of the skin to the link is still required only the funcion skinDynLib2iDynTree wont be required

[traversaro]

Exactly, we still need to convert the quantities in the appropriate frame, but all the information to perform such processing will be stored in the skinContact itself.

[traversaro]

@diegoferigo Requested changes done.

[traversaro]

Furthermore, to avoid having multiple mutually incompatible on wire serializations of skinContact around, I also included the forceTorqueEstimateConfidence in skinContact in this PR (see 2f370f6).
Relative commit message:
Add a new integer attribute (that is correctly transmitted over the network)
that represents the confidence on the force/torque estimate trasmitted
with the skinContact . The actual meaning is implementation defined, but
the idea that more precise estimation (for example estimation using calibrated
skin) should fill this parameter with a greater integer w.r.t. rough estimations
of the contact force.

[traversaro]

I tested the PR on the robot.
The code works fine, but for some reason it is not possible to read the port on which skinContactList is published with yarp read or using the yarpdatadumper . I guess there is something wrong in the serialization code dealing with "text" mode, but I will need more time to understand what is going on.
cc @fjandrad

[traversaro]

Conversion to bottle (necessary for yarp read/yarpdatadumper support) working fine after fdeacb9 . The commits for setting the linkName and frameName in the skinManager disappear, even if I remember coding them. : /

[francesco-romano]

regarding the old discussion with @diegoferigo on ctime vs chrono
The chrono code is a bit trickier.

auto start = std::chrono::system_clock::now();

/* do some work */

auto end = std::chrono::system_clock::now();
auto elapsed =
    std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
std::cout << elapsed.count() << '\n';

(and I would try with high_resolution_clock instead of system_clock)

[traversaro]

I don't think they are equivalent. std::chrono::system_clock::now() is the wall time, while std::clock() is the process time.

[francesco-romano]

Now I understand what you mean.
Well, it depends then on what you want to measure. If you want to measure the CPU time, that you have to stick to the old clock library.

[traversaro]

Finally ready for review. I have updated the iCubGui and skinManager to use the new feature, using the names documented in wiki.icub.org/wiki/ICub_Model_naming_conventions after the discussions in #124 .

According to https://github.com/robotology/robotology.github.io/wiki/Robotology-Maintainers @pattacini is the maintainer for the libraries and modules and @vtikha is the maintainer for the modules and simulators. Let me know if you trust me on this one so I can proceed with the merge.

[alecive]

Btw your pull request should not be accepted if only because of the name you gave to your branch 😎

[vtikha]

@traversaro, you're good to go, i trust you also on this one and i like the name of the branch!

[traversaro]

@matejhof This will render the new version of skinDynLib-based tools incompatible with old dumps of the skin_events:o port, do you think it would be a problem?

[traversaro]

It would be good to merge this PR before the new release, to get a bit of testing. @fjandrad can you find the time today to test this PR on the robot and check that the skinGui is working fine, so that we can merge it? Thanks a lot.

[francesco-romano]

@traversaro before or after the new release?
If we want to get a bit of testing, then we have to merge this after the release.
If it has already been tested, then before the release.

[traversaro]

I tested it on iCubGenova04, and to start using it in codyco-modules it needs to be part of icub-main master (i.e. it needs to be merged before the release). I think it is ready for getting being merged, but I wanted to test it one more time now before merging.

[fjandrad]

What happen to this pull request? @traversaro ?

[traversaro]

Let's try once again on the robot and then let's merge it.

[fjandrad]

@traversaro we should test this again I guess.

[CLAassistant]

All committers have signed the CLA.

[pattacini]

Very old development never merged. I believe we managed to survive without.
I'm in favor of closing unless there are clear reasons to keep it open.

[traversaro]

Let's close it, if we need it we can re open it.