最近学习了modern robotics这本书，该书使用较为前沿的旋量理论构建了机器人的动力学方法。因此尝试使用C++在旋量理论中实现一个机器人动力学解算库。最近已经实现，效果良好，演示视频请看：

【基于旋量理论的开链机器人动力学解算】 https://www.bilibili.com/video/BV1cw41197st/?share_source=copy_web&vd_source=a85d193a0f623b5126589944b51680ef

代码仓库请看：

OTT123/screw_based_robot_dynamic: 基于旋量理论的固定基座开链机械臂动力学解算 基于《现代机器人学》编写。实现了从urdf文件解析旋量理论参数的解析器以及迭代动力学算法。 (github.com)https://github.com/OTT123/screw_based_robot_dynamic

urdf中连杆坐标系（mesh）大多数情况下与质心坐标系不重合。urdf的初始相对位姿和关节旋转轴都是定义在link坐标系中的。但是旋量理论动力学中所有量都在质心坐标系中讨论。因此该工程实现了方法urdf_parser，它可以将urdf中的信息转换为旋量动力学所需的量。经过反复验证修改，现在算法已经和KDL，Pinocchio，matlab robotic tool box的结果相同。

modern robotics 的github仓库中给出的算法是正确的，但是它的算法api所需的参数和书上相比有两点不同：
1. 他需要质心i相对于基坐标系的位姿，而非质心i-1相对于质心i的位姿
2. 他需要关节旋量轴在基坐标系的表示，而非关节i旋量轴在质心坐标系i的表示。

但是，modern robotic github上推荐的仓库mr_urdf_loader （作者： MinchangSung0223）在解析urdf的代码中有一段意义不明的语句，导致他们的工具链的结算结果总是有细微的误差。我将其修改，发现KDL，Pinocchio，matlab robotic tool box， 我的算法 以及 modern robotic， 总共五方算法结果一致。问题解决，是mr_urdf_loader源码有问题。

该工程计算一次惯性矩阵(release模式)耗时150us左右(6kHz)，可以达到实时控制要求。但是开源的Pinocchio，KDL求解一次该参数都在微秒级别，还有巨大的提升空间（可能不仅仅是动力学范畴的问题，还可能与计算机硬件，数值算法优化有关）。

动力学算法头文件如下：

#pragma once
#pragma once
#include <iostream>
#include "Eigen/Dense"
#include "pugixml.hpp"
#include <string>
#include <stdlib.h>
#include <fstream>
#include <sstream>
#include <algorithm>
//自由度
#include "robot_basic_params.h"


Eigen::MatrixXd inv_SE3(Eigen::MatrixXd T);

Eigen::MatrixXd vec2so3(Eigen::MatrixXd p);

Eigen::MatrixXd AD_SE3(Eigen::MatrixXd T);

Eigen::MatrixXd ad_screw(Eigen::MatrixXd S);

Eigen::MatrixXd exp_so3_2_SO3(Eigen::MatrixXd w, double theta);

Eigen::MatrixXd exp_twist_2_SE3(Eigen::MatrixXd S, double theta);

Eigen::VectorXd parse_str_vec(std::string scr, int num);

Eigen::MatrixXd Inertial2inerG(Eigen::MatrixXd inertial_i, double m);

Eigen::Matrix<double, 6, 1> Jaccobi_s_i(Eigen::Matrix<double, 6, 1> theta, Eigen::Matrix<double, 6, 1> screw[], int col_i);
Eigen::Matrix<double, 6, 6> Jaccobi_s(Eigen::Matrix<double, 6, 1> theta, Eigen::Matrix<double, 6, 1> screw[], int n);

Eigen::Matrix<double, 3, 3> rpyToRotationMatrix(const Eigen::Vector3d& rpy);

Eigen::Matrix<double, 3, 3> iner_trans(Eigen::Matrix<double, 4, 4> T, Eigen::Matrix<double, 3, 3> iner, double m);


class my_Robot
{
public:
    Eigen::Matrix<double, 4, 4> links_T[DOF + 1];
    Eigen::Matrix<double, 4, 4> T_C[DOF + 1];
    Eigen::Matrix<double, 4, 4> links_M[DOF + 1];
    Eigen::Matrix<double, 6, 1> links_A[DOF];
    Eigen::Matrix<double, 6, 1> links_B[DOF];
    Eigen::Matrix<double, 6, 1> links_S[DOF];
    Eigen::MatrixXd links_mass = Eigen::MatrixXd(DOF, 1);
    Eigen::Matrix<double, 6, 6> inertial_G[DOF];
    Eigen::Matrix<double, 4, 4> T_dh_link[DOF + 1];
    Eigen::Matrix<double, 4, 4> T_dh_c[DOF + 1];
    Eigen::Matrix<double, 3, 3> iner_DH[DOF + 1];

    my_Robot();
    void set_param_from_src_code();
    //urdf解析器
    void urdf_parser(std::string urdf_name, std::string links_names[], std::string axies_names[]);
};

//动力学算法
class my_Robot_dynamic :public my_Robot
{
private:
    Eigen::Matrix<double, 6 * DOF, DOF> stack_A;
    Eigen::Matrix<double, 6 * DOF, 6 * DOF> stack_inerG;
    Eigen::Matrix<double, 6 * DOF, 6 * DOF> stack_ad_V;
    Eigen::Matrix<double, 6 * DOF, 6 * DOF> stack_ad_A_theta_dot;
    Eigen::Matrix<double, 6 * DOF, 6 * DOF> stack_W;
    Eigen::Matrix<double, 6 * DOF, 6 * DOF> stack_L;
    Eigen::Matrix<double, 6 * DOF, 1> stack_V_base;
    Eigen::Matrix<double, 6 * DOF, 1> stack_V_base_dot;
    Eigen::Matrix<double, 6 * DOF, 1> stack_F_tip;

public:
    Eigen::Matrix<double, DOF, DOF> stack_massMat;
    Eigen::Matrix<double, DOF, 1> stack_corMat;
    Eigen::Matrix<double, DOF, 1> stack_graMat;

    my_Robot_dynamic();
    //逆向动力学
    Eigen::Matrix<double, DOF, 1> forword_dyn(
        Eigen::Matrix<double, DOF, 1> iter_q,
        Eigen::Matrix<double, DOF, 1> iter_q_dot,
        Eigen::Matrix<double, DOF, 1> iter_q_ddot,
        Eigen::Matrix<double, 3, 1> gravity);
    //惯性矩阵
    Eigen::Matrix<double, DOF, DOF> cal_massMat(
        Eigen::Matrix<double, DOF, 1> iter_q);
    //科里奥利力与向心力矩阵
    Eigen::Matrix<double, DOF, 1> cal_corMat(
        Eigen::Matrix<double, DOF, 1> iter_q,
        Eigen::Matrix<double, DOF, 1> iter_q_dot);
    //重力矩阵
    Eigen::Matrix<double, DOF, 1> cal_graMat(
        Eigen::Matrix<double, DOF, 1> iter_q,
        Eigen::Matrix<double, 3, 1> gravity);
};