ROS创建KDL tree

参考链接：https://www.cnblogs.com/21207-iHome/p/8316030.html

简介

KDL(Kinematics and Dynamics Library)中定义了一个树来代表机器人的运动学和动力学参数，ROS中的kdl_parser提供了工具能将机器人描述文件URDF转换为KDL tree.

Kinematic Trees: 链或树形结构。已经有多种方式来定义机构的运动学结构，KDL使用图论中的术语来定义：

• A closed-loop mechanism is a graph, 闭链机构是一幅图
• an open-loop mechanism is a tree,  开链机构是一棵树
• an unbranched tree is a chain. 没有分支的树是一个运动链

　　KDL Chain和KDL Tree都由最基本的KDL Segments元素串接而成，Segment可以理解为机构运动链上的一个运动部件。如下图所示KDL Segment包含关节KDL Joint 以及部件的质量/惯性属性KDL RigidBodyInertia，并且定义了一个参考坐标系Freference和末端坐标系Ftip

末端到关节坐标系的转换由Ttip描述。在一个运动链或树中，子部件会被添加到父部件的末端，因此上一个部件的Ftip就是下一个部件的参考坐标系Freference (tip frame of parent = reference frame of the child). 通常Fjoint和Freference是重合的，但是也可以存在偏移。

C++中创建KDL tree  

为了使用KDL parser需要在package.xml中添加相关依赖项：

<package>
    ...
    <build_depend>kdl_parser</build_depend>
    ...
    <exec_depend>kdl_parser</exec_depend>
    ...
  </package>

如下图：

 另外还需要在C++程序中加入相关的头文件：

#include <kdl_parser/kdl_parser.hpp>

 下面有多种从urdf创建KDL tree的方式：

1. From a file

KDL::Tree my_tree;
   if (!kdl_parser::treeFromFile("filename", my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

 演示：

可以看到我这个工作空间下有一些urdf文件：

 我们编辑test.cpp代码测试一下：

#include "ros/ros.h"
#include <kdl_parser/kdl_parser.hpp>


int main(int argc, char** argv)
{
    ros::init(argc, argv, "ik_test_node");
    ros::NodeHandle n;

    KDL::Tree my_tree;
    if (!kdl_parser::treeFromFile("/home/zhitong/catkin_ws_serial/src/quadruped_9g-master/urdf/leg.urdf", my_tree)){
       ROS_ERROR("Failed to construct kdl tree");
       return false;
    }
    else
    ROS_INFO("Successfull!");
    
}

如下图： 

编译运行成功：

修改源码里文件为不存在的leg1.urdf：

 保存——编译——运行：

出现如下错误，说明工作机制正常。