kdl Inverse Kinematics Solver

edit

Hi,

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.