【kdl】源码解析-动力学求解器

十年一梦实验室

于 2024-03-12 00:09:06 发布

阅读量526

点赞数 5

本文链接：https://blog.csdn.net/cxyhjl/article/details/136644593

版权

求解器思维导图

chainidsolver_recursive_newton_euler

ChainIdSolver_RNE：这个类是用于解决逆动力学问题的，它基于书籍 "Rigid Body Dynamics Algorithms" （Roy Featherstone, 2008）中的算法。

ChainIdSolver_RNE 类的定义如下：

它继承自 ChainIdSolver 类，是 KDL 中专用于逆动力学求解的一部分。
构造函数 ChainIdSolver_RNE 创建求解器实例，接受一个动力学链（Chain 类型）和一个表示重力的向量（Vector grav）作为参数。内部会创建这个动力学链的副本以及分配必要的内存用于计算。
析构函数 ~ChainIdSolver_RNE 是空的，说明类中无需特殊处理的资源释放。
CartToJnt 方法用于计算由 Cartesian 力量到关节扭矩的转换，需要提供当前关节位置 (q)、当前关节速度 (q_dot)、当前关节加速度 (q_dotdot)、外部作用在各个链段上的力量（f_ext 不包括重力），输出结果为关节扭矩 (torques)。
updateInternalDataStructures 方法虚拟实现，用于更新内部数据结构，这在基类 SolverI 中定义，并要求每个具体的求解器类进行实现。

ChainIdSolver_RNE 类的成员变量用于存储计算逆动力学过程中需要的中间值，包括：

const Chain& chain：运动链对象，求解器针对这个链进行计算。
unsigned int nj：运动链中关节的数量。
unsigned int ns：运动链中链段的数量。
std::vector<Frame> X：存储运动链中每个链段相对于基座的坐标变换。
std::vector<Twist> S：存储运动链中每个关节的motion subspace向量。
std::vector<Twist> v：存储运动链中每个链段的速度。
std::vector<Twist> a：存储运动链中每个链段的加速度。
std::vector<Wrench> f：存储运动链中对每个链段产生作用的力和力矩。
Twist ag：存储由重力产生的加速度。

这个类的实现使得用户可以通过提供某一时刻的关节状态和外力信息，来计算出各个关节所需的扭矩，这对于机器人的控制和仿真至关重要。

#ifndef KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP
#define KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP


#include "chainidsolver.hpp"


namespace KDL{
    /**
     * \brief 递归牛顿-欧拉逆动力学求解器
     *
     * 算法的实现是基于 Roy Featherstone 2008 年的书 "Rigid Body
     * Dynamics Algorithms" (ISBN:978-0-387-74314-1) 第96页的伪代码。
     * 
     * 它计算关节的扭矩，给定关节的运动（q,qdot,qdotdot）、作用在段上的外部力
     * （在段的参考框架中表示）以及段的动力学参数。
     */
    class ChainIdSolver_RNE : public ChainIdSolver{
    public:
        /**
         * 该求解器的构造函数，将分配所有必要的内存
         * \param chain 要计算逆动力学的动力链，将制作一个内部副本。
         * \param grav 在计算中使用的重力向量。
         */
        ChainIdSolver_RNE(const Chain& chain,Vector grav);
        ~ChainIdSolver_RNE(){}; // 析构函数
        
        /**
         * 从笛卡尔力计算到关节扭矩的函数。
         * 输入参数：
         * \param q 当前关节位置
         * \param q_dot 当前关节速度
         * \param q_dotdot 当前关节加速度
         * \param f_ext 段上的外部力（不含重力）
         * 输出参数：
         * \param torques 结果关节的扭矩
         */
        int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques);


        /// @copydoc KDL::SolverI::updateInternalDataStructures
        // 更新内部数据结构
        virtual void updateInternalDataStructures();


    private:
        const Chain& chain; // 动力链
        unsigned int nj;    // 关节数量
        unsigned int ns;    // 段数量
        std::vector<Frame> X; // 保存从基坐标系到当前段末端局部坐标系的变换
        std::vector<Twist> S; // 保存当前段的速度
        std::vector<Twist> v; // 保存当前段的速度
        std::vector<Twist> a; // 保存当前段的加速度
        std::vector<Wrench> f; // 保存段受到的力
        Twist ag; // 保存重力加速度
    };
}


#endif

该段代码定义了一个根据机械臂的运动学模型来计算关节力矩的类ChainIdSolver_RNE。主要功能包括：

根据给定的关节位置（q），速度（q_dot），加速度（q_dotdot）和外部扭矩（f_ext），通过牛顿-欧拉递归算法计算相应的关节扭矩（torques）。
通过KDL库提供的数据结构和函数来获取关节和段的数量、位姿、转动惯量等信息，并进行物理量的转换和计算。
编写时考虑了代码的健壮性，具体表现在检查了数组尺寸的正确性，并提供了错误代码以处理可能的异常情况。
设计了从机械臂基部到末端以及从末端反向到基部的两次遍历，以计算节点力和力矩。

这段代码是机器人动力学计算的基础，被应用在机器人控制、模拟和分析等场景中。

这段代码定义了一个名为 ChainIdSolver_RNE 的递归牛顿-欧拉逆动力学求解器类。该类使用递归牛顿-欧拉算法计算KDL::Chain（机器人运动学链）中的逆动力学，给定关节的位置、速度和加速度以及外部力矩，计算出机器人关节所需的力矩。

chainidsolver_vereshchagin

ChainHdSolver_Vereshchagin 的类，该类实现了科学家Vereshchagin于1989年提出的解算运动学和动力学方程的解算器。该求解器可以处理机器人姿态、速度、加速度以及外力、内力和约束力的关系，用于计算动力学和运动学中的关节空间动力和找出末端执行器的动力学约束。

非公开成员函数和变量：

acc_root: 存储机器人基座的加速度。
chain: 存储机器人的运动学链。
nj, ns, nc: 分别储存关节数量、链节数量和约束条件数量。
segment_info: 存储每个链节的动力学参数和计算结果。
total_torques: 存储所有关节上的总扭矩。

此类通常用于高级机器人学应用，包括但不限于在线的动力学计算、控制系统设计以及复杂任务的规划和模拟。

构造函数 ChainHdSolver_Vereshchagin：根据运动链、根加速度和约束力的数量初始化求解器的内部数据结构。
updateInternalDataStructures 方法：更新运动链的段数和关节数信息，并重新初始化总力矩向量和部分结果向量。
CartToJnt 方法：主要求解方法，用于计算给定的运动链关节加速度、约束力矩等。它调用下面的几个子方法来完成求解过程。
initial_upwards_sweep 方法：正向递归遍历运动链，计算各个关节和段的位置、速度、和由运动产生的偏差力。
downwards_sweep 方法：逆向递归遍历运动链，计算其惯性、由链接产生的偏差力和约束力。
constraint_calculation 方法：计算由于约束而产生的约束力数值。
final_upwards_sweep 方法：根据计算结果对关节加速度和约束力矩进行最终的正向递归计算。

此外，类中还包含了一系列get*方法，用于获取链条的各种状态信息，如加速度、力矩和约束力。

整个求解器的核心在于能够考虑机器人或者运动链在存在外力和约束力作用下的动力学行为，并能提供相应的加速度和力矩信息。此求解器通常用于高级的运动控制和分析任务，如在机器人技术中的轨迹规划、力控制等领域。

#ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
#define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP


#include "chainidsolver.hpp"
#include "frames.hpp"
#include "articulatedbodyinertia.hpp"


#include<Eigen/StdVector>


// KDL命名空间的定义
namespace KDL
{
/**
 * @brief 本类基于1989年Vereshchagin提出的算法，为链状结构的机器人实现了速度受限的混合动力学计算。
 * 
 * 此类实例化混合动力学求解器，该求解器基于给定的约束力求解当在链状结构的末端执行器（任务空间/笛卡尔空间）施加约束力时，链状结构中关节空间的约束扭矩和加速度。
 * 在机器人学文献中，这个算法也以以下名称为人所知：速度受限的混合动力学（ACHD）或Popov-Vereshchagin求解器。
 * 
 * 详细描述如下...
 */


// ChainHdSolver_Vereshchagin类的定义
class ChainHdSolver_Vereshchagin : KDL::SolverI
{
    // 类型定义和Eigen库相关的向量和矩阵类型
    typedef std::vector<Twist> Twists;
    typedef std::vector<Frame> Frames;
    typedef Eigen::Matrix<double, 6, 1 > Vector6d;
    typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
    typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;


public:
    /**
     * @brief 构造函数，它将分配所有必要的内存。
     * @param chain 用于计算混合动力学的运动链。函数内部会创建一个副本。
     * @param root_acc 在计算过程中使用的根部分段的加速度扭（通常包含重力）。
     * 注意：相比于KDL的FD和RNE求解器，本求解器采取了与重力加速度相反的符号。
     * @param nc 施加在机器人末端执行器上的约束数（最多为6）。
     */
    ChainHdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc);


    // 析构函数
    ~ChainHdSolver_Vereshchagin()
    {
    };


    /**
     * @brief 此方法计算关节空间的约束扭矩和加速度。
     * 成功时返回0，否则数组大小不匹配时返回-1或-2。
     * 输入参数：
     * @param q 当前关节位置
     * @param q_dot 当前关节速度
     * @param alpha 活动约束方向（单位约束力，相对于机器人基座标系表示）
     * @param beta 加速度能量设定点（相对于上述定义的单位约束力表示）
     * @param f_ext 施加在各段上的外部力（不包括重力，该值已在根加速度中给出）
     * @param ff_torques 预设的关节空间扭矩
     * 
     * 输出参数：
     * @param q_dotdot 结果关节加速度
     * @param constraint_torques 结果关节约束扭矩（每个关节由于在末端执行器上的约束力作用而感受到的）
     * 
     * @return 错误/成功代码
     */
    int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, const JntArray &ff_torques, JntArray &constraint_torques);


    // 更新内部数据结构
    virtual void updateInternalDataStructures();


    //获取基座标系下的链状结构各环节的加速度
    void getTransformedLinkAcceleration(Twists& x_dotdot);


    //获取作用在每个关节上的总扭矩（约束+自然+外部力）
    void getTotalTorque(JntArray &total_tau);


    //获取作用在末端执行器的约束力的大小：Lagrange乘数
    void getContraintForceMagnitude(Eigen::VectorXd &nu_);


    // 其他相关的成员函数声明略去...


```cpp
private:
    // 计算所有笛卡尔空间位姿、扭转和偏置加速度。
    // 该向外扫描过程中还考虑了外部力。
    void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);


    // 这个方法是一个力平衡扫描。它计算了铰接体惯量和偏置力。
    // 此外，也计算了由偏置力和单位力产生的加速度能量。
    void downwards_sweep(const Jacobian& alfa, const JntArray& ff_torques);


    // 该方法计算约束力的大小。
    void constraint_calculation(const JntArray& beta);


    // 该方法将所有加速度贡献（约束、偏置、零空间和父级加速度）综合起来。
    void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques);


private:
    // 私有成员变量
    const Chain& chain; // 运动链的引用
    unsigned int nj; // 关节数量
    unsigned int ns; // 链状结构中的段数
    unsigned int nc; // 末端执行器上的约束数量
    Twist acc_root; // 根部分段的加速度Twist
    Jacobian alfa_N; // 第N个段（也即末端执行器）的Jacobian矩阵
    Jacobian alfa_N2; // 另一个与末端执行器相关的Jacobian矩阵
    Eigen::MatrixXd M_0_inverse; // 矩阵M_0的逆
    Eigen::MatrixXd Um; // 矩阵Um
    Eigen::MatrixXd Vm; // 矩阵Vm
    JntArray beta_N; // 对应于第N个段的beta向量
    Eigen::VectorXd nu; // 约束力的Lagrange乘子
    Eigen::VectorXd nu_sum; // 约束力累加结果
    Eigen::VectorXd Sm; // 向量Sm
    Eigen::VectorXd tmpm; // 临时向量tmpm
    Eigen::VectorXd total_torques; // 关节上感受到的所有扭矩贡献：约束+自然+外部力
    Wrench qdotdot_sum; // 扭转加速度求和
    Frame F_total; // 总位姿


    // segment_info结构体，保存每个段的计算信息
    struct segment_info
    {
        Frame F; // 相对于上一个链接的段的局部位姿
        Frame F_base; // 相对于根部分段的位姿
        Twist Z; // 单位扭转
        Twist v; // 扭转
        Twist acc; // 加速度扭转
        Wrench U; // 偏置力的扳手（笛卡尔空间）
        Wrench R; // 偏置力的扳手
        Wrench R_tilde; // 新的偏置力扳手向量，以矩阵形式表示
        Twist C; // 约束
        Twist A; // 约束
        ArticulatedBodyInertia H; // I（表示为6×6的矩阵）
        ArticulatedBodyInertia P; // I（表示为6×6的矩阵）
        ArticulatedBodyInertia P_tilde; // I（表示为6×6的矩阵）
        Wrench PZ; // 向量U[i] = I_A[i]*S[i]
        Wrench PC; // 向量E[i] = I_A[i]*c[i]
        double D; // 向量D[i] = S[i]^T*U[i]
        Matrix6Xd E; // 由加速度约束产生的虚拟单位约束力矩阵
        Matrix6Xd E_tilde;
        Eigen::MatrixXd M; // 在链i处已生成的加速度能量
        Eigen::VectorXd G; // 在链i处已生成的约束力的大小
        Eigen::VectorXd EZ; // K[i] = Etiltde'*Z
        double nullspaceAccComp; // 加速度零空间贡献
        double constAccComp; // 关节空间约束力对关节空间加速度的贡献
        double biasAccComp; // 关节空间偏置力对关节空间加速度的贡献
        double totalBias; // 总偏置力，包括离心力和科里奥利力在关节子空间中的值
        double u; // 在关节子空间中的向量u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i])


        // 构造函数
        segment_info(unsigned int nc):
            D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
        {
            E.resize(6, nc);
            E_tilde.resize(6, nc);
            G.resize(nc);
            M.resize(nc, nc);
            EZ.resize(nc);
            E.setZero();
            E_tilde.setZero();
            M.setZero();
            G.setZero();
            EZ.setZero();
        };
    };


    // 存储链条每个段的信息
    std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
};


}


# endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP

chainfdsolver_recursive_newton_euler

ChainFdSolver_RNE 继承自 ChainFdSolver，它基于Roy Featherstone的书中第6章所描述的基础算法实现了前向动力学的计算。类中包括如下主要公有方法和私有成员：

公有方法:

ChainFdSolver_RNE(const Chain& chain, Vector grav)：构造函数，用于初始化前向动力学求解器，并将为内部计算内存。参数包含代表机器人的运动学链chain以及在计算中使用的重力矢量grav。
virtual ~ChainFdSolver_RNE()：析构函数。
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot)：此函数基于当前的关节位置q、关节速度q_dot、施加的关节驱动力矩torques和作用在链段上的外部力（不包括重力）f_ext，计算导致的关节加速度q_dotdot。
void RK4Integrator(unsigned int& nj, const double& t, double& dt, JntArray& q, JntArray& q_dot, JntArray& torques, Wrenches& f_ext, ChainFdSolver_RNE& fdsolver, JntArray& q_dotdot, JntArray& dq, JntArray& dq_dot, JntArray& q_temp, JntArray& q_dot_temp)：此函数是一个四阶龙格-库塔（RK4）积分器，用于积分前向动力学求解器得到的关节加速度。
virtual void updateInternalDataStructures()：更新内部数据结构。

私有成员:

const Chain& chain：KDL中Chain对象的引用。
ChainDynParam DynSolver：动态参数求解器。
ChainIdSolver_RNE IdSolver：递归牛顿-欧拉逆动力学求解器。
unsigned int nj：链中关节的数量。
unsigned int ns：链中链段（Segment）的数量。
JntSpaceInertiaMatrix H：关节空间惯性矩阵。
JntArray Tzeroacc：在没有加速度的情况下的关节力矩数组。
Eigen::MatrixXd H_eig：H矩阵对应的Eigen库中的类型。
Eigen::VectorXd Tzeroacc_eig：Tzeroacc数组对应的Eigen库中的类型。
Eigen::MatrixXd L_eig：定位矩阵对应的Eigen库中的类型。
Eigen::VectorXd D_eig：密度矩阵对应的Eigen库中的类型。
Eigen::VectorXd r_eig：用于中间计算的Eigen库Vectors。
Eigen::VectorXd acc_eig：加速度对应的Eigen库Vectors。

类ChainFdSolver_RNE实现了用于计算机器人关节动力学的算法，可以处理关节位置、速度、力矩输入，并计算出关节加速度。该求解器可以用于仿真、控制和分析机器人的动态行为。

#ifndef KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP
#define KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP


#include "chainfdsolver.hpp"  // 包含了基础的前向动力学求解器头文件
#include "chainidsolver_recursive_newton_euler.hpp"  // 包含逆动力学的递归牛顿-欧拉求解器头文件
#include "chaindynparam.hpp"  // 包含链动力学参数头文件


// 定义命名空间KDL
namespace KDL{
    /**
     * \brief 递归牛顿-欧拉前向动力学求解器
     *
     * 该算法实现基于Roy Featherstone所著的《Rigid Body
     * Dynamics Algorithms》（2008年，ISBN:978-0-387-74314-1）。参见第6章基本算法。
     *
     * 它计算了给定关节的位置、速度（q, qdot）和关节上的外部力（在每个段的参考框架中表达）
     * 以及各个段的动力学参数后的关节加速度（qdotdot）。
     */
    class ChainFdSolver_RNE : public ChainFdSolver{
    public:
        /**
         * 解算器的构造函数，将分配所有必要的内存
         * \param chain 要为其计算前向动力学的运动学链，内部会制作副本。
         * \param grav 计算过程中使用的重力向量。
         */
        ChainFdSolver_RNE(const Chain& chain, Vector grav);  // 构造函数
        ~ChainFdSolver_RNE(){};  // 析构函数


        /**
         * 从笛卡尔力到关节扭矩的计算函数。
         * 输入参数：
         * \param q 当前的关节位置
         * \param q_dot 当前的关节速度
         * \param torques 当前的关节扭矩（由控制器施加）
         * \param f_ext 每个段上的外部力（不包括重力）
         * 输出参数：
         * \param q_dotdot 计算得到的关节加速度
         */
        int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot);


        /// @copydoc KDL::SolverI::updateInternalDataStructures
        virtual void updateInternalDataStructures();  // 虚函数，用于更新内部数据结构


        /**
         * 用于整合前向动力学求解器的关节加速度的函数。
         * 输入参数：
         * \param nj 关节数量
         * \param t 当前时间
         * \param dt 积分时段
         * \param q 当前关节位置
         * \param q_dot 当前关节速度
         * \param torques 当前关节扭矩（由控制器施加）
         * \param f_ext 段上的外部力（不包括重力）
         * \param fdsolver 前向动力学求解器
         * 输出参数：
         * \param t 更新后的时间
         * \param q 更新后的关节位置
         * \param q_dot 更新后的关节速度
         * \param q_dotdot 当前关节加速度
         * \param dq 关节位置的增量
         * \param dq_dot 关节速度的增量
         * 临时参数：
         * \param qtemp 中间的关节位置
         * \param qdtemp 中间的关节速度
         */
        void RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
                           KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
                           KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
                           KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp);


    private:
        const Chain& chain;  // 运动学链的引用
        ChainDynParam DynSolver;  // 动力学参数求解器实例
        ChainIdSolver_RNE IdSolver;  // 逆动力学求解器实例
        unsigned int nj;  // 关节数
        unsigned int ns;  // 段数
        JntSpaceInertiaMatrix H;  // 关节空间惯性矩阵
        JntArray Tzeroacc;  // 在无加速度时的关节扭矩
        Eigen::MatrixXd H_eig;  // 对应Eigen库的关节空间惯性矩阵
        Eigen::VectorXd Tzeroacc_eig;  // 对应Eigen库的无加速度时的关节扭矩向量
        Eigen::MatrixXd L_eig;  // Eigen中使用的科里奥利(Lambdadt)矩阵
        Eigen::VectorXd D_eig;  // Eigen中使用的外力子空间动力学(操作空间加速度U+操作空间重力g)向量
        Eigen::VectorXd r_eig;  // Eigen中使用的冗余力向量
        Eigen::VectorXd acc_eig;  // Eigen中使用的加速度向量
    };
} // end of namespace KDL


#endif // KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP

treeidsolver_recursive_newton_euler

TreeIdSolver_RNE 类是 Kinematic Trees 的一个递归牛顿-欧拉逆动力学求解器。

公共成员

构造函数 TreeIdSolver_RNE(const Tree& tree, Vector grav); - 用于分配所有必要的内存资源，并存储动力学树和重力向量的内部引用。
成员函数 int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques); - 用来根据当前关节的位置、速度、加速度以及作用在段上的外部力（不包含重力），计算关节的力矩。
虚函数 virtual void updateInternalDataStructures(); - 用于更新内部数据结构，确保树的修改能够正确反映。

私有成员

辅助函数 void initAuxVariables(); - 用于初始化类中的私有成员变量 X, S, v, a, f。
递归步骤函数 void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques); - 为逆动力学计算进行的递归调用函数。
私有变量包括：
- const Tree& tree; - 引用传递的动力学树。
- unsigned int nj; - 树中关节的数量。
- unsigned int ns; - 树中段的数量。
- std::map<std::string,Frame> X; - 帧数据的映射（段名到帧的映射）。
- std::map<std::string,Twist> S; - 扭转数据的映射。
- std::map<std::string,Twist> v; - 段的速度，用扭转表示。
- std::map<std::string,Twist> a; - 段的加速度，用扭转表示。
- std::map<std::string,Wrench> f; - 所受力矩。
- Twist ag; - 加速度，由传递的重力参数转换得到。

在 TreeIdSolver_RNE 类中，逆动力学问题被转化为计算给定的运动状态（关节的位置、速率和加速度）和外部作用力（通过 STL 映射表示）下，树中每个关节上所需的力矩。这对于机器人动力模型的模拟和控制算法的设计非常重要。

#ifndef KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
#define KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP


#include "treeidsolver.hpp"


// 包含逆动力学求解器的基类头文件


namespace KDL{
    /**
     * \brief Recursive newton euler inverse dynamics solver for kinematic trees.
     * 用于运动学树的递归牛顿-欧拉逆动力学求解器。
     *
     * It calculates the torques for the joints, given the motion of
     * the joints (q,qdot,qdotdot), external forces on the segments
     * (expressed in the segments reference frame) and the dynamical
     * parameters of the segments.
     * 它计算关节的扭矩，给定关节的运动（q,qdot,qdotdot）、分布在各段上的外力
     * （在段的参考框架中表达）以及各段的动力学参数。
     *
     * This is an extension of the inverse dynamic solver for kinematic chains,
     * \see ChainIdSolver_RNE. The main difference is the use of STL maps
     * instead of vectors to represent external wrenches (as well as internal
     * variables exploited during the recursion).
     * 这是运动链逆动力学求解器的扩展，见 ChainIdSolver_RNE。主要区别在于使用 STL 映射
     * 而不是向量来表示外力扭矩（以及在递归过程中使用的内部变量）。
     */
    class TreeIdSolver_RNE : public TreeIdSolver {
    public:
        /**
         * Constructor for the solver, it will allocate all the necessary memory
         * 求解器的构造函数，它将分配所有必要的内存
         * \param tree The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored.
         * \param grav The gravity vector to use during the calculation.
         * \param tree 用于计算逆动力学的运动学树，将存储一个内部引用。
         * \param grav 计算过程中使用的重力向量。
         */
        TreeIdSolver_RNE(const Tree& tree, Vector grav);


        /**
         * Function to calculate from Cartesian forces to joint torques.
         * 从笛卡尔力到关节扭矩的计算函数。
         * Input parameters;
         * 输入参数：
         * \param q The current joint positions
         * \param q_dot The current joint velocities
         * \param q_dotdot The current joint accelerations
         * \param f_ext The external forces (no gravity) on the segments
         * \param q 当前关节位置
         * \param q_dot 当前关节速度
         * \param q_dotdot 当前关节加速度
         * \param f_ext 受到的外力（不包括重力）在各段上
         * Output parameters:
         * 输出参数：
         * \param torques the resulting torques for the joints
         * \param torques 产生的关节扭矩
         */
        int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques);


        /// @copydoc KDL::SolverI::updateInternalDataStructures
        virtual void updateInternalDataStructures();


    private:
        ///Helper function to initialize private members X, S, v, a, f
        ///辅助功能，用于初始化私有成员 X, S, v, a, f
        void initAuxVariables();


        ///One recursion step
        ///一步递归计算
        void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques);


        const Tree& tree; // 对象的内部引用，包含运动学树
        unsigned int nj; // 关节数量
        unsigned int ns; // 段数量
        std::map<std::string,Frame> X; // 用于递归过程中每一段的位置存储
        std::map<std::string,Twist> S; // 用于递归过程中每一段的扭矩存储
        std::map<std::string,Twist> v; // 用于递归过程中每一段的速度存储
        std::map<std::string,Twist> a; // 用于递归过程中每一段的加速度存储
        std::map<std::string,Wrench> f; // 用于递归过程中每一段的受力存储
        Twist ag; // 用于存储加速度
    };
}


#endif

此代码段是递归牛顿-欧拉方法逆动力学求解器TreeIdSolver_RNE的声明头文件。该类继承自TreeIdSolver，目的是用于运动学树结构的动力学计算，具体来说是计算关节的扭矩。

在这个类的构造函数中，需要传入运动学树的实例和重力矢量。CartToJnt方法是该类的核心公共方法，输入参数包含了关节位置q、速度q_dot、加速度q_dotdot和各段的外部力f_ext，输出计算出的关节扭矩torques。

initAuxVariables是一个私有辅助函数，用于初始化类内对每个段的位置X、扭矩S、速度v、加速度a和受力f映射。rne_step是递归计算的核心函数，它负责将递归算法应用到每个段，以计算动力学数值。

该文件通过预处理宏保证头文件只被包含一次，避免了重复定义的问题。这是C++中常规的头文件声明方式，旨在提供清晰的类声明和接口，等待后续的实现文件来填充具体的逻辑细节和算法实现。

作者陈晓永：智能装备专业高级职称，软件工程师，机械设计中级职称，机器人与自动化产线仿真动画制作

The End

十年一梦实验室

关注

5
点赞
踩
16

收藏

觉得还不错? 一键收藏
0
评论
【kdl】源码解析-动力学求解器

求解器思维导图chainidsolver_recursive_newton_eulerChainIdSolver_RNE：这个类是用于解决逆动力学问题的，它基于书籍 "Rigid Body Dynamics Algorithms" （Roy Featherstone, 2008）中的算法。ChainIdSolver_RNE类的定义如下：它继承自ChainIdSolver类，是 KDL 中专用...
复制链接

扫一扫