Решатели прямого кинематического анализа KDL / TRAC показывают ложные решения на Kinova JACO - PullRequest
Новая
       41

Решатели прямого кинематического анализа KDL / TRAC показывают ложные решения на Kinova JACO

0 голосов
/ 18 апреля 2019

Я использую манипулятор Kinova JACO и хочу внедрить решатели TRAC / KDL kinmetaics для тестирования.Когда я принимаю позу и прохожу вперед кинематику KDL, я получаю неправильный результат по сравнению с основной истиной.

Кинова предоставляет мне углы суставов, а также позу конечного эффектора.Я беру заданные заданные углы сочленения и бегу по прямой кинематике.Полученная расчетная поза конечного эффектора не соответствует позе, сообщаемой водителями киновой системы.Я беру большую часть кода из примера trac_ik https://bitbucket.org/traclabs/trac_ik/src/ce44bb2970766313e11b3bbb5a7e80cdc4cab931/trac_ik_examples/src/ik_tests.cpp?at=master&fileviewer=file-view-default
и использую преобразования tf в kdl, с которыми столкнулся во время поиска в Google. 


  std::string chain_start = "j2s7s300_link_base", chain_end = "j2s7s300_end_effector", urdf_param = "/robot_description";
  double timeout = 0.005;
  double eps = 1e-5;

  KDL::Chain chain;
  KDL::JntArray ll, ul; //lower joint limits, upper joint limits

  TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps);
  tracik_solver.getKDLChain(chain);
  tracik_solver.getKDLLimits(ll,ul);

  // Set up KDL & TRAC IK
  KDL::ChainFkSolverPos_recursive fk_solver(chain); // Forward kin. solver
  KDL::ChainIkSolverVel_pinv vik_solver(chain); // PseudoInverse vel solver
  KDL::ChainIkSolverPos_NR_JL kdl_solver(chain,ll,ul,fk_solver, vik_solver, 1, eps); // Joint Limit Solver

  // Spin to grab latest pose and joint angles from callbacks not shown here
  ros::spinOnce();

  // "current_pose" value updated from Kinova publisher
  KDL::Frame current_pose_frame;
  //convert msg to KDL frame
  tf::PoseMsgToKDL(current_pose.pose, current_pose_frame);

  // "current_joints" value updated from Kinova publisher, convert msg to KDL Joint Array
  KDL::JntArray current_joints_KDL(7);
  current_joints_KDL(0)=current_joints.joint1;
  current_joints_KDL(1)=current_joints.joint2;
  current_joints_KDL(2)=current_joints.joint3;
  current_joints_KDL(3)=current_joints.joint4;
  current_joints_KDL(4)=current_joints.joint5;
  current_joints_KDL(5)=current_joints.joint6;
  current_joints_KDL(6)=current_joints.joint7;

  // Forward IK for current pose as per KDL solver
  KDL::Frame recalculated_current_pose_frame;
  fk_solver.JntToCart(current_joints_KDL,recalculated_current_pose_frame);
  // Convert resulting KDL frame to Pose msg
  geometry_msgs::Pose recalculated_current_pose;
  tf::PoseKDLToMsg(recalculated_current_pose_frame,recalculated_current_pose);

  ROS_INFO_STREAM("Actual current pose : \n"
                  << current_pose.pose.position.x <<  " "
                  << current_pose.pose.position.y <<  " "
                  << current_pose.pose.position.z <<  " "
                  << current_pose.pose.orientation.x <<  " "
                  << current_pose.pose.orientation.y <<  " "
                  << current_pose.pose.orientation.z <<  " "
                  << current_pose.pose.orientation.w <<  " "
                  );

  ROS_INFO_STREAM("Recalculated current pose : \n"
                  << recalculated_current_pose.position.x <<  " "
                  << recalculated_current_pose.position.y <<  " "
                  << recalculated_current_pose.position.z <<  " "
                  << recalculated_current_pose.orientation.x <<  " "
                  << recalculated_current_pose.orientation.y <<  " "
                  << recalculated_current_pose.orientation.z <<  " "
                  << recalculated_current_pose.orientation.w <<  " "
                  );

Вывод, который я вижу: 

[ INFO] [1555590522.126735452]: Actual current pose : 
0.212193 -0.264992 0.5042 0.646721 0.3177 0.422695 0.549679 
[ INFO] [1555590522.126794601]: Recalculated current pose : 
-0.209002 0.0495282 -0.0308032 0.843738 0.288675 -0.0823099 -0.444971

, которые должны быть равны, но не равны.

...