76 lines
1.5 KiB
C++
76 lines
1.5 KiB
C++
|
#pragma once
|
||
|
|
|
||
|
|
#include <array>
|
||
|
|
#include <stdint.h>
|
||
|
|
|
||
|
|
#include <unitree/idl/go2/LowCmd_.hpp>
|
||
|
|
|
||
|
|
constexpr int kNumMotors = 20;
|
||
|
|
|
||
|
|
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 = {};
|
||
|
|
};
|
||
|
|
|
||
|
|
enum JointIndex {
|
||
|
|
// Right leg
|
||
|
|
kRightHipYaw = 8,
|
||
|
|
kRightHipRoll = 0,
|
||
|
|
kRightHipPitch = 1,
|
||
|
|
kRightKnee = 2,
|
||
|
|
kRightAnkle = 11,
|
||
|
|
// Left leg
|
||
|
|
kLeftHipYaw = 7,
|
||
|
|
kLeftHipRoll = 3,
|
||
|
|
kLeftHipPitch = 4,
|
||
|
|
kLeftKnee = 5,
|
||
|
|
kLeftAnkle = 10,
|
||
|
|
|
||
|
|
kWaistYaw = 6,
|
||
|
|
|
||
|
|
kNotUsedJoint = 9,
|
||
|
|
|
||
|
|
// Right arm
|
||
|
|
kRightShoulderPitch = 12,
|
||
|
|
kRightShoulderRoll = 13,
|
||
|
|
kRightShoulderYaw = 14,
|
||
|
|
kRightElbow = 15,
|
||
|
|
// Left arm
|
||
|
|
kLeftShoulderPitch = 16,
|
||
|
|
kLeftShoulderRoll = 17,
|
||
|
|
kLeftShoulderYaw = 18,
|
||
|
|
kLeftElbow = 19,
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
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;
|
||
|
|
};
|