Mini Cheetah 代码分析（八）站立模式调整身体姿态

介绍在BalanceStan模式下，机器狗在四脚着地的情况下，调整身体姿态，包括机体欧拉角，位置，和机身高度等。在该模式下，使用的算法同运动是一样的，也是基于力控的优化算法控制。

需要改动以下文件：

1. robot/src/RobotRunner.cpp

2. user/MIT_Controller/FSM_States/ControlFSM.cpp

3. user/MIT_Controller/FSM_States/FSM_State_BalanceStand.cpp

1. robot/src/RobotRunner.cpp

找到这一行if( (rc_control.mode == 0) && controlParameters->use_rc )

在前面一行添加一行如下：

controlParameters->use_rc=0;

2. user/MIT_Controller/FSM_States/ControlFSM.cpp

在runFSM()函数的定义改为如下

template <typename T>
void ControlFSM<T>::runFSM() {
  // Publish state estimator data to other computer
  //for(size_t i(0); i<3; ++i){
    //_state_estimator.p[i] = data._stateEstimator->getResult().position[i];
    //_state_estimator.quat[i] = data._stateEstimator->getResult().orientation[i];
  //}
    //_state_estimator.quat[3] = data._stateEstimator->getResult().orientation[3];
  //state_estimator_lcm.publish("state_estimator_ctrl_pc", &_state_estimator);

  // Check the robot state for safe operation
  u32 count_temp = 20000;
  operatingMode = safetyPreCheck();
    if (iter<1000)
    {
      data.controlParameters->control_mode = K_PASSIVE;
      // printf("Mark K_PASSIVE=0\n");
    }
    else if (iter<2000){
      data.controlParameters->control_mode = K_RECOVERY_STAND;
    }
    else if (iter<count_temp){
      data.controlParameters->control_mode = K_BALANCE_STAND;
      // printf("Mark control_mode = K_BALANCE_STAND\n");
    }
    else if (iter<count_temp+3000){
      data.controlParameters->control_mode = K_RECOVERY_STAND;
      // printf("Mark control_mode = K_RECOVERY_STAND\n");
    }

    else{
      printf("done!\n");
    }


  // Run the robot control code if operating mode is not unsafe
  if (operatingMode != FSM_OperatingMode::ESTOP) {
    // Run normal controls if no transition is detected
    if (operatingMode == FSM_OperatingMode::NORMAL) {
      // Check the current state for any transition
      nextStateName = currentState->checkTransition();

      // Detect a commanded transition
      if (nextStateName != currentState->stateName) {
        // Set the FSM operating mode to transitioning
        operatingMode = FSM_OperatingMode::TRANSITIONING;

        // Get the next FSM State by name
        nextState = getNextState(nextStateName);

        // Print transition initialized info
        //printInfo(1);

      } else {
        // Run the iteration for the current state normally
        currentState->run();
      }
    } else
        printf("operatingMode not ESTOP, abnormal\n");

    // Run the transition code while transition is occuring
    if (operatingMode == FSM_OperatingMode::TRANSITIONING) {
      transitionData = currentState->transition();

      // Check the robot state for safe operation
      safetyPostCheck();

      // Run the state transition
      if (transitionData.done) {
        // Exit the current state cleanly
        currentState->onExit();

        // Print finalizing transition info
        //printInfo(2);

        // Complete the transition
        currentState = nextState;

        // Enter the new current state cleanly
        currentState->onEnter();

        // Return the FSM to normal operation mode
        operatingMode = FSM_OperatingMode::NORMAL;
      }
    } else {
      // Check the robot state for safe operation
      safetyPostCheck();
    }

  } else { // if ESTOP
      printf("BILLCHEN in ESTOP \n");
//      nextState = getNextState(nextStateName);
//      operatingMode = FSM_OperatingMode::NORMAL;
//
//      if(nextState ==statesList.passive)
//        std::cout<<"statesList.passive"<<std::endl;
//      else
//          std::cout<<"statesList.others"<<nextState->stateString <<std::endl;
//
//      if(nextState==statesList.recoveryStand)
//      {
//          operatingMode = FSM_OperatingMode::NORMAL;
//          currentState=statesList.recoveryStand;
//          printf("ESTOP:force to statesList.recoveryStand\n ");
//      }
//      else
//        currentState = statesList.passive;

    currentState = statesList.passive;
    currentState->onEnter();
    nextStateName = currentState->stateName;
  }

  // Print the current state of the FSM
  printInfo(0);

  // Increase the iteration counter
  iter++;
}

3. user/MIT_Controller/FSM_States/FSM_State_BalanceStand.cpp

在FSM_State_BalanceStand<T>::BalanceStandStep()函数中找到如下代码段并更改如下

else{
    // Orientation
    _wbc_data->pBody_RPY_des[0] =
     0.6* this->_data->_desiredStateCommand->gamepadCommand->leftStickAnalog[0];
     _wbc_data->pBody_RPY_des[1] =
      0.4*this->_data->_desiredStateCommand->gamepadCommand->leftStickAnalog[1];//rightStickAnalog[0];
    _wbc_data->pBody_RPY_des[2] -=
      0.6*this->_data->_desiredStateCommand->gamepadCommand->rightStickAnalog[1];

    // Height
    // _wbc_data->pBody_des[2] +=
    //   0.1 * this->_data->_desiredStateCommand->gamepadCommand->rightStickAnalog[0];

    _wbc_data->pBody_des[2] = 0.05*sin(2*(_iter/500)) + 0.3;
  }
  _wbc_data->vBody_Ori_des.setZero();

这段代码将会是机器人机身上下周期性运动。