初始化项目，包含fast_lio_ws和unitree_sdk2

unitree_sdk2/include/unitree/robot/h2/arm/h2_arm_action_api.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+#include <unitree/common/json/jsonize.hpp>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+
+const std::string ARM_ACTION_SERVICE_NAME = "arm";
+const std::string ARM_ACTION_API_VERSION = "1.0.0.14";
+
+const int32_t ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION = 7106;
+const int32_t ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST = 7107;
+const int32_t ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION = 7108;
+const int32_t ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION = 7113;
+
+class JsonizeArmActionCommand : public common::Jsonize {
+ public:
+  void fromJson(common::JsonMap& json) override {
+    common::FromJson(json["action_id"], action_id);
+  }
+
+  void toJson(common::JsonMap& json) const override {
+    common::ToJson(action_id, json["action_id"]);
+  }
+
+  int32_t action_id = 0;
+};
+
+class JsonizeArmActionName : public common::Jsonize {
+ public:
+  void fromJson(common::JsonMap& json) override {
+    common::FromJson(json["action_name"], action_name);
+  }
+
+  void toJson(common::JsonMap& json) const override {
+    common::ToJson(action_name, json["action_name"]);
+  }
+
+  std::string action_name;
+};
+
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree

unitree_sdk2/include/unitree/robot/h2/arm/h2_arm_action_client.hpp

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <cstdint>
+#include <map>
+#include <string>
+
+#include <unitree/robot/client/client.hpp>
+
+#include "h2_arm_action_api.hpp"
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+
+/**
+ * @brief Client for requesting preset H2 upper-body actions.
+ *
+ * The matching robot-side "arm" service generates the trajectory and sends it
+ * through rt/arm_sdk. This client sends action requests over RPC and does not
+ * generate joint trajectories itself.
+ */
+class H2ArmActionClient : public Client {
+ public:
+  H2ArmActionClient() : Client(ARM_ACTION_SERVICE_NAME, false) {}
+
+  void Init() {
+    SetApiVersion(ARM_ACTION_API_VERSION);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION);
+  }
+
+  int32_t ExecuteAction(int32_t action_id) {
+    JsonizeArmActionCommand command;
+    command.action_id = action_id;
+
+    std::string data;
+    const std::string parameter = common::ToJsonString(command);
+    return Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION, parameter, data);
+  }
+
+  int32_t ExecuteAction(const std::string& action_name) {
+    JsonizeArmActionName command;
+    command.action_name = action_name;
+
+    std::string data;
+    const std::string parameter = common::ToJsonString(command);
+    return Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION, parameter, data);
+  }
+
+  int32_t StopCustomAction() {
+    std::string parameter;
+    std::string data;
+    return Call(ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION, parameter, data);
+  }
+
+  int32_t GetActionList(std::string& data) {
+    std::string parameter;
+    return Call(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST, parameter, data);
+  }
+
+  // This is a local list known by the client, not a list queried from the robot.
+  inline static const std::map<std::string, int32_t> action_map = {
+      {"release arm", 99},   {"two-hand kiss", 11},
+      {"left kiss", 12},     {"hands up", 15},
+      {"clap", 17},          {"high five", 18},
+      {"hug", 19},           {"heart", 20},
+      {"right heart", 21},   {"reject", 22},
+      {"right hand up", 23}, {"x-ray", 24},
+      {"face wave", 25},     {"high wave", 26},
+      {"shake hand", 27},
+  };
+};
+
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree

unitree_sdk2/include/unitree/robot/h2/arm/h2_arm_action_error.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_ARMSDK, 7400,
+            "The topic rt/arm_sdk is occupied.")
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_HOLDING, 7401,
+            "The arm is holding. Send action 99 or repeat the last action ID to release it.")
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID, 7402,
+            "Invalid arm action ID.")
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID, 7404,
+            "The current FSM does not support arm actions.")
+
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree

unitree_sdk2/include/unitree/robot/h2/common/terminations.hpp

@@ -0,0 +1,79 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+
+#include <eigen3/Eigen/Dense>
+
+#include <unitree/idl/hg/BmsState_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+
+inline bool bad_orientation(const unitree_hg::msg::dds_::LowState_& lowstate,
+                            float limit_angle = 1.0f) {
+  const auto& imu = lowstate.imu_state();
+  Eigen::Quaternionf quat(imu.quaternion()[0], imu.quaternion()[1],
+                          imu.quaternion()[2], imu.quaternion()[3]);
+  const Eigen::Vector3f projected_gravity_b =
+      quat.conjugate() * Eigen::Vector3f(0, 0, -1);
+  return std::fabs(std::acos(-projected_gravity_b[2])) > limit_angle;
+}
+
+inline bool joint_vel_out_of_limit(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_vel = 10.0f) {
+  const auto& motors = lowstate.motor_state();
+  return std::any_of(motors.begin(), motors.end(), [limit_vel](const auto& motor) {
+    return std::fabs(motor.dq()) > limit_vel;
+  });
+}
+
+inline bool ang_vel_out_of_limit(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_vel = 6.0f) {
+  const auto& gyroscope = lowstate.imu_state().gyroscope();
+  return std::any_of(gyroscope.begin(), gyroscope.end(), [limit_vel](float value) {
+    return std::fabs(value) > limit_vel;
+  });
+}
+
+inline bool motor_winding_overheat(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_temp = 120.0f) {
+  const auto& motors = lowstate.motor_state();
+  return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto& motor) {
+    return motor.temperature()[1] > limit_temp;
+  });
+}
+
+inline bool motor_casing_overheat(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_temp = 85.0f) {
+  const auto& motors = lowstate.motor_state();
+  return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto& motor) {
+    return motor.temperature()[0] > limit_temp;
+  });
+}
+
+inline bool low_battery(const unitree_hg::msg::dds_::BmsState_& bms_state,
+                        float limit_soc = 20.0f) {
+  return bms_state.soc() < limit_soc;
+}
+
+inline bool lost_connection(
+    unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>& subscriber,
+    int64_t timeout_ms = 1000) {
+  const auto now = unitree::common::GetCurrentMonotonicTimeNanosecond();
+  const auto elapsed_ms =
+      (now - subscriber->GetLastDataAvailableTime()) / 1e6;
+  return elapsed_ms > timeout_ms;
+}
+
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree

unitree_sdk2/include/unitree/robot/h2/loco/h2_loco_api.hpp

@@ -0,0 +1,115 @@
+#ifndef __UT_ROBOT_H2_LOCO_API_HPP__
+#define __UT_ROBOT_H2_LOCO_API_HPP__
+
+#include <unitree/common/json/jsonize.hpp>
+#include <variant>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+/*service name*/
+const std::string LOCO_SERVICE_NAME = "sport";
+
+/*api version*/
+const std::string LOCO_API_VERSION = "1.0.0.0";
+
+/*api id*/
+const int32_t ROBOT_API_ID_LOCO_GET_FSM_ID = 7001;
+const int32_t ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002;
+const int32_t ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 7003;
+const int32_t ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 7004;
+const int32_t ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 7005;
+const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 7006;  // deprecated
+const int32_t ROBOT_API_ID_LOCO_GET_ARM_SDK_STATUS = 7007;
+const int32_t ROBOT_API_ID_LOCO_GET_AVAILABLE_FSM_IDS = 7008;
+
+const int32_t ROBOT_API_ID_LOCO_SET_FSM_ID = 7101;
+const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102;
+const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103;
+const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104;
+const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 7105;
+const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106;
+const int32_t ROBOT_API_ID_LOCO_SET_SPEED_MODE = 7107;
+const int32_t ROBOT_API_ID_LOCO_SET_PUNCH_API = 7108;
+const int32_t ROBOT_API_ID_LOCO_SET_ARM_SDK_STATUS = 7109;
+
+using LocoCmd = std::map<std::string, std::variant<int, float, std::vector<float>>>;
+
+class JsonizeDataVecFloat : public common::Jsonize {
+ public:
+  JsonizeDataVecFloat() {}
+  ~JsonizeDataVecFloat() {}
+
+  void fromJson(common::JsonMap& json) { common::FromJson(json["data"], data); }
+
+  void toJson(common::JsonMap& json) const { common::ToJson(data, json["data"]); }
+
+  std::vector<float> data;
+};
+
+class JsonizeVelocityCommand : public common::Jsonize {
+ public:
+  JsonizeVelocityCommand() {}
+  ~JsonizeVelocityCommand() {}
+
+  void fromJson(common::JsonMap& json) {
+    common::FromJson(json["velocity"], velocity);
+    common::FromJson(json["duration"], duration);
+  }
+
+  void toJson(common::JsonMap& json) const {
+    common::ToJson(velocity, json["velocity"]);
+    common::ToJson(duration, json["duration"]);
+  }
+
+  std::vector<float> velocity;
+  float duration;
+};
+
+struct FsmIdInfo {
+  int id;
+  std::string name;
+
+  FsmIdInfo() : id(0), name("") {}
+  FsmIdInfo(int i, const std::string& n) : id(i), name(n) {}
+};
+
+class JsonizeFsmIdList : public common::Jsonize {
+ public:
+  JsonizeFsmIdList() {}
+  ~JsonizeFsmIdList() {}
+
+  void fromJson(common::JsonMap& json) {
+    std::vector<int> ids;
+    std::vector<std::string> names;
+
+    common::FromJson(json["ids"], ids);
+    common::FromJson(json["names"], names);
+
+    fsm_ids.clear();
+    for (size_t i = 0; i < ids.size() && i < names.size(); i++) {
+      fsm_ids.push_back(FsmIdInfo(ids[i], names[i]));
+    }
+  }
+
+  void toJson(common::JsonMap& json) const {
+    std::vector<int> ids;
+    std::vector<std::string> names;
+
+    for (const auto& info : fsm_ids) {
+      ids.push_back(info.id);
+      names.push_back(info.name);
+    }
+
+    common::ToJson(ids, json["ids"]);
+    common::ToJson(names, json["names"]);
+  }
+
+  std::vector<FsmIdInfo> fsm_ids;
+};
+
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree
+
+#endif // __UT_ROBOT_H2_LOCO_API_HPP__

unitree_sdk2/include/unitree/robot/h2/loco/h2_loco_client.hpp

@@ -0,0 +1,315 @@
+#ifndef __UT_ROBOT_H2_LOCO_CLIENT_HPP__
+#define __UT_ROBOT_H2_LOCO_CLIENT_HPP__
+
+#include "h2_loco_api.hpp"
+#include <limits>
+#include <unitree/robot/client/client.hpp>
+#include <unitree/robot/go2/public/jsonize_type.hpp>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+class LocoClient : public Client {
+ public:
+  LocoClient() : Client(LOCO_SERVICE_NAME, false) {}
+  ~LocoClient() {}
+
+  /*Init*/
+  void Init() {
+    SetApiVersion(LOCO_API_VERSION);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_FSM_ID);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_FSM_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_BALANCE_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_PHASE);  // deprecated
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_ARM_SDK_STATUS);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_AVAILABLE_FSM_IDS);
+
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_FSM_ID);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_BALANCE_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_VELOCITY);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_ARM_TASK);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_SPEED_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_PUNCH_API);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_ARM_SDK_STATUS);
+  };
+
+  /*Low Level API Call*/
+  int32_t GetFsmId(int& fsm_id) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_FSM_ID, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataInt json;
+      common::FromJsonString(data, json);
+      fsm_id = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetFsmMode(int& fsm_mode) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_FSM_MODE, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataInt json;
+      common::FromJsonString(data, json);
+      fsm_mode = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetBalanceMode(int& balance_mode) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataInt json;
+      common::FromJsonString(data, json);
+      balance_mode = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetSwingHeight(float& swing_height) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataFloat json;
+      common::FromJsonString(data, json);
+      swing_height = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetStandHeight(float& stand_height) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataFloat json;
+      common::FromJsonString(data, json);
+      stand_height = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetPhase(std::vector<float>& phase) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_PHASE, parameter, data);
+
+    if (ret == 0) {
+      JsonizeDataVecFloat json;
+      common::FromJsonString(data, json);
+      phase = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetArmSdkStatus(bool& arm_sdk_status) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_ARM_SDK_STATUS, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataBool json;
+      common::FromJsonString(data, json);
+      arm_sdk_status = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetAvailableFsmIds(std::vector<int>& ids, std::vector<std::string>& names) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_AVAILABLE_FSM_IDS, parameter, data);
+
+    if (ret == 0) {
+      JsonizeFsmIdList json;
+      common::FromJsonString(data, json);
+      ids.clear();
+      names.clear();
+      for (const auto& info : json.fsm_ids) {
+        ids.push_back(info.id);
+        names.push_back(info.name);
+      }
+    }
+
+    return ret;
+  }
+
+  int32_t SetFsmId(int fsm_id) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = fsm_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter, data);
+  }
+
+  int32_t SetBalanceMode(int balance_mode) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = balance_mode;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, parameter, data);
+  }
+
+  int32_t SetPunchApi(std::vector<float>& punch_api) {
+    std::string parameter, data;
+
+    // go2::JsonizeDataInt json;
+    JsonizeDataVecFloat json;
+    json.data = punch_api;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_PUNCH_API, parameter, data);
+  }
+
+  int32_t SetSwingHeight(float swing_height) {
+    std::string parameter, data;
+
+    go2::JsonizeDataFloat json;
+    json.data = swing_height;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, parameter, data);
+  }
+
+  int32_t SetStandHeight(float stand_height) {
+    std::string parameter, data;
+
+    go2::JsonizeDataFloat json;
+    json.data = stand_height;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter, data);
+  }
+
+  int32_t SetVelocity(float vx, float vy, float omega, float duration = 1.f) {
+    std::string parameter, data;
+
+    JsonizeVelocityCommand json;
+    std::vector<float> velocity = {vx, vy, omega};
+    json.velocity = velocity;
+    json.duration = duration;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter, data);
+  }
+
+  int32_t SetTaskId(int task_id) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = task_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter, data);
+  }
+
+  int32_t SetArmSdkStatus(bool arm_sdk_status) {
+    std::string parameter, data;
+
+    go2::JsonizeDataBool json;
+    json.data = arm_sdk_status;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_ARM_SDK_STATUS, parameter, data);
+  }
+
+  /*High Level API Call*/
+  int32_t Damp() { return SetFsmId(1); }
+
+  int32_t Start() { return SetFsmId(601); }
+
+  int32_t Squat() { return SetFsmId(2); }
+
+  int32_t Sit() { return SetFsmId(3); }
+
+  int32_t StandUp() { return SetFsmId(4); }
+
+  int32_t ZeroTorque() { return SetFsmId(0); }
+
+  int32_t StopMove() { return SetVelocity(0.f, 0.f, 0.f); }
+
+  int32_t HighStand() { return SetStandHeight(std::numeric_limits<uint32_t>::max()); }
+
+  int32_t LowStand() { return SetStandHeight(std::numeric_limits<uint32_t>::min()); }
+
+  int32_t Move(float vx, float vy, float vyaw, bool continous_move) {
+    return SetVelocity(vx, vy, vyaw, continous_move ? 864000.f : 1.f);
+  }
+
+  int32_t Move(float vx, float vy, float vyaw) { return Move(vx, vy, vyaw, continous_move_); }
+
+  int32_t BalanceStand() { return SetBalanceMode(0); }
+
+  int32_t ContinuousGait(bool flag) { return SetBalanceMode(flag ? 1 : 0); }
+
+  int32_t SwitchMoveMode(bool flag) {
+    continous_move_ = flag;
+    return 0;
+  }
+
+  int32_t WaveHand(bool turn_flag = false) { return SetTaskId(turn_flag ? 1 : 0); }
+
+  int32_t ShakeHand(int stage = -1) {
+    switch (stage) {
+      case 0:
+        first_shake_hand_stage_ = false;
+        return SetTaskId(2);
+
+      case 1:
+        first_shake_hand_stage_ = true;
+        return SetTaskId(3);
+
+      default:
+        first_shake_hand_stage_ = !first_shake_hand_stage_;
+        return SetTaskId(first_shake_hand_stage_ ? 3 : 2);
+    }
+  }
+
+  int32_t SetSpeedMode(int speed_mode) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = speed_mode;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_SPEED_MODE, parameter, data);
+  }
+
+  int32_t EnableArmSDK() { return SetArmSdkStatus(true); }
+
+  int32_t DisableArmSDK() { return SetArmSdkStatus(false); }
+
+ private:
+  bool continous_move_ = false;
+  bool first_shake_hand_stage_ = true;
+};
+}  // namespace h2
+
+}  // namespace robot
+}  // namespace unitree
+#endif // __UT_ROBOT_H2_LOCO_CLIENT_HPP__

unitree_sdk2/include/unitree/robot/h2/loco/h2_loco_error.hpp

@@ -0,0 +1,18 @@
+#ifndef __UT_ROBOT_H2_LOCO_ERROR_HPP__
+#define __UT_ROBOT_H2_LOCO_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_LOCOSTATE_NOT_AVAILABLE, 7301, "LocoState not available.")
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_SCHEDULER_STATE_NOT_AVAILABLE, 7301, "LocoState not available.")
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_FSM_ID, 7302, "Invalid fsm id.")
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_TASK_ID, 7303, "Invalid task id.")
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree
+
+#endif // __UT_ROBOT_H2_LOCO_ERROR_HPP__
+