iCub eyes version/vergence coupling handlers

[xEnVrE]

I am trying to understand the code of the coupling handlers for the eyes.

From physical joints space to controlled DoFs space

decouplePos should move from the physical joint space (i.e. left and right eyes) to the controlled DoFs space (i.e.eyes-version and eyes-vergence).

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

Lines 77 to 84 in 8c056e2

	bool EyesCouplingHandler::decouplePos (yarp::sig::Vector& current_pos)
	{
	if (m_coupledJoints.size() != m_couplingSize) return false;
	double temp = current_pos[m_coupledJoints[0]];
	current_pos[m_coupledJoints[0]] = temp + current_pos[m_coupledJoints[1]];
	current_pos[m_coupledJoints[1]] = temp - current_pos[m_coupledJoints[1]];
	return true;
	}

Then, shouldn't it be something like this?

// version = (left + right)  / 2.0
// vergence = left - right
 double left = current_pos[m_coupledJoints[0]]; 
 double right =  current_pos[m_coupledJoints[1]]; 
 current_pos[m_coupledJoints[0]] = (left + right) / 2.0; 
 current_pos[m_coupledJoints[1]] = left - right; 

From controlled DoFs space to physical joints space

decoupleRefPos should move from the controlled DoFs space (i.e.eyes-version and eyes-vergence) to the physical joint space (i.e. left and right eyes)

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

Lines 110 to 117 in 8c056e2

	yarp::sig::Vector EyesCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
	{
	yarp::sig::Vector out = pos_ref;
	if (m_coupledJoints.size() != m_couplingSize) {yError() << "EyesCouplingHandler: Invalid coupling vector"; return out;}
	out[m_coupledJoints[0]] = (pos_ref[m_coupledJoints[0]] + pos_ref[m_coupledJoints[1]])/2;
	out[m_coupledJoints[1]] = (pos_ref[m_coupledJoints[0]] - pos_ref[m_coupledJoints[1]])/2;
	return out;
	}

Then, shouldn't it be something like this?

// left   = version + vergence / 2.0
// right = version - vergence / 2.0
out[m_coupledJoints[0]] = pos_ref[m_coupledJoints[0]] + pos_ref[m_coupledJoints[1]]/2; 
out[m_coupledJoints[1]] = pos_ref[m_coupledJoints[0]] - pos_ref[m_coupledJoints[1]]/2; 

Summary

To summarize, the plugin is implementing these relationships

version = left + right
vergence = left - right

left = (version + vergence)/2
right = (version - vergence)/2

while, according to this documentation, I was expecting

version = (left + right) / 2
vergence = left - right

left = version + (vergence / 2)
right = version - (vergence / 2)

[xEnVrE]

cc @traversaro

[traversaro]

Thanks a lot @xEnVrE for the clear explanation.

I wonder if @randaz81 or @aerydna remember anything related to this, however it may be tricky because the development dates back to approximately four years ago. @pattacini may also want to weigh in on this, clearly with absolutely low priority when he has time given the current situation.

In any case, I try to summarize what you report in a more compact way (correct me if I am wrong):

Eyes

It seems that gazebo-yarp-plugins' EyesCouplingHandler defines the version (in the sense that with definition the existingdecouplePos and decoupleRefPos are coherent) as :

version = left + right

instead of the definition used for example in robotology/community#158 and http://wiki.icub.org/wiki/Vergence,_Version_and_Disparity of:

version = (left + right)/2

[traversaro]

cc @kouroshD

[pattacini]

I can confirm that the law implemented in the gaze controller's code is:

version = (left + right)/2

as per our documentation.
I'm also reasonably confident that the FW copies that as well.

[traversaro]

I dug a bit in old issues, and I found the original issue related to eyes support in #149 (comment) . In that issue, apparently I am the one that reported the definition that is currently implemented, without the division by two:

eyes_version    =  left_eye_pan + right_eye_pan

Even if I was reporting the the Wiki page that contained the version = (left + right)/2, I apparently disregarded it on the basic of what was actually implemented in the firmware, i.e. quoting the original issue:

as you can check directly in the iCub firmware source code in firmware/motorControllerDsp56f807/position_decoupling.c, line 25 or in firmware/motorControllerDsp56f807/pwm_decoupling.c, line 83

Interestingly, the relative line of code in the firmware have not been modified in the meanwhile, see:

• https://github.com/robotology/icub-firmware/blob/fcace9ed15ad99b3a8da656d1782ab3a19c7f7f9/motorControllerDsp56f807/common_source_code/position_decoupling.c#L25
• https://github.com/robotology/icub-firmware/blob/fcace9ed15ad99b3a8da656d1782ab3a19c7f7f9/motorControllerDsp56f807/common_source_code/pwm_decoupling.c#L82

In particular, the "version" numbers are documented in https://github.com/robotology/icub-firmware/blob/fcace9ed15ad99b3a8da656d1782ab3a19c7f7f9/motorControllerDsp56f807/4DC/source_code/include/options.h#L9 , and the one related to the eyes (0x0215 and 0x0115) seems both to use the formula eyes_version = left_eye_pan + right_eye_pan . So unless I am missing something in the firmware (that is perfectly possible) it seems that the gazebo-yarp-plugins' EyesCouplingHandler is coherent with the iCub firmware behaviour.

.

PS: @xEnVrE given the complexity of the issue, probably it could make sense to move the discussion on the finger abduction to a separate issue to simplify discussions.

[xEnVrE]

PS: @xEnVrE given the complexity of the issue, probably it could make sense to move the discussion on the finger abduction to a separate issue to simplify discussions.

Done in #474. Can we move your comment related the finger abduction in the new issue? Thanks.

[pattacini]

Eh! If this is true, that's a very good catch then @traversaro
I've never browsed the FW myself but I clearly remember discussions that gravitated around our main Wiki resource touching on the requirements on the FW as well.

Anyway, let me tone down my assertion above: I can confirm that the gaze controller implements the law with the 2. Now wondering what could be happening with this mismatch between the high-level and the low-level policies. Perhaps the closed-loop is saving our day?

[xEnVrE]

I updated the first comment of the issue, so that my doubts are explained more clearly.

[traversaro]

I think I also found the decoupling code also for ETH-based boards, but also on that case the coupling assumed is eyes_version = left_eye_pan + right_eye_pan, see:
https://github.com/robotology/icub-firmware/blob/v1.15.0/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c#L1127 .

[traversaro]

Ok, last stage: I think the coupling information is now stored in configuration files, but even there the assumed version coupling seems to be eyes_version = left_eye_pan + right_eye_pan :
https://github.com/robotology/robots-configuration/blob/v1.15.0/iCubGenova02/hardware/mechanicals/head-eb21-j2_5-mec.xml#L34

[xEnVrE]

Can this be closed?

[traversaro]

If they work fine, ok for me!