TRAC-IK机器人运动学求解器

 　　TRAC-IK和Orocos KDL类似，也是一种基于数值解的机器人运动学求解器，但是在算法层面上进行了很多改进(Specifically, KDL’s convergence algorithms are based on Newton’s method, which does not work well in the presence of joint limits — common for many robotic platforms. TRAC-IK concurrently runs two IK implementations. One is a simple extension to KDL’s Newton-based convergence algorithm that detects and mitigates local minima due to joint limits by random jumps. The second is an SQP (Sequential Quadratic Programming) nonlinear optimization approach which uses quasi-Newton methods that better handle joint limits. By default, the IK search returns immediately when either of these algorithms converges to an answer )，相比KDL求解效率（成功率和计算时间）高了很多。

　　下面在Ubuntu16.04中安装TRAC-IK（之前已经安装过ROS Kinetic）：

sudo apt-get install ros-kinetic-trac-ik

　　按照ROS教程新建一个名为ik_test的Package，并创建urdf文件夹用于存放机器人URDF描述文件，创建launch文件夹存放launch文件：

　　参考trac_ik_examples修改package.xml以及CMakeLists.txt文件，添加TRAC-IK以及KDL的支持。编写一个简单的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

　　TRAC-IK求解机器人逆运动学函数为CartToJnt：

int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& bounds=KDL::Twist::Zero());

　　第一个参数q_init为关节的初始值，p_in为输入的末端Frame，q_out为求解输出的关节值。基本用法如下：

#include <trac_ik/trac_ik.hpp>

TRAC_IK::TRAC_IK ik_solver(KDL::Chain chain, KDL::JntArray lower_joint_limits, KDL::JntArray upper_joint_limits, double timeout_in_secs=0.005, double error=1e-5, TRAC_IK::SolveType type=TRAC_IK::Speed);  

% OR

TRAC_IK::TRAC_IK ik_solver(string base_link, string tip_link,