Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🪄 Update kinematic interface

Browse Source
This commit is contained in:
Rune Harlyk
2024-05-27 01:23:10 +02:00
committed by Rune Harlyk
parent ccf6f01e4d
commit b338ec0316
2 changed files with 59 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/Kinematics.h
+16 -7
View File
@@ -51,8 +51,17 @@ static esp_err_t inverse(float a[4][4], float b[4][4])
return ESP_OK;
}
typedef struct {
typedef struct {
float omega, phi, psi, xm, ym, zm;
float feet[4][4];
void updateFeet(const float newFeet[4][4]) {
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
feet[i][j] = newFeet[i][j];
}
}
}
} body_state_t;
@@ -93,27 +102,27 @@ public:
}
~Kinematics(){}
esp_err_t calculate_inverse_kinematics(float lp[4][4], body_state_t p, float result[12]) {
esp_err_t calculate_inverse_kinematics(body_state_t body_state, float result[12]) {
esp_err_t ret = ESP_OK;
ret = bodyIK(p);
ret = bodyIK(body_state);
ret += inverse(Tlf, inv);
dspm_mult_f32_ae32((float*) inv, (float*) lp[0], (float*) point, 4, 4, 1);
dspm_mult_f32_ae32((float*) inv, (float*) body_state.feet[0], (float*) point, 4, 4, 1);
legIK((float*) point, result);
ret += inverse(Trf, inv);
dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
dspm_mult_f32_ae32((float*) Q1, (float*) lp[1], (float*) point, 4, 4, 1);
dspm_mult_f32_ae32((float*) Q1, (float*) body_state.feet[1], (float*) point, 4, 4, 1);
legIK((float*) point, result + 3);
ret += inverse(Tlb, inv);
dspm_mult_f32_ae32((float*) inv, (float*) lp[2], (float*) point, 4, 4, 1);
dspm_mult_f32_ae32((float*) inv, (float*) body_state.feet[2], (float*) point, 4, 4, 1);
legIK((float*) point, result + 6);
ret += inverse(Trb, inv);
dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
dspm_mult_f32_ae32((float*) Q1, (float*) lp[3], (float*) point, 4, 4, 1);
dspm_mult_f32_ae32((float*) Q1, (float*) body_state.feet[3], (float*) point, 4, 4, 1);
legIK((float*) point, result + 9);
return ret;
esp32/lib/ESP32-sveltekit/MotionService.h
+43 -37
View File
@@ -45,6 +45,8 @@ class MotionService
_socket->onSubscribe(ANGLES_EVENT,
std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
body_state.updateFeet(default_feet_positions);
}
void anglesEvent(JsonObject &root, int originId)
@@ -60,37 +62,41 @@ class MotionService
void positionEvent(JsonObject &root, int originId)
{
JsonArray array = root["data"].as<JsonArray>();
position.omega = array[0];
position.phi = array[1];
position.psi = array[2];
position.xm = array[3];
position.ym = array[4];
position.zm = array[5];
body_state.omega = array[0];
body_state.phi = array[1];
body_state.psi = array[2];
body_state.xm = array[3];
body_state.ym = array[4];
body_state.zm = array[5];
// syncAngles(String(originId));
}
void handleInput(JsonObject &root, int originId)
{
JsonArray array = root["data"].as<JsonArray>();
for (int i = 0; i < 7; i++)
{
input[i] = array[i];
float lx = array[1];
float ly = array[2];
float rx = array[3];
float ry = array[4];
float h = array[5];
float s = array[6];
body_state.ym = (h + 128) * (float)0.7;
switch (motionState) {
case MOTION_STATE::STAND: {
body_state.phi = rx / 4;
body_state.psi = ry / 4;
body_state.xm = ly / 2;
body_state.zm = lx / 2;
break;
}
case MOTION_STATE::WALK:
position_target.x = rx / 2;
position_target.z = ry / 2;
position_target.yaw = lx / 2;
break;
}
float lx = input[1];
float ly = input[2];
float rx = input[3];
float ry = input[4];
float h = input[5];
float s = input[6];
position = {
0,
rx / 4,
ry / 4,
ly / 2,
(h + 128) * (float)0.7,
lx / 2
};
ESP_LOGI("MotionService", "Input: %.0f %.0f %.0f %.0f %.0f %.0f %.0f", input[0], input[1], input[2], input[3], input[4], input[5], input[6]);
}
void handleMode(JsonObject &root, int originId)
@@ -117,21 +123,21 @@ class MotionService
float new_angles[12] = {0,};
switch (motionState) {
case MOTION_STATE::IDLE:
return false;
break;
case MOTION_STATE::REST:
update_angles(rest_angles, new_angles, false);
break;
case MOTION_STATE::STAND: {
kinematics.calculate_inverse_kinematics(lp, position, new_angles);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
}
case MOTION_STATE::WALK:
lp[0][1] += walk_dir;
if (lp[0][1] >= 50) walk_dir = -1;
if (lp[0][1] <= -100) walk_dir = 1;
gaitPlanner.update_trajectory(position_target, MotionInterval, body_state);
kinematics.calculate_inverse_kinematics(lp, position, new_angles);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
}
return update_angles(new_angles, angles);
@@ -162,25 +168,25 @@ class MotionService
SecurityManager *_securityManager;
TaskManager *_taskManager;
Kinematics kinematics;
GaitPlanner gaitPlanner;
MOTION_STATE motionState = MOTION_STATE::IDLE;
unsigned long _lastUpdate;
constexpr static int MotionInterval = 100;
body_state_t position = {0, 0, 0, 0, 0, 0};
body_state_t body_state = {0,};
position_target_t position_target = {0,};
float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
float lp[4][4] = {
float default_feet_positions[4][4] = {
{ 100, -100, 100, 1},
{ 100, -100, -100, 1},
{-100, -100, 100, 1},
{-100, -100, -100, 1}
};
float input[7] = {0, 0, 0, 0, 0, 0, 0};
float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
float angles[12] = {0,};
float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
float stand_angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90};
MOTION_STATE motionState = MOTION_STATE::IDLE;
unsigned long _lastUpdate;
int walk_dir = 2;
};
#endif