rl_sar代码详解与使用

0. 前言

在现代机器人技术中，强化学习（Reinforcement Learning, RL）已经成为一个重要的研究领域，特别是在四足和人形机器人控制方面。本文将详细介绍rl_sar，一个由fan-ziqi大佬开源的四足/人形机器人强化学习算法仿真验证与实物部署框架。通过对该框架的深入分析，我们将探讨其安装、配置以及在ARM架构的Jetson平台上的实机部署方法。

1. rl_sar框架概述

rl_sar框架的设计旨在提供一个高效的强化学习训练环境，支持多种机器人平台，尤其是四足和人形机器人。该框架不仅可以在模拟环境中运行，还能将训练好的模型部署到实际机器人上，从而实现真实世界中的自主控制。

框架的核心功能包括：

• 强化学习算法的实现与训练
• 机器人运动控制的仿真与验证
• 实物机器人控制的部署
• 支持多种机器人平台的扩展

该框架的开源地址为：GitHub - rl_sar。

2. 安装配置说明（v2.1 ROS-noetic）

在使用rl_sar之前，我们需要确保环境的正确配置。以下是针对v2.1版本在ROS Noetic上的安装步骤。

2.1 拉取代码

首先，我们需要从GitHub上拉取rl_sar的代码：

git clone https://github.com/fan-ziqi/rl_sar.git
cd rl_sar
git tag  # 查看已有的release版本
git checkout v2.1  # 切换到v2.1版本

也可以直接在GitHub的release页面下载对应的.zip文件。

2.2 安装依赖

在安装rl_sar之前，我们需要安装一些必要的依赖包。

ROS依赖包

使用以下命令安装ROS相关依赖包：

sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller ros-noetic-joy

安装libtorch、yaml-cpp、lcm

接下来，我们需要安装libtorch、yaml-cpp和lcm库。

1. 安装libtorch

在任意目录下（以/home/ubuntu/Tools为例）下载并部署libtorch：

cd /home/ubuntu/Tools
wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.0.1%2Bcpu.zip
unzip libtorch-cxx11-abi-shared-with-deps-2.0.1+cpu.zip -d ./
echo 'export Torch_DIR=/home/ubuntu/Tools/libtorch' >> ~/.bashrc
source ~/.bashrc

如果在docker中使用ROS，请将该环境变量添加到docker的.bashrc中，而不是宿主机的.bashrc。

2. 安装yaml-cpp

使用以下命令安装yaml-cpp：

git clone https://github.com/jbeder/yaml-cpp.git
cd yaml-cpp && mkdir build && cd build
cmake -DYAML_BUILD_SHARED_LIBS=on .. && make
sudo make install
sudo ldconfig

3. 安装lcm

使用以下命令安装lcm：

git clone https://github.com/lcm-proj/lcm.git
cd lcm && mkdir build && cd build
cmake .. && make
sudo make install
sudo ldconfig

可能的报错解决：如果在安装lcm时出现Python.h: No such file or directory的错误，请按照以下步骤解决：

1. 通过pkg-config --cflags python3定位Python目录。
2. 在lcm的CMakeLists.txt中添加：

include_directories(BEFORE
    /usr/include/python3.8
    /usr/include/x86_64-linux-gnu/python3.8
)

2.3 编译

进入工作空间根目录并编译：

catkin build

如果在编译时遇到Unable to find either executable 'empy' or Python module 'em'的错误，请在执行catkin build之前执行以下命令：

catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3

2.4 运行

1. 仿真

打开一个终端，启动Gazebo仿真环境：

source devel/setup.bash
roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch

（以go2为例：roslaunch rl_sar gazebo_go2_isaacgym.launch）

在另一个终端中启动控制程序：

source devel/setup.bash
(for cpp version)    rosrun rl_sar rl_sim
(for python version) rosrun rl_sar rl_sim.py

2. 控制方式

• 键盘控制：

  • 按 空格键 切换仿真器运行/停止。
  • W/S 控制水平移动，A/D 控制转向，J/L 控制横向移动，按 0 将所有控制指令设置为零。
  • 如果机器人摔倒，按 R 重置Gazebo环境。
  • 按 0 让机器人从仿真开始的姿态运动到init_pos，按 1 让机器人从init_pos运动到仿真开始的姿态。

• 手柄控制：

  • 按 LB 切换仿真器运行/停止。
  • LY 控制前后移动，LX 控制横向移动，RX 控制转向。
  • 如果机器人摔倒，按 RB+X 重置Gazebo环境。
  • 按 RB+Y 让机器人从仿真开始的姿态运动到init_pos，按 RB+A 让机器人从init_pos运动到仿真开始的姿态。

3. 免网线机载部署配置说明

3.1 前言

Go2 EDU内置上层主控板NVIDIA Jetson Orin，已经预装了Ubuntu20.04，并且预装了两个版本的ROS（1&2），可以通过机身的全功能Type-C接口外接拓展坞后接显示器和鼠标键盘直接在内部开发。在安装配置rl_sar前一定要先解决好ROS的环境问题（例如默python版本等等），否则会遇见各种问题。

3.2 机载部署说明

在Go2机载部署rl_sar主要的问题有两个：

1. 正常下载的libtorch是专门为amd架构的电脑编译的，所以在arm板子上无法使用。
2. 宇树的sdk默认也是为amd架构的电脑编译的，所以在arm板子上也无法使用。

1. 下载并配置rl_sar

参照上文/官方README中正常的安装配置流程下载并且安装好依赖（步骤与之前相同），但是不要编译。

2. libtorch替换为专门为Jetson编译的版本

2.1 安装专门为Jetson编译的pytorch

首先，查看jetpack版本：

使用jtop工具查询Jetpack版本及系统信息，需要安装jetson-stats软件包，该软件包包含jtop工具。使用以下命令进行安装：

sudo pip3 install jetson-stats

安装完成后，启动服务：

sudo systemctl restart jtop.service

运行jtop命令：

sudo jtop

根据对应jetpack版本点击下载whl文件，例如：torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl。

安装依赖：

sudo apt-get -y update
sudo apt-get -y install autoconf bc build-essential g++-8 gcc-8 clang-8 lld-8 gettext-base gfortran-8 iputils-ping libbz2-dev libc++-dev libcgal-dev libffi-dev libfreetype6-dev libhdf5-dev libjpeg-dev liblzma-dev libncurses5-dev libncursesw5-dev libpng-dev libreadline-dev libssl-dev libsqlite3-dev libxml2-dev libxslt-dev locales moreutils openssl python-openssl rsync scons python3-pip libopenblas-dev

安装：

进入你存放whl的文件夹（安装前pip没换源的建议先换个源）：

pip install torch-2.1.0a0+41361538.nv23.06-cp38-cp38-linux_aarch64.whl

whl文件要换成你自己的文件名，安装完成后在终端进入python，通过import torch验证安装是否完成，如果没有报错说明安装成功。

2.2 在pytorch安装文件夹找到libtorch所需文件并替换

查看pytorch安装目录：

python
>>> import torch
>>> print(torch.__file__)

在torch文件夹中找到bin、include、lib、share四个文件夹，替换原本libtorch文件夹中的所有内容。

3. unitree sdk的编译版本更换

unitree sdk针对amd和arm架构都提供了相应的编译版本，而rl_sar默认使用的是amd版本，所以需要修改src/rl_sar中的cmakelist.txt：

1. 将下述代码中x86_64改为aarch64：

include_directories(library/unitree_sdk2/include)
link_directories(library/unitree_sdk2/lib/x86_64)
include_directories(library/unitree_sdk2/thirdparty/include)
include_directories(library/unitree_sdk2/thirdparty/include/ddscxx)
link_directories(library/unitree_sdk2/thirdparty/lib/x86_64)
set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx)

2. 将set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm)中的amd64改为arm64。

4. 编译&使用

此时正常编译即可，使用时运行：

rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>

代码中的<YOUR_NETWORK_INTERFACE>统一为eth0（可以通过ifconfig查看，eth0此时就是上层板子的IP，相当于上层板子取代你原本的电脑给下层板子发指令）。使用时手柄等操控方法不变。这时候就可以通过SSH或者远程桌面或者先接显示器跑程序然后拔下来等等一系列方法摆脱网线的束缚了。

实测是可以机载运行的，但是由于配置时间隔了挺长一段时间，并且没有新狗可以再安装一下进行测试，所以教程并不一定全面可能会遗漏一些细节，大家在机载配置的过程可能还会遇到其他的问题，但是主要的问题和解决办法已经给出，如果大家配置时遇到一些新的问题欢迎反馈和交流。

4. 代码实现

接下来，我们将讨论rl_sar框架中的代码实现部分，包括机器人控制、观察数据处理、模型推理等。

4.1 RL_Real类

RL_Real类是rl_sar框架的核心类之一，负责机器人控制和状态管理。以下是RL_Real类的构造函数和析构函数的实现：

RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_udp(UNITREE_LEGGED_SDK::LOWLEVEL)
{
    // 从YAML文件读取参数
    this->robot_name = "a1"; // 机器人名称
    this->config_name = "legged_gym"; // 配置名称
    std::string robot_path = this->robot_name + "/" + this->config_name; // 机器人路径
    this->ReadYaml(robot_path); // 读取YAML配置

    // 初始化机器人
    this->unitree_udp.InitCmdData(this->unitree_low_command); // 初始化命令数据，将传入的command数据初始化赋值为0

    // 初始化强化学习
    torch::autograd::GradMode::set_enabled(false); // 禁用梯度计算
    torch::set_num_threads(4); // 设置线程数
    if (!this->params.observations_history.empty())
    {
        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size()); // 初始化观察历史缓冲区
    }
    this->InitObservations(); // 初始化观察
    this->InitOutputs(); // 初始化输出
    this->InitControl(); // 初始化控制
    running_state = STATE_WAITING; // 设置初始运行状态为等待

    // 加载模型
    std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/" + this->params.model_name; // 模型路径
    this->model = torch::jit::load(model_path); // 加载TorchScript模型

    // 启动循环
    this->loop_udpSend = std::make_shared<LoopFunc>("loop_udpSend", 0.002, std::bind(&RL_Real::UDPSend, this), 3); // UDP发送循环
    this->loop_udpRecv = std::make_shared<LoopFunc>("loop_udpRecv", 0.002, std::bind(&RL_Real::UDPRecv, this), 3); // UDP接收循环
    this->loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&RL_Real::KeyboardInterface, this)); // 键盘输入循环
    this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Real::RobotControl, this)); // 机器人控制循环
    this->loop_rl = std::make_shared<LoopFunc>("loop_rl", this->params.dt * this->params.decimation, std::bind(&RL_Real::RunModel, this)); // 强化学习循环
    this->loop_udpSend->start(); // 启动UDP发送循环
    this->loop_udpRecv->start(); // 启动UDP接收循环
    this->loop_keyboard->start(); // 启动键盘输入循环
    this->loop_control->start(); // 启动机器人控制循环
    this->loop_rl->start(); // 启动强化学习循环
}

在构造函数中，我们首先读取机器人的配置参数，然后初始化UDP通信、观察、输出和控制等模块。之后，我们加载预训练的模型，并启动多个循环线程以处理不同的任务。

4.2 状态获取与命令设置

获取机器人状态和设置控制命令是机器人控制的重要环节。以下是相关函数的实现：

void RL_Real::GetState(RobotState<double> *state)
{
    this->unitree_udp.GetRecv(this->unitree_low_state); // 从UDP接收状态
    memcpy(&this->unitree_joy, this->unitree_low_state.wirelessRemote, 40); // 复制遥控器状态

    // 根据遥控器按钮更新控制状态
    if ((int)this->unitree_joy.btn.components.R2 == 1)
    {
        this->control.control_state = STATE_POS_GETUP; // 起身
    }
    else if ((int)this->unitree_joy.btn.components.R1 == 1)
    {
        this->control.control_state = STATE_RL_INIT; // RL初始化
    }
    else if ((int)this->unitree_joy.btn.components.L2 == 1)
    {
        this->control.control_state = STATE_POS_GETDOWN; // 躺下
    }

    // 复制陀螺仪和加速度计数据
    for (int i = 0; i < 3; ++i)
    {
        state->imu.gyroscope[i] = this->unitree_low_state.imu.gyroscope[i];
    }
    for (int i = 0; i < this->params.num_of_dofs; ++i)
    {
        state->motor_state.q[i] = this->unitree_low_state.motorState[this->params.state_mapping[i]].q; // 关节位置
        state->motor_state.dq[i] = this->unitree_low_state.motorState[this->params.state_mapping[i]].dq; // 关节速度
        state->motor_state.tau_est[i] = this->unitree_low_state.motorState[this->params.state_mapping[i]].tauEst; // 估计的扭矩
    }
}

void RL_Real::SetCommand(const RobotCommand<double> *command)
{
    for (int i = 0; i < this->params.num_of_dofs; ++i)
    {
        this->unitree_low_command.motorCmd[i].mode = 0x0A; // 设置控制模式
        this->unitree_low_command.motorCmd[i].q = command->motor_command.q[this->params.command_mapping[i]]; // 设置目标位置
        this->unitree_low_command.motorCmd[i].dq = command->motor_command.dq[this->params.command_mapping[i]]; // 设置目标速度
        this->unitree_low_command.motorCmd[i].Kp = command->motor_command.kp[this->params.command_mapping[i]]; // 设置位置控制增益
        this->unitree_low_command.motorCmd[i].Kd = command->motor_command.kd[this->params.command_mapping[i]]; // 设置速度控制增益
        this->unitree_low_command.motorCmd[i].tau = command->motor_command.tau[this->params.command_mapping[i]]; // 设置目标扭矩
    }

    // 执行保护措施
    this->unitree_safe.PowerProtect(this->unitree_low_command, this->unitree_low_state, 8);
    this->unitree_udp.SetSend(this->unitree_low_command); // 发送控制命令
}

在GetState函数中，我们从UDP接收机器人状态，并根据遥控器的输入更新控制状态。而在SetCommand函数中，我们将计算得到的控制命令发送给机器人，确保其按照预期的方式运动。

4.3 强化学习循环

强化学习的核心在于通过与环境的交互不断优化策略。以下是RunModel函数的实现：

void RL_Real::RunModel()
{
    if (this->running_state == STATE_RL_RUNNING) // 如果处于RL运行状态
    {
        // 更新观察数据
        this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0); // 角速度
        this->obs.commands = torch::tensor({{this->unitree_joy.ly, -this->unitree_joy.rx, -this->unitree_joy.lx}}); // 控制命令
        this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0); // 基础四元数
        this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0); // 自由度位置
        this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0); // 自由度速度

        // 前向推理
        this->obs.actions = this->Forward();
        this->ComputeOutput(this->obs.actions, this->output_dof_pos, this->output_dof_vel, this->output_dof_tau); // 计算输出

        // 将输出推送到队列
        if (this->output_dof_pos.defined() && this->output_dof_pos.numel() > 0)
        {
            output_dof_pos_queue.push(this->output_dof_pos);
        }
        if (this->output_dof_vel.defined() && this->output_dof_vel.numel() > 0)
        {
            output_dof_vel_queue.push(this->output_dof_vel);
        }
        if (this->output_dof_tau.defined() && this->output_dof_tau.numel() > 0)
        {
            output_dof_tau_queue.push(this->output_dof_tau);
        }
    }
}

在RunModel函数中，我们首先更新观察数据，然后使用前向推理计算出动作，并将其传递给控制模块。该模块负责将动作转换为具体的控制命令。

4.4 观察数据处理

观察数据的处理是强化学习中至关重要的一步。以下是ComputeObservation函数的实现：

torch::Tensor RL::ComputeObservation()
{
    std::vector<torch::Tensor> obs_list; // 用于存储观察数据的列表

    // 遍历所有观察参数
    for (const std::string &observation : this->params.observations)
    {
        if (observation == "lin_vel") // 线速度
        {
            obs_list.push_back(this->obs.lin_vel * this->params.lin_vel_scale); // 将线速度缩放后添加到列表
        }
        else if (observation == "ang_vel_body") // 机体坐标系下的角速度
        {
            obs_list.push_back(this->obs.ang_vel * this->params.ang_vel_scale); // 将角速度缩放后添加到列表
        }
        // 其他观察参数处理...
    }

    // 将所有观察数据连接成一个Tensor
    torch::Tensor obs = torch::cat(obs_list, 1);
    // 将观察数据限制在指定范围内
    torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs);
    return clamped_obs; // 返回限制后的观察数据
}

在ComputeObservation函数中，我们从机器人状态中提取出各种观察数据，并对其进行缩放和限制，最终返回一个统一的观察数据tensor。

5. 强化学习模型的观察数据计算

在强化学习框架中，计算观察数据是模型决策过程中的关键环节。以下是RL类中ComputeObservation()函数的详细实现，此函数负责提取和处理机器人状态信息，生成适合模型输入的观察数据。

5.1 观察数据计算实现

torch::Tensor RL::ComputeObservation()
{
    std::vector<torch::Tensor> obs_list; // 用于存储观察数据的列表

    // 遍历所有观察参数
    for (const std::string &observation : this->params.observations)
    {
        if (observation == "lin_vel") // 线速度
        {
            obs_list.push_back(this->obs.lin_vel * this->params.lin_vel_scale); // 将线速度缩放后添加到列表
        }
        else if (observation == "ang_vel_body") // 机体坐标系下的角速度
        {
            obs_list.push_back(this->obs.ang_vel * this->params.ang_vel_scale); // 将角速度缩放后添加到列表
        }
        else if (observation == "ang_vel_world") // 世界坐标系下的角速度
        {
            obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale); // 转换并缩放后添加到列表
        }
        else if (observation == "gravity_vec") // 重力向量
        {
            obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec, this->params.framework)); // 转换重力向量并添加到列表
        }
        else if (observation == "commands") // 控制命令
        {
            obs_list.push_back(this->obs.commands * this->params.commands_scale); // 将控制命令缩放后添加到列表
        }
        else if (observation == "dof_pos") // 自由度位置
        {
            torch::Tensor dof_pos_rel = this->obs.dof_pos - this->params.default_dof_pos; // 计算相对位置
            for (int i : this->params.wheel_indices) // 对于轮子索引
            {
                dof_pos_rel[0][i] = 0.0; // 将轮子位置设为0
            }
            obs_list.push_back(dof_pos_rel * this->params.dof_pos_scale); // 将相对位置缩放后添加到列表
        }
        else if (observation == "dof_vel") // 自由度速度
        {
            obs_list.push_back(this->obs.dof_vel * this->params.dof_vel_scale); // 将速度缩放后添加到列表
        }
        else if (observation == "actions") // 动作
        {
            obs_list.push_back(this->obs.actions); // 添加动作到列表
        }
    }

    // 将所有观察数据连接成一个Tensor
    torch::Tensor obs = torch::cat(obs_list, 1);
    torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); // 将观察数据限制在指定范围内
    return clamped_obs; // 返回限制后的观察数据
}

• 观察参数遍历：函数遍历所有定义的观察参数，根据不同的参数名称提取对应的数据并进行处理。
• 数据缩放：在提取线速度、角速度、重力向量等数据时，应用相应的缩放因子以确保数据的量纲一致。
• 坐标系转换：使用QuatRotateInverse函数将某些数据从世界坐标系转换为机体坐标系，确保模型在训练时能够正确理解状态信息。
• Tensor拼接：所有观察数据被拼接成一个单一的Tensor，并限制在预定义的范围内，以便于后续模型处理。

…详情请参照古月居