【MR】现代机器人学-算法库函数分析（C++版停止2024.4.7） - ToB企服应用市场:ToB评测及商务社交产业平台

#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子矩阵为omgmat
result.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子矩阵为omgmat
return 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的输入值大于等于1
return m_ret; // 返回零矩阵
else if (acosinput <= -1) { // 如果acos的输入值小于等于-1
Eigen::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; // 计算GThetaV
m_ret << MatrixExp3(se3mat_cut), GThetaV, // 填充旋转矩阵和GThetaV
0, 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; // 计算logExpand1
Eigen::Matrix3d logExpand2 = (1.0 / theta - 1.0 / std::tan(theta / 2.0) / 2) * omgmat * omgmat / theta; // 计算logExpand2
Eigen::Matrix3d logExpand = logExpand1 + logExpand2; // 计算logExpand
m_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) // 如果行列式大于0
return (M.transpose() * M - Eigen::Matrix3d::Identity()).norm(); // 计算距离
else
return 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) { // 如果行列式大于0
Eigen::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(); // 返回距离
}
else
return 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; // 设置当前关节变量的加速度为1
M.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); // 三次时间缩放
else
st = 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); // 三次时间缩放
else
st = 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); // 三次时间缩放
else
st = 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; // 返回控制轨迹向量
}

复制代码

#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和s
q << 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)); // 断言计算结果与预期结果近似相等
}

复制代码

#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);
}

复制代码