GaitScheduler.cpp

/*!
 * @file GaitScheduler.cpp
 * @brief Logic for fixed-gait timing
 */

/*========================= Gait Scheduler ============================*/
/**
 *
 */

#include "Controllers/GaitScheduler.h"

/*=========================== Gait Data ===============================*/
/**
 * Reset gait data to zeros
 */
template<typename T>
void GaitData<T>::zero()
{
  // Stop any gait transitions
  _nextGait = _currentGait;

  // General Gait descriptors
  periodTimeNominal = 0.0;     // overall period time to scale
  initialPhase = 0.0;          // initial phase to offset
  switchingPhaseNominal = 0.0; // nominal phase to switch contacts
  overrideable = 0;            // allows the gait parameters to be overridden

  // Enable flag for each foot
  gaitEnabled = Eigen::Vector4i::Zero(); // enable gait controlled legs

  // Time based descriptors
  periodTime = Vec4<T>::Zero();          // overall gait period time
  timeStance = Vec4<T>::Zero();          // total stance time
  timeSwing = Vec4<T>::Zero();           // total swing time
  timeStanceRemaining = Vec4<T>::Zero(); // stance time remaining
  timeSwingRemaining = Vec4<T>::Zero();  // swing time remaining

  // Phase based descriptors
  switchingPhase = Vec4<T>::Zero(); // phase to switch to swing
  phaseVariable = Vec4<T>::Zero();  // overall gait phase for each foot
  phaseOffset = Vec4<T>::Zero();    // nominal gait phase offsets
  phaseScale = Vec4<T>::Zero();     // phase scale relative to variable
  phaseStance = Vec4<T>::Zero();    // stance subphase
  phaseSwing = Vec4<T>::Zero();     // swing subphase

  // Scheduled contact states
  contactStateScheduled = Eigen::Vector4i::Zero(); // contact state of the foot
  contactStatePrev = Eigen::Vector4i::Zero();      // previous contact state of the foot
  touchdownScheduled = Eigen::Vector4i::Zero();    // scheduled touchdown flag
  liftoffScheduled = Eigen::Vector4i::Zero();      // scheduled liftoff flag
}

template struct GaitData<double>;
template struct GaitData<float>;

/*========================= Gait Scheduler ============================*/

/**
 * Constructor to automatically setup a basic gait
 */
template<typename T>
GaitScheduler<T>::GaitScheduler(be2r_cmpc_unitree::ros_dynamic_paramsConfig* userParameters,
                                float _dt)
{
  initialize();
  _userParameters = userParameters;
  dt = _dt;
}

/**
 * Initialize the gait data
 */
template<typename T>
void GaitScheduler<T>::initialize()
{
  std::cout << "[GAIT] Initialize Gait Scheduler" << std::endl;

  // Start the gait in a trot since we use this the most
  gaitData._currentGait = GaitType::STAND;

  // Zero all gait data
  gaitData.zero();

  // Create the gait from the nominal initial
  createGait();
  period_time_natural = gaitData.periodTimeNominal;
  switching_phase_natural = gaitData.switchingPhaseNominal;
}

/**
 * Executes the Gait Schedule step to calculate values for the defining
 * gait parameters.
 */
template<typename T>
void GaitScheduler<T>::step()
{
  // Modify the gait with settings
  modifyGait();

  if (gaitData._currentGait != GaitType::STAND)
  {
    // Track the reference phase variable
    gaitData.initialPhase = fmod((gaitData.initialPhase + (dt / gaitData.periodTimeNominal)), 1);
  }

  // Iterate over the feet
  for (int foot = 0; foot < 4; foot++)
  {
    // Set the previous contact state for the next timestep
    gaitData.contactStatePrev(foot) = gaitData.contactStateScheduled(foot);

    if (gaitData.gaitEnabled(foot) == 1)
    {
      // Monotonic time based phase incrementation
      if (gaitData._currentGait == GaitType::STAND)
      {
        // Don't increment the phase when in stand mode
        dphase = 0.0;
      }
      else
      {
        dphase = gaitData.phaseScale(foot) * (dt / gaitData.periodTimeNominal);
      }

      // Find each foot's current phase
      gaitData.phaseVariable(foot) = fmod((gaitData.phaseVariable(foot) + dphase), 1);

      // Check the current contact state
      if (gaitData.phaseVariable(foot) <= gaitData.switchingPhase(foot))
      {
        // Foot is scheduled to be in contact
        gaitData.contactStateScheduled(foot) = 1;

        // Stance subphase calculation
        gaitData.phaseStance(foot) = gaitData.phaseVariable(foot) / gaitData.switchingPhase(foot);

        // Swing phase has not started since foot is in stance
        gaitData.phaseSwing(foot) = 0.0;

        // Calculate the remaining time in stance
        gaitData.timeStanceRemaining(foot) =
          gaitData.periodTime(foot) *
          (gaitData.switchingPhase(foot) - gaitData.phaseVariable(foot));

        // Foot is in stance, no swing time remaining
        gaitData.timeSwingRemaining(foot) = 0.0;

        // First contact signifies scheduled touchdown
        if (gaitData.contactStatePrev(foot) == 0)
        {
          // Set the touchdown flag to 1
          gaitData.touchdownScheduled(foot) = 1;
        }
        else
        {
          // Set the touchdown flag to 0
          gaitData.touchdownScheduled(foot) = 0;
        }
      }
      else
      {
        // Foot is not scheduled to be in contact
        gaitData.contactStateScheduled(foot) = 0;

        // Stance phase has completed since foot is in swing
        gaitData.phaseStance(foot) = 1.0;

        // Swing subphase calculation
        gaitData.phaseSwing(foot) = (gaitData.phaseVariable(foot) - gaitData.switchingPhase(foot)) /
                                    (1.0 - gaitData.switchingPhase(foot));

        // Foot is in swing, no stance time remaining
        gaitData.timeStanceRemaining(foot) = 0.0;

        // Calculate the remaining time in swing
        gaitData.timeSwingRemaining(foot) =
          gaitData.periodTime(foot) * (1 - gaitData.phaseVariable(foot));

        // First contact signifies scheduled touchdown
        if (gaitData.contactStatePrev(foot) == 1)
        {
          // Set the liftoff flag to 1
          gaitData.liftoffScheduled(foot) = 1;
        }
        else
        {
          // Set the liftoff flag to 0
          gaitData.liftoffScheduled(foot) = 0;
        }
      }
    }
    else
    {
      // Leg is not enabled
      gaitData.phaseVariable(foot) = 0.0;

      // Foot is not scheduled to be in contact
      gaitData.contactStateScheduled(foot) = 0;
    }
  }
}

/**
 *
 */
template<typename T>
void GaitScheduler<T>::modifyGait()
{
  // Choose the gaits and parameters as selected
  switch ((int)_userParameters->gait_override)
  {
    case 0:
      // Use default gait and default settings from Control code
      if (gaitData._currentGait != gaitData._nextGait)
      {
        createGait();
      }
      break;

    case 1:
      // Use chosen gait with default settings
      if (gaitData._currentGait != GaitType(_userParameters->gait_type))
      {
        gaitData._nextGait = GaitType(_userParameters->gait_type);
        createGait();
      }
      break;

    case 2:
      // Use chosen gait and chosen settings
      // Change the gait
      if (gaitData._currentGait != GaitType(_userParameters->gait_type))
      {
        gaitData._nextGait = GaitType(_userParameters->gait_type);
        createGait();
      }

      // Adjust the gait parameters
      if (fabs(gaitData.periodTimeNominal - (T)_userParameters->gait_period_time) > 0.0001 ||
          fabs(gaitData.switchingPhaseNominal - (T)_userParameters->gait_switching_phase) > 0.0001)
      {
        // Modify the gait with new parameters if it is overrideable
        if (gaitData.overrideable == 1)
        {
          gaitData.periodTimeNominal = _userParameters->gait_period_time;
          gaitData.switchingPhaseNominal = _userParameters->gait_switching_phase;
          calcAuxiliaryGaitData();
        }
      }
      break;

    case 3:
      // Use NaturalGaitModification from FSM_State
      if (gaitData._currentGait != gaitData._nextGait)
      {
        createGait();
      }
      break;

    case 4:
      // Use NaturalGaitModification from FSM_State
      if (gaitData._currentGait != gaitData._nextGait)
      {
        createGait();
        period_time_natural = gaitData.periodTimeNominal;
        switching_phase_natural = gaitData.switchingPhaseNominal;
      }
      else
      {
        gaitData.periodTimeNominal = period_time_natural;
        gaitData.switchingPhaseNominal = switching_phase_natural;
        calcAuxiliaryGaitData();
      }
      break;
  }
}

/**
 * Creates the gait structure from the important defining parameters of each
 * gait
 *
 * To create a standard gait you should only need to define the following:
 *
 *   gaitData.periodTimeNominal
 *   gaitData.switchingPhaseNominal
 *   gaitData.phaseOffset
 *
 * The rest can be set to:
 *
 *   gaitData.gaitEnabled << 1, 1, 1, 1;
 *   gaitData.initialPhase = 0.0;
 *   gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
 *
 * These add flexibility to be used for very irregular gaits and transitions.
 */
template<typename T>
void GaitScheduler<T>::createGait()
{

  std::cout << "[GAIT] Transitioning gait from " << gaitData.gaitName << " to ";

  // Case structure gets the appropriate parameters
  switch (gaitData._nextGait)
  {
    case GaitType::STAND:
      gaitData.gaitName = "STAND";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 10.0;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 1.0;
      gaitData.phaseOffset << 0.5, 0.5, 0.5, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 0;
      break;

    case GaitType::STAND_CYCLE:
      gaitData.gaitName = "STAND_CYCLE";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 1.0;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 1.0;
      gaitData.phaseOffset << 0.5, 0.5, 0.5, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 0;
      break;

    case GaitType::STATIC_WALK:
      gaitData.gaitName = "STATIC_WALK";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 1.25;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.8;
      gaitData.phaseOffset << 0.25, 0.0, 0.75, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::AMBLE:
      gaitData.gaitName = "AMBLE";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.5;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.6250;
      gaitData.phaseOffset << 0.0, 0.5, 0.25, 0.75;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::TROT_WALK:
      gaitData.gaitName = "TROT_WALK";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.5;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.6;
      gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::TROT:
      gaitData.gaitName = "TROT";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.5;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.5;
      gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::TROT_RUN:
      gaitData.gaitName = "TROT_RUN";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.4;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.4;
      gaitData.phaseOffset << 0.0, 0.5, 0.5, 0.0;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::PACE:
      gaitData.gaitName = "PACE";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.35;
      gaitData.initialPhase = 0.25;
      gaitData.switchingPhaseNominal = 0.5;
      gaitData.phaseOffset << 0.0, 0.5, 0.0, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::BOUND:
      gaitData.gaitName = "BOUND";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.4;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.4;
      gaitData.phaseOffset << 0.0, 0.0, 0.5, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::ROTARY_GALLOP:
      gaitData.gaitName = "ROTARY_GALLOP";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.4;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.2;
      gaitData.phaseOffset << 0.0, 0.8571, 0.3571, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::TRAVERSE_GALLOP:
      // TODO: find the right sequence, should be easy
      gaitData.gaitName = "TRAVERSE_GALLOP";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.5;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.2;
      gaitData.phaseOffset << 0.0, 0.8571, 0.3571, 0.5;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::PRONK:
      gaitData.gaitName = "PRONK";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      gaitData.periodTimeNominal = 0.5;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.5;
      gaitData.phaseOffset << 0.0, 0.0, 0.0, 0.0;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::THREE_FOOT:
      gaitData.gaitName = "THREE_FOOT";
      gaitData.gaitEnabled << 0, 1, 1, 1;
      gaitData.periodTimeNominal = 0.4;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal = 0.666;
      gaitData.phaseOffset << 0.0, 0.666, 0.0, 0.333;
      gaitData.phaseScale << 0.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 1;
      break;

    case GaitType::CUSTOM:
      gaitData.gaitName = "CUSTOM";
      // TODO: get custom gait parameters from operator GUI
      break;

    case GaitType::TRANSITION_TO_STAND:
      gaitData.gaitName = "TRANSITION_TO_STAND";
      gaitData.gaitEnabled << 1, 1, 1, 1;
      T oldGaitPeriodTimeNominal = gaitData.periodTimeNominal;
      gaitData.periodTimeNominal = 3 * gaitData.periodTimeNominal;
      gaitData.initialPhase = 0.0;
      gaitData.switchingPhaseNominal =
        (gaitData.periodTimeNominal +
         oldGaitPeriodTimeNominal * (gaitData.switchingPhaseNominal - 1)) /
        gaitData.periodTimeNominal;
      gaitData.phaseOffset << (gaitData.periodTimeNominal +
                               oldGaitPeriodTimeNominal * (gaitData.phaseVariable(0) - 1)) /
                                gaitData.periodTimeNominal,
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(1) - 1)) /
          gaitData.periodTimeNominal,
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(2) - 1)) /
          gaitData.periodTimeNominal,
        (gaitData.periodTimeNominal + oldGaitPeriodTimeNominal * (gaitData.phaseVariable(3) - 1)) /
          gaitData.periodTimeNominal;
      gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;
      gaitData.overrideable = 0;
      break;
      /*
          case GaitType::TRANSITION_FROM_STAND:
            gaitData.gaitName = "TRANSITION_FROM_STAND";
            gaitData.gaitEnabled << 1, 1, 1, 1;


            gaitData.phaseScale << 1.0, 1.0, 1.0, 1.0;

            break;
        */
  }

  // Gait has switched
  gaitData._currentGait = gaitData._nextGait;

  std::cout << gaitData.gaitName << "\n" << std::endl;

  // Calculate the auxilliary gait information
  calcAuxiliaryGaitData();
}

template<typename T>
void GaitScheduler<T>::calcAuxiliaryGaitData()
{

  if (gaitData.overrideable == 1)
  {
    if (_userParameters->gait_override == 2)
    {
      // gaitData.periodTimeNominal = _userParameters->gait_period_time;
      // gaitData.switchingPhaseNominal = _userParameters->gait_switching_phase;
    }
    else if (_userParameters->gait_override == 4)
    {
      // gaitData.periodTimeNominal = _userParameters->gait_period_time;
      // gaitData.switchingPhaseNominal = _userParameters->gait_switching_phase;
    }
  }

  // Set the gait parameters for each foot
  for (int foot = 0; foot < 4; foot++)
  {
    if (gaitData.gaitEnabled(foot) == 1)
    {
      // The scaled period time for each foot
      gaitData.periodTime(foot) = gaitData.periodTimeNominal / gaitData.phaseScale(foot);

      // Phase at which to switch the foot from stance to swing
      gaitData.switchingPhase(foot) = gaitData.switchingPhaseNominal;

      // Initialize the phase variables according to offset
      gaitData.phaseVariable(foot) = gaitData.initialPhase + gaitData.phaseOffset(foot);

      // Find the total stance time over the gait cycle
      gaitData.timeStance(foot) = gaitData.periodTime(foot) * gaitData.switchingPhase(foot);

      // Find the total swing time over the gait cycle
      gaitData.timeSwing(foot) = gaitData.periodTime(foot) * (1.0 - gaitData.switchingPhase(foot));
    }
    else
    {
      // The scaled period time for each foot
      gaitData.periodTime(foot) = 0.0;

      // Phase at which to switch the foot from stance to swing
      gaitData.switchingPhase(foot) = 0.0;

      // Initialize the phase variables according to offset
      gaitData.phaseVariable(foot) = 0.0;

      // Foot is never in stance
      gaitData.timeStance(foot) = 0.0;

      // Foot is always in "swing"
      gaitData.timeSwing(foot) = 1.0 / gaitData.periodTime(foot);
    }
  }
}

/**
 * Prints relevant information about the gait and current gait state
 */
template<typename T>
void GaitScheduler<T>::printGaitInfo()
{
  // Increment printing iteration
  printIter++;

  // Print at requested frequency
  if (printIter == printNum)
  {
    std::cout << "[GAIT SCHEDULER] Printing Gait Info...\n";
    std::cout << "Gait Type: " << gaitData.gaitName << "\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Enabled: " << gaitData.gaitEnabled(0) << " | " << gaitData.gaitEnabled(1) << " | "
              << gaitData.gaitEnabled(2) << " | " << gaitData.gaitEnabled(3) << "\n";
    std::cout << "Period Time: " << gaitData.periodTime(0) << "s | " << gaitData.periodTime(1)
              << "s | " << gaitData.periodTime(2) << "s | " << gaitData.periodTime(3) << "s\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Contact State: " << gaitData.contactStateScheduled(0) << " | "
              << gaitData.contactStateScheduled(1) << " | " << gaitData.contactStateScheduled(2)
              << " | " << gaitData.contactStateScheduled(3) << "\n";
    std::cout << "Phase Variable: " << gaitData.phaseVariable(0) << " | "
              << gaitData.phaseVariable(1) << " | " << gaitData.phaseVariable(2) << " | "
              << gaitData.phaseVariable(3) << "\n";
    std::cout << "Stance Time Remaining: " << gaitData.timeStanceRemaining(0) << "s | "
              << gaitData.timeStanceRemaining(1) << "s | " << gaitData.timeStanceRemaining(2)
              << "s | " << gaitData.timeStanceRemaining(3) << "s\n";
    std::cout << "Swing Time Remaining: " << gaitData.timeSwingRemaining(0) << "s | "
              << gaitData.timeSwingRemaining(1) << "s | " << gaitData.timeSwingRemaining(2)
              << "s | " << gaitData.timeSwingRemaining(3) << "s\n";
    std::cout << std::endl;

    // Reset iteration counter
    printIter = 0;
  }
}

template class GaitScheduler<double>;
template class GaitScheduler<float>;