Implement limits for coupled joints

[xEnVrE]

Limits for coupled joints

This PR aims at adding support for limits for joints having coupling within the controlboard plugin.

A bit of context: at the moment, a sample configuration file of a part to be controlled with the controlboard is as follows

(...)

[WRAPPER]

joints 6

(...)

[COUPLING]
name_coupling_handler (3 4 5) (name_3 name_4 reserved)

(...)

[LIMITS]
jntPosMax max_0 max_1 max_2 max_3 max_4 max_5
jntPosMin min_0 min_1 min_2 min_3 min_4 min_5

(...)

In this example there are 6 joints. Joints with indexes from 3 to 5 are coupled and the user can only control the joint name_3 and name_4 which couples joints 4 and 5 (hence the string reserved).

Limits are specified in the section LIMITS and correctly used within the controlboard in some parts, e.g., to enforce torque limits in

gazebo-yarp-plugins/plugins/controlboard/src/ControlBoardDriver.cpp

Line 1382 in 927874b

bool GazeboYarpControlBoardDriver::check_joint_within_limits_override_torque(int i, double& ref)

The same limits are used to specify the boundaries of the trajectory generators and to implement ControlBoardWrapper::getLimits. However, the limits required in these two features, corresponding to the DoFs that the user can effectively control, are different from the limits indicated in [LIMITS] if some joints are coupled. Indeed, trajectories are not generated for all the joints actually controlled by Gazebo but only for their coupled counterparts and then the trajectories are converted according to the behavior coded in the name_coupling_handler code.

This PR proposes to change the [COUPLING] section of the configuration file to

(...)

[COUPLING]
name_coupling_handler (3 4 5) (name_3 name_4 reserved) (min_3 min_4) (max_3 max_4)

(...)

I.e. the user needs to specify the limits for the DoF that they can actually control excluding those indicated as reserved. For the remaining joints, that are not part of the coupling section, the limits specified in [LIMITS] are used as in the past. The reasons why limits must be specified by the user and cannot be evaluated automatically can be find in the closed PR #472, #472 (comment)

In order to simplify the review, a summary of the changes required to implement this behavior:

• the struct Range is moved from ControlBoardDriver.h to ControlBoardDriverRange.h so that it is available in other parts of the code
• the base class BaseCouplingHandler accepts a new mandatory parameter containing the limits of the coupled joints
• method GazeboYarpControlBoardDriver::isValidUserDOF is implemented in order to clearly identify DoFs for which a trajectory generator has to be configured (i.e. to skip useless computation for reserved joints)
• methods GazeboYarpControlBoardDriver::{set, get}UserDOFLimit are implemented in order to set/get the right limit for each joint, being it normal or coupled and avoiding glue code
• limits for coupled joints are loaded in GazeboYarpControlBoardDriver::gazebo_init
• correct limits are now employed in GazeboYarpControlBoardDriver::{resetPositionsAndTrajectoryGenerators, changeControlMode, changeInteractionMode, setLimits, getLimits, positionMove}
• the CHANGELOG still needs to be changed (waiting for the review of the PR)

In order to implement these changes, it is required that configuration files in https://github.com/robotology/icub-gazebo/tree/master/icub/conf, https://github.com/robotology/icub-models/tree/master/iCub/conf and https://github.com/robotology/cer-sim/tree/master/gazebo/cer/conf are reviewed. Other robots like https://github.com/vislab-tecnico-lisboa/vizzy might be involved as well.

In some parts of the code I had to limit the implementation with the assumption that at most one coupling handler is available. This is why it is not clear to me/I think it is better to discuss how to handle limits for coupled joints if more than one handler per controlled part is present. By looking at the configuration files I only found multiple coupling handlers in https://github.com/robotology/icub-gazebo/blob/master/icub/conf/gazebo_icub_left_hand_fingers.ini. However, this is not used in any of the model as the solution of having multiple plugins for each finger has been preferred over that of having of a single plugin for all the fingers.

New behavior of coupling handler FingersAbductionCouplingHandler

Within this PR the behavior of the coupling handler FingersAbductionCouplingHandler is changed in order to reflect that the , on the real robot, abduction is minimum when the associated DoF is commanded to 60.0 and maximum when commanded to 0.0 (while, at the moment, the reversed behavior is implemented).

Fixes #348 #474

[traversaro]

Thanks a lot @xEnVrE !

The CI is currently failing, but this will be fixed by #500 .

I went through the code and it seems to be fine, I have two major comments:

• You use reserved and invalid as joint names that it is not possible to use because they have a specific meaning, for this kind of use typically it is a good idea to prefix this "magic" identifiers with a project-specific prefix to avoid conflicts with other uses. At a first glance this is not easy for reserved that is actually already used in models, but if possible I suggest to rename at least invalid to gyp_invalid or something similar.
• In general, I would prefer to avoid breaking changes. Do you think it is feasible to change the logic in https://github.com/robotology/gazebo-yarp-plugins/pull/499/files#diff-a01901eeedeed61dae3fabb62c01e331L195 to check if the numbers of elements of the bottle is 3, and if that is the case print a warning and use as coupled joints limits the limits passed in [LIMITS] (that more and less the current behavior)? Then in gazebo-yarp-plugins 4 we can then remove this deprecated behavior.

Other robots like https://github.com/vislab-tecnico-lisboa/vizzy might be involved as well.

That is a good point, thanks for bringing it up. I guess @plinioMoreno may be interested on this, and I think it is a good reason to keep backward compatibility, if possible.

New behavior of coupling handler FingersAbductionCouplingHandler

Within this PR the behavior of the coupling handler FingersAbductionCouplingHandler is changed in order to reflect that the , on the real robot, abduction is minimum when the associated DoF is commanded to 60.0 and maximum when commanded to 0.0 (while, at the moment, the reversed behavior is implemented).

Is this related to #474 ? Where did you got/how did you obtained the formula that are implemented now?

[xEnVrE]

but if possible I suggest to rename at least invalid to gyp_invalid or something similar.

I don't really like the mechanism exploiting these words that I implemented since, while the invalid keyword was explicitly used in the code of the base coupling handler, the name reserved is not used in the code anywhere but it is silently used in the configuration files as you said. At the end of the story we should probably document it somewhere.

That said, I will rename to gyp_invalid as you suggested.

check if the numbers of elements of the bottle is 3, and if that is the case print a warning and use as coupled joints limits the limits passed in [LIMITS] (that more and less the current behavior)?

I think that is doable and will remove the necessity to update all the configuration files. I'll update the PR. It will require some more logic in the methods *UserDOF* because I will need to check, in the case where a coupling handler is found, if the limits for the handler have been loaded or not. Probably the BaseCouplingHandler is again the right place for this kind of logic.

Is this related to #474 ? Where did you got/how did you obtained the formula that are implemented now?

Yes it is related to that issue. the way I got the formula was by reasoning on it a bit and I will try to explain here.

The joint Hand finger adduction/abduction controls the adduction/abduction of the fingers and, on a real iCub, tipically ranges from 0 to 60 degrees. When it is 0, there is maximum abduction and, viceversa, when it is near 60 there is maximum adduction (i.e. fingers are each close one to the other).

From the actuation standpoint, on the real robot, I think that there is only one tendon which is moved by a motor that determines the overall motion (I might be wrong). What I am sure of is that the middle finger is not involved in this motion, i.e. only the index, ring and little move.

On Gazebo, instead, there are 4 joints to be controlled, namely l_hand_index_0_joint, l_hand_middle_0_joint, l_hand_ring_0_joint and l_hand_little_0_joint. As we said, the middle joint does not move (hence out[m_coupledJoints[1]] = 0.0).

yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
   (...)
    out[m_coupledJoints[0]] = 20.0 - pos_ref[m_coupledJoints[0]]/3;
    out[m_coupledJoints[1]] = 0.0;
    out[m_coupledJoints[2]] = 20.0 - pos_ref[m_coupledJoints[0]]/3;
    out[m_coupledJoints[3]] = 20.0 - pos_ref[m_coupledJoints[0]]/3;
    return out;
}

Then, if we assume that pos_ref[0] contains the trajectory of the Hand finger adduction/abduction DoF, the only one the user can control, we now that it will span the range [0, 60]. This motion has to be shared among three joints which means that we can make the assumption that each joint moves by 1/3 of the overall motion. This choice is arbitrary but also used in iKinFwd (see here).

Finally, in order to have maximum abduction for 0, and maximum adduction for 60, we need to add a 60 / 3 = 20 as an offset and subtract pos_ref[0]/3 from it.

One thing that I don't like is that the offset is hardcoded in the code. In case we want to simulate a different maximum value than 60, the user should be able to decide it somehow. But I think I can take it from the coupling handler itself, now that we added support for limits inside of it.

Moving from the Gazebo feedback to the trajectory feedback is done here

bool FingersAbductionCouplingHandler::decouplePos (yarp::sig::Vector& current_pos)
{
    if (m_coupledJoints.size()!=m_couplingSize) return false;
    current_pos[m_coupledJoints[0]] = (20.0 - current_pos[m_coupledJoints[0]])*3;
    return true;
}

We use the motion of one of the finger to determine the value that the adduction/abduction DoF should have. This is similar to the old behavior

gazebo-yarp-plugins/plugins/controlboard/src/ControlBoardDriverCoupling.cpp

Line 430 in 927874b

current_pos[m_coupledJoints[0]] = current_pos[m_coupledJoints[3]];

except for the offset and for the fact that a different finger was used.

I want to recall that the loop is closed around each joint singularly using the values provided by decoupleRefPos and not using the value provided by decouplePos that is at the trajectory level but that would not take into account the actual motion of the other two fingers.

[traversaro]

It will require some more logic in the methods UserDOF because I will need to check, in the case where a coupling handler is found, if the limits for the handler have been loaded or not. Probably the BaseCouplingHandler is again the right place for this kind of logic.

I was thinking to just set the limits for the handler to the physical limits if the conf file is the old one, so that all the legacy logic is just concentrated in the parser and it is easier to eventually remove it.

[traversaro]

Yes it is related to that issue. the way I got the formula was by reasoning on it a bit and I will try to explain here.

Thanks, this is much more clear.

[traversaro]

fyi @ale-git @randaz81 @kouroshD

[xEnVrE]

I was thinking to just set the limits for the handler to the physical limits if the conf file is the old one, so that all the legacy logic is just concentrated in the parser and it is easier to eventually remove it.

@traversaro I checked if this is doable and we can do it only if I move the code for the coupling handler initialization a little bit forward within the method gazebo_init, otherwise the limits from the [LIMITS] are not ready yet. If this is ok, I will proceed with this solution. I verified that this move should not invalidate any assumption in the initialization flow.

[traversaro]

I was thinking to just set the limits for the handler to the physical limits if the conf file is the old one, so that all the legacy logic is just concentrated in the parser and it is easier to eventually remove it.

@traversaro I checked if this is doable and we can do it only if I move the code for the coupling handler initialization a little bit forward within the method gazebo_init, otherwise the limits from the [LIMITS] are not ready yet. If this is ok, I will proceed with this solution. I verified that this move should not invalidate any assumption in the initialization flow.

Ok!

[xEnVrE]

I addressed the remaining changes.

However, i tested the new abduction handler with the "old" icub-gazebo model with hands and is not working properly because of a mismatch with the joint axes direction (basically I need to understand which is the right one on the real robot in order to understand if the model in icub-gazebo is right or wrong).

What is happening is that, for positive angles, the joint associated to the index joint is moving in what seems to be the wrong direction. I would say we can discuss this f2f.

Hence, I am going to remove the changes to abduction coupling handler from this PR in order to address it separately in another one.

[xEnVrE]

After rebasing to the latest devel, I had also to adapt the new coupling handler proposed in #469 in order to account for the limits. Before merging this PR i think it would be safer to test it against

• the model of Mk3 hand (but I do not have access to the URDF at the moment because it is private)
• the R1 simulated model which contains some coupling in the hand

and, if required, add the proper limits in configuration files (this also holds for models in icub-gazebo).

Regarding the finger abduction, I concluded that whatever model we use, the handling of the abduction for the index finger is "problematic". Indeed

• considering how the lines of the axes are configured, see left hand and right hand
• the fact that we have a single coupling handler for the abduction of both the left and the right hand (same code)

the only way to have a proper motion for both hands is to invert the axis direction of the index abduction joint of the left hand OR of the right hand in the sdf. This solution was already applied to the model in icub-gazebo, hence I adapted the code of the new abduction coupling handler to satisfy this constraint. I will change the new model from icub-models that I equipped with hands, and that is not available upstream at the moment, so that it is compatible too with the same code.

If there are no other changes to be done, I will update the changelog with a last commit.

@traversaro

[traversaro]

the fact that we have a single coupling handler for the abduction of both the left and the right hand (same code)

Note that if it is the best solution, we can also add two separate coupling handlers for left and right, however the solution that you proposed seems to be fine for me.

If there are no other changes to be done, I will update the changelog with a last commit.

Thanks, I think you can update the CHANGELOG, thanks!

the R1 simulated model which contains some coupling in the hand

I will mention a few users that may be interested on this: @randaz81 @vvasco @miccol

the model of Mk3 hand (but I do not have access to the URDF at the moment because it is private)

It is not directly the URDF, but FYI the generation logic is being added in the PR https://github.com/robotology/icub-model-generator/pull/132/files .

Before merging this PR i think it would be safer to test it against

As we are merging in devel, I would go ahead, and if any problem arise it can be reported and fixed anyhow before the release.

[traversaro]

@xEnVrE I have updated the previous comment.

[xEnVrE]

Note that if it is the best solution, we can also add two separate coupling handlers for left and right, however the solution that you proposed seems to be fine for me.

the thing that I really don't like is that if we arbitrarily change the direction of an axis then, when using libraries like iDynTree for making computations along the chain, say up to a frame on the finger, you need to fix the sign of the encoder reading that you might obtain from the encoders readings preprocessed with icub-main getChainJoints methods. I think we can discuss this f2f.

Still, I think it is ok to stick with one handler only and extend the behavior in the future (like two handlers or one handler with a parameter for left/right behavior AND not reversing the axis direction on the SDF).

[xEnVrE]

Thanks, I think you can update the CHANGELOG, thanks!

Done!

[traversaro]

Thanks @xEnVrE !