Fix Control Board wrong joint limits for coupled joints

[xEnVrE]

This PR fixes #348

I had to move the private struct yarp::dev::GazeboYarpControlBoardDriver::Range in a separate header file as it is required both within class yarp::dev::GazeboYarpControlBoardDriver and in the signature of a new method in classes BaseCouplingHandler and derived.

cc @randaz81 @kouroshD @traversaro

[traversaro]

Thanks @xEnVrE ! This may be of interest also of @ale-git that has been developed a new coupling model for the iCub Hand Mk3 .

[traversaro]

As this is a breaking change, can you please target the branch devel and also update the https://github.com/robotology/gazebo-yarp-plugins/blob/devel/CHANGELOG.md ? Thanks!

[xEnVrE]

As this is a breaking change, can you please target the branch devel and also update the https://github.com/robotology/gazebo-yarp-plugins/blob/devel/CHANGELOG.md ? Thanks!

Sure!

[traversaro]

Small typo.
One thing I do not get, but probably I am missing something: if https://github.com/robotology/gazebo-yarp-plugins/pull/472/files#diff-a01901eeedeed61dae3fabb62c01e331R689 is just applying the existing position decoupling rules to the min and max vectors, can't we use the existing decouplePos function on two temporary min and max vectors to get the same result, without duplicating the decoupling position code in multiple methods?

[xEnVrE]

One thing I do not get, but probably I am missing something: if https://github.com/robotology/gazebo-yarp-plugins/pull/472/files#diff-a01901eeedeed61dae3fabb62c01e331R689 is just applying the existing position decoupling rules to the min and max vectors, can't we use the existing decouplePos function on two temporary min and max vectors to get the same result, without duplicating the decoupling position code in multiple methods?

I think it is doable. It was implemented with additional methods because that what was agreed in #348 and also to exploit the existing std::vector<Range>.

Of course, we can change as you suggested by extracting two temporary vectors from m_jointPosLimits, applying decouplePos and then copying values back.

[traversaro]

Of course, we can change as you suggested by extracting two temporary vectors from m_jointPosLimits, applying decouplePos and then copying values back.

I was thinking that probably it make sense to do so, instead of having a lot of decouplePosLimits that are not implemented, to be honest I had completly forgotten the discussion in #348 .

[xEnVrE]

I was thinking that probably it make sense to do so, instead of having a lot of decouplePosLimits that are not implemented, to be honest I had completely forgotten the discussion in #348 .

Fine with me. I will change it then.

[xEnVrE]

I was thinking that probably it make sense to do so, instead of having a lot of decouplePosLimits that are not implemented, to be honest I had completely forgotten the discussion in #348 .

Fine with me. I will change it then.

After some checks on the simulator, I am not sure that this solution will work for all the couplings. In particular, I think that there is something wrong with the handling of both the eyes and fingers abduction. However, it is not easy to understand what is going on since:

• the implementation of the coupling handlers of eyes and fingers abduction is not clear to me;
• the limits of physical joints (i.e. the one simulated inside Gazebo) seem wrong to me.

Hence, I propose to freeze this PR for now. Meanwhile I opened issues #473 and https://github.com/robotology/icub-gazebo/issues/75 to clarify these aspects.

[traversaro]

Hence, I propose to freeze this PR for now. Meanwhile I opened issues #473 and robotology/icub-gazebo#75 to clarify these aspects.

Great, thanks.

[traversaro]

I wonder if also this discussion is related: #324 .

[xEnVrE]

I wonder if also this discussion is related: #324 .

In this issue, at some point @triccyx says (taken from #324 (comment)):

At the end the file could be correct because it describes the limit of joint 4 and 5 that are Vs and Vg after the coupling line:
[COUPLING]
eyes_vergence_control (4 5) (eyes_version eyes_vergence)

where the file is the configuration file associated to the head, i.e. https://github.com/robotology/icub-gazebo/blob/master/icub/conf/gazebo_icub_head.ini

If we try to setup an automatic procedure to infer the limits for coupled joints, using the same logic that we use to move from the physical joint space to the controlled DoFs, i.e. using decouplePos as you were suggesting, we need to be sure that limits in the configuration files are always expressed in terms of physical joints.

As an example, for the index finger - check the configuration file - we have limits for the index proximal and for the two physical distal joints associated with the unique DoF that on the iCub robot is typically identified as {l, r}_index_distal. For the eyes, instead, limits are expressed in terms of tilt, version and vergence while, correct me If I am wrong, they should be expressed in terms of tilt, left_pan and right_pan in order to be coherent (I opened https://github.com/robotology/icub-gazebo/issues/75 for this).

An additional issue is that, given the special nature of the relationships linking version and vergence to left/right pan, I am not sure that applying decuplePos on the left/right pan limits would give us the correct limits for version and vergence.

[CLAassistant]

All committers have signed the CLA.

[xEnVrE]

One thing I do not get, but probably I am missing something: if https://github.com/robotology/gazebo-yarp-plugins/pull/472/files#diff-a01901eeedeed61dae3fabb62c01e331R689 is just applying the existing position decoupling rules to the min and max vectors, can't we use the existing decouplePos function on two temporary min and max vectors to get the same result, without duplicating the decoupling position code in multiple methods?

After some tests with eyes version and vergence is seems to me that is not possible to find joint limits by simply applying the decoupling rules to the min and max vectors. I think that with even more complicated coupling rules, e.g. the one implemented in #469, the outcome is the same, i.e. we need specific methods to handle limits decoupling.

@traversaro

[traversaro]

One thing I do not get, but probably I am missing something: if https://github.com/robotology/gazebo-yarp-plugins/pull/472/files#diff-a01901eeedeed61dae3fabb62c01e331R689 is just applying the existing position decoupling rules to the min and max vectors, can't we use the existing decouplePos function on two temporary min and max vectors to get the same result, without duplicating the decoupling position code in multiple methods?

After some tests with eyes version and vergence is seems to me that is not possible to find joint limits by simply applying the decoupling rules to the min and max vectors. I think that with even more complicated coupling rules, e.g. the one implemented in #469, the outcome is the same, i.e. we need specific methods to handle limits decoupling.

@traversaro

Yes, now that you wrote it explicitly it make totally sense, even for linear coupling it is not true that if (given $x \in \mathbb{R}^n$):

$$ x_{min} < x < x_{max} $$

then for every matrix $A \in \mathbb{R}^n$ you also have:
$$
A x_{min} < A x < A x_{max}
$$

in particular, this is true only if $A$ is composed just by positive coefficients. If $A$ is a general matrix, solving the inequality is more tricky (see https://www.jstor.org/stable/1967869 and https://math.stackexchange.com/questions/1310079/using-linear-algebra-e-g-matrix-methods-to-solve-a-system-of-linear-inequalit). I wonder how we are handling this at the firmware level on the iCub.

Given this, I guess we should take a step back, and try to understand in which way are are actually using joint limits. By doing a rough research ( https://github.com/robotology/gazebo-yarp-plugins/search?q=m_jointPosLimits&unscoped_q=m_jointPosLimits ) it seems that the only place in which the limits are used is in the clipping of the generated trajectories:

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

Line 223 in 0662953

if (m_xf > m_joint_max) m_xf = m_joint_max;

and then the limits are exposed to the downstream user:

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

Line 46 in 01ad1d1

bool GazeboYarpControlBoardDriver::getLimits(int axis, double *min, double *max) //WORKS

So, to be honest I do not remember if the trajectory generation is happening at the level of coupled quanties, or using the motor/physical joints (@xEnVrE if you can provide this info it would be great), so there are two cases:

• Joint trajectory generation is done with the coupled quantities: In that case everything works fine, and we should just clarify that the joint limits specified by the user in the .ini file refer to the coupled quantities (so version and vergence for the eyes, not left_pan and right_pan)
• Joint trajectory generation is done with the coupled quantities: in this case, we need both coupled limits and physical limits. This can be either an appropriate additional method in the coupling classes such as the one proposed in this PR, or the user could specify both the coupled and the physical limits, and we will trust him to provide coherent data.

[traversaro]

Joint trajectory generation is done with the coupled quantities: in this case, we need both coupled limits and physical limits. This can be either an appropriate additional method in the coupling classes such as the one proposed in this PR, or the user could specify both the coupled and the physical limits, and we will trust him to provide coherent data.

Actually that is much problematic then expected, as the solution for a system of inequalities:
$$
Ax > b
$$
do not always have a solution in the form:
$$
x_{min} < x < x_{max}
$$

so it is simply not possible to pass from one to another, unless you explicitly do not consider possible valid configuration.

For a quick example, think of:

$$ A x > 0 $$

with:

$$ A = \begin{bmatrix} 1 & 1 \\\ 1 & -1 \end{bmatrix} $$

and

$$ x = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} $$

The solution is:

$$ x_1 > 0 $$

$$ x_1 > x_2 $$

[xEnVrE]

Hi @traversaro,

thank you for the analysis!

You are totally right. In fact, for my experiments with eyes, I had to manually find max and mins for eyes left and right pan given limits on eyes vergence and version that we can take from robot hardware configurations. I then inserted the evaluated limits for pans on the configuration file of the plugin, while I had to put limits for eyes version and vergence inside the newly introduced EyesCouplingHandler::decouplePosLimits (of course, not the best solution as the plugin is, in an ideal world, decoupled from the configuration file of the robot is used with).

To recap:

• from my understanding, control is done using actual implemented joints on the model, e.g. for eyes left_pan and right_pan, while trajectories are generated for the coupled-joints space, e.g. version and vergence.
• limits inside the configuration files have to be in the implemented-joints space, because those configuration files always contained information (like PID, etc) for each actually implemented joint in the Gazebo model. That is way I was surprised to find that, see https://github.com/robotology/icub-gazebo/issues/75 and https://github.com/robotology/icub-gazebo/blob/14a078e49968c2130d64b8d5635d6f302035d67b/icub/conf/gazebo_icub_head.ini#L72-L75, limits for eyes were specified at version/vergence level instead of pan left / pan right
• limits must be converted to the coupled-joints space inside the controlboard plugin

Relative to the last point, trajectories limits are configured in two places:

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

Lines 383 to 386 in 81f7dc1

	yDebug() << "Initializing Trajectory Generator with default values";
	for (unsigned int i = 0; i < m_numberOfJoints; ++i) {
	m_trajectory_generator[i]->setLimits(m_jointPosLimits[i].min,m_jointPosLimits[i].max);
	m_trajectory_generator[i]->initTrajectory(m_positions[i],m_positions[i],m_trajectoryGenerationReferenceSpeed[i]);

and

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

Lines 383 to 386 in 81f7dc1

	yDebug() << "Initializing Trajectory Generator with default values";
	for (unsigned int i = 0; i < m_numberOfJoints; ++i) {
	m_trajectory_generator[i]->setLimits(m_jointPosLimits[i].min,m_jointPosLimits[i].max);
	m_trajectory_generator[i]->initTrajectory(m_positions[i],m_positions[i],m_trajectoryGenerationReferenceSpeed[i]);

That is way, in this PR, I proposed to modify method setMinMaxPos that is called here

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

Line 290 in 81f7dc1

if (!setMinMaxPos())

in order to modify quantities m_jointPosLimits so that they contain the right limits for coupled joints.

[xEnVrE]

or the user could specify both the coupled and the physical limits, and we will trust him to provide coherent data

I like more this idea than the solution implemented in this PR. This way, the behavior will depend only on the user data that is confined in the configuration file. Instead, at the moment, we might be obliged to have some of the limits in the configuration files and part of the logic in this plugin. Considering that the plugin and the configuration files are in different repositories, I don't really like this solution.

[traversaro]

Ok, thanks @xEnVrE , now everything is crystal clear to me. So the point is that the answer to my question in #472 (review) "can't we use the existing decouplePos function on two temporary min and max vectors to get the same result?" is simply "no", or more in detail: "no, because that works fine only for couplings for which all the coefficients are positive", right?

[xEnVrE]

Right!

Nevertheless, I am more for the solution where the user specifies both limits in the configuration file shipped within a specific repository containing models of a robot.

The best solution would be to follow the path you proposed in #470. This way, correct me If I am wrong, we can move code for coupling handlers inside the repository of robots models such that:

• coupling plugin loading mechanism is in gazebo-yarp-plugins
• configuration file and code for coupling specific to the robot are inside the repository of the robot and not in gazebo-yarp-plugins

[traversaro]

This way, correct me If I am wrong, we can move code for coupling handlers inside the repository of robots models such that:

Exactly, that was the idea. However I think that the modification to correctly handle the limits (either a combination of this PR + modification to load explicitly the coupled axis limits or simply just loading explicitly the coupled axis limits) can be done quite independently from that. If you are interested in working on that proposal and you need help feel free to let me know, thanks!

[traversaro]

@xEnVrE how do you prefer to proceed? We merge this PR for the time being, or you prefer to add instead a new parameter (probably under the [COUPLING] group) to specify the coupled joint limits directly?

[xEnVrE]

Sorry @traversaro, I had no time to handle this yesterday. I will address it next week and I will add new parameters for coupled joint limits.

[traversaro]

Sorry @traversaro, I had no time to handle this yesterday. I will address it next week and I will add new parameters for coupled joint limits.

No hurry, I just wanted to be sure I was not blocking anyone here.

[traversaro]

Hi @xEnVrE, just to understand, are you still interested in work on this in the short term?

[xEnVrE]

Hi @traversaro, definitively. If you want, just to clean things a bit, we can close this PR and I will open a new one since the solution we agreed on is different from the original one.

[traversaro]

Hi @traversaro, definitively. If you want, just to clean things a bit, we can close this PR and I will open a new one since the solution we agreed on is different from the original one.

Yes, probably this is a good idea!