orocod_kdl学习(二):KDL Tree与机器人运动学

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

orocod_kdl学习(二):KDL Tree与机器人运动学_第1张图片

  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

orocod_kdl学习(二):KDL Tree与机器人运动学_第2张图片

KDL segment

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

orocod_kdl学习(二):KDL Tree与机器人运动学_第3张图片

KDL chain

orocod_kdl学习(二):KDL Tree与机器人运动学_第4张图片

KDL tree

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

orocod_kdl学习(二):KDL Tree与机器人运动学_第5张图片

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

orocod_kdl学习(二):KDL Tree与机器人运动学_第6张图片

  

 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("")

# - 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

orocod_kdl学习(二):KDL Tree与机器人运动学_第7张图片

  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 

  下面有多种从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

你可能感兴趣的:(orocod_kdl学习(二):KDL Tree与机器人运动学)