这个是本人在大三期间做的项目 ---- 基于MIT的Cheetah方案设计的十二自由度并联四足机器人，这个项目获得过两个国家级奖项和一个省级奖项。接下来我会将这个机器人的控制部分所有代码进行开源，并配有相关的教程博客，希望能够帮助到在学习相关领域知识或者进行项目开发的同学。

学习建议

Webots是一款开源的多平台机器人仿真软件，也是目前最优秀的一款移动机器人仿真平台之一。
支持多种不同类型的机器人仿真，如工业机械臂，轮式机器人，足式机器人，履带式机器人，汽车，无人机，水下机器人，航天器等。
支持多种虚拟传感器，如相机，雷达，力传感器，位置传感器，陀螺仪，惯性单元，GPS等
还支持多种复杂环境的模拟，如室内，室外，崎岖路面，空中环境，水下环境等。

在本项目的开发中，主要使用了webots，并结合ROS进行仿真环境的搭建，实现了四足机器人力矩控制的仿真，大大缩减了实体测试成本。

因为webots在国内的普及程度不高，所以网上的教程也不多，很多细节问题还是需要读者在自己使用的过程中一点点摸索。
学习资料：我的学习笔记
ROS联合webots实战案例
webots建模指南

学习内容

搭建环境

webots的环境搭建主要包括有场景搭建以及参数配置两部分。

场景搭建

场景的地面一般是采用网格，大小按需配置，再把灯光打上，背景配上即可。

webots内置有很多完整的模型以及各种几何模型，从右方的列表中拖拽进仿真环境中，并进行组合，即可构建复杂的仿真环境。

参数配置

WorldInfo：包含模拟的全局参数。
Viewpoint：定义主要视点相机参数。
TexturedBackground：定义场景的背景（如果稍微旋转视点，您应该看到远处的山）
TexturedBackroundLight：定义与上述背景关联的光。
RectangleArena：定义到目前为止您在该场景中看到的唯一对象。

webots最核心的参数配置是仿真的控制频率：下图中的2代表仿真中每2ms执行一次控制指令，以及反馈外设数据，这个值最小为1。但是受限于电脑的CPU性能，实际值与设置值会有比较大的出入，可能无法达到这么高的控制频率。所以在仿真过程中，需要结合自己的电脑性能来确定该值，并且为了保证一致性，在实体控制时最好也与仿真的控制频率保持一致。

摩擦力系数：需要与实际测试场地的地面情况相匹配。

其余比较常用的属性：阴影开启/关闭，FPS值，坐标系方向等，则根据情况进行调整。
更加详细的内容可以参考该文章：webots环境建模

搭建机器人模型

webots的机器人模型主要有三个来源：软件内置，urdf模型导入以及软件内搭建。

软件内置

webots内置有非常多的机器人模型，并且大都配有相应的控制器，可以直接改写进行控制。

urdf模型导入

参考我的学习笔记：
sw模型生成urdf文件
urdf模型导入webots

软件内搭建

参考该文章：webots机器人建模

设计控制器

webots适配多种类型的控制器，如cpp，python，ROS等。这里主要介绍cpp+ROS的控制器设计方式。

webots是提供了内置的ROS控制器的，用户通过客户端-服务器的通信方式，即可控制机器人模型对应的外设。但是由于每个外设对应一个服务端，对于四足机器人（腿部一共有12个电机），这种方式会造成比较大的控制时延，即驱动不同步，会导致机器人仿真与实体控制的不一致。所以在本项目中，我重新编写了ROS控制器代码，提升了仿真控制效率。

ROS和webots联合编译配置参考该文章：webots模型联合ros仿真，实现ros自定义控制器

1. 配置外设名称

webots的外设以节点形式存在，名称是其唯一的标识，所以要保证每个外设的名称正确并且均不相同，如gyro，imu，RB-joint-1等。

2. 编写webots端控制器

主要负责有以下的功能：

1. 初始化外设
2. 监听webots反馈的外设数据，并打包发布
3. 监听控制指令，并进行数据解包，控制外设

/* 以下是本项目所用控制器的部分代码截取，仅供参考 */

// 包含相关的外设库
#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/Supervisor.hpp>
#include <webots/Accelerometer.hpp>
#include <webots/Gyro.hpp>

// 外设初始化
void webotsController::initDevice(void) {
    imu = robot->getInertialUnit("imu");
    imu->enable(timeStep);

    accel = robot->getAccelerometer("accelerometer");
    accel->enable(timeStep);

    gyro = robot->getGyro("gyro");
    gyro->enable(timeStep);

    for (int i = 0; i < 12; i++) {
        m[i / 3][i % 3] = robot->getMotor(motor_name[i / 3][i % 3]);
        p[i / 3][i % 3] = robot->getPositionSensor(sensor_name[i / 3][i % 3]);
        p[i / 3][i % 3]->enable(timeStep);

        // 使用supervisor来观测电机转速
        motor_v_visor[i / 3][i % 3] = robot->getFromDef(motor_name[i / 3][i % 3]);
    }
    
    // 初始化世界坐标系下的pv观测器
    world_p_v_visor = robot->getFromDef("Zouwu");
}

// imu数据更新
void webotsController::updateImuData(void){
    const double* data;

    // quat
    data = imu->getRollPitchYaw();
    Vec3<float> rpy(-data[0], data[1], data[2]);
    Quat<float> q = ori::rpyToQuat(rpy);
    imu_data.quat.w = q.w();
    imu_data.quat.x = q.x();
    imu_data.quat.y = q.y();
    imu_data.quat.z = q.z();

    // accel
    data = accel->getValues();
    imu_data.accel[0] = data[0];
    imu_data.accel[1] = data[1];
    imu_data.accel[2] = data[2];

    // gyro
    data = gyro->getValues();
    imu_data.gyro[0] = data[0];
    imu_data.gyro[1] = data[1];
    imu_data.gyro[2] = data[2];

    // publish
    imu_puber.publish(imu_data);
}

// 执行控制指令
void webotsController::robotCmd_Callback(const common::RobotCommandConstPtr& msg) {
    float tau;
    static int64_t iter = 0;
    // location control & force control
    for (int leg = 0; leg < 4; leg++) {
        if(msg->flags[leg]){
            // abad
            if(msg->mode_abad[leg]){
                tau = msg->kp_abad[leg] * (msg->q_des_abad[leg] - leg_data.q_abad[leg]) +
                    msg->kd_abad[leg] * (msg->qd_des_abad[leg] - leg_data.qd_abad[leg]) + msg->tau_abad_ff[leg];
                m[leg][0]->setTorque(tau);
            }
            else m[leg][0]->setPosition(msg->q_des_abad[leg]);

            // hip
            if(msg->mode_hip[leg]){
                tau = msg->kp_hip[leg] * (msg->q_des_hip[leg] - leg_data.q_hip[leg]) +
                    msg->kd_hip[leg] * (msg->qd_des_hip[leg] - leg_data.qd_hip[leg]) + msg->tau_hip_ff[leg];
                m[leg][1]->setTorque(tau);
            }
            else m[leg][1]->setPosition(msg->q_des_hip[leg] - ang_bias);

            // knee
            if(msg->mode_hip[leg]){
                tau = msg->kp_knee[leg] * (msg->q_des_knee[leg] - leg_data.q_knee[leg]) +
                    msg->kd_knee[leg] * (msg->qd_des_knee[leg] - leg_data.qd_knee[leg]) + msg->tau_knee_ff[leg];
                m[leg][2]->setTorque(-tau);
            }
            else m[leg][2]->setPosition(- msg-> q_des_knee[leg] - ang_bias);
        }
    }

    iter ++;
}

// 控制器运行函数
void webotsController::run(void) {
    // initiate
    initNode();

    // main loop
    while (robot->step(timeStep) != -1) {
        updateLegData();
        updateCheaterState();
        updateImuData();
        ros::spinOnce();
    };
}

3. 编写客户端控制器

主要负责有以下的功能：

1. 发布控制指令
2. 监听外设数据

/* 以下是本项目所用控制器的部分代码截取，仅供参考 */

// 发布控制指令
void Control_Node::updateRobotCommand(const RobotCommand* cmd) {
    if (cmd != nullptr) {
            convertToMsg(cmd, &_cmd);
            RobotCmd_Puber.publish(_cmd);
    }
}

// 监听IMU数据
void Control_Node::ImuData_Callback(const common::ImuDataConstPtr& msg) {
    VectorNavData nav;
    nav.quat.w() = msg->quat.w;
    nav.quat.x() = msg->quat.x;
    nav.quat.y() = msg->quat.y;
    nav.quat.z() = msg->quat.z;

    nav.gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
    nav.accelerometer << msg->accel[0], msg->accel[1], msg->accel[2];
    _robotRunner->updateVectorNavData(&nav);
}