feat(dual_arm_action_server): add arm error services and IK support

- add ClearArmError service and ArmError topic publisher
- add InverseKinematics service using current joint seed
- implement RealRobotDriver joint error handling and IK wrappers
- wire periodic error publishing and service callbacks

src/dual_arm_action_server/include/dual_arm_action_server/dual_arm_action_server.hpp

@@ -2,6 +2,9 @@
 
 #include "dual_arm_action_server/real_robot_driver.hpp"
 #include "interfaces/action/dual_arm.hpp"
+#include "interfaces/msg/arm_error.hpp"
+#include "interfaces/srv/clear_arm_error.hpp"
+#include "interfaces/srv/inverse_kinematics.hpp"
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <atomic>
@@ -30,6 +33,14 @@ private:
   void handle_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle);
   void execute(const std::shared_ptr<GoalHandleDualArm> goal_handle);
 
+  void clear_arm_error_callback(
+    const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
+    std::shared_ptr<interfaces::srv::ClearArmError::Response> response);
+  void ik_service_callback(
+    const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
+    std::shared_ptr<interfaces::srv::InverseKinematics::Response> response);
+  void error_timer_callback();
+
   bool validate_goal(const DualArm::Goal & goal, std::string & error_msg) const;
   std::vector<interfaces::msg::ArmMotionParams> normalize_goal_params(
     const DualArm::Goal & goal) const;
@@ -38,6 +49,11 @@ private:
   rclcpp::Node::SharedPtr node_;
   rclcpp_action::Server<DualArm>::SharedPtr action_server_;
 
+  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_;
+
   RealRobotDriver driver_;
   std::atomic<bool> executing_left_;
   std::atomic<bool> executing_right_;

src/dual_arm_action_server/include/dual_arm_action_server/real_robot_driver.hpp

@@ -44,6 +44,16 @@ public:
     std::vector<double> & joints_left,
     std::vector<double> & joints_right);
 
+  int clear_joint_error(uint8_t arm_id, int joint_num);
+  int get_joint_error_and_brake(
+    uint8_t arm_id,
+    std::vector<uint16_t> & err_flag,
+    std::vector<uint16_t> & brake_state);
+  int inverse_kinematics(
+    uint8_t arm_id,
+    const rm_pose_t & pose,
+    std::vector<double> & joint_angles);
+
   void stop_arm(uint8_t arm_id);
   void stop_all();
 

src/dual_arm_action_server/src/dual_arm_action_server_node.cpp

@@ -75,6 +75,28 @@ void DualArmActionServer::initialize()
     std::bind(&DualArmActionServer::handle_cancel, this, std::placeholders::_1),
     std::bind(&DualArmActionServer::handle_accepted, this, std::placeholders::_1));
 
+  clear_error_service_ = node_->create_service<interfaces::srv::ClearArmError>(
+    "clear_arm_error",
+    std::bind(
+      &DualArmActionServer::clear_arm_error_callback,
+      this,
+      std::placeholders::_1,
+      std::placeholders::_2));
+
+  ik_service_ = node_->create_service<interfaces::srv::InverseKinematics>(
+    "inverse_kinematics",
+    std::bind(
+      &DualArmActionServer::ik_service_callback,
+      this,
+      std::placeholders::_1,
+      std::placeholders::_2));
+
+  arm_error_pub_ = node_->create_publisher<interfaces::msg::ArmError>("arm_errors", 10);
+
+  error_timer_ = node_->create_wall_timer(
+    std::chrono::milliseconds(500),
+    std::bind(&DualArmActionServer::error_timer_callback, this));
+
   RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready");
 }
 
@@ -340,6 +362,116 @@ void DualArmActionServer::get_target_arms(
   }
 }
 
+void DualArmActionServer::clear_arm_error_callback(
+  const std::shared_ptr<interfaces::srv::ClearArmError::Request> request,
+  std::shared_ptr<interfaces::srv::ClearArmError::Response> response)
+{
+  if (!driver_.is_connected()) {
+    response->success = false;
+    response->message = "Real robot not connected";
+    RCLCPP_ERROR(node_->get_logger(), "ClearArmError failed: real robot not connected");
+    return;
+  }
+
+  if (request->arm_id != LEFT_ARM && request->arm_id != RIGHT_ARM) {
+    response->success = false;
+    response->message = "Invalid arm_id (expected 0=left, 1=right)";
+    RCLCPP_ERROR(node_->get_logger(), "ClearArmError failed: invalid arm_id %d", request->arm_id);
+    return;
+  }
+
+  RCLCPP_INFO(
+    node_->get_logger(),
+    "Clearing error for arm %d, joint %d",
+    request->arm_id,
+    request->joint_num);
+
+  const int result = driver_.clear_joint_error(
+    static_cast<uint8_t>(request->arm_id),
+    request->joint_num);
+  if (result == 0) {
+    response->success = true;
+    response->message = "Clear error success";
+    RCLCPP_INFO(node_->get_logger(), "Clear error success");
+  } else {
+    response->success = false;
+    response->message = "Clear error failed with code: " + std::to_string(result);
+    RCLCPP_ERROR(node_->get_logger(), "Clear error failed with code: %d", result);
+  }
+}
+
+void DualArmActionServer::ik_service_callback(
+  const std::shared_ptr<interfaces::srv::InverseKinematics::Request> request,
+  std::shared_ptr<interfaces::srv::InverseKinematics::Response> response)
+{
+  if (!driver_.is_connected()) {
+    response->result = UNKNOWN_ERR;
+    RCLCPP_ERROR(node_->get_logger(), "InverseKinematics failed: real robot not connected");
+    return;
+  }
+
+  if (request->arm_id != LEFT_ARM && request->arm_id != RIGHT_ARM) {
+    response->result = UNKNOWN_ERR;
+    RCLCPP_ERROR(node_->get_logger(), "InverseKinematics failed: invalid arm_id %d", request->arm_id);
+    return;
+  }
+
+  rm_pose_t pose{};
+  pose.position.x = static_cast<float>(request->pose.position.x);
+  pose.position.y = static_cast<float>(request->pose.position.y);
+  pose.position.z = static_cast<float>(request->pose.position.z);
+  pose.quaternion.x = static_cast<float>(request->pose.orientation.x);
+  pose.quaternion.y = static_cast<float>(request->pose.orientation.y);
+  pose.quaternion.z = static_cast<float>(request->pose.orientation.z);
+  pose.quaternion.w = static_cast<float>(request->pose.orientation.w);
+
+  std::vector<double> joint_angles;
+  const int result = driver_.inverse_kinematics(
+    static_cast<uint8_t>(request->arm_id),
+    pose,
+    joint_angles);
+
+  response->result = result;
+  if (result == 0) {
+    response->joint_angles.clear();
+    response->joint_angles.reserve(joint_angles.size());
+    for (const auto angle : joint_angles) {
+      response->joint_angles.push_back(static_cast<float>(angle));
+    }
+  }
+}
+
+void DualArmActionServer::error_timer_callback()
+{
+  if (!driver_.is_connected()) {
+    return;
+  }
+
+  auto publish_error = [this](uint8_t arm_id) {
+    std::vector<uint16_t> err_flag;
+    std::vector<uint16_t> brake_state;
+    const int result = driver_.get_joint_error_and_brake(arm_id, err_flag, brake_state);
+    if (result != 0) {
+      return;
+    }
+
+    interfaces::msg::ArmError msg;
+    msg.arm_id = static_cast<int8_t>(arm_id);
+    msg.err_flag.reserve(err_flag.size());
+    msg.brake_state.reserve(brake_state.size());
+    for (const auto value : err_flag) {
+      msg.err_flag.push_back(value);
+    }
+    for (const auto value : brake_state) {
+      msg.brake_state.push_back(value);
+    }
+    arm_error_pub_->publish(msg);
+  };
+
+  publish_error(LEFT_ARM);
+  publish_error(RIGHT_ARM);
+}
+
 }  // namespace dual_arm_action_server
 
 int main(int argc, char ** argv)

src/dual_arm_action_server/src/real_robot_driver.cpp

@@ -197,6 +197,72 @@ bool RealRobotDriver::get_current_joints(
   return true;
 }
 
+int RealRobotDriver::clear_joint_error(uint8_t arm_id, int joint_num)
+{
+  rm_robot_handle * handle = handle_for(arm_id);
+  if (!handle || !rm_service_) {
+    return UNKNOWN_ERR;
+  }
+
+  const int result = rm_service_->rm_set_joint_clear_err(handle, joint_num);
+  if (result == 0) {
+    rm_service_->rm_set_joint_en_state(handle, joint_num, 1);
+  }
+  return result;
+}
+
+int RealRobotDriver::get_joint_error_and_brake(
+  uint8_t arm_id,
+  std::vector<uint16_t> & err_flag,
+  std::vector<uint16_t> & brake_state)
+{
+  rm_robot_handle * handle = handle_for(arm_id);
+  if (!handle || !rm_service_) {
+    return UNKNOWN_ERR;
+  }
+
+  err_flag.assign(ARM_DOF, 0);
+  brake_state.assign(ARM_DOF, 0);
+
+  return rm_service_->rm_get_joint_err_flag(handle, err_flag.data(), brake_state.data());
+}
+
+int RealRobotDriver::inverse_kinematics(
+  uint8_t arm_id,
+  const rm_pose_t & pose,
+  std::vector<double> & joint_angles)
+{
+  rm_robot_handle * handle = handle_for(arm_id);
+  if (!handle || !rm_service_) {
+    return UNKNOWN_ERR;
+  }
+
+  rm_inverse_kinematics_params_t params{};
+  params.q_pose = pose;
+  params.flag = 0;
+
+  {
+    std::lock_guard<std::mutex> lock(joints_mutex_);
+    const auto & source = (arm_id == LEFT_ARM) ? joints_left_ : joints_right_;
+    for (size_t i = 0; i < USED_ARM_DOF && i < source.size(); ++i) {
+      params.q_in[i] = static_cast<float>(source[i]);
+    }
+  }
+
+  float q_out[ARM_DOF] = {0.0f};
+  const int result = rm_service_->rm_algo_inverse_kinematics(handle, params, q_out);
+
+  joint_angles.clear();
+  if (result == 0) {
+    joint_angles.reserve(USED_ARM_DOF);
+    for (size_t i = 0; i < USED_ARM_DOF; ++i) {
+      joint_angles.push_back(static_cast<double>(q_out[i]));
+    }
+  }
+
+  return result;
+}
+
 void RealRobotDriver::stop_arm(uint8_t arm_id)
 {
   rm_robot_handle * handle = handle_for(arm_id);