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 <iostream>
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolvervel_pinv_givens.hpp>
#include <kdl/chainiksolvervel_pinv_nso.hpp>
#include <kdl/chainiksolvervel_pinv_nso.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/chainjnttojacdotsolver.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chainfdsolver_recursive_newton_euler.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/kinfam_io.hpp>
#include <trac_ik/trac_ik.hpp>
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;
}