添加自定义求解器插件及使用示例

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/
 )

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)

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include/Kinematics>
+  $<INSTALL_INTERFACE:include>
+  ${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
+)

example/kinematics_example/example/example_Kdl_kinematics.cpp

@@ -0,0 +1,43 @@
+#include <iostream>
+#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<double> a{0.0, 0.0, -0.427, -0.3905, 0.0, 0.0};
+    std::vector<double> d{0.1625, 0.0, 0.0, 0.14750, 0.0965, 0.095};
+    std::vector<double> 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<double, 6> 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;
+}

example/kinematics_example/include/elite_kinematics/kdl_kinematics_example.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <array>
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#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<double, 6> &tcp_offset_xyzRPY) override;  // mat为输出结果
+
+    int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints,
+                const std::array<double, 6> &tcp_offset_xyzRPY) override;  // mat为输入目标位姿
+
+   private:
+    int _jointCount;
+    std::unique_ptr<KDL::ChainIkSolverPos_LMA> _ikSolver;
+    std::unique_ptr<KDL::ChainFkSolverPos_recursive> _fkSolver;
+    std::unique_ptr<KDL::ChainJntToJacSolver> _jacSolver;
+    std::unique_ptr<KDL::Chain> _robotChain;
+
+    JOINTS _dhA;
+    JOINTS _dhD;
+    JOINTS _dhAlpha;
+
+    std::mutex _dataMutex;
+};
+
+}  // namespace ELITE

example/kinematics_example/src/kdl_kinematics_example.cpp

@@ -0,0 +1,145 @@
+#include "elite_kinematics/kdl_kinematics_example.hpp"
+
+#include <cmath>
+#include <cstring>
+#include <iostream>
+
+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<int>(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<double, 6> &tcp_offset_xyzRPY) {
+    if (!_fkSolver || static_cast<int>(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<double, 6> &tcp_offset_xyzRPY) {
+    if (!_ikSolver || static_cast<int>(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<std::mutex> 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<KDL::Chain>();
+
+    _jointCount = static_cast<int>(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<KDL::ChainIkSolverPos_LMA>(*(_robotChain), 1E-10, 500);
+    _fkSolver = std::make_unique<KDL::ChainFkSolverPos_recursive>(*(_robotChain));
+    _jacSolver = std::make_unique<KDL::ChainJntToJacSolver>(*(_robotChain));
+}
+
+}  // namespace ELITE
+
+// =============== 导出为 dlopen 插件 ===============
+extern "C" ELITE::KinematicsBase *createKinematicsPlugin() { return new ELITE::Kinematics(); }

include/Kinematics/kinematics_base.hpp

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <array>
+#include <cstddef>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace ELITE {
+
+using JOINTS = std::vector<double>;
+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<double, 6> &tcp_offset_xyzRPY) = 0;  // 前向运动学，joints为输入关节角，mat为输出结果
+
+    virtual int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints, const std::array<double, 6> &tcp_offset_xyzRPY) = 0;
+};  // 逆向运动学，返回0表示成功,mat为输入目标位姿,joints_near为输入附近关节角,
+    // joints为输出关节角
+
+using KinematicsBasePtr = std::shared_ptr<KinematicsBase>;
+using CreateKinematicsFn = KinematicsBase *(*)();
+
+KinematicsBasePtr loadKinematicsSolver(const std::string &library_path, const std::string &symbol_name = "createKinematicsPlugin");
+
+}  // namespace ELITE