一份可能的kdl求解franka机器人逆运动学代码

 #include <kdl/kdl.hpp>
 #include <kdl/chain.hpp>
 #include <kdl/tree.hpp>
 #include <kdl/segment.hpp>
 #include <kdl/chainfksolver.hpp>
 #include <kdl/chainiksolver.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl/chainiksolverpos_nr.hpp>
 #include <kdl/chainiksolverpos_nr_jl.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/frames_io.hpp>
 #include <kdl/frames.hpp>
 #include <urdf/model.h>


 JntArray joint_init_pose = JntArray(7);

 //kdl frankapanda 模型导入
     KDL::Tree my_tree;
     if (!kdl_parser::treeFromFile("/home/qiren/franka_ws/src/franka_ros/franka_catching_ball_controllers/urdf/panda.urdf", my_tree)){
       ROS_ERROR("Failed to construct kdl tree");
       return false;
     }
     else{
       cout<<"contruct kdl tree completed"<<endl;
     }

//sdk逆运动学求解
    bool exit_value;
    Chain chain;
    exit_value = my_tree.getChain("base","tool0",chain);
    double eps=1E-5;
    int maxiter=500;
    double eps_joints=1E-15;
    ChainIkSolverPos_LMA iksolver = ChainIkSolverPos_LMA(chain,eps,maxiter,eps_joints);
cout<<"1"<<endl;
    // unsigned int nj = chain.getNrOfJoints();
    // JntArray joint_init_pose = JntArray(nj);
    float roll,pitch,yaw;
    float cy = abs(X(5)/sqrt(pow(X(3),2)+pow(X(4),2)+pow(X(5),2)));
    float sy = abs(sqrt(pow(X(3),2)+pow(X(4),2))/sqrt(pow(X(3),2)+pow(X(4),2)+pow(X(5),2)));
    float cp = 1;
    float sp = 0;
    float cr = abs(X(4)/sqrt(pow(X(3),2)+pow(X(4),2)));
    float sr = abs(X(3)/sqrt(pow(X(3),2)+pow(X(4),2)));
    double rot0 = cy*cp;
    double rot1 = cy*sp*sr - sy*cr;
    double rot2 = cy*sp*cr + sy*sr;
    double rot3 = sy*cp;
    double rot4 = sy*sp*sr + cy*cr;
    double rot5 = sy*sp*cr - cy*sr;
    double rot6 = -sp;
    double rot7 = cp*sr;
    double rot8 = cp*cr;
    Rotation rot = Rotation(rot0,rot1,rot2,rot3,rot4,rot5,rot6,rot7,rot8);
    cout<<"rot"<<rot<<endl;

    Vector pose_vec = Vector(0.0057523,-0.188266,0.229913);
    Vector Rot_col_z(X(3),X(4),X(5));
    Rot_col_z.Normalize();
    Vector Rot_col_y(1,1,-(Rot_col_z(0)+Rot_col_z(1))/Rot_col_z(2));
    Rot_col_y.Normalize();
    Vector Rot_col_x;
    Rot_col_x = Rot_col_y*Rot_col_z;
    Rot_col_x.Normalize();
    Rotation rot_matrix(Rot_col_x,Rot_col_y,Rot_col_z);
cout<<rot_matrix<<endl;
Rotation rot_now = Rotation(1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0);


// 0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4
    joint_init_pose(0) = 0;
    joint_init_pose(1) = M_PI_4;
    joint_init_pose(2) = 0;
    joint_init_pose(3) = -3 * M_PI_4;
    joint_init_pose(4) = 0;
    joint_init_pose(5) = M_PI_2;
    joint_init_pose(6) = M_PI_4;
cout<<"joint_init_pose"<<joint_init_pose(0)<<endl;
cout<<"joint_init_pose"<<joint_init_pose(1)<<endl;

    Frame cartpos = Frame(rot_matrix,pose_vec);
    // Frame cartpos = Frame(rot_matrix,pose_vec);
    JntArray jointpositions = JntArray(7);
cout<<cartpos<<endl;
    bool kinematics_status;
    kinematics_status = iksolver.CartToJnt(joint_init_pose,cartpos,jointpositions);
cout<<jointpositions(0)<<endl;
    if(kinematics_status>=0){
        for(int i=0;i<7;i++){
          std::cout <<"jointpositions"<<i<<"     "<< jointpositions(i) << std::endl;
          catching_joint_pose.data.push_back(jointpositions(i));
std::cout <<"jointpositions"<<i<<"     "<< catching_joint_pose.data.at(i) << std::endl;
        }
        cout<<"Success"<<endl;
    }
    else{
      cout<<"error"<<endl;
    }