【kdl】KDL-Examples 简单示例

KDL-Examples | The Orocos Project

https://www.orocos.org/kdl/examples.html

Forward kinematic chain example

这段代码定义并利用了The Kinematics and Dynamics Library (KDL)库，对一个机器人运动学链进行正向位置运动学的计算。简单来说，代码通过用户输入各关节角度的值，通过正向运动学的方法计算出机器人的末端执行器在空间中的位置（也就是机器人手臂的末端相对于基座的位置）。

// 版权所有 (C) 2007 Francois Cauwe <francois at cauwe dot org>
 
// 这个库是免费软件; 你可以重新分发和/或修改
// 它遵循由自由软件基金会发布的GNU较宽松公共许可证条款；
// 版本2.1，或者（由你选择）任何后续版本。


#include <kdl/chain.hpp>                             // 引入KDL命名空间的链结构
#include <kdl/chainfksolver.hpp>                     // 引入KDL命名空间下的正向动力学求解器
#include <kdl/chainfksolverpos_recursive.hpp>        // 引入KDL命名空间下的递归正向位置求解器
#include <kdl/frames_io.hpp>                         // 引入KDL命名空间下的框架输入输出相关功能
#include <stdio.h>                                   // 引入标准输入输出头文件
#include <iostream>                                  // 引入标准输入输出流


using namespace KDL;                                 // 使用KDL命名空间




int main( int argc, char** argv )                    // 主函数入口
{
    // 定义一个运动学链并且向链中添加关节
    KDL::Chain chain;                                // 创建一个链对象
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020)))); // 向链中添加一个绕Z轴旋转的关节和对应的变换矩阵
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480)))); // 向链中添加一个绕X轴旋转的关节和对应的变换矩阵
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645)))); // 向链中添加一个绕X轴旋转的关节和对应的变换矩阵
    chain.addSegment(Segment(Joint(Joint::RotZ)));                             // 向链中添加一个绕Z轴旋转的关节
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120)))); // 向链中添加一个绕X轴旋转的关节和对应的变换矩阵
    chain.addSegment(Segment(Joint(Joint::RotZ)));                             // 向链中添加一个绕Z轴旋转的关节


    // 创建基于运动学链的求解器
    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);    // 创建一个递归的正向位置动力学求解器对象


    // 创建关节数组
    unsigned int nj = chain.getNrOfJoints();           // 得到链中关节的数量
    KDL::JntArray jointpositions = JntArray(nj);       // 根据关节数量创建一个关节位置数组


    // 为关节位置赋一些值
    for(unsigned int i=0;i<nj;i++){                    // 遍历所有关节
        float myinput;
        printf ("Enter the position of joint %i: ",i); // 打印提示信息，让用户输入第i个关节的位置
        scanf ("%e",&myinput);                         // 从标准输入读取关节位置
        jointpositions(i)=(double)myinput;             // 将输入的关节位置转换为双精度浮点数，并赋值给关节位置数组
    }


    // 创建一个框架以包含结果
    KDL::Frame cartpos;                                // 创建一个框架对象，用来存放计算结果


    // 计算正向位置运动学
    bool kinematics_status;
    kinematics_status = fksolver.JntToCart(jointpositions,cartpos); // 使用求解器计算正向运动学，结果会存储在cartpos中
    if(kinematics_status>=0){
        std::cout << cartpos <<std::endl;              // 如果计算成功，打印计算出的末端执行器位置信息
        printf("%s \n","Success, thanks KDL!");        // 打印成功信息
    }else{
        printf("%s \n","Error: could not calculate forward kinematics :("); // 如果计算失败，打印错误信息
    }
}

Inverse position example

// 创建运动链：
KDL::Chain chain;    // 定义一个KDL链对象
// 为链添加关节和对应的参考坐标系（以Segment段的形式添加）
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020)))); // 添加一个绕Z轴旋转的关节，并设置其位置离开基准坐标1.020单位
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480)))); // 添加一个绕X轴旋转的关节，并设置其位置离开上一个坐标0.480单位
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645)))); // 再添加一个绕X轴旋转的关节，并设置其位置离开上一个坐标0.645单位
chain.addSegment(Segment(Joint(Joint::RotZ)));                             // 添加一个绕Z轴旋转的关节
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120)))); // 再添加一个绕X轴旋转的关节，并设置其位置离开上一个坐标0.120单位
chain.addSegment(Segment(Joint(Joint::RotZ)));                             // 最后添加一个绕Z轴旋转的关节


// 创建求解器：
ChainFkSolverPos_recursive fksolver1(chain1); // 创建一个正向位置求解器，主要用来解决从关节位置到末端位置的计算
ChainIkSolverVel_pinv iksolver1v(chain1);     // 创建一个逆向速度求解器，使用伪逆方法进行求解
// 创建一个逆向位置求解器，结合正向位置求解器和逆向速度求解器，并设定最大迭代次数为100次，精度为1e-6
ChainIkSolverPos_NR iksolver1(chain1,fksolver1,iksolver1v,100,1e-6);  


// 创建关节数组：
JntArray q(chain.getNrOfJoints());             // 根据链中关节数量，创建一个实际关节位置的数组
JntArray q_init(chain.getNrOfJoints());        // 创建一个初始关节位置的数组，可能用于给逆向位置求解器一个起始解


// 设置目标框架
Frame F_dest=...;                               // 定义一个目标框架，通常表示末端执行器希望到达的位置和姿态


// 执行逆向位置运动学求解
int ret = iksolverpos.CartToJnt(q_init,F_dest,q); // 调用逆向位置求解器，将末端执行器的目标位置转换成关节位置

以上代码片段通过操作KDL（Kinematics and Dynamics Library，机器人运动学和动力学库）中的类和方法，用来构建一个机器人的运动学链条，以及创建了正向和逆向运动学求解器。其中，正向运动学求解器用于根据给定的关节位置计算末端执行器的位置和姿态；逆向运动学求解器逆过来根据末端执行器的目标位置和姿态求解得出关节角度。这在机器人编程中常用于路径规划和控制任务，确保机器人末端能够达到指定的位置并且保持所需姿态。