适用于RoboMaster工程机器人机械臂控制组件库。
本仓库提供一套面向嵌入式（STM32/RM 工程机器人）的机械臂控制框架，包含：
- tools/：纯数学工具层（矩阵、向量、四元数、坐标变换、解析逆运动学）
- module/：机械臂模块层（连杆 DH 模型、串联机械臂、轨迹规划器、通用机械臂 FSM 基类）
- demo/：实车上稳定运行的使用示例
Engineer-Components/
├── tools/
│ ├── Inc/
│ │ ├── matrix.hpp # 矩阵模板类（纯数组实现）
│ │ ├── vector3d.hpp # 三维向量类
│ │ ├── quaternion.hpp # 四元数类
│ │ ├── transformation.hpp # 姿态/齐次变换互转函数
│ │ ├── operation.hpp # 叉积/点积/四元数乘法/HTM 乘法
│ │ ├── utils.hpp # 常用数学工具函数
│ │ └── ikine_func.hpp # ikine1~5 解析逆解声明
│ └── Src/
│ ├── transformation.cpp
│ └── ikine1.cpp ~ ikine5.cpp
├── module/
│ ├── Inc/
│ │ ├── arm_cmd.hpp # 笛卡尔指令结构体
│ │ ├── link.hpp # DH 参数 / 连杆类
│ │ ├── serial_link.hpp # 串联连杆 / 雅可比 / 牛顿欧拉
│ │ ├── joint_planner.hpp # 单段五次多项式关节规划器
│ │ ├── multinode_joint_planner.hpp # 多节点关节规划器
│ │ ├── linear_planner.hpp # 标量线性插值规划器
│ │ ├── cartesian_planner.hpp # 笛卡尔直线/圆弧规划器
│ │ ├── multinode_cartesian_planner.hpp # 多节点笛卡尔规划器
│ │ ├── planner.hpp # 规划器统一入口（include 合集）
│ │ └── module_arm.hpp # 通用机械臂 FSM 模板基类
│ └── Src/
│ ├── link.cpp
│ ├── serial_link.cpp
│ ├── linear_planner.cpp
│ └── cartesian_planner.cpp
└── demo/
├── ins_arm.hpp / ins_arm.cpp # 6 轴机械臂连杆（DH）定义
├── arm_fsm.hpp / arm_fsm.cpp # 6 轴机械臂 FSM（继承 ModuleArm<6>）
├── mode_config.hpp # 各工作模式的关节/笛卡尔指令常量
└── main_fsm.hpp / main_fsm.cpp # 上层整机 FSM（遥控/裁判系统调度）
所有类位于 namespace engineer（utils.hpp 中另有 engineer::IsFloatEqual 等）。
纯内存数组实现的定长矩阵模板，行优先存储 float data_[rows_*cols_]，无堆分配，适合嵌入式实时环境。
#include "matrix.hpp"
using namespace engineer;
Matrix<3, 1> v({1.0f, 2.0f, 3.0f}); // 数组构造
Matrix<3, 3> A = eye<3, 3>(); // 单位阵
Matrix<3, 3> Z = zeros<3, 3>(); // 零阵
Matrix<2, 2> O = ones<2, 2>(); // 全一阵
Matrix<4, 4> D = diag(Matrix<4, 1>({1,2,3,4})); // 对角阵
float x = m(0, 0); // 元素读写
A += B; A -= B; A *= 2.0f; A /= 3.0f; // 就地运算
Matrix<3, 3> C = A + B; // 加/减/数乘/数除
Matrix<3, 2> P = A * M32; // 矩阵乘法（M32 为 Matrix<3,2>）
Matrix<2, 3> T = A.trans(); // 转置
Matrix<1, 3> r = A.row(0); // 取行/列
Matrix<2, 2> b = A.block<2, 2>(0, 0); // 取子块
A.replace<2, 2>(1, 1, b); // 写子块
float tr = A.trace(); // 迹
float n = A.norm(); // Frobenius 范数
bool eq = (A == B); // 相等比较（容差 1e-4）Vector3 v(1.0f, 2.0f, 3.0f);
Vector3 w = Vector3(mat3x1); // 由 Matrix<3,1> 构造
v.x(); v.y(); v.z(); // 分量访问
Vector3 a = v + w, b = v - w, c = v * 2.0f, d = v / 2.0f;
v += w; v -= w; v *= 2.0f; v /= 2.0f;
float n = v.norm(); // 模长
Vector3 u = v.normalize(); // 归一化
float d = v.dot(w); // 点积
Vector3 c = v.cross(w); // 叉积
bool eq = (v == w); // 容差比较
Matrix<3,1> m = v.matrix(); // 转矩阵约定 [w,x,y,z]，即 w=cos(θ/2)、xyz = r·sin(θ/2)。
Quaternion q1(1.0f, 0, 0, 0);
Quaternion q2(vec3, angle); // 轴角构造（vec 自动归一化）
Quaternion q3(mat4x1); // 由 Matrix<4,1> 构造
Quaternion q4(rot3x3); // 由旋转矩阵构造（Shepperd 法，数值稳定）
q.w(); q.x(); q.y(); q.z();
Quaternion p = q1 * q2; // 四元数乘法
Quaternion s = q1 * 0.5f; // 数乘/加减/数除
float n = q.norm();
Quaternion nq = q.normalize();
Quaternion cq = q.conjugate(); // 共轭
Quaternion iq = q.inv(); // 逆
Matrix<3,3> R = q.r(); // 转旋转矩阵
float ang = q.angvec(axis); // 转轴角（输出单位轴+角度 rad）
Matrix<4,1> m = q.matrix(); // 转矩阵以下函数全部以 Matrix 为数据结构，按"姿态表示"分组互转：
| 函数 | 说明 |
|---|---|
r2rpy / rpy2r |
旋转矩阵 ↔ RPY 欧拉角 [yaw;pitch;roll] |
r2angvec / angvec2r |
旋转矩阵 ↔ 轴角 [rx;ry;rz;θ] |
r2quat / quat2r |
旋转矩阵 ↔ 四元数 [w;x;y;z] |
quat2rpy / rpy2quat |
四元数 ↔ RPY 欧拉角 |
quat2angvec / angvec2quat |
四元数 ↔ 轴角 |
t2r / r2t |
齐次变换矩阵 ↔ 旋转矩阵 |
t2p / p2t |
齐次变换矩阵 ↔ 平移向量 |
rp2t |
旋转矩阵 + 平移 → 齐次变换矩阵 |
invT |
齐次变换矩阵求逆 T⁻¹=[Rᵀ,-Rᵀp;0,1] |
t2rpy / rpy2t |
HTM ↔ RPY 欧拉角 |
t2angvec / angvec2t |
HTM ↔ 轴角 |
t2quat / quat2t |
HTM ↔ 四元数 |
t2twist / twist2t |
HTM ↔ 扭转坐标 [p;φ]（φ=rθ） |
dh2trans(theta, d, a, alpha) |
标准 DH 参数 → 齐次变换矩阵 |
Matrix<3,1> rpy = r2rpy(R); // yaw/pitch/roll
Matrix<4,1> q = r2quat(R); // 注意是 [w;x;y;z]
Matrix<3,3> R2 = quat2r(q);
Matrix<4,4> T = rp2t(R, p);
Matrix<4,4> Tinv = invT(T);
Matrix<3,1> px = t2p(Tinv * T); // 应为 0Matrix<3,1> cv = cross(a, b); // 3 维向量叉积（Matrix / Vector3 重载）
float d = dot(a, b); // 点积
float qd = QuatDot(qa, qb); // 四元数点积（Matrix4x1 / Quaternion 重载）
Matrix<4,1> qm = QuatCross(qa, qb); // 四元数乘法（Hamilton）
Matrix<4,4> T = HTMMultiply(A, B); // 齐次变换矩阵乘法（R1R2 + p 组合，避免直接 4x4 乘）IsFloatEqual(a, b); // 浮点近似相等
IsFloatArrayEqual(arrA, arrB, len); // 数组近似相等
float r = limit(v, min, max); // 限幅
float n = NormalizePeriodData(lb, ub, data); // 周期数据归一化到 [lb, ub)
float a = NormAngle(angle); // 归一化到 [-PI, PI)
Matrix<3,3> S = hat(v); // 反对称矩阵
EngineerAtan2(y, x); //
EngineerAcos(x); EngineerAsin(x);
EngineerCos(x); EngineerSin(x); // 均封装 arm_math 库
bool ok = SinCosBoundProcess(v); // sin/cos 值越界处理（夹到 ±1）内置 5 种构型的解析逆解，全部以 ModuleCartesianCmd（mode 定义于 arm_cmd.hpp）为输入：
// ikine1 —— 6 轴，DH (modified)：均为 0，pi/2，pi/2，a=337,269.74,200… / d=339.01
uint8_t ikine1(ModuleCartesianCmd cmd, float l[4], Matrix<8,6> &sol);
// ikine2 —— 7 根连杆 6 自由度实际 6 轴，l[4]
uint8_t ikine2(ModuleCartesianCmd cmd, float l[4], Matrix<8,6> &sol);
// ikine3 —— 7 轴 L(3) 冗余，多余关节 theta6、theta7 作为输入参数
uint8_t ikine3(ModuleCartesianCmd cmd, float l[4], float theta6, float theta7, Matrix<4,7> &sol);
// ikine4 —— 7 轴（1 个冗余：theta7），l[6]
uint8_t ikine4(ModuleCartesianCmd cmd, float l[6], float theta7, Matrix<8,7> &sol);
// ikine5 —— 6 轴，l[3] = {L3.a, L4.d, L6.d}（demo 使用），l[3] = {0.335, 0.280, 0.074}
uint8_t ikine5(ModuleCartesianCmd cmd, float l[3], Matrix<8,6> &sol);返回值为有效解组数（每行为一组关节角，弧度）；无效行对应元素置 0。调用后需自行对解做角度归一化（NormPeriodData(-PI,PI)）并选解（相对当前关节最小 L2 距离且满足限位），module_arm.hpp 中 ModuleArm::selectSolution() 已内置此逻辑，可直接复用。
各构型具体 DH 参数见 ikine_func.hpp 顶部注释（单位：mm），使用前换算为 m。
struct ModuleCartesianCmd { Matrix<3,1> pos; Matrix<4,1> quat; }; // 矩阵表达（规划器/逆解用）
struct CartesianCmd { Vector3 pos; Quaternion quat; }; // 类表达（上层 FSM 用）
// CartesianCmd::module() 可转为 ModuleCartesianCmdstruct LinkParam { // 连杆描述参数
float offset; // 关节零点偏置（rad）
float d; // DH 参数 d（m）
float a; // DH 参数 a（m）
float alpha; // DH 参数 alpha（rad）
float qmin_, qmax_; // 关节限位（rad），默认 ±PI
float m; // 质量（kg）
Matrix<3,1> centroid; // 质心（本体系，m）
Matrix<3,3> inertia; // 惯量（kg·m²）
};
Link link(link_param); // 构造
Matrix<4,4> T = link.forward(q); // 正解（DH 阵 → 基坐标系下的变换）
Matrix<4,4> Tb = link.backward(q); // 逆（转置）
link.setJointLimit(qmin, qmax); // 修改限位
link.setDHParams(offset,d,a,alpha); // 修改 DH 参数
link.setDynamicParam(m, centroid, inertia);
Link::getDH() / getDynamicParam() 可读回参数。SerialLink sl;
sl.append(Link(link_param1)); // 按基座→末端依次添加连杆
sl.append(Link(link_param2));
float theta[6] = {...};
sl.fkine(theta); // 正解（内部连乘各 link 的 forward）
sl.getT(); // 末端齐次变换
sl.getR(); // 旋转矩阵
sl.getP(); // 平移向量
sl.getQuat(); // 末端姿态（四元数）
sl.getPos(); // 末端位置（Vector3）
// 按索引设置/读取各连杆
sl.setLinkDHParams(idx, offset, d, a, alpha);
sl.setLinkJointLimit(idx, qmin, qmax);
sl.getLinkJointLimit(idx, qmin, qmax); // ModuleArm 用它读限位
sl.setLinkDynamicParam(idx, m, centroid, inertia);
sl.getLinkDynamicParam(idx, m, centroid, inertia);
// 雅可比（模板参数为关节数）
Matrix<6, N> J = sl.Jocobian<N>(theta);
// 动力学（重力补偿用，内部自动计算）
sl.gravityCompensation(theta, torq_out); // 输出：各关节重力补偿力矩全局函数：
GetJocobian<N>(T)：由各连杆Matrix<4,4>数组计算 6×N 雅可比NewtonEuler(num, R, P, param, dtheta, ddtheta, torq, end_force, end_torque)：完整牛顿-欧拉迭代（含末端外负载），输出各关节力矩StaticNewtonEuler(num, R, P, param, torq)：静态（重力）迭代，SerialLink::gravityCompensation内部调用
// ctrl_period：控制周期 (s)；initial_vel_opt：是否带初始速度；periodsub：逐关节的周期性边界标志（1 表示该关节按周期角处理跨 ±π 差值）
JointPlanner<6> jp(0.001f, false, zeros<6,1>());
Matrix<6,1> start = ..., end = ...;
jp.setTraj(start, end, 2.0f); // 规划起点→终点，用时 2s
Matrix<6,1> pos;
jp.plan(pos); // 每个控制周期调用，输出当前位置（rad）
bool done = jp.isPlanFinished();在 JointPlanner 之上增加节点队列，逐段执行。module_arm 默认使用此类。
MultinodeJointPlanner<6> mjp(0.001f);
mjp.setStartPos(now); // 必须先设置起点
mjp.appendNode(node1, t1); // 依次添加节点与对应规划时间
mjp.appendNode(node2, t2);
mjp.plan(pos); // 每周期调用；完成后 pos=终点
mjp.isPlanFinished();对单个 float 值做匀速插值（速度 = 位移/时间）：
LinearPlanner lp(0.001f);
lp.setLinearTraj(0.0f, 1.0f, 2.0f);
float cmd; lp.plan(cmd); // cmd 从 0 → 1，历时 2s支持直线（位置 5 次多项式 + 姿态球面线性插值 slerp）与圆弧（三点定圆）。
CartesianPlanner cp(0.001f);
ModuleCartesianCmd p1, p2;
cp.setLinearTraj(p1, p2, 2.0f); // 直线
// 或
cp.setCircleTraj(p1, pmid, p3, 2.0f); // 圆弧（起点/中点/终点）
ModuleCartesianCmd out;
cp.plan(out); // 每周期调用，输出位置(pos)+姿态(quat)在 CartesianPlanner 之上串行执行多段直线节点（setStartPos + 依次 appendNode）。
核心类。模板参数 NJ = 关节数（编译期常量），封装了机械臂控制的完整 FSM 周期：
update() → genCmd(cmd) → run() （每控制周期调用）
update()：读取电机角度/速度（TD 微分可选）→ 正解 → 重力前馈genCmd()：根据规划类型（Joint/Linear/Circular/None）生成运动指令 → 逆解 → 关节角度参考 → PID 计算输出run()：工作状态 → 驱动电机（普通 / MIT 模式达妙电机）；失能 → 力矩清零 + 锁位
使用方法（通常通过派生类）：
#define NJ 6
class MyArm : public engineer::ModuleArm<NJ> {
// 1. 纯虚函数：必须实现解析逆解
uint8_t solveIkine(ModuleCartesianCmd cmd, const float *free_var,
Matrix<kMaxIkineSol, NJ> &sol) override {
float l[3] = {0.335f, 0.28f, 0.074f};
return ikine5(cmd, l, sol); // 6 轴用 ikine5
}
// 2. （可选）MIT 模式（达妙电机）：哪些关节用 MIT
bool isMitModeMotor(uint8_t idx) override { return idx >= 3; }
float mitKd(uint8_t idx) override { return 1.5f; }
// 3. （可选）末端执行器偏移（fk 时叠加在末端坐标 x 上）
Matrix<3,1> getEndEffectorOffset() override { ... }
// 4. （可选）自定义 PID/限位/电机输出
MultiNodesPid *getActivePid(uint8_t idx) override { ... }
float getJointLimitMax/Min(idx) override {...} // 默认自 serial_link 读
// 5. （可选）重写 calcMotorInputRef/setMotorInput/runOnWorking 完全自定义
};
MyArm arm;
arm.registerMotor(0, &motor0); // 普通电机
arm.registerDaMiao(4, &dm_motor4); // 达妙电机（同时注册为 Motor）
arm.registerMotorPid(i, &pid_i); // 各关节 PID
arm.registerMotorTd(i, &td_i); // （可选）速度滤波 TD
arm.registerJointPlanner(&joint_planner); // MultinodeJointPlanner<NJ>
arm.registerCartesianPlanner(&cartesian_planner); // CartesianPlanner
arm.registerSerialLink(&serial_link); // SerialLink（连杆 + 限位来源）
// 预逆解：把将来要用到的笛卡尔指令提前解算（id 可不连续）
ArmCmd pre_cmd;
pre_cmd.id = 5;
pre_cmd.work_space = kModuleArmWorkSpaceCartesian;
pre_cmd.cartesian_cmd = ...;
pre_cmd.joint(0,0) = 1.57f; // NJ>6 时冗余关节固定值存再这里
arm.setNeedToPreIkine(pre_cmd);
arm.init(); // 缓存关节限位到 joint_max/min[]，并执行预逆解队列主循环使用：
Cmd cmd; // ModuleArmFsmCmd<NJ>
cmd.work_state = kModuleArmWorkStateWorking;
cmd.plan_type = kArmPlanTypeJoint; // 类型：
// kModuleArmPlanTypeJoint 关节空间（多节点，节点可混合 join 指令或笛卡尔指令【预逆解】）
// kModuleArmPlanTypeLinear 笛卡尔空间直线（2 节点：起点、终点）
// kModuleArmPlanTypeCircular 笛卡尔空间圆弧（3 节点：起点、中点、终点）
// kModuleArmPlanTypeNone 瞬间速度（把指令直接作为目标）
cmd.node_num = 1;
cmd.cmd[0].work_space = kModuleArmWorkSpaceJoint;
cmd.cmd[0].joint = ...;
cmd.traj_time[0] = 2.0f; // 各节点规划时间（s）
arm.update(); // 1. 先更新传感数据/正解
arm.genCmd(cmd); // 2. 规划/逆解/PID计算
arm.run(); // 3. 驱动电机查询接口：arm.pose() 返回 ModuleArmPose<NJ>（含 joint/cartesian_pose/matrix）；arm.isAtPos(cmd) 判断是否到位（关节 <0.1 rad，笛卡尔 <30mm）。
master分支与cpp-dev分支的主要区别
- master分支使用arm_math库进行加速，其他无明显区别
- cpp-dev分支部分功能还未在实车上验证，比如通用轨迹规划器和数值逆解算
经测量，demo每周期运行时间最高2ms（计算重力补偿+逆解算），实际控制周期建议设置为3ms