TRAC-IK机器人运动学求解器
发布日期:2021-09-07 21:17:17 浏览次数:2 分类:技术文章

本文共 8268 字,大约阅读时间需要 27 分钟。

   TRAC-IK和类似,也是一种基于数值解的机器人运动学求解器,但是在算法层面上进行了很多(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 ),。

  下面在Ubuntu16.04中安装TRAC-IK(之前已经安装过ROS Kinetic):

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

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

  参考修改package.xml以及CMakeLists.txt文件,添加TRAC-IK以及KDL的支持。编写一个简单的robot.urdf文件,joint1为与基座link0相连的基关节,joint3为末端关节:

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 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); % ORTRAC_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 
#include
#include
#include
#include
#include
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
=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;}
View Code

  test.launch文件如下:  

View Code

  使用catkin_make编译成功,并设置环境后,运行该程序

roslaunch ik_test test.launch

  通过键盘分别输入三个关节值:0,1.5708(90°),0  运动学正逆解计算结果如下图所示:

  

  接下来使用7自由的的Franka panda机器人进行正逆解计算测试。

  中包含Franka机器人的URDF文件,编写panda_test.launch,设置基关节为panda_link0,末端关节(法兰盘)为panda_link8。因此,正逆解计算都是以机器人法兰中心为基准。

View Code

  test.cpp中计算逆解前使用getKDLLimits函数得到机器人关节运动范围,并设定关节初始值在上下限的正中间。

#include "ros/ros.h"#include 
#include
#include
#include
#include
#include
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-6; 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::Distance); KDL::Chain chain; bool valid = ik_solver.getKDLChain(chain); if (!valid) { ROS_ERROR("There was no valid KDL chain found"); return -1; } KDL::JntArray ll, ul; //lower joint limits, upper joint limits valid = ik_solver.getKDLLimits(ll,ul); if (!valid) ROS_INFO("There were no valid KDL joint limits found"); // 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
=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); for (uint j=0; j
View Code

  为了验证TRAC-IK计算结果,使用libfranka中的echo_robot_state先输出机器人当前状态。是末端执行器(这里没有安装末端执行器,因此就是以法兰中心为基准)在机器人基坐标系中的位置姿态矩阵(按列优先存储),q为关节角度。

"O_T_EE": [0.722009,-0.690627,-0.0416898,0,-0.691463,-0.72236,-0.00866381,0,-0.0241316,0.0350823,-0.999093,0,0.371908,0.00215211,0.428567,1], "O_T_EE_d": [0.722007,-0.690631,-0.041642,0,-0.691467,-0.722355,-0.00871983,0,-0.0240581,0.0350898,-0.999095,0,0.371922,0.00215033,0.428596,1], "F_T_EE": [1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1], "EE_T_K": [1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1], "elbow": [-0.0168831,-1], "elbow_d": [-0.0168889,-1],"q": [0.0180582,-0.537152,-0.0168831,-2.60095,0.0297504,2.03967,0.753258], "dq": [-0.00051007,-0.000212418,0.00125443,0.000394549,-2.9871e-05,0.000278746,0.000261954],...
View Code

  开启测试程序,输入上面的关节值,输出如下:

  KDL正解计算的值与O_T_EE一致。根据Franka机器人的,可以写出从法兰到基座的变换矩阵:

  将正解输入与逆解输出的关节值分别代入变换矩阵中,可以发现理论计算与O_T_EE和KDL正解结果一致。

  由于7自由度机械臂有无数组逆解,因此一般要根据某种优化原则,选取其中一组。对于Franka机械臂,可使用TRAC-IK得到逆解后进行关节空间中的轨迹规划,实现类似于“movej”的功能。

 

 

参考:

转载于:https://www.cnblogs.com/21207-iHome/p/10564083.html

转载地址:https://blog.csdn.net/weixin_34304013/article/details/94562429 如侵犯您的版权,请留言回复原文章的地址,我们会给您删除此文章,给您带来不便请您谅解!

上一篇:[二分][DP]BZOJ 2298 HAOI problem a
下一篇:Spring学习(12)--- @Autowired与@Resource 对比

发表评论

最新留言

路过,博主的博客真漂亮。。
[***.116.15.85]2024年03月12日 16时22分25秒

关于作者

    喝酒易醉,品茶养心,人生如梦,品茶悟道,何以解忧?唯有杜康!
-- 愿君每日到此一游!

推荐文章

在mysql中删除表正确的是什么_在MySQL中删除表的操作教程 2019-04-21
mysql有3个共同好友_共同好友mysql 2019-04-21
代理查询 mysql_查询数据库代理设置 2019-04-21
mysql dif_mysqldiff实现MySQL数据表比较 2019-04-21
mysql 允许其他主机访问权限_允许其他主机访问本机MySQL 2019-04-21
druid不能close mysql连接_alibaba druid mysql连接问题 2019-04-21
mysql 设置按天分表_MySQL 优化实战记录 2019-04-21
java连接mysql 不推荐_java连接mysql 2019-04-21
mysql数据库 quota_shell脚本抓取用户存储quota写道mysql并展现到grafana面板 2019-04-21
idea测试连接mysql报错08001_IDEA连接MySQL错误 2019-04-21
layui导入模板数据_layui表格-template模板的三种用法 2019-04-21
mysql分组显示行号_mysql 显示行号,以及分组排序 2019-04-21
MySQL常见的主从复制架构_如何搭建经典的MySQL 主从复制架构 2019-04-21
编写python程序、计算账户余额_小明有20w存款存在余额宝中,按余额宝年收益为3.35%计算,用Python编写程序计算,多少年后小明的存款达到30w?... 2019-04-21
python 公众号引流_公众号引流方法有哪些? 2019-04-21
java 减少内存_java中减少内存占用小技巧 2019-04-21
centos 7 mysql图形界面_centos7-vnstat图形界面搭建 2019-04-21
java 防渗透_「java、工程师工作经验怎么写」-看准网 2019-04-21
java中跳出当前循环怎么做_在java中,如何跳出当前的多重循环? 2019-04-21
java程序中执行maven_java – 将一个enviornment变量传递给Maven中的已执行进程 2019-04-21