251 lines
7.9 KiB
C++
251 lines
7.9 KiB
C++
/**
|
||
******************************************************************************
|
||
* @file arm.cpp
|
||
* @brief 6-DOF robotic arm kinematics control implementation
|
||
******************************************************************************
|
||
*/
|
||
|
||
#include "arm6dof.h"
|
||
#include "toolbox/robotics.h"
|
||
#include "toolbox/matrix.h"
|
||
#include "user_math.h"
|
||
#include <string.h>
|
||
|
||
/* 静态变量 */
|
||
static robotics::Link* s_links = nullptr;
|
||
static robotics::Serial_Link<6>* s_robot = nullptr;
|
||
static bool s_initialized = false;
|
||
|
||
/**
|
||
* dh_params = [
|
||
{"a": 0, "alpha": pi / 2, "d": 0, "theta_offset": 0}, # Joint 1
|
||
{"a": 0.404, "alpha": 0.0, "d": 0, "theta_offset": 0}, # Joint 2
|
||
{"a": 0.091579, "alpha": -pi / 2, "d": 0, "theta_offset": pi / 2}, # Joint 3
|
||
{"a": 0, "alpha": pi / 2, "d": 0.2411, "theta_offset": 0},
|
||
{"a": 0, "alpha": -pi / 2, "d": 0, "theta_offset": 0},
|
||
{"a": 0, "alpha": 0, "d": 0.1094, "theta_offset": 0},
|
||
{"a": 0.0577, "alpha": pi / 2, "d": 0.033001, "theta_offset": 0},
|
||
{"a": 0.05, "alpha": 0, "d": 0, "theta_offset": 0},
|
||
]
|
||
* @brief 根据参数创建DH参数链表
|
||
* @note 标准DH参数，参考 DH_PARAMETERS.md 文档
|
||
*
|
||
* DH参数表（标准DH）：
|
||
* 关节 | θᵢ(变量) | dᵢ | aᵢ | αᵢ | 说明
|
||
* -----|----------|-------|------|--------|------------------
|
||
* 1 | q1 | h0 | 0 | π/2 | 底座旋转（Yaw）
|
||
* 2 | q2 | 0 | L1 | 0 | 肩部俯仰（Pitch）
|
||
* 3 | q3 | 0 | L2 | 0 | 肘部俯仰（Pitch）
|
||
* 4 | q4 | d4 | 0 | π/2 | 腕部滚转（Roll）
|
||
* 5 | q5 | 0 | 0 | -π/2 | 腕部俯仰（Pitch）
|
||
* 6 | q6 | d6 | 0 | 0 | 末端旋转（Roll）
|
||
|
||
/**
|
||
* @brief 初始化机械臂运动学模型
|
||
*/
|
||
void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]) {
|
||
if (dh_params == nullptr) {
|
||
return;
|
||
}
|
||
|
||
// 创建DH链表
|
||
if (s_links != nullptr) {
|
||
delete[] s_links;
|
||
}
|
||
s_links = new robotics::Link[6];
|
||
|
||
for (int i = 0; i < 6; i++) {
|
||
// 构建质心向量（DH连杆坐标系下，由URDF经Rz(-theta_offset)旋转得到）
|
||
Matrixf<3, 1> rc_vec;
|
||
rc_vec[0][0] = dh_params[i].rc[0];
|
||
rc_vec[1][0] = dh_params[i].rc[1];
|
||
rc_vec[2][0] = dh_params[i].rc[2];
|
||
|
||
s_links[i] = robotics::Link(
|
||
dh_params[i].theta, // 初始theta值（通常为0）
|
||
dh_params[i].d, // 连杆偏移 (m)
|
||
dh_params[i].a, // 连杆长度 (m)
|
||
dh_params[i].alpha, // 扭转角 (rad)
|
||
robotics::R, // 旋转关节
|
||
dh_params[i].theta_offset, // theta偏移 (rad)
|
||
dh_params[i].qmin, // 最小角度 (rad)
|
||
dh_params[i].qmax, // 最大角度 (rad)
|
||
dh_params[i].m, // 质量 (kg)
|
||
rc_vec // 质心坐标（DH连杆坐标系，m）
|
||
);
|
||
}
|
||
|
||
// 创建机器人模型
|
||
if (s_robot != nullptr) {
|
||
delete s_robot;
|
||
}
|
||
s_robot = new robotics::Serial_Link<6>(s_links);
|
||
|
||
s_initialized = true;
|
||
}
|
||
|
||
/**
|
||
* @brief 正运动学
|
||
*/
|
||
int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose) {
|
||
if (!s_initialized || q == nullptr || pose == nullptr) {
|
||
return -1;
|
||
}
|
||
|
||
// 将关节角度转换为矩阵格式
|
||
Matrixf<6, 1> q_mat;
|
||
for (int i = 0; i < 6; i++) {
|
||
q_mat[i][0] = q->q[i];
|
||
}
|
||
|
||
// 计算正运动学
|
||
Matrixf<4, 4> T = s_robot->fkine(q_mat);
|
||
|
||
// 提取位置 (x, y, z)
|
||
Matrixf<3, 1> pos = robotics::t2p(T);
|
||
pose->x = pos[0][0];
|
||
pose->y = pos[1][0];
|
||
pose->z = pos[2][0];
|
||
|
||
// 提取姿态 (RPY欧拉角)
|
||
Matrixf<3, 1> rpy = robotics::t2rpy(T);
|
||
pose->yaw = rpy[0][0];
|
||
pose->pitch = rpy[1][0];
|
||
pose->roll = rpy[2][0];
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取指定关节的位姿
|
||
*/
|
||
int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose) {
|
||
if (!s_initialized || q == nullptr || pose == nullptr) {
|
||
return -1;
|
||
}
|
||
|
||
if (joint_num < 1 || joint_num > 6) {
|
||
return -1;
|
||
}
|
||
|
||
// 将关节角度转换为矩阵格式
|
||
Matrixf<6, 1> q_mat;
|
||
for (int i = 0; i < 6; i++) {
|
||
q_mat[i][0] = q->q[i];
|
||
}
|
||
|
||
// 计算到第k个关节的变换矩阵
|
||
Matrixf<4, 4> T = s_robot->fkine(q_mat, joint_num);
|
||
|
||
// 提取位置和姿态
|
||
Matrixf<3, 1> pos = robotics::t2p(T);
|
||
pose->x = pos[0][0];
|
||
pose->y = pos[1][0];
|
||
pose->z = pos[2][0];
|
||
|
||
Matrixf<3, 1> rpy = robotics::t2rpy(T);
|
||
pose->yaw = rpy[0][0];
|
||
pose->pitch = rpy[1][0];
|
||
pose->roll = rpy[2][0];
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 逆运动学
|
||
*/
|
||
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) {
|
||
if (!s_initialized || pose == nullptr || q_result == nullptr) {
|
||
return -1;
|
||
}
|
||
|
||
// 构造目标变换矩阵
|
||
Matrixf<3, 1> rpy_vec;
|
||
rpy_vec[0][0] = pose->yaw;
|
||
rpy_vec[1][0] = pose->pitch;
|
||
rpy_vec[2][0] = pose->roll;
|
||
Matrixf<3, 3> R_target = robotics::rpy2r(rpy_vec);
|
||
|
||
Matrixf<3, 1> p_target;
|
||
p_target[0][0] = pose->x;
|
||
p_target[1][0] = pose->y;
|
||
p_target[2][0] = pose->z;
|
||
|
||
Matrixf<4, 4> T_target = robotics::rp2t(R_target, p_target);
|
||
|
||
// 初始猜测值
|
||
Matrixf<6, 1> q_guess = matrixf::zeros<6, 1>();
|
||
if (q_init != nullptr) {
|
||
for (int i = 0; i < 6; i++) {
|
||
q_guess[i][0] = q_init->q[i];
|
||
}
|
||
}
|
||
|
||
// 调用toolbox的逆运动学求解器（牛顿法）
|
||
Matrixf<6, 1> q_solved = s_robot->ikine(T_target, q_guess, tol, max_iter);
|
||
|
||
// 将结果复制到输出
|
||
for (int i = 0; i < 6; i++) {
|
||
q_result->q[i] = q_solved[i][0];
|
||
}
|
||
|
||
// 验证解的精度：位置误差 + 姿态误差双y重校验
|
||
Matrixf<4, 4> T_verify = s_robot->fkine(q_solved);
|
||
Matrixf<3, 1> p_verify = robotics::t2p(T_verify);
|
||
|
||
float pos_error = (p_verify - p_target).norm();
|
||
|
||
if (pos_error > tol * 10.0f) { // 位置误差过大，未收敛
|
||
return -1;
|
||
}
|
||
|
||
// 姿态误差：计算两旋转矩阵的测地距离 theta = arccos((trace(R_v * R_t^T) - 1) / 2)
|
||
// 使用旋转矩阵方法避免 RPY 奇异点和角度折叠问题
|
||
Matrixf<3, 3> R_verify = robotics::t2r(T_verify);
|
||
Matrixf<3, 3> R_diff = R_verify * R_target.trans();
|
||
float trace_val = R_diff[0][0] + R_diff[1][1] + R_diff[2][2];
|
||
float cos_theta = (trace_val - 1.0f) * 0.5f;
|
||
// 数值钳位防止 acosf 域溢出
|
||
if (cos_theta > 1.0f) cos_theta = 1.0f;
|
||
if (cos_theta < -1.0f) cos_theta = -1.0f;
|
||
float ang_error = acosf(cos_theta); // [0, π]
|
||
|
||
const float ang_tol = 0.1f; // 姿态收敛阈值 (rad ≈ 5.7°)
|
||
if (ang_error > ang_tol) { // 姿态未收敛（收敛到错误分支或近奇异点）
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 计算重力补偿力矩
|
||
* @details 利用牛顿-欧拉逆动力学(rne)，设速度qv=0、加速度qa=0，
|
||
* 计算的关节力矩即为平衡重力所需的力矩。
|
||
*/
|
||
int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]) {
|
||
if (!s_initialized || q == nullptr || gravity_torques == nullptr) {
|
||
return -1;
|
||
}
|
||
|
||
// 关节角度
|
||
Matrixf<6, 1> q_mat;
|
||
for (int i = 0; i < 6; i++) {
|
||
q_mat[i][0] = q->q[i];
|
||
}
|
||
|
||
// 速度和加速度均为零 —— 只计算重力项
|
||
Matrixf<6, 1> qv = matrixf::zeros<6, 1>();
|
||
Matrixf<6, 1> qa = matrixf::zeros<6, 1>();
|
||
|
||
// 调用牛顿-欧拉逆动力学
|
||
Matrixf<6, 1> torques = s_robot->rne(q_mat, qv, qa);
|
||
|
||
for (int i = 0; i < 6; i++) {
|
||
gravity_torques[i] = torques[i][0];
|
||
}
|
||
|
||
return 0;
|
||
}
|