85 lines
2.9 KiB
C
85 lines
2.9 KiB
C
/*
|
||
运动学计算
|
||
*/
|
||
|
||
#include "filter.h"
|
||
|
||
#include "kinematics.h"
|
||
|
||
|
||
int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero){
|
||
real->Pos = (in->Pos-joint_zero) / ratio;
|
||
real->T = in->T * ratio;
|
||
real->W = in->W /ratio;
|
||
return 0;
|
||
}
|
||
|
||
int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero){
|
||
out->Pos = real->Pos * ratio + joint_zero;
|
||
out->T = real->T / ratio;
|
||
out->W = real->W * ratio;
|
||
out->K_P = real->K_P;
|
||
out->K_W = real->K_W;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 单腿正运动学
|
||
* @param theta[3] 关节角度数组（单位：弧度）：[hip, thigh, calf]
|
||
* @param leg_params 单腿机械参数（长度）
|
||
* @param foot_out 足端位置输出 [x, y, z]（单位：米）
|
||
*/
|
||
// void Leg_ForwardKinematics(const float theta[3], const joint_params *leg_params, float foot_out[3]) {
|
||
// float l1 = leg_params->named.lf_hip; // 髋关节到大腿长度
|
||
// float l2 = leg_params->named.lf_thigh; // 大腿长度
|
||
// float l3 = leg_params->named.lf_calf; // 小腿长度
|
||
|
||
// float t1 = theta[0]; // 髋关节侧摆（绕y轴）
|
||
// float t2 = theta[1]; // 髋关节前后
|
||
// float t3 = theta[2]; // 膝关节
|
||
|
||
// // 末端位置
|
||
// float leg_proj = l2 * cosf(t2) + l3 * cosf(t2 + t3);
|
||
// foot_out[0] = cosf(t1) * leg_proj + l1 * cosf(t1); // x
|
||
// foot_out[1] = sinf(t1) * leg_proj + l1 * sinf(t1); // y
|
||
// foot_out[2] = l2 * sinf(t2) + l3 * sinf(t2 + t3); // z
|
||
// }
|
||
|
||
/**
|
||
* @brief 单腿逆运动学
|
||
* @param foot_in 足端目标位置 [x, y, z]（单位：米）
|
||
* @param leg_params 单腿机械参数（长度）
|
||
* @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
|
||
* @return 0成功，-1目标不可达
|
||
*/
|
||
// int Leg_InverseKinematics(const float foot_in[3], const joint_params *leg_params, float theta_out[3]) {
|
||
// float l1 = leg_params->named.lf_hip;
|
||
// float l2 = leg_params->named.lf_thigh;
|
||
// float l3 = leg_params->named.lf_calf;
|
||
|
||
// float x = foot_in[0];
|
||
// float y = foot_in[1];
|
||
// float z = foot_in[2];
|
||
|
||
// // 1. 先算髋关节侧摆角（绕y轴）
|
||
// float r = sqrtf(x * x + y * y);
|
||
// float t1 = atan2f(y, x); // 髋关节侧摆角：使用 atan2f，计
|
||
|
||
// // 2. 去掉髋关节侧摆影响，得到在sagittal平面上的投影
|
||
// float xp = r - l1;
|
||
// float zp = z;
|
||
|
||
// // 3. 逆解余下两个关节
|
||
// float D = (xp * xp + zp * zp - l2 * l2 - l3 * l3) / (2 * l2 * l3);
|
||
// if (D < -1.0f || D > 1.0f) return -1; // 不可达
|
||
|
||
// float t3 = atan2f(-sqrtf(1 - D * D), D); // 膝关节
|
||
// float k1 = l2 + l3 * cosf(t3);
|
||
// float k2 = l3 * sinf(t3);
|
||
// float t2 = atan2f(zp, xp) - atan2f(k2, k1);
|
||
|
||
// theta_out[0] = t1;
|
||
// theta_out[1] = t2;
|
||
// theta_out[2] = t3;
|
||
// return 0;
|
||
// }
|