IKFast求与当前状态最接近的解

IKFast求最优解

在做Kinova机械臂的时候，由于路径规划的需求，要用到IKFast逆运动学求解器。

但是根据程序，IKFast默认的求解结果是返回第一个在关节角范围内的解。
getPositionIK()函数：

bool obeys_limits = true;
for(unsigned int i = 0; i < sol.size(); i++)
{
     // Add tolerance to limit check
     if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
                                         (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
     {
         // One element of solution is not within limits
         obeys_limits = false;
         ROS_DEBUG_STREAM_NAMED("ikfast","Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << "  being  " << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
         break;
     }
}
if(obeys_limits)
{
    // All elements of solution obey limits
    getSolution(solutions,s,solution);
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
    return true;
}

由于在进行机械臂运动规划时，我们一般希望得到的是与上一个关节角最接近的解，所以我们需要对IKFast进行修改。

涉及的几个函数

1. solve()，返回逆解个数；
2. getSolution() ，用于获取所有逆解中的第i个逆解；
3. searchPositionIK()，给定需要求解的姿态pose，搜索其对应的逆解（关节角）；
4. getPositionIK() ，在所有solutions中选取需要的solution

实现过程

1. searchPositionIK()中，通过if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))进入到getPositionIK()函数中；
2. 定义一个容器用于存储满足关节角范围要求的所有逆解std::vector< std::vector > solutions_limits；
3. 由于我们需要找到一个与上一状态最接近的解，因此要定义一个old_solution用于存储上一个pose的逆解。
   这里有一点需要注意的！！
   old_solution应该是一个全局变量，但由于IKFast的程序中都是const函数，无法改变数据成员，因此我们对old_solution的定义应该是mutable std::vector old_solution，否则无法使用。
4. 然后将solutions_limits中所有解分别与old_solution做差，求出差值最小的那个作为我们的solution；
5. 最后需要对old_solution进行更新。

代码实现过程如下：
getPositionIK()：

    std::cout << "choice the solution START:" << std::endl;
    std::vector< std::vector > solutions_limits;
    int put_num = 0;
    for(int s = 0; s < numsol; ++s)
    {
      std::vector sol,sol_1;
      getSolution(solutions,s,sol);
      ROS_DEBUG_NAMED("ikfast","Sol %d: %e   %e   %e   %e   %e   %e", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5]);

      bool obeys_limits = true;
      for(unsigned int i = 0; i < sol.size(); i++)
      {
        if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
                                            (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
        {
          // One element of solution is not within limits
          obeys_limits = false;
          break;
        }
      }

      if(obeys_limits)
      {
        std::cout << "get the solutions in limits:" << std::endl;
        // All elements of solution obey limits
	      getSolution(solutions,s,sol_1);
	      std::cout << std::endl;

	      solutions_limits.push_back(sol_1);
	      put_num++;

        error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
      }
    }
    std::cout << "the number of solutions in limits:" << put_num << std::endl;

    bool has_solutions_in_limit_ = true;
    if(put_num == 0)
    {
      std::cout << "No solution within the limit" << std::endl;
      has_solutions_in_limit_ = false;
      return false;
    }

    double mindist = DBL_MAX;
    int closest = -1;

    if(has_solutions_in_limit_)
    {
      if(old_solution.empty())
        solution = solutions_limits[0];  //solutions_limits[0]
      else
      {
        //求每个solution_limits与前一个点的solution作绝对差，即找出关节角变化最小的solution
	      for(int p=0; p

结果：

后记

由于我用的是kinova的j2n6s300机械臂，且自己对关节角进行了限制。其/joint_state得到的关节角（0~2π）与IKFast逆解求得的关节角（-π～π）存在2π的差值，为了能够与关节角的范围（0.9π～2π）匹配，需要对IKFast求解的结果进行一个2π的加减，否则无法求解出结果。
其修改过程如下：
在ikfast.h中对GetSolution()函数进行修改

    virtual void GetSolution(T* solution, const T* freevalues) const {
        for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
            if( _vbasesol[i].freeind < 0 ){
                solution[i] = _vbasesol[i].foffset;
                
			    //ADD
                if( (solution[i] >= T(-3.14159265358979)) && (solution[i] <= 0)  ) {
                    solution[i] += T(6.28318530717959);
                }
            }
           …… ……
    }