search

robotology / gazebo-yarp-plugins

Plugins to interface Gazebo with YARP.
33 stars 48 forks source link

Reset World Doesn't Work properly if you expect that the robot starts in VOCAB_CM_POSITION_DIRECT #389

Open PeixinC opened 6 years ago

PeixinC commented 6 years ago

Hi, I am trying to do RL on an iCub, so there is a need for resetting the world. The first episode worked fine. The robot arm end effector moved from its initial position to another position say (xt, yt, zt). After the reset world command was given to the Gazebo, these two errors appeared:

[ERROR]gazebo_yarp_controlboard: you tried to call setPosition for a joint that is not in POSITION_DIRECT control mode. [ERROR]Error while trying to command a streaming position direct message on joint group

The end effector moved from its initial position to where it was reset, i.e. (xt,yt,zt), in an abnormal velocity. This happened most of the time after the reset world command. I am using Gazebo 9 with gazebo-yarp-plugins v2.3.72. I am not sure if this problem has been solved in gazebo-yarp-plugins v3.1.0. Is there any way that I can bypass this problem such as changing the version of Gazebo or gazebo-yarp-plugins.

Thanks in advance.

traversaro commented 6 years ago

[ERROR]gazebo_yarp_controlboard: you tried to call setPosition for a joint that is not in POSITION_DIRECT control mode. [ERROR]Error while trying to command a streaming position direct message on joint group

How are you controlling your robot? From these errors, it seems that the gazebo_yarp_controlboard after reset was reset to use the VOCAB_CM_POSITION control mode, but there is some control code that is still calling the yarp::dev::IPositionDirect::setPositions method, that can only work if the joint has been set in the VOCAB_CM_POSITION_DIRECT using the yarp::dev::IControlMode::SetControlMode method.

See this document for more info about the Control Modes: http://wiki.icub.org/images/c/cf/ICub_Control_Modes_1_1.pdf and this materials about the joint/motor control using YARP : https://github.com/vvv-school/vvv18/wiki/School-material#yarp-1 .

PeixinC commented 6 years ago

Thanks for the reply. I am using the iCub cartesian interface. According to this post about iCub "falling" in Gazebo: https://github.com/robotology/QA/issues/115, I changed the configuration for right arm cartesian control such that the PositionControl option was set to on to prevent the iCub falling issue. I think that's why the joint has been set in the VOCAB_CM_POSITION_DIRECT. So, is it possible that the gazebo_yarp_plugin resets to VOCAB_CM_POSITION_DIRECT control mode?

traversaro commented 6 years ago

Hi @PeixinC , sorry for the late reply. At the moment that is not possible. You can considering modifying the gazebo-yarp-plugins source code, or having a custom logic to change the control mode after the reset.

traversaro commented 6 years ago

To be clear: we would be happy to review a PR that introduces an option in gazebo_yarp_plugins to specify the initial/after reset control mode (that will default to VOCAB_CM_POSITION).

richardkxu commented 6 years ago

Thanks for the reply. I found that the error does not cause the problem I met. I just changed the source code such that the VOCAB_CM_POSITION is changed to VOCAB_CM_POSITION_DIRECT, which surpressed the error. I am developing the training environment for the iCub, and I think the reset function in the gazebo-yarp-plugins works fine for at least VOCAB_CM_POSITION and VOCAB_CM_POSITION_DIRECT control modes.

traversaro commented 6 years ago

Hi @richardkxu . If you understood correctly, having an option in the configuration file to specify the initial control mode will work for you, right?