从URDF到KDL(C++&Python)

从URDF到KDL(C++&Python)

毕竟我也是一个搞机器人的，今天就来写一些和机器人紧密相关的事情。

---

KDL 简介

PyKDL是一个神奇的库。里面包含了KDL库与vector，rotation, frame, kinematics, dynamics的相关函数和接口。计算机器人学中的运动学/动力学和坐标变换都是相当的彪悍。甚至可以加载urdf模型计算运动学，调用DH相关接口或者chain接口直接创建机器人模型。这个库主要是面向C++的，通过sig的形式做了相应的python接口。这里先讨论论c++中KDL的使用。
wiki tutorials
official site and git
python documentation
python kdl git

URDF模型

URDF模型是用于描述机器人运动学参数、动力学参数的配置文件。可以从多种来源获得，比如Solidworks可以直接保存出来。当然也可以自己编写，并不是很繁琐。
这篇教程清晰地写明了如何创建URDF模型。还是把最终的代码贴上来：

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

  <joint name="joint1" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 3 0" rpy="0 0 0" />
    <axis xyz="-0.9 0.15 0" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="-2 5 0" rpy="0 0 1.57" />
    <axis xyz="-0.707 0.707 0" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 -1.57" />
    <axis xyz="0.707 -0.707 0" />
  </joint>
</robot>

为了check模型的准确性，这篇教程还简单说明了如何使用urdfdom解析我们创建的URDF模型。还有urdfdom python教程
上述URDF模型代码对应的机构学模型如下：（串联、并联和混联机构都可以写出来）

ROS官方给出的下一篇教程是如何在C++中parse urdf模型。我们这里直接使用URDF模型，因此不过多探讨parse这篇博客。

KDL C++

关键点来了，对于机器人常见的运动学和动力学问题，如何使用这个高效的KDL库进行运算和求解呢？首先是正运动学的求解。

#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/tree.hpp>
#include <kdl/segment.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>

using namespace KDL;
using namespace std;
int main(int argc,char** argv){
    Tree my_tree;
    kdl_parser::treeFromFile("/home/file/catkin_ws/src/KDL_Test/src/ur3_robot.urdf",my_tree);

    bool exit_value;
    Chain chain;
    exit_value = my_tree.getChain("base","tool0",chain);
    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);

    unsigned int nj = chain.getNrOfJoints();
    JntArray jointpositions = JntArray(nj);

    for(unsigned int i=0;i<nj;i++){
        float myinput;
        printf("Enter the position of joint %i: ",i);
        scanf("%e",&myinput);
        jointpositions(i)=(double)myinput;
}

    Frame cartpos;

    bool kinematics_status;
    kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
    if(kinematics_status>=0){
        std::cout << cartpos << std::endl;
        printf("%s \n","Success, thanks KDL!");
    }
    else{
        printf("%s \n","Error:could not calculate forward kinematics : ");
    }
}

然后是逆运动学的求解。

#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/tree.hpp>
#include <kdl/segment.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/frames.hpp>
#include <stdio.h>
#include <iostream>

using namespace KDL;
using namespace std;
int main(int argc,char** argv){
    Tree my_tree;
    kdl_parser::treeFromFile("/home/file/catkin_ws/src/KDL_Test/src/ur3_robot.urdf",my_tree);

    bool exit_value;
    Chain chain;
    exit_value = my_tree.getChain("base","tool0",chain);
    double eps=1E-5;
    int maxiter=500;
    double eps_joints=1E-15;
    ChainIkSolverPos_LMA iksolver = ChainIkSolverPos_LMA(chain,eps,maxiter,eps_joints);

    unsigned int nj = chain.getNrOfJoints();
    JntArray jointGuesspositions = JntArray(nj);

    for(unsigned int i=0;i<nj;i++){
        float myinput;
        printf("Enter the initial guess position of joint %i: ",i);
        scanf("%e",&myinput);
        jointGuesspositions(i)=(double)myinput;
}

    double x,y,z;
    printf("Enter the x: ");
    scanf("%lf",&x);
    printf("Enter the y: ");
    scanf("%lf",&y);
    printf("Enter the z: ");
    scanf("%lf",&z);
    Vector vector = Vector(x,y,z);

    float roll,pitch,yaw;
    printf("Enter the roll: ");
    scanf("%e",&roll);
    printf("Enter the pitch: ");
    scanf("%e",&pitch);
    printf("Enter the yaw: ");
    scanf("%e",&yaw);
    float cy = cos(yaw);
    float sy = sin(yaw);
    float cp = cos(pitch);
    float sp = sin(pitch);
    float cr = cos(roll);
    float sr = sin(roll);
    double rot0 = cy*cp;
    double rot1 = cy*sp*sr - sy*cr;
    double rot2 = cy*sp*cr + sy*sr;
    double rot3 = sy*cp;
    double rot4 = sy*sp*sr + cy*cr;
    double rot5 = sy*sp*cr - cy*sr;
    double rot6 = -sp;
    double rot7 = cp*sr;
    double rot8 = cp*cr;
    Rotation rot = Rotation(rot0,rot1,rot2,rot3,rot4,rot5,rot6,rot7,rot8);

    Frame cartpos = Frame(rot,vector);
    JntArray jointpositions = JntArray(nj);

    bool kinematics_status;
    kinematics_status = iksolver.CartToJnt(jointGuesspositions,cartpos,jointpositions);
    if(kinematics_status>=0){
        for(int i=0;i<nj;i++){
            std::cout << jointpositions(i) << std::endl;
        }
        printf("%s \n","Success, thanks KDL!");
    }
    else{
        printf("%s \n","Error:could not calculate backword kinematics : ");
    }
}

这里需要注意的有三点：
1. 运动学正解输出的是4×4的坐标变换矩阵，如需变成位姿、四元数或者欧拉角需要进行额外的转换。
2. 运动学逆解的计算需要输入目标位姿和迭代角度初值，初值设置不要太离谱，不然就收敛不过去了，就好比深度学习权重初始值不能随机太夸张，否则就会not a number一样。
3. cmakefile里面各种库的地址要搞正确。

KDL Python

python版本我调研了两天。因为我的机器人主控框架是基于python写的，如果采用上述C++版本就需要把这个求解器搞成ros节点，每次调用的时候需要ros通信。事实上此前我都是这样做的，但是我觉得要是能变成python函数每次使用直接调用肯定方便很多，于是我就这样弄了。
首先是如何在python中解析URDF模型：

from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
robot = URDF.from_xml_file('/home/file/catkin_ws/src/KDL_Test/src/ur3_robot.urdf') # get a tree

然后在python中求解机构的运动学正逆解非常方便：

# KDL version 1
#!/usr/bin/env python
#-*- coding:utf-8 -*-
############################
#File Name: kdl_module.py
#Author: Wang
#Mail: wang19920419@hotmail.com
#Created Time:2017-09-09 10:51:39
############################

import sys
sys.path.insert(0, "/home/file/catkin_ws/src/Basic_math/hrl-kdl-indigo-devel/hrl_geom/src")
sys.path.insert(0, "/home/wangxu/catkin_ws/src/Basic_math/hrl-kdl-indigo-devel/pykdl_utils/src")

from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_kinematics import KDLKinematics
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model

robot = URDF.from_xml_file("/home/file/catkin_ws/src/KDL_Test/src/ur3_robot.urdf")
tree = kdl_tree_from_urdf_model(robot)
print tree.getNrOfSegments()
chain = tree.getChain("base", "tool0")
print chain.getNrOfJoints()
kdl_kin = KDLKinematics(robot, "base_link", "ee_link", tree)
q = [0,0,0,0,0,0]
pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
q_ik = kdl_kin.inverse(pose, [0.1,0.1,-0.1,0.1,0.1,0.1]) # inverse kinematics
if q_ik is not None:
    pose_sol = kdl_kin.forward(q_ik) # should equal pose
#J = kdl_kin.jacobian(q)
print 'q:', q
print 'q_ik:', q_ik
print 'pose:', pose
if q_ik is not None:
    print 'pose_sol:', pose_sol
#print 'J:', J

需要说明的：
1. 运动学正解输出的是4×4的坐标变换矩阵，如需变成位姿、四元数或者欧拉角需要进行额外的转换。
2. 运动学逆解的计算需要输入目标位姿和迭代角度初值，初值设置不要太离谱，不然就收敛不过去了，就好比深度学习权重初始值不能随机太夸张，否则就会not a number一样。
3. 如需其他开发，就要在这个基础上增加其他函数，见仁见智。
4. 无论是C++还是python，使用kdl都需要先将urdf parse成kdl能理解的模型。于是就有了两个语言的parser方法：
对应c++：
(http://docs.ros.org/api/kdl_parser/html/namespacekdl__parser.html)
(http://wiki.ros.org/kdl_parser/Tutorials/Start%20using%20the%20KDL%20parser)
对应于python：
(http://wiki.ros.org/pykdl_utils)

KDL是通用的运动学动力学库，对于复杂的运动学问题，比如并联机构的正解问题、串联机构的反解问题、冗余机构的无穷解问题都是利器。