The reference velocity of swingFootTask is wrongly overwritten.
Saeed-Mansouri opened this issue · comments
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:
@Saeed-Mansouri nice catch! 🎣 👍
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;
}