ChainIkSolverPos_NR solver

[yuzhenyuan1227]

When the value of coordinate Y is changed, the six angles of the solution, joint 4 and joint 6, will mutate to 90°, but the predicted value is (0, 0, 0, 0, 0). as follow:

unsigned int n_joints = chain.getNrOfJoints();
KDL::JntArray q_init(n_joints);
for (unsigned int i = 0; i < n_joints; ++i)
{
    q_init(i) = 0.0;
}

If change the predicted values to (0,0,0,90,0,90), then X and Z are solved for the wrong angles.