Problem for align on Vector

[ProvendierCedric]

hi

How do you do?
i tried integrate KDL for aligned the last branch of the robots n one vector.
I not need follow the position just aligned on vector.
i have tried this but not works do you can help me please?

KDL::Chain chain;    

KDL::Vector axisX(1.0, 0.0, 0.0);
KDL::Vector axisY(0.0,1.0, 0.0);
KDL::Vector axisZ(0.0, 0.0, 1.0);
KDL::Vector axisQ(0.707107, 0.0, 0.707107);

Joint defineJointZ("axeC", KDL::Vector(0.0, 0.0, 0.0), axisZ, Joint::RotAxis);
Joint defineJointX("axeA", KDL::Vector(0.0, 0.0, 0.0), axisX, Joint::RotAxis);
Joint defineJointY("axeB", KDL::Vector(0.0, 0.0, 0.0), axisY, Joint::RotAxis);
Joint defineJointQ("axeA", KDL::Vector(0.0, 0.0, 0.0), axisQ, Joint::RotAxis);

chain.addSegment(Segment(Joint(Joint::TransX), Frame(Vector(0.0, 0.0, 0.0))));
chain.addSegment(Segment(Joint(Joint::TransY), Frame(Vector(0.0, 0.0, 0.0))));
chain.addSegment(Segment(Joint(Joint::TransZ), Frame(Vector(0.0, 0.0, 0.0))));

chain.addSegment(Segment("axeA", defineJointZ, Frame(Vector(0.0, 0.0, -1.0))));
chain.addSegment(Segment("axeC", defineJointQ, Frame(Vector(0.0, 0.0, -1.0))));
chain.addSegment(Segment("Tool", Joint(Joint::Fixed), Frame(Vector(0.0, 0.0, -1.0))));

//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain, fksolver1, iksolver1v, 10000, 1e-6);//Maximum 100 iterations, stop at accuracy 1e-6

//Creation of jntarrays:
JntArray q_out(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());
Rotation rot;
rot.DoRotX(M_PI /2);

double RX, RY, RZ;
rot.GetRPY(RX, RY, RZ);
printf("Rotation \n");
std::cout << (RX * 180) / M_PI << std::endl;
std::cout << (RY * 180) / M_PI  << std::endl;
std::cout << (RZ * 180) / M_PI << std::endl;
printf(" \n ");
Frame F_dest = Frame(rot); // point de destination

int ret = iksolver1.CartToJnt(q_init, F_dest, q_out);
for (unsigned int i = 0; i < chain.getNrOfJoints(); i++)
{
    std::string name = chain.getSegment(i).getName()+ " ";
    printf("joint : %i", i);
    printf("%s \n", "  ");
    std::cout << name;
    std::cout <<   (q_out(i) * 180) / M_PI << std::endl;
}

[MatthijsBurgh]

Can you tell me what isn't working? (Maybe provide some output)

[ProvendierCedric]

The result is axe A-45 and axe C90 but the result for this code is axe A = 180 and axe C= 0

[MatthijsBurgh]

Please try to provide (a lot) more context. And please form some solid sentences, so I don't need to guess what you mean.

[ProvendierCedric]

the goal it's calculate the rotations of axis for five axis Machine tool.
I need just aligned my tool with on a vector.
all rotation axis is on head of the machine.
the code that I you send not get the good result and i don't understand why.

thank you very much for your help.

[MatthijsBurgh]

Can you please run the code and provide me with the output.

[ProvendierCedric]

joint 0 , joint 1, joint2 it's the translations so it's not important for aligned the vector.
the result should be : axe A 180 axe C0 for aligne tool on rotation of 90 degrees around X.0 for aligne tool on rotation of 90 degrees around X.

[MatthijsBurgh]

Did you check the return value of the solver? If the solver didn't succeed, it is a good explanation for the incorrect results.

[ProvendierCedric]

Ok, the return value is -5 so the solver didn't succeed.
But do you know why the solver fail?
What wrong in my code?

[MatthijsBurgh]

-5 means the maximum number of iterations is reached.

See

orocos_kinematics_dynamics/orocos_kdl/src/solveri.hpp

Line 101 in 4f705fe

E_MAX_ITERATIONS_EXCEEDED = -5,

[ProvendierCedric]

ChainIkSolverPos_NR iksolver1(chain, fksolver1, iksolver1v, 10000, 1e-6);

I have 10000 iterations maximum.
I need more than 10000 ?
it's strange? no?

[MatthijsBurgh]

I would also think that I would not require that much iterations. Though I don't have the time to figure out if you are using the solvers the wrong way or that the solvers contain a bug. (The latter is also very unlikely as these solvers don't change that much and we have many tests.)

[ProvendierCedric]

It's possible I using the solver the wrong way....
but i don't see my error.