libgazebo_yarp_controlboard.so crashes when joints not found

[arocchi]

with
gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:424: T* boost::shared_ptr::operator->() const [with T = gazebo::physics::Joint]: Assertion `px != 0' failed.

[MirkoFerrati]

Are you asking for a better error message before quitting or for a dynamic joint handler that will keep working even if a joint is not found?

[arocchi]

we should discuss about it. I personally think the implementation should be forgiving, print a big error message if it doesn't find a joint, and continue working.

[traversaro]

@arocchi +1

[MirkoFerrati]

Let's imagine what should happen in the real robot. If a low level board of a joint is turned off, will our algorithms and our yarp wrapper work? What happens under the hood? The yarp wrapper just ignores the missing joint?

[MirkoFerrati]

Anyway the check if a joint exists or not should be done inside GazeboYarpControlBoardDriver::setJointNames().
joint_names should NOT be resized, instead a push_back should be used to add values.
Inside the for loop after the joint_name initialization there should be something like:

if (_robot->getJoint(_robot->GetName()+"::"+joint_name)){
    joint_names.push_back( _robot->GetName()+"::"+joint_name);
}

[arocchi]

@MirkoFerrati regarding the behavior of the wrappers, I am quite sure robotInterface prints a lot of errors but continues working, in both cases when a board turns off mid-operation or when the wrappers are configured for a bigger number of joints (i.e., wrappers for whole body used just for the lower body)
I think I get your reply about joint_names not being resized, also the joint numbers should not be scaled I think (e.g., if you have 7 joints and the joint number 4 is not present, you should maintain a 7 joints structure, and when sending position commands/reading encoders/torques/velocities from a joint which is not present in the model, the plugin should print an error message, while the interfaces should somehow return useful return info, e.g. when doing a setPosition to a missing joint it should return false etc these details need to be evaluated carefully).
Since this modification could not be trivial, even though I'm sure it could be useful, let me put it into context to help prioritize: if we load the control board plugins from a world file, and the model changes (e.g. you disable the hands) then you can't use the same world files anymoree.
Concretely, look at https://github.com/EnricoMingo/iit-coman-ros-pkg/issues/23 : this particular problem can have different/simpler solution, still I think having this functionality in gazebo_yarp_plugins is needed for robustness and ease of use reasons.
What do you think? @MirkoFerrati, you think that just that modification you proposed would solve the problem? How much work is for the MVP (minimum viable product) that does not crash and continues working with unexpectedly missing joints?

[MirkoFerrati]

Ok, i read the code again, my proposed fix was only capable of checking if a joint existed or not, but the plugin would have crashed as soon as the yarp interface used a setPosition on the missing joint.
Now, to create a forgiving implementation WITHOUT changing every single method of the controlboard we need a fake joint inside gazebo, let's name it "fake_joint".
Whenever we can't find a joint j in the robot, we set joint_names[j]="fake_joint".
Doing this, we will redirect all the PositionMove(j), getEncoder(j) and so on to the same joint "fake_joint", which will be set as fixed, returning always zero position and ignoring any jointCmd.
In this way, all the implemented yarp functions will keep working WITHOUT the need to check if a joint exists or not.
To summarize:
A fake joint will keep the code simple, and will avoid lot of checks every single simulation step, thus leaving the plugin as fast as now.

[traversaro]

Ok, I did not read carefully this discussion before. I agree that the controlboard plugin should be robust and never crash Gazebo. My naive position was that we should make a initial check that the joints managed by the controlboard are available in gazebo model, and if the joints are not available the driver should not load (i.e. it open() method return false and an error message is printed).
As you may imagine my opinion is that controlboard plugins should be part of the robot model, not the world, and thus if different models of the robot are generated (for example with/without hands) the appropriate coherent configuration files should be generated with the same process. This would permit also to have the different models available for loading at the same time ( I have to deal with something similar with the different versions of iCub).

[arocchi]

@traversaro it would be nice, but what about the initial configuration? Having a different initial conf for each world file is useful, and using control to set an initial configuration requires for some kind of virtual crane. Am I missing something?

[traversaro]

@arocchi the definition of a initial configuration in the world is definitely a well-defined feature. I'll post my thoughts on it in the afternoon.

[traversaro]

In 65e62f7 there is a fix for crash when not finding a joint name (disabling all the controlboard if a joint names is not found). I guess I can close this issue, if we want to have a more complex behaviour in case of joint name not found we can discuss it in a dedicated enhancement issue.

[arocchi]

ok!