标签：it! lib move name png struct roc sign rpo
TRAC-IK和Orocos KDL类似，也是一种基于数值解的机器人运动学求解器，但是在算法层面上进行了很多改进，相比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>
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, string URDF_param="/robot_description", double timeout_in_secs=0.005, double error=1e-5, TRAC_IK::SolveType type=TRAC_IK::Speed); % NOTE: The last arguments to the constructors are optional. % The type can be one of the following: % Speed: returns very quickly the first solution found % Distance: runs for the full timeout_in_secs, then returns the solution that minimizes SSE from the seed % Manip1: runs for full timeout, returns solution that maximizes sqrt(det(J*J^T)) % Manip2: runs for full timeout, returns solution that minimizes cond(J) = |J|*|J^-1| int rc = ik_solver.CartToJnt(KDL::JntArray joint_seed, KDL::Frame desired_end_effector_pose, KDL::JntArray& return_joints, KDL::Twist tolerances); % NOTE: CartToJnt succeeded if rc >=0 % NOTE: tolerances on the end effector pose are optional, and if not % provided, then by default are 0. If given, the ABS() of the % values will be used to set tolerances at -tol..0..+tol for each of % the 6 Cartesian dimensions of the end effector pose.
下面是一个简单的测试程序，先通过KDL计算正解，然后使用TRAC-IK反算逆解：
#include "ros/ros.h" #include <trac_ik/trac_ik.hpp> #include <kdl/chainiksolverpos_nr_jl.hpp> #include <kdl/chain.hpp> #include <kdl/chainfksolver.hpp> #include <kdl/chainfksolverpos_recursive.hpp> #include <kdl/frames_io.hpp> using namespace KDL; int main(int argc, char **argv) { ros::init(argc, argv, "ik_test"); ros::NodeHandle nh("~"); int num_samples; std::string chain_start, chain_end, urdf_param; double timeout; const double error = 1e-5; nh.param("chain_start", chain_start, std::string("")); nh.param("chain_end", chain_end, std::string("")); if (chain_start=="" || chain_end=="") { ROS_FATAL("Missing chain info in launch file"); exit (-1); } nh.param("timeout", timeout, 0.005); nh.param("urdf_param", urdf_param, std::string("/robot_description")); if (num_samples < 1) num_samples = 1; TRAC_IK::TRAC_IK ik_solver(chain_start, chain_end, urdf_param, timeout, error, TRAC_IK::Speed); KDL::Chain chain; bool valid = ik_solver.getKDLChain(chain); if (!valid) { ROS_ERROR("There was no valid KDL chain found"); return -1; } // Set up KDL IK KDL::ChainFkSolverPos_recursive fk_solver(chain); // Forward kin. solver based on kinematic chain // Create joint array unsigned int nj = chain.getNrOfJoints(); ROS_INFO ("Using %d joints",nj); KDL::JntArray jointpositions = JntArray(nj); // Assign some values to the joint positions 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; } // Create the frame that will contain the results KDL::Frame cartpos; // Calculate forward position kinematics bool kinematics_status; kinematics_status = fk_solver.JntToCart(jointpositions,cartpos); Vector p = cartpos.p; // Origin of the Frame Rotation M = cartpos.M; // Orientation of the Frame double roll, pitch, yaw; M.GetRPY(roll,pitch,yaw); if(kinematics_status>=0){ printf("%s \n","KDL FK Succes"); std::cout <<"Origin: " << p(0) << "," << p(1) << "," << p(2) << std::endl; std::cout <<"RPY: " << roll << "," << pitch << "," << yaw << std::endl; }else{ printf("%s \n","Error: could not calculate forward kinematics :("); } KDL::JntArray joint_seed(nj); KDL::SetToZero(joint_seed); KDL::JntArray result(joint_seed); int rc=ik_solver.CartToJnt(joint_seed,cartpos,result); if(rc < 0) printf("%s \n","Error: could not calculate forward kinematics :("); else{ printf("%s \n","TRAC IK Succes"); for(unsigned int i = 0; i < nj; i++) std::cout << result(i) << " "; } return 0; }
test.launch文件如下：
<?xml version="1.0"?> <launch> <arg name="chain_start" default="link0" /> <arg name="chain_end" default="link3" /> <arg name="timeout" default="0.005" /> <param name="robot_description" textfile="$(find ik_test)/urdf/robot.urdf" /> <node name="ik_test" pkg="ik_test" type="ik_test" output="screen"> <param name="chain_start" value="$(arg chain_start)"/> <param name="chain_end" value="$(arg chain_end)"/> <param name="timeout" value="$(arg timeout)"/> <param name="urdf_param" value="/robot_description"/> </node> </launch>
使用catkin_make编译成功，并设置环境后，运行该程序
roslaunch ik_test test.launch
通过键盘分别输入三个关节值：0，1.5708（90°），0 运动学正逆解计算结果如下图所示：
参考：
API reference of the Kinematics and Dynamics Library
orocos_kdl学习（二）：KDL Tree与机器人运动学
标签：it! lib move name png struct roc sign rpo
原文地址：https://www.cnblogs.com/21207-iHome/p/10564083.html
