Обратная кинематика KDL / TRAC дает плохие решения на Kinova JACO - PullRequest
Новая
       45

Обратная кинематика KDL / TRAC дает плохие решения на Kinova JACO

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

Я хочу протестировать решатели кинематики для KVL / TRAC на манипуляторе-манипуляторе Kinova JACO. Решения IK, предоставляемые этими решателями, плохие / неправильные.

Я в основном использую пример кода из https://bitbucket.org/traclabs/trac_ik/src/ce44bb2970766313e11b3bbb5a7e80cdc4cab931/trac_ik_examples/src/ik_tests.cpp?at=master&fileviewer=file-view-default.

Я читаю текущую позу и углы сочленения, предоставленные драйверами Kinova, по темам. 

  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, updating "current_pose" and "current_joints" in callbacks not shown here
  ros::spinOnce();

  // Current joints 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;

  // Current pose updated from Kinova publisher, convert msg to KDL frame
  // Target pose is simply +5cm in x,y,z
  KDL::Frame target_pose_frame;
  geometry_msgs::Pose target_pose = current_pose.pose;
  target_pose.position.x += 5;
  target_pose.position.y += 5;
  target_pose.position.z += 5;
  tf::PoseMsgToKDL(target_pose, target_pose_frame);

  ROS_INFO_STREAM("Current joints : \n" << current_joints_KDL.data);
  KDL::JntArray result(7);
  int rc;
//  rc = tracik_solver.CartToJnt(current_joints_KDL,target_pose_frame,result);
  rc = kdl_solver.CartToJnt(current_joints_KDL,target_pose_frame,result);
  ROS_INFO_STREAM("rc : " << rc << ". New joints : \n " << result.data);


  kinova_msgs::JointAngles result_joints;
  result_joints.joint1 = result(0);
  result_joints.joint2 = result(1);
  result_joints.joint3 = result(2);
  result_joints.joint4 = result(3);
  result_joints.joint5 = result(4);
  result_joints.joint6 = result(5);
  result_joints.joint7 = result(6);
  kinova_msgs::ArmJointAnglesActionGoal goal;
  goal.goal.angles = result_joints;

Я пробую решатели TRAC и KDL IK. TRAC не показывает никаких изменений в результирующих углах соединения. KDL показывает некоторые значения, но они неверны и приводят робота в совершенно неправильную позу. Возвращаемые значения обоих решателей равны -3, что заставляет меня думать, что IK не удался.

KDL: 

[ INFO] [1555593577.769035401]: Current joints : 
264.727
215.726
324.585
51.9345
228.792
236.658
343.908
[ INFO] [1555593577.769156741]: rc : -3 New joints : 
 246.756
5.46288
329.036
5.75959
229.634
5.14872
351.375

TRAC: 

[ INFO] [1555593507.264829949]: Current joints : 
264.727
215.438
324.529
52.0244
228.792
236.768
343.908
[ INFO] [1555593507.269980894]: rc : -3 New joints : 
 264.727
215.438
324.529
52.0244
228.792
236.768
343.908

Я не уверен, допустил ли я ошибку в том, как я формирую и передаю параметры, или в том, что решатели неправильно настроены в коде, или же решатели просто предоставляют плохие решения в моем случае. Я хотел бы посоветовать, как поступить.

...