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

十年一梦实验室

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

阅读量1k

点赞数 7

CC 4.0 BY-SA版权

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/136644593

博客介绍了多个用于机器人动力学计算的类，如 chainidsolver_recursive_newton_euler 等。这些类基于不同算法，可根据关节状态和外力信息计算关节扭矩或加速度，考虑了代码健壮性，应用于机器人控制、模拟和分析等场景。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

求解器思维导图

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。主要功能包括：