Add Kin and IK interface

CMakeLists.txt

@@ -5,10 +5,8 @@ project(elite-cs-series-sdk VERSION 1.2.0)
 option(ELITE_COMPILE_TESTS "Compile tests" OFF)
 option(ELITE_COMPILE_DOC "Compile documentation" OFF)
 option(ELITE_COMPILE_EXAMPLES "Compile examples" OFF)
-
 include(cmake/utils.cmake)
-
-if(NOT DEFINED CMAKE_CXX_STANDARD)
+if(NOT DEFINED CMAKE_CXX_STANDARD) 
     set(CMAKE_CXX_STANDARD 17)
 endif()
 
@@ -23,10 +21,49 @@ else()
 endif()
 
 # Find third-party library
+##Eigen3
+find_package(Eigen3 QUIET)
+if (Eigen3_FOUND)
+    message(STATUS "Found Eigen3: ${EIGEN3_INCLUDE_DIRS}")
+    include_directories(${EIGEN3_INCLUDE_DIRS})
+else()
+    message(WARNING "Eigen3 not found! Trying fallback path /usr/include/eigen3 ...")
+    if (EXISTS "/usr/include/eigen3/Eigen/Core")
+        include_directories("/usr/include/eigen3")
+        message(STATUS "Using fallback Eigen3 include path: /usr/include/eigen3")
+    else()
+        message(FATAL_ERROR "Eigen3 not found on this system. Please install it with:
+            sudo apt install libeigen3-dev")
+    endif()
+endif()
+###KDL
+find_package(orocos_kdl QUIET)
+if (orocos_kdl_FOUND)
+    message(STATUS "Found Orocos KDL: ${orocos_kdl_INCLUDE_DIRS}")
+    include_directories(${orocos_kdl_INCLUDE_DIRS})
+    list(APPEND THIRDPARTY_LIB orocos-kdl)
+    add_definitions(-DELITE_USE_KDL)
+else()
+    message(WARNING "Orocos KDL not found! Trying fallback include and library paths...")
+    if (EXISTS "/usr/include/kdl/chain.hpp")
+        include_directories("/usr/include")
+        if (EXISTS "/usr/lib/x86_64-linux-gnu/liborocos-kdl.so")
+            link_directories("/usr/lib/x86_64-linux-gnu")
+            target_link_libraries(elite-cs-series-sdk_SHARED orocos-kdl)
+            target_link_libraries(elite-cs-series-sdk_STATIC orocos-kdl)
+            message(STATUS "Using fallback Orocos KDL path: /usr/include + /usr/lib/x86_64-linux-gnu")
+        else()
+            message(WARNING "Found KDL headers but no liborocos-kdl.so in system paths!")
+        endif()
+    else()
+        message(FATAL_ERROR "Orocos KDL not found! Please install it using:
+            sudo apt install liborocos-kdl-dev")
+    endif()
+endif()
 find_package(Boost REQUIRED)
 find_package(libssh)
 if(libssh_FOUND)
-    set(THIRDPARTY_LIB ${THIRDPARTY_LIB} ssh)
+    list(APPEND THIRDPARTY_LIB ssh)
     add_definitions(-DELITE_USE_LIB_SSH)
 endif()
 
@@ -43,6 +80,7 @@ set(
     source/Common/EliteException.cpp
     source/Common/SshUtils.cpp
     source/Common/RtUtils.cpp
+    source/Common/Kinematics.cpp
     source/Primary/PrimaryPort.cpp
     source/Primary/PrimaryPortInterface.cpp
     source/Primary/RobotConfPackage.cpp
@@ -95,6 +133,7 @@ set(
     Elite/ControllerLog.hpp
     Elite/RobotException.hpp
     Common/RtUtils.hpp
+    Common/kinematics.hpp
 )
 
 # Set output library default name
@@ -214,7 +253,6 @@ if(CMAKE_SYSTEM_NAME STREQUAL "Linux" AND ELITE_INSTALL)
         DIRECTORY ${PROJECT_SOURCE_DIR}/source/resources/
         DESTINATION share/Elite
     )
-
     include(CMakePackageConfigHelpers)
     configure_package_config_file(
         "${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}Config.cmake.in"

doc/API/cn/EliteDriver.cn.md

@@ -379,3 +379,23 @@ void registerRobotExceptionCallback(std::function<void(RobotExceptionSharedPtr)>
     - registerRobotExceptionCallback: 回调函数，用于处理接收到的机器人异常。参数为机器人异常的共享指针(参考：[RobotException](./RobotException.cn.md))。
 
 
+### ***获取机器人逆解***
+```cpp
+std::shared_ptr<vector6d_t> getInverseKinematics(const vector6d_t& pose, const vector6d_t& near_joint,
+                                                              const vector6d_t& tcp) 
+```
+
+- ***功能***
+    传入参数获取逆解。
+
+- ***返回值***：返回值为指向<vector6d_t>数据类型的指针。
+
+### ***获取机器人正解***
+```cpp
+std::shared_ptr<vector6d_t> getForwardKinematics(const vector6d_t& joint_positions, const vector6d_t& tcp)
+```
+
+- ***功能***
+    传入参数获取正解。
+
+- ***返回值***：返回值为指向<vector6d_t>数据类型的指针。

include/Common/kinematics.hpp

@@ -0,0 +1,75 @@
+#ifndef __KIN_HPP__
+#define __KIN_HPP__
+#include <stdlib.h>
+#include <string.h>
+#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"
+#include "kinematics.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];
+
+class Kinematics {
+   public:
+    Kinematics();
+    ~Kinematics();
+    bool isDefault(void);
+    void updateKinConfig(const double dh_a[AXIS_COUNT], const double dh_d[AXIS_COUNT], const double dh_alpha[AXIS_COUNT]);
+    // api interface
+    void forward(const JOINTS joints, MAT4X4 mat,
+                 const std::array<double, 6> &tcp_offset_xyzrxryrz =
+                     std::array<double, 6>{});  
+    int inverse(MAT4X4 mat, const JOINTS &joints_near, JOINTS &joints,
+                const std::array<double, 6> &tcp_offset_xyzRPY = std::array<double, 6>{});
+
+   private:
+    // KDL::ChainIkSolverPos *_ikSolver;
+    // KDL::ChainFkSolverPos_recursive *_fkSolver;
+    // KDL::ChainJntToJacSolver *_jacSolver;
+    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

include/Control/ReversePort.hpp

@@ -17,7 +17,7 @@ class ReversePort {
 
 public:
     ReversePort(int port, int buffer_size = 4) {
-        server_ = std::make_shared<TcpServer>(port, 4);
+        server_ = std::make_shared<TcpServer>(port, buffer_size);
     }
     ~ReversePort() = default;
 

include/Control/ScriptCommandInterface.hpp

@@ -1,51 +1,58 @@
 #ifndef __SCRIPT_COMMAND_INTERFACE_HPP__
 #define __SCRIPT_COMMAND_INTERFACE_HPP__
 
-#include "TcpServer.hpp"
+#include <boost/asio.hpp>
+#include <iostream>
+#include <memory>
+#include <vector>
 #include "DataType.hpp"
 #include "ReversePort.hpp"
+#include "TcpServer.hpp"
 
-#include <memory>
-#include <boost/asio.hpp>
-
-namespace ELITE
-{
+namespace ELITE {
 
 class ScriptCommandInterface : public ReversePort {
-private:
-    enum class Cmd{
+   private:
+    enum class Cmd {
         ZERO_FTSENSOR = 0,
         SET_PAYLOAD = 1,
         SET_TOOL_VOLTAGE = 2,
         START_FORCE_MODE = 3,
         END_FORCE_MODE = 4,
+        INVERSE_KINEMATICS = 5,
+        FORWARD_KINEMATICS = 6
     };
-public:
+
+   public:
     static constexpr int SCRIPT_COMMAND_DATA_SIZE = 26;
 
+    std::mutex data_mutex_;
+    std::vector<uint8_t> recv_buffer_;
+    std::vector<int32_t> last_data_;
+    bool data_received_ = false;
     ScriptCommandInterface() = delete;
 
     /**
      * @brief Construct a new Script Command Interface object
-     * 
+     *
      * @param port Server port
      */
     ScriptCommandInterface(int port);
-    
+
     ~ScriptCommandInterface();
 
     /**
      * @brief zero the force/torque applied to the TCP measured by the sensor(tare sensor).
-     * 
-     * @return true 
-     * @return false 
+     *
+     * @return true
+     * @return false
      */
     bool zeroFTSensor();
 
     /**
-     * @brief This command is used to set the mass, 
+     * @brief This command is used to set the mass,
      * center of gravity and moment of inertia of the robot payload
-     * 
+     *
      * @param mass The mass of the payload
      * @param cog The coordinates of the center of gravity of the payload (relative to the flange frame).
      * @return true success
@@ -55,55 +62,83 @@ class ScriptCommandInterface : public ReversePort {
 
     /**
      * @brief Set the tool voltage
-     * 
+     *
      * @param vol Tool voltage
      * @return true success
      * @return false fail
      */
     bool setToolVoltage(const ToolVoltage& vol);
 
     /**
-     * @brief This command is used to enable force control mode and the robot will be controlled in the force control mode. 
-     * 
-     * @param reference_frame A pose vector that defines the force reference frame relative to the base frame. 
+     * @brief This command is used to enable force control mode and the robot will be controlled in the force control mode.
+     *
+     * @param reference_frame A pose vector that defines the force reference frame relative to the base frame.
      * The format is [X,Y,Z,Rx,Ry,Rz], where X, Y, and Z represent position with the unit of m, Rx, Ry, and RZ
      * represent pose with the unit of rad which is defined by standard Euler angles.
-     * @param selection_vector a 6-dimensional vector consisting of 0 and 1 that defines the compliant axis in the force frame. 
-     * 1 represents the axis is compliant and 0 represents the axis is non compliant.  
-     * @param wrench The force/torque applied to the environment by the robot. 
-     * The robot moves/rotates along the compliant axis to adjust its pose to achieve the target force/torque. 
-     * The format is [Fx,Fy,Fz,Mx,My,Mz], where Fx, Fy, and Fz represent the force applied along the 
-     * compliant axis with the unit of N, Mx, My, and Mz represent the torque applied about the 
-     * compliant axis with the unit of Nm. This value is invalid for the non-compliant axis. Due to the 
-     * safety restrictions of joints, the actual applied force/torque is lower than the set one. In the 
+     * @param selection_vector a 6-dimensional vector consisting of 0 and 1 that defines the compliant axis in the force frame.
+     * 1 represents the axis is compliant and 0 represents the axis is non compliant.
+     * @param wrench The force/torque applied to the environment by the robot.
+     * The robot moves/rotates along the compliant axis to adjust its pose to achieve the target force/torque.
+     * The format is [Fx,Fy,Fz,Mx,My,Mz], where Fx, Fy, and Fz represent the force applied along the
+     * compliant axis with the unit of N, Mx, My, and Mz represent the torque applied about the
+     * compliant axis with the unit of Nm. This value is invalid for the non-compliant axis. Due to the
+     * safety restrictions of joints, the actual applied force/torque is lower than the set one. In the
      * separate thread, the command get_tcp_force may be used to read the actual force/torque applied to the environment.
      * @param mode The parameter for force control mode
      * @param limits The parameter for the speed limit. The format is [Vx,Vy,Vz,ωx,ωy,ωz],
-     * where Vx, Vy, and Vz represent the maximum speed for TCP along 
-     * the compliant axis with the unit of m/s, ωx, ωy, and ωz represent the maximum speed for TCP 
-     * about the compliant axis with the unit of rad/s. This parameter is invalid for the non-compliant 
+     * where Vx, Vy, and Vz represent the maximum speed for TCP along
+     * the compliant axis with the unit of m/s, ωx, ωy, and ωz represent the maximum speed for TCP
+     * about the compliant axis with the unit of rad/s. This parameter is invalid for the non-compliant
      * axis whose trajectory will be as set before.
-     * @return true success 
+     * @return true success
      * @return false fail
      */
-    bool startForceMode(const vector6d_t& task_frame, const vector6int32_t& selection_vector,
-                             const vector6d_t& wrench, const ForceMode& mode, const vector6d_t& limits);
-    
+    bool startForceMode(const vector6d_t& task_frame, const vector6int32_t& selection_vector, const vector6d_t& wrench,
+                        const ForceMode& mode, const vector6d_t& limits);
+
     /**
-     * @brief This command is used to disable the force control mode. It also will be performed when the procedure ends. 
-     * 
+     * @brief This command is used to disable the force control mode. It also will be performed when the procedure ends.
+     *
      * @return true success
      * @return false fail
      */
     bool endForceMode();
 
-};
-
-
+    /**
+     * @brief This command calculates the inverse kinematics solution based on the given pose, near joint configuration, and TCP
+     * (Tool Center Point).
+     *
+     * The function computes the joint angles required to achieve a specified pose in Cartesian space.
+     * If the `near_joint` and `tcp` parameters are not provided, the function will use default values.
+     *
+     * @param pose The target pose in Cartesian space [x, y, z, Rx, Ry, Rz], where x, y, z are in meters, and Rx, Ry, Rz are in
+     * radians.
+     * @param near_joint near_joint: The joint angle configuration closest to the desired pose.
+     * @param tcp (Optional) The Tool Center Point (TCP) configuration. If not provided, the default TCP is used.
+     *
+     * @return A shared pointer to a `vector6d_t` containing the joint angles in radians that correspond to the target pose.
+     *         Returns nullptr if no valid solution is found.
+     */
 
+    std::shared_ptr<vector6d_t> getInverseKinematics(const vector6d_t& pose, const vector6d_t& near_joint, const vector6d_t& tcp);
 
+    /**
+     * @brief This command calculates the forward kinematics solution based on the given joint angles and TCP (Tool Center Point).
+     *
+     * The function computes the pose in Cartesian space corresponding to a specified set of joint angles.
+     * If the `tcp` parameter is not provided, the function will use a default value.
+     *
+     * @param joint The joint angles [J1, J2, J3, J4, J5, J6] in radians.
+     * @param tcp (Optional) The Tool Center Point (TCP) configuration. If not provided, the default TCP is used.
+     *
+     * @return A shared pointer to a `vector6d_t` containing the pose [x, y, z, Rx, Ry, Rz], where x, y, z are in meters,
+     *         and Rx, Ry, Rz are in radians. Returns nullptr if no valid solution is found.
+     */
+    std::shared_ptr<vector6d_t> getForwardKinematics(const vector6d_t& joint, const vector6d_t& tcp);
 
-} // namespace ELITE
+    void handReceive(const uint8_t* data, int data_length);
+};
 
+}  // namespace ELITE
 
 #endif

include/Elite/DataType.hpp

@@ -152,6 +152,7 @@ enum class FreedriveAction : int {
     FREEDRIVE_START = 1
 };
 
+
 using vector3d_t = std::array<double, 3>;
 using vector6d_t = std::array<double, 6>;
 using vector6int32_t = std::array<int32_t, 6>;