Mini Cheetah 代码分析（四）学习编写自己的控制器

一、简单介绍

二、实战，添加控制器流程介绍

一、简单介绍

要添加自己的机器人控制器，在“Cheetah Software/user”下添加一个文件夹，然后在user文件夹中的“CmakeLists.txt”中，将该文件夹目录添加进去。“JPos_Controller”是一个非常简单的控制器示例，以此为模板即可完成自己的控制器编写，其功能是直接控制腿部各个关节的位置。

下边主要对\user\JPos_Controller文件夹下的几个文件进行分析。

CMakeLists.txt
JPosUserParameters.h
JPos_Controller.cpp
JPos_Controller.hpp
main.cpp

1.main.cpp

int main(int argc, char** argv) {
  main_helper(argc, argv, new JPos_Controller());
  return 0;
}

将自己编写的控制器的类JPos_Controller，new一个放在main_helper的参数列表即可，main函数的写法是统一的。

2.JPos_Controller.hpp 控制器主要看JPos_Controller这个类的写法。

JPos_Controller继承于RobotController类，class JPos_Controller:public RobotController

并定义了几个包括初始化的几个函数

    virtual void initializeController(){}

    virtual void runController();

    virtual void updateVisualization(){}

和几个用于加载用户参数的userParameters的功能。

2.JPos_Controller.cpp 实现控制器主要内容

JPos_Controller继承于JPos_Controller::runController()

控制器的“runController”函数将以1 kHz的频率自动调用，从该类中基本可以访问所有数据。

runController定义在\robot\include\RobotController.h中。JPos_Controller类的具体实现如下：

定义3*3矩阵kpMat和kdMat。以common文件夹中Eigen::Matrix<T, 3, 3>为类模板

  Mat3<float> kpMat;
  Mat3<float> kdMat;
  //kpMat << 20, 0, 0, 0, 20, 0, 0, 0, 20;
  //kdMat << 2.1, 0, 0, 0, 2.1, 0, 0, 0, 2.1;

如果使用了用户配置参数，则将用户配置参数赋值给kpMat和kdMat矩阵，按照行顺序依次赋值，userParameters.kp不是矩阵，是个变量。

  kpMat << userParameters.kp, 0, 0, 0,  userParameters.kp, 0, 0, 0,  userParameters.kp;
  kdMat <<  userParameters.kd, 0, 0, 0, userParameters.kd, 0, 0, 0, userParameters.kd;

定义一个计数的迭代器static int iter(0);  ++iter;将每一条腿leg的每一个关节jidx的位置q赋值给_jpos_ini矩阵，采样10次。leg的定义位置

  if(iter < 10){
    for(int leg(0); leg<4; ++leg){
      for(int jidx(0); jidx<3; ++jidx){
        _jpos_ini[3*leg+jidx] = _legController->datas[leg].q[jidx];
      }
    }
  }

设定腿部控制器最大扭矩值为150，并使能腿部驱动。

  _legController->_maxTorque = 150;
  _legController->_legsEnabled = true;

以下部分解释以注释的形式给出。_calibrateEncoders和_zeroEncoders的作用

 if(userParameters.calibrate > 0.4) {//如果calibrate> 0.4，则将值赋给编码器校准
    _legController->_calibrateEncoders = userParameters.calibrate;
  } else {
    if(userParameters.zero > 0.5) {//如果calibrate< 0.4且zero > 0.5，_zeroEncoders为true
      _legController->_zeroEncoders = true;
    } else {//如果calibrate< 0.4且zero < 0.5，_zeroEncoders为false,并开始发送腿部位置指令
      _legController->_zeroEncoders = false;

      for(int leg(0); leg<4; ++leg){
        for(int jidx(0); jidx<3; ++jidx){
          float pos = std::sin(.001f * iter);//pos以sin函数赋值，函数运行频率为1k，sin函数变化周期为2*pi
          _legController->commands[leg].qDes[jidx] = pos;//将pos的值赋给各个关节
          _legController->commands[leg].qdDes[jidx] = 0.;//各个关节角速度为0
          _legController->commands[leg].tauFeedForward[jidx] = userParameters.tau_ff;//各个关节前馈力矩的大小，关节扭矩tau与足端力关系tau=J^T * f，足端力f = inv(J^T) * tau。前馈用于补偿一分部关节力用于计算足端力，实现方法在LegController.cpp中。
        }
        _legController->commands[leg].kpJoint = kpMat;//将设定的pd参数赋给控制器
        _legController->commands[leg].kdJoint = kdMat;
      }
    }
  }

腿部控制器的算法和主要实现方法在LegController.cpp中，是重点内容。

其中加载的userParameters文件和参数对应于仿真最右的面板，即user Control Parameters这些参数。

依据CMakeLists.txt中的编译设定 add_executable(jpos_ctrl main.cpp JPos_Controller.cpp)

（在终端中运行./user/JPos_Controller/jpos_ctrl代替./user/MIT_Controller/mit_ctrl m s，应该就可以运行该控制器，尚未验证，可能需要加上 m s的参数。)——发现并不是这样，直接运行报错，提示如下：

Simulation Error

Control code  has error: Control parameter Kd_body wasn't found in parameter collection user-parameters.

应该是缺少config/×××.yaml文件的对应配置，尝试后未发现匹配的文件。能跑通的同学可以在下面评论，我会更新到正文中。

解决办法在robot/src/HardwareBridge.cpp中，定位到这一句

_userControlParameters->initializeFromYamlFile(THIS_COM "config/mc-mit-ctrl-user-parameters.yaml");

将config/mc-mit-ctrl-user-parameters.yaml更改为c3-jpos-user-parameters.yaml即可。

同时，还有Example_Leg_InvDyn文件夹和MiniCheetahSpi_Controller文件中的控制器例子，可以作为编写自己控制器的模板，也可以选择其中一个进行改动。 

二、实战，添加控制器流程介绍

1.在CMakeLists.txt文件中，添加自己控制器的编译程序

add_executable(mycontroller ${SOURCES})

2.在与CMakeLists.txt文件同级目录下

复制包含main_helper(robot_controller);的文件，并将文件名称命名为mycontroller.cpp，这样编译后就会在同级目录下生成一个mycontroller的可执行文件。

找到新建控制器的一行，点进去MIT_Controller()跳转到MIT_Controller.cpp文件中

RobotController *robot_controller = new MIT_Controller();

在void MIT_Controller::runController() {}函数中找到

_controlFSM->runFSM();

点进去跳转到ControlFSM.cpp文件中，进入到状态机切换管理的函数中，在elseif中添加新的控制器模式，比如

        else if(rc_mode ==RC_mode::MYLOCOMOTION)
        {
            data.controlParameters->control_mode=K_MYLOCOMOTION;

添加是，在SateLists中也进行添加，对应的地方进行debug。

比如在ControFSM.cpp文件中

    statesList.MY_locomotion = new FSM_State_MY_Locomotion<T>(&data);

状态列表里添加一个新的控制器

在FSM_States文件夹下，复制出一组新的controller文件，例如FSM_State_MY_Locomotion.cpp和FSM_State_MY_Locomotion.h，在其中新建一个控制器的类。

class FSM_State_VMWBC_Locomotion : public FSM_State<T> {

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    FSM_State_VMWBC_Locomotion(ControlFSMData<T>* _controlFSMData);

    // Behavior to be carried out when entering a state
    void onEnter();

    // Run the normal behavior for the state
    void run();

    // Checks for any transition triggers
    FSM_StateName checkTransition();

    // Manages state specific transitions
    TransitionData<T> transition();

    // Behavior to be carried out when exiting a state
    void onExit();
.
.
.
.

}

在FSM_State_MY_Locomotion.cpp中，定义run()函数

template <typename T>
void FSM_State_MY_Locomotion<T>::run() {
    Timer t1;
    LocomotionControlStep();
//    printf("VM_WBC Solve time %f ms\n", t1.getMs());
}

定义LocomotionControlStep();函数，并跑起来控制器的run()函数

void FSM_State_VMWBC_Locomotion<T>::LocomotionControlStep() {
    cVM_WBC->run<T>(*this->_data);
    Vec3<T> pDes_backup[2];
    Vec3<T> vDes_backup[2];
    Mat3<T> Kp_backup[2];
    Mat3<T> Kd_backup[2];

。
。
。
}

最后在run()中编写自己的控制器函数。