unitree G1 low_level 代码分析（1）

于 2025-04-25 10:08:56 发布
阅读量120 收藏
点赞数 1
分类专栏： 机器人 文章标签： 算法 c++ 机器人 linux
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zl1816313292/article/details/147500637
版权 
#include <yaml-cpp/yaml.h>

#include <cmath>
#include <memory>
#include <mutex>
#include <shared_mutex>

// DDS
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>

// IDL
#include <unitree/idl/hg/LowCmd_.hpp>
#include <unitree/idl/hg/LowState_.hpp>

#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
using namespace unitree::robot::b2;

static const std::string HG_CMD_TOPIC = "rt/lowcmd";
static const std::string HG_STATE_TOPIC = "rt/lowstate";

using namespace unitree::common;
using namespace unitree::robot;

const int G1_NUM_MOTOR = 29;


template <typename T>
// 数据缓冲区类，用于线程安全地存储和访问数据
class DataBuffer {
 public:
  // 设置新数据，使用互斥锁保护写操作
  void SetData(const T &newData) {
    std::unique_lock<std::shared_mutex> lock(mutex);
    data = std::make_shared<T>(newData);
  }

  // 获取数据的只读指针，使用共享锁允许多个读操作
  std::shared_ptr<const T> GetData() {
    std::shared_lock<std::shared_mutex> lock(mutex);
    return data ? data : nullptr;
    
  }

  // 清除数据，将指针置空
  void Clear() {
    std::unique_lock<std::shared_mutex> lock(mutex);
    data = nullptr;
  }

 private:
  // 存储数据的智能指针
  std::shared_ptr<T> data;
  // 读写锁，用于保护数据访问
  std::shared_mutex mutex;
};

// IMU状态结构体
struct ImuState {
  std::array<float, 3> rpy = {};    // 欧拉角(roll, pitch, yaw)
  std::array<float, 3> omega = {};  // 角速度
};

// 电机命令结构体，用于存储电机控制参数
struct MotorCommand {
  std::array<float, G1_NUM_MOTOR> q_target = {};    // 目标位置
  std::array<float, G1_NUM_MOTOR> dq_target = {};   // 目标速度
  std::array<float, G1_NUM_MOTOR> kp = {};          // 位置比例增益
  std::array<float, G1_NUM_MOTOR> kd = {};          // 速度比例增益
  std::array<float, G1_NUM_MOTOR> tau_ff = {};      // 前馈力矩
};

// 电机状态结构体，用于存储电机当前状态
struct MotorState {
  std::array<float, G1_NUM_MOTOR> q = {};   // 当前位置
  std::array<float, G1_NUM_MOTOR> dq = {};  // 当前速度
};

// 电机类型枚举，定义不同规格的电机
enum MotorType { 
  GearboxS = 0,  // 小型减速箱电机
  GearboxM = 1,  // 中型减速箱电机
  GearboxL = 2   // 大型减速箱电机
};

// G1机器人各关节电机类型配置数组
std::array<MotorType, G1_NUM_MOTOR> G1MotorType{
    // clang-format off
    // 腿部电机配置
    GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,  // 左腿
    GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,  // 右腿
    // 腰部电机配置
    GearboxM, GearboxS, GearboxS,
    // 手臂电机配置
    GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS,  // 左臂
    GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS   // 右臂
    // clang-format on
};

//脚踝四个关节 控制模式 PR or AB
enum PRorAB { PR = 0, AB = 1 };

//关节index
enum G1JointValidIndex {
  LeftShoulderPitch = 15,
  LeftShoulderRoll = 16,
  LeftShoulderYaw = 17,
  LeftElbow = 18,
  LeftWristRoll = 19,
  LeftWristPitch = 20,
  LeftWristYaw = 21,
  RightShoulderPitch = 22,
  RightShoulderRoll = 23,
  RightShoulderYaw = 24,
  RightElbow = 25,
  RightWristRoll = 26,
  RightWristPitch = 27,
  RightWristYaw = 28
};

//Crc校验
inline uint32_t Crc32Core(uint32_t *ptr, uint32_t len) {
  uint32_t xbit = 0;
  uint32_t data = 0;
  uint32_t CRC32 = 0xFFFFFFFF;
  const uint32_t dwPolynomial = 0x04c11db7;
  for (uint32_t i = 0; i < len; i++) {
    xbit = 1 << 31;
    data = ptr[i];
    for (uint32_t bits = 0; bits < 32; bits++) {
      if (CRC32 & 0x80000000) {
        CRC32 <<= 1;
        CRC32 ^= dwPolynomial;
      } else
        CRC32 <<= 1;
      if (data & xbit) CRC32 ^= dwPolynomial;

      xbit >>= 1;
    }
  }
  return CRC32;
};

//KP参数设置
float GetMotorKp(MotorType type) {
  switch (type) {
    case GearboxS:
      return 40;
    case GearboxM:
      return 40;
    case GearboxL:
      return 100;
    default:
      return 0;
  }
}

//KD参数设置
float GetMotorKd(MotorType type) {
  switch (type) {
    case GearboxS:
      return 1;
    case GearboxM:
      return 1;
    case GearboxL:
      return 1;
    default:
      return 0;
  }
}

/*流程：
sub：lwd_state一直循环获取各种数据，并依次存进buffer(缓存区)
pub：
1、control，从buffer获取最新的数据，在此基础上修改q/dq等各种数据，并存进缓存区
2、lwd_cmd ，步骤2结束后，从buffer中获取数据，将获取的数据下发
之后循环上述三个步骤，即可实现通过DDS通信控制机器人某些关节移动
*/


class G1Example {
 private:
  double time_;
  double control_dt_;  // [2ms]
  double duration_;    // [3 s]
  PRorAB mode_;
  uint8_t mode_machine_;
  std::vector<std::vector<double>> frames_data_;

  DataBuffer<MotorState> motor_state_buffer_;
  DataBuffer<MotorCommand> motor_command_buffer_;
  DataBuffer<ImuState> imu_state_buffer_;

  ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> lowcmd_publisher_;
  ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_;
  ThreadPtr command_writer_ptr_, control_thread_ptr_;

  std::shared_ptr<MotionSwitcherClient> msc;

 public:
  G1Example(std::string networkInterface)
      : time_(0.0),
        control_dt_(0.002),
        duration_(3.0),
        mode_(PR),
        mode_machine_(0) {
    ChannelFactory::Instance()->Init(0, networkInterface);

    msc.reset(new MotionSwitcherClient());
    msc->SetTimeout(5.0F);
    msc->Init();

    /*Shut down  motion control-related service*/
    while(queryMotionStatus())
    {
        std::cout << "Try to deactivate the motion control-related service." << std::endl;
        int32_t ret = msc->ReleaseMode(); 
        if (ret == 0) {
            std::cout << "ReleaseMode succeeded." << std::endl;
        } else {
            std::cout << "ReleaseMode failed. Error code: " << ret << std::endl;
        }
        sleep(5);
    }

    loadBehaviorLibrary("motion");

    // create publisher
    lowcmd_publisher_.reset(
        new ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(HG_CMD_TOPIC));
    lowcmd_publisher_->InitChannel();

    // create subscriber
    lowstate_subscriber_.reset(
        new ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
            HG_STATE_TOPIC));
    lowstate_subscriber_->InitChannel(
        std::bind(&G1Example::LowStateHandler, this, std::placeholders::_1), 1);

    // create threads
    command_writer_ptr_ =
        CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
                                &G1Example::LowCommandWriter, this);
    control_thread_ptr_ = CreateRecurrentThreadEx(
        "control", UT_CPU_ID_NONE, 2000, &G1Example::Control, this);
  }

  void loadBehaviorLibrary(std::string behavior_name) {
    std::string resource_dir = BLIB_DIR;
    YAML::Node motion = YAML::LoadFile(resource_dir + behavior_name + ".seq");

    std::string content = motion["components"][1]["content"].as<std::string>();
    int num_parts = motion["components"][1]["num_parts"].as<int>();
    std::cout << "BehaviorName: " << behavior_name + ".seq\n";
    std::cout << content << " with " << num_parts << "\n";

    auto frames = motion["components"][1]["frames"];

    for (const auto &frame : frames) {
      std::vector<double> frame_data;
      for (const auto &element : frame) {
        frame_data.push_back(element.as<double>());
      }
      frames_data_.push_back(frame_data);
    }

    std::cout << frames_data_.size() << " knots with " << frames_data_[0].size()
              << " DOF\n";
  }

  //output RPY values
  void ReportRPY() {
    const std::shared_ptr<const ImuState> imu_tmp_ptr =
        imu_state_buffer_.GetData();
    if (imu_tmp_ptr) {
      std::cout << "rpy: [" << imu_tmp_ptr->rpy.at(0) << ", "
                << imu_tmp_ptr->rpy.at(1) << ", " << imu_tmp_ptr->rpy.at(2)
                << "]" << std::endl;
    }
  }

  
  void LowStateHandler(const void *message) {
    unitree_hg::msg::dds_::LowState_ low_state =
        *(const unitree_hg::msg::dds_::LowState_ *)message;

    if (low_state.crc() !=
        Crc32Core((uint32_t *)&low_state,
                  (sizeof(unitree_hg::msg::dds_::LowState_) >> 2) - 1)) {
      std::cout << "low_state CRC Error" << std::endl;
      return;
    }

    // get motor state 获取电机数据信息，设置到buffer中
    MotorState ms_tmp;
    for (int i = 0; i < G1_NUM_MOTOR; ++i) {
      ms_tmp.q.at(i) = low_state.motor_state()[i].q();
      ms_tmp.dq.at(i) = low_state.motor_state()[i].dq();
    }
    motor_state_buffer_.SetData(ms_tmp);

    // get imu state 获取IMU数据信息，设置到buffer中
    ImuState imu_tmp;
    imu_tmp.omega = low_state.imu_state().gyroscope();
    imu_tmp.rpy = low_state.imu_state().rpy();
    imu_state_buffer_.SetData(imu_tmp);

    // update mode machine
    if (mode_machine_ != low_state.mode_machine()) {
      if (mode_machine_ == 0)
        std::cout << "G1 type: " << unsigned(low_state.mode_machine())
                  << std::endl;
      mode_machine_ = low_state.mode_machine();
    }
  }
   
  //下发控制指令
  void LowCommandWriter() {
    unitree_hg::msg::dds_::LowCmd_ dds_low_command;
    dds_low_command.mode_pr() = mode_;
    dds_low_command.mode_machine() = mode_machine_;

    //从buffer中获取最新的motor信息
    const std::shared_ptr<const MotorCommand> mc =
        motor_command_buffer_.GetData();
    if (mc) {
      for (size_t i = 0; i < G1_NUM_MOTOR; i++) {
        dds_low_command.motor_cmd().at(i).mode() = 1;  // 1:Enable, 0:Disable
        dds_low_command.motor_cmd().at(i).tau() = mc->tau_ff.at(i);
        dds_low_command.motor_cmd().at(i).q() = mc->q_target.at(i);
        dds_low_command.motor_cmd().at(i).dq() = mc->dq_target.at(i);
        dds_low_command.motor_cmd().at(i).kp() = mc->kp.at(i);
        dds_low_command.motor_cmd().at(i).kd() = mc->kd.at(i);
      }

      //需要经过crc校验，否则下发失败
      dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
                                        (sizeof(dds_low_command) >> 2) - 1);
                                      
      //下发指令                                   
      lowcmd_publisher_->Write(dds_low_command);
    }
  }

  //控制
  void Control() {
    MotorCommand motor_command_tmp;
    //从buffer中获取当前的motor信息
    const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();

    if (ms) {
      time_ += control_dt_;
      //前3s传入所有关节回0
      if (time_ < duration_) {
        // [Stage 1]: set robot to zero posture
        for (int i = 0; i < G1_NUM_MOTOR; ++i) {
          double ratio = std::clamp(time_ / duration_, 0.0, 1.0);

          double q_des = 0;
          motor_command_tmp.tau_ff.at(i) = 0.0;
          motor_command_tmp.q_target.at(i) =
              (q_des - ms->q.at(i)) * ratio + ms->q.at(i);
          motor_command_tmp.dq_target.at(i) = 0.0;
          motor_command_tmp.kp.at(i) = GetMotorKp(G1MotorType[i]);
          motor_command_tmp.kd.at(i) = GetMotorKd(G1MotorType[i]);
        }
      } else {
        // [Stage 2]: tracking the offline trajectory
        size_t frame_index = (size_t)((time_ - duration_) / control_dt_);
        if (frame_index >= frames_data_.size()) {
          frame_index = frames_data_.size() - 1;
          time_ = 0.0;  // RESET
        }

        if (frame_index % 100 == 0)
          std::cout << "Frame Index: " << frame_index << std::endl;

        for (int i = 0; i < G1_NUM_MOTOR; ++i) {
          size_t index_in_frame = i - LeftShoulderPitch;
          motor_command_tmp.q_target.at(i) =
              (i >= LeftShoulderPitch)
                  ? frames_data_[frame_index][index_in_frame]
                  : 0.0;
          motor_command_tmp.dq_target.at(i) = 0.0;
          motor_command_tmp.tau_ff.at(i) = 0.0;
          motor_command_tmp.kp.at(i) = GetMotorKp(G1MotorType[i]);
          motor_command_tmp.kd.at(i) = GetMotorKd(G1MotorType[i]);
        }
      }

      //每个循环都将修改后的data传入buffer中
      motor_command_buffer_.SetData(motor_command_tmp);
    }
  }

  std::string queryServiceName(std::string form,std::string name)
  {
      if(form == "0")
      {
          if(name == "normal" ) return "sport_mode"; 
          if(name == "ai" ) return "ai_sport"; 
          if(name == "advanced" ) return "advanced_sport"; 
      }
      else
      {
          if(name == "ai-w" ) return "wheeled_sport(go2W)"; 
          if(name == "normal-w" ) return "wheeled_sport(b2W)";
      }
      return "";
  }

  int queryMotionStatus()
  {
      std::string robotForm,motionName;
      int motionStatus;
      int32_t ret = msc->CheckMode(robotForm,motionName);
      if (ret == 0) {
          std::cout << "CheckMode succeeded." << std::endl;
      } else {
          std::cout << "CheckMode failed. Error code: " << ret << std::endl;
      }
      if(motionName.empty())
      {
          std::cout << "The motion control-related service is deactivated." << std::endl;
          motionStatus = 0;
      }
      else
      {
          std::string serviceName = queryServiceName(robotForm,motionName);
          std::cout << "Service: "<< serviceName<< " is activate" << std::endl;
          motionStatus = 1;
      }
      return motionStatus;
  }
};

int main(int argc, char const *argv[]) {
  if (argc < 2) {
    std::cout << "Usage: g1_dual_arm_example network_interface_name"
              << std::endl;
    exit(0);
  }
  std::string networkInterface = argv[1];
  G1Example custom(networkInterface);

  while (true) sleep(10);

  return 0;
}

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值