Coupling handlers for ergoCub hand MK3, 4 and 5 possibly incorrect

[xEnVrE]

Historically, as far as I know, coupling handlers within the controlboard plugin were designed to operate as follows:

• the decouple* methods takes the state of physical joints simulated in Gazebo and reconstruct the state of the DoFs that the user can actually control YARP-side
• the decoupleRef* methods take the desired reference for the DoFs YARP-side and convert it to references, one for each physical joint, simulated in Gazebo

As an example, for the thumb distal joints of iCub2.x we get:

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

Lines 183 to 188 in 4b35f02

	bool ThumbCouplingHandler::decouplePos (yarp::sig::Vector& current_pos)
	{
	if (m_coupledJoints.size() != m_couplingSize) return false;
	current_pos[m_coupledJoints[2]] = current_pos[m_coupledJoints[2]] + current_pos[m_coupledJoints[3]];
	return true;
	}

i.e., the l_thumb_distal YARP-side is reconstructed as the sum of the two physical joints l_hand_thumb_1_joint and l_hand_thumb_2_joint, where I am using the names coming from the iCub joints nomenclature and those from the URDF config files, respectively.

Then, for the second method we have:

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

Lines 210 to 219 in 4b35f02

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

i.e.,

l_thumb_0_joint = l_thumb_oppose
l_thumb_1_joint = l_thumb_proximal
l_thumb_2_joint = l_thumb_distal / 2
l_thumb_3_joint = l_thumb_distal / 2

Let's now have a look at the implementation of the coupling handler for the ergoCub hand MK5 (differently from iCub2.x we don't have a C++ handler for each finger, rather a single C++ handler for the whole hand).

I assume that, given the figure below - taken from the documentation - and given my understanding that

• the joint q1 is what we can actually control (via q0?);
• the joint q2 depends directly on q1 given the leverism via the coupling law q2 = f(q1)

Given the above, a sketch of the decouple* method should be - here I will use names from the URDF conf files and from the ergoCub joints nomenclature and concentrate on the thumb joint

def decouple*:
    (...)
    thumb_oc = thumb_prox // as we are directly controlling only the first link of the thumb
    (...)

While a sketch of the decoupleRef* method should be:

def decoupleRef*:
    (...)
    thumb_prox = thumb_oc // as the user provides a reference YARP-side only for the first link of the thumb
    thumb_dist = f(thumb_prox) // because q2, i.e. thumb_dist, depends on q1, i.e.thumb_prox, given the coupling law q2 = f(q1)
    (...)

Instead, by looking at the code what we found is, for the decouple* method:

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

Line 1327 in 4b35f02

current_pos[m_coupledJoints[1]] = current_pos[m_coupledJoints[1]] + current_pos[m_coupledJoints[2]];

equivalent to thumb_oc = thumb_prox + thumb_dist, and for the decoupleRef* method:

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

Lines 1378 to 1379 in 4b35f02

	out[m_coupledJoints[1]] = decouple(pos_ref[m_coupledJoints[1]], thumb_lut);
	out[m_coupledJoints[2]] = pos_ref[m_coupledJoints[1]] - out[m_coupledJoints[1]];

equivalent to

thumb_prox = f^{-1}(thumb_oc)` // where `f^{-1}` is the inverse of the law `f`, as per the code
thumb_dist = thumb_oc - thumb_prox // i.e. enforcing that thumb_oc  = thumb_dist + thumb_prox

As it can be seen, these are different from the laws that I have sketched above.

Moreover, the configuration files of the ergoCub fingers in simulation are reflecting the same problem.

The section of the limits should contain the limits for the actual physical joints simulated in Gazebo, however it only contains the limits for the coupled ones and the others are set to 0:

The limits for the coupled joints - unfortunately optional software-side - are missing:

Is the reasoning above correct? If not, is there anything I am missing which explains why the current implementation of the coupling handler is different from the one I expected?

Thank you and sorry for the long description - unfortunately coupling laws are always a bit painful to deal with

cc @Nicogene @mfussi66 @pattacini @traversaro

[Nicogene]

cc @ale-git

[ale-git]

Hi @xEnVrE, this assumption isn't correct:

• the joint q1 is what we can actually control (via q0?);

In fact, the YARP side controlled DOF is the angle of the fingertip with respect to the palm, i.e. q1+q2.
With this assumption, we see that the decoupleRef* function has to properly distribute the reference angle (the sum (q1+q2)_ref) between q1 and q2 in order to produce q1_ref and q2_ref for Gazebo, respecting the non linear mechanical coupling law.

[xEnVrE]

I talked f2f with @ale-git which explained that the current implementation of the plugin is reflecting what the firmware of the hand should have done i.e., controlling the angle of the fingertip with respect to the palm. This statement is the same as what @ale-git explained above:

In fact, the YARP side controlled DOF is the angle of the fingertip with respect to the palm, i.e. q1+q2.

However, the current firmware, for that I asked @sgiraz and @AntonioConsilvio, is implementing things differently, i.e. the user controls the angle of the first phalanx with respect to the palm. For that, I think that the implementation of the coupling handlers should be updated as I explained above.

Can I proceed opening a PR for that?

cc @traversaro @randaz81

[traversaro]

I agree on aligning the simulator with the robot, thanks a lot @xEnVrE . It would be great if we could also align the documentation that was added in icub-tech-iit/documentation#249, fyi @Nicogene .

[xEnVrE]

It would be great if we could also align the documentation that was added in icub-tech-iit/documentation#249, fyi @Nicogene

From my understanding, the documentation still holds. It is just how the coupling law is used in Gazebo that is making the difference at the moment.

[pattacini]

I agree on aligning the simulator with the robot, thanks a lot @xEnVrE

I agree too.
Perhaps, it'd be worth waiting for @mfussi66 to chime in.

[xEnVrE]

Meanwhile, I will add some other details / news.

• the coupling law provided in the documentation provides the absolute angle of the second phalanx with respect to the first one, hence extra care must be taken while using the formula in the Gazebo implementation - as Gazebo uses relative angles

• after implementing a possible new version of the Gazebo coupling and setting the correct limits, there is something wrong with respect to the real robot

Simulation (new coupling)
Real (not actuated)
Simulation (old coupling)

With the new coupling the maximum range YARP-side is now correct, that is 90 deg, while with the old one the maximum was 160 - as the user should have controlled the angle of the fingertip in that implementation.

Nonetheless, the angle that the second phalanx makes with respect to the first one is quite different from the reality. Here it is a video showing the comparison:

real_sim_hand-2023-03-20_23.43.01.mp4

For that I am now comparing the actual implementation of the coupling law in Gazebo with the formulae from the official documentation, to make sure that there are no misalignment.

[mfussi66]

I agree on aligning the simulator with the robot, thanks a lot @xEnVrE .

Of course I agree too, I'm getting up to speed and checking the needed modifications.

[pattacini]

Of course I agree too, I'm getting up to speed and checking the needed modifications.

@xEnVrE has already a branch for that, I guess.

[xEnVrE]

@xEnVrE has already a branch for that, I guess.

Indeed, it can be found in https://github.com/xEnVrE/gazebo-yarp-plugins/commits/fix/mk5_coupling

However, it will not work properly if not coupled with the proper configuration file ergocub-software-side. At the moment I am working on a standalone left hand that can be found in https://github.com/xEnVrE/ergocub-software/tree/study/hand_variation/urdf/ergoCub/robots/handMK5GazeboV1.

Changes has been done directly on the URDF - instead on the simmechanics part - just because it is easier to make quick tests.

Edit: Moreover,I did not implement the coupling handlers for the velocity and torque control modes as my goal was to first check the standard coupling laws in joint space.

[pattacini]

Thanks heaps @xEnVrE for your work 👍🏻

@mfussi66 is at moment busy with testing the ergoCub wrist+hand we have in the PROTO lab before the delivery. We will come back to this asap then.

[xEnVrE]

For that I am now comparing the actual implementation of the coupling law in Gazebo with the formulae from the official documentation, to make sure that there are no misalignment.

After discussing a bit with @ale-git in icub-tech-iit/documentation#257, I have changed the implementation of the coupling handlers again.

Starting from the index, the match with the real finger seems much better now:

comparison-2023-03-23_00.01.11.mp4

I will made comparisons with the other fingers as soon as the real hand is available again.

It would be great if we could also align the documentation that was added in icub-tech-iit/documentation#249

At this point, it seems to me that the documentation is to be updated according to the discussion made in icub-tech-iit/documentation#257.

[xEnVrE]

As a side note, I think there is also the need to discuss how the coupling law is to be implemented for the velocity and torque control modes before opening a PR.

[pattacini]

We're gonna organize a meeting for that asap 👍🏻
cc @xEnVrE @mfussi66 @Nicogene @ale-git

[traversaro]

I guess icub-tech-iit/documentation#267 is related, even if I am not 100% of the issue status at the moment.

[xEnVrE]

I guess icub-tech-iit/documentation#267 is related

Yes it is @traversaro

[xEnVrE]

I am not 100% aware of the issue status at the moment

We were waiting to update the documentation before proceeding to update the coupling handlers. I will work on it starting from the next week.

[traversaro]

Great, thanks!