Stop during the first DSP

[Saeed-Mansouri]

Hi Dr. Caron

The below command applied in ModelPredictiveControl::computeZMPRef() might be activated not only for stopping during the first DSP. I think that you should replace the total steps of the target support with nbTargetSupportSteps_.

if(nbTargetSupportSteps_ < 1) // stop during first DSP

[stephane-caron]

This is indeed a controller-specific quirk: the function has two modes (full preview and half preview, documented in phaseDurations), and it is only called from SingleSupport.cpp and DoubleSupport.cpp with either:

• targetSupportDuration == 0.: stop during the first DSP, i.e. the DSP between initContact and targetContact
• targetSupportDuration == ctl.singleSupportDuration(): keep going
  So, with this implementation the condition will only be triggered for stopping during the first DSP. What's bad about this code is that it is not apparent from the ModelPredictiveControl class itself.

I added some doc on this in 3c73788. Also, taking a second look at this piece I realize that phaseDurations is not meant to be called with initSupportDuration + doubleSupportDuration greater than the total duration (in that case, targetSupportDuration would be zero although the intent is not to stop at the first DSP). I added a note on this as well.

[stephane-caron]

@Saeed-Mansouri Thank you for your feedback 😀

I'll be closing the Issue tracker of this repository since bugs and fixes should go to the tracker of the active fork. Feel free to use the new Discussions tab for general questions and discussions.

[Saeed-Mansouri]

Thanks, Dr. Caron.
I have modified some parts of the LIPM to resolve the mentioned issue. With these modifications, initSupportDuration + doubleSupportDuration can be greater than the total duration of the preview horizon, and zmpRef_ also moves continuously from the previous support contact to the current one.

void ModelPredictiveControl::phaseDurations(double initSupportDuration,
                                            double doubleSupportDuration,
                                            double targetSupportDuration, double totalDoubleSupportDuration)
{
  constexpr double T = SAMPLING_PERIOD;

  nbTotalTargetSupportSteps_ = static_cast<unsigned>(std::round(targetSupportDuration/T));
  nbTotalDoubleSupportSteps_ = static_cast<unsigned>(std::round(totalDoubleSupportDuration/T));
  nbOffsetDoubleSupportSteps_ = static_cast<unsigned>(std::round((totalDoubleSupportDuration - doubleSupportDuration)/T));

  unsigned nbStepsSoFar = 0;
  nbInitSupportSteps_ = std::min(static_cast<unsigned>(std::round(initSupportDuration / T)), NB_STEPS - nbStepsSoFar);
  nbStepsSoFar += nbInitSupportSteps_;
  nbDoubleSupportSteps_ =
      std::min(static_cast<unsigned>(std::round(doubleSupportDuration / T)), NB_STEPS - nbStepsSoFar);
  nbStepsSoFar += nbDoubleSupportSteps_;
  nbTargetSupportSteps_ =
      std::min(static_cast<unsigned>(std::round(targetSupportDuration / T)), NB_STEPS - nbStepsSoFar);
  nbStepsSoFar += nbTargetSupportSteps_;
  if(nbTargetSupportSteps_ > 0) // full preview
  {
    nbNextDoubleSupportSteps_ = NB_STEPS - nbStepsSoFar; // always positive
  }
  for(long i = 0; i <= NB_STEPS; i++)
  {
    // SSP constraint is enforced at the very first step of DSP
    if(i < nbInitSupportSteps_ || (0 < i && i == nbInitSupportSteps_))
    {
      indexToHrep_[i] = 0;
    }
    else if(i - nbInitSupportSteps_ < nbDoubleSupportSteps_)
    {
      indexToHrep_[i] = 1;
    }
    else if(nbTargetSupportSteps_ > 0)
    {
      if(i - nbInitSupportSteps_ - nbDoubleSupportSteps_ <= nbTargetSupportSteps_)
      {
        indexToHrep_[i] = 2;
      }
      else if(nbNextDoubleSupportSteps_ > 0)
      {
        indexToHrep_[i] = 3;
      }
      else // (nbNextDoubleSupportSteps_ == 0)
      {
        indexToHrep_[i] = 2;
      }
    }
    else // (nbTargetSupportSteps_ == 0)
    {
      indexToHrep_[i] = 1;
    }
  }
}

void ModelPredictiveControl::computeZMPRef()
{
  zmpRef_.setZero();
  Eigen::Vector2d p_0 = initContact_.anklePos(sole_).head<2>();
  Eigen::Vector2d p_1 = targetContact_.anklePos(sole_).head<2>();
  Eigen::Vector2d p_2 = nextContact_.anklePos(sole_).head<2>();
  if(nbTotalTargetSupportSteps_ < 1) // stop during first DSP
  {
    p_1 = 0.5 * (initContact_.anklePos(sole_) + targetContact_.anklePos(sole_)).head<2>();
  }
  for(long i = 0; i <= NB_STEPS; i++)
  {
    if(indexToHrep_[i] <= 1)
    {
      long j = i - nbInitSupportSteps_;
      double x =0.;
      if (j >= 0)
      {
          x = (nbDoubleSupportSteps_ > 0) ? (static_cast<double>(j) + nbOffsetDoubleSupportSteps_)/ nbTotalDoubleSupportSteps_ : 0.;
      }
      x = clamp(x, 0., 1.);
      zmpRef_.segment<2>(2 * i) = (1. - x) * p_0 + x * p_1;
    }
    else // (indexToHrep_[i] <= 3), which implies nbTargetSupportSteps_ > 0
    {
      long j = i - nbInitSupportSteps_ - nbDoubleSupportSteps_ - nbTargetSupportSteps_;
      double x =0.;
      if (j >= 0)
      {
          x = (nbNextDoubleSupportSteps_ > 0) ? static_cast<double>(j) / nbTotalDoubleSupportSteps_ : 0;
      }
      x = clamp(x, 0., 1.);
      zmpRef_.segment<2>(2 * i) = (1. - x) * p_1 + x * p_2;
    }
  }
}