How to use KDL for inverse kinematics
Hi I'm trying to use KDL for inverse kinematics in ros, but I couldn't understand at which point I have a mistake. Code is like the following. Firstly the method to describe joints of UR10 robot.
KDL::Chain initChainUR10() {
KDL::Chain chain;
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame(KDL::Rotation::RPY(0,-KDL::PI/2,0), KDL::Vector( -0.09265011549, 0, 0.0833499655128 ))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame(KDL::Rotation::RPY(0,0,0), KDL::Vector( 0.612088978291, 0, 0.0196119993925 ))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame(KDL::Rotation::RPY(0,0,0), KDL::Vector( 0.572199583054, 0, -0.00660470128059 ))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame(KDL::Rotation::RPY(0,KDL::PI/2,0), KDL::Vector( 0.0572912693024, 0,0.0584142804146 ))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame(KDL::Rotation::RPY(0,-KDL::PI/2,0), KDL::Vector( -0.0573025941849, 0,0.0583996772766))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame(KDL::Rotation::RPY(0,0,0), KDL::Vector( 0, 0, 0.110513627529 ))));
return chain;}`
Then in the main I'm saying
KDL::Chain chain = initChainUR10();
unsigned int nj = chain.getNrOfJoints();
while(ros::ok()) {
KDL::ChainFkSolverPos_recursive fk_solver(chain);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain);
KDL::ChainIkSolverPos_NR ik_solver(chain, fk_solver,
ik_solver_vel, 1000, 100); //max 100 iterations and stop by an accuracy of 1e-6
KDL::JntArray q_init(chain.getNrOfJoints());
KDL::JntArray q_out(chain.getNrOfJoints());
/*q_init(1,0) = 0.0;
q_init(1,1) = 0.0;
q_init(1,2) = 0.0;*/
KDL::Frame dest_frame(KDL::Vector(xf, yf, zf));
if (ik_solver.CartToJnt(q_init, dest_frame, q_out) < 0) {
ROS_ERROR( "Something really bad happened. You are in trouble now");
return -1;
} else {
// parse output of ik_solver to the robot
for (unsigned int j = 0; j < q_out.rows(); j++) {
std::cout << q_out(j, 1) * 180 / 3.14 << " ";
std::cout << q_out(j, 1) << " ";
std_msgs::Float32 jointMessage;
jointMessage.data = q_out(j, 1) * 180 / 3.14 ;
joints[j].publish(jointMessage);
}
std::cout << std::endl;
}
ros::spinOnce();
loop.sleep();
}
In fact, I couldn't understand how CartToJnt method works. How will I learn the values I need to give to joints after calling this function in order to make robot's end effector to go to (xf, yf, zf) with respect to robot frame (UR10 base) ? Also will I multiply the value with 180 / 3.14 or not? Lastly, when I call the function with different xf, yf, zf the results change seriously only if yf changes, which looks strange for me. What can be the reason of it? Thanks.