ergoCub 1.1 S/N:001 – wrist joints: reference positions differ from measured ones

[PasMarra]

Robot Name 🤖

ergoCub 1.3 S/N:002

Request/Failure description

reference positions retrieved by yarp::dev::IPositionDirect::getRefPosition() are very different from measured positions retrieved by yarp::dev::IEncoders::getEncoders()

These are l_wrist_yaw, l_wrist_roll, l_wrist_pitch in radiants:

Detailed context

An analogous problem was present in ergoCubSN001 and was caused by the firmware, see here

cc @ale-git

Additional context

No response

How does it affect you?

No response

[edersonferrante]

Hello @PasMarra , we will investigate this issue as soon as possible and resolve it if necessary.

cc @Fabrizio69

[PasMarra]

Is there any news?

[Fabrizio69]

cc: @ale-git

[ale-git]

Hi @PasMarra, since -1.09 rad is about -62 deg, is it wrong the real position of the joint (visually checked), or only the measured position? Does it affect roll and pitch as well, or only yaw?

[PasMarra]

Hi @ale-git
I would say that the measured position corresponds to the real position (visually checked).
It affects also the other wrist joints.

[PasMarra]

@ale-git please notice also the following:

The joint sequence is l_wrist_yaw, l_wrist_roll, l_wrist_pitch and the values are in deg.

Right after the calibration, i set the control mode: Position Direct and i get:

Ref pos (deg):  [0.197754, -0.0164795, 0.0329591] 
Meas pos (deg):  [0.0384522, 0.0549318, 0.10437]

so it is as expected.

Then, I move the wrist_roll from 0 to 29 deg using the yarpmotorgui (so control mode: Position), then i set the control mode: Position Direct and i get:

Ref pos (deg):  [0.197754, -0.0164795, 0.0329591] 
Meas pos (deg):  [0.0274659, 28.9875, 0.0823977]

so there is a mismatch.

Then, I move also the wrist_yaw from 0 to -90 deg using the yarpmotorgui (so control mode: Position), then i set the control mode: Position Direct and i get:

Ref pos (deg):  [0.197754, -0.0164795, 0.0329591] 
Meas pos (deg):  [-88.995, 29.0205, 0.186768]

again there is a mismatch in the values.

The issue might be that the reference values aren't properly updated when switching between the two control modes. I'm not sure why the values are correctly aligned right after calibration, but it's possible that, in that case, they're handled differently.

[PasMarra]

Hi @ale-git, are there any news?

[ale-git]

Hi @PasMarra, I have to complete one task, then I'll take care of it.

[ale-git]

Hi @PasMarra, I'm taking care of this issue. I think that the problem could be here:

BOOL JointSet_set_control_mode(JointSet* o, eOmc_controlmode_command_t control_mode_cmd)
{
#ifdef WRIST_MK2
    if(eomc_jsetconstraint_ergocubwrist == o->special_constraint)
    {
#if defined(MC_use_embot_app_mc_Trajectory)
    for(size_t i=0; i<Trajectories::njoints; i++)
    {
        o->wristMK2.trajectories.ypr_trajectory[i]->stop(o->wristMK2.ypr_pos_fbk[i]);
        o->wristMK2.trajectories.prk_trajectory[i]->stop(o->joint[i].pos_fbk); 
    }
#endif         
#if defined(MC_use_Trajectory)        
        Trajectory_stop(&(o->wristMK2.ypr_trajectory[0]), o->wristMK2.ypr_pos_fbk[0]);
        Trajectory_stop(&(o->wristMK2.ypr_trajectory[1]), o->wristMK2.ypr_pos_fbk[1]);
        Trajectory_stop(&(o->wristMK2.ypr_trajectory[2]), o->wristMK2.ypr_pos_fbk[2]);
        
        Trajectory_stop(&(o->wristMK2.prk_trajectory[0]), o->joint[0].pos_fbk);
        Trajectory_stop(&(o->wristMK2.prk_trajectory[1]), o->joint[1].pos_fbk);
        Trajectory_stop(&(o->wristMK2.prk_trajectory[2]), o->joint[2].pos_fbk);
#endif        
    }
#endif