初始化项目，包含fast_lio_ws和unitree_sdk2

unitree_sdk2/example/h1/low_level/base_state.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <array>
+
+struct BaseState {
+  std::array<float, 3> rpy = {};
+  std::array<float, 3> omega = {};
+};

unitree_sdk2/example/h1/low_level/data_buffer.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+
+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;
+};

unitree_sdk2/example/h1/low_level/h1_27dof_example.cpp

@@ -0,0 +1,367 @@
+#include <algorithm>
+#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>
+
+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 H1_NUM_MOTOR = 27;
+
+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;
+};
+
+struct ImuState {
+  std::array<float, 3> rpy = {};
+  std::array<float, 3> omega = {};
+};
+
+struct MotorCommand {
+  std::array<float, H1_NUM_MOTOR> q_target = {};
+  std::array<float, H1_NUM_MOTOR> dq_target = {};
+  std::array<float, H1_NUM_MOTOR> kp = {};
+  std::array<float, H1_NUM_MOTOR> kd = {};
+  std::array<float, H1_NUM_MOTOR> tau_ff = {};
+};
+
+struct MotorState {
+  std::array<float, H1_NUM_MOTOR> q = {};
+  std::array<float, H1_NUM_MOTOR> dq = {};
+};
+
+enum MotorType { GearboxS = 0, GearboxM = 1, GearboxL = 2 };
+
+std::array<MotorType, H1_NUM_MOTOR> H1MotorType{
+    // clang-format off
+    // legs
+    GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
+    GearboxM, GearboxM, GearboxM, GearboxL, GearboxS, GearboxS,
+    // waist
+    GearboxM,
+    // arms
+    GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS,
+    GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS, GearboxS
+    // clang-format on
+};
+
+enum PRorAB { PR = 0, AB = 1 };
+
+enum H1JointIndex {
+  // legs
+  LeftHipYaw = 0,
+  LeftHipPitch = 1,
+  LeftHipRoll = 2,
+  LeftKnee = 3,
+  LeftAnklePitch = 4,
+  LeftAnkleB = 4,
+  LeftAnkleRoll = 5,
+  LeftAnkleA = 5,
+  RightHipYaw = 6,
+  RightHipPitch = 7,
+  RightHipRoll = 8,
+  RightKnee = 9,
+  RightAnklePitch = 10,
+  RightAnkleB = 10,
+  RightAnkleRoll = 11,
+  RightAnkleA = 11,
+  // torso
+  WaistYaw = 12,
+  // arms
+  LeftShoulderPitch = 13,
+  LeftShoulderRoll = 14,
+  LeftShoulderYaw = 15,
+  LeftElbow = 16,
+  LeftWristRoll = 17,
+  LeftWristPitch = 18,
+  LeftWristYaw = 19,
+  RightShoulderPitch = 20,
+  RightShoulderRoll = 21,
+  RightShoulderYaw = 22,
+  RightElbow = 23,
+  RightWristRoll = 24,
+  RightWristPitch = 25,
+  RightWristYaw = 26
+};
+
+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;
+};
+
+float GetMotorKp(MotorType type) {
+  switch (type) {
+    case GearboxS:
+      return 80;
+    case GearboxM:
+      return 100;
+    case GearboxL:
+      return 200;
+    default:
+      return 0;
+  }
+}
+
+float GetMotorKd(MotorType type) {
+  switch (type) {
+    case GearboxS:
+      return 2;
+    case GearboxM:
+      return 3;
+    case GearboxL:
+      return 5;
+    default:
+      return 0;
+  }
+}
+
+class H1Example {
+ private:
+  double time_;
+  double control_dt_;  // [2ms]
+  double duration_;    // [3 s]
+  PRorAB mode_;
+  uint8_t mode_machine_;
+
+  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_;
+
+ public:
+  H1Example(std::string networkInterface)
+      : time_(0.0),
+        control_dt_(0.002),
+        duration_(3.0),
+        mode_(PR),
+        mode_machine_(0) {
+    ChannelFactory::Instance()->Init(0, networkInterface);
+
+    // 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(&H1Example::LowStateHandler, this, std::placeholders::_1), 1);
+
+    // create threads
+    command_writer_ptr_ =
+        CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
+                                &H1Example::LowCommandWriter, this);
+    control_thread_ptr_ = CreateRecurrentThreadEx(
+        "control", UT_CPU_ID_NONE, 2000, &H1Example::Control, this);
+  }
+
+  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
+    MotorState ms_tmp;
+    for (int i = 0; i < H1_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();
+
+      if (low_state.motor_state()[i].motorstate())
+        std::cout << "[ERROR] motor " << i << " with code "
+                  << low_state.motor_state()[i].motorstate() << "\n";
+    }
+    motor_state_buffer_.SetData(ms_tmp);
+
+    // get imu state
+    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_;
+
+    const std::shared_ptr<const MotorCommand> mc =
+        motor_command_buffer_.GetData();
+    if (mc) {
+      for (size_t i = 0; i < H1_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);
+      }
+
+      dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
+                                        (sizeof(dds_low_command) >> 2) - 1);
+      lowcmd_publisher_->Write(dds_low_command);
+    }
+  }
+
+  void Control() {
+    ReportRPY();
+
+    MotorCommand motor_command_tmp;
+    const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
+
+    if (ms) {
+      time_ += control_dt_;
+      if (time_ < duration_ * 1) {
+        // [Stage 1]: set robot to zero posture
+        for (int i = 0; i < H1_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(H1MotorType[i]);
+          motor_command_tmp.kd.at(i) = GetMotorKd(H1MotorType[i]);
+        }
+      } else if (time_ < duration_ * 2) {
+        // [Stage 2]: swing ankle's PR
+        mode_ = PR;
+        double max_P = M_PI * 30.0 / 180.0;
+        double max_R = M_PI * 10.0 / 180.0;
+        double t = time_ - duration_ * 1;
+        double L_P_des = +max_P * std::sin(2.0 * M_PI * t);
+        double L_R_des = +max_R * std::sin(2.0 * M_PI * t);
+        double R_P_des = +max_P * std::sin(2.0 * M_PI * t);
+        double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
+
+        for (int i = 0; i < H1_NUM_MOTOR; ++i) {
+          motor_command_tmp.tau_ff.at(i) = 0.0;
+          motor_command_tmp.q_target.at(i) = 0.0;
+          motor_command_tmp.dq_target.at(i) = 0.0;
+          motor_command_tmp.kp.at(i) = GetMotorKp(H1MotorType[i]);
+          motor_command_tmp.kd.at(i) = GetMotorKd(H1MotorType[i]);
+        }
+
+        motor_command_tmp.q_target.at(LeftAnklePitch) = L_P_des;
+        motor_command_tmp.q_target.at(LeftAnkleRoll) = L_R_des;
+        motor_command_tmp.q_target.at(RightAnklePitch) = R_P_des;
+        motor_command_tmp.q_target.at(RightAnkleRoll) = R_R_des;
+      } else {
+        // [Stage 3]: swing ankle's AB
+        mode_ = AB;
+        double max_A = M_PI * 30.0 / 180.0;
+        double max_B = M_PI * 10.0 / 180.0;
+        double t = time_ - duration_ * 2;
+        double L_A_des = +max_A * std::sin(M_PI * t);
+        double L_B_des = +max_B * std::sin(M_PI * t + M_PI);
+        double R_A_des = -max_A * std::sin(M_PI * t);
+        double R_B_des = -max_B * std::sin(M_PI * t + M_PI);
+
+        for (int i = 0; i < H1_NUM_MOTOR; ++i) {
+          motor_command_tmp.tau_ff.at(i) = 0.0;
+          motor_command_tmp.q_target.at(i) = 0.0;
+          motor_command_tmp.dq_target.at(i) = 0.0;
+          motor_command_tmp.kp.at(i) = GetMotorKp(H1MotorType[i]);
+          motor_command_tmp.kd.at(i) = GetMotorKd(H1MotorType[i]);
+        }
+
+        motor_command_tmp.q_target.at(LeftAnkleA) = L_A_des;
+        motor_command_tmp.q_target.at(LeftAnkleB) = L_B_des;
+        motor_command_tmp.q_target.at(RightAnkleA) = R_A_des;
+        motor_command_tmp.q_target.at(RightAnkleB) = R_B_des;
+      }
+
+      motor_command_buffer_.SetData(motor_command_tmp);
+    }
+  }
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: h1_27dof_example network_interface_name" << std::endl;
+    exit(0);
+  }
+  std::string networkInterface = argv[1];
+  H1Example custom(networkInterface);
+
+  while (true) sleep(10);
+
+  return 0;
+}

unitree_sdk2/example/h1/low_level/h1_2_ankle_track.cpp

@@ -0,0 +1,175 @@
+#include <cmath>
+
+// 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>
+
+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 H1_NUM_MOTOR = 27;
+
+enum PRorAB { PR = 0, AB = 1 };
+
+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;
+};
+
+class H1Example {
+ private:
+  double time_;
+  double control_dt_;  // [2ms]
+  double duration_;    // [3 s]
+  PRorAB mode_;
+
+  unitree_hg::msg::dds_::LowState_ low_state_;
+
+  ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_> lowcmd_publisher_;
+  ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> lowstate_subscriber_;
+  ThreadPtr command_writer_ptr_, control_thread_ptr_;
+
+ public:
+  H1Example(std::string networkInterface)
+      : time_(0.0), control_dt_(0.002), duration_(3.0), mode_(PR) {
+    ChannelFactory::Instance()->Init(0, networkInterface);
+
+    // 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(&H1Example::LowStateHandler, this, std::placeholders::_1), 1);
+
+    // create threads
+    control_thread_ptr_ = CreateRecurrentThreadEx(
+        "control", UT_CPU_ID_NONE, 2000, &H1Example::Control, this);
+  }
+
+  void LowStateHandler(const void *message) {
+    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;
+    }
+  }
+
+  void Control() {
+    unitree_hg::msg::dds_::LowCmd_ dds_low_command;
+    dds_low_command.mode_pr() = mode_;
+    dds_low_command.mode_machine() = low_state_.mode_machine();
+    for (int i = 0; i < H1_NUM_MOTOR; ++i) {
+      dds_low_command.motor_cmd().at(i).mode() = 1;  // 1:Enable, 0:Disable
+      dds_low_command.motor_cmd().at(i).tau() = 0.0;
+      dds_low_command.motor_cmd().at(i).q() = 0.0;
+      dds_low_command.motor_cmd().at(i).dq() = 0.0;
+      dds_low_command.motor_cmd().at(i).kp() = (i < 13) ? 100.0 : 50.0;
+      dds_low_command.motor_cmd().at(i).kd() = 1.0;
+    }
+
+    time_ += control_dt_;
+    if (time_ < duration_) {
+      // [Stage 1]: set robot to zero posture
+      for (int i = 0; i < H1_NUM_MOTOR; ++i) {
+        double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
+        dds_low_command.motor_cmd().at(i).q() =
+            (1. - ratio) * low_state_.motor_state().at(i).q();
+      }
+    } else {
+      // [Stage 2]: swing ankle's PR
+      mode_ = PR;  // Enable PR mode
+
+      // generate sin/cos trajectory
+      double max_P = 0.25;  // [rad]
+      double max_R = 0.25;  // [rad]
+      double t = time_ - duration_;
+      double L_P_des = max_P * std::cos(2.0 * M_PI * t);
+      double L_R_des = max_R * std::sin(2.0 * M_PI * t);
+      double R_P_des = max_P * std::cos(2.0 * M_PI * t);
+      double R_R_des = -max_R * std::sin(2.0 * M_PI * t);
+
+      // update ankle joint position targets
+      float Kp_Pitch = 80;
+      float Kd_Pitch = 1;
+      float Kp_Roll = 80;
+      float Kd_Roll = 1;
+      dds_low_command.motor_cmd().at(4).q() = L_P_des;  // 4: LeftAnklePitch
+      dds_low_command.motor_cmd().at(4).dq() = 0;
+      dds_low_command.motor_cmd().at(4).kp() = Kp_Pitch;
+      dds_low_command.motor_cmd().at(4).kd() = Kd_Pitch;
+      dds_low_command.motor_cmd().at(4).tau() = 0;
+      dds_low_command.motor_cmd().at(5).q() = L_R_des;  // 5: LeftAnkleRoll
+      dds_low_command.motor_cmd().at(5).dq() = 0;
+      dds_low_command.motor_cmd().at(5).kp() = Kp_Roll;
+      dds_low_command.motor_cmd().at(5).kd() = Kd_Roll;
+      dds_low_command.motor_cmd().at(5).tau() = 0;
+      dds_low_command.motor_cmd().at(10).q() = R_P_des;  // 10: RightAnklePitch
+      dds_low_command.motor_cmd().at(10).dq() = 0;
+      dds_low_command.motor_cmd().at(10).kp() = Kp_Pitch;
+      dds_low_command.motor_cmd().at(10).kd() = Kd_Pitch;
+      dds_low_command.motor_cmd().at(10).tau() = 0;
+      dds_low_command.motor_cmd().at(11).q() = R_R_des;  // 11: RightAnkleRoll
+      dds_low_command.motor_cmd().at(11).dq() = 0;
+      dds_low_command.motor_cmd().at(11).kp() = Kp_Roll;
+      dds_low_command.motor_cmd().at(11).kd() = Kd_Roll;
+      dds_low_command.motor_cmd().at(11).tau() = 0;
+
+      float L_P_m = low_state_.motor_state().at(4).q();
+      float L_R_m = low_state_.motor_state().at(5).q();
+      float R_P_m = low_state_.motor_state().at(10).q();
+      float R_R_m = low_state_.motor_state().at(11).q();
+      // clang-format off
+        printf("%f,%f,%f,%f,%f,%f,%f,%f\n",
+               L_P_des, L_P_m, L_R_des, L_R_m,
+               R_P_des, R_P_m, R_R_des, R_R_m);
+      // clang-format on
+    }
+
+    dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
+                                      (sizeof(dds_low_command) >> 2) - 1);
+    lowcmd_publisher_->Write(dds_low_command);
+  }
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: h1_2_ankle_track network_interface_name" << std::endl;
+    exit(0);
+  }
+  std::string networkInterface = argv[1];
+  H1Example custom(networkInterface);
+  while (true) sleep(10);
+  return 0;
+}

unitree_sdk2/example/h1/low_level/humanoid.cpp

@@ -0,0 +1,14 @@
+#include "humanoid.hpp"
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  HumanoidExample example(argv[1]);
+  while (1) {
+    sleep(10);
+  }
+  return 0;
+}

unitree_sdk2/example/h1/low_level/humanoid.hpp

@@ -0,0 +1,263 @@
+#pragma once
+
+#include <iostream>
+#include <stdint.h>
+#include <string>
+
+#include "unitree/robot/channel/channel_publisher.hpp"
+#include "unitree/robot/channel/channel_subscriber.hpp"
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+
+#include "base_state.h"
+#include "data_buffer.hpp"
+#include "motors.hpp"
+
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+using namespace unitree::robot::b2;
+
+static const std::string kTopicLowCommand = "rt/lowcmd";
+static const std::string kTopicLowState = "rt/lowstate";
+
+class HumanoidExample {
+public:
+  HumanoidExample(const std::string &networkInterface = "") {
+    unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface);
+    std::cout << "Initialize channel factory." << std::endl;
+
+    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);
+    }
+
+    lowcmd_publisher_.reset(
+        new unitree::robot::ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(
+            kTopicLowCommand));
+    lowcmd_publisher_->InitChannel();
+    command_writer_ptr_ = unitree::common::CreateRecurrentThreadEx(
+        "command_writer", UT_CPU_ID_NONE, 2000,
+        &HumanoidExample::LowCommandWriter, this);
+
+    lowstate_subscriber_.reset(
+        new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::LowState_>(
+            kTopicLowState));
+    lowstate_subscriber_->InitChannel(
+        std::bind(&HumanoidExample::LowStateHandler, this,
+                  std::placeholders::_1),
+        1);
+    int control_period_us = control_dt_ * 1e6;
+    control_thread_ptr_ = unitree::common::CreateRecurrentThreadEx(
+        "control", UT_CPU_ID_NONE, control_period_us, &HumanoidExample::Control,
+        this);
+
+    int report_period_us = report_dt_ * 1e6;
+    report_rpy_ptr_ = unitree::common::CreateRecurrentThreadEx(
+        "report_rpy", UT_CPU_ID_NONE, report_period_us,
+        &HumanoidExample::ReportRPY, this);
+  }
+
+  ~HumanoidExample() = default;
+
+  void LowCommandWriter() {
+    unitree_go::msg::dds_::LowCmd_ dds_low_command{};
+    dds_low_command.head()[0] = 0xFE;
+    dds_low_command.head()[1] = 0xEF;
+    dds_low_command.level_flag() = 0xFF;
+    dds_low_command.gpio() = 0;
+
+    const std::shared_ptr<const MotorCommand> mc_tmp_ptr =
+        motor_command_buffer_.GetData();
+    if (mc_tmp_ptr) {
+      for (int i = 0; i < kNumMotors; ++i) {
+        if (IsWeakMotor(i)) {
+          dds_low_command.motor_cmd().at(i).mode() = (0x01);
+        } else {
+          dds_low_command.motor_cmd().at(i).mode() = (0x0A);
+        }
+        dds_low_command.motor_cmd().at(i).tau() = mc_tmp_ptr->tau_ff.at(i);
+        dds_low_command.motor_cmd().at(i).q() = mc_tmp_ptr->q_ref.at(i);
+        dds_low_command.motor_cmd().at(i).dq() = mc_tmp_ptr->dq_ref.at(i);
+        dds_low_command.motor_cmd().at(i).kp() = mc_tmp_ptr->kp.at(i);
+        dds_low_command.motor_cmd().at(i).kd() = mc_tmp_ptr->kd.at(i);
+      }
+      dds_low_command.crc() = Crc32Core((uint32_t *)&dds_low_command,
+                                        (sizeof(dds_low_command) >> 2) - 1);
+      lowcmd_publisher_->Write(dds_low_command);
+    }
+  }
+
+  void LowStateHandler(const void *message) {
+    unitree_go::msg::dds_::LowState_ low_state =
+        *(unitree_go::msg::dds_::LowState_ *)message;
+
+    RecordMotorState(low_state);
+    RecordBaseState(low_state);
+  }
+
+  void Control() {
+    MotorCommand motor_command_tmp;
+    const std::shared_ptr<const MotorState> ms_tmp_ptr =
+        motor_state_buffer_.GetData();
+
+    if (ms_tmp_ptr) {
+      time_ += control_dt_;
+      time_ = std::clamp(time_, 0.f, init_duration_);
+      float ratio = time_ / init_duration_;
+      for (int i = 0; i < kNumMotors; ++i) {
+        motor_command_tmp.kp.at(i) = IsWeakMotor(i) ? kp_low_ : kp_high_;
+        motor_command_tmp.kd.at(i) = IsWeakMotor(i) ? kd_low_ : kd_high_;
+        motor_command_tmp.dq_ref.at(i) = 0.f;
+        motor_command_tmp.tau_ff.at(i) = 0.f;
+
+        float q_des = 0.f;
+        if (i == JointIndex::kLeftHipPitch || i == JointIndex::kRightHipPitch) {
+          q_des = hip_pitch_init_pos_;
+        }
+        if (i == JointIndex::kLeftKnee || i == JointIndex::kRightKnee) {
+          q_des = knee_init_pos_;
+        }
+        if (i == JointIndex::kLeftAnkle || i == JointIndex::kRightAnkle) {
+          q_des = ankle_init_pos_;
+        }
+        if (i == JointIndex::kLeftShoulderPitch ||
+            i == JointIndex::kRightShoulderPitch) {
+          q_des = shoulder_pitch_init_pos_;
+        }
+
+        q_des = (q_des - ms_tmp_ptr->q.at(i)) * ratio + ms_tmp_ptr->q.at(i);
+        motor_command_tmp.q_ref.at(i) = q_des;
+      }
+
+      motor_command_buffer_.SetData(motor_command_tmp);
+    }
+  }
+
+  void ReportRPY() {
+    const std::shared_ptr<const BaseState> bs_tmp_ptr =
+        base_state_buffer_.GetData();
+    if (bs_tmp_ptr) {
+      std::cout << "rpy: [" << bs_tmp_ptr->rpy.at(0) << ", "
+                << bs_tmp_ptr->rpy.at(1) << ", " << bs_tmp_ptr->rpy.at(2) << "]"
+                << std::endl;
+    }
+  }
+
+private:
+  void RecordMotorState(const unitree_go::msg::dds_::LowState_ &msg) {
+    MotorState ms_tmp;
+    for (int i = 0; i < kNumMotors; ++i) {
+      ms_tmp.q.at(i) = msg.motor_state()[i].q();
+      ms_tmp.dq.at(i) = msg.motor_state()[i].dq();
+    }
+
+    motor_state_buffer_.SetData(ms_tmp);
+  }
+
+  void RecordBaseState(const unitree_go::msg::dds_::LowState_ &msg) {
+    BaseState bs_tmp;
+    bs_tmp.omega = msg.imu_state().gyroscope();
+    bs_tmp.rpy = msg.imu_state().rpy();
+
+    base_state_buffer_.SetData(bs_tmp);
+  }
+
+  inline bool IsWeakMotor(int motor_index) {
+    return motor_index == JointIndex::kLeftAnkle ||
+           motor_index == JointIndex::kRightAnkle ||
+           motor_index == JointIndex::kRightShoulderPitch ||
+           motor_index == JointIndex::kRightShoulderRoll ||
+           motor_index == JointIndex::kRightShoulderYaw ||
+           motor_index == JointIndex::kRightElbow ||
+           motor_index == JointIndex::kLeftShoulderPitch ||
+           motor_index == JointIndex::kLeftShoulderRoll ||
+           motor_index == JointIndex::kLeftShoulderYaw ||
+           motor_index == JointIndex::kLeftElbow;
+  }
+
+  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;
+  }
+
+  unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_>
+      lowcmd_publisher_;
+  unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_>
+      lowstate_subscriber_;
+
+  DataBuffer<MotorState> motor_state_buffer_;
+  DataBuffer<MotorCommand> motor_command_buffer_;
+  DataBuffer<BaseState> base_state_buffer_;
+
+  std::shared_ptr<MotionSwitcherClient> msc;
+
+  // control params
+  float kp_low_ = 60.f;
+  float kp_high_ = 200.f;
+  float kd_low_ = 1.5f;
+  float kd_high_ = 5.f;
+
+  float control_dt_ = 0.01f;
+
+  float hip_pitch_init_pos_ = -0.5f;
+  float knee_init_pos_ = 1.f;
+  float ankle_init_pos_ = -0.5f;
+  float shoulder_pitch_init_pos_ = 0.4f;
+
+  float time_ = 0.f;
+  float init_duration_ = 10.f;
+
+  float report_dt_ = 0.1f;
+
+  // multithreading
+  unitree::common::ThreadPtr command_writer_ptr_;
+  unitree::common::ThreadPtr control_thread_ptr_;
+  unitree::common::ThreadPtr report_rpy_ptr_;
+};

unitree_sdk2/example/h1/low_level/motors.hpp

@@ -0,0 +1,75 @@
+#pragma once
+
+#include <array>
+#include <stdint.h>
+
+#include <unitree/idl/go2/LowCmd_.hpp>
+
+constexpr int kNumMotors = 20;
+
+struct MotorCommand {
+  std::array<float, kNumMotors> q_ref = {};
+  std::array<float, kNumMotors> dq_ref = {};
+  std::array<float, kNumMotors> kp = {};
+  std::array<float, kNumMotors> kd = {};
+  std::array<float, kNumMotors> tau_ff = {};
+};
+
+struct MotorState {
+  std::array<float, kNumMotors> q = {};
+  std::array<float, kNumMotors> dq = {};
+};
+
+enum JointIndex {
+  // Right leg
+  kRightHipYaw = 8,
+  kRightHipRoll = 0,
+  kRightHipPitch = 1,
+  kRightKnee = 2,
+  kRightAnkle = 11,
+  // Left leg
+  kLeftHipYaw = 7,
+  kLeftHipRoll = 3,
+  kLeftHipPitch = 4,
+  kLeftKnee = 5,
+  kLeftAnkle = 10,
+
+  kWaistYaw = 6,
+
+  kNotUsedJoint = 9,
+
+  // Right arm
+  kRightShoulderPitch = 12,
+  kRightShoulderRoll = 13,
+  kRightShoulderYaw = 14,
+  kRightElbow = 15,
+  // Left arm
+  kLeftShoulderPitch = 16,
+  kLeftShoulderRoll = 17,
+  kLeftShoulderYaw = 18,
+  kLeftElbow = 19,
+
+};
+
+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;
+};