Помогите пожалуйста разобраться с инверсной кинематикой для гексапода. Я использовал https://oscarliang.com/inverse-kinematics-implementation-hexapod-robots/ статью для расчета тригонометрических функций на ардуино мега 2560. На данный момент программа считает углы для каждой сервы для одной ноги. Дальше, как я понял, нужно перейти от системы координат конкретной ноги к общей системе координат (относительно центра тела). Я не могу понять сам принцип, например, как будет робот двигаться вперед? Я должен указать желаемые координаты центра ТЕЛА, дальше преобразовать эти координаты в новые (для каждой ноги относительно своих систем координат, развернутых относительно общей центральной на свой угол), дальше для каждой ноги пересчитать полученные координаты в значения углов, и подать эти значения на сервоприводы? Правильно ли я понял последовательность действий?
Может есть пример кода или подробное описание именно пересчета координат?
Код для ардуино:
- Код: Выделить всё • Развернуть
const float pi = 3.14159265359;
const float coxalength = 28.0;
const float femurlength = 36.0;
const float tibialength = 75.0;
float coxaangle;
float femurangle;
float tibiaangle;
float L;
float L1;
float alpha1;
float alpha2;
void inverse_kinematics_calculations(float x, float y, float z) {
L1 = sqrt(sq(x) + sq(y));
L = sqrt(sq(L1 - coxalength) + sq(z));
alpha1 = acos(z / L) * 180 / pi;
alpha2 = acos((sq(femurlength) + sq(L) - sq(tibialength)) / (2 * femurlength * L)) * 180 / pi;
coxaangle = atan(x / y) * 180 / pi;
femurangle = (alpha1 + alpha2);
tibiaangle = acos((sq(femurlength) + sq(tibialength) - sq(L)) / (2 * femurlength * tibialength)) * 180 / pi;
}
void refresh() {
inverse_kinematics_calculations(60, 60, 0);
Serial.println("Coxa angle = ");
Serial.println(coxaangle);
Serial.println("Femur angle = ");
Serial.println(femurangle);
Serial.println("Tibia angle = ");
Serial.println(tibiaangle);
}
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
refresh();
delay(5000);
}
