kdl Inverse Kinematics Solver

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.

edit retag close merge delete

Sort by » oldest newest most voted

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?

more

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.

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!

( 2012-05-15 04:18:37 -0500 )edit
1

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

( 2012-05-15 08:48:25 -0500 )edit

Hi,

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.

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.

( 2012-05-15 04:14:44 -0500 )edit