Rune Harlyk
159 lines
6.6 KiB
C++
159 lines
6.6 KiB
C++
#ifndef Kinematics_h
|
|
#define Kinematics_h
|
|
|
|
#include <utils/math_utils.h>
|
|
|
|
struct body_state_t {
|
|
float omega, phi, psi, xm, ym, zm;
|
|
float feet[4][4];
|
|
|
|
void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }
|
|
|
|
bool operator==(const body_state_t &other) const {
|
|
if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
|
|
!IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||
|
|
!IS_ALMOST_EQUAL(zm, other.zm)) {
|
|
return false;
|
|
}
|
|
return arrayEqual(feet, other.feet, 0.1);
|
|
}
|
|
};
|
|
|
|
class Kinematics {
|
|
private:
|
|
static constexpr float l1 = 60.5f / 100.0f;
|
|
static constexpr float l2 = 10.0f / 100.0f;
|
|
static constexpr float l3 = 111.1f / 100.0f;
|
|
static constexpr float l4 = 118.5f / 100.0f;
|
|
|
|
static constexpr float L = 207.5f / 100.0f;
|
|
static constexpr float W = 78.0f / 100.0f;
|
|
|
|
const float sHp = sinf(PI_F / 2);
|
|
const float cHp = cosf(PI_F / 2);
|
|
|
|
static constexpr float Ix[4][4] = {{-1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
|
|
|
|
float Trb[4][4] = {0};
|
|
float Trf[4][4] = {0};
|
|
float Tlb[4][4] = {0};
|
|
float Tlf[4][4] = {0};
|
|
|
|
float inv[4][4];
|
|
float point[4];
|
|
float Q1[4][4];
|
|
|
|
body_state_t currentState;
|
|
|
|
public:
|
|
esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) {
|
|
esp_err_t ret = ESP_OK;
|
|
|
|
if (currentState == body_state) return ESP_OK;
|
|
|
|
ret = bodyIK(body_state);
|
|
currentState.omega = body_state.omega;
|
|
currentState.phi = body_state.phi;
|
|
currentState.psi = body_state.psi;
|
|
currentState.xm = body_state.xm;
|
|
currentState.ym = body_state.ym;
|
|
currentState.zm = body_state.zm;
|
|
currentState.updateFeet(body_state.feet);
|
|
|
|
ret += inverse(Tlf, inv);
|
|
MAT_MULT(inv, body_state.feet[0], point, 4, 4, 1);
|
|
legIK((float *)point, result);
|
|
|
|
ret += inverse(Trf, inv);
|
|
MAT_MULT(Ix, inv, Q1, 4, 4, 4);
|
|
MAT_MULT(Q1, body_state.feet[1], point, 4, 4, 1);
|
|
legIK((float *)point, result + 3);
|
|
|
|
ret += inverse(Tlb, inv);
|
|
MAT_MULT(inv, body_state.feet[2], point, 4, 4, 1);
|
|
legIK((float *)point, result + 6);
|
|
|
|
ret += inverse(Trb, inv);
|
|
MAT_MULT(Ix, inv, Q1, 4, 4, 4);
|
|
MAT_MULT(Q1, body_state.feet[3], point, 4, 4, 1);
|
|
legIK((float *)point, result + 9);
|
|
|
|
return ret;
|
|
}
|
|
|
|
esp_err_t bodyIK(const body_state_t p) {
|
|
float cos_omega = COS_DEG_F(p.omega);
|
|
float sin_omega = SIN_DEG_F(p.omega);
|
|
float cos_phi = COS_DEG_F(p.phi);
|
|
float sin_phi = SIN_DEG_F(p.phi);
|
|
float cos_psi = COS_DEG_F(p.psi);
|
|
float sin_psi = SIN_DEG_F(p.psi);
|
|
|
|
float Tm[4][4] = {{cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm},
|
|
{sin_omega * sin_phi * cos_psi + sin_psi * cos_omega,
|
|
-sin_omega * sin_phi * sin_psi + cos_omega * cos_psi, -sin_omega * cos_phi, p.ym},
|
|
{sin_omega * sin_psi - sin_phi * cos_omega * cos_psi,
|
|
sin_omega * cos_psi + sin_phi * sin_psi * cos_omega, cos_omega * cos_phi, p.zm},
|
|
{0, 0, 0, 1}};
|
|
|
|
float point_lf[4][4] = {{cHp, 0, sHp, L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, W / 2}, {0, 0, 0, 1}};
|
|
float point_rf[4][4] = {{cHp, 0, sHp, L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, -W / 2}, {0, 0, 0, 1}};
|
|
float point_lb[4][4] = {{cHp, 0, sHp, -L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, W / 2}, {0, 0, 0, 1}};
|
|
float point_rb[4][4] = {{cHp, 0, sHp, -L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, -W / 2}, {0, 0, 0, 1}};
|
|
|
|
MAT_MULT(Tm, point_lf, Tlf, 4, 4, 4);
|
|
MAT_MULT(Tm, point_rf, Trf, 4, 4, 4);
|
|
MAT_MULT(Tm, point_lb, Tlb, 4, 4, 4);
|
|
MAT_MULT(Tm, point_rb, Trb, 4, 4, 4);
|
|
return ESP_OK;
|
|
}
|
|
|
|
void legIK(float point[4], float result[3]) {
|
|
float x = point[0], y = point[1], z = point[2];
|
|
|
|
float F = sqrtf(x * x + y * y - l1 * l1);
|
|
F = isnanf(F) ? l1 : F;
|
|
float G = F - l2;
|
|
float H = sqrtf(G * G + z * z);
|
|
|
|
float theta1 = -atan2f(y, x) - atan2f(F, -l1);
|
|
float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4);
|
|
float theta3 = atan2(sqrt(1 - D * D), D);
|
|
if (isnan(theta3)) theta3 = 0;
|
|
float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3));
|
|
result[0] = RAD_TO_DEG_F(theta1);
|
|
result[1] = RAD_TO_DEG_F(theta2);
|
|
result[2] = RAD_TO_DEG_F(theta3);
|
|
}
|
|
|
|
esp_err_t inverse(float a[4][4], float b[4][4]) {
|
|
float s[] = {a[0][0] * a[1][1] - a[1][0] * a[0][1], a[0][0] * a[1][2] - a[1][0] * a[0][2],
|
|
a[0][0] * a[1][3] - a[1][0] * a[0][3], a[0][1] * a[1][2] - a[1][1] * a[0][2],
|
|
a[0][1] * a[1][3] - a[1][1] * a[0][3], a[0][2] * a[1][3] - a[1][2] * a[0][3]};
|
|
float c[] = {a[2][0] * a[3][1] - a[3][0] * a[2][1], a[2][0] * a[3][2] - a[3][0] * a[2][2],
|
|
a[2][0] * a[3][3] - a[3][0] * a[2][3], a[2][1] * a[3][2] - a[3][1] * a[2][2],
|
|
a[2][1] * a[3][3] - a[3][1] * a[2][3], a[2][2] * a[3][3] - a[3][2] * a[2][3]};
|
|
float det = s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] - s[4] * c[1] + s[5] * c[0];
|
|
if (det == 0.0) return ESP_FAIL;
|
|
float invdet = 1.0 / det;
|
|
b[0][0] = (a[1][1] * c[5] - a[1][2] * c[4] + a[1][3] * c[3]) * invdet;
|
|
b[0][1] = (-a[0][1] * c[5] + a[0][2] * c[4] - a[0][3] * c[3]) * invdet;
|
|
b[0][2] = (a[3][1] * s[5] - a[3][2] * s[4] + a[3][3] * s[3]) * invdet;
|
|
b[0][3] = (-a[2][1] * s[5] + a[2][2] * s[4] - a[2][3] * s[3]) * invdet;
|
|
b[1][0] = (-a[1][0] * c[5] + a[1][2] * c[2] - a[1][3] * c[1]) * invdet;
|
|
b[1][1] = (a[0][0] * c[5] - a[0][2] * c[2] + a[0][3] * c[1]) * invdet;
|
|
b[1][2] = (-a[3][0] * s[5] + a[3][2] * s[2] - a[3][3] * s[1]) * invdet;
|
|
b[1][3] = (a[2][0] * s[5] - a[2][2] * s[2] + a[2][3] * s[1]) * invdet;
|
|
b[2][0] = (a[1][0] * c[4] - a[1][1] * c[2] + a[1][3] * c[0]) * invdet;
|
|
b[2][1] = (-a[0][0] * c[4] + a[0][1] * c[2] - a[0][3] * c[0]) * invdet;
|
|
b[2][2] = (a[3][0] * s[4] - a[3][1] * s[2] + a[3][3] * s[0]) * invdet;
|
|
b[2][3] = (-a[2][0] * s[4] + a[2][1] * s[2] - a[2][3] * s[0]) * invdet;
|
|
b[3][0] = (-a[1][0] * c[3] + a[1][1] * c[1] - a[1][2] * c[0]) * invdet;
|
|
b[3][1] = (a[0][0] * c[3] - a[0][1] * c[1] + a[0][2] * c[0]) * invdet;
|
|
b[3][2] = (-a[3][0] * s[3] + a[3][1] * s[1] - a[3][2] * s[0]) * invdet;
|
|
b[3][3] = (a[2][0] * s[3] - a[2][1] * s[1] + a[2][2] * s[0]) * invdet;
|
|
return ESP_OK;
|
|
}
|
|
};
|
|
|
|
#endif |