Question

我使用Kinova JACO机械手，并希望出于测试目的而实现TRAC / KDL运动学求解器。当我摆姿势并通过KDL正向运动学时，与地面真实情况相比，我得到了错误的结果。

Kinova为我提供了关节角度以及末端执行器姿势。我采用了报告的给定关节角度并通过了向前运动学。结果计算出的末端执行器姿势与kinova驱动程序报告的姿势不匹配。我从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转换。


  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

应该相等但不相等。

KDL / TRAC正向运动学求解器在Kinova JACO上显示错误的解决方案

0 个答案: