改vmc
This commit is contained in:
parent
b1300f0d7d
commit
62ed731cd2
@ -35,6 +35,7 @@
|
||||
#define VMC_SAFE_SQRT(x) (((x) > 0) ? sqrtf(x) : 0.0f)
|
||||
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
static bool inited = false;
|
||||
/* Private function prototypes ---------------------------------------------- */
|
||||
|
||||
static int8_t VMC_ValidateParams(const VMC_Param_t *param);
|
||||
@ -61,7 +62,7 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
|
||||
// 重置状态
|
||||
VMC_Reset(vmc);
|
||||
|
||||
vmc->initialized = true;
|
||||
inited = true;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -77,7 +78,7 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
|
||||
* - 角度: 顺时针为正
|
||||
*/
|
||||
int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, float body_pitch_rate) {
|
||||
if (vmc == NULL || !vmc->initialized) {
|
||||
if (vmc == NULL || !inited) {
|
||||
return -1;
|
||||
}
|
||||
body_pitch = -body_pitch; // 机体俯仰角取反
|
||||
@ -111,8 +112,8 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
|
||||
leg->phi0 = atan2f(leg->foot_y, leg->foot_x);
|
||||
|
||||
// 计算等效摆动杆角度(相对于机体坐标系)
|
||||
leg->alpha = VMC_PI_2 - leg->phi0;
|
||||
leg->theta = VMC_PI - (VMC_PI_2 + body_pitch - leg->phi0);
|
||||
leg->alpha = M_PI_2 - leg->phi0;
|
||||
leg->theta = M_PI - (M_PI_2 + body_pitch - leg->phi0);
|
||||
|
||||
// 角度归一化
|
||||
VMC_ANGLE_NORMALIZE(leg->theta);
|
||||
@ -124,9 +125,6 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
|
||||
leg->d_theta = body_pitch_rate + leg->d_phi0;
|
||||
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
|
||||
|
||||
// 计算角加速度
|
||||
leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
|
||||
|
||||
VMC_GroundContactDetection(vmc); // 更新地面接触状态
|
||||
return 0;
|
||||
}
|
||||
@ -137,7 +135,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
|
||||
* 根据期望的虚拟力和力矩，通过雅可比矩阵计算关节力矩
|
||||
*/
|
||||
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
|
||||
if (vmc == NULL || !vmc->initialized) {
|
||||
if (vmc == NULL || !inited) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -166,7 +164,7 @@ int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual) {
|
||||
* 基于虚拟力和腿部状态估计地面法向力
|
||||
*/
|
||||
float VMC_GroundContactDetection(VMC_t *vmc) {
|
||||
if (vmc == NULL || !vmc->initialized) {
|
||||
if (vmc == NULL || !inited) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
@ -188,7 +186,7 @@ float VMC_GroundContactDetection(VMC_t *vmc) {
|
||||
* @brief 获取足端位置
|
||||
*/
|
||||
int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
|
||||
if (vmc == NULL || !vmc->initialized || x == NULL || y == NULL) {
|
||||
if (vmc == NULL || !inited || x == NULL || y == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -202,7 +200,7 @@ int8_t VMC_GetFootPosition(const VMC_t *vmc, float *x, float *y) {
|
||||
* @brief 获取等效摆动杆参数
|
||||
*/
|
||||
int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, float *d_length, float *d_angle) {
|
||||
if (vmc == NULL || !vmc->initialized) {
|
||||
if (vmc == NULL || !inited) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -218,7 +216,7 @@ int8_t VMC_GetVirtualLegState(const VMC_t *vmc, float *length, float *angle, flo
|
||||
* @brief 获取关节输出力矩
|
||||
*/
|
||||
int8_t VMC_GetJointTorques(const VMC_t *vmc, float *tau_front, float *tau_rear) {
|
||||
if (vmc == NULL || !vmc->initialized || tau_front == NULL || tau_rear == NULL) {
|
||||
if (vmc == NULL || !inited || tau_front == NULL || tau_rear == NULL) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -248,7 +246,7 @@ void VMC_Reset(VMC_t *vmc) {
|
||||
* @brief 设置虚拟力和力矩
|
||||
*/
|
||||
void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
|
||||
if (vmc == NULL || !vmc->initialized) {
|
||||
if (vmc == NULL || !inited) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -262,7 +260,7 @@ void VMC_SetVirtualForces(VMC_t *vmc, float F_virtual, float T_virtual) {
|
||||
* 根据当前关节配置计算从虚拟力到关节力矩的雅可比矩阵
|
||||
*/
|
||||
int8_t VMC_ComputeJacobian(VMC_t *vmc) {
|
||||
if (vmc == NULL || !vmc->initialized) {
|
||||
if (vmc == NULL || !inited) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -301,7 +299,7 @@ int8_t VMC_ComputeJacobian(VMC_t *vmc) {
|
||||
*/
|
||||
static int8_t VMC_ValidateParams(const VMC_Param_t *param) {
|
||||
if (param->hip_length <= 0 || param->leg_1 <= 0 || param->leg_2 <= 0 ||
|
||||
param->leg_3 <= 0 || param->leg_4 <= 0 || param->wheel_radius <= 0) {
|
||||
param->leg_3 <= 0 || param->leg_4 <= 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
@ -8,12 +8,14 @@ extern "C" {
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include "kinematics.h"
|
||||
#include "user_math.h"
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief VMC虚拟模型控制参数结构体
|
||||
*
|
||||
*/
|
||||
typedef struct {
|
||||
float hip_length; // 髋关节间距
|
||||
@ -21,8 +23,6 @@ typedef struct {
|
||||
float leg_2; // 大腿后端长度 (L2)
|
||||
float leg_3; // 小腿长度 (L3)
|
||||
float leg_4; // 小腿前端长度 (L4)
|
||||
float wheel_radius; // 轮子半径
|
||||
float wheel_mass; // 轮子质量
|
||||
} VMC_Param_t;
|
||||
|
||||
/**
|
||||
@ -44,8 +44,7 @@ typedef struct {
|
||||
float d_L0; // 等效摆动杆长度变化率
|
||||
float theta; // 等效摆动杆角度
|
||||
float d_theta; // 等效摆动杆角速度
|
||||
float dd_theta; // 等效摆动杆角加速度
|
||||
|
||||
|
||||
// 虚拟力和力矩
|
||||
float F_virtual; // 虚拟支撑力
|
||||
float T_virtual; // 虚拟摆动力矩
|
||||
@ -84,23 +83,16 @@ typedef struct {
|
||||
VMC_Param_t param; // VMC参数
|
||||
VMC_Leg_t leg; // 腿部状态
|
||||
float dt; // 控制周期
|
||||
bool initialized; // 初始化标志
|
||||
} VMC_t;
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
|
||||
#define VMC_PI_2 (1.5707963267948966f)
|
||||
#define VMC_PI (3.1415926535897932f)
|
||||
#define VMC_2PI (6.2831853071795865f)
|
||||
|
||||
/* Exported macros ---------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 角度范围限制到[-PI, PI]
|
||||
*/
|
||||
#define VMC_ANGLE_NORMALIZE(angle) do { \
|
||||
while((angle) > VMC_PI) (angle) -= VMC_2PI; \
|
||||
while((angle) < -VMC_PI) (angle) += VMC_2PI; \
|
||||
while((angle) > M_PI) (angle) -= M_2PI; \
|
||||
while((angle) < -M_PI) (angle) += M_2PI; \
|
||||
} while(0)
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
@ -109,7 +101,7 @@ typedef struct {
|
||||
* @brief 初始化VMC控制器
|
||||
* @param vmc VMC控制器实例
|
||||
* @param param VMC参数
|
||||
* @param sample_freq 采样频率 (Hz)
|
||||
* @param sample_freq 控制频率 (Hz)
|
||||
* @return 0:成功, -1:失败
|
||||
*/
|
||||
int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq);
|
||||
@ -135,7 +127,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
|
||||
int8_t VMC_InverseSolve(VMC_t *vmc, float F_virtual, float T_virtual);
|
||||
|
||||
/**
|
||||
* @brief 地面接触检测
|
||||
* @brief 离地检测
|
||||
* @param vmc VMC控制器实例
|
||||
* @return 地面法向力 (N)
|
||||
*/
|
||||
|
||||
Loading…
Reference in New Issue
Block a user