156 lines
6.1 KiB
C++
156 lines
6.1 KiB
C++
//
|
|
// 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;
|
|
}
|
|
|
|
|
|
|
|
|
|
|