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

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中的定義基本是同樣的：

　　也能夠參考MATLAB Robotics System Toolbox中的對Rigid Body Tree Robot Model的描述：

  

 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

　　pykdl_utils中還提供了下列幾個指令用於測試分析咱們的機器人，若是ROS參數服務器中加載了/robot_description則命令行中的xml文件能夠省略：

rosrun pykdl_utils kdl_parser.py [robot.xml]
rosrun pykdl_utils kdl_kinematics.py [robot.xml]
rosrun pykdl_utils joint_kinematics.py [robot.xml]

　　對於咱們上面編寫的robot.urdf文件，能夠用下面命令進行測試：

rosrun pykdl_utils kdl_parser.py `rospack find test`/robot.urdf

　　下面是KDL運動學一些基本的用法，相關函數能夠參考：KDLKinematics Class Reference

#! /usr/bin/env python

# Import the module
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
from pykdl_utils.kdl_kinematics import KDLKinematics

robot = URDF.from_xml_file("/home/sc/catkin_ws/src/test/robot.urdf")

tree = kdl_tree_from_urdf_model(robot)
print tree.getNrOfSegments()

chain = tree.getChain("link0", "link3")
print chain.getNrOfJoints()

# forwawrd kinematics
kdl_kin = KDLKinematics(robot, "link0", "link3")
q = [0, 0, 0]
pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 matrix)
print pose 

q_ik = kdl_kin.inverse(pose) # inverse kinematics
print q_ik

if q_ik is not None:
    pose_sol = kdl_kin.forward(q_ik) # should equal pose
print pose_sol

J = kdl_kin.jacobian(q)
print 'J:', J

　　咱們將URDF文件轉換爲KDL tree之後能夠獲取機構運動鏈/樹的相關信息。KDLKinematics的構造函數根據urdf文件，以及機器人的基座base_link和末端end_link就能夠建立出運動鏈：

def pykdl_utils.kdl_kinematics.KDLKinematics.__init__    (self, urdf, base_link, end_link, kdl_tree = None)        

# Parameters:
# urdf    URDF object of robot.
# base_link    Name of the root link of the kinematic chain.
# end_link    Name of the end link of the kinematic chain.
# kdl_tree    Optional KDL.Tree object to use. If None, one will be generated from the URDF.

 　　正運動學的計算函數forward參數就是關節角度；逆運動學計算函數inverse的參數爲末端位姿矩陣，由於是數值解，還能夠指定初始值，以及關節角的範圍。

# Inverse kinematics for a given pose, returning the joint angles required to obtain the target pose.
def pykdl_utils.kdl_kinematics.KDLKinematics.inverse(self, pose, q_guess = None, min_joints = None, max_joints = None )  
      
# Parameters:
# pose    Pose-like object represeting the target pose of the end effector.
# q_guess    List of joint angles to seed the IK search.
# min_joints    List of joint angles to lower bound the angles on the IK search. If None, the safety limits are used.
# max_joints    List of joint angles to upper bound the angles on the IK search. If None, the safety limits are used.

 

 

 C++中建立KDL tree 

　　爲了使用KDL parser須要在package.xml中添加相關依賴項：

  <package>
    ...
    <build_depend package="kdl_parser" />
    ...
    <run_depend package="kdl_parser" />
    ...
  </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;
   }

　　2. From the parameter server

   KDL::Tree my_tree;
   ros::NodeHandle node;
   std::string robot_desc_string;
   node.param("robot_description", robot_desc_string, std::string());
   if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

　　3. From an xml element

   KDL::Tree my_tree;
   TiXmlDocument xml_doc;
   xml_doc.Parse(xml_string.c_str());
   xml_root = xml_doc.FirstChildElement("robot");
   if (!xml_root){
      ROS_ERROR("Failed to get robot from xml document");
      return false;
   }
   if (!kdl_parser::treeFromXml(xml_root, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

　　4. From a URDF model

   KDL::Tree my_tree;
   urdf::Model my_model;
   if (!my_model.initXml(....)){
      ROS_ERROR("Failed to parse urdf robot model");
      return false;
   }
   if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)){
      ROS_ERROR("Failed to construct kdl tree");
      return false;
   }

 

 

 

參考：

kdl_parser

urdfdom_py

Start using the KDL parser

從URDF到KDL(C++&Python)

PyKDL 1.0.99 documentation

Solidworks 2016中導出URDF文件

Robotics System Toolbox—Rigid Body Tree Robot Model