128 lines
4.6 KiB
C
128 lines
4.6 KiB
C
/**
|
||
******************************************************************************
|
||
* @file arm.h
|
||
* @brief 6-DOF robotic arm kinematics control
|
||
******************************************************************************
|
||
*/
|
||
|
||
#ifndef ARM_H
|
||
#define ARM_H
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include <stdint.h>
|
||
|
||
/* 机器人尺寸参数结构体 */
|
||
typedef struct {
|
||
// 6个运动关节的DH参数
|
||
float theta;
|
||
float d;
|
||
float a;
|
||
float alpha;
|
||
float theta_offset;
|
||
float qmin;
|
||
float qmax;
|
||
float m; // 连杆质量 (kg)
|
||
float rc[3]; // 质心在 DH 连杆坐标系下的坐标 (m): {x, y, z}
|
||
// 由 URDF inertial/origin 经 Rz(-theta_offset) 旋转得到
|
||
} Arm6dof_DHParams_t;
|
||
typedef struct {
|
||
// MIT控制参数（可选，如不设置则使用默认值）
|
||
float kp; // 位置刚度 (N·m/rad)，默认值见Joint类
|
||
float kd; // 阻尼系数 (N·m·s/rad)，默认值见Joint类
|
||
} Arm6dof_MotorMTICtrlParams_t;
|
||
typedef struct {
|
||
Arm6dof_DHParams_t DH_params[6];
|
||
Arm6dof_MotorMTICtrlParams_t motor_ctrl_params[6]; // 每个关节的MIT控制参数（可选）
|
||
struct {
|
||
float x, y, z; // 工具中心偏移 (m)
|
||
float roll, pitch, yaw; // 工具姿态偏移 (rad)
|
||
} tool_offset;
|
||
} Arm6dof_Params_t;
|
||
/* 末端位姿结构体 */
|
||
typedef struct {
|
||
float x, y, z; // 位置 (m)
|
||
float roll, pitch, yaw; // 姿态 (rad) - RPY欧拉角
|
||
} Arm6dof_Pose_t;
|
||
|
||
/* 关节角度结构体 */
|
||
typedef struct {
|
||
float q[6]; // 6个关节角度 (rad)
|
||
} Arm6dof_JointAngles_t;
|
||
|
||
/**
|
||
* @brief 机械臂控制类型枚举
|
||
* - REMOTE_CARTESIAN: 遥控器/PC端控制 - 目标末端位姿（笛卡尔空间）
|
||
* - CUSTOM_JOINT: 自定义控制器 - 目标六个关节角度
|
||
*/
|
||
typedef enum {
|
||
ARM_CTRL_REMOTE_CARTESIAN = 0, /* 遥控器控制 - 目标末端位姿（笛卡尔空间） */
|
||
ARM_CTRL_CUSTOM_JOINT, /* 自定义控制器 - 目标六个关节角度 */
|
||
} Arm_CtrlType_t;
|
||
|
||
/** 机械臂命令结构体 */
|
||
typedef struct {
|
||
bool enable; /* 使能开关 */
|
||
bool set_target_as_current; /* true: 将目标位姿同步为当前实际位姿 */
|
||
Arm_CtrlType_t ctrl_type; /* 控制类型 */
|
||
Arm6dof_Pose_t target_pose; /* 目标末端位姿（世界坐标系，供调试观察） */
|
||
Arm6dof_JointAngles_t target_joints; /* 目标六个关节角度（自定义控制器模式） */
|
||
} Arm_CMD_t;
|
||
/**
|
||
* @brief 初始化机械臂运动学模型
|
||
* @param dh_params 6个关节的DH参数数组
|
||
* @note 必须先调用此函数再使用正/逆运动学
|
||
*/
|
||
void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]);
|
||
|
||
/**
|
||
* @brief 正运动学：根据关节角度计算末端位姿
|
||
* @param q 输入的6个关节角度 (rad)
|
||
* @param pose 输出的末端位姿 (位置m, 姿态rad)
|
||
* @retval 0: 成功, -1: 失败(未初始化)
|
||
*/
|
||
int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose);
|
||
|
||
/**
|
||
* @brief 逆运动学：根据末端位姿计算关节角度
|
||
* @param pose 输入的目标末端位姿 (位置m, 姿态rad)
|
||
* @param q_init 初始关节角度猜测值 (用于数值解迭代)
|
||
* @param q_result 输出的关节角度解 (rad)
|
||
* @param tol 收敛误差容限，位置分量单位为(m)，建议0.001f(即1mm精度)
|
||
* @param max_iter 最大迭代次数，默认10
|
||
* @retval 0: 成功, -1: 失败(未初始化或无解)
|
||
* @note 使用牛顿法数值求解，需要提供合理的初始猜测值
|
||
*/
|
||
int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAngles_t* q_init,
|
||
Arm6dof_JointAngles_t* q_result, float tol, uint16_t max_iter);
|
||
|
||
/**
|
||
* @brief 获取指定关节的位姿 (用于可视化或调试)
|
||
* @param q 输入的6个关节角度 (rad)
|
||
* @param joint_num 关节编号 (1-6)
|
||
* @param pose 输出的该关节位姿
|
||
* @retval 0: 成功, -1: 失败
|
||
*/
|
||
int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose);
|
||
/**
|
||
* @brief 设置机器人参数 (如果需要动态修改)
|
||
* @param params 新的机器人尺寸参数
|
||
*/
|
||
void Arm6dof_SetParams(const Arm6dof_Params_t* params);
|
||
|
||
/**
|
||
* @brief 计算重力补偿力矩（基于牛顿-欧拉逆动力学，速度和加速度为零）
|
||
* @param q 当前6个关节角度 (rad)
|
||
* @param gravity_torques 输出的6个关节重力补偿力矩 (N·m，DH参数单位为m时)
|
||
* @retval 0: 成功, -1: 失败(未初始化)
|
||
*/
|
||
int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#endif // ARM_H
|