宇树四足机器狗代码详解[unitree_ros]中的unitree_legged_control功能包：代码框架整理3

不摆烂的fy

于 2024-08-30 15:49:51 发布

阅读量1k

点赞数 10

文章标签： ubuntu linux 开源机器人 c++

本文链接：https://blog.csdn.net/BIT_TF/article/details/141691286

版权

#这一篇文章会讲讲[unitree_ros](https://github.com/unitreerobotics/unitree_ros)的代码框架，仅代表个人看法，写出来加深自己的印象，欢迎交流。

#附上宇树家的学习资料[《四足机器人控制算法--建模、控制与实践》](https://detail.tmall.com/item.htm？spm=a212k0.12153887.0.0.5487687dBgiovR&id=704510718152)#

下载了GitHub上的压缩包之后，有五个文件（第五个unitree_ros_to_real是空的)，

robots装的是宇树家不同机器狗的描述文件

unittree_controller是一个控制器功能包

unitree_legged_control是底层电机控制功能包

前面两篇文章把unittree_controller控制器功能包的代码分析完了，这篇文章分析unitree_legged_control是底层电机控制功能包

如果对于CMakeLists和package.xml不知道是干嘛的，可以去学习一下ros的基本架构。

1.joint_controller.h

/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
//定义了一个名为 UnitreeJointController 的类，该类继承自 controller_interface::Controller，用于实现ROS中的关节控制器，可能是用于控制Unitree Robotics公司制造的机器人腿部的电机
#ifndef _UNITREE_ROS_JOINT_CONTROLLER_H_//这是预处理器指令，用于防止头文件被多次包含，是C++中常见的头文件保护宏
#define _UNITREE_ROS_JOINT_CONTROLLER_H_

#include <ros/node_handle.h>//节点处理
#include <urdf/model.h>//urdf模型解析
#include <control_toolbox/pid.h>//PID控制
#include <boost/scoped_ptr.hpp>//包含Boost库中的智能指针
#include <boost/thread/condition.hpp>//包含Boost库中的线程同步机制的头文件
#include <realtime_tools/realtime_publisher.h>//实时发布
#include <hardware_interface/joint_command_interface.h>//包含硬件接口
#include <controller_interface/controller.h>//控制器接口的头文件，这些接口定义了与机器人硬件通信的标准方式。
#include <std_msgs/Float64.h>//用于发射和接收64位浮点数
#include <realtime_tools/realtime_buffer.h>//包含实时缓冲区的头文件，用于同步节点间的实时消息传递
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include "unitree_legged_msgs/MotorCmd.h"
#include "unitree_legged_msgs/MotorState.h"
#include <geometry_msgs/WrenchStamped.h>//
#include "unitree_joint_control_tool.h"//包含自定义的Unitree关节控制工具的头文件，可能包含一些用于控制机器人关节的辅助函数或类

#define PMSM      (0x0A) //一些宏定义值 16进制，代表10 ，应该是电机模式 mode=10 PMSM
#define BRAKE     (0x00)
#define PosStopF  (2.146E+9f) //位置停止
#define VelStopF  (16000.0f)  //速度停止

namespace unitree_legged_control
{
    class UnitreeJointController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
    {
private:
        hardware_interface::JointHandle joint;//定义一个关节句柄，用于与机器人的硬件接口进行交互
        ros::Subscriber sub_cmd, sub_ft;//两个订阅者Subscriber，用于接收控制命令sub_cmd，和接收力矩反馈sub_ft
        // ros::Publisher pub_state;
        control_toolbox::Pid pid_controller_;//实例化一个PID控制对象pid_controller_
        boost::scoped_ptr<realtime_tools::RealtimePublisher<unitree_legged_msgs::MotorState> > controller_state_publisher_ ;//一个智能指针，指向实时发布器，用于发布电机状态

public:
        // bool start_up;
        std::string name_space;//定义变量，存储命名空间
        std::string joint_name;//存储关节名称
        float sensor_torque;//用于存储传感器测量的扭矩值
        bool isHip, isThigh, isCalf, rqtTune;//布尔变量，表示关节类型和是否使用rqt工具进行PID参数调整。
        urdf::JointConstSharedPtr joint_urdf;//指向urdf关节的指针
        realtime_tools::RealtimeBuffer<unitree_legged_msgs::MotorCmd> command;//实时缓冲区，用于存储接到的电机命令
        unitree_legged_msgs::MotorCmd lastCmd;//定义结构体变量 lastCmd
        unitree_legged_msgs::MotorState lastState;//
        ServoCmd servoCmd;//可能是一个用于控制伺服电机的自定义命令结构体，定义在unitree_joint_control_tool.h

        UnitreeJointController();//构造函数和析构函数
        ~UnitreeJointController();
        virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);//虚函数，初始化函数，设置控制器的参数和订阅者
        virtual void starting(const ros::Time& time);//启动前调用，应该是开计时器？
        virtual void update(const ros::Time& time, const ros::Duration& period);//更新函数，根据实时循环调用，实现控制逻辑
        virtual void stopping();//停止时调用，开始清理
        void setTorqueCB(const geometry_msgs::WrenchStampedConstPtr& msg);//回调函数，用于处理订阅的力/扭矩和电机命令消息。
        void setCommandCB(const unitree_legged_msgs::MotorCmdConstPtr& msg);
        void positionLimits(double &position);//获取位置、速度、力的极限值）
        void velocityLimits(double &velocity);
        void effortLimits(double &effort);

        void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
        void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);//用于设置和获取PID控制器的增益
        void getGains(double &p, double &i, double &d, double &i_max, double &i_min);

    };
}

#endif

2.joint_controller.cpp（具体实现文件）

/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
                                                        //这个文件定义了unitree_legged_control命名空间，注册了一个ROS插件用于电机控制
// #include "unitree_legged_control/joint_controller.h"
#include "joint_controller.h"
#include <pluginlib/class_list_macros.h>//这个包含指令是用于引入插件库的宏定义。pluginlib是ROS中用于管理插件的一个库
//class_list_macros.h提供了宏，这些宏用于自动注册插件和创建类列表，使得插件能够在运行时被识别和加载
// #define rqtTune // use rqt or not

namespace unitree_legged_control //定义了unitree_legged_control命名空间，用于封装相关的代码以避免命名冲突
{

    UnitreeJointController::UnitreeJointController(){
        memset(&lastCmd, 0, sizeof(unitree_legged_msgs::MotorCmd));
        memset(&lastState, 0, sizeof(unitree_legged_msgs::MotorState));
        memset(&servoCmd, 0, sizeof(ServoCmd)); //构造函数 
    } //使用memset函数将这些变量的内存区域设置为零。这通常用于初始化结构体或类的实例，确保所有成员变量都有确定的初始值

    UnitreeJointController::~UnitreeJointController(){
        sub_ft.shutdown();//析构函数，调用了两个订阅者的shutdown方法，这将取消订阅话题，释放相关资源
        sub_cmd.shutdown();
    }
//回调函数，用于处理订阅到的力矩消息
    void UnitreeJointController::setTorqueCB(const geometry_msgs::WrenchStampedConstPtr& msg)
    {
        if(isHip) sensor_torque = msg->wrench.torque.x;
        else sensor_torque = msg->wrench.torque.y;
        // printf("sensor torque%f\n", sensor_torque);
    }//根据isHip变量的值，它从消息中提取x或y分量作为传感器扭矩，并将其存储在sensor_torque成员变量中。注释掉的printf语句可能是用于调试目的
//回调函数，用于处理订阅到的电机命令消息
    void UnitreeJointController::setCommandCB(const unitree_legged_msgs::MotorCmdConstPtr& msg)
    {
        lastCmd.mode = msg->mode;
        lastCmd.q = msg->q;
        lastCmd.Kp = msg->Kp;
        lastCmd.dq = msg->dq;
        lastCmd.Kd = msg->Kd;
        lastCmd.tau = msg->tau;
        // the writeFromNonRT can be used in RT, if you have the guarantee that
        //  * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
        //  * there is only one single rt thread
        command.writeFromNonRT(lastCmd);//使用writeFromNonRT方法将命令写入实时缓冲区，这个方法适用于实时线程，用于在实时和非实时线程之间同步数据
    }

    // Controller initialization in non-realtime ROS控制器接口的一部分，用于在非实时环境中初始化控制器
    bool UnitreeJointController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
    {//接受两个参数：一个指向EffortJointInterface的指针，用于与机器人的硬件接口交互；一个ros::NodeHandle引用，用于处理ROS参数和话题
        isHip = false;//标志变量设为false
        isThigh = false;
        isCalf = false;
        // rqtTune = false; //用于判断是否用rqt工具调整了pid参数
        sensor_torque = 0;//传感器力矩初设为0
        name_space = n.getNamespace();
        if (!n.getParam("joint", joint_name)){//尝试从ROS参数服务器获取名为joint的参数，该参数应包含要控制的关节名称。
            ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str());//不存在就打印报错信息
            return false;
        }
        
        // load pid param from ymal only if rqt need 
        // if(rqtTune) {
#ifdef rqtTune //使用ros::NodeHandle创建的子节点句柄初始化pid_controller_成员变量。这个子节点句柄指向参数服务器上的pid节点，其中应包含PID控制器的参数
            // Load PID Controller using gains set on parameter server
            if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
                return false;
#endif
        // }
//解析urdf模型 设置关节属性、初始化ros发布者和订阅者
        urdf::Model urdf; // Get URDF info about joint
        if (!urdf.initParamWithNodeHandle("robot_description", n)){//使用initParamWithNodeHandle方法初始化URDF模型，该方法从ROS参数服务器获取名为robot_description的URDF描述。如果初始化失败，打印错误并返回false
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }
        joint_urdf = urdf.getJoint(joint_name);//通过getJoint方法获取名为joint_name的关节的URDF表示
        if (!joint_urdf){
            ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());//没找到就打印出来
            return false;
        }
        if(joint_name == "FR_hip_joint" || joint_name == "FL_hip_joint" || joint_name == "RR_hip_joint" || joint_name == "RL_hip_joint"){
            isHip = true;//如果名称等于hip髋关节，就把hip标志变量设为真
        }
        if(joint_name == "FR_calf_joint" || joint_name == "FL_calf_joint" || joint_name == "RR_calf_joint" || joint_name == "RL_calf_joint"){
            isCalf = true;//如果名称等于Calf小腿关节，标志变量设为真
        }        
        joint = robot->getHandle(joint_name);//使用EffortJointInterface的getHandle方法获取名为joint_name的关节的硬件接口句柄。

        // Start command subscriber
        sub_ft = n.subscribe(name_space + "/" +"joint_wrench", 1, &UnitreeJointController::setTorqueCB, this);//订阅名为name_space + "/joint_wrench"的话题，回调函数为setTorqueCB，存到sensor_torque
        sub_cmd = n.subscribe("command", 20, &UnitreeJointController::setCommandCB, this);//订阅话题名称为“command”的topic，队列长度为20，消息在回调函数中存到lastCmd

        // pub_state = n.advertise<unitree_legged_msgs::MotorState>(name_space + "/state", 20); 
        // Start realtime state publisher 开始发布实时状态
        controller_state_publisher_.reset(
            new realtime_tools::RealtimePublisher<unitree_legged_msgs::MotorState>(n, name_space + "/state", 1));//使用realtime_tools::RealtimePublisher创建一个实时发布者，用于发布关节状态        

        return true;
    }
//调用pid_controller_.setGains方法设置pid参数p i d i最大值 i最小值 布尔值antiwindup，指示是否启用防积分饱和功能
    void UnitreeJointController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
    {
        pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
    }
//调用pid_controller_.setGains方法获得pid参数p i d i最大值 i最小值 布尔值antiwindup，指示是否启用防积分饱和功能
    void UnitreeJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
    {
        pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
    }
//一个重载，当调用者不需要返回防积分饱和状态时使用。它通过一个临时布尔变量dummy来忽略防积分饱和状态的返回值
    void UnitreeJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
    {
        bool dummy;
        pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
    }

    // Controller startup in realtime
    void UnitreeJointController::starting(const ros::Time& time)
    {
        // lastCmd.Kp = 0;
        // lastCmd.Kd = 0;
        double init_pos = joint.getPosition();//设置初始位置为 获取当前位置
        lastCmd.q = init_pos;   //把值赋给lastCmd
        lastState.q = init_pos; //把值赋给lastState
        lastCmd.dq = 0;
        lastState.dq = 0;
        lastCmd.tau = 0;
        lastState.tauEst = 0;
        command.initRT(lastCmd); //初始化缓冲区

        pid_controller_.reset();//重置pid控制器
    }

    // Controller update loop in realtime 实时更新控制器，参数分别表示当前时间点和周期
    void UnitreeJointController::update(const ros::Time& time, const ros::Duration& period)
    {
        double currentPos, currentVel, calcTorque;//声明变量 当前位置 当前速度 计算得到的扭矩
        lastCmd = *(command.readFromRT());//从实时命令缓冲区读取最新的命令，并将其存储在lastCmd变量中

        // set command data
        if(lastCmd.mode == PMSM) { //mode=10 PMSM（永磁同步电机模式）
            servoCmd.pos = lastCmd.q;
            positionLimits(servoCmd.pos);//限制位置的范围
            servoCmd.posStiffness = lastCmd.Kp;//位置刚度设置
            if(fabs(lastCmd.q - PosStopF) < 0.00001){
                servoCmd.posStiffness = 0; //有一点抖动正常，不可能位置完全为0，而阈值就是PosStopF
            }
            servoCmd.vel = lastCmd.dq;  //速度值也做一次重复的工作
            velocityLimits(servoCmd.vel);
            servoCmd.velStiffness = lastCmd.Kd;
            if(fabs(lastCmd.dq - VelStopF) < 0.00001){
                servoCmd.velStiffness = 0;
            }
            servoCmd.torque = lastCmd.tau; //赋值力矩给servoCmd
            effortLimits(servoCmd.torque); //限制力矩的值
        }
        if(lastCmd.mode == BRAKE) { //如果退出永磁同步电机模式
            servoCmd.posStiffness = 0;
            servoCmd.vel = 0;
            servoCmd.velStiffness = 20; //速度刚度（即阻尼c）
            servoCmd.torque = 0;
            effortLimits(servoCmd.torque);
        }

        // } else {
        //     servoCmd.posStiffness = 0;
        //     servoCmd.velStiffness = 5;
        //     servoCmd.torque = 0;
        // }
        
        // rqt set P D gains
        // if(rqtTune) {
#ifdef rqtTune //如果定义了rqtTune，就编译下面
            double i, i_max, i_min;
            getGains(servoCmd.posStiffness,i,servoCmd.velStiffness,i_max,i_min);
#endif  //通过getGains函数获取当前的PID增益，包括比例增益、积分增益、积分最大值、积分最小值和速度刚度
        // } 

        currentPos = joint.getPosition();  //从硬件接口获取关节的当前位置，并赋值给currentPos
        currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec());//据当前位置、上次状态的位置和速度，以及周期（period.toSec()），计算当前速度currentVel
        calcTorque = computeTorque(currentPos, currentVel, servoCmd);   //调用computeTorque函数，根据当前位置、当前速度、电机当前状态 计算所需的扭矩
        effortLimits(calcTorque);//限值

        joint.setCommand(calcTorque);//将计算出的扭矩calcTorque作为命令发送到硬件接口

        lastState.q = currentPos; //将当前角度和角速度设为上一个状态
        lastState.dq = currentVel;
        // lastState.tauEst = calcTorque;
        // lastState.tauEst = sensor_torque;
        lastState.tauEst = joint.getEffort();//这个值按道理是硬件接口来的力矩，但是为什么不用sensor_torque呢，区别在哪呢？

        // pub_state.publish(lastState);
        // publish state
        if (controller_state_publisher_ && controller_state_publisher_->trylock()) { //如果实时发布器存在并且成功锁定，更新并发布关节状态
            controller_state_publisher_->msg_.q = lastState.q; //给发布器的消息赋值
            controller_state_publisher_->msg_.dq = lastState.dq;
            controller_state_publisher_->msg_.tauEst = lastState.tauEst;
            controller_state_publisher_->unlockAndPublish(); //解除锁定
        }

        // printf("sensor torque%f\n", sensor_torque);

        // if(joint_name == "wrist1_joint") printf("wrist1 setp:%f  getp:%f t:%f\n", servoCmd.pos, currentPos, calcTorque);
    }

    // Controller stopping in realtime 实时控制器的停止方法，当前为空实现
    void UnitreeJointController::stopping(){}
//根据URDF定义的限制，使用clamp函数限制位置值
    void UnitreeJointController::positionLimits(double &position)
    {
        if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
            clamp(position, joint_urdf->limits->lower, joint_urdf->limits->upper);
    }
//根据urdf定义限制，限值速度值
    void UnitreeJointController::velocityLimits(double &velocity)
    {
        if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
            clamp(velocity, -joint_urdf->limits->velocity, joint_urdf->limits->velocity);
    }
//根据URDF定义的限制，使用clamp函数限制扭矩值
    void UnitreeJointController::effortLimits(double &effort)
    {
        if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
            clamp(effort, -joint_urdf->limits->effort, joint_urdf->limits->effort);
    }

} // namespace 结束这个unitree_legged_control命名空间

// Register controller to pluginlib 使用PLUGINLIB_EXPORT_CLASS宏将UnitreeJointController类注册为ROS插件，使其能够在运行时被识别和加载
PLUGINLIB_EXPORT_CLASS(unitree_legged_control::UnitreeJointController, controller_interface::ControllerBase);

3.unitree_joint_control_tool.h

/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
                                            //定义了一个名为 LaikagoControlTool 的工具集，用于机器人关节控制
// #ifndef _UNITREE_JOINT_CONTROL_TOOL_H_
// #define _UNITREE_JOINT_CONTROL_TOOL_H_
#ifndef _LAIKAGO_CONTROL_TOOL_H_ //和上面被注释的语句只有名称区别，没有功能区别
#define _LAIKAGO_CONTROL_TOOL_H_

#include <stdio.h>//标准输入输出库
#include <stdint.h>//标准整数
#include <algorithm>//算法
#include <math.h>//数学方法库

#define posStopF (2.146E+9f)  // stop position control mode
#define velStopF (16000.0f)   // stop velocity control mode

typedef struct //定义这个ServoCmd结构体
{
    uint8_t mode;//模式
    double pos;//位置
    double posStiffness;//位置刚度
    double vel;//速度
    double velStiffness;//速度刚度（阻尼）
    double torque;//力矩
} ServoCmd;

double clamp(double&, double, double);  //使用方法eg. clamp(1.5, -1, 1) = 1
double computeVel(double current_position, double last_position, double last_velocity, double duration);  // get current velocity
double computeTorque(double current_position, double current_velocity, ServoCmd&);  // get torque

#endif

4.unitree_joint_control_tool.cpp

#include "unitree_joint_control_tool.h"
//浮点数的限制值函数clamp
float clamp(float &val, float min_val, float max_val)
{
    val = std::min(std::max(val, min_val), max_val);
}
//双精度浮点数 限值在范围内
double clamp(double &val, double min_val, double max_val)
{
    val = std::min(std::max(val, min_val), max_val);
}
//据当前位置、上次位置、上次速度、周期来计算速度
double computeVel(double currentPos, double lastPos, double lastVel, double period)
{
    return lastVel*0.35f + 0.65f*(currentPos-lastPos)/period;//简单的滤波方法，上次速度占0.35权重，这次计算的速度占0.65
}
//计算力矩
double computeTorque(double currentPos, double currentVel, ServoCmd &cmd)
{
    double targetPos, targetVel, targetTorque, posStiffness, velStiffness, calcTorque;
    targetPos = cmd.pos; //目标位置
    targetVel = cmd.vel; //目标速度
    targetTorque = cmd.torque;//目标力矩
    posStiffness = cmd.posStiffness;//刚度
    velStiffness = cmd.velStiffness;//阻尼
    if(fabs(targetPos-posStopF) < 1e-6) posStiffness = 0;
    if(fabs(targetVel-velStopF) < 1e-6) velStiffness = 0;
    calcTorque = posStiffness*(targetPos-currentPos) + velStiffness*(targetVel-currentVel) + targetTorque;//计算力矩
    return calcTorque;
}