rm lib for ik cal

src/brain/CMakeLists.txt

@@ -42,7 +42,10 @@ add_executable(brain_node
 
 target_include_directories(brain_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+  interfaces/include
+)
 
 target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
@@ -68,6 +71,7 @@ ament_target_dependencies(brain_node
 target_link_libraries(brain_node
   Boost::thread
   yaml-cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so
 )
 
 install(TARGETS brain_node
@@ -163,9 +167,12 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_cerebellum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_cerebellum_node PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+    )
     target_link_libraries(test_cerebellum_node Boost::thread)
-    target_link_libraries(test_cerebellum_node yaml-cpp)
+    target_link_libraries(test_cerebellum_node yaml-cpp ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
   endif()
 
   ament_add_gtest(test_cerebrum_node
@@ -190,9 +197,12 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_cerebrum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_cerebrum_node PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+    )
     target_link_libraries(test_cerebrum_node Boost::thread)
-    target_link_libraries(test_cerebrum_node yaml-cpp)
+    target_link_libraries(test_cerebrum_node yaml-cpp ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
   endif()
 
   ament_add_gtest(test_sm_cerebellum
@@ -211,7 +221,11 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_sm_cerebellum PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_sm_cerebellum PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+    )
+    target_link_libraries(test_sm_cerebellum ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
   endif()
 
   # ExecuteBtAction end-to-end single skill test
@@ -236,8 +250,11 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
-    target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
+    target_include_directories(test_execute_bt_action PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+    )
+    target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
   endif()
 
   # Extended ExecuteBtAction scenario tests (multi-skill, failure, RUN interpolation, timeout, stats, cancel)
@@ -262,8 +279,11 @@ if(BUILD_TESTING)
       tf2_ros
       tf2_geometry_msgs
     )
-    target_include_directories(test_execute_bt_action_extended PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
-    target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp)
+    target_include_directories(test_execute_bt_action_extended PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+    )
+    target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
   endif()
 
   # Additional isolated ExecuteBtAction scenario tests
@@ -278,8 +298,11 @@ if(BUILD_TESTING)
       target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
       ament_target_dependencies(${test_name}
         rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
-      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
-      target_link_libraries(${test_name} Boost::thread yaml-cpp)
+      target_include_directories(${test_name} PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+        ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+      )
+      target_link_libraries(${test_name} Boost::thread yaml-cpp ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
     endif()
   endforeach()
 
@@ -293,8 +316,11 @@ if(BUILD_TESTING)
     target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
     ament_target_dependencies(test_cerebrum_utils
       rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
-    target_include_directories(test_cerebrum_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
-    target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
+    target_include_directories(test_cerebrum_utils PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+      ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
+    )
+    target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp ${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/lib/libapi_cpp.so)
   endif()
 endif()
 

src/brain/config/bt_vision_grasp.xml

@@ -4,10 +4,10 @@
       <!-- Retry vision + hand until GripperControl_H succeeds -->
       <RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
         <Sequence>
-          <GripperCmd_H name="gripper_open" />
+          <!-- <GripperCmd_H name="gripper_open" /> -->
           <VisionObjectRecognition_H name="VisionObjectRecognition_H" />
-          <Arm_H name="right_arm_vision_grasp" />
-          <GripperCmd_H name="gripper_close" />
+          <!-- <Arm_H name="right_arm_vision_grasp" /> -->
+          <!-- <GripperCmd_H name="gripper_close" /> -->
         </Sequence>
       </RetryUntilSuccessful>
       <!-- After Arm_H eventually succeeds, run the hand -->

src/brain/include/brain/calc_grasp_pose.hpp

@@ -15,20 +15,52 @@ struct Pose {
 };
 
 struct GraspResult {
+    std::string name;
     Pose pre_grasp_pose;
     Pose grasp_pose;
 };
 
 class GraspPoseCalculator {
 public:
+    GraspPoseCalculator();
     static GraspResult calculate(
         const tf2::Vector3& target_pos,
         const tf2::Quaternion& target_quat,
         const std::string& grasp_type = "side",
+        double angle_deg = 0.0,
+        const std::string& palm_facing = "down",
         double safety_dist = 0.06,
         double finger_length = 0.0,
         const std::string& arm = "right"
     );
+    static double evaluate_grasp_quality(
+        const tf2::Vector3& target_pos,
+        double angle_deg,
+        const std::string& arm = "right"
+    );
+    static std::vector<double> find_optimal_grasp_angles(
+        const tf2::Vector3& target_pos,
+        const std::string& arm = "right",
+        int top_k = 3
+    );
+    static int rm_arm_inverse_kinematics(
+        const tf2::Vector3& target_pos,
+        const tf2::Quaternion& target_quat,
+        std::vector<double>& joint_angles
+    );
+
+private:
+    static bool rm_arm_algo_init_done_;
+
+    static GraspResult create_grasp_matrix(
+        const tf2::Vector3& target_pos,
+        const tf2::Vector3& approach_vec,
+        const tf2::Vector3& palm_normal_vec,
+        double safety_dist,
+        double finger_length
+    );
+    static void rm_arm_algo_init(void);
+
 };
 
 } // namespace brain

src/brain/include/brain/cerebellum_node.hpp

@@ -18,6 +18,7 @@
 #include <smacc2/smacc_signal_detector.hpp>
 #include "brain/sm_cerebellum.hpp"  // for CerebellumData::ExecResult
 // For from_yaml<T>(YAML::Node)
+#include <sensor_msgs/msg/joint_state.hpp>
 #include "brain/payload_converters.hpp"
 #include "brain/robot_config.hpp"
 #include "brain/calc_grasp_pose.hpp"
@@ -42,10 +43,12 @@ public:
    */
   explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
-  std::vector<geometry_msgs::msg::Pose> side_pre_grasp_poses_;
-  std::vector<geometry_msgs::msg::Pose> side_grasp_poses_;
-  std::vector<geometry_msgs::msg::Pose> top_pre_grasp_poses_;
-  std::vector<geometry_msgs::msg::Pose> top_grasp_poses_;
+  std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
+  std::vector<geometry_msgs::msg::Pose> grasp_poses_;
+
+  std::vector<double> left_arm_joint_angles_;
+  std::vector<double> right_arm_joint_angles_;
+  std::mutex joint_state_mutex_;
 
 private:
   struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
@@ -100,6 +103,9 @@ private:
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   std::string arm_target_frame_{"base_link_rightarm"};
   double arm_transform_timeout_sec_{1};
+  
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
+  void JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
   double grab_width_{0.0};
 
   // Current accepted sequence epoch (from client) used to filter/reject stale goals

src/brain/include/brain/sm_cerebellum.hpp

@@ -149,10 +149,10 @@ struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
         while (!d.execute_fn) {
           std::this_thread::sleep_for(5ms);
           auto now = std::chrono::steady_clock::now();
-            if (now - last_warn > 5s) {
-              RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
-              last_warn = now;
-            }
+          if (now - last_warn > 10s) {
+            RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
+            last_warn = now;
+          }
         }
         try {
           auto res = d.execute_fn(this->getLogger());

src/brain/src/calc_grasp_pose.cpp

@@ -1,63 +1,50 @@
 #include "brain/calc_grasp_pose.hpp"
 #include <cmath>
 #include <iostream>
+#include <algorithm>
+#include <vector>
+#include <utility>
+#include <string.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logger.hpp>
+
+#include "arm_action_define.h"
+#include "rm_interface.h"
 
 namespace brain {
 
+bool GraspPoseCalculator::rm_arm_algo_init_done_ = false;
+
+GraspPoseCalculator::GraspPoseCalculator()
+{
+    rm_arm_algo_init();
+}
+
 tf2::Vector3 normalize_vector(const tf2::Vector3& v) {
     if (v.length() == 0) return v;
     return v.normalized();
 }
 
-GraspResult GraspPoseCalculator::calculate(
+GraspResult GraspPoseCalculator::create_grasp_matrix(
     const tf2::Vector3& target_pos,
-    const tf2::Quaternion& target_quat,
-    const std::string& grasp_type,
+    const tf2::Vector3& approach_vec,
+    const tf2::Vector3& palm_normal_vec,
     double safety_dist,
-    double finger_length,
-    const std::string& arm
+    double finger_length
 ) {
-    tf2::Transform T_target(target_quat, target_pos);
-    (void) arm;
-    
-    tf2::Vector3 V_world_up(-1.0, 0.0, 0.0);
-    tf2::Vector3 V_world_down = -V_world_up; // [1, 0, 0]
-    
-    tf2::Vector3 P_shoulder(0, 0, 0);
-    tf2::Vector3 P_target = target_pos;
-    tf2::Vector3 V_sb = P_target - P_shoulder;
-    tf2::Transform T_grasp;
-    T_grasp.setIdentity();
-    T_grasp.setOrigin(P_target);
-    
-    tf2::Vector3 z_axis, y_axis, x_axis;
-
-    if (grasp_type == "side") {
-        tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
-        z_axis = normalize_vector(V_horiz);
-        x_axis = normalize_vector(V_world_down);
-        y_axis = z_axis.cross(x_axis);
-        x_axis = y_axis.cross(z_axis);
-    } else if (grasp_type == "top") {
-        z_axis = normalize_vector(V_world_down);
-        tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
-        x_axis = normalize_vector(V_horiz);
-        y_axis = z_axis.cross(x_axis);
-        x_axis = y_axis.cross(z_axis);
-    } else {
-        tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
-        z_axis = normalize_vector(V_horiz);
-        x_axis = normalize_vector(V_world_down);
-        y_axis = z_axis.cross(x_axis);
-        x_axis = y_axis.cross(z_axis);
-    }
-
+    tf2::Vector3 z_axis = normalize_vector(approach_vec);
+    tf2::Vector3 x_axis = normalize_vector(palm_normal_vec);
+    tf2::Vector3 y_axis = z_axis.cross(x_axis);
+    x_axis = y_axis.cross(z_axis);
     tf2::Matrix3x3 R_grasp(
         x_axis.x(), y_axis.x(), z_axis.x(),
         x_axis.y(), y_axis.y(), z_axis.y(),
         x_axis.z(), y_axis.z(), z_axis.z()
     );
     
+    tf2::Transform T_grasp;
+    T_grasp.setIdentity();
+    T_grasp.setOrigin(target_pos);
     T_grasp.setBasis(R_grasp);
     T_grasp.setOrigin(T_grasp.getOrigin() - z_axis * finger_length);
     tf2::Transform T_pre = T_grasp;
@@ -72,4 +59,194 @@ GraspResult GraspPoseCalculator::calculate(
     return result;
 }
 
+GraspResult GraspPoseCalculator::calculate(
+    const tf2::Vector3& target_pos,
+    const tf2::Quaternion& target_quat,
+    const std::string& grasp_type,
+    double angle_deg,
+    const std::string& palm_facing,
+    double safety_dist,
+    double finger_length,
+    const std::string& arm
+) {
+    (void)target_quat;
+    (void)arm;
+
+    tf2::Vector3 V_world_up(-1.0, 0.0, 0.0);
+    tf2::Vector3 V_world_down = -V_world_up;
+    tf2::Vector3 P_shoulder(0, 0, 0);
+    tf2::Vector3 V_sb = target_pos - P_shoulder;
+    tf2::Vector3 V_horiz = V_sb - (V_sb.dot(V_world_up) * V_world_up);
+    tf2::Vector3 approach_vec;
+    tf2::Vector3 palm_normal;
+    std::string name_suffix;
+    
+    if (grasp_type == "side") {
+        tf2::Vector3 V_horiz_sb(0, V_sb.y(), V_sb.z());
+        double base_angle = 0.0;
+        if (V_horiz_sb.length() >= 1e-3) {
+            base_angle = std::atan2(V_horiz_sb.z(), V_horiz_sb.y());
+        }
+        double rad = angle_deg * M_PI / 180.0;
+        double angle = base_angle + rad;
+        double dy = std::cos(angle);
+        double dz = std::sin(angle);
+        approach_vec = tf2::Vector3(0, dy, dz); // Horizontal approach
+
+        if (palm_facing == "up") {
+            palm_normal = V_world_up;
+            name_suffix = "Side_Angle_" + std::to_string((int)angle_deg) + "_Palm_Up";
+        } else {
+            palm_normal = V_world_down;
+            name_suffix = "Side_Angle_" + std::to_string((int)angle_deg) + "_Palm_Down";
+        }
+    } else if (grasp_type == "top") {
+        approach_vec = V_world_down;
+        palm_normal = V_horiz;
+        name_suffix = "Top_Grasp";
+    } else {
+        approach_vec = V_horiz;
+        palm_normal = V_world_down;
+        name_suffix = "Side_Default";
+    }
+    
+    GraspResult result = create_grasp_matrix(target_pos, approach_vec, palm_normal, safety_dist, finger_length);
+    result.name = name_suffix;
+    return result;
+}
+
+double GraspPoseCalculator::evaluate_grasp_quality(
+    const tf2::Vector3& target_pos,
+    double angle_deg,
+    const std::string& arm
+) {
+    double score = 0.0;
+    bool is_right = (arm.find("right") != std::string::npos);
+    bool is_outward = (is_right && angle_deg < 0) || (!is_right && angle_deg > 0);
+    
+    if (is_outward) {
+        score += 50.0;
+    } else if (std::abs(angle_deg) < 1e-3) {
+        score += 10.0;
+    } else {
+        score -= 50.0;
+    }
+    
+    double abs_angle = std::abs(angle_deg);
+    if (abs_angle < 10.0) {
+        score -= 20.0;
+    } else if (abs_angle >= 20.0 && abs_angle <= 50.0) {
+        score += 30.0;
+    } else if (abs_angle > 60.0) {
+        score -= 20.0;
+    }
+    
+    tf2::Vector3 P_shoulder(0, 0, 0);
+    tf2::Vector3 V_sb = target_pos - P_shoulder;
+    tf2::Vector3 V_horiz_sb(0, V_sb.y(), V_sb.z());
+    
+    double base_angle = std::atan2(V_horiz_sb.z(), V_horiz_sb.y());
+    double rad = angle_deg * M_PI / 180.0;
+    double angle = base_angle + rad;
+    
+    tf2::Vector3 approach_vec(0, std::cos(angle), std::sin(angle));
+    tf2::Vector3 wrist_pos = target_pos - approach_vec * 0.2;
+    double dist_shoulder_wrist = wrist_pos.length();
+    
+    if (dist_shoulder_wrist > 0.8) {
+        score -= 100.0;
+    } else if (dist_shoulder_wrist < 0.2) {
+        score -= 100.0;
+    } else if (dist_shoulder_wrist >= 0.35 && dist_shoulder_wrist <= 0.65) {
+        score += 20.0;
+    }
+    return score;
+}
+
+std::vector<double> GraspPoseCalculator::find_optimal_grasp_angles(
+    const tf2::Vector3& target_pos,
+    const std::string& arm,
+    int top_k
+) {
+    std::vector<std::pair<double, double>> scored_angles;
+    std::vector<double> angles;
+    bool is_right = (arm.find("right") != std::string::npos);
+    if (is_right) {
+        for (double a = 0.0; a >= -60.0; a -= 5.0) angles.push_back(a);
+    } else {
+        for (double a = 0.0; a <= 60.0; a += 5.0) angles.push_back(a);
+    }
+    for (double angle : angles) {
+        double score = evaluate_grasp_quality(target_pos, angle, arm);
+        scored_angles.push_back({score, angle});
+    }
+    std::sort(scored_angles.begin(), scored_angles.end(), 
+        [](const std::pair<double, double>& a, const std::pair<double, double>& b) {
+            return a.first > b.first;
+        }
+    );
+    std::vector<double> result;
+    for (int i = 0; i < std::min((int)scored_angles.size(), top_k); ++i) {
+        result.push_back(scored_angles[i].second);
+    }
+    return result;
+}
+
+void GraspPoseCalculator::rm_arm_algo_init(void)
+{
+    rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
+    rm_force_type_e Type = RM_MODEL_RM_B_E;
+    rm_algo_init_sys_data(Mode, Type);
+    rm_arm_algo_init_done_ = true;
+}
+
+int GraspPoseCalculator::rm_arm_inverse_kinematics(
+    const tf2::Vector3& target_pos,
+    const tf2::Quaternion& target_quat,
+    std::vector<double>& joint_angles
+) {
+    if (!rm_arm_algo_init_done_) {
+        rm_arm_algo_init();
+    }
+
+    float joints[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+    if (joint_angles.size() < USED_ARM_DOF) {
+        joint_angles.resize(USED_ARM_DOF, 0.0);
+    }
+    for (int i = 0; i < USED_ARM_DOF; i++) {
+        joints[i] = static_cast<float>(joint_angles[i]);
+    }
+    rm_inverse_kinematics_params_t ik_params;
+    memcpy(ik_params.q_in, joints, sizeof(joints));
+    ik_params.q_pose.position.x = target_pos.x();
+    ik_params.q_pose.position.y = target_pos.y();
+    ik_params.q_pose.position.z = target_pos.z();
+    ik_params.q_pose.quaternion.x = target_quat.x();
+    ik_params.q_pose.quaternion.y = target_quat.y();
+    ik_params.q_pose.quaternion.z = target_quat.z();
+    ik_params.q_pose.quaternion.w = target_quat.w();
+    ik_params.flag = 0;
+
+#ifdef RM_ALGO_IK_DEBUG
+    RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"), 
+        "IK Target Pos: [%.4f, %.4f, %.4f], Quat: [%.4f, %.4f, %.4f, %.4f]",
+        ik_params.q_pose.position.x, ik_params.q_pose.position.y, ik_params.q_pose.position.z,
+        ik_params.q_pose.quaternion.x, ik_params.q_pose.quaternion.y,
+        ik_params.q_pose.quaternion.z, ik_params.q_pose.quaternion.w);
+    RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"), 
+        "IK Input Joints: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
+        joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]);
+#endif
+
+    int result = rm_algo_inverse_kinematics(NULL, ik_params, joints);
+    if (result != 0) {
+        RCLCPP_ERROR(rclcpp::get_logger("GraspPoseCalculator"), "Inverse kinematics failed with error code %d", result);
+        return ARM_AIM_CANNOT_REACH;
+    }
+    for (int i = 0; i < USED_ARM_DOF; i++) {
+        joint_angles[i] = static_cast<double>(joints[i]);
+    }
+    return 0;
+}
+
 } // namespace brain

src/brain/src/cerebellum_hooks.cpp

@@ -3,6 +3,9 @@
 #include "brain/payload_converters.hpp"
 #include "arm_action_define.h"
 #include <rclcpp/rclcpp.hpp>
+#include <sstream>
+#include <iomanip>
+#include <mutex>
 
 namespace brain
 {
@@ -500,6 +503,8 @@ void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
   node->skill_manager_->configure_action_hooks<interfaces::action::GripperCmd>("GripperCmd", std::move(hooks));
 }
 
+#define DEBUG_GRASP_POSE_CALCULATION
+
 void CerebellumHooks::ConfigureServiceHooks(CerebellumNode * node)
 {
   if (!node->skill_manager_) {
@@ -514,6 +519,139 @@ void CerebellumHooks::ConfigureServiceHooks(CerebellumNode * node)
 void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * node)
 {
   using VisionSrv = interfaces::srv::VisionObjectRecognition;
+
+  auto UpdateTargetPose = [node](const interfaces::msg::PoseClassAndID & obj, const std_msgs::msg::Header & header) -> bool {
+    if (node->target_frame_ == obj.class_name) {
+      node->target_pose_[node->target_frame_].header = header;
+      node->target_pose_[node->target_frame_].header.frame_id = "RightLink6";
+      node->target_pose_[node->target_frame_].pose = obj.pose;
+      // node->target_pose_[node->target_frame_].pose.position.x -= obj.grab_width / 2;
+      // node->target_pose_[node->target_frame_].pose.position.z -= 0.175;
+      node->grab_width_ = obj.grab_width;
+      RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose", node->target_frame_.c_str(), node->grab_width_);
+      return true;
+    } else {
+      RCLCPP_WARN(node->get_logger(), "object class_name[%s] not match target_frame_[%s]", 
+        obj.class_name.c_str(), node->target_frame_.c_str());
+      return false;
+    }
+  };
+
+  auto CalculateGraspPoses = [node](const std::string & skill_name) -> bool {
+    const auto & p = node->target_pose_[node->target_frame_].pose;
+
+    RCLCPP_INFO(node->get_logger(), "target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
+      p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
+
+    geometry_msgs::msg::PoseStamped pose_in_arm;
+    const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
+    if (!transformed) {
+      RCLCPP_WARN(node->get_logger(), "[%s] coordinate transformation failed, continuing with %s frame data", 
+        skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
+      return false;
+    }
+    
+    {
+      RCLCPP_INFO(node->get_logger(), "[%s] target pose transformed to %s frame", skill_name.c_str(), node->arm_target_frame_.c_str());
+      RCLCPP_INFO(node->get_logger(), "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", 
+        pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z,
+        pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
+
+      tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
+      tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
+
+      std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
+      if (best_angles.empty()) {
+        RCLCPP_WARN(node->get_logger(), "[%s]no valid grasp angles found", skill_name.c_str());
+        return false;
+      }
+
+      best_angles.clear();
+      best_angles.push_back(0);
+      
+      {
+        std::vector<std::string> palm_facings = {"up"}; //{"up", "down"};
+        std::vector<double> current_joint_angles;
+        {
+          std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
+          if ((node->arm_target_frame_.find("right") != std::string::npos)) {
+            current_joint_angles = node->right_arm_joint_angles_;
+          } else {
+            current_joint_angles = node->left_arm_joint_angles_;
+          }
+        }
+
+        for (double angle : best_angles) {
+          for (const auto& palm : palm_facings) {
+            auto result = brain::GraspPoseCalculator::calculate(target_pos, 
+              target_quat, "side", angle, palm, 0.06, 0.175, node->arm_target_frame_);
+
+            geometry_msgs::msg::Pose pre_grasp_msg;
+            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
+            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
+            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
+            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
+            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
+            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
+            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
+
+            std::vector<double> pre_grasp_joint_angles = current_joint_angles;
+            std::vector<double> grasp_joint_angles = current_joint_angles;
+
+#ifdef DEBUG_GRASP_POSE_CALCULATION
+            RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]", 
+              skill_name.c_str(), angle, palm.c_str(),
+              result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
+              result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
+#endif
+
+            if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position, 
+              result.pre_grasp_pose.orientation, pre_grasp_joint_angles) != 0) {
+              RCLCPP_WARN(node->get_logger(), "[%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.", 
+                skill_name.c_str(), angle, palm.c_str());
+              continue;
+            }
+
+            geometry_msgs::msg::Pose grasp_msg;
+            grasp_msg.position.x = result.grasp_pose.position.x();
+            grasp_msg.position.y = result.grasp_pose.position.y();
+            grasp_msg.position.z = result.grasp_pose.position.z();
+            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
+            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
+            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
+            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
+
+#ifdef DEBUG_GRASP_POSE_CALCULATION
+            RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]", 
+              skill_name.c_str(), angle, palm.c_str(),
+              result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
+              result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
+#endif
+
+            if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.grasp_pose.position,
+              result.grasp_pose.orientation, grasp_joint_angles) != 0) {
+              RCLCPP_WARN(node->get_logger(), "[%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.", 
+                skill_name.c_str(), angle, palm.c_str());
+              continue;
+            }
+
+            node->pre_grasp_poses_.push_back(pre_grasp_msg);
+            node->grasp_poses_.push_back(grasp_msg);
+
+            RCLCPP_INFO(node->get_logger(), "Generated Pose (Angle: %.1f, Palm: %s)", angle, palm.c_str());
+            RCLCPP_INFO(node->get_logger(), "  Pre: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
+              result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
+              result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
+            RCLCPP_INFO(node->get_logger(), "  Grasp: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
+              result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
+              result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
+          }
+        }
+      }
+    }
+    return true;
+  };
+
   {
     SkillManager::ServiceHookOptions<VisionSrv> hooks;
     hooks.wait_timeout = [](const std::string &) {
@@ -539,7 +677,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
 
         return req;
       };
-    hooks.on_response = [node](
+    hooks.on_response = [node, UpdateTargetPose, CalculateGraspPoses](
         const std::string & skill_name,
         const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
         const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
@@ -557,153 +695,29 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
           skill_name.c_str(), resp->success, resp->objects.size(), 
           resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
 
-        node->side_pre_grasp_poses_.clear();
-        node->side_grasp_poses_.clear();
-        node->top_pre_grasp_poses_.clear();
-        node->top_grasp_poses_.clear();
+        node->pre_grasp_poses_.clear();
+        node->grasp_poses_.clear();
 
         for (size_t i = 0; i < resp->objects.size(); ++i) {
           const auto & obj = resp->objects[i];
-          if (node->target_frame_ == obj.class_name) {
-            node->target_pose_[node->target_frame_].header = resp->header;
-            node->target_pose_[node->target_frame_].header.frame_id = "RightLink6";
-            node->target_pose_[node->target_frame_].pose = obj.pose;
-            node->target_pose_[node->target_frame_].pose.position.x -=  obj.grab_width/2;
-            // node->target_pose_[node->target_frame_].pose.position.z -= 0.19;
-            node->grab_width_ = obj.grab_width;
-            RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose", node->target_frame_.c_str(), node->grab_width_);
+          RCLCPP_INFO(node->get_logger(), "  [%zu] class='%s' id=%d", i, obj.class_name.c_str(), obj.class_id);
+          if (!UpdateTargetPose(obj, resp->header)) {
+            continue;
           }
-
-          RCLCPP_INFO(node->get_logger(), "  [%zu] class='%s' id=%d", i,
-            obj.class_name.c_str(), obj.class_id);
-
-          const auto & p = obj.pose;
-          RCLCPP_INFO(node->get_logger(),
-            "target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-            p.position.x, p.position.y, p.position.z,
-            p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
-
-          geometry_msgs::msg::PoseStamped pose_in_arm;
-          const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
-          if (!transformed) {
-            RCLCPP_WARN(node->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
-              skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
-            return false;
-          } else {
-            RCLCPP_INFO(node->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
-              skill_name.c_str(), node->arm_target_frame_.c_str());
-
-            RCLCPP_INFO(node->get_logger(),
-              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-              pose_in_arm.pose.position.x,
-              pose_in_arm.pose.position.y,
-              pose_in_arm.pose.position.z,
-              pose_in_arm.pose.orientation.x,
-              pose_in_arm.pose.orientation.y,
-              pose_in_arm.pose.orientation.z,
-              pose_in_arm.pose.orientation.w);
-
-            tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
-            tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, 
-              pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
-
-            auto result = brain::GraspPoseCalculator::calculate(
-                target_pos, 
-                target_quat, 
-                "side", 
-                0.07, // safety_dist
-                0.17   // finger_length
-            );
-
-            geometry_msgs::msg::Pose pre_grasp_msg;
-            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
-            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
-            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
-            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
-            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
-            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
-            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
-            node->side_pre_grasp_poses_.push_back(pre_grasp_msg);
-
-            geometry_msgs::msg::Pose grasp_msg;
-            grasp_msg.position.x = result.grasp_pose.position.x();
-            grasp_msg.position.y = result.grasp_pose.position.y();
-            grasp_msg.position.z = result.grasp_pose.position.z();
-            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
-            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
-            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
-            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
-            node->side_grasp_poses_.push_back(grasp_msg);
-
-            RCLCPP_INFO(node->get_logger(), "Pre Grasp Pose(side): ");
-            RCLCPP_INFO(node->get_logger(),
-              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-              result.pre_grasp_pose.position.x(),
-              result.pre_grasp_pose.position.y(),
-              result.pre_grasp_pose.position.z(),
-              result.pre_grasp_pose.orientation.x(),
-              result.pre_grasp_pose.orientation.y(),
-              result.pre_grasp_pose.orientation.z(),
-              result.pre_grasp_pose.orientation.w());
-            RCLCPP_INFO(node->get_logger(), "Grasp Pose: ");
-            RCLCPP_INFO(node->get_logger(),
-              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-              result.grasp_pose.position.x(),
-              result.grasp_pose.position.y(),
-              result.grasp_pose.position.z(),
-              result.grasp_pose.orientation.x(),
-              result.grasp_pose.orientation.y(),
-              result.grasp_pose.orientation.z(),
-              result.grasp_pose.orientation.w());
-
-            result = brain::GraspPoseCalculator::calculate(
-                target_pos, 
-                target_quat, 
-                "top", 
-                0.02, // safety_dist
-                0.0   // finger_length
-            );
-
-            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
-            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
-            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
-            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
-            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
-            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
-            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
-            node->top_pre_grasp_poses_.push_back(pre_grasp_msg);
-
-            grasp_msg.position.x = result.grasp_pose.position.x();
-            grasp_msg.position.y = result.grasp_pose.position.y();
-            grasp_msg.position.z = result.grasp_pose.position.z();
-            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
-            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
-            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
-            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
-            node->top_grasp_poses_.push_back(grasp_msg);
-
-            RCLCPP_INFO(node->get_logger(), "Pre Grasp Pose(top): ");
-            RCLCPP_INFO(node->get_logger(),
-              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-              result.pre_grasp_pose.position.x(),
-              result.pre_grasp_pose.position.y(),
-              result.pre_grasp_pose.position.z(),
-              result.pre_grasp_pose.orientation.x(),
-              result.pre_grasp_pose.orientation.y(),
-              result.pre_grasp_pose.orientation.z(),
-              result.pre_grasp_pose.orientation.w());
-            RCLCPP_INFO(node->get_logger(), "Grasp Pose: ");
-            RCLCPP_INFO(node->get_logger(),
-              "%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
-              result.grasp_pose.position.x(),
-              result.grasp_pose.position.y(),
-              result.grasp_pose.position.z(),
-              result.grasp_pose.orientation.x(),
-              result.grasp_pose.orientation.y(),
-              result.grasp_pose.orientation.z(),
-              result.grasp_pose.orientation.w());
+          if (!CalculateGraspPoses(skill_name)) {
+            continue;
           }
         }
+
+        if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
+          RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
+          return false;
+        }
+        if (node->pre_grasp_poses_.size() != node->grasp_poses_.size()) {
+          RCLCPP_WARN(node->get_logger(), "[%s] pre_grasp_poses_ size %zu not match grasp_poses_ size %zu",
+            skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
+          return false;
+        }
         return true;
       };
     node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));

src/brain/src/cerebellum_node.cpp

@@ -13,6 +13,7 @@
 #include <cmath>
 #include <future>
 #include <iostream>
+#include <iomanip>
 #include <limits>
 #include <memory>
 #include <cstdint>
@@ -87,6 +88,9 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
   tf_buffer_->setUsingDedicatedThread(true);
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
+  joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
+    "/joint_states", 10, std::bind(&CerebellumNode::JointStateCallback, this, std::placeholders::_1));
+
   action_registry_ = std::make_unique<ActionServerRegistry>(this);
   action_clients_ = std::make_unique<ActionClientRegistry>(this);
   skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);
@@ -111,16 +115,6 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
 #endif
 }
 
-
-
-
-
-
-
-
-
-
-
 /**
  * @brief Declare all configurable ROS parameters and load their initial values.
  *
@@ -883,7 +877,7 @@ void CerebellumNode::LogStatsPeriodic()
   for (const auto & kv : skill_success_counts_) {
     size_t fails = skill_failure_counts_[kv.first];
     RCLCPP_INFO(
-      this->get_logger(), "  skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
+      this->get_logger(), "skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
       fails);
   }
   if (stats_pub_) {
@@ -1278,4 +1272,31 @@ brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequenceConcurrent(
   return result;
 }
 
+void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lk(joint_state_mutex_);
+  if (msg->position.size() >= 21) {
+    left_arm_joint_angles_.clear();
+    // left_arm_subset = positions[9:15] -> indices 9 to 14
+    for (size_t i = 9; i < 15; ++i) {
+      double angle_deg = msg->position[i] * 180.0 / 3.14;
+      left_arm_joint_angles_.push_back(angle_deg);
+    }
+    right_arm_joint_angles_.clear();
+    // right_arm_subset = positions[15:21] -> indices 15 to 20
+    for (size_t i = 15; i < 21; ++i) {
+      double angle_deg = msg->position[i] * 180.0 / 3.14;
+      right_arm_joint_angles_.push_back(angle_deg);
+    }
+#ifdef DEBUG_JOINT_STATE
+    RCLCPP_INFO(this->get_logger(), "Received left arm joint angle[%f, %f, %f, %f, %f, %f] ", 
+      left_arm_joint_angles_[0], left_arm_joint_angles_[1], left_arm_joint_angles_[2], 
+      left_arm_joint_angles_[3], left_arm_joint_angles_[4], left_arm_joint_angles_[5]);
+    RCLCPP_INFO(this->get_logger(), "Received right arm joint angle[%f, %f, %f, %f, %f, %f] ", 
+      right_arm_joint_angles_[0], right_arm_joint_angles_[1], right_arm_joint_angles_[2], 
+      right_arm_joint_angles_[3], right_arm_joint_angles_[4], right_arm_joint_angles_[5]);
+#endif
+  }
+}
+
 }  // namespace brain

src/thirdparty/rm_arm/include/arm_define.h

@@ -0,0 +1,65 @@
+#ifndef ARM_DEFINE_H
+#define ARM_DEFINE_H
+
+#include "arm_action_define.h"
+
+#define MAX_ARM_GOAL_COUNT 16
+#define MAX_TRAJECTORY_HISTORY 64
+#define ARM_FOLLOWING_PERIOD (5e6) //ns == 5ms
+#define ARM_FOLLOWING_CHECKING_STEP (4)
+#define ARM_FOLLOWING_CHECKING (ARM_FOLLOWING_PERIOD * ARM_FOLLOWING_CHECKING_STEP) //ns == 20ms
+#define GET_FUNC_LINE_STR() \
+    (std::string(__func__ ) + ":" + std::to_string(__LINE__))
+
+#define LEFT_ARM_IP_ADDRESS "192.168.2.2"
+#define RIGHT_ARM_IP_ADDRESS "192.168.2.18"
+#define ARM_IP_PORT (8080)
+
+#define TRAJECTORY_COMPLETE 1
+#define TRAJECTORY_ONGOING 0
+#define TRAJECTORY_ERROR -1
+
+/*typedef enum {
+    ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0,
+    ARM_COMMAND_TYPE_POSE_STEP_ON,
+    ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE,
+    ARM_COMMAND_TYPE_POSE_DIRECT_MOVE,
+    ARM_COMMAND_TYPE_ERR
+} ArmCommandTypeE;
+
+typedef enum {
+    POSE_POSITION_X = 0,
+    POSE_POSITION_Y,
+    POSE_POSITION_Z,
+    POSE_QUATERNION_X,
+    POSE_QUATERNION_Y,
+    POSE_QUATERNION_Z,
+    POSE_QUATERNION_W,
+    POSE_DIMENSION, // should be 7
+} PoseDimensionE;
+
+typedef enum {
+    LEFT_ARM = 0,
+    RIGHT_ARM,
+    ARM_ID_ERR
+} ArmIdE;
+ 
+
+typedef enum {
+    OK = 0,
+    UNKNOWN_ERR = -1,
+    ARM_NOW_FORCE_MOVING = -2,
+    ARM_COLLISION = -3,
+    ARM_AIM_CANNOT_REACH = -4,
+    ARM_NOW_NO_GOAL = -5,
+    ARM_GOAL_CANCELLED = -6,
+} ErrCodeE;*/
+
+typedef enum {
+    GOAL_TYPE_MOVING = 0,
+    GOAL_TYPE_STEPPING,
+    GOAL_TYPE_POSE_STEPPING,
+    GOAL_TYPE_ERROR
+} GoalTypeE;
+
+#endif // ARM_DEFINE_H

src/thirdparty/rm_arm/include/rm_interface_global.h

@@ -0,0 +1,16 @@
+#ifndef RM_INTERFACE_GLOBAL_H
+#define RM_INTERFACE_GLOBAL_H
+
+#ifdef __linux
+#define RM_INTERFACE_EXPORT
+#endif
+
+#if _WIN32
+#if defined(RM_INTERFACE_LIBRARY)
+#  define RM_INTERFACE_EXPORT __declspec(dllexport)
+#else
+#  define RM_INTERFACE_EXPORT __declspec(dllexport)
+#endif
+#endif
+
+#endif // RM_INTERFACE_GLOBAL_H

src/thirdparty/rm_arm/include/rm_version.h

@@ -0,0 +1,16 @@
+
+#ifndef RM_VERSION_H
+#define RM_VERSION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define SDK_VERSION ("1.1.1")
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+