cpp重构平衡控制
This commit is contained in:
parent
dd58f2cb2d
commit
f2e3b63eac
|
|
@ -15,7 +15,7 @@ set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
|
|||
|
||||
# project settings
|
||||
project(wheel_legged C CXX ASM)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
|
||||
#Uncomment for hardware floating point
|
||||
|
|
@ -53,9 +53,9 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D
|
|||
bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
|
||||
modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
|
||||
modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
|
||||
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter
|
||||
modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter modules/matrix
|
||||
modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor
|
||||
application application/chassis application/cmd application/gimbal application/shoot
|
||||
application application/chassis application/cmd application/gimbal application/shoot application/balance
|
||||
Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)
|
||||
|
||||
include_directories(Middlewares/ST/ARM/DSP/Inc)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,106 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/2.
|
||||
//
|
||||
|
||||
#include "balance.h"
|
||||
void get_lqr_k(float L0, float K[12]);
|
||||
|
||||
balance::balance(Balance_Init_config_s *InitConfig):
|
||||
leg_r_(&InitConfig->legInitConfig),leg_l_(&InitConfig->legInitConfig)
|
||||
{
|
||||
PIDInit(&leg_coordination_PID_,&InitConfig->leg_cor_PID_config);
|
||||
PIDInit(&leg_length_PID_,&InitConfig->legInitConfig.length_PID_conf);
|
||||
memset(state_,0,6);
|
||||
memset(target_state_,0,6);
|
||||
}
|
||||
|
||||
void balance::feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z)
|
||||
{
|
||||
leg_r_.state_update(leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
||||
leg_l_.state_update(leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
||||
|
||||
Matrixf<4,1> leg_r_state = leg_r_.get_state();
|
||||
Matrixf<4,1> leg_l_state = leg_l_.get_state();
|
||||
float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
|
||||
float phi_average = (leg_r_state[1][0] + leg_l_state[1][0])/2;
|
||||
float phi_dot_average = (leg_r_state[3][0] + leg_l_state[3][0])/2;
|
||||
|
||||
float theta = PI/2 - phi_average - body_phi;
|
||||
float theta_dot = -phi_dot_average - body_phi_dot;
|
||||
state_[0] = theta;
|
||||
state_[1] = theta_dot;
|
||||
state_[2] = x; state_[3] = x_dot;
|
||||
state_[4] = body_phi; state_[5] = body_phi_dot;
|
||||
|
||||
float K_mat[12];
|
||||
get_lqr_k(L_average,K_mat);
|
||||
balance_LQR_.set_k(K_mat);
|
||||
}
|
||||
|
||||
void balance::control_loop() {
|
||||
balance_LQR_.update(state_,target_state_);
|
||||
|
||||
Matrixf<2,1> control = balance_LQR_.get_control();
|
||||
Matrixf<4,1> leg_r_state = leg_r_.get_state();
|
||||
Matrixf<4,1> leg_l_state = leg_l_.get_state();
|
||||
|
||||
float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
|
||||
float F_pid_out= PIDCalculate(&leg_length_PID_,L_average,target_length_);
|
||||
float coordination_err = leg_r_state[1][0] - leg_l_state[1][0];
|
||||
float cor_Tp = PIDCalculate(&leg_coordination_PID_,coordination_err,0);
|
||||
|
||||
leg_r_.input_update(F_pid_out + leg_r_.F_feedforward_ + 20,control[1][0] + cor_Tp);
|
||||
leg_l_.input_update(F_pid_out + leg_l_.F_feedforward_ + 20,control[1][0] - cor_Tp);
|
||||
}
|
||||
|
||||
void balance::target_set(float x) {
|
||||
target_state_[2] = x;
|
||||
}
|
||||
|
||||
Matrixf<2,1> balance::get_control() {
|
||||
return balance_LQR_.get_control();
|
||||
}
|
||||
|
||||
void get_lqr_k(float L0, float K[12])
|
||||
{
|
||||
float t2;
|
||||
float t3;
|
||||
|
||||
float K_temp[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
t2 = L0 * L0;
|
||||
t3 = t2 * L0;
|
||||
/* 24-Jan-2024 14:05:09 */
|
||||
K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F;
|
||||
K_temp[1] =
|
||||
((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F;
|
||||
K_temp[2] =
|
||||
((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F;
|
||||
K_temp[3] =
|
||||
((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F;
|
||||
K_temp[4] =
|
||||
((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F;
|
||||
K_temp[5] =
|
||||
((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F;
|
||||
K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) -
|
||||
10.4576302F;
|
||||
K_temp[7] =
|
||||
((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F;
|
||||
K_temp[8] =
|
||||
((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F;
|
||||
K_temp[9] =
|
||||
((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F;
|
||||
K_temp[10] =
|
||||
((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F;
|
||||
K_temp[11] =
|
||||
((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F;
|
||||
|
||||
//matlab赋值顺序不同
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[i] = K_temp[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[6+i] = K_temp[2*i+1];
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/2.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_BALANCE_H
|
||||
#define WHEEL_LEGGED_BALANCE_H
|
||||
|
||||
#include "leg.h"
|
||||
#include "LQR.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Leg_init_config_s legInitConfig;
|
||||
PID_Init_Config_s leg_cor_PID_config;
|
||||
}Balance_Init_config_s;
|
||||
|
||||
class balance {
|
||||
public:
|
||||
balance(Balance_Init_config_s* InitConfig);
|
||||
void feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
|
||||
void control_loop();
|
||||
void target_set(float x);
|
||||
Matrixf<2,1> get_control();
|
||||
|
||||
private:
|
||||
leg leg_r_;
|
||||
leg leg_l_;
|
||||
float target_length_; //平均腿长目标值
|
||||
PIDInstance leg_coordination_PID_; //双腿协调PD控制
|
||||
PIDInstance leg_length_PID_; //平均腿长控制
|
||||
|
||||
float state_[6];
|
||||
float target_state_[6];
|
||||
LQR<6,2> balance_LQR_;
|
||||
};
|
||||
|
||||
|
||||
#endif //WHEEL_LEGGED_BALANCE_H
|
||||
|
|
@ -0,0 +1,156 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/1.
|
||||
//
|
||||
|
||||
|
||||
#include "leg.h"
|
||||
#include <cmath>
|
||||
|
||||
|
||||
static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
|
||||
|
||||
leg::leg(Leg_init_config_s* legInitConfig)
|
||||
{
|
||||
PIDInit(&Length_PID_,&legInitConfig->length_PID_conf);
|
||||
PIDInit(&Phi0_PID_,&legInitConfig->phi0_PID_conf);
|
||||
|
||||
target_L0_ = legInitConfig->init_target_L0;
|
||||
F_feedforward_ = legInitConfig->F_feedforward;
|
||||
}
|
||||
|
||||
void leg::input_update(float F, float Tp)
|
||||
{
|
||||
legInput_ = Matrixf<2,1>(new (float[2]){F,Tp});
|
||||
}
|
||||
|
||||
void leg::state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot)
|
||||
{
|
||||
if(phi1<=PI/2)
|
||||
phi1 = PI/2;
|
||||
if(phi4>=PI/2)
|
||||
phi4 = PI/2;
|
||||
|
||||
legState_.phi1 = phi1;
|
||||
legState_.phi4 = phi4;
|
||||
|
||||
//以机体中点建立坐标系
|
||||
float xb = -l[4]/2+l[0]* arm_cos_f32(legState_.phi1);
|
||||
float yb = l[0]* arm_sin_f32(legState_.phi1);
|
||||
|
||||
float xd = l[4]/2+l[3]* arm_cos_f32(legState_.phi4);
|
||||
float yd = l[3]* arm_sin_f32(legState_.phi4);
|
||||
|
||||
float A0 = 2*l[1]*(xd-xb);
|
||||
float B0 = 2*l[1]*(yd-yb);
|
||||
|
||||
float lbd2 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
|
||||
float C0 = l[1]*l[1]+lbd2-l[2]*l[2];
|
||||
|
||||
float tanPhi = 0;
|
||||
|
||||
arm_sqrt_f32((A0*A0 + B0*B0 - C0*C0),&tanPhi);
|
||||
//tanPhi = (B0+tanPhi)/(A0+C0);
|
||||
|
||||
float phi2 = 2* atan2f(B0+tanPhi,A0+C0);
|
||||
float xc = xb + l[1] * arm_cos_f32(phi2);
|
||||
float yc = yb + l[1] * arm_sin_f32(phi2);
|
||||
|
||||
float phi3 = acosf(xc-xd/l[2]);
|
||||
float L0 = 0;
|
||||
arm_sqrt_f32((xc*xc + yc*yc),&L0);
|
||||
float phi0 = atan2f(yc,xc);
|
||||
|
||||
//赋值
|
||||
legState_.L0 = L0;
|
||||
legState_.phi0 = phi0;
|
||||
legState_.phi2 = phi2;
|
||||
legState_.phi3 = phi3;
|
||||
|
||||
get_dot(phi1_dot,phi4_dot);
|
||||
}
|
||||
|
||||
void leg::calculate_output() {
|
||||
float sinPhi23 = arm_sin_f32(legState_.phi2 - legState_.phi3);
|
||||
float sinPhi12 = arm_sin_f32(legState_.phi1 - legState_.phi2);
|
||||
float sinPhi34 = arm_sin_f32(legState_.phi3 - legState_.phi4);
|
||||
|
||||
float sinPhi03 = arm_sin_f32(legState_.phi0 - legState_.phi3);
|
||||
float sinPhi02 = arm_sin_f32(legState_.phi0 - legState_.phi2);
|
||||
float cosPhi03 = arm_cos_f32(legState_.phi0 - legState_.phi3);
|
||||
float cosPhi02 = arm_cos_f32(legState_.phi0 - legState_.phi2);
|
||||
|
||||
float Jacobi_data[4] = {0};
|
||||
Jacobi_data[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
|
||||
Jacobi_data[1] = -(l[0]* cosPhi03*sinPhi12)/(legState_.L0 * sinPhi23);
|
||||
|
||||
Jacobi_data[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
|
||||
Jacobi_data[3] = -(l[3]* cosPhi02*sinPhi34)/(legState_.L0 * sinPhi23);
|
||||
|
||||
VMC_Jacobi_ = Matrixf<2,2>(Jacobi_data);
|
||||
legOutput_ = VMC_Jacobi_ * legInput_;
|
||||
}
|
||||
|
||||
void leg::feedback_update(float T1_fb, float T2_fb) {
|
||||
Matrixf<2,1> T12(new (float[2]){T1_fb,T2_fb});
|
||||
legFeedback_ = matrixf::inv(VMC_Jacobi_) * T12;
|
||||
}
|
||||
|
||||
void leg::get_dot(float phi1_dot,float phi4_dot)
|
||||
{
|
||||
//多项式拟合方法获得雅可比矩阵 待测试
|
||||
float phi_1 = legState_.phi1;
|
||||
float phi_4 = legState_.phi4;
|
||||
|
||||
float phi1_sq = phi_1*phi_1;
|
||||
float phi4_sq = phi_4*phi_4;
|
||||
float pose_Jacobi[4] = {0};
|
||||
float temp = sqrtf(powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2));
|
||||
|
||||
pose_Jacobi[0] =
|
||||
(0.5f*(2.0f*(0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - 2.0f*(0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
|
||||
pose_Jacobi[1] =
|
||||
(0.5f*(2.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + 2.0f*(0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
|
||||
|
||||
float temp2 = (powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2)/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + 1.0f);
|
||||
|
||||
pose_Jacobi[2] =
|
||||
((0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + ((0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2))/temp2;
|
||||
pose_Jacobi[3] =
|
||||
-(1.0f*((0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - (1.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2)))/temp2;
|
||||
Matrixf<2,2> pose_J(pose_Jacobi);
|
||||
|
||||
State_dot_ = pose_J * Matrixf<2,1>(new (float[2]){phi1_dot,phi4_dot});
|
||||
}
|
||||
|
||||
Matrixf<4, 1> leg::get_state() {
|
||||
return Matrixf<4, 1>(new (float[4]) {legState_.L0,legState_.phi0,*State_dot_[0],*State_dot_[1]});
|
||||
}
|
||||
|
||||
float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {
|
||||
//求导所用时间
|
||||
static uint32_t dot_cnt = 0;
|
||||
static float last_theta_dot,last_L_dot;
|
||||
float L = legState_.L0;
|
||||
float L_dot = State_dot_[0][0];
|
||||
|
||||
float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
|
||||
float L_dot2 = (last_L_dot - L_dot) / dot_dt;
|
||||
|
||||
float cos_theta = arm_cos_f32(theta);
|
||||
float sin_theta = arm_sin_f32(theta);
|
||||
float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta_dot * sin_theta
|
||||
+ L * theta_dot2 * sin_theta + L * theta_dot * theta_dot * cos_theta;
|
||||
|
||||
float P = legFeedback_[0][0]*cos_theta + legFeedback_[1][0]*sin_theta/L;
|
||||
|
||||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/1.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_LEG_H
|
||||
#define WHEEL_LEGGED_LEG_H
|
||||
|
||||
#include <matrix.h>
|
||||
#include "controller.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float L0;
|
||||
float phi0;
|
||||
|
||||
float phi1;
|
||||
float phi4;
|
||||
|
||||
float phi2;
|
||||
float phi3;
|
||||
|
||||
uint32_t DWT_CNT;
|
||||
float dt; //用于计算时间并求导
|
||||
float last_phi0;
|
||||
float phi0_dot;
|
||||
|
||||
float L0_dot;
|
||||
|
||||
}Leg_State_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
PID_Init_Config_s length_PID_conf;
|
||||
PID_Init_Config_s phi0_PID_conf;
|
||||
float init_target_L0; //初始腿长目标值
|
||||
float F_feedforward;
|
||||
}Leg_init_config_s;
|
||||
|
||||
class leg
|
||||
{
|
||||
public:
|
||||
leg(Leg_init_config_s* legInitConfig);
|
||||
void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
|
||||
void input_update(float F,float Tp);
|
||||
void feedback_update(float T1_fb,float T2_fb);
|
||||
void calculate_output();
|
||||
Matrixf<4,1> get_state();
|
||||
float calculate_FN(float MotionAccel_z,float theta,float theta_dot);
|
||||
|
||||
float F_feedforward_; //支持力前馈补偿
|
||||
private:
|
||||
Leg_State_t legState_;
|
||||
Matrixf<2,1> legInput_;
|
||||
Matrixf<2,1> legOutput_;
|
||||
Matrixf<2,1> legFeedback_;
|
||||
|
||||
Matrixf<2,1> State_dot_; // L0 和 phi0 的导数
|
||||
|
||||
float target_L0_;
|
||||
float target_phi0_;
|
||||
PIDInstance Length_PID_;
|
||||
PIDInstance Phi0_PID_;
|
||||
|
||||
Matrixf<2,2> VMC_Jacobi_;
|
||||
|
||||
void get_dot(float phi1_dot,float phi4_dot);
|
||||
};
|
||||
|
||||
#endif //WHEEL_LEGGED_LEG_H
|
||||
|
|
@ -2,7 +2,7 @@
|
|||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#include "balance.h"
|
||||
#include "balance_old.h"
|
||||
|
||||
static void mylqr_k(float L0, float K[12]);
|
||||
|
||||
|
|
@ -47,16 +47,7 @@ void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],
|
|||
balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot;
|
||||
|
||||
mylqr_k(L_average,K_matrix);
|
||||
float K_matrix_fix[12];
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K_matrix_fix[i] = K_matrix[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K_matrix_fix[6+i] = K_matrix[2*i+1];
|
||||
}
|
||||
|
||||
|
||||
LQR_set_K(&balanceControl->balance_LQR,K_matrix_fix);
|
||||
LQR_set_K(&balanceControl->balance_LQR,K_matrix);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -116,34 +107,44 @@ void mylqr_k(float L0, float K[12])
|
|||
{
|
||||
float t2;
|
||||
float t3;
|
||||
|
||||
float K_temp[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
t2 = L0 * L0;
|
||||
t3 = t2 * L0;
|
||||
/* 24-Jan-2024 14:05:09 */
|
||||
K[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F;
|
||||
K[1] =
|
||||
K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F;
|
||||
K_temp[1] =
|
||||
((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F;
|
||||
K[2] =
|
||||
K_temp[2] =
|
||||
((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F;
|
||||
K[3] =
|
||||
K_temp[3] =
|
||||
((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F;
|
||||
K[4] =
|
||||
K_temp[4] =
|
||||
((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F;
|
||||
K[5] =
|
||||
K_temp[5] =
|
||||
((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F;
|
||||
K[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) -
|
||||
K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) -
|
||||
10.4576302F;
|
||||
K[7] =
|
||||
K_temp[7] =
|
||||
((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F;
|
||||
K[8] =
|
||||
K_temp[8] =
|
||||
((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F;
|
||||
K[9] =
|
||||
K_temp[9] =
|
||||
((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F;
|
||||
K[10] =
|
||||
K_temp[10] =
|
||||
((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F;
|
||||
K[11] =
|
||||
K_temp[11] =
|
||||
((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F;
|
||||
|
||||
//matlab赋值顺序不同
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[i] = K_temp[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[6+i] = K_temp[2*i+1];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -169,7 +170,7 @@ float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float
|
|||
|
||||
float P = F_fb*cos_theta + Tp_fb*sin_theta/L;
|
||||
|
||||
float F_N = P + 0.8 * (zw_dot2 + 9.8);
|
||||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
|
||||
|
|
@ -2,11 +2,11 @@
|
|||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_BALANCE_H
|
||||
#define WHEEL_LEGGED_BALANCE_H
|
||||
#ifndef WHEEL_LEGGED_BALANCE_OLD_H
|
||||
#define WHEEL_LEGGED_BALANCE_OLD_H
|
||||
|
||||
#include "leg.h"
|
||||
#include "LQR.h"
|
||||
#include "leg_old.h"
|
||||
#include "LQR_old.h"
|
||||
#include "controller.h"
|
||||
#include "kalman_filter.h"
|
||||
typedef struct
|
||||
|
|
@ -40,4 +40,4 @@ void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],
|
|||
void Balance_Control_Loop(Balance_Control_t* balanceControl);
|
||||
void target_x_set(Balance_Control_t* balanceControl,float x);
|
||||
|
||||
#endif //WHEEL_LEGGED_BALANCE_H
|
||||
#endif //WHEEL_LEGGED_BALANCE_OLD_H
|
||||
|
|
@ -22,7 +22,7 @@
|
|||
#include "power_meter.h"
|
||||
#include "ins_task.h"
|
||||
|
||||
#include "balance.h"
|
||||
#include "balance_old.h"
|
||||
#include "observer.h"
|
||||
|
||||
#include "general_def.h"
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
// Created by SJQ on 2023/12/25.
|
||||
//
|
||||
|
||||
#include "leg.h"
|
||||
#include "leg_old.h"
|
||||
#include "math.h"
|
||||
#include "main.h"
|
||||
#include "arm_math.h"
|
||||
|
|
@ -179,56 +179,6 @@ void Leg_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb)
|
|||
legInstance->legFeedback.Tp = F_Tp_array[1];
|
||||
}
|
||||
|
||||
|
||||
////此段代码为matlab生成
|
||||
//static void calculate_leg_pos(const float phi1, const float phi4, float leg_pos[4])
|
||||
//{
|
||||
// float A0;
|
||||
// float B0;
|
||||
// float C0;
|
||||
// float L0;
|
||||
// float phi2;
|
||||
// float scale;
|
||||
// float xc;
|
||||
// float xd;
|
||||
// float yb_tmp;
|
||||
// /* 以机体中点建立坐标系 */
|
||||
// L0 = 0.15F * cosf(phi1);
|
||||
// yb_tmp = 0.15F * sinf(phi1);
|
||||
// xd = 0.15F * cosf(phi4) + 0.075F;
|
||||
// A0 = 0.54F * (xd - (L0 - 0.075F));
|
||||
// C0 = 0.15F * sinf(phi4) - yb_tmp;
|
||||
// B0 = 0.54F * C0;
|
||||
// scale = (0.15F * cosf(phi4) + 0.075F) - (L0 - 0.075F);
|
||||
// C0 = ((scale * scale + C0 * C0) + 0.0729000047F) - 0.0729000047F;
|
||||
// phi2 = 2.0F * atanf((B0 + sqrtf((A0 * A0 + B0 * B0) - C0 * C0)) / (A0 + C0));
|
||||
// xc = L0 + 0.27F * cosf(phi2);
|
||||
// C0 = yb_tmp + 0.27F * sinf(phi2);
|
||||
// scale = 1.29246971E-26F;
|
||||
// A0 = fabsf(xc);
|
||||
// if (A0 > 1.29246971E-26F) {
|
||||
// L0 = 1.0F;
|
||||
// scale = A0;
|
||||
// } else {
|
||||
// B0 = A0 / 1.29246971E-26F;
|
||||
// L0 = B0 * B0;
|
||||
// }
|
||||
// A0 = fabsf(C0);
|
||||
// if (A0 > scale) {
|
||||
// B0 = scale / A0;
|
||||
// L0 = L0 * B0 * B0 + 1.0F;
|
||||
// scale = A0;
|
||||
// } else {
|
||||
// B0 = A0 / scale;
|
||||
// L0 += B0 * B0;
|
||||
// }
|
||||
// L0 = scale * sqrtf(L0);
|
||||
// leg_pos[0] = L0;
|
||||
// leg_pos[1] = atanf(C0 / xc);
|
||||
// leg_pos[2] = phi2;
|
||||
// leg_pos[3] = acosf(xc - xd / 0.27F);
|
||||
//}
|
||||
//
|
||||
void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi3,
|
||||
float phi4, float F, float Tp, float T12[2])
|
||||
{
|
||||
|
|
@ -2,8 +2,8 @@
|
|||
// Created by SJQ on 2023/12/25.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_LEG_H
|
||||
#define WHEEL_LEGGED_LEG_H
|
||||
#ifndef WHEEL_LEGGED_LEG_OLD_H
|
||||
#define WHEEL_LEGGED_LEG_OLD_H
|
||||
#include "controller.h"
|
||||
|
||||
typedef struct
|
||||
|
|
@ -78,4 +78,4 @@ void Leg_length_control_loop(LegInstance* legInstance);
|
|||
void VMC_getT(LegInstance* legInstance);
|
||||
|
||||
|
||||
#endif //WHEEL_LEGGED_LEG_H
|
||||
#endif //WHEEL_LEGGED_LEG_OLD_H
|
||||
|
|
@ -9,7 +9,7 @@
|
|||
#include "bmi088.h"
|
||||
#include "vofa.h"
|
||||
|
||||
static attitude_t *gimba_IMU_data; // 云台IMU数据
|
||||
static INS_t *gimba_IMU_data; // 云台IMU数据
|
||||
static DJIMotorInstance *yaw_motor, *pitch_motor;
|
||||
|
||||
static ECMotorInstance *big_yaw_motor;
|
||||
|
|
|
|||
|
|
@ -196,7 +196,7 @@ typedef struct
|
|||
|
||||
typedef struct
|
||||
{
|
||||
attitude_t gimbal_imu_data;
|
||||
INS_t gimbal_imu_data;
|
||||
uint16_t yaw_motor_single_round_angle;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,24 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/2.
|
||||
//
|
||||
|
||||
#include "LQR.h"
|
||||
|
||||
template <int _state,int _control>
|
||||
void LQR<_state,_control>::set_k(float *K_value)
|
||||
{
|
||||
K_ = Matrixf<_state,_control>((float[_state * _control])K_value);
|
||||
}
|
||||
template <int _state,int _control>
|
||||
void LQR<_state,_control>::update(float* new_state,float* target_state)
|
||||
{
|
||||
state_vec_ = Matrixf<_state,1>((float[_state])new_state);
|
||||
state_cmd_ = Matrixf<_state,1>((float[_state])target_state);
|
||||
|
||||
control_vec_ = K_ * (state_cmd_ - state_vec_);
|
||||
}
|
||||
template <int _state,int _control>
|
||||
Matrixf<_control,1> LQR<_state,_control>::get_control()
|
||||
{
|
||||
return control_vec_;
|
||||
}
|
||||
|
|
@ -1,30 +1,26 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/29.
|
||||
// Created by SJQ on 2024/3/2.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_LQR_H
|
||||
#define WHEEL_LEGGED_LQR_H
|
||||
#include "user_lib.h"
|
||||
typedef struct
|
||||
{
|
||||
float* state_vector;
|
||||
float* control_vector;
|
||||
|
||||
float* state_cmd;
|
||||
#include <matrix.h>
|
||||
template <int _state,int _control>
|
||||
class LQR {
|
||||
public:
|
||||
LQR(){};
|
||||
void update(float* new_state,float* target_state);
|
||||
void set_k(float *K_value);
|
||||
Matrixf<_control,1> get_control();
|
||||
private:
|
||||
Matrixf<_state,1> state_vec_;
|
||||
|
||||
int state_num; //状态量维数
|
||||
int control_num; //控制量维数
|
||||
Matrixf<_state,1> state_cmd_;
|
||||
Matrixf<_control,1> control_vec_;
|
||||
|
||||
mat control_matrix; //其实是向量 定义成矩阵方便计算 @todo：更新DSP库 新版本库支持矩阵直接乘数组
|
||||
mat state_matrix;
|
||||
mat state_cmd_matrix;
|
||||
Matrixf<_control,_state> K_;
|
||||
};
|
||||
|
||||
mat state_err;
|
||||
mat K_matrix;
|
||||
}LQRInstance;
|
||||
|
||||
void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt);
|
||||
void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state);
|
||||
void LQR_set_K(LQRInstance* lqrInstance, float *K_value);
|
||||
|
||||
#endif //WHEEL_LEGGED_LQR_H
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
// Created by SJQ on 2023/12/29.
|
||||
//
|
||||
|
||||
#include "LQR.h"
|
||||
#include "LQR_old.h"
|
||||
|
||||
void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt)
|
||||
{
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/29.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_LQR_OLD_H
|
||||
#define WHEEL_LEGGED_LQR_OLD_H
|
||||
#include "user_lib.h"
|
||||
typedef struct
|
||||
{
|
||||
float* state_vector;
|
||||
float* control_vector;
|
||||
|
||||
float* state_cmd;
|
||||
|
||||
int state_num; //状态量维数
|
||||
int control_num; //控制量维数
|
||||
|
||||
mat control_matrix; //其实是向量 定义成矩阵方便计算 @todo：更新DSP库 新版本库支持矩阵直接乘数组
|
||||
mat state_matrix;
|
||||
mat state_cmd_matrix;
|
||||
|
||||
mat state_err;
|
||||
mat K_matrix;
|
||||
}LQRInstance;
|
||||
|
||||
void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt);
|
||||
void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state);
|
||||
void LQR_set_K(LQRInstance* lqrInstance, float *K_value);
|
||||
|
||||
#endif //WHEEL_LEGGED_LQR_OLD_H
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @file matrix.cpp/h
|
||||
* @brief Matrix/vector calculation. 矩阵/向量运算
|
||||
* @author Spoon Guan
|
||||
******************************************************************************
|
||||
* Copyright (c) 2023 Team JiaoLong-SJTU
|
||||
* All rights reserved.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#include "matrix.h"
|
||||
|
||||
// hat of vector
|
||||
Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) {
|
||||
float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0,
|
||||
-vec[0][0], -vec[1][0], vec[0][0], 0};
|
||||
return Matrixf<3, 3>(hat);
|
||||
}
|
||||
|
||||
// cross product
|
||||
Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) {
|
||||
return vector3f::hat(vec1) * vec2;
|
||||
}
|
||||
|
|
@ -0,0 +1,259 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @file matrix.cpp/h
|
||||
* @brief Matrix/vector calculation. 矩阵/向量运算
|
||||
* @author Spoon Guan
|
||||
******************************************************************************
|
||||
* Copyright (c) 2023 Team JiaoLong-SJTU
|
||||
* All rights reserved.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#ifndef MATRIX_H
|
||||
#define MATRIX_H
|
||||
#include "stm32f4xx.h"
|
||||
#include "arm_math.h"
|
||||
|
||||
// Matrix class
|
||||
template <int _rows, int _cols>
|
||||
class Matrixf {
|
||||
public:
|
||||
// Constructor without input data
|
||||
Matrixf(void) : rows_(_rows), cols_(_cols) {
|
||||
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
|
||||
}
|
||||
// Constructor with input data
|
||||
Matrixf(float data[_rows * _cols]) : Matrixf() {
|
||||
memcpy(this->data_, data, _rows * _cols * sizeof(float));
|
||||
}
|
||||
// Copy constructor
|
||||
Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() {
|
||||
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
|
||||
}
|
||||
// Destructor
|
||||
~Matrixf(void) {}
|
||||
|
||||
// Row size
|
||||
int rows(void) { return _rows; }
|
||||
// Column size
|
||||
int cols(void) { return _cols; }
|
||||
|
||||
// Element
|
||||
float* operator[](const int& row) { return &this->data_[row * _cols]; }
|
||||
|
||||
// Operators
|
||||
Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) {
|
||||
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
|
||||
return *this;
|
||||
}
|
||||
Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) {
|
||||
arm_status s;
|
||||
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) {
|
||||
arm_status s;
|
||||
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
Matrixf<_rows, _cols>& operator*=(const float& val) {
|
||||
arm_status s;
|
||||
s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
Matrixf<_rows, _cols>& operator/=(const float& val) {
|
||||
arm_status s;
|
||||
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_);
|
||||
return *this;
|
||||
}
|
||||
Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) {
|
||||
arm_status s;
|
||||
Matrixf<_rows, _cols> res;
|
||||
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) {
|
||||
arm_status s;
|
||||
Matrixf<_rows, _cols> res;
|
||||
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
Matrixf<_rows, _cols> operator*(const float& val) {
|
||||
arm_status s;
|
||||
Matrixf<_rows, _cols> res;
|
||||
s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
friend Matrixf<_rows, _cols> operator*(const float& val,
|
||||
const Matrixf<_rows, _cols>& mat) {
|
||||
arm_status s;
|
||||
Matrixf<_rows, _cols> res;
|
||||
s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
Matrixf<_rows, _cols> operator/(const float& val) {
|
||||
arm_status s;
|
||||
Matrixf<_rows, _cols> res;
|
||||
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
// Matrix multiplication
|
||||
template <int cols>
|
||||
friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1,
|
||||
const Matrixf<_cols, cols>& mat2) {
|
||||
arm_status s;
|
||||
Matrixf<_rows, cols> res;
|
||||
s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
|
||||
// Submatrix
|
||||
template <int rows, int cols>
|
||||
Matrixf<rows, cols> block(const int& start_row, const int& start_col) {
|
||||
Matrixf<rows, cols> res;
|
||||
for (int row = start_row; row < start_row + rows; row++) {
|
||||
memcpy((float*)res[0] + (row - start_row) * cols,
|
||||
(float*)this->data_ + row * _cols + start_col,
|
||||
cols * sizeof(float));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
// Specific row
|
||||
Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); }
|
||||
// Specific column
|
||||
Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); }
|
||||
|
||||
// Transpose
|
||||
Matrixf<_cols, _rows> trans(void) {
|
||||
Matrixf<_cols, _rows> res;
|
||||
arm_mat_trans_f32(&arm_mat_, &res.arm_mat_);
|
||||
return res;
|
||||
}
|
||||
// Trace
|
||||
float trace(void) {
|
||||
float res = 0;
|
||||
for (int i = 0; i < fmin(_rows, _cols); i++) {
|
||||
res += (*this)[i][i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
// Norm
|
||||
float norm(void) { return sqrtf((this->trans() * *this)[0][0]); }
|
||||
|
||||
public:
|
||||
// arm matrix instance
|
||||
arm_matrix_instance_f32 arm_mat_;
|
||||
|
||||
protected:
|
||||
// size
|
||||
int rows_, cols_;
|
||||
// data
|
||||
float data_[_rows * _cols];
|
||||
};
|
||||
|
||||
// Matrix funtions
|
||||
namespace matrixf {
|
||||
|
||||
// Special Matrices
|
||||
// Zero matrix
|
||||
template <int _rows, int _cols>
|
||||
Matrixf<_rows, _cols> zeros(void) {
|
||||
float data[_rows * _cols] = {0};
|
||||
return Matrixf<_rows, _cols>(data);
|
||||
}
|
||||
// Ones matrix
|
||||
template <int _rows, int _cols>
|
||||
Matrixf<_rows, _cols> ones(void) {
|
||||
float data[_rows * _cols] = {0};
|
||||
for (int i = 0; i < _rows * _cols; i++) {
|
||||
data[i] = 1;
|
||||
}
|
||||
return Matrixf<_rows, _cols>(data);
|
||||
}
|
||||
// Identity matrix
|
||||
template <int _rows, int _cols>
|
||||
Matrixf<_rows, _cols> eye(void) {
|
||||
float data[_rows * _cols] = {0};
|
||||
for (int i = 0; i < fmin(_rows, _cols); i++) {
|
||||
data[i * _cols + i] = 1;
|
||||
}
|
||||
return Matrixf<_rows, _cols>(data);
|
||||
}
|
||||
// Diagonal matrix
|
||||
template <int _rows, int _cols>
|
||||
Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) {
|
||||
Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>();
|
||||
for (int i = 0; i < fmin(_rows, _cols); i++) {
|
||||
res[i][i] = vec[i][0];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
// Inverse
|
||||
template <int _dim>
|
||||
Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) {
|
||||
arm_status s;
|
||||
// extended matrix [A|I]
|
||||
Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>();
|
||||
for (int i = 0; i < _dim; i++) {
|
||||
memcpy(ext_mat[i], mat[i], _dim * sizeof(float));
|
||||
ext_mat[i][_dim + i] = 1;
|
||||
}
|
||||
// elimination
|
||||
for (int i = 0; i < _dim; i++) {
|
||||
// find maximum absolute value in the first column in lower right block
|
||||
float abs_max = fabs(ext_mat[i][i]);
|
||||
int abs_max_row = i;
|
||||
for (int row = i; row < _dim; row++) {
|
||||
if (abs_max < fabs(ext_mat[row][i])) {
|
||||
abs_max = fabs(ext_mat[row][i]);
|
||||
abs_max_row = row;
|
||||
}
|
||||
}
|
||||
if (abs_max < 1e-12f) { // singular
|
||||
return matrixf::zeros<_dim, _dim>();
|
||||
s = ARM_MATH_SINGULAR;
|
||||
}
|
||||
if (abs_max_row != i) { // row exchange
|
||||
float tmp;
|
||||
Matrixf<1, 2 * _dim> row_i = ext_mat.row(i);
|
||||
Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row);
|
||||
memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float));
|
||||
memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float));
|
||||
}
|
||||
float k = 1.f / ext_mat[i][i];
|
||||
for (int col = i; col < 2 * _dim; col++) {
|
||||
ext_mat[i][col] *= k;
|
||||
}
|
||||
for (int row = 0; row < _dim; row++) {
|
||||
if (row == i) {
|
||||
continue;
|
||||
}
|
||||
k = ext_mat[row][i];
|
||||
for (int j = i; j < 2 * _dim; j++) {
|
||||
ext_mat[row][j] -= k * ext_mat[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
// inv = ext_mat(:,n+1:2n)
|
||||
s = ARM_MATH_SUCCESS;
|
||||
Matrixf<_dim, _dim> res;
|
||||
for (int i = 0; i < _dim; i++) {
|
||||
memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
} // namespace matrixf
|
||||
|
||||
namespace vector3f {
|
||||
|
||||
// hat of vector
|
||||
Matrixf<3, 3> hat(Matrixf<3, 1> vec);
|
||||
|
||||
// cross product
|
||||
Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2);
|
||||
|
||||
} // namespace vector3f
|
||||
|
||||
#endif // MATRIX_H
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @file utils.cpp/h
|
||||
* @brief General math utils. 常用数学工具函数
|
||||
* @author Spoon Guan
|
||||
******************************************************************************
|
||||
* Copyright (c) 2023 Team JiaoLong-SJTU
|
||||
* All rights reserved.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#include "utils.h"
|
||||
|
||||
float math::limit(float val, const float& min, const float& max) {
|
||||
if (min > max)
|
||||
return val;
|
||||
else if (val < min)
|
||||
val = min;
|
||||
else if (val > max)
|
||||
val = max;
|
||||
return val;
|
||||
}
|
||||
|
||||
float math::limitMin(float val, const float& min) {
|
||||
if (val < min)
|
||||
val = min;
|
||||
return val;
|
||||
}
|
||||
|
||||
float math::limitMax(float val, const float& max) {
|
||||
if (val > max)
|
||||
val = max;
|
||||
return val;
|
||||
}
|
||||
|
||||
float math::loopLimit(float val, const float& min, const float& max) {
|
||||
if (min >= max)
|
||||
return val;
|
||||
if (val > max) {
|
||||
while (val > max)
|
||||
val -= (max - min);
|
||||
} else if (val < min) {
|
||||
while (val < min)
|
||||
val += (max - min);
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
float math::sign(const float& val) {
|
||||
if (val > 0)
|
||||
return 1;
|
||||
else if (val < 0)
|
||||
return -1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
/**
|
||||
******************************************************************************
|
||||
* @file utils.cpp/h
|
||||
* @brief General math utils. 常用数学工具函数
|
||||
* @author Spoon Guan
|
||||
******************************************************************************
|
||||
* Copyright (c) 2023 Team JiaoLong-SJTU
|
||||
* All rights reserved.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#ifndef UTILS_H
|
||||
#define UTILS_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
namespace math {
|
||||
|
||||
float limit(float val, const float& min, const float& max);
|
||||
float limitMin(float val, const float& min);
|
||||
float limitMax(float val, const float& max);
|
||||
float loopLimit(float val, const float& min, const float& max);
|
||||
float sign(const float& val);
|
||||
|
||||
} // namespace math
|
||||
|
||||
#endif // UTILS_H
|
||||
Loading…
Reference in New Issue