feat: added torque and imu measurements in compacted utils headers

example/g1/low_level/utils/base_state.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include <array>
+
+struct BaseState {
+  std::array<float, 3> rpy = {};
+  std::array<float, 3> omega = {};
+  std::array<float, 4> quat = {};
+  std::array<float, 3> acc = {};
+};

example/g1/low_level/utils/data_buffer.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+
+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;
+};

example/g1/low_level/utils/motors.hpp

@@ -0,0 +1,83 @@
+#pragma once
+
+#include <array>
+#include <stdint.h>
+
+// #include <unitree/idl/go2/LowCmd_.hpp>
+
+constexpr int kNumMotors = 29;
+
+struct MotorCommand {
+  std::array<float, kNumMotors> q_ref = {};
+  std::array<float, kNumMotors> dq_ref = {};
+  std::array<float, kNumMotors> kp = {};
+  std::array<float, kNumMotors> kd = {};
+  std::array<float, kNumMotors> tau_ff = {};
+};
+
+struct MotorState {
+  std::array<float, kNumMotors> q = {};
+  std::array<float, kNumMotors> dq = {};
+  std::array<float, kNumMotors> tau = {};
+};
+
+enum JointIndex {
+  LeftHipPitch = 0,
+  LeftHipRoll = 1,
+  LeftHipYaw = 2,
+  LeftKnee = 3,
+  LeftAnklePitch = 4,
+  LeftAnkleB = 4,
+  LeftAnkleRoll = 5,
+  LeftAnkleA = 5,
+  RightHipPitch = 6,
+  RightHipRoll = 7,
+  RightHipYaw = 8,
+  RightKnee = 9,
+  RightAnklePitch = 10,
+  RightAnkleB = 10,
+  RightAnkleRoll = 11,
+  RightAnkleA = 11,
+  WaistYaw = 12,
+  WaistRoll = 13,        // NOTE INVALID for g1 23dof/29dof with waist locked
+  WaistA = 13,           // NOTE INVALID for g1 23dof/29dof with waist locked
+  WaistPitch = 14,       // NOTE INVALID for g1 23dof/29dof with waist locked
+  WaistB = 14,           // NOTE INVALID for g1 23dof/29dof with waist locked
+  LeftShoulderPitch = 15,
+  LeftShoulderRoll = 16,
+  LeftShoulderYaw = 17,
+  LeftElbow = 18,
+  LeftWristRoll = 19,
+  LeftWristPitch = 20,   // NOTE INVALID for g1 23dof
+  LeftWristYaw = 21,     // NOTE INVALID for g1 23dof
+  RightShoulderPitch = 22,
+  RightShoulderRoll = 23,
+  RightShoulderYaw = 24,
+  RightElbow = 25,
+  RightWristRoll = 26,
+  RightWristPitch = 27,  // NOTE INVALID for g1 23dof
+  RightWristYaw = 28     // NOTE INVALID for g1 23dof
+};
+
+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;
+};