Use qInit instead of qIn to initialize mc_rtc

[rafaelxero]

i.e. use target commands instead of current encoders values

In all the robots, every time that mc_rtc starts, there is a small sudden motion of the robot (a discontinuity)

This is because the behaviour prior to this commit is to use the current joint angles as desired angles.
However, by doing that, for an instant the torque required by the PD controller will be zero, and the required current will be zero.
Then, an error will have to be created to inject torque once again.
In PD control there is always steady state error.

What was done to solve this problem is to connect the previous command values to qInit and use these ones to initialize mc_rtc

[gergondet]

Hi @rafaelxero

Thanks for looking into this issue. It has affected the start for a while indeed. However it also means that the robot starting configuration include the steady state error from the PD right? It's probably not big enough to change anything though and in a sense the previous behavior also introduces a discontinuity between the initial controller state and the robot so this new behavior is probably much better...

Regarding the PR, can you:

1. Update the included sim_mc.py here
2. Make sure qInit has the correct size and fallback to qIn otherwise (this will be the case on some existing projects in the wild) with an mc_rtc::log::warning() to tell the users to update the Python script

Thanks

Edit: the build issue is not related to your change, it should resolves itself on the next push

[rafaelxero]

Hey @gergondet
We checked on HRP-4CR... before the fix there was a noticeable discontinuity, and after the fix there is not.
From the point of view of the iob, with the fix applied, nothing changes in the command when mc_rtc starts.

As with respect to the changes that you asked, I already performed them

However, I am getting Wrong formatting in src/MCControl.cpp during the check, do you know why?

[gergondet]

Hi @rafaelxero

Thanks for the fix. You can fix the formatting by running ./.clang-format-fix.sh in the repository. Please install clang-format-6.0 on Ubuntu as there might be subtle formatting changes between clang versions and this is the version that the CI checks with :(

[rafaelxero]

Hey @gergondet

Thank you for the suggestion... I applied it and now all checks have passed