参考链接: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:
保存——编译——运行:
出现如下错误,说明工作机制正常。