基于KDL的机器人运动学DH模型构建和求解

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
project(trac_ik_solver)
find_package(orocos_KDL REQUIRED)
find_package(Boost)
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})
#SET(ENV{PKG_CONFIG_PATH} /usr/lib/x86_64-linux-gnu/pkgconfig)
#find_package(PkgConfig)
#pkg_search_module(Boost REQUIRED)
include_directories(${orocos_kdl_INCLUDE_DIRS})
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
file(GLOB SRCS ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
add_executable(${PROJECT_NAME} "main.cpp" ${SRCS})
target_link_libraries(${PROJECT_NAME} ${orocos_kdl_LIBRARIES} nlopt  pthread ${Boost_LIBRARIES})

main.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 

#include 
#include 


#include 
#include 

#include 

using namespace std;

using namespace KDL;

Chain get_ur3_robot() {
    Chain ur3;
    ur3.addSegment(Segment("base"));
    ur3.addSegment(Segment("joint1", Joint(Joint::RotZ),
                           Frame::DH(1.85427703966640276e-05, M_PI / 2 + -0.000626175392884231741, 0.15190 + -5.56630992100959343e-05, -5.05343367106411137e-05)));
    ur3.addSegment(Segment("joint2", Joint(Joint::RotZ),
                           Frame::DH(-0.24365 + 0.000418280909411400392, -0.00167062908967277168, 8.16504944596539062, -0.0561242156763783057)));
    ur3.addSegment(Segment("joint3", Joint(Joint::RotZ),
                           Frame::DH(-0.21325 + 0.001565903694771692, 0.00403522262881641277, -14.4282499666997612, -0.0628157016125706208)));
    ur3.addSegment(Segment("joint4", Joint(Joint::RotZ),
                           Frame::DH(-9.66540899511680791e-06, M_PI / 2 - 0.000368945620575544808, 0.11235 + 6.26317959859598705, 0.118814438034714087)));
    ur3.addSegment(Segment("joint5", Joint(Joint::RotZ),
                           Frame::DH(-2.93410449126249217e-05, -M_PI / 2 + 0.000173987342825920877, 0.08535 + -3.9946104585394937e-05, 5.68822754944051158e-05)));
    ur3.addSegment(Segment("tcp", Joint(Joint::RotZ),
                           Frame::DH(0.0, 0.0, 0.08190 + 0.000367523159254548593, -1.45429512938645578e-05)));
    return ur3;
}

int main() {
//    Frame frame1 = Frame::DH(1.85427703966640276e-05, M_PI / 2 + - 0.000626175392884231741, 0.15190 + - 5.56630992100959343e-05, -5.05343367106411137e-05);
//    Eigen::Vector3d v3d1;
//    frame1.M.GetEulerZYX(v3d1[0], v3d1[1], v3d1[2]);
//    std::cout << v3d1 << "\n";
//    std::cout << frame1.p << "\n";
//    Frame frame2 = Frame::DH(-0.24365 + 0.000418280909411400392, -0.00167062908967277168, 8.16504944596539062, -0.0561242156763783057);
//    Eigen::Vector3d v3d2;
//    frame2.M.GetEulerZYX(v3d2[0], v3d2[1], v3d2[2]);
//    std::cout << v3d2 << "\n";
//    std::cout << frame2.p << "\n";
//    Frame frame3 = Frame::DH(-0.21325 + 0.001565903694771692, 0.00403522262881641277, -14.4282499666997612, -0.0628157016125706208);
//    Eigen::Vector3d v3d3;
//    frame3.M.GetEulerZYX(v3d3[0], v3d3[1], v3d3[2]);
//    std::cout << v3d3 << "\n";
//    std::cout << frame3.p << "\n";
//    Frame frame4 = Frame::DH(-9.66540899511680791e-06, M_PI / 2 - 0.000368945620575544808, 0.11235 + 6.26317959859598705, 0.118814438034714087);
//    Eigen::Vector3d v3d4;
//    frame4.M.GetEulerZYX(v3d4[0], v3d4[1], v3d4[2]);
//    std::cout << v3d4 << "\n";
//    std::cout << frame4.p << "\n";
//    Frame frame5 = Frame::DH(-2.93410449126249217e-05, -M_PI / 2 + 0.000173987342825920877, 0.08535 + -3.9946104585394937e-05, 5.68822754944051158e-05);
//    Eigen::Vector3d v3d5;
//    frame5.M.GetEulerZYX(v3d5[0], v3d5[1], v3d5[2]);
//    std::cout << v3d5 << "\n";
//    std::cout << frame5.p << "\n";
//    Frame tcp = Frame::DH(0.0, 0.0, 0.08190 + 0.000367523159254548593, -1.45429512938645578e-05);
//    Eigen::Vector3d v3tcp;
//    tcp.M.GetEulerZYX(v3tcp[0], v3tcp[1], v3tcp[2]);
//    std::cout << v3tcp << "\n";
//    std::cout << tcp.p << "\n";
    //
    //
    //
    //    Vector v1(1, 2, 3);
//    std::cout << v1 << "\n";
//    v1.ReverseSign();
//    std::cout << v1 << "\n";
    Chain ur3_robot = get_ur3_robot();
    ChainFkSolverPos_recursive fksolver(ur3_robot);
//    JntArray q_init(ur3_robot.getNrOfJoints());
    JntArray q(ur3_robot.getNrOfJoints());
    Eigen::VectorXd joint_angles(6);
    joint_angles << 0, 0, 0, 0, 0, 0;
//    joint_angles << 0.161429, -1.17692, 1.66764, -2.07981, -1.61873, 1.0;
//    std::cout << q_init.data.setRandom() << "\n";
//    q_init.data = q_init.data.setRandom() * M_PI;
//    std::cout << joint_angles << "\n";
//    joint_angles << 0.161429, -1.17692, 1.66764, -2.07981, -1.61873, 1.01775;
//    q.data = joint_angles;
//    std::cout << joint_angles << "\n";
    Frame T;
    fksolver.JntToCart(q, T);
    std::cout << T.p << "\n";
    Eigen::Vector3d zyx;
    T.M.GetEulerZYX(zyx[0], zyx[1], zyx[2]);
    std::cout << zyx.transpose() << "\n";
//    //trac_ik_solver
//    JntArray lower_joint_limits(ur3_robot.getNrOfJoints());
//    joint_angles << -M_PI, -M_PI, -M_PI, -M_PI, -M_PI, -M_PI;
//    lower_joint_limits.data = joint_angles;
//    JntArray upper_joint_limits(ur3_robot.getNrOfJoints());
//    joint_angles << M_PI, M_PI, M_PI, M_PI, M_PI, M_PI;
//    upper_joint_limits.data = joint_angles;
//    TRAC_IK::TRAC_IK ik_solver(ur3_robot, lower_joint_limits, upper_joint_limits, 0.005, 1e-10);
//    JntArray result(ur3_robot.getNrOfJoints());
//    if (ik_solver.CartToJnt(q_init, T, result) > 0) {
//        std::cout << result << "\n";
//    }
//    //
//    fksolver.JntToCart(result, T);
//    std::cout << T << "\n";
//    //
//    Eigen::Vector3d rpy;
//    T.M.GetRPY(rpy[0], rpy[1], rpy[2]);
//    Eigen::Vector3d zyx;
//    T.M.GetEulerZYX(zyx[0], zyx[1], zyx[2]);
//    ChainIkSolverPos_LMA iksolver(ur3_robot);
//    //kdl_ik_solver
//    iksolver.CartToJnt(q_init, T, result);
//    fksolver.JntToCart(result, T);
//    std::cout << result << "\n";
//    std::cout << T << "\n";
//    ChainIkSolverVel_pinv viksolver(ur3_robot);
    std::cout << T.p << "\n";
    std::cout << T.M << "\n";
    std::cout << T.M.GetRot() << "\n"; //get robot's axis-angle presentation format
    return 0;
}