Compatibility with new yarp InteractionMode interfaces

[traversaro]

The new version of yarp will introduce some backward-incompatible changes on some control board interfaces (https://github.com/robotology/yarp/tree/InteractionMode). We should modify gazebo-yarp-plugins to work with the new interfaces.

For the transition time I guess we can make gazebo-yarp-plugins compatible with both the new and the old interfaces, by checking the yarp version and using conditional compilation. (For example: https://github.com/robotology-playground/yarp-wholebodyinterface/commit/6fd2a5e40911b8322ab2e7c73f78069fc1bca078).

[traversaro]

Initial support to compile against new IOpenLoopControl interface added in this commit: https://github.com/robotology/gazebo_yarp_plugins/commit/d67dbd29b3c3b1860c12f6a287de035020e36108 . Probably will be necessary to add other interfaces to the controlboard driver to comply to the new API.

[traversaro]

The new interaction mode API has been merged in master, so I am tackling quickly this issue.

[traversaro]

I guess the first thing is to implement IControlMode2 , because the remote controlboard maps the old IControlMode messages to IControlMode2 style message, but if the device does not implement IControlMode2 the switch fails.

[barbalberto]

Yae, the remotControlBoard maps all the calling to the new interface, but the ControlBoardWrapper, when it receives the iControlMode2 commands check if the interface is implemented will use the new one, otherwise it will fallback to the old interfaces whenever possible thaks to the code hereafter.

Therefore it should work as before even withou updating the plugins, can you give it a try?

Thanks, Alberto

virtual bool setControlMode(const int j, const int mode)
    {
        bool ret = true;
        int off=device.lut[j].offset;
        int subIndex=device.lut[j].deviceEntry;

        std::cout<< "New setControlMode SINGLE for joint " << j << " mode " << yarp::os::Vocab::decode(mode) << std::endl;

        yarp::dev::impl::SubDevice *p=device.getSubdevice(subIndex);
        if (!p)
            return false;

        if (p->iMode2)
             ret = p->iMode2->setControlMode(off+base, mode);
        else
        {
            if (p->iMode)
            {
                legacySetControlMode(j, mode);
            }
        }
        return ret;
    }

[traversaro]

Ok perfect!

[traversaro]

@barbalberto I can confirm it seems to work.

[traversaro]

By using a lot of #ifdef is possible to implement the new interactionMode interfaces while retaining compatibility with the old interfaces (and Yarp < 2.3.63 ). However for sake of maintainability, readability of the code (and to simplify implementation) I guess we can just implement the new interfaces and depend on Yarp >= 2.3.63 .

@MirkoFerrati @EnricoMingo @arocchi what is the walkman plan? You have already scheduled the switch to Yarp 2.3.63 ?

cc @lornat75

[MirkoFerrati]

We are willing to switch but we have not a detailed plan. I am more worried about the real robot compatibility. What's required by our control side to use the new InteractionMode?

[EnricoMingo]

What is the interaction mode?

[traversaro]

The interaction mode is the new semantics (with some new interfaces introduced to comply to this semantics) for the yarp controlboards. There is a nice document describing them and the changes with respect to the past interfaces, I have sent you a copy via email.

@barbalberto I guess that now that the new interaction mode is in master we can publish the document describing it?

[EnricoMingo]

If I understand is not mandatory to use it right?

[traversaro]

Yes, for now the new interfaces are backward compatible with the code written to use the old interfaces. @barbalberto can provided additional details.

[barbalberto]

@randaz81 as the updated version of the document, ask him and @lornat75 where to publish it. I think it would also be nice to send an email to mailinglist describing the changes.

@traversaro can you explain me better what those #ifdef are for? Once the new interface will be implemented, the wrapper will use the new one in place of the old one so the should be no need of 'strict' dependency from a specific yarp version.

@EnricoMingo the behaviour is almost 100% backward compatible", the only difference is that, if the robot is complaiant, the old setPositionMode will not bring the robot back to a stiff position control as it was meant to do before. This is not currently an issue on the coman because, if you remember, we disabled this transiction inside the comanInterface for issues with the real robot doing this transition. Btw, the only change we will have to do is substitute the call of setImpedancePositionMode with setInteractionMode(complaiant) and setPositionMode with setInteractionMode(stiff)

[traversaro]

@traversaro currently we depend on yarp minimum version 2.3.62, #ifdefs are necessary to continue to compile against yarp 2.3.62 using the old interfaces and against yarp 2.3.63 using the new interfaces.

As soon as walkman switches to 2.3.63 we can just depend on the new interfaces, and drop all the #ifdefs .

[traversaro]

@barbalberto started adding support for the new interaction modes, making the master branch compatible only with Yarp 2.3.63 or higher. This is ok for guys in CoDyCo/iCub and Walkman/Coman. What about Vizzy users @miaragao? Do you thinks is ok to depend on the latest yarp?

[mikearagao]

No problem for me. I was away in August and Vizzy simulation hasn't been used since then, I want to fix some stuff before others using it, so it's just me right now.

I have no problem keeping Yarp updated :wink:

[traversaro]

The specification for the new expected behaviour of the yarp motor interfaces is: http://wiki.icub.org/images/c/cf/ICub_Control_Modes_1_1.pdf

[traversaro]

Note to self on expected behaviour that I was not able to find:

• reset reference values on control mode switch (randaz told that for position is current position, while for velocity/torque/openloop is 0).
• what to do if one joint is out of bound in multi joints I*Control2 methods, see https://github.com/robotology/gazebo-yarp-plugins/pull/139#discussion_r18762287 .

[arocchi]

@liesrock spotted a nasty inconsistency in control mode/interaction mode in issue #146 From the document, every control mode except position, position direct and velocity don't have a meaningful interaction mode setting. I suppose the right thing to return the user when asking the interaction mode while control mode is torque or idle should be UNKNOWN Notice: we are spotting these errors thanks to the ongoing work a robotic unit test suite, the interested parties should contact @liesrock for details ;)

[traversaro]

@randaz81 to do a recap, the current policy to set the initialized the desired "quantity" in the low level control loop is:

Control Mode	Initial desired value
Position	Measured encoder value
Velocity	0
Torque	Measured torque value
OpenLoop	0

Ref : https://github.com/robotology/icub-main/issues/234#issuecomment-155485963 .

[traversaro]

As far as I know, the gazebo_yarp_controlboard is compatible with the interaction mode semantics. Any eventual lack of compliance can be discussed in a new issue.