【MR】现代机器人学-算法库函数解析（C++版截止2024.4.7）

算法库提供以下函数的实现

1. NearZero: 判断一个值是否可以忽略为0。

2. ad: 计算给定6维向量的6x6矩阵[adV]。

3. Normalize: 返回输入向量的归一化版本。

4. VecToso3: 返回角速度向量的反对称矩阵表示。

5. so3ToVec: 返回由反对称矩阵表示的角速度向量。

6. AxisAng3: 将指数旋转转换为其单独的分量。

7. MatrixExp3: 将指数旋转转换为旋转矩阵。

8. MatrixLog3: 计算旋转矩阵的矩阵对数。

9. RpToTrans: 将旋转矩阵和位置向量组合成一个特殊欧几里得群(SE3)齐次变换矩阵。

10. TransToRp: 从变换矩阵表示中分离出旋转矩阵和位置向量。

11. VecTose3: 将空间速度向量转换为变换矩阵。

12. se3ToVec: 将变换矩阵转换为空间速度向量。

13. Adjoint: 提供变换矩阵的伴随表示。

14. MatrixExp6: 螺旋轴的旋转扩展。

15. MatrixLog6: 计算齐次变换矩阵的矩阵对数。

16. FKinSpace: 计算末端执行器框架（用于当前空间位置计算）。

17. FKinBody: 计算末端执行器框架（用于当前身体位置计算）。

18. JacobianSpace: 给出空间雅可比矩阵。

19. JacobianBody: 给出身体雅可比矩阵。

20. TransInv: 反转齐次变换矩阵。

21. RotInv: 反转旋转矩阵。

22. ScrewToAxis: 将螺旋轴的参数描述转换为归一化螺旋轴。

23. AxisAng6: 将6维指数坐标转换为螺旋轴-角度形式。

24. ProjectToSO3: 将一个矩阵投影到SO(3)。

25. ProjectToSE3: 将一个矩阵投影到SE(3)。

26. DistanceToSO3: 返回Frobenius范数以描述矩阵M与SO(3)流形的距离。

27. DistanceToSE3: 返回Frobenius范数以描述矩阵T与SE(3)流形的距离。

28. TestIfSO3: 如果矩阵M接近或在SO(3)流形上，返回true。

29. TestIfSE3: 如果矩阵T接近或在SE(3)流形上，返回true。

30. IKinBody: 计算开链机器人在身体坐标系中的逆运动学。

31. IKinSpace: 计算开链机器人在空间坐标系中的逆运动学。

32. InverseDynamics: 使用前后牛顿-欧拉迭代求解方程。

33. GravityForces: 计算动力学方程中的重力项。

34. MassMatrix: 计算关节串联链的数值惯性矩阵。

35. VelQuadraticForces: 计算科里奥利和离心项向量。

36. EndEffectorForces: 计算末端执行器力所需的关节力和力矩。

37. ForwardDynamics: 通过求解方程计算关节加速度。

38. EulerStep: 使用一阶欧拉积分计算下一个时间步的关节角度和速度。

39. InverseDynamicsTrajectory: 计算沿给定轨迹移动串联链所需的关节力/力矩。

40. ForwardDynamicsTrajectory: 计算给定开环关节力/力矩历史的串联链运动。

41. ComputedTorque: 计算特定时间点的关节控制力矩。

42. CubicTimeScaling: 计算三次时间缩放的s(t)。

43. QuinticTimeScaling: 计算五次时间缩放的s(t)。

44. JointTrajectory: 计算关节空间中的直线轨迹。

45. ScrewTrajectory: 计算对应于空间螺旋轴螺旋运动的轨迹列表。

46. CartesianTrajectory: 计算对应于末端执行器框架原点沿直线运动的轨迹列表。

47. SimulateControl: 计算给定开环关节力/力矩历史的串联链运动。

以下对源码文件 modern_robotics.cpp  lib_test.cpp  modern_robotics.h 进行了中文逐行注释。

modern_robotics.cpp：

#include "../include/modern_robotics.h" // 包含现代机器人库的头文件/** modernRobotics.cpp* 从modern_robotics.py改编而来，由modernrobotics.org提供* 提供有用的雅可比矩阵和框架表示函数*/
#include <Eigen/Dense> // 包含Eigen库，用于矩阵和向量运算
#include <cmath> // 包含cmath库，用于数学运算
#include <vector> // 包含vector库，用于动态数组# define M_PI 3.14159265358979323846  /* 定义圆周率常量 */namespace mr { // 定义mr命名空间/* 函数：判断一个值是否可以忽略为0* 输入：要检查的double类型值* 返回：布尔值，true表示可以忽略，false表示不能忽略*/bool NearZero(const double val) {return (std::abs(val) < .000001); // 如果绝对值小于0.000001，则认为可以忽略}/** 函数：计算给定6维向量的6x6矩阵[adV]* 输入：Eigen::VectorXd (6x1) 向量* 输出：Eigen::MatrixXd (6x6) 矩阵* 备注：可用于计算李括号[V1, V2] = [adV1]V2*/Eigen::MatrixXd ad(Eigen::VectorXd V) {Eigen::Matrix3d omgmat = VecToso3(Eigen::Vector3d(V(0), V(1), V(2))); // 提取前3个元素并转换为反对称矩阵Eigen::MatrixXd result(6, 6); // 定义6x6矩阵result.topLeftCorner<3, 3>() = omgmat; // 左上角3x3子矩阵为omgmatresult.topRightCorner<3, 3>() = Eigen::Matrix3d::Zero(3, 3); // 右上角3x3子矩阵为零矩阵result.bottomLeftCorner<3, 3>() = VecToso3(Eigen::Vector3d(V(3), V(4), V(5))); // 左下角3x3子矩阵为后3个元素转换的反对称矩阵result.bottomRightCorner<3, 3>() = omgmat; // 右下角3x3子矩阵为omgmatreturn result; // 返回结果矩阵}/* 函数：返回输入向量的归一化版本* 输入：Eigen::MatrixXd 矩阵* 输出：Eigen::MatrixXd 矩阵* 备注：MatrixXd用于行向量的情况，需要复制，因MatrixXd转换的便利性而有用*/Eigen::MatrixXd Normalize(Eigen::MatrixXd V) {V.normalize(); // 对向量进行归一化return V; // 返回归一化后的向量}/* 函数：返回角速度向量的反对称矩阵表示* 输入：Eigen::Vector3d 3x1角速度向量* 返回：Eigen::MatrixXd 3x3反对称矩阵*/Eigen::Matrix3d VecToso3(const Eigen::Vector3d& omg) {Eigen::Matrix3d m_ret; // 定义3x3矩阵m_ret << 0, -omg(2), omg(1), // 填充矩阵元素omg(2), 0, -omg(0),-omg(1), omg(0), 0;return m_ret; // 返回反对称矩阵}/* 函数：返回由反对称矩阵表示的角速度向量* 输入：Eigen::MatrixXd 3x3反对称矩阵* 返回：Eigen::Vector3d 3x1角速度向量*/Eigen::Vector3d so3ToVec(const Eigen::MatrixXd& so3mat) {Eigen::Vector3d v_ret; // 定义3x1向量v_ret << so3mat(2, 1), so3mat(0, 2), so3mat(1, 0); // 提取矩阵元素填充向量return v_ret; // 返回角速度向量}/* 函数：将指数旋转转换为其单独的分量* 输入：指数旋转（以旋转轴和旋转角度表示的旋转矩阵）* 返回：旋转轴和旋转角度[x, y, z, theta]*/Eigen::Vector4d AxisAng3(const Eigen::Vector3d& expc3) {Eigen::Vector4d v_ret; // 定义4x1向量v_ret << Normalize(expc3), expc3.norm(); // 归一化旋转轴并计算旋转角度return v_ret; // 返回结果向量}/* 函数：将指数旋转转换为旋转矩阵* 输入：旋转的指数表示* 返回：旋转矩阵*/Eigen::Matrix3d MatrixExp3(const Eigen::Matrix3d& so3mat) {Eigen::Vector3d omgtheta = so3ToVec(so3mat); // 将反对称矩阵转换为角速度向量Eigen::Matrix3d m_ret = Eigen::Matrix3d::Identity(); // 定义单位矩阵if (NearZero(so3mat.norm())) { // 如果矩阵的范数接近零return m_ret; // 返回单位矩阵}else {double theta = (AxisAng3(omgtheta))(3); // 提取旋转角度Eigen::Matrix3d omgmat = so3mat * (1 / theta); // 计算归一化的反对称矩阵return m_ret + std::sin(theta) * omgmat + ((1 - std::cos(theta)) * (omgmat * omgmat)); // 返回旋转矩阵}}
}/* 函数：计算旋转矩阵的矩阵对数* 输入：旋转矩阵* 返回：旋转的矩阵对数*/Eigen::Matrix3d MatrixLog3(const Eigen::Matrix3d& R) {double acosinput = (R.trace() - 1) / 2.0; // 计算acos的输入值Eigen::MatrixXd m_ret = Eigen::MatrixXd::Zero(3, 3); // 初始化3x3零矩阵if (acosinput >= 1) // 如果acos的输入值大于等于1return m_ret; // 返回零矩阵else if (acosinput <= -1) { // 如果acos的输入值小于等于-1Eigen::Vector3d omg; // 定义3x1向量if (!NearZero(1 + R(2, 2))) // 如果1 + R(2, 2)不接近零omg = (1.0 / std::sqrt(2 * (1 + R(2, 2)))) * Eigen::Vector3d(R(0, 2), R(1, 2), 1 + R(2, 2)); // 计算角速度向量else if (!NearZero(1 + R(1, 1))) // 如果1 + R(1, 1)不接近零omg = (1.0 / std::sqrt(2 * (1 + R(1, 1)))) * Eigen::Vector3d(R(0, 1), 1 + R(1, 1), R(2, 1)); // 计算角速度向量else // 否则omg = (1.0 / std::sqrt(2 * (1 + R(0, 0)))) * Eigen::Vector3d(1 + R(0, 0), R(1, 0), R(2, 0)); // 计算角速度向量m_ret = VecToso3(M_PI * omg); // 计算反对称矩阵return m_ret; // 返回反对称矩阵}else { // 否则double theta = std::acos(acosinput); // 计算旋转角度m_ret = theta / 2.0 / sin(theta) * (R - R.transpose()); // 计算矩阵对数return m_ret; // 返回矩阵对数}}/* 函数：将旋转矩阵和位置向量组合成一个特殊欧几里得群(SE3)齐次变换矩阵* 输入：旋转矩阵(R)，位置向量(p)* 返回：矩阵T = [ [R, p],*         [0, 1] ]*/Eigen::MatrixXd RpToTrans(const Eigen::Matrix3d& R, const Eigen::Vector3d& p) {Eigen::MatrixXd m_ret(4, 4); // 定义4x4矩阵m_ret << R, p, // 填充旋转矩阵和位置向量0, 0, 0, 1; // 填充最后一行return m_ret; // 返回齐次变换矩阵}/* 函数：从变换矩阵表示中分离出旋转矩阵和位置向量* 输入：齐次变换矩阵* 返回：包含旋转矩阵和位置向量的std::vector*/std::vector<Eigen::MatrixXd> TransToRp(const Eigen::MatrixXd& T) {std::vector<Eigen::MatrixXd> Rp_ret; // 定义向量用于存储结果Eigen::Matrix3d R_ret; // 定义3x3旋转矩阵// 获取左上角3x3子矩阵R_ret = T.block<3, 3>(0, 0);Eigen::Vector3d p_ret(T(0, 3), T(1, 3), T(2, 3)); // 提取位置向量Rp_ret.push_back(R_ret); // 将旋转矩阵添加到结果向量中Rp_ret.push_back(p_ret); // 将位置向量添加到结果向量中return Rp_ret; // 返回结果向量}/* 函数：将空间速度向量转换为变换矩阵* 输入：空间速度向量[角速度，线速度]* 返回：变换矩阵*/Eigen::MatrixXd VecTose3(const Eigen::VectorXd& V) {// 分离角速度（指数表示）和线速度Eigen::Vector3d exp(V(0), V(1), V(2)); // 提取角速度Eigen::Vector3d linear(V(3), V(4), V(5)); // 提取线速度// 将值填充到变换矩阵的适当部分Eigen::MatrixXd m_ret(4, 4); // 定义4x4矩阵m_ret << VecToso3(exp), linear, // 填充角速度的反对称矩阵和线速度0, 0, 0, 0; // 填充最后一行return m_ret; // 返回变换矩阵}/* 函数：将变换矩阵转换为空间速度向量* 输入：变换矩阵* 返回：空间速度向量[角速度，线速度]*/Eigen::VectorXd se3ToVec(const Eigen::MatrixXd& T) {Eigen::VectorXd m_ret(6); // 定义6x1向量m_ret << T(2, 1), T(0, 2), T(1, 0), T(0, 3), T(1, 3), T(2, 3); // 提取矩阵元素填充向量return m_ret; // 返回空间速度向量}/* 函数：提供变换矩阵的伴随表示*       用于改变空间速度向量的参考系* 输入：4x4变换矩阵SE(3)* 返回：6x6矩阵的伴随表示*/Eigen::MatrixXd Adjoint(const Eigen::MatrixXd& T) {std::vector<Eigen::MatrixXd> R = TransToRp(T); // 将变换矩阵分解为旋转矩阵和位置向量Eigen::MatrixXd ad_ret(6, 6); // 定义6x6矩阵ad_ret = Eigen::MatrixXd::Zero(6, 6); // 初始化为零矩阵Eigen::MatrixXd zeroes = Eigen::MatrixXd::Zero(3, 3); // 定义3x3零矩阵ad_ret << R[0], zeroes, // 填充旋转矩阵和零矩阵VecToso3(R[1]) * R[0], R[0]; // 填充位置向量的反对称矩阵乘以旋转矩阵和旋转矩阵return ad_ret; // 返回伴随矩阵}/* 函数：螺旋轴的旋转扩展* 输入：指数坐标的se3矩阵表示（变换矩阵）* 返回：表示旋转的6x6矩阵*/Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd& se3mat) {// 从变换矩阵中提取角速度向量Eigen::Matrix3d se3mat_cut = se3mat.block<3, 3>(0, 0); // 提取3x3子矩阵Eigen::Vector3d omgtheta = so3ToVec(se3mat_cut); // 将3x3子矩阵转换为角速度向量Eigen::MatrixXd m_ret(4, 4); // 定义4x4矩阵// 如果旋转可以忽略，m_ret = [[单位矩阵, 角速度]]//                        [    0    ,    1    ]]if (NearZero(omgtheta.norm())) {// 重用之前的变量，它们具有所需的大小se3mat_cut = Eigen::MatrixXd::Identity(3, 3); // 定义3x3单位矩阵omgtheta << se3mat(0, 3), se3mat(1, 3), se3mat(2, 3); // 提取线速度m_ret << se3mat_cut, omgtheta, // 填充单位矩阵和线速度0, 0, 0, 1; // 填充最后一行return m_ret; // 返回变换矩阵}// 如果旋转不可忽略，参考MR第105页else {double theta = (AxisAng3(omgtheta))(3); // 提取旋转角度Eigen::Matrix3d omgmat = se3mat.block<3, 3>(0, 0) / theta; // 计算归一化的反对称矩阵Eigen::Matrix3d expExpand = Eigen::MatrixXd::Identity(3, 3) * theta + (1 - std::cos(theta)) * omgmat + ((theta - std::sin(theta)) * (omgmat * omgmat)); // 计算旋转矩阵的指数扩展Eigen::Vector3d linear(se3mat(0, 3), se3mat(1, 3), se3mat(2, 3)); // 提取线速度Eigen::Vector3d GThetaV = (expExpand * linear) / theta; // 计算GThetaVm_ret << MatrixExp3(se3mat_cut), GThetaV, // 填充旋转矩阵和GThetaV0, 0, 0, 1; // 填充最后一行return m_ret; // 返回变换矩阵}}/* 函数：计算变换矩阵的矩阵对数* 输入：变换矩阵* 返回：变换矩阵的矩阵对数*/Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd& T) {Eigen::MatrixXd m_ret(4, 4); // 定义4x4矩阵auto rp = mr::TransToRp(T); // 将变换矩阵分解为旋转矩阵和位置向量Eigen::Matrix3d omgmat = MatrixLog3(rp.at(0)); // 计算旋转矩阵的矩阵对数Eigen::Matrix3d zeros3d = Eigen::Matrix3d::Zero(3, 3); // 定义3x3零矩阵if (NearZero(omgmat.norm())) { // 如果旋转矩阵的范数接近零m_ret << zeros3d, rp.at(1), // 填充零矩阵和位置向量0, 0, 0, 0; // 填充最后一行}else {double theta = std::acos((rp.at(0).trace() - 1) / 2.0); // 计算旋转角度Eigen::Matrix3d logExpand1 = Eigen::MatrixXd::Identity(3, 3) - omgmat / 2.0; // 计算logExpand1Eigen::Matrix3d logExpand2 = (1.0 / theta - 1.0 / std::tan(theta / 2.0) / 2) * omgmat * omgmat / theta; // 计算logExpand2Eigen::Matrix3d logExpand = logExpand1 + logExpand2; // 计算logExpandm_ret << omgmat, logExpand * rp.at(1), // 填充旋转矩阵和logExpand乘以位置向量0, 0, 0, 0; // 填充最后一行}return m_ret; // 返回矩阵对数}/* 函数：计算末端执行器框架（用于当前空间位置计算）* 输入：末端执行器的初始配置（位置和方向）*       机械臂在初始位置时的关节螺旋轴在空间框架中的表示*       关节坐标列表* 返回：表示关节在指定坐标时末端执行器框架的变换矩阵* 备注：FK表示正向运动学*/Eigen::MatrixXd FKinSpace(const Eigen::MatrixXd& M, const Eigen::MatrixXd& Slist, const Eigen::VectorXd& thetaList) {Eigen::MatrixXd T = M; // 初始化变换矩阵为初始配置for (int i = (thetaList.size() - 1); i > -1; i--) { // 遍历关节坐标列表T = MatrixExp6(VecTose3(Slist.col(i) * thetaList(i))) * T; // 计算每个关节的变换矩阵并相乘}return T; // 返回末端执行器的变换矩阵}/** 函数：计算末端执行器框架（用于当前身体位置计算）* 输入：末端执行器的初始配置（位置和方向）*       机械臂在初始位置时的关节螺旋轴在身体框架中的表示*       关节坐标列表* 返回：表示关节在指定坐标时末端执行器框架的变换矩阵* 备注：FK表示正向运动学*/Eigen::MatrixXd FKinBody(const Eigen::MatrixXd& M, const Eigen::MatrixXd& Blist, const Eigen::VectorXd& thetaList) {Eigen::MatrixXd T = M; // 初始化变换矩阵为初始配置for (int i = 0; i < thetaList.size(); i++) { // 遍历关节坐标列表T = T * MatrixExp6(VecTose3(Blist.col(i) * thetaList(i))); // 计算每个关节的变换矩阵并相乘}return T; // 返回末端执行器的变换矩阵}/* 函数：计算空间雅可比矩阵* 输入：初始位置的螺旋轴，关节配置* 返回：6xn的空间雅可比矩阵*/Eigen::MatrixXd JacobianSpace(const Eigen::MatrixXd& Slist, const Eigen::MatrixXd& thetaList) {Eigen::MatrixXd Js = Slist; // 初始化空间雅可比矩阵为螺旋轴列表Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4, 4); // 初始化4x4单位矩阵Eigen::VectorXd sListTemp(Slist.col(0).size()); // 定义临时向量存储螺旋轴for (int i = 1; i < thetaList.size(); i++) { // 遍历关节配置sListTemp << Slist.col(i - 1) * thetaList(i - 1); // 计算当前关节的螺旋轴T = T * MatrixExp6(VecTose3(sListTemp)); // 计算变换矩阵// std::cout << "array: " << sListTemp << std::endl;Js.col(i) = Adjoint(T) * Slist.col(i); // 计算雅可比矩阵的列}return Js; // 返回空间雅可比矩阵}/** 函数：计算身体雅可比矩阵* 输入：身体位置的螺旋轴，关节配置* 返回：6xn的身体雅可比矩阵*/Eigen::MatrixXd JacobianBody(const Eigen::MatrixXd& Blist, const Eigen::MatrixXd& thetaList) {Eigen::MatrixXd Jb = Blist; // 初始化身体雅可比矩阵为螺旋轴列表Eigen::MatrixXd T = Eigen::MatrixXd::Identity(4, 4); // 初始化4x4单位矩阵Eigen::VectorXd bListTemp(Blist.col(0).size()); // 定义临时向量存储螺旋轴for (int i = thetaList.size() - 2; i >= 0; i--) { // 逆序遍历关节配置bListTemp << Blist.col(i + 1) * thetaList(i + 1); // 计算当前关节的螺旋轴T = T * MatrixExp6(VecTose3(-1 * bListTemp)); // 计算变换矩阵// std::cout << "array: " << sListTemp << std::endl;Jb.col(i) = Adjoint(T) * Blist.col(i); // 计算雅可比矩阵的列}return Jb; // 返回身体雅可比矩阵}/* 函数：计算变换矩阵的逆* 输入：变换矩阵* 返回：变换矩阵的逆*/Eigen::MatrixXd TransInv(const Eigen::MatrixXd& transform) {auto rp = mr::TransToRp(transform); // 将变换矩阵分解为旋转矩阵和位置向量auto Rt = rp.at(0).transpose(); // 计算旋转矩阵的转置auto t = -(Rt * rp.at(1)); // 计算位置向量的逆Eigen::MatrixXd inv(4, 4); // 定义4x4矩阵inv = Eigen::MatrixXd::Zero(4, 4); // 初始化为零矩阵inv.block(0, 0, 3, 3) = Rt; // 填充旋转矩阵的转置inv.block(0, 3, 3, 1) = t; // 填充位置向量的逆inv(3, 3) = 1; // 填充最后一行return inv; // 返回逆矩阵}/* 函数：计算旋转矩阵的逆* 输入：旋转矩阵* 返回：旋转矩阵的逆*/Eigen::MatrixXd RotInv(const Eigen::MatrixXd& rotMatrix) {return rotMatrix.transpose(); // 返回旋转矩阵的转置}/* 函数：将螺旋轴转换为轴向量* 输入：位置向量q，方向向量s，螺旋轴的螺距h* 返回：6维轴向量*/Eigen::VectorXd ScrewToAxis(Eigen::Vector3d q, Eigen::Vector3d s, double h) {Eigen::VectorXd axis(6); // 定义6维向量axis.segment(0, 3) = s; // 填充方向向量axis.segment(3, 3) = q.cross(s) + (h * s); // 计算并填充位置向量return axis; // 返回轴向量}/* 函数：将指数坐标转换为轴角表示* 输入：6维指数坐标* 返回：7维轴角表示*/Eigen::VectorXd AxisAng6(const Eigen::VectorXd& expc6) {Eigen::VectorXd v_ret(7); // 定义7维向量double theta = Eigen::Vector3d(expc6(0), expc6(1), expc6(2)).norm(); // 计算旋转角度if (NearZero(theta)) // 如果旋转角度接近零theta = Eigen::Vector3d(expc6(3), expc6(4), expc6(5)).norm(); // 计算平移量的范数v_ret << expc6 / theta, theta; // 填充轴角表示return v_ret; // 返回轴角表示}/* 函数：将矩阵投影到SO(3)群* 输入：矩阵M* 返回：SO(3)群的矩阵*/Eigen::MatrixXd ProjectToSO3(const Eigen::MatrixXd& M) {Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); // 计算奇异值分解Eigen::MatrixXd R = svd.matrixU() * svd.matrixV().transpose(); // 计算旋转矩阵if (R.determinant() < 0) // 如果行列式小于0// 在这种情况下，结果可能远离M；反转第三列的符号R.col(2) *= -1;return R; // 返回旋转矩阵}/* 函数：将矩阵投影到SE(3)群* 输入：矩阵M* 返回：SE(3)群的矩阵*/Eigen::MatrixXd ProjectToSE3(const Eigen::MatrixXd& M) {Eigen::Matrix3d R = M.block<3, 3>(0, 0); // 提取旋转矩阵Eigen::Vector3d t = M.block<3, 1>(0, 3); // 提取位置向量Eigen::MatrixXd T = RpToTrans(ProjectToSO3(R), t); // 计算SE(3)群的矩阵return T; // 返回SE(3)群的矩阵}/* 函数：计算矩阵到SO(3)群的距离* 输入：3x3矩阵M* 返回：距离*/double DistanceToSO3(const Eigen::Matrix3d& M) {if (M.determinant() > 0) // 如果行列式大于0return (M.transpose() * M - Eigen::Matrix3d::Identity()).norm(); // 计算距离elsereturn 1.0e9; // 返回一个大值表示距离很远}/* 函数：计算矩阵到SE(3)群的距离* 输入：4x4矩阵T* 返回：距离*/double DistanceToSE3(const Eigen::Matrix4d& T) {Eigen::Matrix3d matR = T.block<3, 3>(0, 0); // 提取旋转矩阵if (matR.determinant() > 0) { // 如果行列式大于0Eigen::Matrix4d m_ret; // 定义4x4矩阵m_ret << matR.transpose() * matR, Eigen::Vector3d::Zero(3), // 计算距离矩阵T.row(3);m_ret = m_ret - Eigen::Matrix4d::Identity(); // 计算距离return m_ret.norm(); // 返回距离}elsereturn 1.0e9; // 返回一个大值表示距离很远}/* 函数：测试矩阵是否属于SO(3)群* 输入：3x3矩阵M* 返回：布尔值，表示矩阵是否属于SO(3)群*/bool TestIfSO3(const Eigen::Matrix3d& M) {return std::abs(DistanceToSO3(M)) < 1e-3; // 判断矩阵到SO(3)群的距离是否小于1e-3}/* 函数：测试矩阵是否属于SE(3)群* 输入：4x4矩阵T* 返回：布尔值，表示矩阵是否属于SE(3)群*/bool TestIfSE3(const Eigen::Matrix4d& T) {return std::abs(DistanceToSE3(T)) < 1e-3; // 判断矩阵到SE(3)群的距离是否小于1e-3}/* 函数：计算逆运动学（身体坐标系）* 输入：螺旋轴列表Blist，末端执行器的初始配置M，目标变换矩阵T，关节角度列表thetalist，角速度误差阈值eomg，线速度误差阈值ev* 返回：布尔值，表示是否成功找到解*/bool IKinBody(const Eigen::MatrixXd& Blist, const Eigen::MatrixXd& M, const Eigen::MatrixXd& T,Eigen::VectorXd& thetalist, double eomg, double ev) {int i = 0; // 迭代次数int maxiterations = 20; // 最大迭代次数Eigen::MatrixXd Tfk = FKinBody(M, Blist, thetalist); // 计算当前关节角度下的末端执行器变换矩阵Eigen::MatrixXd Tdiff = TransInv(Tfk) * T; // 计算变换矩阵差Eigen::VectorXd Vb = se3ToVec(MatrixLog6(Tdiff)); // 计算变换矩阵对数Eigen::Vector3d angular(Vb(0), Vb(1), Vb(2)); // 提取角速度Eigen::Vector3d linear(Vb(3), Vb(4), Vb(5)); // 提取线速度bool err = (angular.norm() > eomg || linear.norm() > ev); // 判断误差是否超过阈值Eigen::MatrixXd Jb; // 定义雅可比矩阵while (err && i < maxiterations) { // 迭代求解Jb = JacobianBody(Blist, thetalist); // 计算雅可比矩阵thetalist += Jb.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Vb); // 更新关节角度i += 1; // 增加迭代次数// 迭代计算Tfk = FKinBody(M, Blist, thetalist); // 计算当前关节角度下的末端执行器变换矩阵Tdiff = TransInv(Tfk) * T; // 计算变换矩阵差Vb = se3ToVec(MatrixLog6(Tdiff)); // 计算变换矩阵对数angular = Eigen::Vector3d(Vb(0), Vb(1), Vb(2)); // 提取角速度linear = Eigen::Vector3d(Vb(3), Vb(4), Vb(5)); // 提取线速度err = (angular.norm() > eomg || linear.norm() > ev); // 判断误差是否超过阈值}return !err; // 返回是否成功找到解}/* 函数：计算逆运动学（空间坐标系）* 输入：螺旋轴列表Slist，末端执行器的初始配置M，目标变换矩阵T，关节角度列表thetalist，角速度误差阈值eomg，线速度误差阈值ev* 返回：布尔值，表示是否成功找到解*/bool IKinSpace(const Eigen::MatrixXd& Slist, const Eigen::MatrixXd& M, const Eigen::MatrixXd& T,Eigen::VectorXd& thetalist, double eomg, double ev) {int i = 0; // 迭代次数int maxiterations = 20; // 最大迭代次数Eigen::MatrixXd Tfk = FKinSpace(M, Slist, thetalist); // 计算当前关节角度下的末端执行器变换矩阵Eigen::MatrixXd Tdiff = TransInv(Tfk) * T; // 计算变换矩阵差Eigen::VectorXd Vs = Adjoint(Tfk) * se3ToVec(MatrixLog6(Tdiff)); // 计算变换矩阵对数并转换到空间坐标系Eigen::Vector3d angular(Vs(0), Vs(1), Vs(2)); // 提取角速度Eigen::Vector3d linear(Vs(3), Vs(4), Vs(5)); // 提取线速度bool err = (angular.norm() > eomg || linear.norm() > ev); // 判断误差是否超过阈值Eigen::MatrixXd Js; // 定义雅可比矩阵while (err && i < maxiterations) { // 迭代求解Js = JacobianSpace(Slist, thetalist); // 计算雅可比矩阵thetalist += Js.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Vs); // 更新关节角度i += 1; // 增加迭代次数// 迭代计算Tfk = FKinSpace(M, Slist, thetalist); // 计算当前关节角度下的末端执行器变换矩阵Tdiff = TransInv(Tfk) * T; // 计算变换矩阵差Vs = Adjoint(Tfk) * se3ToVec(MatrixLog6(Tdiff)); // 计算变换矩阵对数并转换到空间坐标系angular = Eigen::Vector3d(Vs(0), Vs(1), Vs(2)); // 提取角速度linear = Eigen::Vector3d(Vs(3), Vs(4), Vs(5)); // 提取线速度err = (angular.norm() > eomg || linear.norm() > ev); // 判断误差是否超过阈值}return !err; // 返回是否成功找到解}/** 函数：使用前向-后向牛顿-欧拉迭代求解方程：* taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...*           + g(thetalist) + Jtr(thetalist) * Ftip* 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度的n维向量*  ddthetalist: 关节加速度的n维向量*  g: 重力向量g*  Ftip: 末端执行器施加的空间力，以{n+1}坐标系表示*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  taulist: 所需关节力/力矩的n维向量**/Eigen::VectorXd InverseDynamics(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::VectorXd& ddthetalist,const Eigen::VectorXd& g, const Eigen::VectorXd& Ftip, const std::vector<Eigen::MatrixXd>& Mlist,const std::vector<Eigen::MatrixXd>& Glist, const Eigen::MatrixXd& Slist) {// 列表的大小int n = thetalist.size();Eigen::MatrixXd Mi = Eigen::MatrixXd::Identity(4, 4); // 初始化4x4单位矩阵Eigen::MatrixXd Ai = Eigen::MatrixXd::Zero(6, n); // 初始化6xn零矩阵std::vector<Eigen::MatrixXd> AdTi; // 定义伴随矩阵列表for (int i = 0; i < n + 1; i++) {AdTi.push_back(Eigen::MatrixXd::Zero(6, 6)); // 初始化6x6零矩阵并添加到伴随矩阵列表中}Eigen::MatrixXd Vi = Eigen::MatrixXd::Zero(6, n + 1); // 初始化6x(n+1)零矩阵，用于存储速度Eigen::MatrixXd Vdi = Eigen::MatrixXd::Zero(6, n + 1); // 初始化6x(n+1)零矩阵，用于存储加速度Vdi.block(3, 0, 3, 1) = -g; // 设置初始加速度为重力向量的负值AdTi[n] = mr::Adjoint(mr::TransInv(Mlist[n])); // 计算末端执行器的伴随矩阵Eigen::VectorXd Fi = Ftip; // 初始化末端执行器施加的力Eigen::VectorXd taulist = Eigen::VectorXd::Zero(n); // 初始化关节力/力矩的n维向量// 前向迭代for (int i = 0; i < n; i++) {Mi = Mi * Mlist[i]; // 更新变换矩阵Ai.col(i) = mr::Adjoint(mr::TransInv(Mi)) * Slist.col(i); // 计算螺旋轴的伴随表示AdTi[i] = mr::Adjoint(mr::MatrixExp6(mr::VecTose3(Ai.col(i) * -thetalist(i)))* mr::TransInv(Mlist[i])); // 计算伴随矩阵Vi.col(i + 1) = AdTi[i] * Vi.col(i) + Ai.col(i) * dthetalist(i); // 计算速度Vdi.col(i + 1) = AdTi[i] * Vdi.col(i) + Ai.col(i) * ddthetalist(i)+ ad(Vi.col(i + 1)) * Ai.col(i) * dthetalist(i); // 计算加速度}// 后向迭代for (int i = n - 1; i >= 0; i--) {Fi = AdTi[i + 1].transpose() * Fi + Glist[i] * Vdi.col(i + 1)- ad(Vi.col(i + 1)).transpose() * (Glist[i] * Vi.col(i + 1)); // 计算力taulist(i) = Fi.transpose() * Ai.col(i); // 计算关节力/力矩}return taulist; // 返回关节力/力矩的n维向量}/** 函数：此函数调用InverseDynamics，设置Ftip = 0，dthetalist = 0，ddthetalist = 0。* 目的是计算动力学方程中的一个重要项* 输入：*  thetalist: 关节变量的n维向量*  g: 重力向量g*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  grav: 显示重力对动力学影响的3维向量**/Eigen::VectorXd GravityForces(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& g,const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist, const Eigen::MatrixXd& Slist) {int n = thetalist.size(); // 获取关节变量的数量Eigen::VectorXd dummylist = Eigen::VectorXd::Zero(n); // 初始化n维零向量Eigen::VectorXd dummyForce = Eigen::VectorXd::Zero(6); // 初始化6维零向量Eigen::VectorXd grav = mr::InverseDynamics(thetalist, dummylist, dummylist, g,dummyForce, Mlist, Glist, Slist); // 调用InverseDynamics计算重力影响return grav; // 返回重力影响向量}/** 函数：此函数调用InverseDynamics n次，每次传递一个单元素为1的ddthetalist向量，其他输入为零。* 每次调用InverseDynamics生成一个列，这些列组合起来形成惯性矩阵。** 输入：*  thetalist: 关节变量的n维向量*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  M: 给定配置thetalist的n关节串联链的数值惯性矩阵M(thetalist)*/Eigen::MatrixXd MassMatrix(const Eigen::VectorXd& thetalist,const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist, const Eigen::MatrixXd& Slist) {int n = thetalist.size(); // 获取关节变量的数量Eigen::VectorXd dummylist = Eigen::VectorXd::Zero(n); // 初始化n维零向量Eigen::VectorXd dummyg = Eigen::VectorXd::Zero(3); // 初始化3维零向量Eigen::VectorXd dummyforce = Eigen::VectorXd::Zero(6); // 初始化6维零向量Eigen::MatrixXd M = Eigen::MatrixXd::Zero(n, n); // 初始化n x n零矩阵for (int i = 0; i < n; i++) { // 遍历每个关节变量Eigen::VectorXd ddthetalist = Eigen::VectorXd::Zero(n); // 初始化n维零向量ddthetalist(i) = 1; // 设置当前关节变量的加速度为1M.col(i) = mr::InverseDynamics(thetalist, dummylist, ddthetalist,dummyg, dummyforce, Mlist, Glist, Slist); // 调用InverseDynamics计算惯性矩阵的列}return M; // 返回惯性矩阵}/** 函数：此函数调用InverseDynamics，设置g = 0，Ftip = 0，ddthetalist = 0。** 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度的列表*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  c: 给定thetalist和dthetalist的科里奥利和离心项向量c(thetalist,dthetalist)*/Eigen::VectorXd VelQuadraticForces(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist,const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist, const Eigen::MatrixXd& Slist) {int n = thetalist.size(); // 获取关节变量的数量Eigen::VectorXd dummylist = Eigen::VectorXd::Zero(n); // 初始化n维零向量Eigen::VectorXd dummyg = Eigen::VectorXd::Zero(3); // 初始化3维零向量Eigen::VectorXd dummyforce = Eigen::VectorXd::Zero(6); // 初始化6维零向量Eigen::VectorXd c = mr::InverseDynamics(thetalist, dthetalist, dummylist,dummyg, dummyforce, Mlist, Glist, Slist); // 调用InverseDynamics计算科里奥利和离心项向量return c; // 返回科里奥利和离心项向量}/** 函数：此函数调用InverseDynamics，设置g = 0，dthetalist = 0，ddthetalist = 0。** 输入：*  thetalist: 关节变量的n维向量*  Ftip: 末端执行器施加的空间力，以{n+1}坐标系表示*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  JTFtip: 仅为产生末端执行器力Ftip所需的关节力和力矩*/Eigen::VectorXd EndEffectorForces(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& Ftip,const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist, const Eigen::MatrixXd& Slist) {int n = thetalist.size(); // 获取关节变量的数量Eigen::VectorXd dummylist = Eigen::VectorXd::Zero(n); // 初始化n维零向量Eigen::VectorXd dummyg = Eigen::VectorXd::Zero(3); // 初始化3维零向量Eigen::VectorXd JTFtip = mr::InverseDynamics(thetalist, dummylist, dummylist,dummyg, Ftip, Mlist, Glist, Slist); // 调用InverseDynamics计算关节力和力矩return JTFtip; // 返回关节力和力矩}/** 函数：通过求解以下方程计算ddthetalist：* Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist)*                                  - g(thetalist) - Jtr(thetalist) * Ftip* 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度的n维向量*  taulist: 关节力/力矩的n维向量*  g: 重力向量g*  Ftip: 末端执行器施加的空间力，以{n+1}坐标系表示*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  ddthetalist: 计算得到的关节加速度**/Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::VectorXd& taulist,const Eigen::VectorXd& g, const Eigen::VectorXd& Ftip, const std::vector<Eigen::MatrixXd>& Mlist,const std::vector<Eigen::MatrixXd>& Glist, const Eigen::MatrixXd& Slist) {Eigen::VectorXd totalForce = taulist - mr::VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist)- mr::GravityForces(thetalist, g, Mlist, Glist, Slist)- mr::EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist); // 计算总力Eigen::MatrixXd M = mr::MassMatrix(thetalist, Mlist, Glist, Slist); // 计算质量矩阵// 使用LDLT分解，因为M是正定矩阵Eigen::VectorXd ddthetalist = M.ldlt().solve(totalForce); // 求解关节加速度return ddthetalist; // 返回关节加速度}/* 函数：使用欧拉法更新关节角度和速度* 输入：*  thetalist: 关节角度的向量*  dthetalist: 关节速度的向量*  ddthetalist: 关节加速度的向量*  dt: 时间步长*/void EulerStep(Eigen::VectorXd& thetalist, Eigen::VectorXd& dthetalist, const Eigen::VectorXd& ddthetalist, double dt) {thetalist += dthetalist * dt; // 更新关节角度dthetalist += ddthetalist * dt; // 更新关节速度return;}/* 函数：计算逆动力学轨迹* 输入：*  thetamat: 关节角度矩阵*  dthetamat: 关节速度矩阵*  ddthetamat: 关节加速度矩阵*  g: 重力向量g*  Ftipmat: 末端执行器施加的空间力矩阵*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  taumat: 关节力/力矩矩阵*/Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd& thetamat, const Eigen::MatrixXd& dthetamat, const Eigen::MatrixXd& ddthetamat,const Eigen::VectorXd& g, const Eigen::MatrixXd& Ftipmat, const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist,const Eigen::MatrixXd& Slist) {Eigen::MatrixXd thetamatT = thetamat.transpose(); // 转置关节角度矩阵Eigen::MatrixXd dthetamatT = dthetamat.transpose(); // 转置关节速度矩阵Eigen::MatrixXd ddthetamatT = ddthetamat.transpose(); // 转置关节加速度矩阵Eigen::MatrixXd FtipmatT = Ftipmat.transpose(); // 转置末端执行器力矩阵int N = thetamat.rows();  // 轨迹点数量int dof = thetamat.cols(); // 自由度数量Eigen::MatrixXd taumatT = Eigen::MatrixXd::Zero(dof, N); // 初始化关节力/力矩矩阵for (int i = 0; i < N; ++i) { // 遍历每个轨迹点taumatT.col(i) = InverseDynamics(thetamatT.col(i), dthetamatT.col(i), ddthetamatT.col(i), g, FtipmatT.col(i), Mlist, Glist, Slist); // 计算关节力/力矩}Eigen::MatrixXd taumat = taumatT.transpose(); // 转置关节力/力矩矩阵return taumat; // 返回关节力/力矩矩阵}/** 函数：计算正向动力学轨迹* 输入：*  thetalist: 关节角度的向量*  dthetalist: 关节速度的向量*  taumat: 力/力矩矩阵*  g: 重力向量*  Ftipmat: 末端执行器施加的空间力矩阵*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式*  dt: 时间步长*  intRes: 积分分辨率* 输出：*  JointTraj_ret: 包含关节角度和速度轨迹的向量*/std::vector<Eigen::MatrixXd> ForwardDynamicsTrajectory(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::MatrixXd& taumat,const Eigen::VectorXd& g, const Eigen::MatrixXd& Ftipmat, const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist,const Eigen::MatrixXd& Slist, double dt, int intRes) {Eigen::MatrixXd taumatT = taumat.transpose(); // 转置力/力矩矩阵Eigen::MatrixXd FtipmatT = Ftipmat.transpose(); // 转置末端执行器力矩阵int N = taumat.rows();  // 力/力矩点的数量int dof = taumat.cols(); // 自由度数量Eigen::MatrixXd thetamatT = Eigen::MatrixXd::Zero(dof, N); // 初始化关节角度矩阵Eigen::MatrixXd dthetamatT = Eigen::MatrixXd::Zero(dof, N); // 初始化关节速度矩阵thetamatT.col(0) = thetalist; // 设置初始关节角度dthetamatT.col(0) = dthetalist; // 设置初始关节速度Eigen::VectorXd thetacurrent = thetalist; // 当前关节角度Eigen::VectorXd dthetacurrent = dthetalist; // 当前关节速度Eigen::VectorXd ddthetalist; // 关节加速度for (int i = 0; i < N - 1; ++i) { // 遍历每个力/力矩点for (int j = 0; j < intRes; ++j) { // 进行积分分辨率次迭代ddthetalist = ForwardDynamics(thetacurrent, dthetacurrent, taumatT.col(i), g, FtipmatT.col(i), Mlist, Glist, Slist); // 计算关节加速度EulerStep(thetacurrent, dthetacurrent, ddthetalist, 1.0 * dt / intRes); // 使用欧拉法更新关节角度和速度}thetamatT.col(i + 1) = thetacurrent; // 更新关节角度矩阵dthetamatT.col(i + 1) = dthetacurrent; // 更新关节速度矩阵}std::vector<Eigen::MatrixXd> JointTraj_ret; // 定义返回的关节轨迹向量JointTraj_ret.push_back(thetamatT.transpose()); // 添加关节角度轨迹JointTraj_ret.push_back(dthetamatT.transpose()); // 添加关节速度轨迹return JointTraj_ret; // 返回关节轨迹向量}/* 函数：计算关节力矩* 输入：*  thetalist: 关节角度的向量*  dthetalist: 关节速度的向量*  eint: 积分误差*  g: 重力向量*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式*  thetalistd: 期望关节角度*  dthetalistd: 期望关节速度*  ddthetalistd: 期望关节加速度*  Kp: 比例增益*  Ki: 积分增益*  Kd: 微分增益* 输出：*  tau_computed: 计算得到的关节力矩*/Eigen::VectorXd ComputedTorque(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::VectorXd& eint,const Eigen::VectorXd& g, const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist,const Eigen::MatrixXd& Slist, const Eigen::VectorXd& thetalistd, const Eigen::VectorXd& dthetalistd, const Eigen::VectorXd& ddthetalistd,double Kp, double Ki, double Kd) {Eigen::VectorXd e = thetalistd - thetalist;  // 位置误差Eigen::VectorXd tau_feedforward = MassMatrix(thetalist, Mlist, Glist, Slist) * (Kp * e + Ki * (eint + e) + Kd * (dthetalistd - dthetalist)); // 前馈力矩Eigen::VectorXd Ftip = Eigen::VectorXd::Zero(6); // 初始化末端执行器力Eigen::VectorXd tau_inversedyn = InverseDynamics(thetalist, dthetalist, ddthetalistd, g, Ftip, Mlist, Glist, Slist); // 逆动力学力矩Eigen::VectorXd tau_computed = tau_feedforward + tau_inversedyn; // 计算总力矩return tau_computed; // 返回计算得到的关节力矩}/* 函数：三次时间缩放* 输入：*  Tf: 总时间*  t: 当前时间* 输出：*  st: 缩放后的时间*/double CubicTimeScaling(double Tf, double t) {double timeratio = 1.0 * t / Tf; // 计算时间比率double st = 3 * pow(timeratio, 2) - 2 * pow(timeratio, 3); // 计算三次时间缩放return st; // 返回缩放后的时间}/* 函数：五次时间缩放* 输入：*  Tf: 总时间*  t: 当前时间* 输出：*  st: 缩放后的时间*/double QuinticTimeScaling(double Tf, double t) {double timeratio = 1.0 * t / Tf; // 计算时间比率double st = 10 * pow(timeratio, 3) - 15 * pow(timeratio, 4) + 6 * pow(timeratio, 5); // 计算五次时间缩放return st; // 返回缩放后的时间}/* 函数：计算关节轨迹* 输入：*  thetastart: 初始关节角度*  thetaend: 结束关节角度*  Tf: 总时间*  N: 轨迹点数量*  method: 时间缩放方法（3表示三次，5表示五次）* 输出：*  traj: 关节轨迹矩阵*/Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd& thetastart, const Eigen::VectorXd& thetaend, double Tf, int N, int method) {double timegap = Tf / (N - 1); // 计算时间间隔Eigen::MatrixXd trajT = Eigen::MatrixXd::Zero(thetastart.size(), N); // 初始化轨迹矩阵double st;for (int i = 0; i < N; ++i) { // 遍历每个轨迹点if (method == 3)st = CubicTimeScaling(Tf, timegap * i); // 三次时间缩放elsest = QuinticTimeScaling(Tf, timegap * i); // 五次时间缩放trajT.col(i) = st * thetaend + (1 - st) * thetastart; // 计算轨迹点}Eigen::MatrixXd traj = trajT.transpose(); // 转置轨迹矩阵return traj; // 返回关节轨迹矩阵}/* 函数：计算螺旋轨迹* 输入：*  Xstart: 初始变换矩阵*  Xend: 结束变换矩阵*  Tf: 总时间*  N: 轨迹点数量*  method: 时间缩放方法（3表示三次，5表示五次）* 输出：*  traj: 螺旋轨迹向量*/std::vector<Eigen::MatrixXd> ScrewTrajectory(const Eigen::MatrixXd& Xstart, const Eigen::MatrixXd& Xend, double Tf, int N, int method) {double timegap = Tf / (N - 1); // 计算时间间隔std::vector<Eigen::MatrixXd> traj(N); // 初始化轨迹向量double st;for (int i = 0; i < N; ++i) { // 遍历每个轨迹点if (method == 3)st = CubicTimeScaling(Tf, timegap * i); // 三次时间缩放elsest = QuinticTimeScaling(Tf, timegap * i); // 五次时间缩放Eigen::MatrixXd Ttemp = MatrixLog6(TransInv(Xstart) * Xend); // 计算变换矩阵对数traj.at(i) = Xstart * MatrixExp6(Ttemp * st); // 计算轨迹点}return traj; // 返回螺旋轨迹向量}/** 函数：计算笛卡尔轨迹* 输入：*  Xstart: 初始变换矩阵*  Xend: 结束变换矩阵*  Tf: 总时间*  N: 轨迹点数量*  method: 时间缩放方法（3表示三次，5表示五次）* 输出：*  traj: 笛卡尔轨迹向量*/std::vector<Eigen::MatrixXd> CartesianTrajectory(const Eigen::MatrixXd& Xstart, const Eigen::MatrixXd& Xend, double Tf, int N, int method) {double timegap = Tf / (N - 1); // 计算时间间隔std::vector<Eigen::MatrixXd> traj(N); // 初始化轨迹向量std::vector<Eigen::MatrixXd> Rpstart = TransToRp(Xstart); // 将初始变换矩阵分解为旋转矩阵和位置向量std::vector<Eigen::MatrixXd> Rpend = TransToRp(Xend); // 将结束变换矩阵分解为旋转矩阵和位置向量Eigen::Matrix3d Rstart = Rpstart[0]; Eigen::Vector3d pstart = Rpstart[1]; // 提取初始旋转矩阵和位置向量Eigen::Matrix3d Rend = Rpend[0]; Eigen::Vector3d pend = Rpend[1]; // 提取结束旋转矩阵和位置向量double st;for (int i = 0; i < N; ++i) { // 遍历每个轨迹点if (method == 3)st = CubicTimeScaling(Tf, timegap * i); // 三次时间缩放elsest = QuinticTimeScaling(Tf, timegap * i); // 五次时间缩放Eigen::Matrix3d Ri = Rstart * MatrixExp3(MatrixLog3(Rstart.transpose() * Rend) * st); // 计算当前旋转矩阵Eigen::Vector3d pi = st * pend + (1 - st) * pstart; // 计算当前位置向量Eigen::MatrixXd traji(4, 4); // 定义4x4变换矩阵traji << Ri, pi, // 填充旋转矩阵和位置向量0, 0, 0, 1; // 填充最后一行traj.at(i) = traji; // 将当前变换矩阵添加到轨迹向量中}return traj; // 返回笛卡尔轨迹向量}/** 函数：模拟控制* 输入：*  thetalist: 关节角度的向量*  dthetalist: 关节速度的向量*  g: 重力向量*  Ftipmat: 末端执行器施加的空间力矩阵*  Mlist: 链接帧{i}相对于{i-1}在初始位置的列表*  Glist: 链接的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式*  thetamatd: 期望关节角度矩阵*  dthetamatd: 期望关节速度矩阵*  ddthetamatd: 期望关节加速度矩阵*  gtilde: 估计的重力向量*  Mtildelist: 估计的链接帧列表*  Gtildelist: 估计的空间惯性矩阵列表*  Kp: 比例增益*  Ki: 积分增益*  Kd: 微分增益*  dt: 时间步长*  intRes: 积分分辨率* 输出：*  ControlTauTraj_ret: 包含关节力矩和角度轨迹的向量*/std::vector<Eigen::MatrixXd> SimulateControl(const Eigen::VectorXd& thetalist, const Eigen::VectorXd& dthetalist, const Eigen::VectorXd& g,const Eigen::MatrixXd& Ftipmat, const std::vector<Eigen::MatrixXd>& Mlist, const std::vector<Eigen::MatrixXd>& Glist,const Eigen::MatrixXd& Slist, const Eigen::MatrixXd& thetamatd, const Eigen::MatrixXd& dthetamatd, const Eigen::MatrixXd& ddthetamatd,const Eigen::VectorXd& gtilde, const std::vector<Eigen::MatrixXd>& Mtildelist, const std::vector<Eigen::MatrixXd>& Gtildelist,double Kp, double Ki, double Kd, double dt, int intRes) {Eigen::MatrixXd FtipmatT = Ftipmat.transpose(); // 转置末端执行器力矩阵Eigen::MatrixXd thetamatdT = thetamatd.transpose(); // 转置期望关节角度矩阵Eigen::MatrixXd dthetamatdT = dthetamatd.transpose(); // 转置期望关节速度矩阵Eigen::MatrixXd ddthetamatdT = ddthetamatd.transpose(); // 转置期望关节加速度矩阵int m = thetamatdT.rows(); int n = thetamatdT.cols(); // 获取矩阵的行数和列数Eigen::VectorXd thetacurrent = thetalist; // 当前关节角度Eigen::VectorXd dthetacurrent = dthetalist; // 当前关节速度Eigen::VectorXd eint = Eigen::VectorXd::Zero(m); // 初始化积分误差Eigen::MatrixXd taumatT = Eigen::MatrixXd::Zero(m, n); // 初始化关节力矩矩阵Eigen::MatrixXd thetamatT = Eigen::MatrixXd::Zero(m, n); // 初始化关节角度矩阵Eigen::VectorXd taulist; // 关节力矩向量Eigen::VectorXd ddthetalist; // 关节加速度向量for (int i = 0; i < n; ++i) { // 遍历每个时间步taulist = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, Mtildelist, Gtildelist, Slist, thetamatdT.col(i),dthetamatdT.col(i), ddthetamatdT.col(i), Kp, Ki, Kd); // 计算关节力矩for (int j = 0; j < intRes; ++j) { // 进行积分分辨率次迭代ddthetalist = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, FtipmatT.col(i), Mlist, Glist, Slist); // 计算关节加速度EulerStep(thetacurrent, dthetacurrent, ddthetalist, dt / intRes); // 使用欧拉法更新关节角度和速度}taumatT.col(i) = taulist; // 更新关节力矩矩阵thetamatT.col(i) = thetacurrent; // 更新关节角度矩阵eint += dt * (thetamatdT.col(i) - thetacurrent); // 更新积分误差}std::vector<Eigen::MatrixXd> ControlTauTraj_ret; // 定义返回的控制轨迹向量ControlTauTraj_ret.push_back(taumatT.transpose()); // 添加关节力矩轨迹ControlTauTraj_ret.push_back(thetamatT.transpose()); // 添加关节角度轨迹return ControlTauTraj_ret; // 返回控制轨迹向量}

库测试 lib_test.cpp

#include <iostream> // 包含输入输出流库
#include <Eigen/Dense> // 包含Eigen库，用于矩阵和向量运算
#include "../include/modern_robotics.h" // 包含现代机器人库的头文件
#include "gtest/gtest.h" // 包含Google测试框架的头文件# define M_PI 3.14159265358979323846  /* 定义圆周率常量 *//* 测试函数：VecToSO3Test */
TEST(MRTest, VecToSO3Test) {Eigen::Vector3d vec(1, 2, 3); // 定义3维向量Eigen::Matrix3d result(3, 3); // 定义3x3矩阵result << 0, -3, 2, // 填充矩阵元素3, 0, -1,-2, 1, 0;EXPECT_EQ(result, mr::VecToso3(vec)); // 断言VecToso3函数的输出与预期结果相等
}/* 测试函数：JacobianSpaceTest */
TEST(MRTest, JacobianSpaceTest) {Eigen::MatrixXd s_list(6, 3); // 定义6x3矩阵，用于存储螺旋轴s_list << 0, 0, 0, // 填充矩阵元素0, 1, -1,1, 0, 0,0, -0.0711, 0.0711,0, 0, 0,0, 0, -0.2795;Eigen::VectorXd theta(3); // 定义3维向量，用于存储关节角度theta << 1.0472, 1.0472, 1.0472; // 填充向量元素Eigen::MatrixXd result(6, 3); // 定义6x3矩阵，用于存储预期结果result << 0, -0.866, 0.866, // 填充矩阵元素0, 0.5, -0.5,1, 0, 0,0, -0.0355, -0.0855,0, -0.0615, -0.1481,0, 0, -0.1398;Eigen::MatrixXd tmp_result = mr::JacobianSpace(s_list, theta); // 计算空间雅可比矩阵// std::cout << tmp_result << std::endl;ASSERT_TRUE(mr::JacobianSpace(s_list, theta).isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：JacobianBodyTest */
TEST(MRTest, JacobianBodyTest) {Eigen::MatrixXd b_list(6, 3); // 定义6x3矩阵，用于存储螺旋轴b_list << 0, 0, 0, // 填充矩阵元素0, 1, -1,1, 0, 0,0.0425, 0, 0,0.5515, 0, 0,0, -0.5515, 0.2720;Eigen::VectorXd theta(3); // 定义3维向量，用于存储关节角度theta << 0, 0, 1.5708; // 填充向量元素Eigen::MatrixXd result(6, 3); // 定义6x3矩阵，用于存储预期结果result << 1, 0, 0, // 填充矩阵元素0, 1, -1,0, 0, 0,0, -0.2795, 0,0.2795, 0, 0,-0.0425, -0.2720, 0.2720;Eigen::MatrixXd tmp_result = mr::JacobianBody(b_list, theta); // 计算身体雅可比矩阵// std::cout << tmp_result << std::endl;ASSERT_TRUE(mr::JacobianBody(b_list, theta).isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：adTest */
TEST(MRTest, adTest) {Eigen::VectorXd V(6); // 定义6维向量V << 1, 2, 3, 4, 5, 6; // 填充向量元素Eigen::MatrixXd result(6, 6); // 定义6x6矩阵，用于存储预期结果result << 0, -3, 2, 0, 0, 0, // 填充矩阵元素3, 0, -1, 0, 0, 0,-2, 1, 0, 0, 0, 0,0, -6, 5, 0, -3, 2,6, 0, -4, 3, 0, -1,-5, 4, 0, -2, 1, 0;ASSERT_TRUE(mr::ad(V).isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：TransInvTest */
TEST(MRTest, TransInvTest) {Eigen::MatrixXd input(4, 4); // 定义4x4矩阵，用于存储输入变换矩阵input << 1, 0, 0, 0, // 填充矩阵元素0, 0, -1, 0,0, 1, 0, 3,0, 0, 0, 1;Eigen::MatrixXd result(4, 4); // 定义4x4矩阵，用于存储预期结果result << 1, 0, 0, 0, // 填充矩阵元素0, 0, 1, -3,0, -1, 0, 0,0, 0, 0, 1;auto inv = mr::TransInv(input); // 计算变换矩阵的逆ASSERT_TRUE(inv.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：RotInvTest */
TEST(MRTest, RotInvTest) {Eigen::MatrixXd input(3, 3); // 定义3x3矩阵，用于存储输入旋转矩阵input << 0, 0, 1, // 填充矩阵元素1, 0, 0,0, 1, 0;Eigen::MatrixXd result(3, 3); // 定义3x3矩阵，用于存储预期结果result << 0, 1, 0, // 填充矩阵元素0, 0, 1,1, 0, 0;auto inv = mr::RotInv(input); // 计算旋转矩阵的逆ASSERT_TRUE(inv.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：ScrewToAxisTest */
TEST(MRTest, ScrewToAxisTest) {Eigen::Vector3d q, s; // 定义3维向量q和sq << 3, 0, 1; // 填充向量q的元素s << 0, 0, 1; // 填充向量s的元素double h = 2; // 定义螺距Eigen::VectorXd axis = mr::ScrewToAxis(q, s, h); // 计算螺旋轴Eigen::VectorXd result(6); // 定义6维向量，用于存储预期结果result << 0, 0, 1, 0, -3, 2; // 填充向量元素ASSERT_TRUE(axis.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：FKInBodyTest */
TEST(MRTest, FKInBodyTest) {Eigen::MatrixXd M(4, 4); // 定义4x4矩阵，用于存储初始变换矩阵M << -1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 6,0, 0, -1, 2,0, 0, 0, 1;Eigen::MatrixXd Blist(6, 3); // 定义6x3矩阵，用于存储螺旋轴列表Blist << 0, 0, 0, // 填充矩阵元素0, 0, 0,-1, 0, 1,2, 0, 0,0, 1, 0,0, 0, 0.1;Eigen::VectorXd thetaList(3); // 定义3维向量，用于存储关节角度thetaList << M_PI / 2.0, 3, M_PI; // 填充向量元素Eigen::MatrixXd result(4, 4); // 定义4x4矩阵，用于存储预期结果result << 0, 1, 0, -5, // 填充矩阵元素1, 0, 0, 4,0, 0, -1, 1.68584073,0, 0, 0, 1;Eigen::MatrixXd FKCal = mr::FKinBody(M, Blist, thetaList); // 计算正向运动学ASSERT_TRUE(FKCal.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：FKInSpaceTest */
TEST(MRTest, FKInSpaceTest) {Eigen::MatrixXd M(4, 4); // 定义4x4矩阵，用于存储初始变换矩阵M << -1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 6,0, 0, -1, 2,0, 0, 0, 1;Eigen::MatrixXd Slist(6, 3); // 定义6x3矩阵，用于存储螺旋轴列表Slist << 0, 0, 0, // 填充矩阵元素0, 0, 0,1, 0, -1,4, 0, -6,0, 1, 0,0, 0, -0.1;Eigen::VectorXd thetaList(3); // 定义3维向量，用于存储关节角度thetaList << M_PI / 2.0, 3, M_PI; // 填充向量元素Eigen::MatrixXd result(4, 4); // 定义4x4矩阵，用于存储预期结果result << 0, 1, 0, -5, // 填充矩阵元素1, 0, 0, 4,0, 0, -1, 1.68584073,0, 0, 0, 1;Eigen::MatrixXd FKCal = mr::FKinBody(M, Slist, thetaList); // 计算正向运动学ASSERT_TRUE(FKCal.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：AxisAng6Test */
TEST(MRTest, AxisAng6Test) {Eigen::VectorXd input(6); // 定义6维向量，用于存储输入数据Eigen::VectorXd result(7); // 定义7维向量，用于存储预期结果input << 1.0, 0.0, 0.0, 1.0, 2.0, 3.0; // 填充输入向量元素result << 1.0, 0.0, 0.0, 1.0, 2.0, 3.0, 1.0; // 填充预期结果向量元素Eigen::VectorXd output = mr::AxisAng6(input); // 计算轴角表示ASSERT_TRUE(output.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：MatrixLog6Test */
TEST(MRTest, MatrixLog6Test) {Eigen::MatrixXd Tinput(4, 4); // 定义4x4矩阵，用于存储输入变换矩阵Eigen::MatrixXd result(4, 4); // 定义4x4矩阵，用于存储预期结果Tinput << 1, 0, 0, 0, // 填充矩阵元素0, 0, -1, 0,0, 1, 0, 3,0, 0, 0, 1;result << 0, 0, 0, 0, // 填充矩阵元素0, 0, -1.57079633, 2.35619449,0, 1.57079633, 0, 2.35619449,0, 0, 0, 0;Eigen::MatrixXd Toutput = mr::MatrixLog6(Tinput); // 计算变换矩阵的对数ASSERT_TRUE(Toutput.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：DistanceToSO3Test */
TEST(MRTest, DistanceToSO3Test) {Eigen::Matrix3d input; // 定义3x3矩阵，用于存储输入数据double result = 0.088353; // 定义预期结果input << 1.0, 0.0, 0.0, // 填充矩阵元素0.0, 0.1, -0.95,0.0, 1.0, 0.1;EXPECT_NEAR(result, mr::DistanceToSO3(input), 3); // 断言计算结果与预期结果近似相等
}/* 测试函数：DistanceToSE3Test */
TEST(MRTest, DistanceToSE3Test) {Eigen::Matrix4d input; // 定义4x4矩阵，用于存储输入数据double result = 0.134931; // 定义预期结果input << 1.0, 0.0, 0.0, 1.2, // 填充矩阵元素0.0, 0.1, -0.95, 1.5,0.0, 1.0, 0.1, -0.9,0.0, 0.0, 0.1, 0.98;EXPECT_NEAR(result, mr::DistanceToSE3(input), 3); // 断言计算结果与预期结果近似相等
}/* 测试函数：TestIfSO3Test */
TEST(MRTest, TestIfSO3Test) {Eigen::Matrix3d input; // 定义3x3矩阵，用于存储输入数据bool result = false; // 定义预期结果input << 1.0, 0.0, 0.0, // 填充矩阵元素0.0, 0.1, -0.95,0.0, 1.0, 0.1;ASSERT_EQ(result, mr::TestIfSO3(input)); // 断言计算结果与预期结果相等
}/* 测试函数：TestIfSE3Test */
TEST(MRTest, TestIfSE3Test) {Eigen::Matrix4d input; // 定义4x4矩阵，用于存储输入数据bool result = false; // 定义预期结果input << 1.0, 0.0, 0.0, 1.2, // 填充矩阵元素0.0, 0.1, -0.95, 1.5,0.0, 1.0, 0.1, -0.9,0.0, 0.0, 0.1, 0.98;ASSERT_EQ(result, mr::TestIfSE3(input)); // 断言计算结果与预期结果相等
}
/* 测试函数：IKinBodyTest */
TEST(MRTest, IKinBodyTest) {Eigen::MatrixXd BlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表BlistT << 0, 0, -1, 2, 0, 0, // 填充矩阵元素0, 0, 0, 0, 1, 0,0, 0, 1, 0, 0, 0.1;Eigen::MatrixXd Blist = BlistT.transpose(); // 转置矩阵Eigen::Matrix4d M; // 定义4x4矩阵，用于存储初始变换矩阵M << -1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 6,0, 0, -1, 2,0, 0, 0, 1;Eigen::Matrix4d T; // 定义4x4矩阵，用于存储目标变换矩阵T << 0, 1, 0, -5, // 填充矩阵元素1, 0, 0, 4,0, 0, -1, 1.6858,0, 0, 0, 1;Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 1.5, 2.5, 3; // 填充向量元素double eomg = 0.01; // 定义角速度误差阈值double ev = 0.001; // 定义线速度误差阈值bool b_result = true; // 定义预期结果Eigen::VectorXd theta_result(3); // 定义3维向量，用于存储预期关节角度theta_result << 1.57073819, 2.999667, 3.14153913; // 填充向量元素bool iRet = mr::IKinBody(Blist, M, T, thetalist, eomg, ev); // 调用IKinBody函数ASSERT_EQ(b_result, iRet); // 断言计算结果与预期结果相等ASSERT_TRUE(thetalist.isApprox(theta_result, 4)); // 断言计算关节角度与预期结果近似相等
}/* 测试函数：IKinSpaceTest */
TEST(MRTest, IKinSpaceTest) {Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 0, 0, 1, 4, 0, 0, // 填充矩阵元素0, 0, 0, 0, 1, 0,0, 0, -1, -6, 0, -0.1;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::Matrix4d M; // 定义4x4矩阵，用于存储初始变换矩阵M << -1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 6,0, 0, -1, 2,0, 0, 0, 1;Eigen::Matrix4d T; // 定义4x4矩阵，用于存储目标变换矩阵T << 0, 1, 0, -5, // 填充矩阵元素1, 0, 0, 4,0, 0, -1, 1.6858,0, 0, 0, 1;Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 1.5, 2.5, 3; // 填充向量元素double eomg = 0.01; // 定义角速度误差阈值double ev = 0.001; // 定义线速度误差阈值bool b_result = true; // 定义预期结果Eigen::VectorXd theta_result(3); // 定义3维向量，用于存储预期关节角度theta_result << 1.57073783, 2.99966384, 3.1415342; // 填充向量元素bool iRet = mr::IKinSpace(Slist, M, T, thetalist, eomg, ev); // 调用IKinSpace函数ASSERT_EQ(b_result, iRet); // 断言计算结果与预期结果相等ASSERT_TRUE(thetalist.isApprox(theta_result, 4)); // 断言计算关节角度与预期结果近似相等
}/* 测试函数：AdjointTest */
TEST(MRTest, AdjointTest) {Eigen::Matrix4d T; // 定义4x4矩阵，用于存储变换矩阵T << 1, 0, 0, 0, // 填充矩阵元素0, 0, -1, 0,0, 1, 0, 3,0, 0, 0, 1;Eigen::MatrixXd result(6, 6); // 定义6x6矩阵，用于存储预期结果result <<1, 0, 0, 0, 0, 0, // 填充矩阵元素0, 0, -1, 0, 0, 0,0, 1, 0, 0, 0, 0,0, 0, 3, 1, 0, 0,3, 0, 0, 0, 0, -1,0, 0, 0, 0, 1, 0;ASSERT_TRUE(mr::Adjoint(T).isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：InverseDynamicsTest */
TEST(MRTest, InverseDynamicsTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素Eigen::VectorXd ddthetalist(3); // 定义3维向量，用于存储关节加速度ddthetalist << 2, 1.5, 1; // 填充向量元素Eigen::VectorXd g(3); // 定义3维向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素Eigen::VectorXd Ftip(6); // 定义6维向量，用于存储末端执行器力Ftip << 1, 1, 1, 1, 1, 1; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395, // 填充矩阵元素0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::VectorXd taulist = mr::InverseDynamics(thetalist, dthetalist, ddthetalist, g,Ftip, Mlist, Glist, Slist); // 调用InverseDynamics函数计算关节力矩Eigen::VectorXd result(3); // 定义3维向量，用于存储预期结果result << 74.6962, -33.0677, -3.23057; // 填充向量元素ASSERT_TRUE(taulist.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：GravityForcesTest */
TEST(MRTest, GravityForcesTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd g(3); // 定义3维向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::VectorXd grav = mr::GravityForces(thetalist, g, Mlist, Glist, Slist); // 调用GravityForces函数计算重力影响Eigen::VectorXd result(3); // 定义3维向量，用于存储预期结果result << 28.4033, -37.6409, -5.4416; // 填充向量元素ASSERT_TRUE(grav.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：MassMatrixTest */
TEST(MRTest, MassMatrixTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::MatrixXd M = mr::MassMatrix(thetalist, Mlist, Glist, Slist); // 调用MassMatrix函数计算质量矩阵Eigen::MatrixXd result(3, 3); // 定义3x3矩阵，用于存储预期结果result << 22.5433, -0.3071, -0.0072, // 填充矩阵元素-0.3071, 1.9685, 0.4322,-0.0072, 0.4322, 0.1916;ASSERT_TRUE(M.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}
/* 测试函数：VelQuadraticForcesTest */
TEST(MRTest, VelQuadraticForcesTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::VectorXd c = mr::VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist); // 调用VelQuadraticForces函数计算速度二次项力Eigen::VectorXd result(3); // 定义3维向量，用于存储预期结果result << 0.2645, -0.0551, -0.0069; // 填充向量元素ASSERT_TRUE(c.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}
/* 测试函数：EndEffectorForcesTest */
TEST(MRTest, EndEffectorForcesTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd Ftip(6); // 定义6维向量，用于存储末端执行器力Ftip << 1, 1, 1, 1, 1, 1; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::VectorXd JTFtip = mr::EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist); // 调用EndEffectorForces函数计算末端执行器力Eigen::VectorXd result(3); // 定义3维向量，用于存储预期结果result << 1.4095, 1.8577, 1.3924; // 填充向量元素ASSERT_TRUE(JTFtip.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：ForwardDynamicsTest */
TEST(MRTest, ForwardDynamicsTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素Eigen::VectorXd taulist(3); // 定义3维向量，用于存储关节力矩taulist << 0.5, 0.6, 0.7; // 填充向量元素Eigen::VectorXd g(3); // 定义3维向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素Eigen::VectorXd Ftip(6); // 定义6维向量，用于存储末端执行器力Ftip << 1, 1, 1, 1, 1, 1; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::VectorXd ddthetalist = mr::ForwardDynamics(thetalist, dthetalist, taulist, g,Ftip, Mlist, Glist, Slist); // 调用ForwardDynamics函数计算关节加速度Eigen::VectorXd result(3); // 定义3维向量，用于存储预期结果result << -0.9739, 25.5847, -32.9150; // 填充向量元素ASSERT_TRUE(ddthetalist.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}
/* 测试函数：EulerStepTest */
TEST(MRTest, EulerStepTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素Eigen::VectorXd ddthetalist(3); // 定义3维向量，用于存储关节加速度ddthetalist << 2, 1.5, 1; // 填充向量元素double dt = 0.1; // 定义时间步长mr::EulerStep(thetalist, dthetalist, ddthetalist, dt); // 调用EulerStep函数更新关节角度和速度Eigen::VectorXd result_thetalistNext(3); // 定义3维向量，用于存储预期的下一步关节角度result_thetalistNext << 0.11, 0.12, 0.13; // 填充向量元素Eigen::VectorXd result_dthetalistNext(3); // 定义3维向量，用于存储预期的下一步关节速度result_dthetalistNext << 0.3, 0.35, 0.4; // 填充向量元素ASSERT_TRUE(thetalist.isApprox(result_thetalistNext, 4)); // 断言计算结果与预期结果近似相等ASSERT_TRUE(dthetalist.isApprox(result_dthetalistNext, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：ComputedTorqueTest */
TEST(MRTest, ComputedTorqueTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素Eigen::VectorXd eint(3); // 定义3维向量，用于存储积分误差eint << 0.2, 0.2, 0.2; // 填充向量元素Eigen::VectorXd g(3); // 定义3维向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵Eigen::VectorXd thetalistd(3); // 定义3维向量，用于存储期望关节角度thetalistd << 1.0, 1.0, 1.0; // 填充向量元素Eigen::VectorXd dthetalistd(3); // 定义3维向量，用于存储期望关节速度dthetalistd << 2, 1.2, 2; // 填充向量元素Eigen::VectorXd ddthetalistd(3); // 定义3维向量，用于存储期望关节加速度ddthetalistd << 0.1, 0.1, 0.1; // 填充向量元素double Kp = 1.3; // 定义比例增益double Ki = 1.2; // 定义积分增益double Kd = 1.1; // 定义微分增益Eigen::VectorXd taulist = mr::ComputedTorque(thetalist, dthetalist, eint, g,Mlist, Glist, Slist, thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd); // 调用ComputedTorque函数计算关节力矩Eigen::VectorXd result(3); // 定义3维向量，用于存储预期结果result << 133.00525246, -29.94223324, -3.03276856; // 填充向量元素ASSERT_TRUE(taulist.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：CubicTimeScalingTest */
TEST(MRTest, CubicTimeScalingTest) {double Tf = 2.0; // 定义总时间double t = 0.6; // 定义当前时间double result = 0.216; // 定义预期结果EXPECT_NEAR(result, mr::CubicTimeScaling(Tf, t), 3); // 断言计算结果与预期结果近似相等
}/* 测试函数：QuinticTimeScalingTest */
TEST(MRTest, QuinticTimeScalingTest) {double Tf = 2.0; // 定义总时间double t = 0.6; // 定义当前时间double result = 0.16308; // 定义预期结果EXPECT_NEAR(result, mr::QuinticTimeScaling(Tf, t), 3); // 断言计算结果与预期结果近似相等
}/* 测试函数：JointTrajectoryTest */
TEST(MRTest, JointTrajectoryTest) {int dof = 8; // 定义自由度数量Eigen::VectorXd thetastart(dof); // 定义向量，用于存储初始关节角度thetastart << 1, 0, 0, 1, 1, 0.2, 0, 1; // 填充向量元素Eigen::VectorXd thetaend(dof); // 定义向量，用于存储结束关节角度thetaend << 1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1; // 填充向量元素double Tf = 4.0; // 定义总时间int N = 6; // 定义轨迹点数量int method = 3; // 定义时间缩放方法Eigen::MatrixXd result(N, dof); // 定义矩阵，用于存储预期结果result << 1, 0, 0, 1, 1, 0.2, 0, 1, // 填充矩阵元素1.0208, 0.052, 0.0624, 1.0104, 1.104, 0.3872, 0.0936, 1,1.0704, 0.176, 0.2112, 1.0352, 1.352, 0.8336, 0.3168, 1,1.1296, 0.324, 0.3888, 1.0648, 1.648, 1.3664, 0.5832, 1,1.1792, 0.448, 0.5376, 1.0896, 1.896, 1.8128, 0.8064, 1,1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1;Eigen::MatrixXd traj = mr::JointTrajectory(thetastart, thetaend, Tf, N, method); // 调用JointTrajectory函数计算关节轨迹ASSERT_TRUE(traj.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：ScrewTrajectoryTest */
TEST(MRTest, ScrewTrajectoryTest) {Eigen::MatrixXd Xstart(4, 4); // 定义4x4矩阵，用于存储初始变换矩阵Xstart << 1, 0, 0, 1, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 1,0, 0, 0, 1;Eigen::MatrixXd Xend(4, 4); // 定义4x4矩阵，用于存储结束变换矩阵Xend << 0, 0, 1, 0.1, // 填充矩阵元素1, 0, 0, 0,0, 1, 0, 4.1,0, 0, 0, 1;double Tf = 5.0; // 定义总时间int N = 4; // 定义轨迹点数量int method = 3; // 定义时间缩放方法std::vector<Eigen::MatrixXd> result(N); // 定义向量，用于存储预期结果result[0] = Xstart; // 设置初始变换矩阵Eigen::Matrix4d X12; // 定义4x4矩阵，用于存储中间变换矩阵X12 << 0.904, -0.25, 0.346, 0.441, // 填充矩阵元素0.346, 0.904, -0.25, 0.529,-0.25, 0.346, 0.904, 1.601,0, 0, 0, 1;Eigen::Matrix4d X23; // 定义4x4矩阵，用于存储中间变换矩阵X23 << 0.346, -0.25, 0.904, -0.117, // 填充矩阵元素0.904, 0.346, -0.25, 0.473,-0.25, 0.904, 0.346, 3.274,0, 0, 0, 1;result[1] = X12; // 设置中间变换矩阵result[2] = X23; // 设置中间变换矩阵result[3] = Xend; // 设置结束变换矩阵std::vector<Eigen::MatrixXd> traj = mr::ScrewTrajectory(Xstart, Xend, Tf, N, method); // 调用ScrewTrajectory函数计算螺旋轨迹for (int i = 0; i < N; ++i) { // 遍历每个轨迹点ASSERT_TRUE(traj[i].isApprox(result[i], 4)); // 断言计算结果与预期结果近似相等}
}/* 测试函数：CartesianTrajectoryTest */
TEST(MRTest, CartesianTrajectoryTest) {Eigen::MatrixXd Xstart(4, 4); // 定义4x4矩阵，用于存储初始变换矩阵Xstart << 1, 0, 0, 1, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 1,0, 0, 0, 1;Eigen::MatrixXd Xend(4, 4); // 定义4x4矩阵，用于存储结束变换矩阵Xend << 0, 0, 1, 0.1, // 填充矩阵元素1, 0, 0, 0,0, 1, 0, 4.1,0, 0, 0, 1;double Tf = 5.0; // 定义总时间int N = 4; // 定义轨迹点数量int method = 5; // 定义时间缩放方法std::vector<Eigen::MatrixXd> result(N); // 定义向量，用于存储预期结果result[0] = Xstart; // 设置初始变换矩阵Eigen::Matrix4d X12; // 定义4x4矩阵，用于存储中间变换矩阵X12 << 0.937, -0.214, 0.277, 0.811, // 填充矩阵元素0.277, 0.937, -0.214, 0,-0.214, 0.277, 0.937, 1.651,0, 0, 0, 1;Eigen::Matrix4d X23; // 定义4x4矩阵，用于存储中间变换矩阵X23 << 0.277, -0.214, 0.937, 0.289, // 填充矩阵元素0.937, 0.277, -0.214, 0,-0.214, 0.937, 0.277, 3.449,0, 0, 0, 1;result[1] = X12; // 设置中间变换矩阵result[2] = X23; // 设置中间变换矩阵result[3] = Xend; // 设置结束变换矩阵std::vector<Eigen::MatrixXd> traj = mr::CartesianTrajectory(Xstart, Xend, Tf, N, method); // 调用CartesianTrajectory函数计算笛卡尔轨迹for (int i = 0; i < N; ++i) { // 遍历每个轨迹点ASSERT_TRUE(traj[i].isApprox(result[i], 4)); // 断言计算结果与预期结果近似相等}
}/* 测试函数：InverseDynamicsTrajectoryTest */
TEST(MRTest, InverseDynamicsTrajectoryTest) {int dof = 3; // 定义自由度数量Eigen::VectorXd thetastart(dof); // 定义向量，用于存储初始关节角度thetastart << 0, 0, 0; // 填充向量元素Eigen::VectorXd thetaend(dof); // 定义向量，用于存储结束关节角度thetaend << M_PI / 2, M_PI / 2, M_PI / 2; // 填充向量元素double Tf = 3.0; // 定义总时间int N = 1000; // 定义轨迹点数量int method = 5; // 定义时间缩放方法Eigen::MatrixXd traj = mr::JointTrajectory(thetastart, thetaend, Tf, N, method); // 计算关节轨迹Eigen::MatrixXd thetamat = traj; // 关节角度矩阵Eigen::MatrixXd dthetamat = Eigen::MatrixXd::Zero(N, dof); // 初始化关节速度矩阵Eigen::MatrixXd ddthetamat = Eigen::MatrixXd::Zero(N, dof); // 初始化关节加速度矩阵double dt = Tf / (N - 1.0); // 计算时间步长for (int i = 0; i < N - 1; ++i) { // 遍历每个时间步dthetamat.row(i + 1) = (thetamat.row(i + 1) - thetamat.row(i)) / dt; // 计算关节速度ddthetamat.row(i + 1) = (dthetamat.row(i + 1) - dthetamat.row(i)) / dt; // 计算关节加速度}Eigen::VectorXd g(3); // 定义向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素Eigen::MatrixXd Ftipmat = Eigen::MatrixXd::Zero(N, 6); // 初始化末端执行器力矩阵std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵int numTest = 3; // 定义测试点数量Eigen::MatrixXd result(numTest, dof); // 定义矩阵，用于存储预期结果Eigen::VectorXd tau_timestep_beg(3); // 定义向量，用于存储起始时间步的关节力矩tau_timestep_beg << 13.22970794, -36.262108, -4.181341; // 填充向量元素Eigen::VectorXd tau_timestep_mid(3); // 定义向量，用于存储中间时间步的关节力矩tau_timestep_mid << 115.55863434, -22.05129215, 1.00916115; // 填充向量元素Eigen::VectorXd tau_timestep_end(3); // 定义向量，用于存储结束时间步的关节力矩tau_timestep_end << 81.12700926, -23.20753925, 2.48432708; // 填充向量元素result << tau_timestep_beg.transpose(), // 填充矩阵元素tau_timestep_mid.transpose(),tau_timestep_end.transpose();Eigen::MatrixXd taumat = mr::InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, Ftipmat, Mlist, Glist, Slist); // 调用InverseDynamicsTrajectory函数计算关节力矩轨迹Eigen::MatrixXd taumat_timestep(numTest, dof); // 定义矩阵，用于存储测试点的关节力矩taumat_timestep << taumat.row(0), // 填充矩阵元素taumat.row(int(N / 2) - 1),taumat.row(N - 1);ASSERT_TRUE(taumat_timestep.isApprox(result, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：ForwardDynamicsTrajectoryTest */
TEST(MRTest, ForwardDynamicsTrajectoryTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素int N = 10, dof = 3; // 定义轨迹点数量和自由度数量Eigen::MatrixXd taumat(N, 3); // 定义矩阵，用于存储关节力矩taumat << 3.63, -6.58, -5.57, // 填充矩阵元素3.74, -5.55, -5.5,4.31, -0.68, -5.19, 5.18, 5.63, -4.31,5.85, 8.17, -2.59,5.78, 2.79, -1.7,4.99, -5.3, -1.19,4.08, -9.41, 0.07,3.56, -10.1, 0.97,3.49, -9.41, 1.23;Eigen::VectorXd g(3); // 定义向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素Eigen::MatrixXd Ftipmat = Eigen::MatrixXd::Zero(N, 6); // 初始化末端执行器力矩阵std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵double dt = 0.1; // 定义时间步长int intRes = 8; // 定义积分分辨率Eigen::MatrixXd result_thetamat(N, dof); // 定义矩阵，用于存储预期的关节角度轨迹Eigen::MatrixXd result_dthetamat(N, dof); // 定义矩阵，用于存储预期的关节速度轨迹result_thetamat << 0.1, 0.1, 0.1, // 填充矩阵元素0.10643138, 0.2625997, -0.22664947,0.10197954, 0.71581297, -1.22521632,0.0801044, 1.33930884, -2.28074132,0.0282165, 2.11957376, -3.07544297,-0.07123855, 2.87726666, -3.83289684,-0.20136466, 3.397858, -4.83821609,-0.32380092, 3.73338535, -5.98695747,-0.41523262, 3.85883317, -7.01130559,-0.4638099, 3.63178793, -7.63190052;result_dthetamat << 0.1, 0.2, 0.3, // 填充矩阵元素0.01212502, 3.42975773, -7.74792602,-0.13052771, 5.55997471, -11.22722784,-0.35521041, 7.11775879, -9.18173035,-0.77358795, 8.17307573, -7.05744594,-1.2350231, 6.35907497, -8.99784746,-1.31426299, 4.07685875, -11.18480509,-1.06794821, 2.49227786, -11.69748583,-0.70264871, -0.55925705, -8.16067131,-0.1455669, -4.57149985, -3.43135114;std::vector<Eigen::MatrixXd> traj = mr::ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, Mlist, Glist, Slist, dt, intRes); // 调用ForwardDynamicsTrajectory函数计算正向动力学轨迹Eigen::MatrixXd traj_theta = traj.at(0); // 获取关节角度轨迹Eigen::MatrixXd traj_dtheta = traj.at(1); // 获取关节速度轨迹ASSERT_TRUE(traj_theta.isApprox(result_thetamat, 4)); // 断言计算结果与预期结果近似相等ASSERT_TRUE(traj_dtheta.isApprox(result_dthetamat, 4)); // 断言计算结果与预期结果近似相等
}/* 测试函数：SimulateControlTest */
TEST(MRTest, SimulateControlTest) {Eigen::VectorXd thetalist(3); // 定义3维向量，用于存储关节角度thetalist << 0.1, 0.1, 0.1; // 填充向量元素Eigen::VectorXd dthetalist(3); // 定义3维向量，用于存储关节速度dthetalist << 0.1, 0.2, 0.3; // 填充向量元素Eigen::VectorXd g(3); // 定义3维向量，用于存储重力向量g << 0, 0, -9.8; // 填充向量元素std::vector<Eigen::MatrixXd> Mlist; // 定义矩阵列表，用于存储变换矩阵std::vector<Eigen::MatrixXd> Glist; // 定义矩阵列表，用于存储惯性矩阵Eigen::Matrix4d M01; // 定义4x4矩阵，用于存储变换矩阵M01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.089159,0, 0, 0, 1;Eigen::Matrix4d M12; // 定义4x4矩阵，用于存储变换矩阵M12 << 0, 0, 1, 0.28, // 填充矩阵元素0, 1, 0, 0.13585,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d M23; // 定义4x4矩阵，用于存储变换矩阵M23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.1197,0, 0, 1, 0.395,0, 0, 0, 1;Eigen::Matrix4d M34; // 定义4x4矩阵，用于存储变换矩阵M34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.14225,0, 0, 0, 1;Mlist.push_back(M01); // 将变换矩阵添加到列表中Mlist.push_back(M12); // 将变换矩阵添加到列表中Mlist.push_back(M23); // 将变换矩阵添加到列表中Mlist.push_back(M34); // 将变换矩阵添加到列表中Eigen::VectorXd G1(6); // 定义6维向量，用于存储惯性矩阵元素G1 << 0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7; // 填充向量元素Eigen::VectorXd G2(6); // 定义6维向量，用于存储惯性矩阵元素G2 << 0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393; // 填充向量元素Eigen::VectorXd G3(6); // 定义6维向量，用于存储惯性矩阵元素G3 << 0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275; // 填充向量元素Glist.push_back(G1.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G2.asDiagonal()); // 将惯性矩阵添加到列表中Glist.push_back(G3.asDiagonal()); // 将惯性矩阵添加到列表中Eigen::MatrixXd SlistT(3, 6); // 定义3x6矩阵，用于存储螺旋轴列表SlistT << 1, 0, 1, 0, 1, 0, // 填充矩阵元素0, 1, 0, -0.089, 0, 0,0, 1, 0, -0.089, 0, 0.425;Eigen::MatrixXd Slist = SlistT.transpose(); // 转置矩阵double dt = 0.01; // 定义时间步长Eigen::VectorXd thetaend(3); // 定义向量，用于存储结束关节角度thetaend << M_PI / 2, M_PI / 2, M_PI / 2; // 填充向量元素double Tf = 1.0; // 定义总时间int N = int(1.0 * Tf / dt); // 计算轨迹点数量int method = 5; // 定义时间缩放方法Eigen::MatrixXd traj = mr::JointTrajectory(thetalist, thetaend, Tf, N, method); // 计算关节轨迹Eigen::MatrixXd thetamatd = traj; // 期望关节角度矩阵Eigen::MatrixXd dthetamatd = Eigen::MatrixXd::Zero(N, 3); // 初始化期望关节速度矩阵Eigen::MatrixXd ddthetamatd = Eigen::MatrixXd::Zero(N, 3); // 初始化期望关节加速度矩阵dt = Tf / (N - 1.0); // 计算时间步长for (int i = 0; i < N - 1; ++i) { // 遍历每个时间步dthetamatd.row(i + 1) = (thetamatd.row(i + 1) - thetamatd.row(i)) / dt; // 计算期望关节速度ddthetamatd.row(i + 1) = (dthetamatd.row(i + 1) - dthetamatd.row(i)) / dt; // 计算期望关节加速度}Eigen::VectorXd gtilde(3); // 定义向量，用于存储估计的重力向量gtilde << 0.8, 0.2, -8.8; // 填充向量元素std::vector<Eigen::MatrixXd> Mtildelist; // 定义矩阵列表，用于存储估计的变换矩阵std::vector<Eigen::MatrixXd> Gtildelist; // 定义矩阵列表，用于存储估计的惯性矩阵Eigen::Matrix4d Mhat01; // 定义4x4矩阵，用于存储估计的变换矩阵Mhat01 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.1,0, 0, 0, 1;Eigen::Matrix4d Mhat12; // 定义4x4矩阵，用于存储估计的变换矩阵Mhat12 << 0, 0, 1, 0.3, // 填充矩阵元素0, 1, 0, 0.2,-1, 0, 0, 0,0, 0, 0, 1;Eigen::Matrix4d Mhat23; // 定义4x4矩阵，用于存储估计的变换矩阵Mhat23 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, -0.2,0, 0, 1, 0.4,0, 0, 0, 1;Eigen::Matrix4d Mhat34; // 定义4x4矩阵，用于存储估计的变换矩阵Mhat34 << 1, 0, 0, 0, // 填充矩阵元素0, 1, 0, 0,0, 0, 1, 0.2,0, 0, 0, 1;Mtildelist.push_back(Mhat01); // 将估计的变换矩阵添加到列表中Mtildelist.push_back(Mhat12); // 将估计的变换矩阵添加到列表中Mtildelist.push_back(Mhat23); // 将估计的变换矩阵添加到列表中Mtildelist.push_back(Mhat34); // 将估计的变换矩阵添加到列表中Eigen::VectorXd Ghat1(6); // 定义6维向量，用于存储估计的惯性矩阵元素Ghat1 << 0.1, 0.1, 0.1, 4, 4, 4; // 填充向量元素Eigen::VectorXd Ghat2(6); // 定义6维向量，用于存储估计的惯性矩阵元素Ghat2 << 0.3, 0.3, 0.1, 9, 9, 9; // 填充向量元素Eigen::VectorXd Ghat3(6); // 定义6维向量，用于存储估计的惯性矩阵元素Ghat3 << 0.1, 0.1, 0.1, 3, 3, 3; // 填充向量元素Gtildelist.push_back(Ghat1.asDiagonal()); // 将估计的惯性矩阵添加到列表中Gtildelist.push_back(Ghat2.asDiagonal()); // 将估计的惯性矩阵添加到列表中Gtildelist.push_back(Ghat3.asDiagonal()); // 将估计的惯性矩阵添加到列表中Eigen::MatrixXd Ftipmat = Eigen::MatrixXd::Ones(N, 6); // 初始化末端执行器力矩阵double Kp = 20.0; // 定义比例增益double Ki = 10.0; // 定义积分增益double Kd = 18.0; // 定义微分增益int intRes = 8; // 定义积分分辨率int numTest = 3;  // 定义测试点数量Eigen::MatrixXd result_taumat(numTest, 3); // 定义矩阵，用于存储预期的关节力矩Eigen::MatrixXd result_thetamat(numTest, 3); // 定义矩阵，用于存储预期的关节角度Eigen::VectorXd tau_timestep_beg(3); // 定义向量，用于存储起始时间步的关节力矩tau_timestep_beg << -14.2640765, -54.06797429, -11.265448; // 填充向量元素Eigen::VectorXd tau_timestep_mid(3); // 定义向量，用于存储中间时间步的关节力矩tau_timestep_mid << 31.98269367, 9.89625811, 1.47810165; // 填充向量元素Eigen::VectorXd tau_timestep_end(3); // 定义向量，用于存储结束时间步的关节力矩tau_timestep_end << 57.04391384, 4.75360586, -1.66561523; // 填充向量元素result_taumat << tau_timestep_beg.transpose(), // 填充矩阵元素tau_timestep_mid.transpose(),tau_timestep_end.transpose();Eigen::VectorXd theta_timestep_beg(3); // 定义向量，用于存储起始时间步的关节角度theta_timestep_beg << 0.10092029, 0.10190511, 0.10160667; // 填充向量元素Eigen::VectorXd theta_timestep_mid(3); // 定义向量，用于存储中间时间步的关节角度theta_timestep_mid << 0.85794085, 1.55124503, 2.80130978; // 填充向量元素Eigen::VectorXd theta_timestep_end(3); // 定义向量，用于存储结束时间步的关节角度theta_timestep_end << 1.56344023, 3.07994906, 4.52269971; // 填充向量元素result_thetamat << theta_timestep_beg.transpose(), // 填充矩阵元素theta_timestep_mid.transpose(),theta_timestep_end.transpose();std::vector<Eigen::MatrixXd> controlTraj = mr::SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, Slist, thetamatd, dthetamatd,ddthetamatd, gtilde, Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes); // 调用SimulateControl函数模拟控制Eigen::MatrixXd traj_tau = controlTraj.at(0); // 获取关节力矩轨迹Eigen::MatrixXd traj_theta = controlTraj.at(1); // 获取关节角度轨迹Eigen::MatrixXd traj_tau_timestep(numTest, 3); // 定义矩阵，用于存储测试点的关节力矩traj_tau_timestep << traj_tau.row(0), // 填充矩阵元素traj_tau.row(int(N / 2) - 1),traj_tau.row(N - 1);Eigen::MatrixXd traj_theta_timestep(numTest, 3); // 定义矩阵，用于存储测试点的关节角度traj_theta_timestep << traj_theta.row(0), // 填充矩阵元素traj_theta.row(int(N / 2) - 1),traj_theta.row(N - 1);ASSERT_TRUE(traj_tau_timestep.isApprox(result_taumat, 4)); // 断言计算结果与预期结果近似相等ASSERT_TRUE(traj_theta_timestep.isApprox(result_thetamat, 4)); // 断言计算结果与预期结果近似相等
}

头文件 modern_robotics.h 

#pragma once // 防止头文件被重复包含#include <Eigen/Dense> // 包含Eigen库，用于矩阵和向量运算
#include <vector> // 包含标准库vector，用于存储动态数组namespace mr { // 定义命名空间mr/** 函数：判断一个值是否可以忽略为0* 输入：需要检查的double类型值* 返回：布尔值，true表示可以忽略，false表示不能忽略*/
bool NearZero(const double); // 声明NearZero函数/** 函数：计算给定6维向量的6x6矩阵[adV]* 输入：Eigen::VectorXd (6x1) 6维向量* 输出：Eigen::MatrixXd (6x6) 6x6矩阵* 备注：可用于计算李括号[V1, V2] = [adV1]V2*/
Eigen::MatrixXd ad(Eigen::VectorXd); // 声明ad函数/** 函数：返回输入向量的归一化版本* 输入：Eigen::MatrixXd 矩阵* 输出：Eigen::MatrixXd 矩阵* 备注：MatrixXd用于行向量的情况，需要复制，因MatrixXd的类型转换而有用*/
Eigen::MatrixXd Normalize(Eigen::MatrixXd); // 声明Normalize函数/** 函数：返回角速度向量的反对称矩阵表示* 输入：Eigen::Vector3d 3x1角速度向量* 返回：Eigen::MatrixXd 3x3反对称矩阵*/
Eigen::Matrix3d VecToso3(const Eigen::Vector3d&); // 声明VecToso3函数/** 函数：返回由反对称矩阵表示的角速度向量* 输入：Eigen::MatrixXd 3x3反对称矩阵* 返回：Eigen::Vector3d 3x1角速度向量*/
Eigen::Vector3d so3ToVec(const Eigen::MatrixXd&); // 声明so3ToVec函数/** 函数：将指数旋转转换为其单独的分量* 输入：指数旋转（旋转矩阵，包含旋转轴和旋转角度）* 返回：旋转轴和旋转角度[x, y, z, theta]*/
Eigen::Vector4d AxisAng3(const Eigen::Vector3d&); // 声明AxisAng3函数/** 函数：将指数旋转转换为旋转矩阵* 输入：指数旋转表示的旋转* 返回：旋转矩阵*/
Eigen::Matrix3d MatrixExp3(const Eigen::Matrix3d&); // 声明MatrixExp3函数/** 函数：计算旋转矩阵的矩阵对数* 输入：旋转矩阵* 返回：旋转的矩阵对数*/
Eigen::Matrix3d MatrixLog3(const Eigen::Matrix3d&); // 声明MatrixLog3函数/** 函数：将旋转矩阵和位置向量组合成一个特殊欧几里得群(SE3)齐次变换矩阵* 输入：旋转矩阵(R)，位置向量(p)* 返回：矩阵T = [ [R, p],*                [0, 1] ]*/
Eigen::MatrixXd RpToTrans(const Eigen::Matrix3d&, const Eigen::Vector3d&); // 声明RpToTrans函数/** 函数：从变换矩阵表示中分离出旋转矩阵和位置向量* 输入：齐次变换矩阵* 返回：包含旋转矩阵和位置向量的std::vector*/
std::vector<Eigen::MatrixXd> TransToRp(const Eigen::MatrixXd&); // 声明TransToRp函数/** 函数：将空间速度向量转换为变换矩阵* 输入：空间速度向量[角速度，线速度]* 返回：变换矩阵*/
Eigen::MatrixXd VecTose3(const Eigen::VectorXd&); // 声明VecTose3函数/** 函数：将变换矩阵转换为空间速度向量* 输入：变换矩阵* 返回：空间速度向量[角速度，线速度]*/
Eigen::VectorXd se3ToVec(const Eigen::MatrixXd&); // 声明se3ToVec函数/** 函数：提供变换矩阵的伴随表示*       用于改变空间速度向量的参考系* 输入：4x4变换矩阵SE(3)* 返回：6x6矩阵的伴随表示*/
Eigen::MatrixXd Adjoint(const Eigen::MatrixXd&);/** 函数：螺旋轴的旋转扩展* 输入：指数坐标的se3矩阵表示（变换矩阵）* 返回：表示旋转的6x6矩阵*/
Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&);/** 函数：计算齐次变换矩阵的矩阵对数* 输入：R：SE3中的变换矩阵* 返回：R的矩阵对数*/
Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&);/** 函数：计算末端执行器框架（用于当前空间位置计算）* 输入：末端执行器的初始配置（位置和方向）*       当机械臂处于初始位置时的关节螺旋轴*       关节坐标列表* 返回：表示关节在指定坐标时末端执行器框架的变换矩阵* 备注：FK表示正向运动学*/
Eigen::MatrixXd FKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&);/** 函数：计算末端执行器框架（用于当前身体位置计算）* 输入：末端执行器的初始配置（位置和方向）*       当机械臂处于初始位置时的关节螺旋轴*       关节坐标列表* 返回：表示关节在指定坐标时末端执行器框架的变换矩阵* 备注：FK表示正向运动学*/
Eigen::MatrixXd FKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&);/** 函数：给出空间雅可比矩阵* 输入：初始位置的螺旋轴，关节配置* 返回：6xn空间雅可比矩阵*/
Eigen::MatrixXd JacobianSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&);/** 函数：给出身体雅可比矩阵* 输入：身体位置的螺旋轴，关节配置* 返回：6xn身体雅可比矩阵*/
Eigen::MatrixXd JacobianBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&);/** 反转齐次变换矩阵* 输入：齐次变换矩阵T* 返回：T的逆矩阵*/
Eigen::MatrixXd TransInv(const Eigen::MatrixXd&);/** 反转旋转矩阵* 输入：旋转矩阵R* 返回：R的逆矩阵*/
Eigen::MatrixXd RotInv(const Eigen::MatrixXd&);/** 将螺旋轴的参数描述转换为归一化螺旋轴* 输入：* q：螺旋轴上的一点* s：螺旋轴方向的单位向量* h：螺旋轴的螺距* 返回：由输入描述的归一化螺旋轴*/
Eigen::VectorXd ScrewToAxis(Eigen::Vector3d q, Eigen::Vector3d s, double h);/** 函数：将6维指数坐标转换为螺旋轴-角度形式* 输入：* expc6：刚体运动的6维指数坐标 S*theta* 返回：对应的归一化螺旋轴S；沿/绕S移动的距离theta，形式为[S, theta]* 备注：返回std::map<S, theta>是否更好？*/
Eigen::VectorXd AxisAng6(const Eigen::VectorXd&);/** 函数：将一个矩阵投影到SO(3)* 输入：* M: 一个接近SO(3)的矩阵，将其投影到SO(3)* 返回：SO(3)中最接近的矩阵R* 使用奇异值分解将矩阵mat投影到SO(3)中最接近的矩阵* （参见http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review）。* 此函数仅适用于接近SO(3)的矩阵。*/
Eigen::MatrixXd ProjectToSO3(const Eigen::MatrixXd&);/** 函数：将一个矩阵投影到SE(3)* 输入：* M: 一个接近SE(3)的4x4矩阵，将其投影到SE(3)* 返回：SE(3)中最接近的矩阵T* 使用奇异值分解将矩阵mat投影到SE(3)中最接近的矩阵* （参见http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review）。* 此函数仅适用于接近SE(3)的矩阵。*/
Eigen::MatrixXd ProjectToSE3(const Eigen::MatrixXd&);/** 函数：返回Frobenius范数以描述矩阵M与SO(3)流形的距离* 输入：* M: 一个3x3矩阵* 输出：* 返回矩阵mat到SO(3)流形的距离，使用以下方法：* 如果det(M) <= 0，返回一个大数。* 如果det(M) > 0，返回norm(M^T*M - I)。*/
double DistanceToSO3(const Eigen::Matrix3d&);/** 函数：返回Frobenius范数以描述矩阵T与SE(3)流形的距离* 输入：* T: 一个4x4矩阵* 输出：* 返回矩阵T到SE(3)流形的距离，使用以下方法：* 计算矩阵T的前三行前三列子矩阵matR的行列式。* 如果det(matR) <= 0，返回一个大数。* 如果det(matR) > 0，将mat的前三行前三列子矩阵替换为matR^T*matR，* 并将mat的第四列的前三个元素设为零。然后返回norm(T - I)。*/
double DistanceToSE3(const Eigen::Matrix4d&);/** 函数：如果矩阵M接近或在SO(3)流形上，返回true* 输入：* M: 一个3x3矩阵* 输出：* 如果M非常接近或在SO(3)中，返回true，否则返回false*/
bool TestIfSO3(const Eigen::Matrix3d&);/** 函数：如果矩阵T接近或在SE(3)流形上，返回true* 输入：* T: 一个4x4矩阵* 输出：* 如果T非常接近或在SE(3)中，返回true，否则返回false*/
bool TestIfSE3(const Eigen::Matrix4d&);/** 函数：计算开链机器人在身体坐标系中的逆运动学* 输入：* Blist: 末端执行器在初始位置时的关节螺旋轴，以轴为列的矩阵格式* M: 末端执行器的初始配置* T: 末端执行器的期望配置Tsd* thetalist[in][out]: 关节角的初始猜测和结果输出，接近满足Tsd* eomg: 末端执行器方向误差的小正容差。返回的关节角必须使末端执行器方向误差小于eomg* ev: 末端执行器线性位置误差的小正容差。返回的关节角必须使末端执行器位置误差小于ev* 输出：* success: 逻辑值，TRUE表示函数找到了解决方案，FALSE表示在最大迭代次数内未找到满足eomg和ev容差的解决方案* thetalist[in][out]: 在指定容差内实现T的关节角*/
bool IKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double);/** 函数：计算开链机器人在空间坐标系中的逆运动学* 输入：* Slist: 机械臂在初始位置时的关节螺旋轴，以轴为列的矩阵格式* M: 末端执行器的初始配置* T: 末端执行器的期望配置Tsd* thetalist[in][out]: 关节角的初始猜测和结果输出，接近满足Tsd* eomg: 末端执行器方向误差的小正容差。返回的关节角必须使末端执行器方向误差小于eomg* ev: 末端执行器线性位置误差的小正容差。返回的关节角必须使末端执行器位置误差小于ev* 输出：* success: 逻辑值，TRUE表示函数找到了解决方案，FALSE表示在最大迭代次数内未找到满足eomg和ev容差的解决方案* thetalist[in][out]: 在指定容差内实现T的关节角*/
bool IKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double);/* * 函数：使用前后牛顿-欧拉迭代求解方程：* taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...*           + g(thetalist) + Jtr(thetalist) * Ftip* 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度的n维向量*  ddthetalist: 关节加速度的n维向量*  g: 重力向量g*  Ftip: 由末端执行器施加的空间力，以{n+1}坐标系表示*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式* * 输出：*  taulist: 所需关节力/力矩的n维向量* */
Eigen::VectorXd InverseDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);/* * 函数：调用InverseDynamics函数，设置Ftip = 0, dthetalist = 0, 和 *   ddthetalist = 0。目的是计算动力学方程中的一个重要项       * 输入：*  thetalist: 关节变量的n维向量*  g: 重力向量g*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式* * 输出：*  grav: 显示重力对动力学影响的3维向量* */
Eigen::VectorXd GravityForces(const Eigen::VectorXd&, const Eigen::VectorXd&,const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);/* * 函数：调用InverseDynamics函数n次，每次传递一个* ddthetalist向量，其中一个元素等于1，所有其他* 输入设置为零。每次调用InverseDynamics生成一个* 列，这些列被组装成惯性矩阵。       ** 输入：*  thetalist: 关节变量的n维向量*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式* * 输出：*  M: 在给定配置thetalist下n关节串联链的数值惯性矩阵M(thetalist)*/
Eigen::MatrixXd MassMatrix(const Eigen::VectorXd&,const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);/* * 函数：调用InverseDynamics函数，设置g = 0, Ftip = 0, 和 * ddthetalist = 0。      ** 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度列表*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式* * 输出：*  c: 给定thetalist和dthetalist的科里奥利和离心项向量c(thetalist,dthetalist)*/
Eigen::VectorXd VelQuadraticForces(const Eigen::VectorXd&, const Eigen::VectorXd&,const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);/* * 函数：调用InverseDynamics函数，设置g = 0, dthetalist = 0, 和 * ddthetalist = 0。  ** 输入：*  thetalist: 关节变量的n维向量 *  Ftip: 由末端执行器施加的空间力，以{n+1}坐标系表示*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式* * 输出：*  JTFtip: 仅用于产生末端执行器力Ftip所需的关节力和力矩*/
Eigen::VectorXd EndEffectorForces(const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);/* * 函数：通过求解以下方程计算ddthetalist：* Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) *                                  - g(thetalist) - Jtr(thetalist) * Ftip* 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度的n维向量*  taulist: 关节力/力矩的n维向量*  g: 重力向量g*  Ftip: 由末端执行器施加的空间力，以{n+1}坐标系表示*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式* * 输出：*  ddthetalist: 结果关节加速度* */
Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);/** 函数：使用一阶欧拉积分计算下一个时间步的关节角度和速度* 输入：*  thetalist[in]: 关节变量的n维向量*  dthetalist[in]: 关节速度的n维向量*  ddthetalist: 关节加速度的n维向量*  dt: 时间步长delta t** 输出：*  thetalist[out]: 一阶欧拉积分后经过dt时间的关节变量向量*  dthetalist[out]: 一阶欧拉积分后经过dt时间的关节速度向量*/
void EulerStep(Eigen::VectorXd&, Eigen::VectorXd&, const Eigen::VectorXd&, double);/** 函数：使用逆动力学计算沿给定轨迹移动串联链所需的关节力/力矩* 输入：*  thetamat: 机器人关节变量的N x n矩阵（N：轨迹时间步点数；n：机器人关节数）*  dthetamat: 机器人关节速度的N x n矩阵*  ddthetamat: 机器人关节加速度的N x n矩阵*  g: 重力向量g*  Ftipmat: 由末端执行器施加的空间力的N x 6矩阵（如果没有末端力，用户应输入零矩阵）*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式** 输出：*  taumat: 指定轨迹的关节力/力矩的N x n矩阵，其中每一行是每个时间步的关节力/力矩向量*/
Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,const Eigen::MatrixXd&);/** 函数：给定开环关节力/力矩历史，计算串联链的运动* 输入：*  thetalist: 初始关节变量的n维向量*  dthetalist: 初始关节速度的n维向量*  taumat: 关节力/力矩的N x n矩阵，其中每一行是任意时间步的关节努力*  g: 重力向量g*  Ftipmat: 由末端执行器施加的空间力的N x 6矩阵（如果没有末端力，用户应输入零矩阵）*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式*  dt: 连续关节力/力矩之间的时间步长*  intRes: 积分分辨率是在每个时间步之间进行积分（欧拉）的次数。*      必须是大于或等于1的整数值** 输出：std::vector包含[thetamat, dthetamat]*  thetamat: 由指定关节力/力矩产生的关节角度的N x n矩阵*  dthetamat: 关节速度的N x n矩阵*/
std::vector<Eigen::MatrixXd> ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&,const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,const Eigen::MatrixXd&, double, int);/** 函数：计算特定时间点的关节控制力矩* 输入：*  thetalist: 关节变量的n维向量*  dthetalist: 关节速度的n维向量*  eint: 关节误差的时间积分的n维向量*  g: 重力向量g*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式*  thetalistd: 参考关节变量的n维向量*  dthetalistd: 参考关节速度的n维向量*  ddthetalistd: 参考关节加速度的n维向量*  Kp: 反馈比例增益（每个关节相同）*  Ki: 反馈积分增益（每个关节相同）*  Kd: 反馈微分增益（每个关节相同）** 输出：*  tau_computed: 由反馈线性化控制器在当前时刻计算的关节力/力矩向量*/
Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double);/** 函数：计算三次时间缩放的s(t)* 输入：*  Tf: 从静止到静止的运动总时间（秒）*  t: 当前时间t，满足0 < t < Tf** 输出：*  st: 对应于三次多项式运动的路径参数，该运动在开始和结束时速度为零*/
double CubicTimeScaling(double, double);/** 函数：计算五次时间缩放的s(t)* 输入：*  Tf: 从静止到静止的运动总时间（秒）*  t: 当前时间t，满足0 < t < Tf** 输出：*  st: 对应于五次多项式运动的路径参数，该运动在开始和结束时速度和加速度为零*/
double QuinticTimeScaling(double, double);/** 函数：计算关节空间中的直线轨迹* 输入：*  thetastart: 初始关节变量*  thetaend: 最终关节变量*  Tf: 从静止到静止的运动总时间（秒）*  N: 离散轨迹表示中的点数N > 1（开始和停止）*  method: 时间缩放方法，其中3表示三次（三级多项式）时间缩放，5表示五次（五级多项式）时间缩放** 输出：*  traj: 轨迹为N x n矩阵，其中每一行是某一时刻的n维关节变量向量。第一行是thetastart，N行是thetaend。每行之间的时间间隔为Tf / (N - 1)*/
Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, double, int, int);/** 函数：计算对应于空间螺旋轴螺旋运动的N个SE(3)矩阵的轨迹列表* 输入：*  Xstart: 初始末端执行器配置*  Xend: 最终末端执行器配置*  Tf: 从静止到静止的运动总时间（秒）*  N: 离散轨迹表示中的点数N > 1（开始和停止）*  method: 时间缩放方法，其中3表示三次（三级多项式）时间缩放，5表示五次（五级多项式）时间缩放** 输出：*  traj: 离散轨迹为N个SE(3)矩阵的列表，时间间隔为Tf/(N-1)。列表中的第一个是Xstart，N个是Xend*/
std::vector<Eigen::MatrixXd> ScrewTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);/** 函数：计算对应于末端执行器框架原点沿直线运动的N个SE(3)矩阵的轨迹列表* 输入：*  Xstart: 初始末端执行器配置*  Xend: 最终末端执行器配置*  Tf: 从静止到静止的运动总时间（秒）*  N: 离散轨迹表示中的点数N > 1（开始和停止）*  method: 时间缩放方法，其中3表示三次（三级多项式）时间缩放，5表示五次（五级多项式）时间缩放** 输出：*  traj: 离散轨迹为N个SE(3)矩阵的列表，时间间隔为Tf/(N-1)。列表中的第一个是Xstart，N个是Xend* 备注：*  此函数类似于ScrewTrajectory，不同之处在于末端执行器框架的原点沿直线运动，与旋转运动解耦。*/
std::vector<Eigen::MatrixXd> CartesianTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);/** 函数：给定开环关节力/力矩历史，计算串联链的运动* 输入：*  thetalist: 初始关节变量的n维向量*  dthetalist: 初始关节速度的n维向量*  g: 重力向量g*  Ftipmat: 由末端执行器施加的空间力的N x 6矩阵（如果没有末端力，用户应输入零矩阵）*  Mlist: 初始位置时各连杆相对于前一个连杆的坐标系列表*  Glist: 各连杆的空间惯性矩阵Gi*  Slist: 关节在空间坐标系中的螺旋轴Si，以螺旋轴为列的矩阵格式*  thetamatd: 参考轨迹中的期望关节变量的N x n矩阵*  dthetamatd: 期望关节速度的N x n矩阵*  ddthetamatd: 期望关节加速度的N x n矩阵*  gtilde: 基于实际机器人模型的重力向量（上面给出的实际值）*  Mtildelist: 基于实际机器人模型的连杆框架位置（上面给出的实际值）*  Gtildelist: 基于实际机器人模型的连杆空间惯性（上面给出的实际值）*  Kp: 反馈比例增益（每个关节相同）*  Ki: 反馈积分增益（每个关节相同）*  Kd: 反馈微分增益（每个关节相同）*  dt: 参考轨迹点之间的时间步长*  intRes: 积分分辨率是在每个时间步之间进行积分（欧拉）的次数。*      必须是大于或等于1的整数值** 输出：std::vector包含[taumat, thetamat]*  taumat: 控制器命令的关节力/力矩的N x n矩阵，其中每一行的n个力/力矩对应于单个时间点*  thetamat: 实际关节角度的N x n矩阵*/
std::vector<Eigen::MatrixXd> SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,double, double, double, double, int);}