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

unitree_sdk2/example/g1/high_level/g1_arm5_sdk_dds_example.cpp

@@ -0,0 +1,256 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+constexpr float kPi = 3.141592654;
+constexpr float kPi_2 = 1.57079632;
+
+enum JointIndex {
+    // Left leg
+    kLeftHipPitch = 0,
+    kLeftHipRoll = 1,
+    kLeftHipYaw = 2,
+    kLeftKnee = 3,
+    kLeftAnkle  = 4,
+    kLeftAnkleRoll = 5,
+
+    // Right leg
+    kRightHipPitch = 6,
+    kRightHipRoll = 7,
+    kRightHipYaw = 8,
+    kRightKnee = 9,
+    kRightAnkle = 10,
+    kRightAnkleRoll = 11,
+
+    kWaistYaw = 12,
+    kWaistRoll = 13,
+    kWaistPitch = 14,
+
+    // Left arm
+    kLeftShoulderPitch = 15,
+    kLeftShoulderRoll = 16,
+    kLeftShoulderYaw = 17,
+    kLeftElbowPitch = 18,
+    kLeftElbowRoll = 19,
+    
+    // Right arm
+    kRightShoulderPitch = 22,
+    kRightShoulderRoll = 23,
+    kRightShoulderYaw = 24,
+    kRightElbowPitch = 25,
+    kRightElbowRoll = 26,
+
+    kNotUsedJoint = 29,
+    kNotUsedJoint1 = 30,
+    kNotUsedJoint2 = 31,
+    kNotUsedJoint3 = 32,
+    kNotUsedJoint4 = 33,
+    kNotUsedJoint5 = 34
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
+      arm_sdk_publisher;
+  unitree_hg::msg::dds_::LowCmd_ msg;
+
+  arm_sdk_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
+          kTopicArmSDK));
+  arm_sdk_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  std::array<JointIndex, 13> arm_joints = {
+      JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
+      JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbowPitch,
+      JointIndex::kLeftElbowRoll,
+      JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
+      JointIndex::kRightShoulderYaw,   JointIndex::kRightElbowPitch,
+      JointIndex::kRightElbowRoll,
+      JointIndex::kWaistYaw,
+      JointIndex::kWaistRoll,
+      JointIndex::kWaistPitch};
+
+  float weight = 0.f;
+  float weight_rate = 0.2f;
+
+  float kp = 60.f;
+  float kd = 1.5f;
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float delta_weight = weight_rate * control_dt;
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  std::array<float, 13> init_pos{0, 0, 0, 0, 0,
+                                 0, 0, 0, 0, 0,
+                                 0, 0, 0};
+
+  std::array<float, 13> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 
+                                     0.f, 0.f, 0.f};
+
+  // wait for init
+  std::cout << "Press ENTER to init arms ...";
+  std::cin.get();
+
+  // get current joint position
+  std::array<float, 13> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < arm_joints.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing arms ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    // set weight
+	weight = 1.0;
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+	float phase = 1.0 * i / init_time_steps;
+	std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start arm ctrl!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 13> current_jpos_des{};
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping arm ctrl ...";
+  float stop_time = 2.0f;
+  int stop_time_steps = static_cast<int>(stop_time / control_dt);
+
+  for (int i = 0; i < stop_time_steps; ++i) {
+    // increase weight
+    weight -= delta_weight;
+    weight = std::clamp(weight, 0.f, 1.f);
+
+    // set weight
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // set weight
+  msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(0);
+  // send dds msg
+  arm_sdk_publisher->Write(msg);
+  
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}

unitree_sdk2/example/g1/high_level/g1_arm7_sdk_dds_example.cpp

@@ -0,0 +1,262 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+constexpr float kPi = 3.141592654;
+constexpr float kPi_2 = 1.57079632;
+
+enum JointIndex {
+    // Left leg
+    kLeftHipPitch,
+    kLeftHipRoll,
+    kLeftHipYaw,
+    kLeftKnee,
+    kLeftAnkle,
+    kLeftAnkleRoll,
+
+    // Right leg
+    kRightHipPitch,
+    kRightHipRoll,
+    kRightHipYaw,
+    kRightKnee,
+    kRightAnkle,
+    kRightAnkleRoll,
+
+    kWaistYaw,
+    kWaistRoll,
+    kWaistPitch,
+
+    // Left arm
+    kLeftShoulderPitch,
+    kLeftShoulderRoll,
+    kLeftShoulderYaw,
+    kLeftElbow,
+    kLeftWristRoll,
+    kLeftWristPitch,
+    kLeftWristYaw,
+    // Right arm
+    kRightShoulderPitch,
+    kRightShoulderRoll,
+    kRightShoulderYaw,
+    kRightElbow,
+    kRightWristRoll,
+    kRightWristPitch,
+    kRightWristYaw,
+
+    kNotUsedJoint,
+    kNotUsedJoint1,
+    kNotUsedJoint2,
+    kNotUsedJoint3,
+    kNotUsedJoint4,
+    kNotUsedJoint5
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
+      arm_sdk_publisher;
+  unitree_hg::msg::dds_::LowCmd_ msg;
+
+  arm_sdk_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
+          kTopicArmSDK));
+  arm_sdk_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  std::array<JointIndex, 17> arm_joints = {
+      JointIndex::kLeftShoulderPitch,  JointIndex::kLeftShoulderRoll,
+      JointIndex::kLeftShoulderYaw,    JointIndex::kLeftElbow,
+      JointIndex::kLeftWristRoll,       JointIndex::kLeftWristPitch,
+      JointIndex::kLeftWristYaw,
+      JointIndex::kRightShoulderPitch, JointIndex::kRightShoulderRoll,
+      JointIndex::kRightShoulderYaw,   JointIndex::kRightElbow,
+      JointIndex::kRightWristRoll,      JointIndex::kRightWristPitch,
+      JointIndex::kRightWristYaw,
+      JointIndex::kWaistYaw,
+      JointIndex::kWaistRoll,
+      JointIndex::kWaistPitch};
+
+  float weight = 0.f;
+  float weight_rate = 0.2f;
+
+  float kp = 60.f;
+  float kd = 1.5f;
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float delta_weight = weight_rate * control_dt;
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  std::array<float, 17> init_pos{0, 0, 0, 0, 0, 0, 0,
+                                  0, 0, 0, 0, 0, 0, 0,
+                                  0, 0, 0};
+
+  std::array<float, 17> target_pos = {0.f, kPi_2,  0.f, kPi_2, 0.f, 0.f, 0.f,
+                                     0.f, -kPi_2, 0.f, kPi_2, 0.f, 0.f, 0.f, 
+                                     0, 0, 0};
+
+  // wait for init
+  std::cout << "Press ENTER to init arms ...";
+  std::cin.get();
+
+  // get current joint position
+  std::array<float, 17> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < arm_joints.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(arm_joints.at(i)).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing arms ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    // increase weight
+    weight = 1.0;
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+    float phase = 1.0 * i / init_time_steps;
+    std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start arm ctrl ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start arm ctrl!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 17> current_jpos_des{};
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(arm_joints.at(j)).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(arm_joints.at(j)).dq(dq);
+      msg.motor_cmd().at(arm_joints.at(j)).kp(kp);
+      msg.motor_cmd().at(arm_joints.at(j)).kd(kd);
+      msg.motor_cmd().at(arm_joints.at(j)).tau(tau_ff);
+    }
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping arm ctrl ...";
+  float stop_time = 2.0f;
+  int stop_time_steps = static_cast<int>(stop_time / control_dt);
+
+  for (int i = 0; i < stop_time_steps; ++i) {
+    // increase weight
+    weight -= delta_weight;
+    weight = std::clamp(weight, 0.f, 1.f);
+
+    // set weight
+    msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(weight);
+
+    // send dds msg
+    arm_sdk_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+
+  // set weight
+  msg.motor_cmd().at(JointIndex::kNotUsedJoint).q(0);
+  // send dds msg
+  arm_sdk_publisher->Write(msg);
+
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}

unitree_sdk2/example/g1/high_level/g1_arm_action_example.cpp

@@ -0,0 +1,85 @@
+/**
+ * @file g1_arm_action_example.cpp
+ * @brief This example demonstrates how to use the G1 Arm Action Client to execute predefined arm actions.
+ */
+#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
+#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
+#include <boost/program_options.hpp>
+
+namespace po = boost::program_options;
+using namespace unitree::robot;
+using namespace unitree::robot::g1;
+
+int main(int argc, const char** argv) 
+{
+    std::cout << " --- Unitree Robotics --- \n";
+    std::cout << "     G1 Arm Action Example      \n\n";
+
+    // Parse command line arguments
+    po::options_description desc("Unitree G1 Arm Action Example.");
+    desc.add_options()
+        ("help,h", "show help message")
+        ("network,n", po::value<std::string>()->default_value(""), "dds network interface")
+        ("list,l", "list all supported actions")
+        ("id,i", po::value<int>(), "action id to execute, 0 to list all supported actions")
+        ("name", po::value<std::string>(), "custom action name to execute")
+        ("stop", "stop the current custom action")
+        ;
+    po::variables_map vm;
+    po::store(po::parse_command_line(argc, argv, desc), vm);
+    po::notify(vm);
+    if(argc < 2 || vm.count("help")) {
+        std::cout << desc << std::endl;
+        return 0;
+    }
+
+    // DDS Init
+    ChannelFactory::Instance()->Init(0, vm["network"].as<std::string>());
+
+    auto client = std::make_shared<G1ArmActionClient>();
+    client->Init();
+    // Attention: unitree actions are all less than 10s, 
+    //   but the custom actions may be longer.
+    client->SetTimeout(10.f);
+
+    if(vm.count("list") || (vm.count("id") && vm["id"].as<int>() == 0)){
+        std::string action_list_data;
+        int32_t ret = client->GetActionList(action_list_data);
+        if (ret != 0) {
+            std::cerr << "Failed to get action list, error code: " << ret << "\n";
+        }
+        std::cout << "Available actions:\n" << action_list_data << std::endl;
+    } else if (vm.count("id")) {
+        int32_t ret = client->ExecuteAction(vm["id"].as<int>());
+        if(ret != 0) {
+            switch (ret)
+            {
+            case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
+                std::cout << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC << std::endl;
+                break;
+            case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
+                std::cout << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
+                break;
+            case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
+                std::cout << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
+                break;
+            case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
+                std::cout << "The actions are only supported in fsm id {500, 501, 801}" << std::endl;
+                std::cout << "You can subscribe the topic rt/sportmodestate to check the fsm id." << std::endl;
+                std::cout << "And in the state 801, the actions are only supported in the fsm mode {0, 3}." << std::endl;
+                std::cout << "If an error is still returned at this point, ignore this action.";
+                break;
+            default:
+                std::cerr << "Execute action failed, error code: " << ret << std::endl;
+                break;
+            }
+        }
+    } else if (vm.count("name")) {
+        int32_t ret = client->ExecuteAction(vm["name"].as<std::string>());
+        if(ret != 0) std::cout << "Execute custom action failed, error code: " << ret << std::endl;
+    } else if (vm.count("stop")) {
+        int32_t ret = client->StopCustomAction();
+    }
+
+    return 0;
+};

unitree_sdk2/example/g1/high_level/g1_hand_sdk_example.cpp

@@ -0,0 +1,76 @@
+// G1 Hand SDK Control Example
+//
+//   Motor_real = weight * Hand_SDK + (1 - weight) * G1_Cmd
+//
+// Demonstrates how a user process injects commands for the 4 hand motors
+// into ai_sport via the DDS topic `rt/hand_sdk`. This example takes full
+// control of the hand (weight = 1.0) and toggles tau between +0.3 and
+// -0.3 every second to alternate between "close" and "open".
+//
+//
+// Run:
+//   ./hand_sdk_example              # use default network interface
+//   ./hand_sdk_example eth0         # specify a network interface
+
+#include <unitree/robot/channel/channel_factory.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/idl/go2/MotorCmds_.hpp>
+
+#include <algorithm>
+#include <chrono>
+#include <thread>
+
+class HandSdk {
+public:
+    static constexpr int kMotorNum = 4;
+
+    explicit HandSdk(const std::string& topic = "rt/hand_sdk")
+        : publisher_(std::make_shared<unitree::robot::ChannelPublisher<
+              unitree_go::msg::dds_::MotorCmds_>>(topic)) {
+        publisher_->InitChannel();
+        msg_.cmds().resize(kMotorNum);
+    }
+
+    // weight is in [0, 1] and is encoded as `weight * 100` (uint8) in cmds[0].mode.
+    float weight() const {
+        return std::clamp(msg_.cmds()[0].mode() / 100.f, 0.f, 1.f);
+    }
+    void set_weight(float w) {
+        msg_.cmds()[0].mode(
+            static_cast<uint8_t>(std::clamp(w, 0.f, 1.f) * 100.f));
+    }
+
+    // Positive tau closes the hand, negative tau opens it.
+    void set_tau(float tau) {
+        for (int i = 0; i < kMotorNum; ++i) {
+            msg_.cmds()[i].tau(tau);
+        }
+    }
+
+    void write() { publisher_->Write(msg_); }
+
+private:
+    std::shared_ptr<unitree::robot::ChannelPublisher<
+        unitree_go::msg::dds_::MotorCmds_>> publisher_;
+    unitree_go::msg::dds_::MotorCmds_ msg_;
+};
+
+int main(int argc, char** argv) {
+    if (argc > 1) {
+        unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+    } else {
+        unitree::robot::ChannelFactory::Instance()->Init(0);
+    }
+
+    HandSdk hand_sdk;
+    hand_sdk.set_weight(1.0f);
+
+    float tau = 0.3f;
+    while (true) {
+        tau = -tau;
+        hand_sdk.set_tau(tau);
+        hand_sdk.write();
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+    return 0;
+}

unitree_sdk2/example/g1/high_level/g1_loco_client_example.cpp

@@ -0,0 +1,273 @@
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/robot/g1/loco/g1_loco_api.hpp>
+#include <unitree/robot/g1/loco/g1_loco_client.hpp>
+
+std::vector<float> stringToFloatVector(const std::string &str) {
+  std::vector<float> result;
+  std::stringstream ss(str);
+  float num;
+  while (ss >> num) {
+    result.push_back(num);
+    // ignore any trailing whitespace
+    ss.ignore();
+  }
+  return result;
+}
+
+int main(int argc, char const *argv[]) {
+  std::map<std::string, std::string> args = {{"network_interface", "lo"}};
+
+  std::map<std::string, std::string> values;
+  for (int i = 1; i < argc; ++i) {
+    std::string arg = argv[i];
+    if (arg.substr(0, 2) == "--") {
+      size_t pos = arg.find("=");
+      std::string key, value;
+      if (pos != std::string::npos) {
+        key = arg.substr(2, pos - 2);
+        value = arg.substr(pos + 1);
+
+        if (value.front() == '"' && value.back() == '"') {
+          value = value.substr(1, value.length() - 2);
+        }
+      } else {
+        key = arg.substr(2);
+        value = "";
+      }
+      if (args.find(key) != args.end()) {
+        args[key] = value;
+      } else {
+        args.insert({{key, value}});
+      }
+    }
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0,
+                                                   args["network_interface"]);
+
+  unitree::robot::g1::LocoClient client;
+
+  client.Init();
+  client.SetTimeout(10.f);
+
+  for (const auto &arg_pair : args) {
+    std::cout << "Processing command: [" << arg_pair.first << "] with param: ["
+              << arg_pair.second << "] ..." << std::endl;
+    if (arg_pair.first == "network_interface") {
+      continue;
+    }
+
+    if (arg_pair.first == "get_fsm_id") {
+      int fsm_id;
+      client.GetFsmId(fsm_id);
+      std::cout << "current fsm_id: " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "get_fsm_mode") {
+      int fsm_mode;
+      client.GetFsmMode(fsm_mode);
+      std::cout << "current fsm_mode: " << fsm_mode << std::endl;
+    }
+
+    if (arg_pair.first == "get_balance_mode") {
+      int balance_mode;
+      client.GetBalanceMode(balance_mode);
+      std::cout << "current balance_mode: " << balance_mode << std::endl;
+    }
+
+    if (arg_pair.first == "get_swing_height") {
+      float swing_height;
+      client.GetSwingHeight(swing_height);
+      std::cout << "current swing_height: " << swing_height << std::endl;
+    }
+
+    if (arg_pair.first == "get_stand_height") {
+      float stand_height;
+      client.GetStandHeight(stand_height);
+      std::cout << "current stand_height: " << stand_height << std::endl;
+    }
+
+    if (arg_pair.first == "get_phase") {
+      std::vector<float> phase;
+      client.GetPhase(phase);
+      std::cout << "current phase: (";
+      for (const auto &p : phase) {
+        std::cout << p << ", ";
+      }
+      std::cout << ")" << std::endl;
+    }
+
+    if (arg_pair.first == "set_fsm_id") {
+      int fsm_id = std::stoi(arg_pair.second);
+      client.SetFsmId(fsm_id);
+      std::cout << "set fsm_id to " << fsm_id << std::endl;
+    }
+
+    if (arg_pair.first == "set_balance_mode") {
+      int balance_mode = std::stoi(arg_pair.second);
+      client.SetBalanceMode(balance_mode);
+      std::cout << "set balance_mode to " << balance_mode << std::endl;
+    }
+
+    if (arg_pair.first == "set_swing_height") {
+      float swing_height = std::stof(arg_pair.second);
+      client.SetSwingHeight(swing_height);
+      std::cout << "set swing_height to " << swing_height << std::endl;
+    }
+
+    if (arg_pair.first == "set_stand_height") {
+      float stand_height = std::stof(arg_pair.second);
+      client.SetStandHeight(stand_height);
+      std::cout << "set stand_height to " << stand_height << std::endl;
+    }
+
+    if (arg_pair.first == "set_velocity") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega, duration;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = 1.f;
+      } else if (param_size == 4) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+        duration = param.at(3);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size
+                  << std::endl;
+        return 1;
+      }
+
+      client.SetVelocity(vx, vy, omega, duration);
+      std::cout << "set velocity to " << arg_pair.second << std::endl;
+    }
+
+    if (arg_pair.first == "damp") {
+      client.Damp();
+    }
+
+    if (arg_pair.first == "start") {
+      client.Start();
+    }
+
+    if (arg_pair.first == "squat") {
+      client.Squat();
+    }
+
+    if (arg_pair.first == "sit") {
+      client.Sit();
+    }
+
+    if (arg_pair.first == "stand_up") {
+      client.StandUp();
+    }
+
+    if (arg_pair.first == "zero_torque") {
+      client.ZeroTorque();
+    }
+
+    if (arg_pair.first == "stop_move") {
+      client.StopMove();
+    }
+
+    if (arg_pair.first == "high_stand") {
+      client.HighStand();
+    }
+
+    if (arg_pair.first == "low_stand") {
+      client.LowStand();
+    }
+
+    if (arg_pair.first == "balance_stand") {
+      client.BalanceStand();
+    }
+
+    if (arg_pair.first == "continous_gait") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.ContinuousGait(flag);
+    }
+
+    if (arg_pair.first == "switch_move_mode") {
+      bool flag;
+      if (arg_pair.second == "true") {
+        flag = true;
+      } else if (arg_pair.second == "false") {
+        flag = false;
+      } else {
+        std::cerr << "invalid argument: " << arg_pair.second << std::endl;
+        return 1;
+      }
+      client.SwitchMoveMode(flag);
+    }
+
+    if (arg_pair.first == "move") {
+      std::vector<float> param = stringToFloatVector(arg_pair.second);
+      auto param_size = param.size();
+      float vx, vy, omega;
+      if (param_size == 3) {
+        vx = param.at(0);
+        vy = param.at(1);
+        omega = param.at(2);
+      } else {
+        std::cerr << "Invalid param size for method SetVelocity: " << param_size
+                  << std::endl;
+        return 1;
+      }
+      client.Move(vx, vy, omega);
+    }
+
+    if (arg_pair.first == "set_task_id") {
+      int task_id = std::stoi(arg_pair.second);
+      client.SetTaskId(task_id);
+      std::cout << "set task_id to " << task_id << std::endl;
+    }
+
+    if (arg_pair.first == "shake_hand") {
+      client.ShakeHand(0);
+      std::cout << "Shake hand starts! Waiting for 10 s for ending"
+                << std::endl;
+      std::this_thread::sleep_for(std::chrono::seconds(10));
+      std::cout << "Shake hand ends!" << std::endl;
+      client.ShakeHand(1);
+    }
+
+    if (arg_pair.first == "wave_hand") {
+      client.WaveHand();
+      std::cout << "wave hand" << std::endl;
+    }
+
+    if (arg_pair.first == "wave_hand_with_turn") {
+      client.WaveHand(true);
+      std::cout << "wave hand with turn" << std::endl;
+    }
+
+    if (arg_pair.first == "set_speed_mode") {
+      client.SetSpeedMode(std::stoi(arg_pair.second));
+      std::cout << "set speed mode" << std::endl;
+    }
+
+    if (arg_pair.first == "get_mimic_motion") {
+      std::string data;
+      client.GetMimicMotion(data);
+      std::cout << "Mimic motion: " << data << std::endl;
+    }
+
+    std::cout << "Done!" << std::endl;
+  }
+
+  return 0;
+}

unitree_sdk2/example/g1/high_level/g1_userctrl_dds_example.cpp

@@ -0,0 +1,248 @@
+#include <array>
+#include <chrono>
+#include <iostream>
+#include <thread>
+
+#include <unitree/idl/hg/LowCmd_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include "unitree/robot/g1/loco/g1_loco_api.hpp"
+#include "unitree/robot/g1/loco/g1_loco_client.hpp"
+
+static const std::string kTopicUserCtrl = "rt/user_lowcmd";
+static const std::string kTopicState = "rt/lowstate";
+
+enum JointIndex {
+    // Left leg
+    kLeftHipPitch,
+    kLeftHipRoll,
+    kLeftHipYaw,
+    kLeftKnee,
+    kLeftAnkle,
+    kLeftAnkleRoll,
+
+    // Right leg
+    kRightHipPitch,
+    kRightHipRoll,
+    kRightHipYaw,
+    kRightKnee,
+    kRightAnkle,
+    kRightAnkleRoll,
+
+    kWaistYaw,
+    kWaistRoll,
+    kWaistPitch,
+
+    // Left arm
+    kLeftShoulderPitch,
+    kLeftShoulderRoll,
+    kLeftShoulderYaw,
+    kLeftElbow,
+    kLeftWristRoll,
+    kLeftWristPitch,
+    kLeftWristYaw,
+    // Right arm
+    kRightShoulderPitch,
+    kRightShoulderRoll,
+    kRightShoulderYaw,
+    kRightElbow,
+    kRightWristRoll,
+    kRightWristPitch,
+    kRightWristYaw,
+
+    kNotUsedJoint,
+    kNotUsedJoint1,
+    kNotUsedJoint2,
+    kNotUsedJoint3,
+    kNotUsedJoint4,
+    kNotUsedJoint5
+};
+
+int main(int argc, char const *argv[]) {
+  if (argc < 2) {
+    std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+    exit(-1);
+  }
+
+  unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
+
+  unitree::robot::ChannelPublisherPtr<unitree_hg::msg::dds_::LowCmd_>
+      user_ctrl_publisher;
+  unitree_hg::msg::dds_::LowCmd_ msg;
+
+  user_ctrl_publisher.reset(
+      new unitree::robot::ChannelPublisher<unitree_hg::msg::dds_::LowCmd_>(
+          kTopicUserCtrl));
+  user_ctrl_publisher->InitChannel();
+
+  unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>
+      low_state_subscriber;
+
+  // create subscriber
+  unitree_hg::msg::dds_::LowState_ state_msg;
+  low_state_subscriber.reset(
+      new unitree::robot::ChannelSubscriber<unitree_hg::msg::dds_::LowState_>(
+          kTopicState));
+  low_state_subscriber->InitChannel([&](const void *msg) {
+        auto s = ( const unitree_hg::msg::dds_::LowState_* )msg;
+        memcpy( &state_msg, s, sizeof( unitree_hg::msg::dds_::LowState_ ) );
+  }, 1);
+
+  unitree::robot::g1::LocoClient client;
+  client.Init();
+  client.SetTimeout(5.f);
+  int current_fsm_id;
+  client.GetFsmId(current_fsm_id);
+  if(current_fsm_id != 1){
+    std::cout << "Current fsm is not PASSIVE, exitting ..." <<std::endl;
+    exit(-1);
+  }
+
+  float kp = 60.f;
+  float kd = 1.5f;
+  float dq = 0.f;
+  float tau_ff = 0.f;
+
+  float control_dt = 0.02f;
+  float max_joint_velocity = 0.5f;
+
+  float max_joint_delta = max_joint_velocity * control_dt;
+  auto sleep_time =
+      std::chrono::milliseconds(static_cast<int>(control_dt / 0.001f));
+
+  std::array<float, 29> init_pos{-0.1, 0, 0, 0.3, -0.2, 0,
+                                 -0.1, 0, 0, 0.3, -0.2, 0,
+                                 0, 0, 0,
+                                 0.2, 0.2, 0, 0.9, 0, 0, 0,
+                                 0.2, -0.2, 0, 0.9, 0, 0, 0};
+
+  std::array<float, 29> target_pos = {-1.0, 0, 0, 1.5, 0.2, 0,
+                                      -1.0, 0, 0, 1.5, 0.2, 0,
+                                      0, 0, 0,
+                                      0, 0.2, 0, 0, 0, 0, 0,
+                                      0, -0.2, 0, 0, 0, 0, 0};
+
+  // wait for init
+  std::cout << "Press ENTER to init user control ...";
+  std::cin.get();
+  for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(j).q(0);
+      msg.motor_cmd().at(j).dq(0);
+      msg.motor_cmd().at(j).kp(0);
+      msg.motor_cmd().at(j).kd(kd);
+      msg.motor_cmd().at(j).tau(0);
+    }
+  user_ctrl_publisher->Write(msg);
+
+  client.SwitchToUserCtrl();
+
+  // get current joint position
+  std::array<float, 29> current_jpos{};
+  std::cout<<"Current joint position: ";
+  for (int i = 0; i < init_pos.size(); ++i) {
+	current_jpos.at(i) = state_msg.motor_state().at(i).q();
+	std::cout << current_jpos.at(i) << " ";
+  }
+  std::cout << std::endl;
+
+  // set init pos
+  std::cout << "Initailizing motors ...";
+  float init_time = 2.0f;
+  int init_time_steps = static_cast<int>(init_time / control_dt);
+
+  for (int i = 0; i < init_time_steps; ++i) {
+    
+	  float phase = 1.0 * i / init_time_steps;
+	  std::cout << "Phase: " << phase << std::endl;
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(j).q(init_pos.at(j) * phase + current_jpos.at(j) * (1 - phase));
+      msg.motor_cmd().at(j).dq(dq);
+      msg.motor_cmd().at(j).kp(kp);
+      msg.motor_cmd().at(j).kd(kd);
+      msg.motor_cmd().at(j).tau(tau_ff);
+    }
+
+    // send dds msg
+    user_ctrl_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  std::cout << "Done!" << std::endl;
+
+  // wait for control
+  std::cout << "Press ENTER to start user control ..." << std::endl;
+  std::cin.get();
+
+  // start control
+  std::cout << "Start user control!" << std::endl;
+  float period = 5.f;
+  int num_time_steps = static_cast<int>(period / control_dt);
+
+  std::array<float, 29> current_jpos_des{};
+  for (int i = 0; i < init_pos.size(); ++i) {
+	  current_jpos_des.at(i) = state_msg.motor_state().at(i).q();
+  }
+
+  // lift arms up
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(target_pos.at(j) - current_jpos_des.at(j),
+                     -max_joint_delta, max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(j).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(j).dq(dq);
+      msg.motor_cmd().at(j).kp(kp);
+      msg.motor_cmd().at(j).kd(kd);
+      msg.motor_cmd().at(j).tau(tau_ff);
+    }
+
+    // send dds msg
+    user_ctrl_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // put arms down
+  for (int i = 0; i < num_time_steps; ++i) {
+    // update jpos des
+    for (int j = 0; j < init_pos.size(); ++j) {
+      current_jpos_des.at(j) +=
+          std::clamp(init_pos.at(j) - current_jpos_des.at(j), -max_joint_delta,
+                     max_joint_delta);
+    }
+
+    // set control joints
+    for (int j = 0; j < init_pos.size(); ++j) {
+      msg.motor_cmd().at(j).q(current_jpos_des.at(j));
+      msg.motor_cmd().at(j).dq(dq);
+      msg.motor_cmd().at(j).kp(kp);
+      msg.motor_cmd().at(j).kd(kd);
+      msg.motor_cmd().at(j).tau(tau_ff);
+    }
+
+    // send dds msg
+    user_ctrl_publisher->Write(msg);
+
+    // sleep
+    std::this_thread::sleep_for(sleep_time);
+  }
+
+  // stop control
+  std::cout << "Stoping user control ...";
+  client.SwitchToInternalCtrl(unitree::robot::g1::InternalFsmMode::LAST);
+  
+  std::cout << "Done!" << std::endl;
+
+  return 0;
+}