orocos_kdl学习（二）：KDL Tree与机器人运动学

　　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，并且定义了一个参考坐标系F_reference和末端坐标系F_tip

orocos_kdl学习（二）：KDL Tree与机器人运动学

KDL segment

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

KDL chain

KDL tree

　　KDL中的定义与URDF中的定义基本是一样的：

orocos_kdl学习（二）：KDL Tree与机器人运动学

　　也可以参考MATLAB Robotics System Toolbox中的对Rigid Body Tree Robot Model的描述：

orocos_kdl学习（二）：KDL Tree与机器人运动学

Python中创建KDL tree

　　参考pykdl_utils，pykdl_utils中包含了kdl_parser.py（用于解析URDF文件并将其转换为KDL tree或chain），kdl_kinematics.py（封装了KDL kinematics的一系列函数，使得用Python调用更方便）等实用程序。下面先安装urdfdom_py（Python implementation of the URDF parser）：

sudo apt-get install ros-indigo-urdfdom-py

　　然后在github上下载pykdl_utils的源代码，使用catkin_make进行编译。

convert URDF objects into PyKDL.Tree

　　首先通过urdf_parser_py来解析URDF文件，有下面几种使用方式：通过xml字符串解析、xml文件解析，以及从ROS 参数服务器获取robot_description字符串信息。

#! /usr/bin/env python

# Load the urdf_parser_py manifest, you use your own package
# name on the condition but in this case, you need to depend on
# urdf_parser_py.
import roslib; roslib.load_manifest('urdfdom_py')
import rospy

# Import the module

from urdf_parser_py.urdf import URDF

# 1. Parse a string containing the robot description in URDF.
# Pro: no need to have a roscore running.
# Cons: n/a
# Note: it is rare to receive the robot model as a string.
robot = URDF.from_xml_string("<robot name='myrobot'></robot>")

# - OR -

# 2. Load the module from a file.
# Pro: no need to have a roscore running.
# Cons: using hardcoded file location is not portable.
robot = URDF.from_xml_file()

# - OR -

# 3. Load the module from the parameter server.
# Pro: automatic, no arguments are needed, consistent
#      with other ROS nodes.
# Cons: need roscore to be running and the parameter to
#      to be set beforehand (through a roslaunch file for
#      instance).
robot = URDF.from_parameter_server()

# Print the robot
print(robot)

　　下面编写一个简单的robot.urdf文件，创建一个连杆机器人。joint1为与基座link0相连的基关节，joint3为末端关节。

<robot name="test_robot">
    <link name="link0" />
    <link name="link1" />
    <link name="link2" />
    <link name="link3" />

    <joint name="joint1" type="continuous">
        <parent link="link0"/>
        <child link="link1"/>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <axis xyz="1 0 0" />
    </joint>

    <joint name="joint2" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
        <origin xyz="0 0 1" rpy="0 0 0" />
        <axis xyz="1 0 0" />
    </joint>

    <joint name="joint3" type="continuous">
        <parent link="link2"/>
        <child link="link3"/>
        <origin xyz="0 0 1" rpy="0 0 0" />
        <axis xyz="1 0 0" />
    </joint>

</robot>

View Code