diff --git a/CMakeLists.txt b/CMakeLists.txt index 43508de..f851779 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,6 +62,7 @@ set( source/Elite/RemoteUpgrade.cpp source/Elite/ControllerLog.cpp source/Elite/SerialCommunication.cpp + source/Kinematics/kinematics_base.cpp ) set( @@ -73,6 +74,7 @@ set( ${PROJECT_SOURCE_DIR}/include/Dashboard ${PROJECT_SOURCE_DIR}/include/Elite ${PROJECT_SOURCE_DIR}/include/Control + ${PROJECT_SOURCE_DIR}/include/Kinematics ${PROJECT_SOURCE_DIR}/include/ ${PROJECT_BINARY_DIR}/include/ ) diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 2b9b19d..a56636d 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -35,5 +35,5 @@ foreach(SOURCE ${SOURCES}) ${Boost_INCLUDE_DIRS} ) endforeach() - - +//添加kinematics_example子目录 +add_subdirectory(kinematics_example ${CMAKE_BINARY_DIR}/example/kinematics_example_build) diff --git a/example/kinematics_example/CMakeLists.txt b/example/kinematics_example/CMakeLists.txt new file mode 100644 index 0000000..94c8dee --- /dev/null +++ b/example/kinematics_example/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.14) +project(elite_kinematics_example CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(EXAMPLE_OUTPUT_DIR ${CMAKE_BINARY_DIR}/example) +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${EXAMPLE_OUTPUT_DIR}) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${EXAMPLE_OUTPUT_DIR}) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${EXAMPLE_OUTPUT_DIR}) + +# 依赖 KDL +find_package(orocos_kdl REQUIRED) + +add_library(elite_kdl_kinematics_example SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/kdl_kinematics_example.cpp +) + +# 头文件搜索路径 +target_include_directories(elite_kdl_kinematics_example PUBLIC + $ + $ + $ + ${EIGEN3_INCLUDE_DIRS} + +) + +# 依赖库 +target_link_libraries(elite_kdl_kinematics_example + orocos-kdl +) +if(UNIX AND NOT APPLE) + target_link_libraries(elite_kdl_kinematics_example dl) +endif() + +# 示例可执行文件 +add_executable(kinematics_example + ${CMAKE_CURRENT_SOURCE_DIR}/example/example_Kdl_kinematics.cpp +) +target_link_libraries(kinematics_example PRIVATE + elite_kdl_kinematics_example + elite-cs-series-sdk::static + ${SYSTEM_LIB} +) +if(UNIX AND NOT APPLE) + target_link_libraries(kinematics_example PRIVATE dl) +endif() +target_include_directories(kinematics_example PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_SOURCE_DIR}/include/Kinematics +) +set_target_properties(kinematics_example PROPERTIES + BUILD_RPATH "$ORIGIN" + RUNTIME_OUTPUT_DIRECTORY ${EXAMPLE_OUTPUT_DIR} +) + +install(TARGETS elite_kdl_kinematics_example + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) diff --git a/example/kinematics_example/example/example_Kdl_kinematics.cpp b/example/kinematics_example/example/example_Kdl_kinematics.cpp new file mode 100644 index 0000000..bbb7c81 --- /dev/null +++ b/example/kinematics_example/example/example_Kdl_kinematics.cpp @@ -0,0 +1,43 @@ +#include +#include "kinematics_base.hpp" + +int main() { + auto kin = ELITE::loadKinematicsSolver("libelite_kdl_kinematics_example.so"); + if (!kin) { + std::cerr << "Failed to load kinematics plugin\n"; + return 1; + } + + std::vector a{0.0, 0.0, -0.427, -0.3905, 0.0, 0.0}; + std::vector d{0.1625, 0.0, 0.0, 0.14750, 0.0965, 0.095}; + std::vector alpha{0.0, 1.5707963, 0.0, 0.0, 1.5707963, -1.5707963}; + + kin->configDH(a, d, alpha); // DH参数配置 + + // 示例位姿输入q + ELITE::JOINTS q{0.223, -1.628, 0.796, -1.084, 1.452, 0.978}; + ELITE::MAT4X4 T{}; + std::array tcp{0, 0, 0, 0, 0, 0}; + + kin->forward(q, T, tcp); + std::cout << "Forward kinematics (TCP 4x4 matrix):\n"; + std::cout.setf(std::ios::fixed); + std::cout.precision(6); + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + std::cout << T[r][c] << (c == 3 ? "" : "\t"); + } + std::cout << "\n"; + } + ELITE::JOINTS q_near{0.6, -1.1, 0.0, 0.0, 1.9, 3.0}; + ELITE::JOINTS q_out{}; + int ret = kin->inverse(T, q_near, q_out, tcp); + std::cout << "IK ret = " << ret << "\n"; + if (ret == 0) { + std::cout << "Inverse kinematics joints (rad): "; + for (std::size_t i = 0; i < q_out.size(); ++i) { + std::cout << q_out[i] << (i + 1 == q_out.size() ? "\n" : ", "); + } + } + return 0; +} diff --git a/example/kinematics_example/include/elite_kinematics/kdl_kinematics_example.hpp b/example/kinematics_example/include/elite_kinematics/kdl_kinematics_example.hpp new file mode 100644 index 0000000..a87e2f0 --- /dev/null +++ b/example/kinematics_example/include/elite_kinematics/kdl_kinematics_example.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "kinematics_base.hpp" +namespace ELITE { + +class Kinematics : public KinematicsBase { + public: + Kinematics(); + ~Kinematics() override; + + // ==== KinematicsBase 接口实现 ==== + std::string name() const override { return "KDL_Kinematics_example"; } + std::size_t dof() const override { return _jointCount; } + bool isDefault() const override; // 是否为默认求解器 + + void configDH(const JOINTS &dh_a, const JOINTS &dh_d, const JOINTS &dh_alpha) override; + + void forward(const JOINTS &joints, MAT4X4 mat, const std::array &tcp_offset_xyzRPY) override; // mat为输出结果 + + int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints, + const std::array &tcp_offset_xyzRPY) override; // mat为输入目标位姿 + + private: + int _jointCount; + std::unique_ptr _ikSolver; + std::unique_ptr _fkSolver; + std::unique_ptr _jacSolver; + std::unique_ptr _robotChain; + + JOINTS _dhA; + JOINTS _dhD; + JOINTS _dhAlpha; + + std::mutex _dataMutex; +}; + +} // namespace ELITE diff --git a/example/kinematics_example/src/kdl_kinematics_example.cpp b/example/kinematics_example/src/kdl_kinematics_example.cpp new file mode 100644 index 0000000..6365689 --- /dev/null +++ b/example/kinematics_example/src/kdl_kinematics_example.cpp @@ -0,0 +1,145 @@ +#include "elite_kinematics/kdl_kinematics_example.hpp" + +#include +#include +#include + +namespace ELITE { + +namespace { +void KDLJointsToJoints(const KDL::JntArray &kdl_joints, JOINTS &joints) { + joints.resize(kdl_joints.rows()); + for (unsigned int i = 0; i < kdl_joints.rows(); i++) { + joints[i] = kdl_joints(i); + } +} + +KDL::JntArray JointsToKDLJoints(const JOINTS &joints, int joint_count) { + KDL::JntArray kdl_joints(joint_count); + for (int i = 0; i < joint_count; i++) { + kdl_joints(i) = (i < static_cast(joints.size())) ? joints[i] : 0.0; + } + return kdl_joints; +} + +void KDLFrameToMat4x4(const KDL::Frame &frame, MAT4X4 mat4x4) { + mat4x4[0][0] = frame.M.data[0]; + mat4x4[0][1] = frame.M.data[1]; + mat4x4[0][2] = frame.M.data[2]; + mat4x4[0][3] = frame.p.data[0]; + + mat4x4[1][0] = frame.M.data[3]; + mat4x4[1][1] = frame.M.data[4]; + mat4x4[1][2] = frame.M.data[5]; + mat4x4[1][3] = frame.p.data[1]; + + mat4x4[2][0] = frame.M.data[6]; + mat4x4[2][1] = frame.M.data[7]; + mat4x4[2][2] = frame.M.data[8]; + mat4x4[2][3] = frame.p.data[2]; + + mat4x4[3][0] = 0; + mat4x4[3][1] = 0; + mat4x4[3][2] = 0; + mat4x4[3][3] = 1; +} + +KDL::Frame Mat4x4ToKDLFrame(const MAT4X4 mat4x4) { + KDL::Frame frame = KDL::Frame::Identity(); + frame.M.data[0] = mat4x4[0][0]; + frame.M.data[1] = mat4x4[0][1]; + frame.M.data[2] = mat4x4[0][2]; + frame.p.data[0] = mat4x4[0][3]; + + frame.M.data[3] = mat4x4[1][0]; + frame.M.data[4] = mat4x4[1][1]; + frame.M.data[5] = mat4x4[1][2]; + frame.p.data[1] = mat4x4[1][3]; + + frame.M.data[6] = mat4x4[2][0]; + frame.M.data[7] = mat4x4[2][1]; + frame.M.data[8] = mat4x4[2][2]; + frame.p.data[2] = mat4x4[2][3]; + return frame; +} +} // namespace + +Kinematics::Kinematics() : _jointCount(0) {} + +Kinematics::~Kinematics() = default; + +void Kinematics::forward(const JOINTS &joints, MAT4X4 mat, const std::array &tcp_offset_xyzRPY) { + if (!_fkSolver || static_cast(joints.size()) < _jointCount) { + std::cerr << "Please set Kinematics config first by updateKinConfig\n"; + return; + } + KDL::JntArray kdl_joints = JointsToKDLJoints(joints, _jointCount); + KDL::Frame end_effector_frame; + _fkSolver->JntToCart(kdl_joints, end_effector_frame); + + KDL::Rotation rot = KDL::Rotation::EulerZYX(tcp_offset_xyzRPY[5], tcp_offset_xyzRPY[4], tcp_offset_xyzRPY[3]); + + KDL::Frame tcp_offset(rot, KDL::Vector(tcp_offset_xyzRPY[0], tcp_offset_xyzRPY[1], tcp_offset_xyzRPY[2])); + KDL::Frame tcp_frame = end_effector_frame * tcp_offset; + + KDLFrameToMat4x4(tcp_frame, mat); +} + +int Kinematics::inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints, const std::array &tcp_offset_xyzRPY) { + if (!_ikSolver || static_cast(joints_near.size()) < _jointCount) { + return -1; + } + KDL::Rotation rot = KDL::Rotation::RPY(tcp_offset_xyzRPY[3], tcp_offset_xyzRPY[4], tcp_offset_xyzRPY[5]); + KDL::Frame tcp_offset(rot, KDL::Vector(tcp_offset_xyzRPY[0], tcp_offset_xyzRPY[1], tcp_offset_xyzRPY[2])); + + KDL::JntArray kdl_joints_near = JointsToKDLJoints(joints_near, _jointCount); + KDL::Frame frame = Mat4x4ToKDLFrame(mat); + frame = frame * tcp_offset.Inverse(); + KDL::JntArray ret_joints(_jointCount); + int result = _ikSolver->CartToJnt(kdl_joints_near, frame, ret_joints); + if (result == KDL::SolverI::E_NOERROR) { + KDLJointsToJoints(ret_joints, joints); + } + return (result == KDL::SolverI::E_NOERROR) ? 0 : -1; +} + +bool Kinematics::isDefault() const { return false; } + +void Kinematics::configDH(const JOINTS &dh_a, const JOINTS &dh_d, const JOINTS &dh_alpha) { + std::lock_guard lock(_dataMutex); + + if (!(dh_a.size() == dh_d.size() && dh_d.size() == dh_alpha.size()) || dh_a.empty()) { + std::cerr << "Invalid DH sizes\n"; + return; + } + + _ikSolver.reset(); + _fkSolver.reset(); + _jacSolver.reset(); + _robotChain.reset(); + + _robotChain = std::make_unique(); + + _jointCount = static_cast(dh_a.size()); + _dhA = dh_a; + _dhD = dh_d; + _dhAlpha = dh_alpha; + + for (int i = 0; i < _jointCount; i++) { + KDL::Frame rot = + KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0), KDL::Vector(0, std::cos(_dhAlpha[i]), std::sin(_dhAlpha[i])), + KDL::Vector(0, -std::sin(_dhAlpha[i]), std::cos(_dhAlpha[i])))); + KDL::Frame trans = KDL::Frame(KDL::Vector(_dhA[i], 0, _dhD[i])); + _robotChain->addSegment(KDL::Segment("Link" + std::to_string(i), KDL::Joint(KDL::Joint::None), rot * trans)); + _robotChain->addSegment(KDL::Segment("Link" + std::to_string(i), KDL::Joint(KDL::Joint::RotZ))); + } + + _ikSolver = std::make_unique(*(_robotChain), 1E-10, 500); + _fkSolver = std::make_unique(*(_robotChain)); + _jacSolver = std::make_unique(*(_robotChain)); +} + +} // namespace ELITE + +// =============== 导出为 dlopen 插件 =============== +extern "C" ELITE::KinematicsBase *createKinematicsPlugin() { return new ELITE::Kinematics(); } diff --git a/include/Kinematics/kinematics_base.hpp b/include/Kinematics/kinematics_base.hpp new file mode 100644 index 0000000..04bd691 --- /dev/null +++ b/include/Kinematics/kinematics_base.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace ELITE { + +using JOINTS = std::vector; +using MAT4X4 = double[4][4]; + +class KinematicsBase { + public: + virtual ~KinematicsBase() = default; + + virtual std::string name() const = 0; + virtual std::size_t dof() const = 0; + virtual bool isDefault() const { return false; } + + virtual void configDH(const JOINTS &dh_a, const JOINTS &dh_d, + const JOINTS &dh_alpha) = 0; // 配置DH参数 + + virtual void forward(const JOINTS &joints, MAT4X4 mat, + const std::array &tcp_offset_xyzRPY) = 0; // 前向运动学，joints为输入关节角，mat为输出结果 + + virtual int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints, const std::array &tcp_offset_xyzRPY) = 0; +}; // 逆向运动学，返回0表示成功,mat为输入目标位姿,joints_near为输入附近关节角, + // joints为输出关节角 + +using KinematicsBasePtr = std::shared_ptr; +using CreateKinematicsFn = KinematicsBase *(*)(); + +KinematicsBasePtr loadKinematicsSolver(const std::string &library_path, const std::string &symbol_name = "createKinematicsPlugin"); + +} // namespace ELITE diff --git a/source/Kinematics/kinematics_base.cpp b/source/Kinematics/kinematics_base.cpp new file mode 100644 index 0000000..a1667b8 --- /dev/null +++ b/source/Kinematics/kinematics_base.cpp @@ -0,0 +1,44 @@ +#include "kinematics_base.hpp" + +#ifdef _WIN32 +#include +#else +#include +#endif + +namespace ELITE { + +KinematicsBasePtr loadKinematicsSolver(const std::string &library_path, const std::string &symbol_name) { +#ifdef _WIN32 + HMODULE handle = ::LoadLibraryA(library_path.c_str()); + if (!handle) return nullptr; + + auto fn = reinterpret_cast(::GetProcAddress(handle, symbol_name.c_str())); + if (!fn) { + ::FreeLibrary(handle); + return nullptr; + } +#else + void *handle = ::dlopen(library_path.c_str(), RTLD_LAZY); + if (!handle) return nullptr; + + auto fn = reinterpret_cast(::dlsym(handle, symbol_name.c_str())); + if (!fn) { + ::dlclose(handle); + return nullptr; + } +#endif + + KinematicsBase *raw = fn(); + if (!raw) { +#ifdef _WIN32 + ::FreeLibrary(handle); +#else + ::dlclose(handle); +#endif + return nullptr; + } + return KinematicsBasePtr(raw); +} + +} // namespace ELITE