C++版本
This commit is contained in:
cyy_mac
717
fast_lowcmd_cpp/go1_fast_lowcmd.cpp
Normal file
717
fast_lowcmd_cpp/go1_fast_lowcmd.cpp
Normal file
@@ -0,0 +1,717 @@
|
||||
#define PY_SSIZE_T_CLEAN
|
||||
#include <Python.h>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr Py_ssize_t LOWCMD_SIZE = 616;
|
||||
constexpr Py_ssize_t CRC_OFFSET = 612;
|
||||
constexpr Py_ssize_t MOTOR_OFFSET = 22;
|
||||
constexpr Py_ssize_t MOTOR_SIZE = 27;
|
||||
constexpr int MOTOR_COUNT = 20;
|
||||
constexpr int SERVO_COUNT = 12;
|
||||
constexpr uint32_t CRC_POLY = 0x04c11db7u;
|
||||
|
||||
std::array<uint32_t, 256> make_crc_table() {
|
||||
std::array<uint32_t, 256> table{};
|
||||
for (uint32_t byte = 0; byte < 256; ++byte) {
|
||||
uint32_t crc = byte << 24;
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (crc & 0x80000000u) {
|
||||
crc = (crc << 1) ^ CRC_POLY;
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
table[byte] = crc;
|
||||
}
|
||||
return table;
|
||||
}
|
||||
|
||||
const std::array<uint32_t, 256> CRC_TABLE = make_crc_table();
|
||||
|
||||
inline void put_u16_le(uint8_t* p, uint16_t v) {
|
||||
p[0] = static_cast<uint8_t>(v & 0xffu);
|
||||
p[1] = static_cast<uint8_t>((v >> 8) & 0xffu);
|
||||
}
|
||||
|
||||
inline void put_u32_le(uint8_t* p, uint32_t v) {
|
||||
p[0] = static_cast<uint8_t>(v & 0xffu);
|
||||
p[1] = static_cast<uint8_t>((v >> 8) & 0xffu);
|
||||
p[2] = static_cast<uint8_t>((v >> 16) & 0xffu);
|
||||
p[3] = static_cast<uint8_t>((v >> 24) & 0xffu);
|
||||
}
|
||||
|
||||
inline uint32_t get_u32_le(const uint8_t* p) {
|
||||
return (static_cast<uint32_t>(p[0]))
|
||||
| (static_cast<uint32_t>(p[1]) << 8)
|
||||
| (static_cast<uint32_t>(p[2]) << 16)
|
||||
| (static_cast<uint32_t>(p[3]) << 24);
|
||||
}
|
||||
|
||||
inline void put_f32_le(uint8_t* p, double value) {
|
||||
float f = static_cast<float>(value);
|
||||
uint32_t raw;
|
||||
static_assert(sizeof(raw) == sizeof(f), "float must be 32-bit");
|
||||
std::memcpy(&raw, &f, sizeof(raw));
|
||||
put_u32_le(p, raw);
|
||||
}
|
||||
|
||||
uint32_t crc32_unitree(const uint8_t* data, Py_ssize_t len) {
|
||||
uint32_t crc = 0xffffffffu;
|
||||
Py_ssize_t words = len / 4;
|
||||
for (Py_ssize_t i = 0; i < words; ++i) {
|
||||
uint32_t word = get_u32_le(data + i * 4);
|
||||
crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ ((word >> 24) & 0xffu)) & 0xffu];
|
||||
crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ ((word >> 16) & 0xffu)) & 0xffu];
|
||||
crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ ((word >> 8) & 0xffu)) & 0xffu];
|
||||
crc = (crc << 8) ^ CRC_TABLE[((crc >> 24) ^ (word & 0xffu)) & 0xffu];
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
double round_half_even(double value, double scale) {
|
||||
double scaled = value * scale;
|
||||
double floor_v = std::floor(scaled);
|
||||
double frac = scaled - floor_v;
|
||||
double rounded;
|
||||
constexpr double eps = 1e-12;
|
||||
if (frac > 0.5 + eps) {
|
||||
rounded = floor_v + 1.0;
|
||||
} else if (frac < 0.5 - eps) {
|
||||
rounded = floor_v;
|
||||
} else {
|
||||
rounded = (std::fmod(floor_v, 2.0) == 0.0) ? floor_v : floor_v + 1.0;
|
||||
}
|
||||
return rounded / scale;
|
||||
}
|
||||
|
||||
void encode_tau(uint8_t* p, double tau) {
|
||||
tau = round_half_even(tau, 100.0);
|
||||
int integer_part = static_cast<int>(tau);
|
||||
double fractional_part = tau - static_cast<double>(integer_part);
|
||||
bool neg = false;
|
||||
if (tau < 0.0) {
|
||||
neg = true;
|
||||
integer_part = 255 + integer_part;
|
||||
}
|
||||
if (fractional_part == 0.0) {
|
||||
neg = false;
|
||||
}
|
||||
int hex_value = static_cast<int>(fractional_part * 256.0);
|
||||
if (neg) {
|
||||
hex_value = 255 + hex_value + 1;
|
||||
}
|
||||
p[0] = static_cast<uint8_t>(hex_value & 0xff);
|
||||
p[1] = static_cast<uint8_t>(integer_part & 0xff);
|
||||
}
|
||||
|
||||
void encode_kp(uint8_t* p, double kp) {
|
||||
double base_d = std::floor(kp);
|
||||
int base = static_cast<int>(base_d);
|
||||
double frac_d = round_half_even(kp - base_d, 10.0);
|
||||
int frac = static_cast<int>(frac_d * 10.0);
|
||||
int val;
|
||||
if (frac < 5) {
|
||||
val = base * 32 + frac * 3;
|
||||
} else {
|
||||
val = base * 32 + (frac - 1) * 3 + 4;
|
||||
}
|
||||
put_u16_le(p, static_cast<uint16_t>(val));
|
||||
}
|
||||
|
||||
void encode_kd(uint8_t* p, double kd) {
|
||||
int integer_part = static_cast<int>(kd);
|
||||
double fractional_part = round_half_even(kd - integer_part, 10.0);
|
||||
int frac = static_cast<int>(fractional_part * 10.0);
|
||||
uint8_t nibble = 0;
|
||||
switch (frac) {
|
||||
case 0: nibble = 0x0; break;
|
||||
case 1: nibble = 0x1; break;
|
||||
case 2: nibble = 0x3; break;
|
||||
case 3: nibble = 0x4; break;
|
||||
case 4: nibble = 0x6; break;
|
||||
case 5: nibble = 0x8; break;
|
||||
case 6: nibble = 0x9; break;
|
||||
case 7: nibble = 0xb; break;
|
||||
case 8: nibble = 0xc; break;
|
||||
case 9: nibble = 0xe; break;
|
||||
default: nibble = 0x0; break;
|
||||
}
|
||||
put_u16_le(p, static_cast<uint16_t>((integer_part << 4) | nibble));
|
||||
}
|
||||
|
||||
void encode_motor(uint8_t* p, int mode, double q, double dq, double tau, double kp, double kd) {
|
||||
p[0] = static_cast<uint8_t>(mode & 0xff);
|
||||
put_f32_le(p + 1, q);
|
||||
put_f32_le(p + 5, dq);
|
||||
encode_tau(p + 9, tau);
|
||||
encode_kp(p + 11, kp);
|
||||
encode_kd(p + 13, kd);
|
||||
// reserve[0..2] are already zeroed by the packet initializer.
|
||||
}
|
||||
|
||||
void encode_motor_with_reserve(
|
||||
uint8_t* p, int mode, double q, double dq, double tau, double kp, double kd,
|
||||
uint32_t reserve0, uint32_t reserve1, uint32_t reserve2) {
|
||||
encode_motor(p, mode, q, dq, tau, kp, kd);
|
||||
put_u32_le(p + 15, reserve0);
|
||||
put_u32_le(p + 19, reserve1);
|
||||
put_u32_le(p + 23, reserve2);
|
||||
}
|
||||
|
||||
void init_lowcmd_header(uint8_t* cmd) {
|
||||
std::memset(cmd, 0, LOWCMD_SIZE);
|
||||
cmd[0] = 0xfe;
|
||||
cmd[1] = 0xef;
|
||||
cmd[2] = 0xff;
|
||||
cmd[20] = 0x3a;
|
||||
cmd[21] = 0xc0;
|
||||
}
|
||||
|
||||
void finish_crc(uint8_t* cmd) {
|
||||
uint32_t crc = crc32_unitree(cmd, CRC_OFFSET);
|
||||
put_u32_le(cmd + CRC_OFFSET, crc);
|
||||
}
|
||||
|
||||
bool read_double_sequence(PyObject* obj, double* out, int count, double default_value, const char* name) {
|
||||
for (int i = 0; i < count; ++i) {
|
||||
out[i] = default_value;
|
||||
}
|
||||
if (obj == nullptr || obj == Py_None) {
|
||||
return true;
|
||||
}
|
||||
if (PyNumber_Check(obj) && !PySequence_Check(obj)) {
|
||||
double v = PyFloat_AsDouble(obj);
|
||||
if (PyErr_Occurred()) {
|
||||
return false;
|
||||
}
|
||||
for (int i = 0; i < count; ++i) {
|
||||
out[i] = v;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
PyObject* seq = PySequence_Fast(obj, name);
|
||||
if (!seq) {
|
||||
return false;
|
||||
}
|
||||
Py_ssize_t n = PySequence_Fast_GET_SIZE(seq);
|
||||
if (n < count) {
|
||||
Py_DECREF(seq);
|
||||
PyErr_Format(PyExc_ValueError, "%s must contain at least %d values", name, count);
|
||||
return false;
|
||||
}
|
||||
PyObject** items = PySequence_Fast_ITEMS(seq);
|
||||
for (int i = 0; i < count; ++i) {
|
||||
out[i] = PyFloat_AsDouble(items[i]);
|
||||
if (PyErr_Occurred()) {
|
||||
Py_DECREF(seq);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
Py_DECREF(seq);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool read_int_sequence(PyObject* obj, int* out, int count, int default_value, const char* name) {
|
||||
for (int i = 0; i < count; ++i) {
|
||||
out[i] = default_value;
|
||||
}
|
||||
if (obj == nullptr || obj == Py_None) {
|
||||
return true;
|
||||
}
|
||||
if (PyLong_Check(obj)) {
|
||||
long v = PyLong_AsLong(obj);
|
||||
if (PyErr_Occurred()) {
|
||||
return false;
|
||||
}
|
||||
for (int i = 0; i < count; ++i) {
|
||||
out[i] = static_cast<int>(v);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
PyObject* seq = PySequence_Fast(obj, name);
|
||||
if (!seq) {
|
||||
return false;
|
||||
}
|
||||
Py_ssize_t n = PySequence_Fast_GET_SIZE(seq);
|
||||
if (n < count) {
|
||||
Py_DECREF(seq);
|
||||
PyErr_Format(PyExc_ValueError, "%s must contain at least %d values", name, count);
|
||||
return false;
|
||||
}
|
||||
PyObject** items = PySequence_Fast_ITEMS(seq);
|
||||
for (int i = 0; i < count; ++i) {
|
||||
long v = PyLong_AsLong(items[i]);
|
||||
if (PyErr_Occurred()) {
|
||||
Py_DECREF(seq);
|
||||
return false;
|
||||
}
|
||||
out[i] = static_cast<int>(v);
|
||||
}
|
||||
Py_DECREF(seq);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool get_int_attr(PyObject* obj, const char* name, int* out) {
|
||||
PyObject* value = PyObject_GetAttrString(obj, name);
|
||||
if (!value) {
|
||||
return false;
|
||||
}
|
||||
long v = PyLong_AsLong(value);
|
||||
Py_DECREF(value);
|
||||
if (PyErr_Occurred()) {
|
||||
return false;
|
||||
}
|
||||
*out = static_cast<int>(v);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool get_double_attr(PyObject* obj, const char* name, double* out) {
|
||||
PyObject* value = PyObject_GetAttrString(obj, name);
|
||||
if (!value) {
|
||||
return false;
|
||||
}
|
||||
double v = PyFloat_AsDouble(value);
|
||||
Py_DECREF(value);
|
||||
if (PyErr_Occurred()) {
|
||||
return false;
|
||||
}
|
||||
*out = v;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool copy_bytes_attr(PyObject* obj, const char* name, uint8_t* out, Py_ssize_t expected_len) {
|
||||
PyObject* value = PyObject_GetAttrString(obj, name);
|
||||
if (!value) {
|
||||
return false;
|
||||
}
|
||||
char* data = nullptr;
|
||||
Py_ssize_t len = 0;
|
||||
if (PyBytes_AsStringAndSize(value, &data, &len) < 0) {
|
||||
Py_DECREF(value);
|
||||
return false;
|
||||
}
|
||||
if (len != expected_len) {
|
||||
Py_DECREF(value);
|
||||
PyErr_Format(PyExc_ValueError, "%s must be %zd bytes", name, expected_len);
|
||||
return false;
|
||||
}
|
||||
std::memcpy(out, data, static_cast<size_t>(expected_len));
|
||||
Py_DECREF(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool read_reserve3(PyObject* motor, uint32_t reserve[3]) {
|
||||
reserve[0] = reserve[1] = reserve[2] = 0;
|
||||
PyObject* value = PyObject_GetAttrString(motor, "reserve");
|
||||
if (!value) {
|
||||
return false;
|
||||
}
|
||||
PyObject* seq = PySequence_Fast(value, "motor.reserve must be a sequence");
|
||||
Py_DECREF(value);
|
||||
if (!seq) {
|
||||
return false;
|
||||
}
|
||||
Py_ssize_t n = PySequence_Fast_GET_SIZE(seq);
|
||||
if (n < 3) {
|
||||
Py_DECREF(seq);
|
||||
PyErr_SetString(PyExc_ValueError, "motor.reserve must contain at least 3 values");
|
||||
return false;
|
||||
}
|
||||
PyObject** items = PySequence_Fast_ITEMS(seq);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
unsigned long v = PyLong_AsUnsignedLong(items[i]);
|
||||
if (PyErr_Occurred()) {
|
||||
Py_DECREF(seq);
|
||||
return false;
|
||||
}
|
||||
reserve[i] = static_cast<uint32_t>(v);
|
||||
}
|
||||
Py_DECREF(seq);
|
||||
return true;
|
||||
}
|
||||
|
||||
PyObject* build_plain_from_lowcmd_obj(PyObject* lowcmd) {
|
||||
PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
|
||||
if (!result) {
|
||||
return nullptr;
|
||||
}
|
||||
auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
|
||||
init_lowcmd_header(cmd);
|
||||
|
||||
int value = 0;
|
||||
if (!copy_bytes_attr(lowcmd, "head", cmd, 2)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
if (!get_int_attr(lowcmd, "levelFlag", &value)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
cmd[2] = static_cast<uint8_t>(value & 0xff);
|
||||
if (!get_int_attr(lowcmd, "frameReserve", &value)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
cmd[3] = static_cast<uint8_t>(value & 0xff);
|
||||
if (!copy_bytes_attr(lowcmd, "SN", cmd + 4, 8)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
if (!copy_bytes_attr(lowcmd, "version", cmd + 12, 8)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
if (!get_int_attr(lowcmd, "bandWidth", &value)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
cmd[20] = static_cast<uint8_t>((value >> 8) & 0xff);
|
||||
cmd[21] = static_cast<uint8_t>(value & 0xff);
|
||||
|
||||
PyObject* motors = PyObject_GetAttrString(lowcmd, "motorCmd");
|
||||
if (!motors) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* seq = PySequence_Fast(motors, "lowcmd.motorCmd must be a sequence");
|
||||
Py_DECREF(motors);
|
||||
if (!seq) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
if (PySequence_Fast_GET_SIZE(seq) < MOTOR_COUNT) {
|
||||
Py_DECREF(seq);
|
||||
Py_DECREF(result);
|
||||
PyErr_SetString(PyExc_ValueError, "lowcmd.motorCmd must contain at least 20 motors");
|
||||
return nullptr;
|
||||
}
|
||||
PyObject** items = PySequence_Fast_ITEMS(seq);
|
||||
for (int i = 0; i < MOTOR_COUNT; ++i) {
|
||||
PyObject* motor = items[i];
|
||||
int mode = 0;
|
||||
double q = 0.0, dq = 0.0, tau = 0.0, kp = 0.0, kd = 0.0;
|
||||
uint32_t reserve[3];
|
||||
if (!get_int_attr(motor, "mode", &mode)
|
||||
|| !get_double_attr(motor, "q", &q)
|
||||
|| !get_double_attr(motor, "dq", &dq)
|
||||
|| !get_double_attr(motor, "tau", &tau)
|
||||
|| !get_double_attr(motor, "Kp", &kp)
|
||||
|| !get_double_attr(motor, "Kd", &kd)
|
||||
|| !read_reserve3(motor, reserve)) {
|
||||
Py_DECREF(seq);
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
encode_motor_with_reserve(
|
||||
cmd + MOTOR_OFFSET + i * MOTOR_SIZE,
|
||||
mode, q, dq, tau, kp, kd,
|
||||
reserve[0], reserve[1], reserve[2]);
|
||||
}
|
||||
Py_DECREF(seq);
|
||||
|
||||
if (!copy_bytes_attr(lowcmd, "wirelessRemote", cmd + 566, 40)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
if (!copy_bytes_attr(lowcmd, "reserve", cmd + 606, 4)) {
|
||||
Py_DECREF(result);
|
||||
return nullptr;
|
||||
}
|
||||
finish_crc(cmd);
|
||||
return result;
|
||||
}
|
||||
|
||||
struct FastLowCmdBuilder {
|
||||
PyObject_HEAD
|
||||
uint32_t P[18];
|
||||
uint32_t S[4][256];
|
||||
};
|
||||
|
||||
uint32_t bf_f(FastLowCmdBuilder* self, uint32_t x) {
|
||||
return (((self->S[0][(x >> 24) & 0xffu] + self->S[1][(x >> 16) & 0xffu]) & 0xffffffffu)
|
||||
^ self->S[2][(x >> 8) & 0xffu]) + self->S[3][x & 0xffu];
|
||||
}
|
||||
|
||||
void encrypt_block_le(FastLowCmdBuilder* self, const uint8_t* in, uint8_t* out) {
|
||||
uint32_t L = get_u32_le(in);
|
||||
uint32_t R = get_u32_le(in + 4);
|
||||
for (int i = 0; i < 16; ++i) {
|
||||
L ^= self->P[i];
|
||||
R ^= bf_f(self, L);
|
||||
uint32_t tmp = L;
|
||||
L = R;
|
||||
R = tmp;
|
||||
}
|
||||
uint32_t tmp = L;
|
||||
L = R;
|
||||
R = tmp;
|
||||
R ^= self->P[16];
|
||||
L ^= self->P[17];
|
||||
put_u32_le(out, L);
|
||||
put_u32_le(out + 4, R);
|
||||
}
|
||||
|
||||
PyObject* encrypt_bytes(FastLowCmdBuilder* self, const uint8_t* data, Py_ssize_t len) {
|
||||
if (len % 8 != 0) {
|
||||
PyErr_SetString(PyExc_ValueError, "data length must be a multiple of 8");
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* result = PyBytes_FromStringAndSize(nullptr, len);
|
||||
if (!result) {
|
||||
return nullptr;
|
||||
}
|
||||
auto* out = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
|
||||
for (Py_ssize_t i = 0; i < len; i += 8) {
|
||||
encrypt_block_le(self, data + i, out + i);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int FastLowCmdBuilder_init(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
|
||||
const char* state_path = nullptr;
|
||||
static const char* kwlist[] = {"state_path", nullptr};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, kwargs, "s", const_cast<char**>(kwlist), &state_path)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::ifstream f(state_path, std::ios::binary);
|
||||
if (!f) {
|
||||
PyErr_Format(PyExc_FileNotFoundError, "cannot open Blowfish state file: %s", state_path);
|
||||
return -1;
|
||||
}
|
||||
std::array<uint8_t, 4168> buf{};
|
||||
f.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(buf.size()));
|
||||
if (f.gcount() < static_cast<std::streamsize>(buf.size())) {
|
||||
PyErr_Format(PyExc_ValueError, "Blowfish state file must contain at least 4168 bytes: %s", state_path);
|
||||
return -1;
|
||||
}
|
||||
const uint8_t* p = buf.data();
|
||||
for (int i = 0; i < 18; ++i) {
|
||||
self->P[i] = get_u32_le(p + i * 4);
|
||||
}
|
||||
p += 18 * 4;
|
||||
for (int s = 0; s < 4; ++s) {
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
self->S[s][i] = get_u32_le(p + (s * 256 + i) * 4);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_plain_damping(FastLowCmdBuilder*, PyObject*) {
|
||||
PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
|
||||
if (!result) {
|
||||
return nullptr;
|
||||
}
|
||||
auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
|
||||
init_lowcmd_header(cmd);
|
||||
finish_crc(cmd);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_encrypted_damping(FastLowCmdBuilder* self, PyObject*) {
|
||||
uint8_t cmd[LOWCMD_SIZE];
|
||||
init_lowcmd_header(cmd);
|
||||
finish_crc(cmd);
|
||||
return encrypt_bytes(self, cmd, LOWCMD_SIZE);
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_plain_servo12(FastLowCmdBuilder*, PyObject* args, PyObject* kwargs) {
|
||||
PyObject* q_obj = nullptr;
|
||||
PyObject* dq_obj = nullptr;
|
||||
PyObject* tau_obj = nullptr;
|
||||
PyObject* kp_obj = nullptr;
|
||||
PyObject* kd_obj = nullptr;
|
||||
static const char* kwlist[] = {"q", "dq", "tau", "kp", "kd", nullptr};
|
||||
if (!PyArg_ParseTupleAndKeywords(
|
||||
args, kwargs, "O|OOOO", const_cast<char**>(kwlist),
|
||||
&q_obj, &dq_obj, &tau_obj, &kp_obj, &kd_obj)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
double q[SERVO_COUNT], dq[SERVO_COUNT], tau[SERVO_COUNT], kp[SERVO_COUNT], kd[SERVO_COUNT];
|
||||
if (!read_double_sequence(q_obj, q, SERVO_COUNT, 0.0, "q")) return nullptr;
|
||||
if (!read_double_sequence(dq_obj, dq, SERVO_COUNT, 0.0, "dq")) return nullptr;
|
||||
if (!read_double_sequence(tau_obj, tau, SERVO_COUNT, 0.0, "tau")) return nullptr;
|
||||
if (!read_double_sequence(kp_obj, kp, SERVO_COUNT, 0.0, "kp")) return nullptr;
|
||||
if (!read_double_sequence(kd_obj, kd, SERVO_COUNT, 0.0, "kd")) return nullptr;
|
||||
|
||||
PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
|
||||
if (!result) {
|
||||
return nullptr;
|
||||
}
|
||||
auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
|
||||
init_lowcmd_header(cmd);
|
||||
for (int i = 0; i < SERVO_COUNT; ++i) {
|
||||
encode_motor(cmd + MOTOR_OFFSET + i * MOTOR_SIZE, 0x0a, q[i], dq[i], tau[i], kp[i], kd[i]);
|
||||
}
|
||||
finish_crc(cmd);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_encrypted_servo12(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
|
||||
PyObject* plain = FastLowCmdBuilder_build_plain_servo12(self, args, kwargs);
|
||||
if (!plain) {
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* result = encrypt_bytes(
|
||||
self,
|
||||
reinterpret_cast<const uint8_t*>(PyBytes_AS_STRING(plain)),
|
||||
PyBytes_GET_SIZE(plain));
|
||||
Py_DECREF(plain);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_plain_fields20(FastLowCmdBuilder*, PyObject* args, PyObject* kwargs) {
|
||||
PyObject* mode_obj = nullptr;
|
||||
PyObject* q_obj = nullptr;
|
||||
PyObject* dq_obj = nullptr;
|
||||
PyObject* tau_obj = nullptr;
|
||||
PyObject* kp_obj = nullptr;
|
||||
PyObject* kd_obj = nullptr;
|
||||
static const char* kwlist[] = {"mode", "q", "dq", "tau", "kp", "kd", nullptr};
|
||||
if (!PyArg_ParseTupleAndKeywords(
|
||||
args, kwargs, "|OOOOOO", const_cast<char**>(kwlist),
|
||||
&mode_obj, &q_obj, &dq_obj, &tau_obj, &kp_obj, &kd_obj)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
int mode[MOTOR_COUNT];
|
||||
double q[MOTOR_COUNT], dq[MOTOR_COUNT], tau[MOTOR_COUNT], kp[MOTOR_COUNT], kd[MOTOR_COUNT];
|
||||
if (!read_int_sequence(mode_obj, mode, MOTOR_COUNT, 0, "mode")) return nullptr;
|
||||
if (!read_double_sequence(q_obj, q, MOTOR_COUNT, 0.0, "q")) return nullptr;
|
||||
if (!read_double_sequence(dq_obj, dq, MOTOR_COUNT, 0.0, "dq")) return nullptr;
|
||||
if (!read_double_sequence(tau_obj, tau, MOTOR_COUNT, 0.0, "tau")) return nullptr;
|
||||
if (!read_double_sequence(kp_obj, kp, MOTOR_COUNT, 0.0, "kp")) return nullptr;
|
||||
if (!read_double_sequence(kd_obj, kd, MOTOR_COUNT, 0.0, "kd")) return nullptr;
|
||||
|
||||
PyObject* result = PyBytes_FromStringAndSize(nullptr, LOWCMD_SIZE);
|
||||
if (!result) {
|
||||
return nullptr;
|
||||
}
|
||||
auto* cmd = reinterpret_cast<uint8_t*>(PyBytes_AS_STRING(result));
|
||||
init_lowcmd_header(cmd);
|
||||
for (int i = 0; i < MOTOR_COUNT; ++i) {
|
||||
encode_motor(cmd + MOTOR_OFFSET + i * MOTOR_SIZE, mode[i], q[i], dq[i], tau[i], kp[i], kd[i]);
|
||||
}
|
||||
finish_crc(cmd);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_encrypted_fields20(FastLowCmdBuilder* self, PyObject* args, PyObject* kwargs) {
|
||||
PyObject* plain = FastLowCmdBuilder_build_plain_fields20(self, args, kwargs);
|
||||
if (!plain) {
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* result = encrypt_bytes(
|
||||
self,
|
||||
reinterpret_cast<const uint8_t*>(PyBytes_AS_STRING(plain)),
|
||||
PyBytes_GET_SIZE(plain));
|
||||
Py_DECREF(plain);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_plain_lowcmd(FastLowCmdBuilder*, PyObject* args) {
|
||||
PyObject* lowcmd = nullptr;
|
||||
if (!PyArg_ParseTuple(args, "O", &lowcmd)) {
|
||||
return nullptr;
|
||||
}
|
||||
return build_plain_from_lowcmd_obj(lowcmd);
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_build_encrypted_lowcmd(FastLowCmdBuilder* self, PyObject* args) {
|
||||
PyObject* lowcmd = nullptr;
|
||||
if (!PyArg_ParseTuple(args, "O", &lowcmd)) {
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* plain = build_plain_from_lowcmd_obj(lowcmd);
|
||||
if (!plain) {
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* result = encrypt_bytes(
|
||||
self,
|
||||
reinterpret_cast<const uint8_t*>(PyBytes_AS_STRING(plain)),
|
||||
PyBytes_GET_SIZE(plain));
|
||||
Py_DECREF(plain);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyObject* FastLowCmdBuilder_encrypt(FastLowCmdBuilder* self, PyObject* args) {
|
||||
Py_buffer view;
|
||||
if (!PyArg_ParseTuple(args, "y*", &view)) {
|
||||
return nullptr;
|
||||
}
|
||||
PyObject* result = encrypt_bytes(self, reinterpret_cast<const uint8_t*>(view.buf), view.len);
|
||||
PyBuffer_Release(&view);
|
||||
return result;
|
||||
}
|
||||
|
||||
PyMethodDef FastLowCmdBuilder_methods[] = {
|
||||
{"build_plain_damping", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_damping), METH_NOARGS,
|
||||
"Build a plain all-damping LowCmd packet."},
|
||||
{"build_encrypted_damping", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_damping), METH_NOARGS,
|
||||
"Build an encrypted all-damping LowCmd packet."},
|
||||
{"build_plain_servo12", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_servo12),
|
||||
METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet with first 12 motors in servo mode."},
|
||||
{"build_encrypted_servo12", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_servo12),
|
||||
METH_VARARGS | METH_KEYWORDS, "Build an encrypted LowCmd packet with first 12 motors in servo mode."},
|
||||
{"build_plain_fields20", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_fields20),
|
||||
METH_VARARGS | METH_KEYWORDS, "Build a plain LowCmd packet from 20 motor field arrays."},
|
||||
{"build_encrypted_fields20", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_fields20),
|
||||
METH_VARARGS | METH_KEYWORDS, "Build an encrypted LowCmd packet from 20 motor field arrays."},
|
||||
{"build_plain_lowcmd", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_plain_lowcmd), METH_VARARGS,
|
||||
"Build a plain LowCmd packet from a Python LowCmd object."},
|
||||
{"build_encrypted_lowcmd", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_build_encrypted_lowcmd), METH_VARARGS,
|
||||
"Build an encrypted LowCmd packet from a Python LowCmd object."},
|
||||
{"encrypt", reinterpret_cast<PyCFunction>(FastLowCmdBuilder_encrypt), METH_VARARGS,
|
||||
"Encrypt a bytes-like object with the loaded Blowfish state."},
|
||||
{nullptr, nullptr, 0, nullptr},
|
||||
};
|
||||
|
||||
PyTypeObject FastLowCmdBuilderType = {
|
||||
PyVarObject_HEAD_INIT(nullptr, 0)
|
||||
};
|
||||
|
||||
PyModuleDef module = {
|
||||
PyModuleDef_HEAD_INIT,
|
||||
"go1_fast_lowcmd",
|
||||
"Native CPython LowCmd builder/encrypter for go1_pro_sdk.",
|
||||
-1,
|
||||
nullptr,
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
PyMODINIT_FUNC PyInit_go1_fast_lowcmd(void) {
|
||||
FastLowCmdBuilderType.tp_name = "go1_fast_lowcmd.FastLowCmdBuilder";
|
||||
FastLowCmdBuilderType.tp_basicsize = sizeof(FastLowCmdBuilder);
|
||||
FastLowCmdBuilderType.tp_flags = Py_TPFLAGS_DEFAULT;
|
||||
FastLowCmdBuilderType.tp_doc = "Native LowCmd builder with persistent Blowfish state.";
|
||||
FastLowCmdBuilderType.tp_init = reinterpret_cast<initproc>(FastLowCmdBuilder_init);
|
||||
FastLowCmdBuilderType.tp_new = PyType_GenericNew;
|
||||
FastLowCmdBuilderType.tp_methods = FastLowCmdBuilder_methods;
|
||||
|
||||
if (PyType_Ready(&FastLowCmdBuilderType) < 0) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
PyObject* m = PyModule_Create(&module);
|
||||
if (!m) {
|
||||
return nullptr;
|
||||
}
|
||||
Py_INCREF(&FastLowCmdBuilderType);
|
||||
if (PyModule_AddObject(m, "FastLowCmdBuilder", reinterpret_cast<PyObject*>(&FastLowCmdBuilderType)) < 0) {
|
||||
Py_DECREF(&FastLowCmdBuilderType);
|
||||
Py_DECREF(m);
|
||||
return nullptr;
|
||||
}
|
||||
PyModule_AddIntConstant(m, "LOWCMD_SIZE", LOWCMD_SIZE);
|
||||
return m;
|
||||
}
|
||||
Reference in New Issue
Block a user