How to use KDL for inverse kinematics

edit

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.