Question

我希望在Kinova JACO机械手手臂上测试KDL / TRAC invserse运动学求解器。这些求解器提供的IK解决方案是错误的/错误的。

我主要使用https://bitbucket.org/traclabs/trac_ik/src/ce44bb2970766313e11b3bbb5a7e80cdc4cab931/trac_ik_examples/src/ik_tests.cpp?at=master&fileviewer=file-view-default中的示例代码。

我阅读了基诺瓦驾驶员提供的有关主题的当前姿势和关节角度。

  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

我不确定我在形成和传递参数的方式方面是否犯了错误，或者不确定求解器是否在代码中正确设置，或者不确定求解器在我的情况下是否提供了错误的解决方案。我想建议如何进行。

KDL / TRAC逆运动学给Kinova JACO带来了不好的解决方案

0 个答案: