💯 Adds initial IK and servo functionality
This commit is contained in:
Rune Harlyk
@@ -0,0 +1,42 @@
|
||||
#ifndef IK_CONFIG_H
|
||||
#define IK_CONFIG_H
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// SERVOS
|
||||
#define Servo_Foot 0
|
||||
#define Servo_Leg 1
|
||||
#define Servo_Shoulder 2
|
||||
|
||||
#define LEG_LF 0 // 0, 1, 2
|
||||
#define LEG_RF 1 // 3, 4, 5
|
||||
#define LEG_LB 2 // 6, 7, 8
|
||||
#define LEG_RB 3 // 9, 10, 11
|
||||
|
||||
#define L1 60.5 // y Distance between Shoulder Servo and Leg
|
||||
#define L2 10 // z Distance between Shoulder Servo and Leg
|
||||
#define L3 100.7 // Length of upper leg
|
||||
#define L4 118.5 // Length of lower leg
|
||||
#define L 207.5 // Distance between front and back servos
|
||||
#define W 78 // Distance between left and right shoulder
|
||||
|
||||
// predefined calculations
|
||||
#define L1L1 3660.25 //L1*L1
|
||||
// #define L1L2 3760.25 //L1*L1+L2*L2
|
||||
// #define LL12 1210 //2*L1*L2
|
||||
#define L3L3 10140.49 //L3*L3
|
||||
#define L4L4 14042.25 //L4*L4
|
||||
#define LL34 23865.9 //2*L3*L4
|
||||
|
||||
#define SERVO_STEP_ANGLE 2
|
||||
#define MOTION_STEP_ANGLE 5
|
||||
#define MOTION_STEP_MOVEMENT 5
|
||||
#define MOTION_STEP_ALFA 0.20
|
||||
|
||||
extern const int16_t servo_min[12] ;
|
||||
extern const float servo_conversion[12] ;
|
||||
extern const float theta_range[3][2];
|
||||
extern const int8_t servo_invert[12];
|
||||
extern int16_t servo_angles[4][3];
|
||||
|
||||
#endif
|
||||
+14
-1
@@ -33,6 +33,7 @@
|
||||
*/
|
||||
#define SCREEN_WIDTH 128
|
||||
#define SCREEN_HEIGHT 64
|
||||
#define SCREEN_RESET -1
|
||||
|
||||
|
||||
/*
|
||||
@@ -54,4 +55,16 @@
|
||||
*/
|
||||
#define USS_LEFT 12
|
||||
#define USS_RIGHT 13
|
||||
#define USS_MAX_DISTANCE 200
|
||||
#define USS_MAX_DISTANCE 200
|
||||
|
||||
/*
|
||||
* Button settings
|
||||
*/
|
||||
#define BUTTON 16
|
||||
#define BUTTON_LED 2
|
||||
|
||||
/*
|
||||
* PWM controller settings
|
||||
*/
|
||||
#define SERVO_OSCILLATOR_FREQUENCY 27000000
|
||||
#define SERVO_FREQ 50
|
||||
@@ -0,0 +1,27 @@
|
||||
#ifndef IK_TASK_H
|
||||
#define IK_TASK_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <servo.h>
|
||||
#include <IK_config.h>
|
||||
#include <spot_ik.h>
|
||||
|
||||
#define DEGREES2RAD 0.017453292519943
|
||||
|
||||
typedef struct {
|
||||
float omega;
|
||||
float phi;
|
||||
float psi;
|
||||
float xm;
|
||||
float ym;
|
||||
float zm;
|
||||
bool set;
|
||||
} position_t;
|
||||
|
||||
void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm);
|
||||
|
||||
void reset_position();
|
||||
|
||||
void task_ik(void *ignore);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,16 @@
|
||||
#ifndef SERVO_H
|
||||
#define SERVO_H
|
||||
|
||||
#include <esp_err.h>
|
||||
|
||||
typedef struct {
|
||||
uint16_t pulse_0;
|
||||
uint16_t pulse_180;
|
||||
int8_t invert;
|
||||
} servo_settings_t;
|
||||
|
||||
esp_err_t disable_servos();
|
||||
esp_err_t setup_pwm_controller();
|
||||
esp_err_t set_servo(uint8_t id, uint16_t angle);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,93 @@
|
||||
#ifndef SERVO_CONFIG_H
|
||||
#define SERVO_CONFIG_H
|
||||
|
||||
//#include "servo.h"
|
||||
#include "spot_ik.h"
|
||||
|
||||
// RF - Right Front Leg
|
||||
// lower leg
|
||||
#define RF_LOWER_SERVO_CHANNEL 8
|
||||
#define RF_LOWER_SERVO_CENTER 306
|
||||
#define RF_LOWER_SERVO_RANGE 385
|
||||
#define RF_LOWER_SERVO_DIRECTION 1
|
||||
#define RF_LOWER_SERVO_CENTER_ANG_DEG 99.86f
|
||||
// upper leg
|
||||
#define RF_UPPER_SERVO_CHANNEL 7
|
||||
#define RF_UPPER_SERVO_CENTER 306
|
||||
#define RF_UPPER_SERVO_RANGE 407
|
||||
#define RF_UPPER_SERVO_DIRECTION 1
|
||||
#define RF_UPPER_SERVO_CENTER_ANG_DEG -31.62f
|
||||
// shoulder joint
|
||||
#define RF_HIP_SERVO_CHANNEL 6
|
||||
#define RF_HIP_SERVO_CENTER 306
|
||||
#define RF_HIP_SERVO_RANGE 396
|
||||
#define RF_HIP_SERVO_DIRECTION -1
|
||||
#define RF_HIP_SERVO_CENTER_ANG_DEG 1.67f
|
||||
|
||||
// RB - Right Back Leg
|
||||
// lower leg
|
||||
#define RB_LOWER_SERVO_CHANNEL 2
|
||||
#define RB_LOWER_SERVO_CENTER 306
|
||||
#define RB_LOWER_SERVO_RANGE 369
|
||||
#define RB_LOWER_SERVO_DIRECTION 1
|
||||
#define RB_LOWER_SERVO_CENTER_ANG_DEG 95.37f
|
||||
// upper leg
|
||||
#define RB_UPPER_SERVO_CHANNEL 1
|
||||
#define RB_UPPER_SERVO_CENTER 306
|
||||
#define RB_UPPER_SERVO_RANGE 381
|
||||
#define RB_UPPER_SERVO_DIRECTION 1
|
||||
#define RB_UPPER_SERVO_CENTER_ANG_DEG -37.21f
|
||||
// shoulder joint
|
||||
#define RB_HIP_SERVO_CHANNEL 0
|
||||
#define RB_HIP_SERVO_CENTER 306
|
||||
#define RB_HIP_SERVO_RANGE 403
|
||||
#define RB_HIP_SERVO_DIRECTION 1
|
||||
#define RB_HIP_SERVO_CENTER_ANG_DEG -3.27f
|
||||
|
||||
// LB - Left Back Leg
|
||||
// lower leg
|
||||
#define LB_LOWER_SERVO_CHANNEL 5
|
||||
#define LB_LOWER_SERVO_CENTER 306
|
||||
#define LB_LOWER_SERVO_RANGE 374
|
||||
#define LB_LOWER_SERVO_DIRECTION 1
|
||||
#define LB_LOWER_SERVO_CENTER_ANG_DEG -92.65f
|
||||
// upper leg
|
||||
#define LB_UPPER_SERVO_CHANNEL 4
|
||||
#define LB_UPPER_SERVO_CENTER 306
|
||||
#define LB_UPPER_SERVO_RANGE 403
|
||||
#define LB_UPPER_SERVO_DIRECTION 1
|
||||
#define LB_UPPER_SERVO_CENTER_ANG_DEG 91.23f
|
||||
// shoulder joint
|
||||
#define LB_HIP_SERVO_CHANNEL 3
|
||||
#define LB_HIP_SERVO_CENTER 306
|
||||
#define LB_HIP_SERVO_RANGE 367
|
||||
#define LB_HIP_SERVO_DIRECTION -1
|
||||
#define LB_HIP_SERVO_CENTER_ANG_DEG -7.20f
|
||||
|
||||
// Lf - Left fRONT Leg
|
||||
// lower leg
|
||||
#define LF_LOWER_SERVO_CHANNEL 11
|
||||
#define LF_LOWER_SERVO_CENTER 306
|
||||
#define LF_LOWER_SERVO_RANGE 385
|
||||
#define LF_LOWER_SERVO_DIRECTION 1
|
||||
#define LF_LOWER_SERVO_CENTER_ANG_DEG -87.43f
|
||||
// upper leg
|
||||
#define LF_UPPER_SERVO_CHANNEL 10
|
||||
#define LF_UPPER_SERVO_CENTER 306
|
||||
#define LF_UPPER_SERVO_RANGE 388
|
||||
#define LF_UPPER_SERVO_DIRECTION 1
|
||||
#define LF_UPPER_SERVO_CENTER_ANG_DEG 38.21f
|
||||
// shoulder joint
|
||||
#define LF_HIP_SERVO_CHANNEL 9
|
||||
#define LF_HIP_SERVO_CENTER 306
|
||||
#define LF_HIP_SERVO_RANGE 388
|
||||
#define LF_HIP_SERVO_DIRECTION 1
|
||||
#define LF_HIP_SERVO_CENTER_ANG_DEG 4.67f
|
||||
|
||||
extern const int16_t servo_min[12] ;
|
||||
extern const float servo_conversion[12] ;
|
||||
extern const float theta_range[3][2];
|
||||
extern const int8_t servo_invert[12];
|
||||
extern int16_t servo_angles[4][3];
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef SPOT_IK_H
|
||||
#define SPOT_IK_H
|
||||
|
||||
#include "esp_err.h"
|
||||
|
||||
typedef struct {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} point;
|
||||
|
||||
esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t servo_angles[3]);
|
||||
esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm);
|
||||
esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]);
|
||||
|
||||
void print_matrix(float * matrix, int n, int m, char* name) ;
|
||||
void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines);
|
||||
|
||||
#endif
|
||||
+139
@@ -0,0 +1,139 @@
|
||||
#include <ik_task.h>
|
||||
|
||||
static const char* TAG = "IK TASK";
|
||||
|
||||
position_t spot_position = {.omega=0,.phi=0,.psi=0,.xm=-40,.ym=-170, .zm=0, .set=1};
|
||||
position_t goal_position = {0,};
|
||||
int16_t servo_angles[4][3] = {{90, 150, 0}, {90, 30, 180}, {90, 150, 0}, {90, 30, 180}};
|
||||
int16_t servo_angles_goal[4][3] = {0,};
|
||||
|
||||
void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm) {
|
||||
goal_position.omega = omega;
|
||||
goal_position.phi = phi;
|
||||
goal_position.psi = psi;
|
||||
goal_position.xm = xm;
|
||||
goal_position.ym = ym;
|
||||
goal_position.zm = zm;
|
||||
goal_position.set = true;
|
||||
}
|
||||
|
||||
void reset_position() {
|
||||
set_orientation_cb(0, 0, 0, 0, 0, 0);
|
||||
}
|
||||
|
||||
void set_leg_servos() {
|
||||
for (int l = 0; l<4; l++) {
|
||||
// for (int l = 1; l<2; l++) {
|
||||
for (int s=0;s<3;s++) {
|
||||
if (servo_angles[l][s] != servo_angles_goal[l][s]) {
|
||||
servo_angles[l][s] = servo_angles_goal[l][s];
|
||||
set_servo(l*3 + s, servo_angles[l][s]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void iterate_to_position() {
|
||||
ESP_LOGI(TAG, "GOAL (%f,%f,%f - %f,%f,%f)", goal_position.omega, goal_position.phi, goal_position.psi, goal_position.xm, goal_position.ym, goal_position.zm);
|
||||
|
||||
do {
|
||||
spot_position.set = false;
|
||||
int diff = 0;
|
||||
|
||||
if (goal_position.omega != spot_position.omega) {
|
||||
diff = goal_position.omega - spot_position.omega;
|
||||
if (abs(diff) < MOTION_STEP_ANGLE) {
|
||||
spot_position.omega = goal_position.omega ;
|
||||
} else {
|
||||
diff = diff < 0 ? -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
|
||||
spot_position.omega += diff;
|
||||
spot_position.set = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (goal_position.phi != spot_position.phi) {
|
||||
diff = goal_position.phi - spot_position.phi;
|
||||
if (abs(diff) < MOTION_STEP_ANGLE) {
|
||||
spot_position.phi = goal_position.phi ;
|
||||
} else {
|
||||
diff = diff < 0 ? -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
|
||||
spot_position.phi += diff;
|
||||
spot_position.set = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (goal_position.psi != spot_position.psi) {
|
||||
diff = goal_position.psi - spot_position.psi;
|
||||
if (abs(diff) < MOTION_STEP_ANGLE) {
|
||||
spot_position.psi = goal_position.psi ;
|
||||
} else {
|
||||
diff = diff < 0 ? -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
|
||||
spot_position.psi += diff;
|
||||
spot_position.set = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (goal_position.xm != spot_position.xm) {
|
||||
diff = goal_position.xm - spot_position.xm;
|
||||
if (abs(diff) < MOTION_STEP_MOVEMENT) {
|
||||
spot_position.xm = goal_position.xm ;
|
||||
} else {
|
||||
diff = diff < 0 ? -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
|
||||
spot_position.xm += diff;
|
||||
spot_position.set = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (goal_position.ym != spot_position.ym) {
|
||||
diff = goal_position.ym - spot_position.ym;
|
||||
if (abs(diff) < MOTION_STEP_MOVEMENT) {
|
||||
spot_position.ym = goal_position.ym ;
|
||||
} else {
|
||||
diff = diff < 0 ? -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
|
||||
spot_position.ym += diff;
|
||||
spot_position.set = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (goal_position.zm != spot_position.zm) {
|
||||
diff = goal_position.zm - spot_position.zm;
|
||||
if (abs(diff) < MOTION_STEP_MOVEMENT) {
|
||||
spot_position.zm = goal_position.zm ;
|
||||
} else {
|
||||
diff = diff < 0 ? -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
|
||||
spot_position.zm += diff;
|
||||
spot_position.set = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
ESP_LOGI(TAG, "CURRENT (%f,%f,%f - %f,%f,%f) %d", spot_position.omega, spot_position.phi, spot_position.psi, spot_position.xm, spot_position.ym, spot_position.zm, spot_position.set);
|
||||
|
||||
esp_err_t ret = spot_IK(spot_position.omega*DEGREES2RAD, spot_position.phi*DEGREES2RAD, spot_position.psi*DEGREES2RAD, spot_position.xm, spot_position.ym, spot_position.zm, servo_angles_goal);
|
||||
ESP_LOGD(TAG, "Valid IK %d", ret==ESP_OK);
|
||||
if (ret == ESP_OK) {
|
||||
//print_int_matrix((int16_t*) servo_angles_goal, 4, 3, "servo_angles_goal", false);
|
||||
set_leg_servos();
|
||||
}
|
||||
|
||||
} while (spot_position.set);
|
||||
|
||||
|
||||
//set_new_orientation_act_value((int16_t) spot_position.omega, (int16_t) spot_position.phi, (int16_t) spot_position.psi, (int16_t) spot_position.xm, (int16_t) spot_position.ym, (int16_t) spot_position.zm);
|
||||
}
|
||||
|
||||
void task_ik(void *ignore){
|
||||
ESP_LOGI(TAG, "Executing on core %d", xPortGetCoreID());
|
||||
esp_err_t ret;
|
||||
|
||||
reset_position();
|
||||
|
||||
for(;;) {
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
if (goal_position.set) {
|
||||
goal_position.set = false;
|
||||
iterate_to_position();
|
||||
}
|
||||
}
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
+7
-8
@@ -20,6 +20,11 @@
|
||||
#include <config.h>
|
||||
#include <camera.h>
|
||||
|
||||
#include <servo.h>
|
||||
#include <ik_task.h>
|
||||
|
||||
static const char* TAG = "MAIN";
|
||||
|
||||
DNSServer dnsServer;
|
||||
AsyncWebSocket ws(WEBSOCKET_PATH);
|
||||
AsyncEventSource events(EVENTSOURCE_PATH);
|
||||
@@ -31,8 +36,7 @@ NewPing sonar[2] = {
|
||||
};
|
||||
|
||||
MPU6050 mpu(Wire);
|
||||
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
|
||||
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
|
||||
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, SCREEN_RESET);
|
||||
|
||||
bool MPU_READY = false;
|
||||
bool PWM_READY = false;
|
||||
@@ -101,11 +105,6 @@ bool setupMPU(){
|
||||
return 1;
|
||||
}
|
||||
|
||||
bool setupPWMController(){
|
||||
pwm.begin();
|
||||
pwm.setOscillatorFrequency(27000000);
|
||||
pwm.setPWMFreq(50);
|
||||
return 1;
|
||||
}
|
||||
|
||||
void setup(){
|
||||
@@ -117,7 +116,7 @@ void setup(){
|
||||
|
||||
OLED_READY = setupOLED();
|
||||
MPU_READY = setupMPU();
|
||||
PWM_READY = setupPWMController();
|
||||
PWM_READY = setup_pwm_controller();
|
||||
|
||||
setupCamera();
|
||||
setupWiFi();
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
#include <Adafruit_PWMServoDriver.h>
|
||||
|
||||
#include "servo.h"
|
||||
#include "config.h"
|
||||
|
||||
#include "servo_config.h"
|
||||
|
||||
static const char* TAG = "SERVO";
|
||||
|
||||
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
|
||||
|
||||
const int16_t servo_min[12] = {153,118,138,121,116,125,131,150,148,130,158,165};
|
||||
const float servo_conversion[12] = {2.011111,2.011111,2.000000,2.050000,1.966667,2.027778,2.038889,1.677778,1.622222,2.027778,1.927778,1.650000};
|
||||
const int8_t servo_invert[12] = {1,0,1, 0,1,0, 0,0,1, 1,1,0};
|
||||
const float theta_range[3][2] = {{-M_PI / 3, M_PI/3}, {-2 * M_PI/3, M_PI/3}, {0, M_PI}};
|
||||
|
||||
esp_err_t disable_servos(){
|
||||
ESP_LOGI(TAG, "Disabling servos");
|
||||
pwm.setPWM(0, 0, 0);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t setup_pwm_controller(){
|
||||
pwm.begin();
|
||||
pwm.setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
|
||||
pwm.setPWMFreq(SERVO_FREQ);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t set_servo(uint8_t id, uint16_t angle) {
|
||||
esp_err_t ret;
|
||||
uint16_t pulse = (uint16_t) (0.5 + servo_min[id] + (angle * servo_conversion[id]));
|
||||
ESP_LOGI(TAG, "setPWM of servo %d, %d degrees -> Pulse %d", id, angle, pulse);
|
||||
ret = pwm.setPWM(id, 0, pulse);
|
||||
|
||||
if (ret == ESP_OK) return ESP_OK;
|
||||
else return ESP_FAIL;
|
||||
}
|
||||
+264
@@ -0,0 +1,264 @@
|
||||
#include "spot_ik.h"
|
||||
#include "IK_config.h"
|
||||
|
||||
#include "esp_dsp.h"
|
||||
#include "math.h"
|
||||
|
||||
// https://github.com/maartenweyn/SpotMicro_ESP32/blob/master/code/esp-idf/ik_test/main/spot_ik.c
|
||||
|
||||
static const char* TAG = "IK";
|
||||
|
||||
#define RAD2DEGREES 57.295779513082321 // 180 / PI
|
||||
#define DEGREES2RAD 0.017453292519943
|
||||
|
||||
// float orientation[3] = {M_PI / 8, M_PI / 8, 0}; //A 3x1 arry with Spot's Roll, Pitch, Yaw angles - omega, phi, psi
|
||||
// float position[3] = {0, 0, 0}; //A 3x1 array with Spot's X, Y, Z coordinates
|
||||
static float Rx[4][4] = {{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // X-axis rotation matrix
|
||||
static float Ry[4][4] = {{0, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // Y-axis rotation matrix
|
||||
static float Rz[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; // Z-axis rotation matrix
|
||||
static float Rxyz[4][4] = {0,};
|
||||
static float Tm[4][4] = {0,}; // Transformation Matrix
|
||||
static float T[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; //Translation <Atrix
|
||||
static float Trb[4][4] = {0,}; //Tranformation Matrix Right Back
|
||||
static float Trf[4][4] = {0,}; //Tranformation Matrix Right Front
|
||||
static float Tlb[4][4] = {0,}; //Tranformation Matrix Left Back
|
||||
static float Tlf[4][4] = {0,}; //Tranformation Matrix Left Front
|
||||
|
||||
static const float Trb2[4][4] = {{0, 0, 1, -L/2}, {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Back
|
||||
static const float Trf2[4][4] = {{0, 0, 1, L/2}, {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Front
|
||||
static const float Tlb2[4][4] = {{0, 0, 1, -L/2}, {0, 1, 0, 0}, {-1, 0, 0, W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Back
|
||||
static const float Tlf2[4][4] = {{0, 0, 1, L/2}, {0, 1, 0, 0}, {-1, 0, 0, W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Front
|
||||
|
||||
static float Lp[4][4] = {{80, -130, 100, 1}, {80, -130, -100, 1}, {-130, -130, 100, 1}, {-130, -130, -100, 1}};
|
||||
|
||||
static const float Ix[4][4] = {{-1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
|
||||
|
||||
static esp_err_t inverse(float a[4][4], float b[4][4]) {
|
||||
float s0 = a[0][0] * a[1][1] - a[1][0] * a[0][1];
|
||||
float s1 = a[0][0] * a[1][2] - a[1][0] * a[0][2];
|
||||
float s2 = a[0][0] * a[1][3] - a[1][0] * a[0][3];
|
||||
float s3 = a[0][1] * a[1][2] - a[1][1] * a[0][2];
|
||||
float s4 = a[0][1] * a[1][3] - a[1][1] * a[0][3];
|
||||
float s5 = a[0][2] * a[1][3] - a[1][2] * a[0][3];
|
||||
|
||||
float c5 = a[2][2] * a[3][3] - a[3][2] * a[2][3];
|
||||
float c4 = a[2][1] * a[3][3] - a[3][1] * a[2][3];
|
||||
float c3 = a[2][1] * a[3][2] - a[3][1] * a[2][2];
|
||||
float c2 = a[2][0] * a[3][3] - a[3][0] * a[2][3];
|
||||
float c1 = a[2][0] * a[3][2] - a[3][0] * a[2][2];
|
||||
float c0 = a[2][0] * a[3][1] - a[3][0] * a[2][1];
|
||||
|
||||
// Should check for 0 determinant
|
||||
float det = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
|
||||
|
||||
if (det == 0.0) return ESP_FAIL;
|
||||
|
||||
float invdet = 1.0 / det;
|
||||
|
||||
b[0][0] = ( a[1][1] * c5 - a[1][2] * c4 + a[1][3] * c3) * invdet;
|
||||
b[0][1] = (-a[0][1] * c5 + a[0][2] * c4 - a[0][3] * c3) * invdet;
|
||||
b[0][2] = ( a[3][1] * s5 - a[3][2] * s4 + a[3][3] * s3) * invdet;
|
||||
b[0][3] = (-a[2][1] * s5 + a[2][2] * s4 - a[2][3] * s3) * invdet;
|
||||
|
||||
b[1][0] = (-a[1][0] * c5 + a[1][2] * c2 - a[1][3] * c1) * invdet;
|
||||
b[1][1] = ( a[0][0] * c5 - a[0][2] * c2 + a[0][3] * c1) * invdet;
|
||||
b[1][2] = (-a[3][0] * s5 + a[3][2] * s2 - a[3][3] * s1) * invdet;
|
||||
b[1][3] = ( a[2][0] * s5 - a[2][2] * s2 + a[2][3] * s1) * invdet;
|
||||
|
||||
b[2][0] = ( a[1][0] * c4 - a[1][1] * c2 + a[1][3] * c0) * invdet;
|
||||
b[2][1] = (-a[0][0] * c4 + a[0][1] * c2 - a[0][3] * c0) * invdet;
|
||||
b[2][2] = ( a[3][0] * s4 - a[3][1] * s2 + a[3][3] * s0) * invdet;
|
||||
b[2][3] = (-a[2][0] * s4 + a[2][1] * s2 - a[2][3] * s0) * invdet;
|
||||
|
||||
b[3][0] = (-a[1][0] * c3 + a[1][1] * c1 - a[1][2] * c0) * invdet;
|
||||
b[3][1] = ( a[0][0] * c3 - a[0][1] * c1 + a[0][2] * c0) * invdet;
|
||||
b[3][2] = (-a[3][0] * s3 + a[3][1] * s1 - a[3][2] * s0) * invdet;
|
||||
b[3][3] = ( a[2][0] * s3 - a[2][1] * s1 + a[2][2] * s0) * invdet;
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
void print_matrix(float * matrix, int n, int m, char* name) {
|
||||
printf("Matrix %s:\n", name);
|
||||
for (int i=0;i<n;i++) {
|
||||
for (int j=0;j<m;j++)
|
||||
printf("%.2f ", matrix[i*m + j]);
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines) {
|
||||
printf("Matrix %s:\n", name);
|
||||
for (int i=0;i<n;i++) {
|
||||
for (int j=0;j<m;j++)
|
||||
printf("%3d ", matrix[i*m + j]);
|
||||
if (newlines) printf("\n");
|
||||
}
|
||||
if (!newlines) printf("\n");
|
||||
}
|
||||
|
||||
esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm) {
|
||||
float cos_omega = cos(omega);
|
||||
float sin_omega = sin(omega);
|
||||
float cos_phi = cos(phi);
|
||||
float sin_phi = sin(phi);
|
||||
float cos_psi = cos(psi);
|
||||
float sin_psi = sin(psi);
|
||||
|
||||
Rx[1][1] = cos_omega;
|
||||
Rx[1][2] = -sin_omega;
|
||||
Rx[2][1] = sin_omega;
|
||||
Rx[2][2] = cos_omega;
|
||||
|
||||
Ry[0][0] = cos_phi;
|
||||
Ry[0][2] = sin_phi;
|
||||
Ry[2][0] = -sin_phi;
|
||||
Ry[2][2] = cos_phi;
|
||||
|
||||
Rz[0][0] = cos_psi;
|
||||
Rz[0][1] = -sin_psi;
|
||||
Rz[1][0] = sin_psi;
|
||||
Rz[1][1] = cos_psi;
|
||||
|
||||
float Rxy[4][4];
|
||||
dspm_mult_f32_ae32((float*) Rx, (float*) Ry, (float*) Rxy, 4, 4, 4);
|
||||
dspm_mult_f32_ae32((float*) Rxy, (float*) Rz, (float*) Rxyz, 4, 4, 4);
|
||||
|
||||
// print_matrix((float*) Rx, 4, 4, "Rx");
|
||||
// print_matrix((float*) Ry, 4, 4, "Ry");
|
||||
// print_matrix((float*) Rz, 4, 4, "Rz");
|
||||
// print_matrix((float*) Rxyz, 4, 4, "Rxyz");
|
||||
|
||||
T[0][3] = xm;
|
||||
T[1][3] = ym;
|
||||
T[2][3] = zm;
|
||||
|
||||
dsps_add_f32_ae32((float*) T, (float*) Rxyz, (float*) Tm, 16, 1, 1, 1);
|
||||
// print_matrix((float*) Tm, 4, 4, "Tm");
|
||||
|
||||
dspm_mult_f32_ae32((float*) Tm, (float*) Trb2, (float*) Trb, 4, 4, 4);
|
||||
dspm_mult_f32_ae32((float*) Tm, (float*) Trf2, (float*) Trf, 4, 4, 4);
|
||||
dspm_mult_f32_ae32((float*) Tm, (float*) Tlf2, (float*) Tlf, 4, 4, 4);
|
||||
dspm_mult_f32_ae32((float*) Tm, (float*) Tlb2, (float*) Tlb, 4, 4, 4);
|
||||
// print_matrix((float*) Trb, 4, 4, "Trb");
|
||||
// print_matrix((float*) Trf, 4, 4, "Trf");
|
||||
// print_matrix((float*) Tlf, 4, 4, "Tlf");
|
||||
// print_matrix((float*) Tlb, 4, 4, "Tlb");
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
|
||||
|
||||
esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t* servo_angles) {
|
||||
//float x = (leg_id == LEG_RF || leg_id == LEG_RB) ? -p[0] : p[0];
|
||||
// Rxy Lenght of shoulder-point on x/y plane only
|
||||
float Rxy2 = p[0]*p[0] + p[1]*p[1] - L1L1;
|
||||
if (Rxy2 < 0) Rxy2 = 0;
|
||||
float Rxy = sqrt(Rxy2);
|
||||
// Dxy Distance we need to cover in xy plane
|
||||
float Dxy = Rxy - L2;
|
||||
// Dxyz 3D Distance we need to cover
|
||||
float Dxyz = sqrt(Dxy*Dxy+p[2]*p[2]);
|
||||
|
||||
// ESP_LOGD(tag, "IK Rxy,Dxy,Dxyz %.2f %.2f %.2f", Rxy, Dxy, Dxyz);
|
||||
|
||||
// The angle we need to cover - the angle already covered because of the offset of the servo
|
||||
float theta1 = -atan2(p[1], p[0]) - atan2(Rxy, -L1);
|
||||
if (isnan(theta1)) return -1;
|
||||
|
||||
// ESP_LOGD(tag, "T1 %.2f = (- %.2f - %.2f)", theta1, -atan2(p[1], p[0]), atan2(Rxy, -L1));
|
||||
|
||||
float d=(Dxyz*Dxyz - L3L3 - L4L4)/(LL34);
|
||||
float theta3 = acos(d);
|
||||
if (isnan(theta3)) return -3;
|
||||
|
||||
float theta2 = atan2(p[2], Dxy) - atan2(L4 * sin(theta3), L3 + L4 * cos(theta3));
|
||||
if (isnan(theta2)) return -2;
|
||||
// ESP_LOGD(tag, "IK %.2f - atan2( %.2f, %.2f) (%.2f)", atan2(p[2], Dxy), L4 * sin(theta3), L3 + L4 * cos(theta3), atan2(L4 * sin(theta3), L3 + L4 * cos(theta3)));
|
||||
|
||||
|
||||
if (servo_invert[leg_id*3]) {
|
||||
servo_angles[0] = 90 - (int16_t) (0.5 + theta1 * RAD2DEGREES);
|
||||
} else {
|
||||
servo_angles[0] = 90 + (int16_t) (0.5 + theta1 * RAD2DEGREES);
|
||||
}
|
||||
|
||||
if (servo_invert[leg_id*3 + 1]) {
|
||||
servo_angles[1] = 120 + (int16_t) (0.5 + theta2 * RAD2DEGREES);
|
||||
} else {
|
||||
servo_angles[1] = 60 - (int16_t) (0.5 + theta2 * RAD2DEGREES);
|
||||
}
|
||||
|
||||
if (servo_invert[leg_id*3 + 2]) {
|
||||
servo_angles[2] = 180 - (int16_t) (0.5 + theta3 * RAD2DEGREES);
|
||||
} else {
|
||||
servo_angles[2] = (int16_t) (0.5 + theta3 * RAD2DEGREES);
|
||||
}
|
||||
|
||||
if (servo_angles[1] < 0) servo_angles[1] += 360;
|
||||
if (servo_angles[1] > 360) servo_angles[1] -= 360;
|
||||
|
||||
// ESP_LOGD(tag, "IK T2 %.2f -> %d", theta2, servo_angles[1]);
|
||||
// ESP_LOGD(tag, "IK D %.2f, T3 %.2f -> %d", d, theta3, servo_angles[2]);
|
||||
|
||||
|
||||
if (theta1 < theta_range[0][0] || theta1 > theta_range[0][1]) return -4;
|
||||
if (theta2 < theta_range[1][0] || theta2 > theta_range[1][1]) return -5;
|
||||
if (theta3 < theta_range[2][0] || theta3 > theta_range[2][1]) return -6;
|
||||
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]) {
|
||||
esp_err_t ret = ESP_OK;
|
||||
body_IK(omega, phi, psi, xm, ym, zm);
|
||||
|
||||
float inv[4][4];
|
||||
float Q1[4][4];
|
||||
float Q[4];
|
||||
|
||||
|
||||
ret += inverse(Tlf, inv);
|
||||
dspm_mult_f32_ae32((float*) inv, (float*) Lp[0], (float*) Q, 4, 4, 1);
|
||||
// print_matrix((float*) Q, 1, 4, "Q LF");
|
||||
ret += leg_IK((float*) Q, LEG_LF, (int16_t*) servoangles[0]);
|
||||
//printf("leg_IK return %d\n", ret);
|
||||
//print_int_matrix((int16_t*) servo_angles[0], 1, 3, "servo_angles LF", 1);
|
||||
|
||||
|
||||
|
||||
// printf("\n\nRF-----\n");
|
||||
ret += inverse(Trf, inv);
|
||||
// print_matrix((float*) Ix, 4, 4, "Ix");
|
||||
// print_matrix((float*) inv, 4, 4, "inv Trf");
|
||||
dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
|
||||
// print_matrix((float*) Q1, 4, 4, "Q1 RF");
|
||||
dspm_mult_f32_ae32((float*) Q1, (float*) Lp[1], (float*) Q, 4, 4, 1);
|
||||
// print_matrix((float*) Q, 1, 4, "Q RF");
|
||||
ret += leg_IK((float*) Q, LEG_RF, (int16_t*) servoangles[1]);
|
||||
//printf("leg_IK return %d\n", ret);
|
||||
//print_int_matrix((int16_t*) servo_angles[1], 1, 3, "servo_angles RF");
|
||||
|
||||
ret += inverse(Tlb, inv);
|
||||
dspm_mult_f32_ae32((float*) inv, (float*) Lp[2], (float*) Q, 4, 4, 1);
|
||||
// print_matrix((float*) Q, 1, 4, "Q LB");
|
||||
ret += leg_IK((float*) Q, LEG_LB, (int16_t*) servoangles[2]);
|
||||
//printf("leg_IK return %d\n", ret);
|
||||
//print_int_matrix((int16_t*) servo_angles[2], 1, 3, "servo_angles LB");
|
||||
|
||||
// printf("\n\nRB-----\n");
|
||||
ret += inverse(Trb, inv);
|
||||
// print_matrix((float*) inv, 4, 4, "inv Trf");
|
||||
dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
|
||||
// print_matrix((float*) Q1, 4, 4, "Q1 RF");
|
||||
dspm_mult_f32_ae32((float*) Q1, (float*) Lp[3], (float*) Q, 4, 4, 1);
|
||||
|
||||
// print_matrix((float*) Q, 1, 4, "Q RB");
|
||||
ret += leg_IK((float*) Q, LEG_RB, (int16_t*) servoangles[3]);
|
||||
//printf("leg_IK return %d\n", ret);
|
||||
//print_int_matrix((int16_t*) servo_angles[3], 1, 3, "servo_angles RB");
|
||||
|
||||
return ret;
|
||||
}
|
||||
Reference in New Issue
Block a user