feat: 初始化 Go2 导航相关代码与配置

unitree_sdk2/example/h2/low_level/gamepad.hpp

@@ -0,0 +1,128 @@
+#pragma once
+
+#include <cmath>
+
+namespace unitree::common
+{
+
+    // bytecode mapping for raw joystick data
+    // 16b
+    typedef union
+    {
+        struct
+        {
+            uint8_t R1 : 1;
+            uint8_t L1 : 1;
+            uint8_t start : 1;
+            uint8_t select : 1;
+            uint8_t R2 : 1;
+            uint8_t L2 : 1;
+            uint8_t F1 : 1;
+            uint8_t F2 : 1;
+            uint8_t A : 1;
+            uint8_t B : 1;
+            uint8_t X : 1;
+            uint8_t Y : 1;
+            uint8_t up : 1;
+            uint8_t right : 1;
+            uint8_t down : 1;
+            uint8_t left : 1;
+        } components;
+        uint16_t value;
+    } xKeySwitchUnion;
+
+    // 40 Byte (now used 24B)
+    typedef struct
+    {
+        uint8_t head[2];
+        xKeySwitchUnion btn;
+        float lx;
+        float rx;
+        float ry;
+        float L2;
+        float ly;
+
+        uint8_t idle[16];
+    } xRockerBtnDataStruct;
+
+    typedef union
+    {
+        xRockerBtnDataStruct RF_RX;
+        uint8_t buff[40];
+    } REMOTE_DATA_RX;
+
+    class Button
+    {
+    public:
+        Button() {}
+
+        void update(bool state)
+        {
+            on_press = state ? state != pressed : false;
+            on_release = state ? false : state != pressed;
+            pressed = state;
+        }
+
+        bool pressed = false;
+        bool on_press = false;
+        bool on_release = false;
+    };
+
+    class Gamepad
+    {
+    public:
+        Gamepad() {}
+
+        void update(xRockerBtnDataStruct &key_data)
+        {
+            lx = lx * (1 - smooth) + (std::fabs(key_data.lx) < dead_zone ? 0.0 : key_data.lx) * smooth;
+            rx = rx * (1 - smooth) + (std::fabs(key_data.rx) < dead_zone ? 0.0 : key_data.rx) * smooth;
+            ry = ry * (1 - smooth) + (std::fabs(key_data.ry) < dead_zone ? 0.0 : key_data.ry) * smooth;
+            l2 = l2 * (1 - smooth) + (std::fabs(key_data.L2) < dead_zone ? 0.0 : key_data.L2) * smooth;
+            ly = ly * (1 - smooth) + (std::fabs(key_data.ly) < dead_zone ? 0.0 : key_data.ly) * smooth;
+
+            R1.update(key_data.btn.components.R1);
+            L1.update(key_data.btn.components.L1);
+            start.update(key_data.btn.components.start);
+            select.update(key_data.btn.components.select);
+            R2.update(key_data.btn.components.R2);
+            L2.update(key_data.btn.components.L2);
+            F1.update(key_data.btn.components.F1);
+            F2.update(key_data.btn.components.F2);
+            A.update(key_data.btn.components.A);
+            B.update(key_data.btn.components.B);
+            X.update(key_data.btn.components.X);
+            Y.update(key_data.btn.components.Y);
+            up.update(key_data.btn.components.up);
+            right.update(key_data.btn.components.right);
+            down.update(key_data.btn.components.down);
+            left.update(key_data.btn.components.left);
+        }
+
+        float lx = 0.;
+        float rx = 0.;
+        float ry = 0.;
+        float l2 = 0.;
+        float ly = 0.;
+
+        float smooth = 0.03;
+        float dead_zone = 0.01;
+
+        Button R1;
+        Button L1;
+        Button start;
+        Button select;
+        Button R2;
+        Button L2;
+        Button F1;
+        Button F2;
+        Button A;
+        Button B;
+        Button X;
+        Button Y;
+        Button up;
+        Button right;
+        Button down;
+        Button left;
+    };
+} // namespace unitree::common

unitree_sdk2/example/h2/low_level/h2_ankle_swing_example.cpp

@@ -0,0 +1,380 @@
+#include <cmath>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+
+#include "gamepad.hpp"
+
+// DDS
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+// IDL
+#include <unitree/idl/hg/IMUState_.hpp>
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+
+static const std::string HG_CMD_TOPIC = "rt/lowcmd";
+static const std::string HG_IMU_TORSO = "rt/secondary_imu";
+static const std::string HG_STATE_TOPIC = "rt/lowstate";
+
+using namespace unitree::common;
+using namespace unitree::robot;
+using namespace unitree_hg::msg::dds_;
+
+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;
+};
+
+const int H2_NUM_MOTOR = 31;
+struct ImuState {
+  std::array<float, 3> rpy = {};
+  std::array<float, 3> omega = {};
+};
+struct MotorCommand {
+  std::array<float, H2_NUM_MOTOR> q_target = {};
+  std::array<float, H2_NUM_MOTOR> dq_target = {};
+  std::array<float, H2_NUM_MOTOR> kp = {};
+  std::array<float, H2_NUM_MOTOR> kd = {};
+  std::array<float, H2_NUM_MOTOR> tau_ff = {};
+};
+struct MotorState {
+  std::array<float, H2_NUM_MOTOR> q = {};
+  std::array<float, H2_NUM_MOTOR> dq = {};
+};
+
+// Stiffness for all H2 Joints
+std::array<float, H2_NUM_MOTOR> Kp{
+    150, 150, 150, 250, 60, 90,
+    150, 150, 150, 250, 60, 90,
+    200, 200, 200,
+    90,  60,  20,  60,  4,  4,  4,
+    90,  60,  20,  60,  4,  4,  4,
+    30,  30,
+};
+
+// Damping for all H2 Joints
+std::array<float, H2_NUM_MOTOR> Kd{
+    2.0, 2.0, 2.0, 2.0, 0.3, 0.1,
+    2.0, 2.0, 2.0, 2.0, 0.3, 0.1,
+    2.5, 5.0, 5.0,
+    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+    1.0, 1.0,
+};
+
+enum class Mode {
+  PR = 0, // Series Control for Ptich/Roll Joints
+  AB = 1  // Parallel Control for A/B Joints
+};
+
+enum H2JointIndex {
+  LeftHipPitch = 0,
+  LeftHipRoll = 1,
+  LeftHipYaw = 2,
+  LeftKnee = 3,
+  LeftAnkleRoll = 4,
+  LeftAnkleRollRaw = 4,
+  LeftAnklePitch = 5,
+  LeftAnklePitchRaw = 5,
+  RightHipPitch = 6,
+  RightHipRoll = 7,
+  RightHipYaw = 8,
+  RightKnee = 9,
+  RightAnkleRoll = 10,
+  RightAnkleRollRaw = 10,
+  RightAnklePitch = 11,
+  RightAnklePitchRaw = 11,
+  WaistYaw = 12,
+  WaistRoll = 13,       
+  WaistA = 13,    
+  WaistPitch = 14,   
+  WaistB = 14,
+  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,
+  HEAD_PITCH = 29,
+  HEAD_YAW = 30,
+};
+
+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 H2Example {
+private:
+  double time_;
+  double control_dt_; // [2ms]
+  double duration_;   // [3 s]
+  int counter_;
+  Mode mode_pr_;
+  uint8_t mode_machine_;
+
+  Gamepad gamepad_;
+  REMOTE_DATA_RX rx_;
+
+  DataBuffer<MotorState> motor_state_buffer_;
+  DataBuffer<MotorCommand> motor_command_buffer_;
+  DataBuffer<ImuState> imu_state_buffer_;
+
+  ChannelPublisherPtr<LowCmd_> lowcmd_publisher_;
+  ChannelSubscriberPtr<LowState_> lowstate_subscriber_;
+  ChannelSubscriberPtr<IMUState_> imutorso_subscriber_;
+  ThreadPtr command_writer_ptr_, control_thread_ptr_;
+
+  std::shared_ptr<unitree::robot::b2::MotionSwitcherClient> msc_;
+
+public:
+  H2Example(std::string networkInterface)
+      : time_(0.0), control_dt_(0.002), duration_(3.0), counter_(0),
+        mode_pr_(Mode::PR), mode_machine_(0) {
+    ChannelFactory::Instance()->Init(0, networkInterface);
+
+    // try to shutdown motion control-related service
+    msc_ = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
+    msc_->SetTimeout(5.0f);
+    msc_->Init();
+    std::string form, name;
+    while (msc_->CheckMode(form, name), !name.empty()) {
+      if (msc_->ReleaseMode())
+        std::cout << "Failed to switch to Release Mode\n";
+      sleep(5);
+    }
+
+    // create publisher
+    lowcmd_publisher_.reset(new ChannelPublisher<LowCmd_>(HG_CMD_TOPIC));
+    lowcmd_publisher_->InitChannel();
+    // create subscriber
+    lowstate_subscriber_.reset(
+        new ChannelSubscriber<LowState_>(HG_STATE_TOPIC));
+    lowstate_subscriber_->InitChannel(
+        std::bind(&H2Example::LowStateHandler, this, std::placeholders::_1), 1);
+    imutorso_subscriber_.reset(new ChannelSubscriber<IMUState_>(HG_IMU_TORSO));
+    imutorso_subscriber_->InitChannel(
+        std::bind(&H2Example::imuTorsoHandler, this, std::placeholders::_1), 1);
+    // create threads
+    command_writer_ptr_ =
+        CreateRecurrentThreadEx("command_writer", UT_CPU_ID_NONE, 2000,
+                                &H2Example::LowCommandWriter, this);
+    control_thread_ptr_ = CreateRecurrentThreadEx(
+        "control", UT_CPU_ID_NONE, 2000, &H2Example::Control, this);
+  }
+
+  void imuTorsoHandler(const void *message) {
+    IMUState_ imu_torso = *(const IMUState_ *)message;
+    auto &rpy = imu_torso.rpy();
+    if (counter_ % 500 == 0)
+      printf("IMU.torso.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
+  }
+
+  void LowStateHandler(const void *message) {
+    LowState_ low_state = *(const LowState_ *)message;
+    if (low_state.crc() !=
+        Crc32Core((uint32_t *)&low_state, (sizeof(LowState_) >> 2) - 1)) {
+      std::cout << "[ERROR] CRC Error" << std::endl;
+      return;
+    }
+
+    // get motor state
+    MotorState ms_tmp;
+    for (int i = 0; i < H2_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() && i <= RightAnkleRoll)
+        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 gamepad
+    memcpy(rx_.buff, &low_state.wireless_remote()[0], 40);
+    gamepad_.update(rx_.RF_RX);
+
+    // update mode machine
+    if (mode_machine_ != low_state.mode_machine()) {
+      if (mode_machine_ == 0)
+        std::cout << "H2 type: " << unsigned(low_state.mode_machine())
+                  << std::endl;
+      mode_machine_ = low_state.mode_machine();
+    }
+
+    // report robot status every second
+    if (++counter_ % 500 == 0) {
+      counter_ = 0;
+      // IMU
+      auto &rpy = low_state.imu_state().rpy();
+      printf("IMU.pelvis.rpy: %.2f %.2f %.2f\n", rpy[0], rpy[1], rpy[2]);
+
+      // RC
+      printf("gamepad_.A.pressed: %d\n", static_cast<int>(gamepad_.A.pressed));
+      printf("gamepad_.B.pressed: %d\n", static_cast<int>(gamepad_.B.pressed));
+      printf("gamepad_.X.pressed: %d\n", static_cast<int>(gamepad_.X.pressed));
+      printf("gamepad_.Y.pressed: %d\n", static_cast<int>(gamepad_.Y.pressed));
+
+      // Motor
+      auto &ms = low_state.motor_state();
+      printf("All %d Motors:", H2_NUM_MOTOR);
+      printf("\nmode: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,", ms[i].mode());
+      printf("\npos: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].q());
+      printf("\nvel: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].dq());
+      printf("\ntau_est: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].tau_est());
+      printf("\ntemperature: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%d,%d;", ms[i].temperature()[0], ms[i].temperature()[1]);
+      printf("\nvol: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%.2f,", ms[i].vol());
+      printf("\nsensor: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,%u;", ms[i].sensor()[0], ms[i].sensor()[1]);
+      printf("\nmotorstate: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,", ms[i].motorstate());
+      printf("\nreserve: ");
+      for (int i = 0; i < H2_NUM_MOTOR; ++i)
+        printf("%u,%u,%u,%u;", ms[i].reserve()[0], ms[i].reserve()[1],
+               ms[i].reserve()[2], ms[i].reserve()[3]);
+      printf("\n");
+    }
+  }
+
+  void LowCommandWriter() {
+    LowCmd_ dds_low_command;
+    dds_low_command.mode_pr() = static_cast<uint8_t>(mode_pr_);
+    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 < H2_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() {
+    MotorCommand motor_command_tmp;
+    const std::shared_ptr<const MotorState> ms = motor_state_buffer_.GetData();
+
+    for (int i = 0; i < H2_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) = Kp[i];
+      motor_command_tmp.kd.at(i) = Kd[i];
+    }
+
+    if (ms) {
+      time_ += control_dt_;
+      if (time_ < duration_) {
+        // [Stage 1]: set robot to zero posture
+        for (int i = 0; i < H2_NUM_MOTOR; ++i) {
+          double ratio = std::clamp(time_ / duration_, 0.0, 1.0);
+          motor_command_tmp.q_target.at(i) = (1.0 - ratio) * ms->q.at(i);
+        }
+      } else if (time_ < duration_ * 2) {
+        // [Stage 2]: swing ankle using PR mode
+        mode_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_;
+        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);
+
+        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;
+      }
+
+      motor_command_buffer_.SetData(motor_command_tmp);
+    }
+  }
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: h2_ankle_swing_example network_interface" << std::endl;
+    exit(0);
+  }
+  std::string networkInterface = argv[1];
+  H2Example custom(networkInterface);
+  while (true)
+    sleep(10);
+  return 0;
+}
+

unitree_sdk2/example/h2/low_level/h2_dual_arm_example.cpp

@@ -0,0 +1,185 @@
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <csignal>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <yaml-cpp/yaml.h>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+namespace {
+
+using LowCmd = unitree_hg::msg::dds_::LowCmd_;
+using LowState = unitree_hg::msg::dds_::LowState_;
+constexpr char kCmdTopic[] = "rt/lowcmd";
+constexpr char kStateTopic[] = "rt/lowstate";
+constexpr int kNumMotors = 31;
+constexpr int kLeftShoulderPitch = 15;
+constexpr int kRightWristYaw = 28;
+constexpr int kHeadPitch = 29;
+constexpr int kHeadYaw = 30;
+constexpr double kControlDt = 0.002;
+constexpr double kZeroDuration = 3.0;
+// Replay the recorded trajectory at 60% of its original speed.
+constexpr double kTrajectorySpeedScale = 0.6;
+
+std::atomic<bool> g_stop{false};
+void HandleSignal(int) { g_stop.store(true); }
+
+const std::array<float, kNumMotors> kKp = {
+    150, 150, 150, 250, 60, 90, 150, 150, 150, 250, 60, 90,
+    200, 200, 200, 90, 60, 20, 60, 4, 4, 4, 90, 60, 20, 60,
+    4, 4, 4, 30, 30};
+const std::array<float, kNumMotors> kKd = {
+    2, 2, 2, 2, .3f, .1f, 2, 2, 2, 2, .3f, .1f, 2.5f, 5, 5,
+    2, 1, .4f, 1, .2f, .2f, .2f, 2, 1, .4f, 1, .2f, .2f, .2f, 1, 1};
+
+inline uint32_t Crc32Core(uint32_t* ptr, uint32_t len) {
+  uint32_t crc = 0xFFFFFFFF, polynomial = 0x04c11db7;
+  for (uint32_t i = 0; i < len; ++i) {
+    uint32_t data = ptr[i], bit = 1u << 31;
+    for (int j = 0; j < 32; ++j) {
+      crc = (crc & 0x80000000) ? (crc << 1) ^ polynomial : crc << 1;
+      if (data & bit) crc ^= polynomial;
+      bit >>= 1;
+    }
+  }
+  return crc;
+}
+
+class StateBuffer {
+ public:
+  void Set(const LowState& state) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state_ = state;
+    ready_ = true;
+  }
+  bool Get(LowState& state) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!ready_) return false;
+    state = state_;
+    return true;
+  }
+
+ private:
+  mutable std::mutex mutex_;
+  LowState state_{};
+  bool ready_ = false;
+};
+
+std::vector<std::array<float, 14>> LoadTrajectory(const std::string& path) {
+  YAML::Node root = YAML::LoadFile(path);
+  if (root["frame_rate"].as<int>() != 500) {
+    throw std::runtime_error("motion.seq must use 500 Hz");
+  }
+  for (const auto& component : root["components"]) {
+    if (component["content"].as<std::string>("") != "JointDisplacement") continue;
+    std::vector<std::array<float, 14>> frames;
+    for (const auto& frame : component["frames"]) {
+      if (frame.size() != 14) throw std::runtime_error("trajectory frame is not 14-DOF");
+      std::array<float, 14> values{};
+      for (size_t i = 0; i < values.size(); ++i) values[i] = frame[i].as<float>();
+      frames.push_back(values);
+    }
+    return frames;
+  }
+  throw std::runtime_error("JointDisplacement component not found");
+}
+
+}  // namespace
+
+int main(int argc, char** argv) {
+  if (argc < 2) {
+    std::cout << "Usage: h2_dual_arm_example network_interface" << std::endl;
+    return 0;
+  }
+  ::signal(SIGINT, HandleSignal);
+  ::signal(SIGTERM, HandleSignal);
+
+  try {
+    const std::string network = argv[1];
+    const std::string trajectory_path = std::string(H2_DUAL_ARM_BEHAVIOR_DIR) + "motion.seq";
+    const auto trajectory = LoadTrajectory(trajectory_path);
+    std::cout << "Loaded " << trajectory.size() << " frames from " << trajectory_path << std::endl;
+
+    unitree::robot::ChannelFactory::Instance()->Init(0, network);
+    auto motion_switcher = std::make_shared<unitree::robot::b2::MotionSwitcherClient>();
+    motion_switcher->SetTimeout(5.0f);
+    motion_switcher->Init();
+    std::string form, mode;
+    while (!g_stop.load() && motion_switcher->CheckMode(form, mode) == 0 && !mode.empty()) {
+      std::cout << "Releasing motion service: " << mode << std::endl;
+      if (motion_switcher->ReleaseMode() != 0) throw std::runtime_error("ReleaseMode failed");
+      std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    auto publisher = std::make_shared<unitree::robot::ChannelPublisher<LowCmd>>(kCmdTopic);
+    publisher->InitChannel();
+    auto state_buffer = std::make_shared<StateBuffer>();
+    auto subscriber = std::make_shared<unitree::robot::ChannelSubscriber<LowState>>(kStateTopic);
+    subscriber->InitChannel([state_buffer](const void* message) {
+      const auto& state = *static_cast<const LowState*>(message);
+      if (state.crc() == Crc32Core((uint32_t*)&state, (sizeof(LowState) >> 2) - 1)) state_buffer->Set(state);
+    }, 1);
+
+    LowState state;
+    while (!g_stop.load() && !state_buffer->Get(state)) std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    if (g_stop.load()) return 0;
+
+    LowCmd command{};
+    double time = 0.0;
+    while (!g_stop.load()) {
+      if (!state_buffer->Get(state)) continue;
+      command.mode_pr() = 0;
+      command.mode_machine() = state.mode_machine();
+      const double ratio = std::clamp(time / kZeroDuration, 0.0, 1.0);
+      const size_t frame = time < kZeroDuration
+          ? 0
+          : std::min(static_cast<size_t>((time - kZeroDuration) *
+                                         kTrajectorySpeedScale / kControlDt),
+                     trajectory.size() - 1);
+      for (int i = 0; i < kNumMotors; ++i) {
+        auto& motor = command.motor_cmd().at(i);
+        motor.mode(1);
+        motor.tau(0.0f);
+        motor.dq(0.0f);
+        motor.kp(kKp[i]);
+        motor.kd(kKd[i]);
+        if (time < kZeroDuration) {
+          motor.q(static_cast<float>((1.0 - ratio) * state.motor_state().at(i).q()));
+        } else if (i >= kLeftShoulderPitch && i <= kRightWristYaw) {
+          motor.q(trajectory[frame][static_cast<size_t>(i - kLeftShoulderPitch)]);
+        } else {
+          motor.q(0.0f);
+        }
+      }
+      command.crc() = Crc32Core((uint32_t*)&command, (sizeof(LowCmd) >> 2) - 1);
+      publisher->Write(command);
+      time += kControlDt;
+      if (time >= kZeroDuration +
+                      trajectory.size() * kControlDt / kTrajectorySpeedScale) {
+        time = 0.0;
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    }
+    std::cout << "Stopped by Ctrl+C; low-level command loop exited." << std::endl;
+  } catch (const std::exception& error) {
+    std::cerr << "h2_dual_arm_example failed: " << error.what() << std::endl;
+    return 1;
+  }
+  return 0;
+}

unitree_sdk2/example/h2/low_level/terminations.cpp

@@ -0,0 +1,47 @@
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <thread>
+
+#include <boost/program_options.hpp>
+
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/robot/h2/common/terminations.hpp>
+
+namespace po = boost::program_options;
+using unitree::robot::ChannelFactory;
+using unitree::robot::ChannelSubscriber;
+using unitree_hg::msg::dds_::LowState_;
+
+int main(int argc, char** argv) {
+  po::options_description options("Unitree H2 termination functions testing.");
+  options.add_options()
+      ("network,n", po::value<std::string>()->default_value(""),
+       "dds network interface");
+  po::variables_map args;
+  po::store(po::parse_command_line(argc, argv, options), args);
+  po::notify(args);
+  std::cout << options << std::endl;
+
+  ChannelFactory::Instance()->Init(0, args["network"].as<std::string>());
+
+  auto lowstate_subscriber =
+      std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
+  LowState_ lowstate;
+  lowstate_subscriber->InitChannel([&lowstate](const void* message) {
+    lowstate = *static_cast<const LowState_*>(message);
+  });
+
+  std::cout << "Checking terminations..." << std::endl;
+  while (true) {
+    if (unitree::robot::h2::bad_orientation(lowstate, 1.0f)) {
+      std::cout << "Bad orientation detected!" << std::endl;
+    }
+    if (unitree::robot::h2::lost_connection(lowstate_subscriber, 1000)) {
+      std::cout << "Lost connection!" << std::endl;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+}