Z's learning note

Search

SearchSearch

trac_ik

Dec 09, 2021, 6 min read

Trac-IK

概述

  • 安装 有两种配置方式，一种是独立安装（ros-$ROS_DISTRO-trac-ik），另一种是配置为MoveIt求解插件（ros-$ROS_DISTRO-trac-ik-kinematics-plugin）
  • 代码库
  • Python封装
  • Trac-IK的求解算法：

    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. Secondary constraints of distance and manipulability are also provided in order to receive back the “best” IK solution.

  • Trac-IK的MoveIt插件和Python求解器是相对的独立的：
    • MoveIt中的配置不会被应用到Python求解器上。
    • Python求解器初始化时可以自行配置允许位置偏差等参数（MoveIt插件无法配置允许位置偏差）。
    • Python求解器中的机器人配置信息来源于rospy.get_param('/robot_description')，在求解器实例化时已经完成，所以后续修改的机器人信息不会反应在求解器中（如重新配置关节限位）。
    • Python求解器不会考虑机器人自我碰撞以及与空间中物体碰撞的情况

MoveIt!的求解插件

  • 参考
  • 安装TRAC_IK
    • sudo apt-get install ros-\$ROS_DISTRO-trac-ik-kinematics-plugin
  • 在机器人的MoveIt配置中修改kinematics.yaml文件（如Yumi的catkin_ws/src/yumi/yumi_moveit_config）
  • 将kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 替换为 kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  • kinematics_solver_search_resolution：不可用

     The Cartesian error distance used to determine a valid solution is 1e-5, as that is what is hard-coded into MoveIt’s KDL plugin.

  • kinematics_solver_timeout：单位为秒
  • kinematics_solver_attempts：不可用，TRAC-IK在求解不能收敛时会自动重启
  • position_only_ik：支持，具体参考配置文件设置

优化求解的类型

  • 求解类型：共四种 Speed, Distance, Manipulation1, Manipulation2 （详情参考 trac_ik_lib ）。默认为 Speed。
    • Speed: 尽快得出第一个解
    • Distance: 运行至超时，返回从种子位置出发搜索得到的SSE最小的解（then returns the solution that minimizes SSE from the seed）
    • Manip1: 运行至超时，returns solution that maximizes (the product of the singular values of the Jacobian)
    • Manip2: 运行至超时，returns solution that minimizes the ratio of min to max singular values of the Jacobian.
  • 修改求解类型的方法：

    In the kinematics.yaml of your MoveIt cfg package, like so (manipulator is the group name here, change to whatever you have of course):

     
manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  solve_type: Distance
  • 查看当前配置/动态修改当前配置：rosparam rosparam get /robot_description_kinematics/left_arm/solve_type? /rviz_YOUR_PC_NAME_AND_SOME_SERIAL_NUMBER/left_arm/solve_type?

使用Python API对IK进行求解

安装

  • Install sudo apt-get install ros-\$ROS_DISTRO-trac-ik

使用步骤

  • 导入模块 
    from trac_ik_python.trac_ik import IK
  • 实例化IK求解器：TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps); 
    def __init__(self, base_link, tip_link,
            timeout=0.005, epsilon=1e-5, solve_type="Speed",
            urdf_string=None):
    chain_start, chain_end Python版本：ik_solver = IK("torso_lift_link", "r_wrist_roll_link") 注意给的参数是link名不是joint名！
    • 查看chain中包含的关节： 
      ik_solver = IK("yumi_body", "gripper_l_base")
ik_solver.joint_names
    • 求解IK 可以设置
    def get_ik(self, qinit,
            x, y, z,
            rx, ry, rz, rw,
            bx=1e-5, by=1e-5, bz=1e-5,
            brx=1e-3, bry=1e-3, brz=1e-3)
:param float bx: X allowed bound.
:param float by: Y allowed bound.
:param float bz: Z allowed bound.
:param float brx: rotation over X allowed bound.
:param float bry: rotation over Y allowed bound.
:param float brz: rotation over Z allowed bound.
      `ik_sol = ik_solver.get_ik(seed_state, x, y, z, qx, qy, qz, qw)`
  • 设置新的关节上下限： 
    from trac_ik_python.trac_ik import IK
 
ik_solver = IK("torso_lift_link",
               "r_wrist_roll_link")
 
lower_bound, upper_bound = ik_solver.get_joint_limits()
new_lower_bound = [0.0]* ik_solver.number_of_joints
# lower_bound: (-2.1353981494903564, -0.35359999537467957, -3.75, -2.121299982070923, -3.4028234663852886e+38, -2.0, -3.4028234663852886e+38) 
# upper_bound: (0.5646018385887146, 1.2963000535964966, 0.6499999761581421, -0.15000000596046448, 3.4028234663852886e+38, -0.10000000149011612, 3.4028234663852886e+38)
ik_solver.set_joint_limits(new_lower_bound, upper_bound)

错误排除

  • 在URDF中缺少base_link到world的连接，参考[[2.application/robotics/robot_software/ros/urdf#机械臂仿真时world与base_link的连接链|机械臂仿真时world与base_link的连接链]]

Backlinks