How to use KDL for inverse kinematics

asked 2020-03-14 09:27:52 -0600

musa.ihtiyar gravatar image

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.

edit retag flag offensive close merge delete