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;
}