添加自定义正逆解求解器动态插件

eli_cs_robot_moveit_config/config/kinematics.yaml

@@ -1,8 +1,9 @@
 /**:
   ros__parameters:
     robot_description_kinematics:
-      cs_manipulator:
-        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+      cs_manipulator:          #  move group 
+        kinematics_solver: elite_moveit/EliteKinematicsPlugin_example1  # 自定义插件名
         kinematics_solver_search_resolution: 0.005
         kinematics_solver_timeout: 0.005
-        kinematics_solver_attempts: 3
+        kinematics_solver_attempts: 3        # 可选
+

eli_cs_robot_moveit_config/launch/cs_moveit.launch.py

@@ -223,6 +223,7 @@ def launch_setup(context, *args, **kwargs):
             servo_params,
             robot_description,
             robot_description_semantic,
+            robot_description_kinematics,
         ],
         output="screen",
     )

elite_kinematics/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.8)
+project(elite_kinematics)
+
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(orocos_kdl REQUIRED)
+
+# 构建共享库
+add_library(kinematics_example1 SHARED
+  src/kinematics_example1.cpp
+)
+
+# 查找头文件
+target_include_directories(kinematics_example1 PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(kinematics_example1
+  pluginlib
+  orocos_kdl
+)
+
+# 导出 pluginlib 插件描述
+pluginlib_export_plugin_description_file(
+  elite_kinematics
+  kinematics_plugin.xml
+)
+
+install(TARGETS kinematics_example1
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES kinematics_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(kinematics_example1)
+ament_export_dependencies(pluginlib orocos_kdl)
+
+ament_package()

elite_kinematics/include/Log.hpp

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <cstdio>
+
+// Lightweight logging shim to keep the solver self contained.
+#define ELITE_LOG_ERROR(fmt, ...) \
+  std::fprintf(stderr, "[elite_kinematics][error] " fmt "\n", ##__VA_ARGS__)
+

elite_kinematics/include/elite_kinematics/kinematics_example1.hpp

@@ -0,0 +1,99 @@
+#ifndef __KIN_HPP__
+#define __KIN_HPP__
+
+#include <stdlib.h>
+#include <string.h>
+#include <array>
+#include <cmath>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "kdl/chain.hpp"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainiksolverpos_lma.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
+#include "kdl/frames_io.hpp"
+
+#define PI1 3.14
+#define UNUSED(x) (void)(x)
+
+namespace KDL {
+class ChainIkSolverPos;
+class ChainFkSolverPos_recursive;
+class Chain;
+class ChainJntToJacSolver;
+class JntArray;
+class Frame;
+}  // namespace KDL
+
+#define AXIS_COUNT 6  // 默认机械臂6自由度
+
+#ifndef ROBOT_STRUCTURE_TYPE
+#define ROBOT_STRUCTURE_TYPE int
+#endif
+
+#define MIN_LEN 1.0e-9
+
+typedef double JOINTS[AXIS_COUNT];
+typedef double VEC3D[3];
+typedef double PNT3D[3];
+
+typedef struct _rframe {
+    VEC3D X;
+    VEC3D Y;
+    VEC3D Z;
+    PNT3D O;
+    double scale;
+} RFRAME;
+
+typedef double MAT4X4[4][4];
+
+// ================== 基类接口（给 pluginlib 用） ==================
+//在moveit2中可以不用写base类
+class KinematicsBase {
+   public:
+    virtual ~KinematicsBase() = default;
+    virtual void updateKinConfig(const double dh_a[AXIS_COUNT], const double dh_d[AXIS_COUNT],
+                                 const double dh_alpha[AXIS_COUNT]) = 0;
+
+    virtual void forward(const JOINTS joints, MAT4X4 mat,
+                         const std::array<double, 6> &tcp_offset_xyzrxryrz = std::array<double, 6>{}) = 0;
+
+    virtual int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints,
+                        const std::array<double, 6> &tcp_offset_xyzRPY = std::array<double, 6>{}) = 0;
+};
+class Kinematics : public KinematicsBase {
+   public:
+    Kinematics();
+    ~Kinematics() override;
+
+    bool isDefault(void);
+
+    void updateKinConfig(const double dh_a[AXIS_COUNT], const double dh_d[AXIS_COUNT], const double dh_alpha[AXIS_COUNT]) override;
+
+    // api interface
+    void forward(const JOINTS joints, MAT4X4 mat,
+                 const std::array<double, 6> &tcp_offset_xyzrxryrz = std::array<double, 6>{}) override;
+
+    int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints,
+                const std::array<double, 6> &tcp_offset_xyzRPY = std::array<double, 6>{}) override;
+
+    // 直接用外部构造好的 KDL Chain
+    void updateKdlChain(const KDL::Chain &chain);
+
+   private:
+    std::unique_ptr<KDL::ChainIkSolverPos> _ikSolver;
+    std::unique_ptr<KDL::ChainFkSolverPos_recursive> _fkSolver;
+    std::unique_ptr<KDL::ChainJntToJacSolver> _jacSolver;
+    std::unique_ptr<KDL::Chain> _robotChain;
+    int _jointCount;
+
+    std::mutex _dataMutex;
+    double _dhA[AXIS_COUNT];
+    double _dhD[AXIS_COUNT];
+    double _dhAlpha[AXIS_COUNT];
+};
+
+#endif  // __KIN_HPP__

elite_kinematics/kinematics_plugin.xml

@@ -0,0 +1,10 @@
+<library path="lib/libkinematics_example1">
+  <class name="elite_kinematics_/Kinematics_example1"
+         type="Kinematics_example1 "
+         base_class_type="KinematicsBase">
+    <description>
+      Custom KDL-based kinematics solver.
+    </description>
+  </class>
+</library>
+

elite_kinematics/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>elite_kinematics</name>
+  <version>0.0.1</version>
+  <description>Custom kinematics plugin using KDL and pluginlib</description>
+
+  <maintainer email="[email protected]">Dukang</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>pluginlib</depend>
+  <depend>orocos_kdl</depend>
+  <export>
+    <!-- 告诉 pluginlib 插件描述文件在哪里 -->
+    <pluginlib plugin="${prefix}/kinematics_plugin.xml"/>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
+

elite_kinematics/src/kinematics_example1.cpp

@@ -0,0 +1,187 @@
+
+#include "elite_kinematics/kinematics_example1.hpp"
+#include <pluginlib/class_list_macros.hpp>
+#include "Log.hpp"
+static const RFRAME base_frame = {
+    .X = {-1.0, 0.0, 0.0},
+    .Y = {0.0, -1.0, 0.0},
+    .Z = {0.0, 0.0, 1.0},
+    .O = {0},
+    .scale = 1.0,
+};
+
+static const RFRAME flange_frame = {
+    .X = {0.0, 0.0, 1.0},
+    .Y = {-1.0, 0.0, 0.0},
+    .Z = {0.0, -1.0, 0.0},
+    .O = {0},
+    .scale = 1.0,
+};
+
+static void KDLJointsToJoints(const KDL::JntArray &kdl_joints, JOINTS joints) {
+    for (int i = 0; i < AXIS_COUNT; i++) {
+        joints[i] = kdl_joints(i);
+    }
+}
+
+static KDL::JntArray JointsToKDLJoints(const JOINTS joints) {
+    KDL::JntArray kdl_joints(AXIS_COUNT);
+    for (int i = 0; i < AXIS_COUNT; i++) {
+        kdl_joints(i) = joints[i];
+    }
+    return kdl_joints;
+}
+
+static 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;
+}
+
+static 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;
+}
+
+Kinematics::Kinematics() {
+    _jointCount = AXIS_COUNT;
+    _ikSolver = nullptr;
+    _fkSolver = nullptr;
+    _jacSolver = nullptr;
+    _robotChain = nullptr;
+    memset(_dhA, 0, sizeof(_dhA));
+    memset(_dhD, 0, sizeof(_dhD));
+    memset(_dhAlpha, 0, sizeof(_dhAlpha));
+}
+
+Kinematics::~Kinematics() {
+    _ikSolver.reset();
+    _fkSolver.reset();
+    _jacSolver.reset();
+    _robotChain.reset();
+}
+
+void Kinematics::forward(const JOINTS joints, MAT4X4 mat, const std::array<double, 6> &tcp_offset_xyzRPY) {
+    KDL::JntArray kdl_joints = JointsToKDLJoints(joints);
+    KDL::Frame end_effector_frame;
+
+    if (_fkSolver == nullptr) {
+        ELITE_LOG_ERROR("Please set Kinematics config first by Function: setKDLconfig !");
+        return;
+    }
+    if (tcp_offset_xyzRPY.size() != 6) {
+        ELITE_LOG_ERROR("TCP offset must have 6 elements!");
+        return;
+    }
+    _fkSolver->JntToCart(kdl_joints, end_effector_frame);
+
+    KDL::Rotation rot;
+
+    // RPY
+    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 == nullptr) {
+        return -1;
+    }
+    KDL::Rotation rot;
+    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);
+    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(void) { return false; }
+
+void Kinematics::updateKinConfig(const double dh_a[AXIS_COUNT], const double dh_d[AXIS_COUNT], const double dh_alpha[AXIS_COUNT]) {
+    _dataMutex.lock();
+
+    // 自动释放之前的资源
+    _ikSolver.reset();
+    _fkSolver.reset();
+    _jacSolver.reset();
+    _robotChain.reset();
+
+    _robotChain = std::make_unique<KDL::Chain>();
+
+    for (int i = 0; i < _jointCount; i++) {
+        _dhA[i] = dh_a[i];
+        _dhD[i] = dh_d[i];
+        _dhAlpha[i] = dh_alpha[i];
+    }
+
+    // 根据新的 DH 参数配置机器人链
+    for (int i = 0; i < _jointCount; i++) {
+        KDL::Frame rot = KDL::Frame(KDL::Rotation(KDL::Vector(1, 0, 0), KDL::Vector(0, cos(_dhAlpha[i]), sin(_dhAlpha[i])),
+                                                  KDL::Vector(0, -sin(_dhAlpha[i]), 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));
+
+    _dataMutex.unlock();
+}
+
+void Kinematics::updateKdlChain(const KDL::Chain &chain) {
+    std::lock_guard<std::mutex> lock(_dataMutex);
+
+    _ikSolver.reset();
+    _fkSolver.reset();
+    _jacSolver.reset();
+    _robotChain = std::make_unique<KDL::Chain>(chain);
+    _jointCount = chain.getNrOfJoints();
+
+    _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);
+}
+// =============== 导出为 pluginlib 插件 ===============
+PLUGINLIB_EXPORT_CLASS(Kinematics, KinematicsBase)