unitree_guide/ros/sdk 安装参考：(我的环境跟博主是一样的)

四足机器人控制算法——建模、控制与实践（unitree_guide配置、自主导航ROS仿真）-CSDN博客文章浏览阅读6.9k次，点赞41次，收藏98次。宇树科技 unitree_guide 配置_四足机器人控制算法https://blog.csdn.net/Teriri_/article/details/141787540?fromshare=blogdetail&sharetype=blogdetail&sharerId=141787540&sharerefer=PC&sharesource=m0_60673426&sharefrom=from_link

基于宇树《四足机器人控制算法》学习笔记

1.电机控制模式

•  位置模式：电机输出转到一个固定的目标位置
• 速度模式：电机固定速度转动
• 阻尼模式：开机时自动进入，固定扭矩无法转动
• 力矩模式：输出恒定力矩
• 零力矩模式：转轴力矩为0，阻力很小
• 混合控制：同时输出力矩，速度和位置，应用最多
• 电机运行模式(mode)：0：停转；5：开环；10：伺服闭环

2.机器狗状态机

• 启动时为passive （阻尼模式）：机器狗为趴下状态
• 按键2为fixedStand（固定站立模式）：关节电机转至给定值锁死。
• 按键3为freeStand（自由站立）：可以根据手柄或键盘命令更改姿态。
• 按键4为trotting（对脚步态模式）：使用键盘或手柄控制时，用对角步态行走。
• 按键5为moveBase（导航避障模式）：将move_base的命令转为机器狗运动指令

•  状态机执行函数：
  • 在FSMState.h头文件中定义了基类其中关键的函数如下
            

        virtual void enter() = 0;//进入状态(切换状态时执行)
        virtual void run() = 0;//运行状态(一直循环执行)
        virtual void exit() = 0;//退出状态(退出该状态时执行)
        virtual FSMStateName checkChange() {return FSMStateName::INVALID;}//检查状态转换条件(是否需要切换其他步态)

3.关节零角度定义

        遵循右手坐标系，机器人正前方为 X，向左为Y，向上为Z

        小腿大腿垂直于地面为0角度(小腿大腿平行，实际并不能到达0角度，只作为定义)

4. 固定站立模式

• 在unitree_guide\include\FSM\FSMState.h 中定义了FSMState 的基类

  • class FSMState{
    public:
        FSMState(CtrlComponents *ctrlComp, FSMStateName stateName, std::string stateNameString);
    
        virtual void enter() = 0;//进入状态函数，切换状态后执行一次
        virtual void run() = 0;//状态循环函数，没有检测到切换状态，就一直在这个函数循环
        virtual void exit() = 0;//检测到切换状态时退出此状态
        virtual FSMStateName checkChange() {return FSMStateName::INVALID;}//检查状态变更
    
        FSMStateName _stateName;
        std::string _stateNameString;
    protected:
        CtrlComponents *_ctrlComp;
        FSMStateName _nextStateName;
    
        LowlevelCmd *_lowCmd;
        LowlevelState *_lowState;
        UserValue _userValue;
    };

• State_FixedStand.cpp中继承了FSMState类。它用于控制一个机器人在固定站立状态下的行为。

  class State_FixedStand : public FSMState {//State_FixedStand`继承自`FSMState
  public:
      State_FixedStand(CtrlComponents *ctrlComp)
                  : FSMState(ctrlComp, FSMStateName::FIXEDSTAND, "fixed stand") {}
  //初始化状态的基本信息，包括控制组件、状态名称

• enter()方法

  • 循环遍历四个腿，根据当前控制平台（Gazebo或real）设置相应的kp/kd。
      - 将每个腿的速度（dq）和力矩（tau）设为零。（位置模式不需要力矩和转速，只需要角度值)
      - 循环遍历12个电机，将每个电机的目标角度设为当前角度，并记录起始位置。
      - 调用setAllStance()，用于设置机器人的整体姿态。

void State_FixedStand::enter() {//进入状态时的初始化函数
    for(int i=0; i<4; i++) {
        if(_ctrlComp->ctrlPlatform == CtrlPlatform::GAZEBO) {
            _lowCmd->setSimStanceGain(i);
        } else if(_ctrlComp->ctrlPlatform == CtrlPlatform::REALROBOT) {
            _lowCmd->setRealStanceGain(i);
        }
        _lowCmd->setZeroDq(i);//转速设0
        _lowCmd->setZeroTau(i);//力矩设0
    }
    for(int i=0; i<12; i++) {
        _lowCmd->motorCmd[i].q = _lowState->motorState[i].q;
        _startPos[i] = _lowState->motorState[i].q;//记录起始位置
    }
    _ctrlComp->setAllStance();
}
```
/*循环遍历四个腿，根据当前控制平台（Gazebo或real）设置相应的kp/kd。
  - 将每个腿的速度（dq）和力矩（tau）设为零。
  - 循环遍历12个电机，将每个电机的目标角度设为当前角度，并记录起始位置。
  - 调用setAllStance()，用于设置机器人的整体姿态。*/

include\message\LowlevelCmd.h
 void setSimStanceGain(int legID){
        motorCmd[legID*3+0].mode = 10;
        motorCmd[legID*3+0].Kp = 180;
        motorCmd[legID*3+0].Kd = 8;
        motorCmd[legID*3+1].mode = 10;
        motorCmd[legID*3+1].Kp = 180;
        motorCmd[legID*3+1].Kd = 8;
        motorCmd[legID*3+2].mode = 10;
        motorCmd[legID*3+2].Kp = 300;//小腿力臂最大，负载也最大值调高
        motorCmd[legID*3+2].Kd = 15;
    }

• run()方法

  • 从起始角度到目标角度进行线性插值，使关节缓慢运行至目标角度

void State_FixedStand::run() {
    _percent += (float)1/_duration;
    _percent = _percent > 1 ? 1 : _percent;
    for(int j=0; j<12; j++) {
        _lowCmd->motorCmd[j].q = (1 - _percent)*_startPos[j] + _percent*_targetPos[j]; 
    }
}
/*持续执行的行为：
  - 增加完成百分比：根据状态持续时间（`_duration`）线性增加完成度（`_percent`），最多达到1。
  - 插值目标位置：每个电机的目标角度根据起始位置（`_startPos`）和目标位置（`_targetPos`）进行线性插值，逐步移动到目标位置。*/

• exit()方法

```cpp
void State_FixedStand::exit() {
    _percent = 0;
}
```
//离开状态时的清理：重置完成百分比，以便下次进入状态时重新开始。

• checkChange()方法

FSMStateName State_FixedStand::checkChange() {
    if(_lowState->userCmd == UserCommand::L2_B) {
        return FSMStateName::PASSIVE;
    } else if(_lowState->userCmd == UserCommand::L2_X) {
        return FSMStateName::FREESTAND;
    } else if(_lowState->userCmd == UserCommand::START) {
        return FSMStateName::TROTTING;
    } else if(_lowState->userCmd == UserCommand::L1_X) {
        return FSMStateName::BALANCETEST;
    } else if(_lowState->userCmd == UserCommand::L1_A) {
        return FSMStateName::SWINGTEST;
    } else if(_lowState->userCmd == UserCommand::L1_Y) {
        return FSMStateName::STEPTEST;
    }
#ifdef COMPILE_WITH_MOVE_BASE
    else if(_lowState->userCmd == UserCommand::L2_Y) {
        return FSMStateName::MOVE_BASE;
    }
#endif  // COMPILE_WITH_MOVE_BASE
    else {
        return FSMStateName::FIXEDSTAND;
    }
}

/*状态转换检查：
  - 根据用户命令（`userCmd`）判断是否需要转换到其他状态。
  - 返回对应的状态枚举值，如`PASSIVE`、`FREESTAND`等。
  - 如果没有有效命令，保持当前状态（`FIXEDSTAND`）。*/

5.编译选项

   文件目录：unitree_guide\CMakeLists.txt

cmake_minimum_required(VERSION 3.14)
project(unitree_guide)

set(ROBOT_TYPE Go1)         # 机器人型号, support Go1 and A1 currently
set(PLATFORM amd64)         #上位机环境架构 to compile, support amd64 and arm64

set(CATKIN_MAKE ON)        # Use CATKIN_MAKE or not, ON or OFF
set(SIMULATION ON)         # Use Gazebo or not, ON or OFF
set(REAL_ROBOT OFF)          # Link real robot or not, ON or OFF
set(DEBUG OFF)              # Use debug functions or not, ON or OFF
set(MOVE_BASE OFF)               # 是否使用导航模式

6.