Mini Cheetah 代码分析（一）

代码仓库链接：GitHub - mit-biomimetics/Cheetah-Software

此软件包含了真机和仿真模拟。

#首先是文件夹结构：

documentation文件夹包含了有关的入门指南。

common文件夹包含了动态和实用程序的公共库。包含了一些test测试程序和

resources文件夹包含了用于可视化的模型文件。

robot文件夹包含了机器人代码。

sim文件夹包含了仿真代码，只有这部分代码依赖QT。

third-party包含了经过修改过的第三方库。次级目录有Goldfarb_Optimizer优化器，JCQP，LordIMU驱动，qpOASES优化器，osqp优化器，SOEM的EtherCAT主站驱动，惯性导航系统VectorNav驱动。

robot文件夹包含了控制器代码。包含腿部逆动力学的例子Example_Leg_InvDyn，JPos_Controller，MiniCheetahSpi_Controller，MIT_Controller几个文件夹，主要用的是在MIT_Controller中。用的比较多的文件是\user\MIT_Controller\Controllers\convexMPC\ConvexMPCLocomotion.cpp

下面从documentation的getting_started.md文件开始分析，学会如何下载安装和运行MIT Controller，并创建自己的控制器。

#环境和依赖，Ubuntu系统，用于通信的LCM 1.3.1 ，可视化的Qt 5.10.0 or newer，矩阵运算的Eigen库。其他的安装使用

sudo apt install mesa-common-dev freeglut3-dev coinor-libipopt-dev libblas-dev liblapack-dev gfortran liblapack-dev coinor-libipopt-dev cmake gcc build-essential libglib2.0-dev

#下载代码

git clone https://github.com/mit-biomimetics/Cheetah-Software.git

#然后编译，有时候需要多编译几次。
 

cd Cheetah-Software
cd scripts # for now, you must actually go into this folder
./make_types.sh # you may see an error like `rm: cannot remove...` but this is okay
cd ..
mkdir build
cd build
cmake .. # there are still some warnings here
make -j

#测试是否成功安装

common文件中有些测试例程。切换目录到build文件夹中，运行common/test-common

OSQP和CASADI的测试会报错。

手柄：使用Logitech F310手柄，后部的开关应该在"X"位置，连接时，状态灯会熄灭。

#仿真例程

仿真环境可以通过yaml文件配置，主要是`config/simulator-defaults.yaml` 和`config/default-terrain.yaml`。default-terrain文件中提供了添加网格，盒子和台阶的注释掉的例子，可以修改底板摩擦力的大小（mu参数）。

开启仿真，编译过后，在build文件夹中，运行./sim/sim开启仿真界面窗口，选择 "Mini Cheetah" 和"Simulator", then 点击 "Start".出现机器人模型。左侧面板允许您更改模拟器设置。最有用的设置是“simulation_speed”，默认值加载在simulator-defaults.yaml文件中，当模拟器打开时，加载的设置文件值必须与代码中定义的参数完全相同。如果添加或删除参数，必须重新编译'sim'。可以随时保存和加载参数，“use_spring_damper”参数更改后需要重新启动模拟器才能生效。

中央面板用于更改控制器中未设置的机器人参数，默认值在“mini-cheetah-defatuls.yaml”文件中配置，单击“Start”时生效。如果停止并启动模拟器，文件将被重新加载。加载的设置文件必须与代码中定义的参数集完全相同。如果添加或删除参数，必须重新编译'sim'。目前，这些参数中的大多数都不起作用，最有用的设置是“cheater_mode”，可以向机器人发送机器人的当前位置/方向/速度，以及控制器采样间隔“controller_dt”。

右面板允许更改特定控制器的参数，称为用户参数"User Parameters"。如果写的控制器代码不使用用户参数，它将不起作用。如果代码中使用了了用户参数，则加载的代码的配置参数需要与代码中相同，否则控制器将无法启动。

要启动机器人控制器，运行`user/MIT_Controller/mit_ctrl m s`。“m”参数代表mini cheetah，“s”表示连接到仿真模拟器。它使用共享内存与模拟器通信。模拟器开始运行，机器人应该是趴着的位置。在模拟器窗口的中央面板，将控制模式（control mode）设置为10。一旦机器人停止移动，将控制模式（control mode）设置为1。然后，将控制模式设置为4，机器人将开始小跑。灰色的是模拟中机器人的实际位置，红色的是我们的状态估计器对机器人位置的估计。打开“作弊模式”将使估计位置与实际位置相等。要调整模拟视图，可以在屏幕上单击并拖动并滚动。按住“t”以使模拟尽可能快地运行。按空格键以打开“自由摄影机”模式，可以使用w、a、s、d、r、f键移动相机，然后单击并拖动以调整方向。

#调试

使用了LCM(https://lcm-proj.github.io/)将控制接口连接到实际的mini cheetah硬件，并在运行模拟器时用作调试工具。`make_types.sh`为LCM数据类型生成C++头文件。当模拟器运行时，可以运行`scripts/launch_lcm_spy.sh` 打开LCM spy程序，显示来自模拟器和控制器的详细信息。你可以点击数据流来绘图，这对调试来说很好。还有一个名为“lcm logger”的工具，可以将lcm数据保存到文件中。

#编写机器人控制器

要添加自己的机器人控制器，在“Cheetah Software/user”下添加一个文件夹，然后在user文件夹中的“CmakeLists.txt”中，将该文件夹目录添加进去。“JPos_Controller”是一个非常简单的控制器示例，“JPosUserParameters.h`文件声明了两个用户参数，可以从模拟器界面进行配置，但使用user parameters是可选的，非必须。“JPos_Controller.hpp”和"JPos_Controller.cpp"是实际控制器文件，他们应该继承于`RobotController`。在"JPos_Controller.cpp"控制器中`getUserControlParameters`方法返回指向用户参数的指针。如果不使用用户参数，“getUserControlParameters”应该返回“nullptr”。最后，“main”函数必须与“main.cpp”中的示例类似。

控制器的“runController”函数将以1 kHz的频率自动调用。在这里，您可以访问以下内容：

- `_quadruped` :包含有关机器人的模型参数（连杆长度、传动比、惯性等）。“getHipLocation”函数返回“hip”在身体坐标系中的位置。x轴指向前方，y轴指向左侧，z轴指向上方。腿是这样排列编号的

```
前
1 0  右
3 2
后
```

腿的编号leg index {0:FR, 1:FL, 2:BR, 3:BL}

坐标系建立如图

- `_model` : 包含机器人的动力学模型。这可以用来计算正运动学、雅可比矩阵等。

- `_legController`:机器人腿的接口。这些数据运行频率大约为700Hz的频率下，与硬件一致。有多种控制腿部的方法，所有控制器的结果都被添加到一起。

    - `commands[leg_id].tauFeedForward` :Leg的关节扭矩 (Nm, at the joint).  关节索引为1髋关节ab/ad, 2髋关节hip, 3膝盖关节knee。
    - `commands[leg_id].forceFeedForward` : 足端力 (N), 基于髋关节坐标系. (坐标系与身体坐标系方向相同，原点是髋部)
    - `commands[leg_id].qDes` : 关节PD控制器的期望的关节位置（弧度制），关节索引为1髋关节ab/ad, 2髋关节hip, 3膝盖关节knee。 `(0,0,0)` 表示腿竖直向下。.
    - `commands[leg_id].qdDes` : 关节PD控制器期望的关节速度（rad/sec）。

    - `commands[leg_id].pDes, vDes` : 笛卡尔坐标下PD控制器所需的脚位置/速度（米，基于髋关节坐标系）
    - `commands[leg_id].kpCartesian, kdCartesian, kpJoint, kdJoint` : PD控制器增益（3x3矩阵），对角矩阵。
    - `datas[leg_id].q` :腿关节编码器数值（弧度制）。关节索引为1髋关节ab/ad, 2髋关节hip, 3膝盖关节knee。 `(0,0,0)` 表示腿竖直向下。.
    - `datas[leg_id].qd` : 腿关节速度（弧度/秒）.  顺序与`q`相同。

    - `datas[leg_id].p`  : 在髋关节坐标系下足端的笛卡尔位置。(坐标系与身体坐标系方向相同，原点是髋部)
    - `datas[leg_id].v`  : 在髋关节坐标系下足端的速度。
    - `datas[leg_id].tau` : 从所有控制器的组合中估计电机转矩的大小，运行在电机控制回路的关节PD控制器实际上以40 kHz的频率运行。

- `_stateEstimate, _stateEstimatorContainer` 提供的状态估计器的结果和接口，如果提供了机器人的接触状态，它将决定机器人在世界坐标系上的位置/速度。

- `_driverCommand` : 手柄的输入。

- `_controlParameters` : 中央面板中输入的数值。

- `_visualizationData` : 将调试可视化添加到模拟器窗口的接口。

- `_robotType` : 选择是mini Cheetah还是Cheetah 3 robot。

如果你想了解更多的运行内容，查看“robot”文件夹。实际上由“RobotRunner”类运行控制代码，并与“HardwareBridge”或“SimulationBridge”连接。“rt”文件夹中的代码负责与硬件的交互。

#在机器人上运行

在机器人上运行与运行模拟器非常相似。通过手柄、用户参数和机器人参数。在模拟窗口中，您将只看到机器人的状态估计，作弊模式将不起作用，调试可视化不起作用。