Preventing QP Solver Failures in Walking Scenarios

[Zhenyuan-Fu]

HI, @gergondet, I am now able to use mc_rtc and MPC to enable torque-controlled walking in actual robots. However, during experiments, I often encounter situations where the WBC (Whole Body Control) solver fails (QP failed) when switching between the left and right legs. To prevent QP failure, I have added a "Protecting state" in the FSM (Finite State Machine) with a high damping controller (Kp = 0, Kd = 100), which effectively protects the robot. But, after QP failure occurs, it's not possible to transition into the FSM's protecting state. At the moment of QP failure, is there a way for the state controller to provide an early warning through some form of flag or indicator?

[gergondet]

Hello @Zhenyuan-Fu

For now you have to write the logic into your controller's run function. Something like this could work:

bool MyController::run()
{
  // Note: if you are using a specific feedback type use that in the calls to `run()`
  bool res = mc_control::fsm::Controller::run();
  if(!res && !executor_.state() == "MyEmergencyState")
  {
    /** This interrupts the current state to force a transition to `MyEmergencyState` then force it to run one more step of control */
    executor_.resume("MyEmergencyState");
    return mc_control::fsm::Controller::run();
  }
  return res;
}

[Zhenyuan-Fu]

Thank you very much, Pierre.