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

rm_arm_control/include/arm_driver.hpp

@@ -58,6 +58,18 @@ public:
         return UNKNOWN_ERR;
     }
 
+    static int ClearJointError(int armId, int joint_num) {
+        (void)armId; (void)joint_num;
+        return 0; // Simulation always success
+    }
+
+    static int GetJointErrorAndBrake(int armId, uint16_t *err_flag, uint16_t *brake_state) {
+        (void)armId;
+        if (err_flag) *err_flag = 0;
+        if (brake_state) *brake_state = 0;
+        return 0;
+    }
+
     void PublicThreadUpdate();
 
 private:

rm_arm_control/include/rm_arm_node.hpp

@@ -5,6 +5,9 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "interfaces/action/arm.hpp"
 #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"
 
@@ -27,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;
 
@@ -46,7 +50,18 @@ private:
     static rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr armStatesPub_;
     std::unordered_map<int64_t, std::shared_ptr<GoalHandleArm>> goal_handles_;
 
+    // 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();
 };
 
 #endif // RM_ARM_NODE_HPP

rm_arm_control/include/rm_driver_kinematics.hpp

@@ -56,6 +56,9 @@ public:
     int ForwardKinematics(float *jointAngles, float *endEffectorPose);
     int InverseKinematics(const float *endEffectorPose, float *jointAngles);
 
+    static int ClearJointError(int armId, int joint_num);
+    static int GetJointErrorAndBrake(int armId, uint16_t *err_flag, uint16_t *brake_state);
+
     static RM_Service *rmService_;
 
 private:

rm_arm_control/simulator/rm_driver_kinematics.cpp

@@ -240,8 +240,8 @@ int RmArmDriverAndKinematics::computeMovingTrajectory(NodataTrajectory *newTraje
     for (int i = 0; i < USED_ARM_DOF; ++i) {
         joints[i] = end[i];
     }
-    rm_movej(robotHandle, joints, 5, 1, 0, 0);
-    return 0;
+    return rm_movej(robotHandle, joints, 40, 1, 0, 0);
+    // return 0;
 }
 
 int RmArmDriverAndKinematics::computeSteppingTrajectory(NodataTrajectory *newTrajectory, const float *start, const float *speed, int armId)
@@ -386,4 +386,35 @@ int RmArmDriverAndKinematics::InverseKinematics(const float *endEffectorPose, fl
     return 0;
 }
 
+int RmArmDriverAndKinematics::ClearJointError(int armId, int joint_num)
+{
+    if (rmService_ == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    rm_robot_handle *handle = (armId == LEFT_ARM) ? leftRobotHandle_ : rightRobotHandle_;
+    if (handle == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    //clear error
+    int ret = rmService_->rm_set_joint_clear_err(handle, joint_num);
+    if (ret == 0) {
+        //enable joint
+        rmService_->rm_set_joint_en_state(handle, joint_num, 1);
+    }
+    
+    return ret;
+}
+
+int RmArmDriverAndKinematics::GetJointErrorAndBrake(int armId, uint16_t *err_flag, uint16_t *brake_state)
+{
+    if (rmService_ == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    rm_robot_handle *handle = (armId == LEFT_ARM) ? leftRobotHandle_ : rightRobotHandle_;
+    if (handle == nullptr) {
+        return UNKNOWN_ERR;
+    }
+    return rmService_->rm_get_joint_err_flag(handle, err_flag, brake_state);
+}
+
 

rm_arm_control/src/rm_arm_handler.cpp

@@ -30,12 +30,15 @@ void PrintData(ArmStatusCallbackData *data)
 {
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK==================");
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode);
-    /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
+    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
+    std::string current = "current: ";
     for (size_t i = 0; i < USED_ARM_DOF; i++){
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
+        current += std::to_string(data->jointCurrent[i]) + " ";
+        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
     }
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", current.c_str());
     
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "enable:");
+    /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "enable:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
         RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointEnFlag[i]);
     }
@@ -43,8 +46,8 @@ void PrintData(ArmStatusCallbackData *data)
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "error code:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
         RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]);
-    }
-    */
+    }*/
+    
     std::string posStr = "joints position: ";
     for (size_t i = 0; i < USED_ARM_DOF; i++){
         posStr += std::to_string(data->jointAngle[i]) + " ";
@@ -54,18 +57,23 @@ void PrintData(ArmStatusCallbackData *data)
     /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
         RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]);
-    }
-    
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
-    for (size_t i = 0; i < USED_ARM_DOF; i++){
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointTemp[i]);
-    }
-    
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "voltage:");
-    for (size_t i = 0; i < USED_ARM_DOF; i++){
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
     }*/
     
+    posStr = "temperature: ";
+    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
+    for (size_t i = 0; i < USED_ARM_DOF; i++){
+        posStr += std::to_string(data->jointTemp[i]) + " ";
+        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointTemp[i]);
+    }
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
+    
+    posStr = "voltage: ";
+    //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "voltage:");
+    for (size_t i = 0; i < USED_ARM_DOF; i++){
+        //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
+        posStr += std::to_string(data->jointVoltage[i]) + " ";
+    }
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
 }
 
 void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
@@ -90,7 +98,8 @@ void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)
         armStatus = ARM_STATUS_WAITING;
     }
     if (temp != ARM_STATUS_READY){
-        PrintData(data);
+        // if (armType_ == 1)
+        //     PrintData(data);
     }
 }
 
@@ -235,6 +244,7 @@ int RmArmHandler::StartNewTrajectory()
     int result = trapezoidalTrajectory_->computeTrajectory(start, goal, goalType);
     if (result != 0) {
         goalManager_->GoalRunOff();
+        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "computeTrajectory Failed, GoalRunOff.");
     }
     /*Trajectory* nowTrajectory = trapezoidalTrajectory_->getNowTrajectory();
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Pose stepping continue. accs:%d, unis:%d, vels:%f, %f, %f, %f, %f, %f",

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) {
@@ -127,6 +127,17 @@ RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_ar
 
     armStatesPub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
 
+    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(
+        500ms, std::bind(&RmArmNode::ErrorTimerCallback, this));
+
     this->action_server_ = rclcpp_action::create_server<ArmAction>(
         this,
         "ArmAction",
@@ -279,10 +290,10 @@ void RmArmNode::ArmActionFeedback(int coll)
         feedback->float_length = POSE_DIMENSION;
         feedback->int_data_array = {coll};
         feedback->float_data_array = {pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]};
-        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Publishing feedback for command id: %ld, joints: %f, %f, %f, %f, %f, %f, %f",
-            feedback->command_id, feedback->float_data_array[0], feedback->float_data_array[1],
-            feedback->float_data_array[2], feedback->float_data_array[3],
-            feedback->float_data_array[4], feedback->float_data_array[5], feedback->float_data_array[6]);
+        // RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Publishing feedback for command id: %ld, joints: %f, %f, %f, %f, %f, %f, %f",
+        //     feedback->command_id, feedback->float_data_array[0], feedback->float_data_array[1],
+        //     feedback->float_data_array[2], feedback->float_data_array[3],
+        //     feedback->float_data_array[4], feedback->float_data_array[5], feedback->float_data_array[6]);
         goal_handle->publish_feedback(feedback);
     }
 }
@@ -508,3 +519,73 @@ void RmArmNode::CallbackRealtimeArmJointState(ArmStatusCallbackData *data)
         armStatesPub_->publish(armStates);
     }
 }
+
+void RmArmNode::ClearArmErrorCallback(const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
+                                     std::shared_ptr<interfaces::srv::ClearArmError::Response> response)
+{
+    RCLCPP_INFO(this->get_logger(), "Clearing error for arm %d, joint %d", request->arm_id, request->joint_num);
+    int result = ArmDriver::ClearJointError(request->arm_id, request->joint_num);
+    if (result == 0) {
+        response->success = true;
+        response->message = "Clear error success";
+        RCLCPP_INFO(this->get_logger(), "Clear error success");
+    } else {
+        response->success = false;
+        response->message = "Clear error failed with code: " + std::to_string(result);
+        RCLCPP_ERROR(this->get_logger(), "Clear error failed with code: %d", result);
+    }
+}
+
+void RmArmNode::ErrorTimerCallback()
+{
+    auto publish_error = [&](int arm_id) {
+        uint16_t err_flags[ARM_DOF] = {0};
+        uint16_t brake_states[ARM_DOF] = {0};
+        
+        int result = ArmDriver::GetJointErrorAndBrake(arm_id, err_flags, brake_states);
+        if (result == 0) {
+            interfaces::msg::ArmError msg;
+            msg.arm_id = arm_id;
+            for(int i=0; i<ARM_DOF; ++i) {
+                msg.err_flag.push_back(err_flags[i]);
+                msg.brake_state.push_back(brake_states[i]);
+            }
+            arm_error_pub_->publish(msg);
+        }
+    };
+
+    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]);
+        }
+    }
+}