113 lines
3.5 KiB
C
113 lines
3.5 KiB
C
/*
|
||
一些运动学相关的计算函数
|
||
*/
|
||
|
||
#include "component/kinematics.h"
|
||
#include "component/user_math.h"
|
||
|
||
/*串联腿单腿转摆动杆正运动学*/
|
||
/*大腿小腿均按照水平为0点，向下为正方向*/
|
||
|
||
/**
|
||
* @brief 串联腿单腿转摆动杆正运动学
|
||
* @param param 腿部长度参数
|
||
* @param hip_angle 髋关节角度(弧度)
|
||
* @param knee_angle 膝关节角度(弧度)
|
||
* @param angle 计算到的等效摆动杆角度
|
||
* @param heigh 计算到的等效摆动杆高度
|
||
* @return
|
||
*/
|
||
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) {
|
||
if (param == NULL || hip_angle == NULL || knee_angle == NULL || angle == NULL || height == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
|
||
float L1 = param->thigh_length;
|
||
float L2 = param->calf_length;
|
||
float q1 = *hip_angle; // 髋关节角度
|
||
float q2 = *knee_angle; // 膝关节角度
|
||
|
||
float q4 = (M_PI - q1 - q2)/2;
|
||
|
||
// 使用余弦定理求解等效摆动杆长度
|
||
// L2*L2 = L1*L1 + L3*L3 - 2*L1*L3*cosf(q4);
|
||
// 整理得：L3*L3 - 2*L1*cosf(q4)*L3 + (L1*L1 - L2*L2) = 0
|
||
|
||
float a = 1.0f;
|
||
float b = -2.0f * L1 * cosf(q4);
|
||
float c = L1*L1 - L2*L2;
|
||
|
||
float discriminant = b*b - 4*a*c;
|
||
|
||
// 检查判别式，确保有实数解
|
||
if (discriminant < 0) {
|
||
return -2; // 无实数解，配置不可达
|
||
}
|
||
|
||
float sqrt_discriminant = sqrtf(discriminant);
|
||
float L3_1 = (-b + sqrt_discriminant) / (2*a);
|
||
float L3_2 = (-b - sqrt_discriminant) / (2*a);
|
||
|
||
// 选择正的解（物理意义上的长度）
|
||
float L3;
|
||
if (L3_1 > 0 && L3_2 > 0) {
|
||
// 两个正解，选择较小的（通常对应机构的正常工作姿态）
|
||
L3 = (L3_1 < L3_2) ? L3_1 : L3_2;
|
||
} else if (L3_1 > 0) {
|
||
L3 = L3_1;
|
||
} else if (L3_2 > 0) {
|
||
L3 = L3_2;
|
||
} else {
|
||
return -3; // 无正解
|
||
}
|
||
|
||
*angle = q1 + q4;
|
||
*height = L3;
|
||
|
||
return 0; // 成功
|
||
}
|
||
|
||
/*串联腿单腿转摆动杆逆运动学*/
|
||
/*大腿小腿均按照水平为0点，向下为正方向*/
|
||
/**
|
||
* @brief 串联腿单腿转摆动杆逆运动学
|
||
* @param param 腿部长度参数
|
||
* @param angle 输入的等效摆动杆角度
|
||
* @param height 输入的等效摆动杆高度
|
||
* @param hip_angle 计算到的髋关节角度(弧度)
|
||
* @param knee_angle 计算到的膝关节角度(弧度)
|
||
* @return
|
||
*/
|
||
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) {
|
||
if (param == NULL || angle == NULL || height == NULL || hip_angle == NULL || knee_angle == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
float L1 = param->thigh_length;
|
||
float L2 = param->calf_length;
|
||
float q = *angle; // 摆动杆角度
|
||
float h = *height; // 摆动杆长度
|
||
|
||
// 由正解可知：q = q1 + q4，h = L3
|
||
// 由余弦定理：L2^2 = L1^2 + h^2 - 2*L1*h*cos(q4)
|
||
// 整理得：cos(q4) = (L1^2 + h^2 - L2^2) / (2*L1*h)
|
||
float cos_q4 = (L1*L1 + h*h - L2*L2) / (2.0f * L1 * h);
|
||
|
||
// 检查 cos_q4 是否在 [-1, 1] 范围内
|
||
if (cos_q4 < -1.0f || cos_q4 > 1.0f) {
|
||
return -2; // 不可达
|
||
}
|
||
|
||
float q4 = acosf(cos_q4);
|
||
|
||
// 由正解：q = q1 + q4，且 q4 = (PI - q1 - q2)/2
|
||
// 整理得：q1 = q - q4
|
||
float q1 = q - q4;
|
||
|
||
// 再由 q4 = (PI - q1 - q2)/2，整理得：q2 = PI - 2*q4 - q1
|
||
float q2 = M_PI - 2.0f * q4 - q1;
|
||
|
||
*hip_angle = q1;
|
||
*knee_angle = q2;
|
||
|
||
return 0; // 成功
|
||
} |