feat(arm_control): add inverse_kinematics service and implement server with current joints as seed

rm_arm_control/include/rm_arm_node.hpp

@@ -7,6 +7,7 @@
 #include <std_msgs/msg/string.hpp>
 #include "interfaces/msg/arm_error.hpp"
 #include "interfaces/srv/clear_arm_error.hpp"
+#include "interfaces/srv/inverse_kinematics.hpp"
 #include "arm_driver_define.hpp"
 #include "rm_arm_handler.hpp"
 
@@ -29,6 +30,7 @@ public:
 private:
     sem_t sem_;
     ArmDriver *armDriver_;
+    Kinematics *kinematics_;
     rclcpp_action::Server<ArmAction>::SharedPtr action_server_;
     static sensor_msgs::msg::JointState armStates;
 
@@ -50,12 +52,15 @@ private:
 
     // NEW: Service and Topic
     rclcpp::Service<interfaces::srv::ClearArmError>::SharedPtr clear_error_service_;
+    rclcpp::Service<interfaces::srv::InverseKinematics>::SharedPtr ik_service_;
     rclcpp::Publisher<interfaces::msg::ArmError>::SharedPtr arm_error_pub_;
     rclcpp::TimerBase::SharedPtr error_timer_;
 
     void CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg);
     void ClearArmErrorCallback(const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
                                std::shared_ptr<interfaces::srv::ClearArmError::Response> response);
+    void IKServiceCallback(const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
+                           std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);
     void ErrorTimerCallback();
 };
 

rm_arm_control/src/rm_arm_node.cpp

@@ -103,13 +103,13 @@ static RmArmHandler *GetArmHandler(RmArmNode *node, int body_id)
 #define RIGHT_ARM_JOINT_START_INDEX 15
 #define JOINT_ALL_CNT 29
 // 机器人控制器构造函数
-RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_arm_control"), armDriver_(armDriver)
+RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_arm_control"), armDriver_(armDriver), kinematics_(kinematics)
 {
     localNodePtr = this;
     sem_init(&sem_, 0, 0);
 
-    KinematicsManager *leftTrajectory = new KinematicsManager(kinematics, LEFT_ARM);
-    KinematicsManager *rightTrajectory = new KinematicsManager(kinematics, RIGHT_ARM);
+    KinematicsManager *leftTrajectory = new KinematicsManager(kinematics_, LEFT_ARM);
+    KinematicsManager *rightTrajectory = new KinematicsManager(kinematics_, RIGHT_ARM);
 
     leftArmHandler_ = new RmArmHandler(armDriver_, LEFT_ARM, &sem_, leftTrajectory);
     if (leftArmHandler_ != NULL && leftArmHandler_->armStatus == ARM_STATUS_READY) {
@@ -130,6 +130,9 @@ RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_ar
     clear_error_service_ = this->create_service<interfaces::srv::ClearArmError>(
         "clear_arm_error", std::bind(&RmArmNode::ClearArmErrorCallback, this, std::placeholders::_1, std::placeholders::_2));
     
+    ik_service_ = this->create_service<interfaces::srv::InverseKinematics>(
+        "inverse_kinematics", std::bind(&RmArmNode::IKServiceCallback, this, std::placeholders::_1, std::placeholders::_2));
+    
     arm_error_pub_ = this->create_publisher<interfaces::msg::ArmError>("arm_errors", 10);
     
     error_timer_ = this->create_wall_timer(
@@ -554,3 +557,35 @@ void RmArmNode::ErrorTimerCallback()
     publish_error(LEFT_ARM);
     publish_error(RIGHT_ARM);
 }
+
+void RmArmNode::IKServiceCallback(const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
+                                 std::shared_ptr<interfaces::srv::InverseKinematics::Response> response)
+{
+    float endEffectorPose[POSE_DIMENSION];
+    endEffectorPose[POSE_POSITION_X] = request->pose.position.x;
+    endEffectorPose[POSE_POSITION_Y] = request->pose.position.y;
+    endEffectorPose[POSE_POSITION_Z] = request->pose.position.z;
+    endEffectorPose[POSE_QUATERNION_X] = request->pose.orientation.x;
+    endEffectorPose[POSE_QUATERNION_Y] = request->pose.orientation.y;
+    endEffectorPose[POSE_QUATERNION_Z] = request->pose.orientation.z;
+    endEffectorPose[POSE_QUATERNION_W] = request->pose.orientation.w;
+
+    float jointAngles[USED_ARM_DOF] = {0.0f};
+    
+    // Get the current joint angles for the specified arm as a start point for IK
+    RmArmHandler *handle = GetArmHandler(this, request->arm_id);
+    if (handle != NULL) {
+        for (int i = 0; i < USED_ARM_DOF; i++) {
+            jointAngles[i] = handle->nowJoints_[i];
+        }
+    }
+
+    int result = kinematics_->InverseKinematics(endEffectorPose, jointAngles);
+    
+    response->result = result;
+    if (result == 0) {
+        for (int i = 0; i < USED_ARM_DOF; i++) {
+            response->joint_angles.push_back(jointAngles[i]);
+        }
+    }
+}