ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

kdl Inverse Kinematics Solver

asked 2012-05-14 02:05:09 -0600

SLAMnect gravatar image


We are trying to solve inverse kinematics with the kdl lib, which is included in ROS. We followed the instruction from the homepage of kdl and initialized the solvers. This working very well, but:

When we try to parse values for the solvers we get strange values back. Here the code which is not working:

robot_tree.getChain("base_link", "left_forearm", left_arm_chain
KDL::ChainFkSolverPos_recursive fk_solver(left_arm_chain);
    KDL::ChainIkSolverVel_pinv ik_solver_vel(left_arm_chain);
    KDL::ChainIkSolverPos_NR ik_solver(left_arm_chain, fk_solver,
            ik_solver_vel, 1000, 100); //max 100 iterations and stop by an accuracy of 1e-6

    KDL::JntArray q_init(left_arm_chain.getNrOfJoints());
    KDL::JntArray q_out(left_arm_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(0.0, 5.0, 0.0));

    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(1, j) * 180 / 3.14 << "  ";
        std::cout << std::endl;

This line is not working:

ik_solver.CartToJnt(q_init, dest_frame, q_out)

The funktion gives back a value smaller than 0 which implies that something went wrong.

any advice? thanks so far.

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted

answered 2012-05-14 04:23:45 -0600

Miguel Prada gravatar image

At first glance the code seems fine to me. However there's several things that might be going wrong.

1) Are you sure that the position you are requesting is reachable by the robot? 5 meters seems quite far for any reasonable robot arm...

2) Depending on your chain configuration, the initial joint position given to the iteration and the desired cartesian position, it may happen that you are having problems with getting stuck in a singular configuration. If the iterative method is trying to move _exactly_ in a singular direction then it will never leave the initial seed state (all joints equal to 0). Could you try to change the initial position and/or the desired position slightly (e.g. all joints to 0.1) and see if this changes anything?

edit flag offensive delete link more

answered 2012-05-15 04:01:10 -0600

If you have problems with the KDL solver, it might be worth to try the closed form IK solver 'ikfast' which is part of OpenRAVE. A short description of how one can import a URDF model to generate the IK can be found here.

edit flag offensive delete link more


I didn't know about this solver but it looks very promising. I'll have a look as soon as I can. Thanks for pointing it out!

Miguel Prada gravatar image Miguel Prada  ( 2012-05-15 04:18:37 -0600 )edit

Another vote for ikfast from OpenRAVE, it works great for our custom 7DOF arm, definitely should give it a try.

arebgun gravatar image arebgun  ( 2012-05-15 08:48:25 -0600 )edit

answered 2012-05-15 03:47:21 -0600

SLAMnect gravatar image


thanks for the answer. I tried the suggestions you made and in some cases I get a resonable answer from the solver. It seems that, against my thoughts, the solver is not able to solve a projection of the goal to the workspace.

Thanks for the quick response. Problem solved.

edit flag offensive delete link more


In the general case no, it won't solve for a projection of the goal to the workspace. It might happen on some rare ocasions but you souldn't count on it.

Miguel Prada gravatar image Miguel Prada  ( 2012-05-15 04:14:44 -0600 )edit

Question Tools


Asked: 2012-05-14 02:05:09 -0600

Seen: 4,340 times

Last updated: May 15 '12