The reference velocity of swingFootTask is wrongly overwritten.

[Saeed-Mansouri]

Hi @stephane-caron,

After updating the reference velocity of swingFootTask in updateSwingFoot(), you wrongly overwrite it in the single support phase by running updateFootForceDifferenceControl() of the stabilizer. As you can see in a log file, the reference velocity of swingFootTask is zero which is incorrect. By fixing this issue, I have seen a significant improvement in tracking the swing foot trajectory. You can see this improvement in the following graphs. (I should mention that I have applied #29 )

Tracking the vertical swing foot trajectory by zero reference velocity:

Tracking the vertical swing foot trajectory by correct reference velocity:

[gergondet]

Hi @Saeed-Mansouri

Do you mind opening a PR with the change you applied?

Thanks

[stephane-caron]

@Saeed-Mansouri nice catch! :fishing_pole_and_fish: :+1:

The guilty overwrite is here:

  if(contactState_ != ContactState::DoubleSupport || inTheAir_)
  {
    dfzForceError_ = 0.;
    dfzHeightError_ = 0.;
    vdcHeightError_ = 0.;
    leftFootTask->refVelB({{0., 0., 0.}, {0., 0., 0.}});  // don't do if swing foot!
    rightFootTask->refVelB({{0., 0., 0.}, {0., 0., 0.}});  // don't do if swing foot!
    return;
  }