/** ****************************************************************************** * @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 /* 静态变量 */ 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; }