114 lines
3.3 KiB
C
114 lines
3.3 KiB
C
/*
|
||
运动学计算
|
||
*/
|
||
|
||
#include "filter.h"
|
||
#include <math.h>
|
||
#include "kinematics.h"
|
||
|
||
static float q1_ik(float py, float pz, float l1) {
|
||
float q1;
|
||
float L = sqrtf(py*py + pz*pz - l1*l1);
|
||
q1 = atan2f(pz*l1 + py*L, py*l1 - pz*L);
|
||
return q1;
|
||
}
|
||
|
||
static float q3_ik(float b3z, float b4z, float b) {
|
||
float q3, temp;
|
||
temp = (b3z*b3z + b4z*b4z - b*b) / (2.0f * fabsf(b3z * b4z));
|
||
if (temp > 1.0f) temp = 1.0f;
|
||
if (temp < -1.0f) temp = -1.0f;
|
||
q3 = acosf(temp);
|
||
q3 = -(M_PI - q3);
|
||
return q3;
|
||
}
|
||
|
||
static float q2_ik(float q1, float q3, float px, float py, float pz, float b3z, float b4z) {
|
||
float a1 = py * sinf(q1) - pz * cosf(q1);
|
||
float a2 = px;
|
||
float m1 = b4z * sinf(q3);
|
||
float m2 = b3z + b4z * cosf(q3);
|
||
float q2 = atan2f(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1);
|
||
return q2;
|
||
}
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3], Kinematics_Sign_t *sign) {
|
||
float l1 = leg_params->hip * sign->hip;
|
||
float l2 = -leg_params->thigh;
|
||
float l3 = -leg_params->calf;
|
||
|
||
float q1 = theta_in[0];
|
||
float q2 = theta_in[1] * sign->thigh;
|
||
float q3 = theta_in[2] * sign->calf;
|
||
|
||
float s1 = sinf(q1);
|
||
float s2 = sinf(q2);
|
||
float s3 = sinf(q3);
|
||
|
||
float c1 = cosf(q1);
|
||
float c2 = cosf(q2);
|
||
float c3 = cosf(q3);
|
||
|
||
float c23 = c2 * c3 - s2 * s3;
|
||
float s23 = s2 * c3 + c2 * s3;
|
||
|
||
foot_out[0] = l3 * s23 + l2 * s2;
|
||
foot_out[1] = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
|
||
foot_out[2] = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
|
||
}
|
||
|
||
/**
|
||
* @brief 单腿逆运动学
|
||
* @param foot_in 足端目标位置 [x, y, z]（单位：米）
|
||
* @param leg_params 单腿机械参数（长度）
|
||
* @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
|
||
* @return 0成功，-1目标不可达
|
||
*/
|
||
int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign) {
|
||
float px = foot_in[0];
|
||
float py = -foot_in[1];
|
||
float pz = foot_in[2];
|
||
|
||
float b2y = leg_params->hip * sign->hip;
|
||
float b3z = -leg_params->thigh;
|
||
float b4z = -leg_params->calf;
|
||
float a = leg_params->hip;
|
||
float c = sqrtf(px*px + py*py + pz*pz);
|
||
float b = sqrtf(c*c - a*a);
|
||
|
||
// 检查可达性
|
||
if (b != b) return -1; // NaN
|
||
if (b > fabsf(b3z) + fabsf(b4z)) return -1;
|
||
|
||
float q1 = q1_ik(py, pz, b2y);
|
||
float q3 = q3_ik(b3z, b4z, b);
|
||
float q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
|
||
|
||
// theta_out[0] = q1;
|
||
if (sign->hip == sign->thigh){
|
||
theta_out[0] = q1;
|
||
} else {
|
||
theta_out[0] = -q1;
|
||
}
|
||
theta_out[1] = q2 * sign->thigh;
|
||
theta_out[2] = q3 * sign->calf;
|
||
return 0;
|
||
}
|