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

unitree_sdk2/include/unitree/robot/g1/loco/g1_loco_api.hpp

@@ -0,0 +1,80 @@
+#ifndef __UT_ROBOT_G1_LOCO_API_HPP__
+#define __UT_ROBOT_G1_LOCO_API_HPP__
+
+#include <unitree/common/json/jsonize.hpp>
+#include <variant>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+/*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_FSM_API = 7100;
+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_SWITCH_TO_USER_CTRL = 7110;
+const int32_t ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL = 7111;
+
+
+using LocoCmd =
+    std::map<std::string, std::variant<int, float, std::vector<float>>>;
+
+enum class InternalFsmMode{
+  LAST, PASSIVE, WALKRUN
+};
+
+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;
+};
+
+} // namespace g1
+} // namespace robot
+} // namespace unitree
+
+#endif // __UT_ROBOT_G1_LOCO_API_HPP__

unitree_sdk2/include/unitree/robot/g1/loco/g1_loco_client.hpp

@@ -0,0 +1,315 @@
+#ifndef __UT_ROBOT_G1_LOCO_CLIENT_HPP__
+#define __UT_ROBOT_G1_LOCO_CLIENT_HPP__
+
+#include "g1_loco_api.hpp"
+#include <limits>
+#include <unitree/robot/client/client.hpp>
+#include <unitree/robot/go2/public/jsonize_type.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+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_FSM_API);
+    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_SWITCH_TO_USER_CTRL);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL);
+  };
+
+  /*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 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 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 SwitchToUserCtrl() {
+    std::string parameter, data;
+
+    go2::JsonizeDataBool json;
+    parameter = common::ToJsonString(json);
+    return Call(ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL, parameter, data);
+  }
+
+  int32_t SwitchToInternalCtrl(InternalFsmMode mode) {
+    std::string parameter, data;
+    go2::JsonizeDataInt json;
+    switch (mode)
+    {
+    case InternalFsmMode::LAST:
+      json.data = 0;
+      break;
+    case InternalFsmMode::PASSIVE:
+      json.data = 1;
+      break;
+    case InternalFsmMode::WALKRUN:
+      json.data = 2;
+      break;
+    }
+    parameter = common::ToJsonString(json);
+    return Call(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL, parameter, data);
+  }
+
+  /*High Level API Call*/
+  int32_t Damp() { return SetFsmId(1); }
+
+  int32_t Start() { return SetFsmId(500); }
+
+  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);
+  }
+
+  /**
+   * @brief Get available mimic motions from the robot.
+   *
+   * @param[out] data Response data containing available motions.
+   *
+   * @return Response data format:
+   * {
+   *   "fsm_id": 550,
+   *   "api_id": 1,
+   *   "motion": {
+   *     "507": { # motion_api_id
+   *       "duration": float,
+   *       "name": string
+   *     },
+   *     ...
+   *   }
+   * }
+   *
+   * @note Use set_fsm_id to invoke specific motion_api_id with prefix 550.
+   *       Example: --set_fsm_id 550507
+   * 
+   * @attention The Robot must be in a stable state (e.g., standing);
+   */
+  int32_t GetMimicMotion(std::string& data) {
+    std::string parameter = "{\"fsm_id\": 550, \"api_id\": 1}";
+    return _fsm_api(parameter, data);
+  }
+
+  int32_t _fsm_api(std::string parameter, std::string& data) {
+    return Call(ROBOT_API_ID_LOCO_FSM_API, parameter, data);
+  }
+
+private:
+  bool continous_move_ = false;
+  bool first_shake_hand_stage_ = true;
+};
+} // namespace g1
+
+} // namespace robot
+} // namespace unitree
+#endif // __UT_ROBOT_G1_LOCO_CLIENT_HPP__

unitree_sdk2/include/unitree/robot/g1/loco/g1_loco_error.hpp

@@ -0,0 +1,17 @@
+#ifndef __UT_ROBOT_G1_LOCO_ERROR_HPP__
+#define __UT_ROBOT_G1_LOCO_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_LOCOSTATE_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 g1
+} // namespace robot
+} // namespace unitree
+
+#endif // __UT_ROBOT_G1_LOCO_ERROR_HPP__