wrist's joints: getRefPosition() returns a value that differs from setPosition()

[PasMarra]

If I set a position with yarp::dev::IPositionDirect::setPosition() and then I call yarp::dev::IPositionDirect::getRefPosition(), I get the same value, as I expect, for all the joints but the wrist's ones.

As an example, in the image below you can see what happens with the wrist_pitch:

• Traj/Des is what I set with yarp::dev::IPositionDirect::setPosition()
• PID/Ref is what I read with yarp::dev::IPositionDirect::getRefPosition()

It is important to notice that this problem does not appear in simulation.

[ale-git]

Hi @PasMarra, is the real wrist motion as expected, and thus only the getRefPosition() readings are wrong, or the real wrist motion is coherent with getRefPosition()?

[PasMarra]

Hi @ale-git

The real wrist motion (ENC/meas in the image above), retrieved with yarp::dev::IEncoders::getEncoder(), is coherent with yarp::dev::IPositionDirect::setPosition() (Traj/Des in the image above), meaning that the motion is as expected. Only the yarp::dev::IPositionDirect::getRefPosition() (PID/Ref in the image above) is wrong.

[ale-git]

I think that the problem is here, where the special case of wrist kinematics isn't managed in reading:
https://github.com/robotology/icub-firmware/blob/fa7f652528f9aa2cee32c4028e8c9256aef23da0/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c#L1509

as it is managed in writing:
https://github.com/robotology/icub-firmware/blob/fa7f652528f9aa2cee32c4028e8c9256aef23da0/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c#L1557

[pattacini]

We will prepare a patch for that.

[Nicogene]

It is important to notice that this problem does not appear in simulation.

For the records, in simulation the wrist has not any coupling involved, it is composed by the relative serial mechanism

[ale-git]

robotology/icub-firmware#522

This PR should fix the problem. Needs to be tested.

[vigisushrutha23]

@ale-git Is the PR ready for testing on the robot.? If so I could help with the testing tomorrow from 11, when I have the slot on EcubSN001.

[ale-git]

Hi @vigisushrutha23, I'll prepare the binary .hex in the evening. It is already functionally tested, in fact this version (2.9) is already running in SN001 wrists if nobody has flashed other versions in the meanwhile. The remaining test to do is to check if it solves the target feedback mismatch.

[ale-git]

Hi @vigisushrutha23, this is the draft PR with amc.hex wrist firmware

robotology/icub-firmware-build#167

[Nicogene]

Hi @vigisushrutha23 and @PasMarra !

What was the outcome of yesterday's test?

[vigisushrutha23]

@Nicogene Sorry I forgot to update here. We were not able to run experiments yesterday due to network issues. @PasMarra and I will run tests tomorrow as the network issue has been solved during my reserved slot from 10-13

[PasMarra]

Signals recap

• meas_or_ref is a boolean: 1 => trajectory initialized from yarp::dev::IEncoders::getEncoder(); 0 => trajectory initialized from yarp::dev::IPositionDirect::getRefPosition();
• Traj/Ini is the initial value for the trajectory, according to the previous variable;
• ENC/meas is what I read with yarp::dev::IEncoders::getEncoder();
• Traj/Des is what I set with yarp::dev::IPositionDirect::setPosition().

That said, if yarp::dev::IPositionDirect::setPosition() is equal to yarp::dev::IPositionDirect::getRefPosition(), then when meas_or_ref goes to zero there is no variation in Traj/Des, i.e. it appears to be a continuous signal.
So, I would say that it works fine now.

Left wrist

Roll

Pitch

Yaw

Right wrist

Roll

Pitch

Yaw

[PasMarra]

Anyway, there is also another problem with some joints. As you can see from the video below, there is a small jump during the motion of the wrist:

MicrosoftTeams-video.2.mp4

MicrosoftTeams-video.mp4

MicrosoftTeams-video.1.mp4

Notice that this jump does not seem to appear in the any of the plots above.

[pattacini]

Thanks heaps @PasMarra for testing the patch 🚀

If the jumps you noticed are not visible in the plot it means that there must be some loose part in the wrist levers needing to be tightened, most likely.

We don't have a direct measure of the position of the wrist but it is in turn reconstructed from the motor encoders. For this reason, it may be more convenient to report #261 (comment) to https://github.com/robotology/icub-tech-support/issues instead.

[Nicogene]

The PRs has been merged, closing