kdl介绍

kdl安装

正解和反解

正解

在一个系统中，已知关节角度，求解末端坐标位置

反解

在一个系统中，已知末端位置，求解关节角度

URDF模型

<?xml version="1.0" encoding="UTF-8" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5">

    <xacro:property name="pi" value="3.1415926"/>
    <xacro:property name="pi_2" value="${pi/2}"/>

    <xacro:property name="d1" value="0.089159"/>
    <xacro:property name="d4" value="0.10915"/>
    <xacro:property name="d5" value="0.09465"/>
    <xacro:property name="d6" value="0.0823"/>

    <xacro:property name="a2" value="-0.425"/>
    <xacro:property name="a3" value="-0.39225"/>

    <xacro:property name="alpha1" value="${pi_2}"/>
    <xacro:property name="alpha4" value="${pi_2}"/>
    <xacro:property name="alpha5" value="-${pi_2}"/>

    <xacro:macro name="HMLink" params="name xyz:='0 0 0' rpy:='0 0 0'">
        <link name="${name}">
            <visual>
                <origin xyz="${xyz}" rpy="${rpy}"/>
                <geometry>
                    <mesh filename="package://demo_urdf/meshes/ur5/visual/${name}.dae"/>
                </geometry>
            </visual>
        </link>
    </xacro:macro>

    <xacro:macro name="HMJoint" params="name parent child xyz:='0 0 0' rpy:='0 0 0'">
        <joint name="${name}" type="revolute">
            <parent link="${parent}"/>
            <child link="${child}"/>
            <origin xyz="${xyz}" rpy="${rpy}"/>
            <axis xyz="0 0 1"/>

            <limit effort="30" velocity="1.0" lower="-3.1415926" upper="3.1415926"/>
        </joint>
    </xacro:macro>

    <xacro:HMLink name="base" xyz="0 0 0.004" rpy="0 0 ${pi_2}"/>

    <xacro:HMLink name="shoulder" rpy="0 0 ${pi}"/>

    <xacro:HMJoint name="joint1"
                   parent="base"
                   child="shoulder"
                   xyz="0 0 ${d1}"/>

    <xacro:HMLink name="upperarm" xyz="0 0 0.136" rpy="${pi_2} 0 -${pi_2}"/>

    <xacro:HMJoint name="joint2"
                   parent="shoulder"
                   child="upperarm"
                   rpy="${alpha1} 0 0"/>

    <xacro:HMLink name="forearm" xyz="0 0 0.016" rpy="${pi_2} 0 -${pi_2}"/>

    <xacro:HMJoint name="joint3"
                   parent="upperarm"
                   child="forearm"
                   xyz="${a2} 0 0"/>

    <xacro:HMLink name="wrist1" xyz="0 0 -0.09315" rpy="${pi_2} 0 0"/>
    <xacro:HMJoint name="joint4"
                   parent="forearm"
                   child="wrist1"
                   xyz="${a3} 0 ${d4}"/>

    <xacro:HMLink name="wrist2" xyz="0 0 -0.09625"/>
    <xacro:HMJoint name="joint5"
                   parent="wrist1"
                   child="wrist2"
                   xyz="0 -${d5} 0"
                   rpy="${alpha4} 0 0"/>

    <xacro:HMLink name="wrist3" xyz="0 0.0015 -0.082" rpy="${pi_2} 0 0"/>
    <xacro:HMJoint name="joint6"
                   parent="wrist2"
                   child="wrist3"
                   xyz="0 ${d6} 0"
                   rpy="${alpha5} 0 0"/>
</robot>

kdl库使用

开发环境

sudo apt install liborocos-kdl-dev
sudo apt install libkdl-parser-dev

Note

kdl是kinematic dynamic library缩写，提供正反解功能。 kdl-parser是kdl的解析工具，可以将urdf解析成kdl对应的对象。

工程配置

find_package(orocos_kdl)
find_package(kdl_parser)

include_directories(
        ${kdl_parser_INCLUDE_DIRS}
        ${orocos_kdl_INCLUDE_DIRS}
)
......
target_link_libraries(
        ...
        ${orocos_kdl_LIBRARIES}
        ${kdl_parser_LIBRARIES}
)

模型加载

#include <kdl/chain.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>

......

KDL::Tree tree;
kdl_parser::treeFromFile("../ur5.xacro", tree);

KDL::Chain chain;
tree.getChain("base", "wrist3", chain);

Note

通过kdl_parser::treeFromFile去加载urdf文件，将加载的结果转化为KDL::Tree

通过KDL::Tree中，去活动KDL::Chain,需要指定起始和结束的连杆

KDL::Chain相当于urdf的对象映射

正解

#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

......

KDL::ChainFkSolverPos_recursive solver(chain);

unsigned int num = chain.getNrOfJoints();
KDL::JntArray in(num);
in(0) = KDL::deg2rad * 15;
in(1) = KDL::deg2rad * 15;
in(2) = KDL::deg2rad * 15;
in(3) = KDL::deg2rad * 15;
in(4) = KDL::deg2rad * 15;
in(5) = KDL::deg2rad * 15;

KDL::Frame out;

int state = solver.JntToCart(in, out);
if (state >= 0) {
    //有解
    // 位置 (x,y,z)
    cout << "位置信息: " << endl;
    cout << out.p << endl;
    // 姿态 euler(roll, pitch, yaw) , 四元素，旋转矩阵
    // matrix 旋转矩阵
      cout << p_out.M << endl;
    double roll;
    double pitch;
    double yaw;
    out.M.GetRPY(roll, pitch, yaw);
    cout << "rpy(" << roll * KDL::rad2deg << ", " << pitch * KDL::rad2deg << ", " << yaw * KDL::rad2deg << ") ";
}

!!!note
    `KDL::JntArray`表示的是关节角

    `KDL::Frame`表示的是位姿

    正解就是已知关节角，求位姿。

### 反解
```cpp
KDL::ChainFkSolverPos_recursive fk(chain);
KDL::ChainIkSolverVel_pinv ik(chain);
KDL::ChainIkSolverPos_NR solver(chain, fk, ik);

unsigned int num = chain.getNrOfJoints();
KDL::JntArray in(num);
in(0) = KDL::deg2rad * 30;
in(1) = KDL::deg2rad * -10;
in(2) = KDL::deg2rad * 10;
in(3) = KDL::deg2rad * 15;
in(4) = KDL::deg2rad * 10;
in(5) = KDL::deg2rad * 10;

double roll = 178.57;
double pitch = 1.43;
double yaw = -179.97;
double x = -0.12009;
double y = -0.431804;
double z = 0.14606;

KDL::Frame frame(KDL::Rotation::EulerZYX(yaw * KDL::deg2rad, pitch * KDL::deg2rad, roll * KDL::deg2rad),
                 KDL::Vector(x, y, z));
KDL::JntArray out;

int state = solver.CartToJnt(in, frame, out);
if (state) {
    unsigned int columns = out.columns();
    unsigned int rows = out.rows();
    for (int i = 0; i < columns; ++i) {
        for (int j = 0; j < rows; ++j) {
            // double &d = out.data.coeffRef(j, i);
            // std::cout << d * KDL::rad2deg << " ";
            std::cout << out.data.row(j).col(i) * KDL::rad2deg << " ";
        }
        std::cout << std::endl;
    }
} else {
    std::cout << "无解" << std::endl;
}