88 lines
2.5 KiB
C
88 lines
2.5 KiB
C
/*
|
||
运动学计算
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include "user_math.h"
|
||
|
||
typedef struct {
|
||
float T; // 当前实际电机输出力矩(电机本身的力矩)(Nm)
|
||
float W; // 当前实际电机速度(电机本身的速度)(rad/s)
|
||
float Pos; // 当前电机位置(rad)
|
||
} Kinematics_JointData_t;
|
||
|
||
typedef struct {
|
||
float T; // 期望关节的输出力矩(电机本身的力矩)(Nm)
|
||
float W; // 期望关节速度(电机本身的速度)(rad/s)
|
||
float Pos; // 期望关节位置(rad)
|
||
float K_P; // 关节刚度系数(0-25.599)
|
||
float K_W; // 关节速度系数(0-25.599)
|
||
} Kinematics_JointCMD_t;
|
||
|
||
typedef struct {
|
||
float hip; // 髋关节长度(m)
|
||
float thigh; // 大腿长度(m)
|
||
float calf; // 小腿长度(m)
|
||
} Kinematics_LinkLength_t;
|
||
|
||
typedef struct {
|
||
float x;
|
||
float y;
|
||
float z;
|
||
} Kinematics_LegOffset_t;
|
||
|
||
typedef struct {
|
||
float hip;
|
||
float thigh;
|
||
float calf;
|
||
} Kinematics_Sign_t;
|
||
|
||
|
||
/**
|
||
* @brief 计算实际反馈
|
||
* @param real 减速后关节数据
|
||
* @param in 输入关节数据
|
||
* @param ratio 减速比
|
||
* @return
|
||
*/
|
||
int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero);
|
||
|
||
/**
|
||
* @brief 计算实际输出
|
||
* @param real 减速后关节输出
|
||
* @param out 实际输出关节数据
|
||
* @param ratio 减速比
|
||
* @return
|
||
*/
|
||
int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero);
|
||
|
||
|
||
/**
|
||
* @brief 单腿正运动学
|
||
* @param theta_in 关节角度输入数组（单位：弧度）：[hip, thigh, calf]
|
||
* @param leg_params 单腿机械参数（长度）
|
||
* @param foot_out 足端位置输出 [x, y, z]（单位：米）
|
||
* @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
|
||
*/
|
||
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3], Kinematics_Sign_t *sign);
|
||
|
||
/**
|
||
* @brief 单腿逆运动学
|
||
* @param foot_in 足端目标位置 [x, y, z]（单位：米）
|
||
* @param leg_params 单腿机械参数（长度）
|
||
* @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
|
||
* @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
|
||
* @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);
|
||
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|