我希望在Kinova JACO机械手手臂上测试KDL / TRAC invserse运动学求解器。这些求解器提供的IK解决方案是错误的/错误的。
我阅读了基诺瓦驾驶员提供的有关主题的当前姿势和关节角度。
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
我不确定我在形成和传递参数的方式方面是否犯了错误,或者不确定求解器是否在代码中正确设置,或者不确定求解器在我的情况下是否提供了错误的解决方案。我想建议如何进行。