VelocityControl mode not working on iCubGenova02, but working on iCubSim

[lunagava]

We are trying to control in velocity mode but it is not working with the iCub, whereas the same code works in Gazebo simulator. We are asking for someone next week (possibly ASAP) to check together with us the reason why it is happening.
We are wondering if some configuration files changed due to recent updates.

@sgiraz @mfussi66, can you help us?

cc @arrenglover @marco-monforte

[mfussi66]

Hi @lunagava ,
Asking some questions to better understand the scenario:

1. What are you using to control the robot? Is it the yarpmotorgui or a custom application?
2. If it's a custom application, can you post the code snippet a Gist that controls the robot in velocity mode? A minimal reproducible example would be ideal.

[lunagava]

Hi @mfussi66,

Thanks for the reply. I'm just using a custom code to control in velocity mode (VOCAB_CM_VELOCITY). I do not know if it is possible to send velocity commands in velocity control mode from yarpmotorgui.

I attach the Gist here https://gist.github.com/lunagava/44b9726c2210f8e905b972598d2c26e3, but as I already said the code works in simulation, so I think is a problem with the real robot iCubGenova02.

If you know someone that can help with this problem, please tag him/her.

[mfussi66]

• You are setting your remote as

option.put("remote","/icubSim/head");

It might be a trivial question, but are you sure that before running the code on the robot you replace the port with /icub/head ?

• Your mainmodule inherits both from RFModule and Thread. Usually for a simple control task just inheriting from RFmodule is sufficient, and inheriting from both is wrong. Is there a specific reason for this? @marco-monforte

[lunagava]

Yes, I'm pretty sure of putting icub/head when I controlled the real robot, otherwise it should have complained about not finding /icubSim/head since the simulator was not on during robot test.
To share the Gist, I just deleted all the things you may not be interested related to our algorithm to keep it simple for you to check. We needed the Thread for our vision algorithm computation, so if you want you can delete the run() method.

[pattacini]

Just for clarification, inheriting a class from both Thread and RFModule is not correct logically, as RFModule::updateModule() provides you with a periodic function called at a rate established by RFModule::getPeriod(). Plus, and more importantly, there'll be a clash between the methods of the two classes, especially upon closure, as we saw together with @marco-monforte.

My warm recommendation is to fix this asap.

---

If you need more threads, instead, the pattern still prescribes relying on RFModule for the main module so that you can have command-line options, smooth shutting down, RPC service, a background thread and then you can spawn internal thread objects¹ from within RFModule::configure() at will.

Footnotes

1. Thread objects may be of class yarp::os::Thread, yarp::os::PeriodicThread, or std::thread. ↩

[mfussi66]

Inspecting the issue F2F we found out that the velocity control needed to be explicitly enabled through the function RemoteControlBoard::enablePid(VOCAB_PIDTYPE_VELOCITY, jointNumber).

[mfussi66]

This operation might not be needed. In any case for future reference here is a tutorial on velocity control: https://github.com/vvv-school/tutorial_joint-interface/blob/master/src/velocity.cpp

[pattacini]

Actually, to use the velocity mode, one needs only to enable it through the setControlMode() service.

[lunagava]

We tested the velocity controller live on the robot together with @mfussi66 and the robot did not move. We tried to control in velocity mode from the yarpmotorgui (that is possible only when you tick enable velocity control option from the view menu) to see what was happening. We noticed that only the neck roll was moving is we set the value. After doing some tests, also pitch and yaw started working unexpectedly and we do not know why and how. Now, it is working also when you send the control command from the custom code.

Thanks again @mfussi66 for helping me to unblock the situation.

[pattacini]

It seems to me that it could be more a general failure of the robot per se. Maybe related to robotology/icub-tech-support#1373 (comment)?

However, all good that you managed to sort it out 👍🏻

[marco-monforte]

The two things happened at different times, but I agree. It might be all due to the robot's age, as @Uboldi80 was mentioning yesterday regarding the issue above 😕