cpp对齐官方

include/unitree_legged_sdk/a1_const.h

@@ -0,0 +1,11 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_A1_CONST_H_
+#define GO1_PRO_UNITREE_LEGGED_A1_CONST_H_
+namespace UNITREE_LEGGED_SDK {
+constexpr double a1_Hip_max = 0.802;
+constexpr double a1_Hip_min = -0.802;
+constexpr double a1_Thigh_max = 4.19;
+constexpr double a1_Thigh_min = -1.05;
+constexpr double a1_Calf_max = -0.916;
+constexpr double a1_Calf_min = -2.7;
+}  // namespace UNITREE_LEGGED_SDK
+#endif

include/unitree_legged_sdk/aliengo_const.h

@@ -0,0 +1,11 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_ALIENGO_CONST_H_
+#define GO1_PRO_UNITREE_LEGGED_ALIENGO_CONST_H_
+namespace UNITREE_LEGGED_SDK {
+constexpr double aliengo_Hip_max = 1.047;
+constexpr double aliengo_Hip_min = -0.873;
+constexpr double aliengo_Thigh_max = 3.927;
+constexpr double aliengo_Thigh_min = -0.524;
+constexpr double aliengo_Calf_max = -0.611;
+constexpr double aliengo_Calf_min = -2.775;
+}  // namespace UNITREE_LEGGED_SDK
+#endif

include/unitree_legged_sdk/b1_const.h

@@ -0,0 +1,11 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_B1_CONST_H_
+#define GO1_PRO_UNITREE_LEGGED_B1_CONST_H_
+namespace UNITREE_LEGGED_SDK {
+constexpr double b1_Hip_max = 0.75;
+constexpr double b1_Hip_min = -0.75;
+constexpr double b1_Thigh_max = 3.5;
+constexpr double b1_Thigh_min = -1.0;
+constexpr double b1_Calf_max = -0.6;
+constexpr double b1_Calf_min = -2.6;
+}  // namespace UNITREE_LEGGED_SDK
+#endif

include/unitree_legged_sdk/comm.h

@@ -0,0 +1,179 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_COMM_H_
+#define GO1_PRO_UNITREE_LEGGED_COMM_H_
+
+#include <array>
+#include <cstdint>
+
+namespace UNITREE_LEGGED_SDK {
+
+constexpr int HIGHLEVEL = 0xee;
+constexpr int LOWLEVEL = 0xff;
+constexpr int TRIGERLEVEL = 0xf0;
+constexpr double PosStopF = 2.146E+9f;
+constexpr double VelStopF = 16000.0f;
+
+extern const int HIGH_CMD_LENGTH;
+extern const int HIGH_STATE_LENGTH;
+extern const int LOW_CMD_LENGTH;
+extern const int LOW_STATE_LENGTH;
+
+#pragma pack(push, 1)
+
+struct BmsCmd {
+  uint8_t off;
+  std::array<uint8_t, 3> reserve;
+};
+
+struct BmsState {
+  uint8_t version_h;
+  uint8_t version_l;
+  uint8_t bms_status;
+  uint8_t SOC;
+  int32_t current;
+  uint16_t cycle;
+  std::array<int8_t, 2> BQ_NTC;
+  std::array<int8_t, 2> MCU_NTC;
+  std::array<uint16_t, 10> cell_vol;
+};
+
+struct Cartesian {
+  float x;
+  float y;
+  float z;
+};
+
+struct IMU {
+  std::array<float, 4> quaternion;
+  std::array<float, 3> gyroscope;
+  std::array<float, 3> accelerometer;
+  std::array<float, 3> rpy;
+  int8_t temperature;
+};
+
+struct LED {
+  uint8_t r;
+  uint8_t g;
+  uint8_t b;
+};
+
+struct MotorState {
+  uint8_t mode;
+  float q;
+  float dq;
+  float ddq;
+  float tauEst;
+  float q_raw;
+  float dq_raw;
+  float ddq_raw;
+  int8_t temperature;
+  std::array<uint32_t, 2> reserve;
+};
+
+struct MotorCmd {
+  uint8_t mode;
+  float q;
+  float dq;
+  float tau;
+  float Kp;
+  float Kd;
+  std::array<uint32_t, 3> reserve;
+};
+
+struct LowState {
+  std::array<uint8_t, 2> head;
+  uint8_t levelFlag;
+  uint8_t frameReserve;
+  std::array<uint32_t, 2> SN;
+  std::array<uint32_t, 2> version;
+  uint16_t bandWidth;
+  IMU imu;
+  std::array<MotorState, 20> motorState;
+  BmsState bms;
+  std::array<int16_t, 4> footForce;
+  std::array<int16_t, 4> footForceEst;
+  uint32_t tick;
+  std::array<uint8_t, 40> wirelessRemote;
+  uint32_t reserve;
+  uint32_t crc;
+};
+
+struct LowCmd {
+  std::array<uint8_t, 2> head;
+  uint8_t levelFlag;
+  uint8_t frameReserve;
+  std::array<uint32_t, 2> SN;
+  std::array<uint32_t, 2> version;
+  uint16_t bandWidth;
+  std::array<MotorCmd, 20> motorCmd;
+  BmsCmd bms;
+  std::array<uint8_t, 40> wirelessRemote;
+  uint32_t reserve;
+  uint32_t crc;
+};
+
+struct HighState {
+  std::array<uint8_t, 2> head;
+  uint8_t levelFlag;
+  uint8_t frameReserve;
+  std::array<uint32_t, 2> SN;
+  std::array<uint32_t, 2> version;
+  uint16_t bandWidth;
+  IMU imu;
+  std::array<MotorState, 20> motorState;
+  BmsState bms;
+  std::array<int16_t, 4> footForce;
+  std::array<int16_t, 4> footForceEst;
+  uint8_t mode;
+  float progress;
+  uint8_t gaitType;
+  float footRaiseHeight;
+  std::array<float, 3> position;
+  float bodyHeight;
+  std::array<float, 3> velocity;
+  float yawSpeed;
+  std::array<float, 4> rangeObstacle;
+  std::array<Cartesian, 4> footPosition2Body;
+  std::array<Cartesian, 4> footSpeed2Body;
+  std::array<uint8_t, 40> wirelessRemote;
+  uint32_t reserve;
+  uint32_t crc;
+};
+
+struct HighCmd {
+  std::array<uint8_t, 2> head;
+  uint8_t levelFlag;
+  uint8_t frameReserve;
+  std::array<uint32_t, 2> SN;
+  std::array<uint32_t, 2> version;
+  uint16_t bandWidth;
+  uint8_t mode;
+  uint8_t gaitType;
+  uint8_t speedLevel;
+  float footRaiseHeight;
+  float bodyHeight;
+  std::array<float, 2> position;
+  std::array<float, 3> euler;
+  std::array<float, 2> velocity;
+  float yawSpeed;
+  BmsCmd bms;
+  std::array<LED, 4> led;
+  std::array<uint8_t, 40> wirelessRemote;
+  uint32_t reserve;
+  uint32_t crc;
+};
+
+#pragma pack(pop)
+
+struct UDPState {
+  unsigned long long TotalCount;
+  unsigned long long SendCount;
+  unsigned long long RecvCount;
+  unsigned long long SendError;
+  unsigned long long FlagError;
+  unsigned long long RecvCRCError;
+  unsigned long long RecvLoseError;
+};
+
+}  // namespace UNITREE_LEGGED_SDK
+
+#endif

include/unitree_legged_sdk/go1_const.h

@@ -0,0 +1,11 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_GO1_CONST_H_
+#define GO1_PRO_UNITREE_LEGGED_GO1_CONST_H_
+namespace UNITREE_LEGGED_SDK {
+constexpr double go1_Hip_max = 1.047;
+constexpr double go1_Hip_min = -1.047;
+constexpr double go1_Thigh_max = 2.966;
+constexpr double go1_Thigh_min = -0.663;
+constexpr double go1_Calf_max = -0.837;
+constexpr double go1_Calf_min = -2.721;
+}  // namespace UNITREE_LEGGED_SDK
+#endif

include/unitree_legged_sdk/joystick.h

@@ -0,0 +1,27 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_JOYSTICK_H_
+#define GO1_PRO_UNITREE_LEGGED_JOYSTICK_H_
+
+#include <cstdint>
+
+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;
+
+typedef struct {
+  uint8_t head[2];
+  xKeySwitchUnion btn;
+  float lx;
+  float rx;
+  float ry;
+  float L2;
+  float ly;
+  uint8_t idle[16];
+} xRockerBtnDataStruct;
+
+#endif

include/unitree_legged_sdk/loop.h

@@ -0,0 +1,48 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_LOOP_H_
+#define GO1_PRO_UNITREE_LEGGED_LOOP_H_
+
+#include <atomic>
+#include <boost/function.hpp>
+#include <string>
+#include <thread>
+
+namespace UNITREE_LEGGED_SDK {
+
+constexpr int THREAD_PRIORITY = 95;
+typedef boost::function<void()> Callback;
+
+class Loop {
+ public:
+  Loop(std::string name, float period, int bindCPU = -1)
+      : _name(name), _period(period), _bindCPU(bindCPU) {}
+  virtual ~Loop();
+  void start();
+  void shutdown();
+  virtual void functionCB() = 0;
+
+ private:
+  void entryFunc();
+  std::string _name;
+  float _period;
+  int _bindCPU;
+  bool _bind_cpu_flag = false;
+  bool _isrunning = false;
+  std::atomic<bool> _running_atomic{false};
+  std::thread _thread;
+};
+
+class LoopFunc : public Loop {
+ public:
+  LoopFunc(std::string name, float period, const Callback& cb)
+      : Loop(name, period), _fp(cb) {}
+  LoopFunc(std::string name, float period, int bindCPU, const Callback& cb)
+      : Loop(name, period, bindCPU), _fp(cb) {}
+  void functionCB() override { _fp(); }
+
+ private:
+  boost::function<void()> _fp;
+};
+
+}  // namespace UNITREE_LEGGED_SDK
+
+#endif

include/unitree_legged_sdk/quadruped.h

@@ -0,0 +1,33 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_QUADRUPED_H_
+#define GO1_PRO_UNITREE_LEGGED_QUADRUPED_H_
+
+#include <string>
+
+namespace UNITREE_LEGGED_SDK {
+
+enum class LeggedType { Aliengo, A1, Go1, B1 };
+
+std::string VersionSDK();
+int InitEnvironment();
+
+constexpr int FR_ = 0;
+constexpr int FL_ = 1;
+constexpr int RR_ = 2;
+constexpr int RL_ = 3;
+
+constexpr int FR_0 = 0;
+constexpr int FR_1 = 1;
+constexpr int FR_2 = 2;
+constexpr int FL_0 = 3;
+constexpr int FL_1 = 4;
+constexpr int FL_2 = 5;
+constexpr int RR_0 = 6;
+constexpr int RR_1 = 7;
+constexpr int RR_2 = 8;
+constexpr int RL_0 = 9;
+constexpr int RL_1 = 10;
+constexpr int RL_2 = 11;
+
+}  // namespace UNITREE_LEGGED_SDK
+
+#endif

include/unitree_legged_sdk/safety.h

@@ -0,0 +1,30 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_SAFETY_H_
+#define GO1_PRO_UNITREE_LEGGED_SAFETY_H_
+
+#include "comm.h"
+#include "quadruped.h"
+
+namespace UNITREE_LEGGED_SDK {
+
+class Safety {
+ public:
+  Safety(LeggedType type);
+  ~Safety();
+  void PositionLimit(LowCmd& cmd);
+  int PowerProtect(LowCmd& cmd, LowState& state, int factor);
+  int PositionProtect(LowCmd& cmd, LowState& state, double limit = 0.087);
+
+ private:
+  int WattLimit;
+  int Wcount;
+  double Hip_max;
+  double Hip_min;
+  double Thigh_max;
+  double Thigh_min;
+  double Calf_max;
+  double Calf_min;
+};
+
+}  // namespace UNITREE_LEGGED_SDK
+
+#endif

include/unitree_legged_sdk/udp.h

@@ -0,0 +1,62 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_UDP_H_
+#define GO1_PRO_UNITREE_LEGGED_UDP_H_
+
+#include "comm.h"
+#include "unitree_legged_sdk/quadruped.h"
+#include <cstdint>
+
+namespace UNITREE_LEGGED_SDK {
+
+constexpr int UDP_CLIENT_PORT = 8080;
+constexpr int UDP_SERVER_PORT = 8007;
+constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10";
+constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161";
+
+enum RecvEnum { nonBlock = 0x00, block = 0x01, blockTimeout = 0x02 };
+
+class UDP {
+ public:
+  UDP(uint8_t level, uint16_t localPort, const char* targetIP, uint16_t targetPort);
+  UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort,
+      int sendLength, int recvLength, bool initiativeDisconnect = false,
+      RecvEnum recvType = RecvEnum::nonBlock);
+  UDP(uint16_t localPort, int sendLength, int recvLength,
+      bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock,
+      bool setIpPort = false);
+  ~UDP();
+
+  UDP(const UDP&) = delete;
+  UDP& operator=(const UDP&) = delete;
+
+  void SetIpPort(const char* targetIP, uint16_t targetPort);
+  void SetRecvTimeout(int time);
+  void SetDisconnectTime(float callback_dt, float disconnectTime);
+  void SetAccessibleTime(float callback_dt, float accessibleTime);
+
+  int Send();
+  int Recv();
+
+  void InitCmdData(HighCmd& cmd);
+  void InitCmdData(LowCmd& cmd);
+  int SetSend(char* data);
+  int SetSend(HighCmd& cmd);
+  int SetSend(LowCmd& cmd);
+  void GetRecv(char* data);
+  void GetRecv(HighState& state);
+  void GetRecv(LowState& state);
+
+  UDPState udpState;
+  char* targetIP;
+  uint16_t targetPort;
+  char* localIP;
+  uint16_t localPort;
+  bool accessible = false;
+
+ private:
+  struct Impl;
+  Impl* impl_;
+};
+
+}  // namespace UNITREE_LEGGED_SDK
+
+#endif

include/unitree_legged_sdk/unitree_legged_sdk.h

@@ -0,0 +1,14 @@
+#ifndef GO1_PRO_UNITREE_LEGGED_SDK_H_
+#define GO1_PRO_UNITREE_LEGGED_SDK_H_
+
+#include "comm.h"
+#include "joystick.h"
+#include "loop.h"
+#include "quadruped.h"
+#include "safety.h"
+#include "udp.h"
+#include <boost/bind.hpp>
+
+#define UT UNITREE_LEGGED_SDK
+
+#endif