添加行为树xml注释

优化robot work info处理
Refactor IK client: move from GraspPoseCalculator to CerebellumNode
2026-02-03 09:21:44 +08:00 · 2026-01-29 18:45:19 +08:00 · 2026-01-28 15:29:30 +08:00 · 2026-01-27 18:51:29 +08:00 · 2026-01-22 17:31:06 +08:00 · 2026-01-19 17:55:49 +08:00
63 changed files with 6470 additions and 1307 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,7 @@
 build/
 install/
 log/
+logs/

 .vscode/
 .venv/
--- a/src/brain/CMakeLists.txt
+++ b/src/brain/CMakeLists.txt
@@ -14,7 +14,6 @@ find_package(rclcpp_components REQUIRED)
 # find_package(<dependency> REQUIRED)

 find_package(interfaces REQUIRED)
-find_package(brain_interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(smacc2 REQUIRED)
 find_package(smacc2_msgs REQUIRED)
@@ -35,13 +34,17 @@ find_package(tf2_geometry_msgs REQUIRED)
 add_executable(brain_node
  src/brain_node.cpp
  src/cerebellum_node.cpp
+  src/cerebellum_hooks.cpp
  src/cerebrum_node.cpp
  src/skill_manager.cpp
+  src/calc_grasp_pose.cpp
 )

 target_include_directories(brain_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  interfaces/include
+)

 target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

@@ -51,7 +54,6 @@ ament_target_dependencies(brain_node
  rclcpp_action
  behaviortree_cpp
  interfaces
-  brain_interfaces
  std_msgs
  smacc2
  smacc2_msgs
@@ -76,6 +78,11 @@ install(TARGETS brain_node
 install(DIRECTORY config/
  DESTINATION share/${PROJECT_NAME}/config)

+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch)
+
+set (BUILD_TESTING OFF)
+
 if(BUILD_TESTING)
  if(NOT DEFINED ENV{SKIP_LINT})
    find_package(ament_lint_auto REQUIRED)
@@ -103,7 +110,6 @@ if(BUILD_TESTING)
      rclcpp
      rclcpp_action
      interfaces
-      brain_interfaces
      nav2_msgs
      std_srvs
      tf2
@@ -158,14 +164,15 @@ if(BUILD_TESTING)
      smacc2_msgs
      ament_index_cpp
      interfaces
-      brain_interfaces
      nav2_msgs
      std_srvs
      tf2
      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
+    )
    target_link_libraries(test_cerebellum_node Boost::thread)
    target_link_libraries(test_cerebellum_node yaml-cpp)
  endif()
@@ -183,7 +190,6 @@ if(BUILD_TESTING)
      rclcpp_action
      behaviortree_cpp
      interfaces
-      brain_interfaces
      smacc2
      smacc2_msgs
      ament_index_cpp
@@ -193,7 +199,9 @@ 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
+    )
    target_link_libraries(test_cerebrum_node Boost::thread)
    target_link_libraries(test_cerebrum_node yaml-cpp)
  endif()
@@ -208,14 +216,16 @@ if(BUILD_TESTING)
      smacc2
      smacc2_msgs
      interfaces
-      brain_interfaces
      nav2_msgs
      std_srvs
      tf2
      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
+    )
+    target_link_libraries(test_sm_cerebellum)
  endif()

  # ExecuteBtAction end-to-end single skill test
@@ -231,7 +241,6 @@ if(BUILD_TESTING)
      rclcpp_action
      behaviortree_cpp
      interfaces
-      brain_interfaces
      smacc2
      smacc2_msgs
      ament_index_cpp
@@ -241,7 +250,9 @@ if(BUILD_TESTING)
      tf2_ros
      tf2_geometry_msgs
    )
-    target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    target_include_directories(test_execute_bt_action PRIVATE
+      ${CMAKE_CURRENT_SOURCE_DIR}/include
+    )
    target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
  endif()

@@ -258,7 +269,6 @@ if(BUILD_TESTING)
      rclcpp_action
      behaviortree_cpp
      interfaces
-      brain_interfaces
      smacc2
      smacc2_msgs
      ament_index_cpp
@@ -268,7 +278,9 @@ if(BUILD_TESTING)
      tf2_ros
      tf2_geometry_msgs
    )
-    target_include_directories(test_execute_bt_action_extended PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+    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)
  endif()

@@ -283,8 +295,10 @@ if(BUILD_TESTING)
    if(TARGET ${test_name})
      target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
      ament_target_dependencies(${test_name}
-        rclcpp rclcpp_action behaviortree_cpp interfaces brain_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)
+        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)
    endif()
  endforeach()
@@ -298,8 +312,10 @@ if(BUILD_TESTING)
  if(TARGET test_cerebrum_utils)
    target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
    ament_target_dependencies(test_cerebrum_utils
-      rclcpp rclcpp_action behaviortree_cpp interfaces brain_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)
+      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)
  endif()
 endif()
--- a/src/brain/config/brain_node_params.yaml
+++ b/src/brain/config/brain_node_params.yaml
@@ -0,0 +1,115 @@
+# Configuration file for brain node parameters
+# This file contains configurable parameters for cerebrum and cerebellum nodes
+
+cerebrum_node:
+  ros__parameters:
+    # Timing parameters
+    tick_rate_hz: 10.0
+    rebuild_interval_ms: 5000
+    stats_publish_interval_ms: 10000
+    
+    # Configuration paths
+    # robot_config_path: ""
+    # skill_file_path: ""
+    
+    # Behavior tree parameters
+    default_bt_timeout: 300.0
+    enable_sequence_rebuild: true
+    
+    # Original cerebrum parameters
+    node_timeout_sec: 60.0
+    allow_bt_rebuild_during_execution: false
+    allowed_action_skills: ""
+    fixed_skill_sequence: ""
+    random_skill_pick_count: 5
+    
+    # Dynamic skill parameters (examples - these would be set per skill)
+    # cerebrum:
+    #   bt:
+    #     VisionObjectRecognition:
+    #       topic: "vision_object_recognition"
+    #       payload_yaml: ""
+    #       timeout_ms: "10000"
+    #     Arm:
+    #       topic: "arm_control"
+    #       payload_yaml: ""
+    #       timeout_ms: "30000"
+    #     # Example instance-specific overrides
+    #     Arm:
+    #       s1_arm_move_to_object:
+    #         topic: "arm_control_specific"
+    #         payload_yaml: "{target_pose: {x: 0.1, y: 0.2, z: 0.3}}"
+    #         timeout_ms: "45000"
+
+cerebellum_node:
+  ros__parameters:
+    # Action execution parameters
+    abort_on_failure: true
+    default_action_timeout_sec: 45.0
+    vision_grasp_object_timeout_sec: 120.0
+    
+    # Map parameters
+    map_save_url: "/tmp/humanoid_map"
+    map_save_image_format: "pgm"
+    map_save_free_thresh: 0.25
+    map_save_occupied_thresh: 0.65
+    map_load_url: ""
+    nav_goal_yaw_offset: 0.0
+    nav_goal_distance_tolerance: 0.5
+    slam_enable_mapping_default: true
+    
+    # Configuration paths
+    # robot_config_path: ""
+    # robot_skill_file_path: ""
+    
+    # Statistics parameters
+    enable_stats_publisher: true
+    stats_publish_interval_ms: 5000
+    
+    # Skill-specific timeouts (format: "SkillA=30,SkillB=10")
+    skill_timeouts: ""
+    
+    # Arm-specific parameters
+    arm_target_frame: "base_link_rightarm"  # 右臂基座坐标系
+    arm_transform_timeout_sec: 2.0          # 坐标变换超时时间
+
+    # Grasp pose calculation parameters
+    top_cam_right_arm_z_threshold: 0.05   #满足右臂抓取的头部相机输出的z坐标阈值
+    top_cam_right_arm_x_offset: 0.10      #右臂相机视觉识别时的x坐标偏移
+    top_cam_right_arm_y: 0.33             #右臂相机视觉识别时的y坐标
+    top_cam_right_arm_orientation: [-0.7071, 0.0, 0.0, 0.7071]    #右臂相机视觉识别时的四元数
+    top_cam_left_arm_z_threshold: -0.10   #满足左臂抓取的头部相机输出的z坐标阈值
+    top_cam_left_arm_x_offset: 0.10       #左臂相机视觉识别时的x坐标偏移
+    top_cam_left_arm_y: -0.33             #左臂相机视觉识别时的y坐标
+    top_cam_left_arm_orientation: [0.7071, 0.0, 0.0, 0.7071]      #左臂相机视觉识别时的四元数
+    side_cam_right_arm_y_offset: 0.0      # 补偿右臂y轴方向的抓取偏差（前后）
+    side_cam_right_arm_z_offset: 0.02     # 补偿右臂z轴方向的抓取偏差（左右）
+    side_cam_left_arm_y_offset: 0.02       # 补偿左臂y轴方向的抓取偏差（前后）
+    side_cam_left_arm_z_offset: 0.01      # 补偿左臂z轴方向的抓取偏差（左右）
+    take_object_arm_x: -0.05              # 抓取物体时x坐标
+    left_camera_frame: "LeftLink6"        # 左臂相机坐标系
+    right_camera_frame: "RightLink6"      # 右臂相机坐标系
+    target_frames: ["apple", "bottle", "medicine_box"]  # 识别物体的列表
+    target_frame: "medicine_box"          # 默认识别物体
+    grasp_palm_facings: ["down"]          # 抓取时手掌朝向
+    grasp_best_angles: [0.0]              # 抓取时最佳角度
+    grasp_type: "side"                    # 抓取类型
+    grasp_safety_dist: 0.10               # 抓取安全距离
+    grasp_finger_length: 0.17             # 抓取手指长度
+    hand_control_default_mode: 1          # 手控制默认模式
+    gripper_default_loc: 30               # 夹爪默认位置
+    arm_cmd_type_take_photo: 100          # 拍照
+    arm_cmd_type_pre_grasp: 101           # 预抓取
+    arm_cmd_type_grasp: 102               # 抓取
+    arm_cmd_type_take_object: 110         # 拿取动作
+    arm_cmd_type_custom_min: 100          # 自定义动作最小值
+    arm_cmd_type_custom_max: 120          # 自定义动作最大值
+    gripper_default_speed: 0              # 夹爪默认速度
+    gripper_default_torque: 0             # 夹爪默认力矩
+    gripper_default_mode: 0               # 夹爪默认模式
+
+    # Service hook timeouts (seconds)
+    vision_object_recognition_wait_timeout_sec: 2.0   # 物体识别服务调用超时时间
+    vision_object_recognition_call_timeout_sec: 5.0   # 物体识别服务等待超时时间
+    map_load_wait_timeout_sec: 2.0        # 地图加载服务调用超时时间
+    map_load_call_timeout_sec: 5.0        # 地图加载服务等待超时时间
--- a/src/brain/config/bt_carry_boxes_sch1.params.yaml
+++ b/src/brain/config/bt_carry_boxes_sch1.params.yaml
@@ -11,7 +11,7 @@

    '
 - name: s1_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -21,11 +21,11 @@

    frame_time_stamp: 0

-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s2_arm_move_to_snapshot_pose
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -35,17 +35,17 @@

    frame_time_stamp: 0

-    data_array: [-85, -35, -40, 1, -70, -180, -95, 35, 40, -1, 70, -10]
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]

    '
 - name: s3_wheel_move_to_pickup_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5

    move_angle: 0.0

    '
- name: s4_arm_grasp_box
-  params: 'body_id: 0
+- name: s3_snapshot_action1
+  params: 'body_id: 2

    data_type: 2

@@ -55,11 +55,95 @@

    frame_time_stamp: 0

-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-85, -35, -60, 1, -90, -175, -95, 35, 60, -1, 90, -5]
+    
+    '
+- name: s3_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -95, 40, 40, -1, 70, -10]
+    
+    '
+- name: s3_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s3_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 70, 35, -40, 75, -60]
+    
+    '
+- name: s3_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s4_arm_prepare_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
+
+    '
+- name: s4_arm_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]

    '
 - name: s5_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -69,7 +153,7 @@

    frame_time_stamp: 0

-    data_array: [-110, -70, -25, 1, -85, -170, -70, 70, 25, -1, 85, -10]
+    data_array: [-110, -81.8, 0, 0, -100, -80, -70, 81.8, 0, 0, 100, -100]

    '
 - name: s6_wheel_move_back_to_second_position
@@ -85,13 +169,13 @@

    '
 - name: s8_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 37.0

    move_yaw_degree: 0

    '
 - name: s9_arm_put_down_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -101,11 +185,11 @@

    frame_time_stamp: 0

-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]

    '
 - name: s10_arm_release
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -115,11 +199,11 @@

    frame_time_stamp: 0

-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]

    '
 - name: s10_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -129,13 +213,13 @@

    frame_time_stamp: 0

-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]

    '
- name: s11_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s11_waist_bend_up
+  params: 'move_pitch_degree: -37.0

-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0

    '
 - name: s12_waist_turn_around
@@ -145,7 +229,7 @@

    '
 - name: s13_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -155,11 +239,11 @@

    frame_time_stamp: 0

-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s14_arm_move_to_snapshot_pose
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -169,17 +253,45 @@

    frame_time_stamp: 0

-    data_array: [-85, -35, -40, 1, -70, -180, -95, 35, 40, -1, 70, -10]
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
+
+    '
+- name: s14_arm_move_pre_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s14_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s15_wheel_move_to_pickup_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5

    move_angle: 0.0

    '
- name: s16_arm_grasp_box
-  params: 'body_id: 0
+- name: s15_snapshot_action1
+  params: 'body_id: 2

    data_type: 2

@@ -189,11 +301,95 @@

    frame_time_stamp: 0

-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-85, -35, -60, 1, -90, -175, -95, 35, 60, -1, 90, -5]
+    
+    '
+- name: s15_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -95, 40, 40, -1, 70, -10]
+    
+    '
+- name: s15_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s15_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 70, 35, -40, 75, -60]
+    
+    '
+- name: s15_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-85, -40, -40, 1, -70, -170, -55, 50, 35, -40, 95, -60]
+    
+    '
+- name: s16_arm_prepare_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]
+
+    '
+- name: s16_arm_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]

    '
 - name: s17_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -203,7 +399,7 @@

    frame_time_stamp: 0

-    data_array: [-110, -70, -25, 1, -85, -170, -70, 70, 25, -1, 85, -10]
+    data_array: [-110, -81.8, 0, 0, -100, -80, -70, 81.8, 0, 0, 100, -100]

    '
 - name: s18_wheel_move_back_to_origin
@@ -219,13 +415,13 @@

    '
 - name: s20_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 37.0

    move_yaw_degree: 0

    '
 - name: s21_arm_put_down_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -235,11 +431,11 @@

    frame_time_stamp: 0

-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]

    '
 - name: s22_arm_release
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -249,11 +445,11 @@

    frame_time_stamp: 0

-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]

    '
 - name: s22_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -263,13 +459,13 @@

    frame_time_stamp: 0

-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -78, 0, 0, -100, -65, -70, 78, 0, 0, 100, -115]

    '
- name: s23_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s23_waist_bend_up
+  params: 'move_pitch_degree: -37.0

-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0

    '
 - name: s24_waist_turn_around
@@ -279,7 +475,7 @@

    '
 - name: s25_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -289,7 +485,7 @@

    frame_time_stamp: 0

-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s26_wheel_move_back_to_origin
@@ -297,4 +493,32 @@

    move_angle: 0.0

+    '
+- name: s26_arm_move_pre_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s26_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]
+
    '
--- a/src/brain/config/bt_carry_boxes_sch1.xml
+++ b/src/brain/config/bt_carry_boxes_sch1.xml
@@ -1,37 +1,62 @@
+<!--
+  功能描述: 机器人搬运箱子流程1（单臂+底盘，3次重试）。
+  流程: 清理完成标记 -> 机械臂回零 -> 底盘到抓取点 -> 多组拍照位与抓取位动作 -> 抓取抬起
+        -> 底盘到放置点 -> 腰部下俯与放置并行 -> 松爪 -> 抬臂与腰部上仰并行 -> 回初始位
+        -> 第二次抓取放置循环 -> 回原点。
+-->
 <root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <Sequence name="root">
      <!-- Retry waist + arm + hand until HandControl_H succeeds -->
-      <RetryUntilSuccessful name="retry_all_action" num_attempts="5">
+      <RetryUntilSuccessful name="retry_all_action" num_attempts="3">
        <Sequence>
+          <ClearDoneInstances_H name="clear_done_instances_retry" />
          <!-- <MoveHome_H name="s0_motion_move_home" /> -->
-          <!-- <Arm_H name="s1_arm_move_origin_position" /> -->
-          <Arm_H name="s2_arm_move_to_snapshot_pose" />
+          <Arm_H name="s1_arm_move_origin_position" />
          <MoveWheel_H name="s3_wheel_move_to_pickup_position" />
+          <Arm_H name="s2_arm_move_to_snapshot_pose" />
+          <Arm_H name="s3_snapshot_action1" />
+          <Arm_H name="s3_snapshot_action2" />
+          <Arm_H name="s3_snapshot_top_action1" />
+          <Arm_H name="s3_snapshot_top_action2" />
+          <Arm_H name="s3_snapshot_top_action3" />
+          <Arm_H name="s4_arm_prepare_grasp_box" />
          <Arm_H name="s4_arm_grasp_box" />
          <Arm_H name="s5_arm_pickup_box" />
          <MoveWheel_H name="s6_wheel_move_back_to_second_position" />
-          <MoveWaist_H name="s7_waist_turn_around" />
-          <MoveWaist_H name="s8_waist_bend_down" />
-          <Arm_H name="s9_arm_put_down_box" />
+          <Parallel name="parallel_action_3">
+            <MoveWaist_H name="s8_waist_bend_down" />
+            <Arm_H name="s9_arm_put_down_box" />
+          </Parallel>
          <Arm_H name="s10_arm_release" />
-          <Arm_H name="s10_arm_raise_up" />
-          <MoveWaist_H name="s11_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s12_waist_turn_around" /> -->
-          <!-- <Arm_H name="s13_arm_move_origin_position" /> -->
-          <Arm_H name="s14_arm_move_to_snapshot_pose" />
+          <Parallel name="parallel_action_4">
+            <Arm_H name="s10_arm_raise_up" />
+            <MoveWaist_H name="s11_waist_bend_up" />
+          </Parallel>
+          <Arm_H name="s14_arm_move_pre_origin_position" />
+          <Arm_H name="s14_arm_move_origin_position" />
          <MoveWheel_H name="s15_wheel_move_to_pickup_position" />
+          <Arm_H name="s14_arm_move_to_snapshot_pose" />
+          <Arm_H name="s15_snapshot_action1" />
+          <Arm_H name="s15_snapshot_action2" />
+          <Arm_H name="s15_snapshot_top_action1" />
+          <Arm_H name="s15_snapshot_top_action2" />
+          <Arm_H name="s15_snapshot_top_action3" />
+          <Arm_H name="s16_arm_prepare_grasp_box" />
          <Arm_H name="s16_arm_grasp_box" />
          <Arm_H name="s17_arm_pickup_box" />
          <MoveWheel_H name="s18_wheel_move_back_to_origin" />
-          <MoveWaist_H name="s19_waist_turn_around" />
-          <MoveWaist_H name="s20_waist_bend_down" />
-          <Arm_H name="s21_arm_put_down_box" />
+          <Parallel name="parallel_action_7">
+            <MoveWaist_H name="s20_waist_bend_down" />
+            <Arm_H name="s21_arm_put_down_box" />
+          </Parallel>
          <Arm_H name="s22_arm_release" />
-          <Arm_H name="s22_arm_raise_up" />
-          <MoveWaist_H name="s23_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s24_waist_turn_around" /> -->
-          <!-- <Arm_H name="s25_arm_move_origin_position" /> -->
+          <Parallel name="parallel_action_8">
+            <Arm_H name="s22_arm_raise_up" />
+            <MoveWaist_H name="s23_waist_bend_up" />
+          </Parallel>
+            <Arm_H name="s26_arm_move_pre_origin_position" />
+          <Arm_H name="s26_arm_move_origin_position" />
          <MoveWheel_H name="s26_wheel_move_back_to_origin" />
        </Sequence>
      </RetryUntilSuccessful>
--- a/src/brain/config/bt_carry_boxes_sch2.params.yaml
+++ b/src/brain/config/bt_carry_boxes_sch2.params.yaml
@@ -7,7 +7,7 @@

    '
 - name: s1_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -17,7 +17,7 @@

    frame_time_stamp: 0

-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s2_wheel_move_to_origin_pickup_position
@@ -33,13 +33,13 @@

    '
 - name: s4_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 38.0

    move_yaw_degree: 0.0

    '
- name: s5_arm_prepare_to_grasp
-  params: 'body_id: 0
+- name: s4_arm_move_pre_origin_position
+  params: 'body_id: 2

    data_type: 2

@@ -49,11 +49,109 @@

    frame_time_stamp: 0

-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s4_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s4_snapshot_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-105, -55, -35, 0, -110, -160, -75, 55, 35, 0, 110, -20]
+
+    '
+- name: s4_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s4_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s4_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 80, 35, -40, 65, -55]
+
+    '
+- name: s4_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s5_arm_prepare_to_grasp
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-110, -75, 0, 0, -100, -65, -70, 75, 0, 0, 100, -115]

    '
 - name: s6_arm_grasp_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -63,11 +161,11 @@

    frame_time_stamp: 0

-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]

    '
 - name: s7_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -77,13 +175,13 @@

    frame_time_stamp: 0

-    data_array: [-110, -70, -25, 1, -85, -150, -70, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]

    '
- name: s8_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s8_waist_bend_up
+  params: 'move_pitch_degree: -38.0

-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0

    '
 - name: s9_waist_turn_around
@@ -93,13 +191,13 @@

    '
 - name: s10_wheel_move_to_putdown_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5

    move_angle: 0.0

    '
 - name: s11_arm_putdown_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -109,11 +207,11 @@

    frame_time_stamp: 0

-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]

    '
 - name: s12_arm_release_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -123,11 +221,11 @@

    frame_time_stamp: 0

-    data_array: [-80, -60, -25, 1, -85, -170, -100, 60, 25, -1, 85, -10]
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]

    '
 - name: s12_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -137,7 +235,35 @@

    frame_time_stamp: 0

-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -75, 0, 0, -100, -80, -70, 75, 0, 0, 100, -100]
+
+    '
+- name: s13_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
+
+    '
+- name: s13_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s13_wheel_move_to_origin_pickup_position
@@ -159,13 +285,13 @@

    '
 - name: s15_waist_bend_down
-  params: 'move_pitch_degree: 25.0
+  params: 'move_pitch_degree: 38.0

    move_yaw_degree: 0.0

    '
- name: s16_arm_prepare_to_grasp
-  params: 'body_id: 0
+- name: s15_arm_move_pre_origin_position
+  params: 'body_id: 2

    data_type: 2

@@ -175,11 +301,109 @@

    frame_time_stamp: 0

-    data_array: [-85, -60, -25, 1, -85, -150, -95, 60, 25, -1, 85, -30]
+    data_array: [-30, -55, -30, 0, -100, -65, -150, 55, 30, 0, 100, -115]
+
+    '
+- name: s15_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s15_snapshot_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-105, -55, -35, 0, -110, -160, -75, 55, 35, 0, 110, -20]
+
+    '
+- name: s15_snapshot_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -68, 70, 0, 0, 80, -20]
+
+    '
+- name: s15_snapshot_top_action1
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s15_snapshot_top_action2
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 80, 35, -40, 65, -55]
+
+    '
+- name: s15_snapshot_top_action3
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-112, -70, 0, 0, -80, -160, -30, 60, 35, -40, 85, -60]
+
+    '
+- name: s16_arm_prepare_to_grasp
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-110, -75, 0, 0, -100, -65, -70, 75, 0, 0, 100, -115]

    '
 - name: s17_arm_grasp_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -189,11 +413,11 @@

    frame_time_stamp: 0

-    data_array: [-85, -70, -25, 1, -85, -150, -95, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]

    '
 - name: s18_arm_pickup_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -203,13 +427,13 @@

    frame_time_stamp: 0

-    data_array: [-110, -70, -25, 1, -85, -150, -70, 70, 25, -1, 85, -30]
+    data_array: [-110, -81.8, 0, 0, -100, -65, -70, 81.8, 0, 0, 100, -115]

    '
- name: s19_waist_bend_up_and_turn_around
-  params: 'move_pitch_degree: -25.0
+- name: s19_waist_bend_up
+  params: 'move_pitch_degree: -38.0

-    move_yaw_degree: -90.0
+    move_yaw_degree: 0.0

    '
 - name: s20_waist_turn_around
@@ -219,13 +443,13 @@

    '
 - name: s21_wheel_move_to_putdown_position
-  params: 'move_distance: 0.5
+  params: 'move_distance: 1.5

    move_angle: 0.0

    '
 - name: s22_arm_putdown_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -235,11 +459,11 @@

    frame_time_stamp: 0

-    data_array: [-80, -70, -25, 1, -85, -170, -100, 70, 25, -1, 85, -10]
+    data_array: [-80, -81.8, 0, 0, -100, -80, -100, 81.8, 0, 0, 100, -100]

    '
 - name: s23_arm_release_box
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -249,11 +473,11 @@

    frame_time_stamp: 0

-    data_array: [-80, -60, -25, 1, -85, -170, -100, 60, 25, -1, 85, -10]
+    data_array: [-80, -75, 0, 0, -100, -80, -100, 75, 0, 0, 100, -100]

    '
 - name: s23_arm_raise_up
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -263,7 +487,35 @@

    frame_time_stamp: 0

-    data_array: [-110, -60, -25, 1, -85, -170, -70, 60, 25, -1, 85, -10]
+    data_array: [-110, -75, 0, 0, -100, -80, -70, 75, 0, 0, 100, -100]
+
+    '
+- name: s23_arm_move_to_snapshot_pose
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-90, -30, -45, 0, 0, -180, -90, 30, 45, 0, 0, 0]
+
+    '
+- name: s23_arm_move_origin_position
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
 - name: s24_wheel_move_to_origin_pickup_position
@@ -273,7 +525,7 @@

    '
 - name: s25_arm_move_origin_position
-  params: 'body_id: 0
+  params: 'body_id: 2

    data_type: 2

@@ -283,6 +535,6 @@

    frame_time_stamp: 0

-    data_array: [-5, -60, -30, 0, 0, -180, -175, 60, 30, 0, 0, 0]
+    data_array: [-5, -55, -30, 0, 0, -180, -175, 55, 30, 0, 0, 0]

    '
--- a/src/brain/config/bt_carry_boxes_sch2.xml
+++ b/src/brain/config/bt_carry_boxes_sch2.xml
@@ -1,37 +1,61 @@
+<!--
+  功能描述: 机器人搬运箱子流程2（单臂+底盘+腰部俯仰，3次重试，适配高低位箱体）。
+  流程: 回零 -> 底盘到抓取点 -> 预抓取位 -> 腰部下俯拍照 -> 多角度拍照识别 -> 抓取
+        -> 抬起与腰部上仰并行 -> 底盘到放置点 -> 放置/松爪 -> 抬臂 -> 返回拍照位与回零
+        -> 第二次抓取放置循环。
+-->
 <root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <Sequence name="root">
      <!-- Retry waist + arm + hand until HandControl_H succeeds -->
-      <RetryUntilSuccessful name="retry_all_action" num_attempts="5">
+      <RetryUntilSuccessful name="retry_all_action" num_attempts="3">
        <Sequence>
+          <ClearDoneInstances_H name="clear_done_instances_retry" />
          <Arm_H name="s1_arm_move_origin_position" />
          <MoveWheel_H name="s2_wheel_move_to_origin_pickup_position" />
-          <MoveWaist_H name="s3_waist_turn_around" />
+          <Arm_H name="s4_arm_move_pre_origin_position" />
+          <Arm_H name="s4_arm_move_to_snapshot_pose" />
          <MoveWaist_H name="s4_waist_bend_down" />
+          <Arm_H name="s4_snapshot_action1" />
+          <Arm_H name="s4_snapshot_action2" />
+          <Arm_H name="s4_snapshot_top_action1" />
+          <Arm_H name="s4_snapshot_top_action2" />
+          <Arm_H name="s4_snapshot_top_action3" />
          <Arm_H name="s5_arm_prepare_to_grasp" />
          <Arm_H name="s6_arm_grasp_box" />
-          <Arm_H name="s7_arm_pickup_box" />
-          <MoveWaist_H name="s8_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s9_waist_turn_around" /> -->
+          <Parallel name="parallel_action_3">
+            <Arm_H name="s7_arm_pickup_box" />
+            <MoveWaist_H name="s8_waist_bend_up" />
+          </Parallel>
          <MoveWheel_H name="s10_wheel_move_to_putdown_position" />
          <Arm_H name="s11_arm_putdown_box" />
          <Arm_H name="s12_arm_release_box" />
          <Arm_H name="s12_arm_raise_up" />
-          <MoveWheel_H name="s13_wheel_move_to_origin_pickup_position" />
-          <MoveWaist_H name="s14_waist_turn_around" />
+          <Arm_H name="s13_arm_move_to_snapshot_pose" />
+          <Arm_H name="s13_arm_move_origin_position" />
          <MoveWheel_H name="s15_wheel_move_to_second_pickup_position" />
+          <Arm_H name="s15_arm_move_pre_origin_position" />
+          <Arm_H name="s15_arm_move_to_snapshot_pose" />
          <MoveWaist_H name="s15_waist_bend_down" />
+          <Arm_H name="s15_snapshot_action1" />
+          <Arm_H name="s15_snapshot_action2" />
+          <Arm_H name="s15_snapshot_top_action1" />
+          <Arm_H name="s15_snapshot_top_action2" />
+          <Arm_H name="s15_snapshot_top_action3" />
          <Arm_H name="s16_arm_prepare_to_grasp" />
          <Arm_H name="s17_arm_grasp_box" />
-          <Arm_H name="s18_arm_pickup_box" />
-          <MoveWaist_H name="s19_waist_bend_up_and_turn_around" />
-          <!-- <MoveWaist_H name="s20_waist_turn_around" /> -->
+          <Parallel name="parallel_action_7">
+            <Arm_H name="s18_arm_pickup_box" />
+            <MoveWaist_H name="s19_waist_bend_up" />
+          </Parallel>
          <MoveWheel_H name="s21_wheel_move_to_putdown_position" />
          <Arm_H name="s22_arm_putdown_box" />
          <Arm_H name="s23_arm_release_box" />
          <Arm_H name="s23_arm_raise_up" />
+          <Arm_H name="s23_arm_move_to_snapshot_pose" />
+          <Arm_H name="s23_arm_move_origin_position" />
          <MoveWheel_H name="s24_wheel_move_to_origin_pickup_position" />
-          <!-- <Arm_H name="s25_arm_move_origin_position" /> -->
+          <Arm_H name="s25_arm_move_origin_position" />
        </Sequence>
      </RetryUntilSuccessful>      
    </Sequence>
--- a/src/brain/config/bt_vision_grasp.params.yaml
+++ b/src/brain/config/bt_vision_grasp.params.yaml
@@ -1,32 +0,0 @@
- name: root
-  params: '{}
-
-    '
- name: retry_vision_hand
-  params: '{}
-
-    '
- name: VisionObjectRecognition_H
-  params: '{}
-
-    '
- name: Arm_H
-  params: 'body_id: 0
-
-    data_type: 0
-
-    data_length: 0
-
-    command_id: 0
-
-    frame_time_stamp: 0
-
-    data_array: []
-
-    '
- name: HandControl_H
-  params: 'mode: 0
-
-    effort: 0.0
-
-    '
--- a/src/brain/config/bt_vision_grasp.xml
+++ b/src/brain/config/bt_vision_grasp.xml
@@ -1,15 +0,0 @@
-<root BTCPP_format="4">
-  <BehaviorTree ID="MainTree">
-    <Sequence name="root">
-      <!-- Retry vision + hand until HandControl_H succeeds -->
-      <RetryUntilSuccessful name="retry_vision_hand" num_attempts="5">
-        <Sequence>
-          <VisionObjectRecognition_H name="VisionObjectRecognition_H" />
-          <Arm_H name="Arm_H" />
-        </Sequence>
-      </RetryUntilSuccessful>
-      <!-- After Arm_H eventually succeeds, run the hand -->
-      <HandControl_H name="HandControl_H" />
-    </Sequence>
-  </BehaviorTree>
-</root>
--- a/src/brain/config/bt_vision_grasp_dual_arm.params.yaml
+++ b/src/brain/config/bt_vision_grasp_dual_arm.params.yaml
@@ -0,0 +1,406 @@
+- name: root
+  params: '{}
+
+    '
+- name: s1_right_hand_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_right_arm_initial
+  params: &arm_inittial_right 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 80, -5, -112, -12, -62, 175]
+    
+    '
+- name: s1_left_arm_initial
+  params: &arm_inittial_left 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [100, 5, 112, 12, 62, 5, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s1_right_arm_pre_cam_take_photo
+  params: 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 14.29, -34.07, -122.53, -76.89, 84.40, 112.89]
+    
+    '
+- name: s1_left_arm_pre_cam_take_photo
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s2_right_arm_cam_take_photo
+  params: &right_arm_cam_take_photo 'body_id: 1
+
+    data_type: 100
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s2_left_arm_cam_take_photo
+  params: &left_arm_cam_take_photo 'body_id: 0
+
+    data_type: 100
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    '
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    '
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp_right 'body_id: 1
+
+    data_type: 101
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp_left 'body_id: 0
+
+    data_type: 101
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_right_arm_vision_grasp
+  params: 'body_id: 1
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_left_arm_vision_grasp
+  params: 'body_id: 0
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_right_hand_gripper_close
+  params: &gripper_close 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_hand_gripper_close
+  params: *gripper_close
+- name: s4_right_arm_take_box
+  params: 'body_id: 1
+
+    data_type: 110
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_left_arm_take_box
+  params: 'body_id: 0
+
+    data_type: 110
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s5_right_arm_take_box_off
+  params: 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
+    
+    '
+- name: s5_left_arm_take_box_off
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9_right 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
+    
+    '
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9_left 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s7_right_arm_put_down_box
+  params: 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 85.28, -103.86, 19.46, 40.34, -7.37, 49.94]
+    
+    '
+- name: s7_left_arm_put_down_box
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s8_right_hand_gripper_open
+  params: *gripper_open
+- name: s8_left_hand_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9_right
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9_left
+- name: s11_right_arm_initial
+  params: *arm_inittial_right
+- name: s11_left_arm_initial
+  params: *arm_inittial_left
+- name: s12_right_hand_gripper_open
+  params: *gripper_open
+- name: s12_left_hand_gripper_open
+  params: *gripper_open
+- name: s13_right_arm_initial
+  params: *arm_inittial_right
+- name: s13_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: s14_right_arm_initial
+  params: *arm_inittial_right
+- name: s14_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_left_grasp
+  params: '{}
+
+    '
+- name: s15_right_hand_gripper_open
+  params: *gripper_open
+- name: s15_left_hand_gripper_open
+  params: *gripper_open
+- name: s15_right_arm_pre_grasp
+  params: *pre_grasp_right
+- name: s15_left_arm_pre_grasp
+  params: *pre_grasp_left
+- name: s16_right_arm_initial
+  params: *arm_inittial_right
+- name: s16_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_arm_vision
+  params: '{}
+
+    '
+- name: s17_right_arm_initial
+  params: *arm_inittial_right
+- name: s17_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_left_arm_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_arm_vision
+  params: '{}
+
+    '
+- name: s0_right_hand_gripper_open
+  params: *gripper_open
+- name: s0_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_wheel_move_to_origin_pickup_position
+  params: &origin_pickup_position 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: -0.5
+
+    move_angle: 0.0
+
+    '
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s10_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s10_wheel_move_to_origin_pickup_position
+  params: *origin_pickup_position
+- name: s15_left_arm_cam_take_photo
+  params: *left_arm_cam_take_photo
+- name: s15_right_arm_cam_take_photo
+  params: *right_arm_cam_take_photo
+- name: s15_right_arm_initial_vision
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_vision
+  params: *arm_inittial_left
+- name: s15_right_arm_initial_grasp
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_grasp
+  params: *arm_inittial_left
+- name: s15_right_arm_initial_cam
+  params: *arm_inittial_right
+- name: s15_left_arm_initial_cam
+  params: *arm_inittial_left
--- a/src/brain/config/bt_vision_grasp_dual_arm.xml
+++ b/src/brain/config/bt_vision_grasp_dual_arm.xml
@@ -0,0 +1,351 @@
+<!--
+  功能描述: 双臂协同视觉抓取流程（不含底盘，含分阶段恢复与完成标记清理）。
+  流程: 初始化状态变量 -> 头部相机视觉定位（失败进入视觉恢复）-> 双臂同步到手眼拍照位
+        -> 串行左右手眼视觉识别 -> 双臂异步抓取（各自含抓取恢复）-> 双臂放置 -> 双臂回初始位。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <Script code="right_arm_cam_take_photo:=false; left_arm_cam_take_photo:=false; right_arm_done:=false; left_arm_done:=false; right_vision_ok:=false; left_vision_ok:=false" />
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Phase 1: Shared Vision Setup (Top Camera) -->
+            <Fallback name="shared_vision_with_recovery">
+              <SubTree ID="vision_setup_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+
+            <!-- Move both arms to positions where their hand cameras can see potential objects -->
+            <SubTree ID="dual_arm_cam_subtree" right_cam_status="{right_arm_cam_take_photo}" left_cam_status="{left_arm_cam_take_photo}"/>
+
+            <!-- Phase 2: Sequential Hand Vision -->
+            <SubTree ID="sequential_hand_vision_subtree" right_done="{right_arm_done}" left_done="{left_arm_done}" right_cam_ok="{right_arm_cam_take_photo}" left_cam_ok="{left_arm_cam_take_photo}" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}"/>
+
+            <!-- Phase 3: Independent Dual Arm Operation -->
+            <SubTree ID="dual_arm_grasp_subtree_wrapper" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <!-- Phase 4: Dual Arm Putdown -->
+            <SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <!-- Phase 5: Dual Arm Hand Vision -->
+            <Parallel name="dual_arm_init_action">
+              <Arm_H name="s11_right_arm_initial" />
+              <Arm_H name="s11_left_arm_initial" />
+            </Parallel>
+
+          </Sequence>
+          
+          <Sequence>
+            <!-- Overall fallback recovery -->
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence>
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Shared setup and head-camera vision -->
+  <BehaviorTree ID="vision_setup_subtree"> 
+    <Sequence name="vision_setup_root">
+      <Parallel name="parallel_arm_init_action">
+        <GripperCmd1_H name="s0_right_hand_gripper_open" />
+        <GripperCmd0_H name="s0_left_hand_gripper_open" />
+        <Arm_H name="s1_right_arm_initial" />
+        <Arm_H name="s1_left_arm_initial" />
+      </Parallel>
+      <Parallel name="parallel_init_action">
+        <GripperCmd1_H name="s1_right_hand_gripper_open" />
+        <GripperCmd0_H name="s1_left_hand_gripper_open" />
+        <!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" />
+        <Arm_H name="s1_left_arm_pre_cam_take_photo" /> -->
+      </Parallel>
+      <RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Right Arm Specific Subtrees -->
+  <BehaviorTree ID="right_arm_hand_vision_subtree">
+    <Fallback name="right_hand_vision_fallback">
+      <RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+      <Sequence>
+        <Arm_H name="s15_right_arm_initial_vision" />
+        <AlwaysFailure />
+      </Sequence>
+    </Fallback>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_subtree"> 
+    <Sequence name="right_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s3_right_arm_vision_pre_grasp" />
+          <Arm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_right_hand_gripper_close" />
+          <Arm_H name="s4_right_arm_take_box" />
+          <Arm_H name="s5_right_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_putdown_subtree">
+    <Sequence name="right_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Arm_H name="s7_right_arm_grasp_box" /> -->
+          <Arm_H name="s7_right_arm_put_down_box" />
+          <GripperCmd1_H name="s8_right_hand_gripper_open" />
+          <Arm_H name="s9_right_arm_grasp_box" />
+          <!-- <Arm_H name="s11_right_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Left Arm Specific Subtrees -->
+  <BehaviorTree ID="left_arm_hand_vision_subtree">
+    <Fallback name="left_hand_vision_fallback">
+      <RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+      <Sequence>
+        <Arm_H name="s15_left_arm_initial_vision" />
+        <AlwaysFailure />
+      </Sequence>
+    </Fallback>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_subtree"> 
+    <Sequence name="left_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s3_left_arm_vision_pre_grasp" />
+          <Arm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_left_hand_gripper_close" />
+          <Arm_H name="s4_left_arm_take_box" />
+          <Arm_H name="s5_left_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_putdown_subtree">
+    <Sequence name="left_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Arm_H name="s7_left_arm_grasp_box" /> -->
+          <Arm_H name="s7_left_arm_put_down_box" />
+          <GripperCmd0_H name="s8_left_hand_gripper_open" />
+          <Arm_H name="s9_left_arm_grasp_box" />
+          <!-- <Arm_H name="s11_left_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Recovery Subtrees -->
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <Parallel name="overall_recovery_parallel">
+        <GripperCmd1_H name="s12_right_hand_gripper_open" />
+        <GripperCmd0_H name="s12_left_hand_gripper_open" />
+        <Arm_H name="s13_right_arm_initial" />
+        <Arm_H name="s13_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_overall" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <Parallel name="arm_initial2">
+        <Arm_H name="s14_right_arm_initial" />
+        <Arm_H name="s14_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_vision" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_recovery_subtree">
+    <Sequence name="right_grasp_recovery_root">
+      <GripperCmd1_H name="s15_right_hand_gripper_open" />
+      <Arm_H name="s15_right_arm_pre_grasp" />
+      <Arm_H name="s15_right_arm_cam_take_photo" />
+      <Arm_H name="s15_right_arm_initial_grasp" />
+      <ClearDoneInstances_H name="clear_done_instances_right_grasp" match="right" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_recovery_subtree">
+    <Sequence name="left_grasp_recovery_root">
+      <GripperCmd0_H name="s15_left_hand_gripper_open" />
+      <Arm_H name="s15_left_arm_pre_grasp" />
+      <Arm_H name="s15_left_arm_cam_take_photo" />
+      <Arm_H name="s15_left_arm_initial_grasp" />
+      <ClearDoneInstances_H name="clear_done_instances_left_grasp" match="left" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="arm_vision_recovery_subtree">
+    <Sequence name="arm_vision_recovery_root">
+      <Parallel name="arm_initial3">
+        <Arm_H name="s16_right_arm_initial" />
+        <Arm_H name="s16_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_arm_vision" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="left_arm_vision_recovery_subtree">
+    <Sequence name="left_arm_vision_recovery_root">
+      <Parallel name="left_arm_initial">
+        <Arm_H name="s17_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_left_arm_vision" match="left" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="right_arm_vision_recovery_subtree">
+    <Sequence name="right_arm_vision_recovery_root">
+      <Parallel name="right_arm_initial">
+        <Arm_H name="s17_right_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_right_arm_vision" match="right" />
+    </Sequence>
+  </BehaviorTree>
+
+
+<!-- 子树：双臂相机同步拍照位 -->
+<BehaviorTree ID="dual_arm_cam_subtree">
+  <Sequence name="cam_sync_root">
+    <Script code="right_cam_status:=false; left_cam_status:=false" />
+    <Parallel name="dual_arm_cam_take_photo" success_count="2" failure_count="2">
+      <!-- 右臂 -->
+      <Fallback>
+        <Sequence>
+          <Arm_H name="s2_right_arm_cam_take_photo" />
+          <Script code="right_cam_status:=true" />
+        </Sequence>
+        <Arm_H name="s15_right_arm_initial_cam" />
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂 -->
+      <Fallback>
+        <Sequence>
+          <Arm_H name="s2_left_arm_cam_take_photo" />
+          <Script code="left_cam_status:=true" />
+        </Sequence>
+        <Arm_H name="s15_left_arm_initial_cam" />
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_cam_status==true || left_cam_status==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：串行手眼视觉识别 -->
+<BehaviorTree ID="sequential_hand_vision_subtree">
+  <Sequence name="hand_vision_root">
+    <Script code="right_v_ok:=false; left_v_ok:=false" />
+    <!-- 右臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="right_done==false &amp;&amp; right_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="right_arm_hand_vision_subtree"/>
+            <Script code="right_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+    <!-- 左臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="left_done==false &amp;&amp; left_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="left_arm_hand_vision_subtree"/>
+            <Script code="left_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：异步双臂抓取 -->
+<BehaviorTree ID="dual_arm_grasp_subtree_wrapper">
+  <Sequence name="grasp_sync_root">
+    <Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
+      <!-- 右臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_subtree"/>
+          <Script code="right_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_subtree"/>
+          <Script code="left_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_done==true || left_done==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：双臂放置流程 -->
+<BehaviorTree ID="dual_arm_putdown_subtree_wrapper">
+  <Sequence name="putdown_sync_root">
+    <Script code="left_p_ok:=false; right_p_ok:=false" />
+    <Parallel name="dual_arm_putdown" success_count="2" failure_count="2">
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_done==true" />
+          <SubTree ID="left_arm_putdown_subtree"/>
+          <Script code="left_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_done==true" />
+          <SubTree ID="right_arm_putdown_subtree"/>
+          <Script code="right_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="left_p_ok==true || right_p_ok==true" />
+  </Sequence>
+</BehaviorTree>
+
+</root>
--- a/src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml
+++ b/src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml
@@ -0,0 +1,398 @@
+- name: root
+  params: '{}
+
+    '
+- name: s1_right_hand_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_right_arm_initial
+  params: &arm_inittial_right 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 80, -5, -112, -12, -62, 175]
+    
+    '
+- name: s1_left_arm_initial
+  params: &arm_inittial_left 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [100, 5, 112, 12, 62, 5, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s1_right_arm_pre_cam_take_photo
+  params: 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 14.29, -34.07, -122.53, -76.89, 84.40, 112.89]
+    
+    '
+- name: s1_left_arm_pre_cam_take_photo
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s2_right_arm_cam_take_photo
+  params: &right_arm_cam_take_photo 'body_id: 1
+
+    data_type: 100
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s2_left_arm_cam_take_photo
+  params: &left_arm_cam_take_photo 'body_id: 0
+
+    data_type: 100
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    '
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    '
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp_right 'body_id: 1
+
+    data_type: 101
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp_left 'body_id: 0
+
+    data_type: 101
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_right_arm_vision_grasp
+  params: 'body_id: 1
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_left_arm_vision_grasp
+  params: 'body_id: 0
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_right_hand_gripper_close
+  params: &gripper_close 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_hand_gripper_close
+  params: *gripper_close
+- name: s4_right_arm_take_box
+  params: 'body_id: 1
+
+    data_type: 110
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_left_arm_take_box
+  params: 'body_id: 0
+
+    data_type: 110
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s5_right_arm_take_box_off
+  params: 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
+    
+    '
+- name: s5_left_arm_take_box_off
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9_right 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 96.31, -103.06, 17.69, -53.40, -7.81, 143.19]
+    
+    '
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9_left 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s7_right_arm_put_down_box
+  params: 'body_id: 1
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [0, 0, 0, 0, 0, 0, 85.28, -103.86, 19.46, 40.34, -7.37, 49.94]
+    
+    '
+- name: s7_left_arm_put_down_box
+  params: 'body_id: 0
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15, 0, 0, 0, 0, 0, 0]
+    
+    '
+- name: s8_right_hand_gripper_open
+  params: *gripper_open
+- name: s8_left_hand_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9_right
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9_left
+- name: s11_right_arm_initial
+  params: *arm_inittial_right
+- name: s11_left_arm_initial
+  params: *arm_inittial_left
+- name: s12_right_hand_gripper_open
+  params: *gripper_open
+- name: s12_left_hand_gripper_open
+  params: *gripper_open
+- name: s13_right_arm_initial
+  params: *arm_inittial_right
+- name: s13_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_overall
+  params: '{}
+
+    '
+- name: s14_right_arm_initial
+  params: *arm_inittial_right
+- name: s14_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_grasp
+  params: '{}
+
+    '
+- name: clear_done_instances_left_grasp
+  params: '{}
+
+    '
+- name: s15_right_hand_gripper_open
+  params: *gripper_open
+- name: s15_left_hand_gripper_open
+  params: *gripper_open
+- name: s15_right_arm_pre_grasp
+  params: *pre_grasp_right
+- name: s15_left_arm_pre_grasp
+  params: *pre_grasp_left
+- name: s15_right_arm_initial
+  params: *arm_inittial_right
+- name: s15_left_arm_initial
+  params: *arm_inittial_left
+- name: s16_right_arm_initial
+  params: *arm_inittial_right
+- name: s16_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_arm_vision
+  params: '{}
+
+    '
+- name: s17_right_arm_initial
+  params: *arm_inittial_right
+- name: s17_left_arm_initial
+  params: *arm_inittial_left
+- name: clear_done_instances_left_arm_vision
+  params: '{}
+
+    '
+- name: clear_done_instances_right_arm_vision
+  params: '{}
+
+    '
+- name: s0_right_hand_gripper_open
+  params: *gripper_open
+- name: s0_left_hand_gripper_open
+  params: *gripper_open
+- name: s1_wheel_move_to_origin_pickup_position
+  params: &origin_pickup_position 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: -0.5
+
+    move_angle: 0.0
+
+    '
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s10_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s10_wheel_move_to_origin_pickup_position
+  params: *origin_pickup_position
+- name: s15_left_arm_cam_take_photo
+  params: *left_arm_cam_take_photo
+- name: s15_right_arm_cam_take_photo
+  params: *right_arm_cam_take_photo
--- a/src/brain/config/bt_vision_grasp_dual_arm_demo1.xml
+++ b/src/brain/config/bt_vision_grasp_dual_arm_demo1.xml
@@ -0,0 +1,354 @@
+<!--
+  功能描述: 双臂协同视觉抓取演示流程1（全机身协作，含失败恢复与重试）。
+  流程: 底盘移动到抓取工位 -> 头部相机定位 + 双臂手眼识别 -> 双臂异步抓取
+        -> 底盘移动到放置工位并腰部旋转 -> 双臂放置 -> 腰部复位 -> 底盘回抓取位 -> 双臂回初始位。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <Script code="right_arm_cam_take_photo:=false; left_arm_cam_take_photo:=false; right_arm_done:=false; left_arm_done:=false; right_vision_ok:=false; left_vision_ok:=false" />
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Phase 0: Move Wheel to Origin Pickup Position -->
+            <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
+
+            <!-- Phase 1: Shared Vision Setup (Top Camera) -->
+            <Fallback name="shared_vision_with_recovery">
+              <SubTree ID="vision_setup_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+
+            <!-- Move both arms to positions where their hand cameras can see potential objects -->
+            <SubTree ID="dual_arm_cam_subtree" right_cam_status="{right_arm_cam_take_photo}" left_cam_status="{left_arm_cam_take_photo}"/>
+
+            <!-- Phase 2: Sequential Hand Vision -->
+            <SubTree ID="sequential_hand_vision_subtree" right_done="{right_arm_done}" left_done="{left_arm_done}" right_cam_ok="{right_arm_cam_take_photo}" left_cam_ok="{left_arm_cam_take_photo}" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}"/>
+
+            <!-- Phase 3: Independent Dual Arm Operation -->
+            <SubTree ID="dual_arm_grasp_subtree_wrapper" right_v_ok="{right_vision_ok}" left_v_ok="{left_vision_ok}" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <Parallel name="parallel_action_2">
+              <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+              <MoveWaist_H name="s6_move_waist_turn_left_90" />
+            </Parallel>
+
+            <!-- Phase 4: Dual Arm Putdown -->
+            <SubTree ID="dual_arm_putdown_subtree_wrapper" right_done="{right_arm_done}" left_done="{left_arm_done}"/>
+
+            <MoveWaist_H name="s10_move_waist_turn_right_90" />
+
+            <!-- Phase 5: Dual Arm Initialization -->
+            <Parallel name="dual_arm_init_action">
+              <MoveWheel_H name="s10_wheel_move_to_origin_pickup_position" />
+              <Arm_H name="s11_right_arm_initial" />
+              <Arm_H name="s11_left_arm_initial" />
+            </Parallel>
+
+          </Sequence>
+          
+          <Sequence>
+            <!-- Overall fallback recovery -->
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence>
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Shared setup and head-camera vision -->
+  <BehaviorTree ID="vision_setup_subtree"> 
+    <Sequence name="vision_setup_root">
+      <Parallel name="parallel_arm_init_action">
+        <GripperCmd1_H name="s0_right_hand_gripper_open" />
+        <GripperCmd0_H name="s0_left_hand_gripper_open" />
+        <Arm_H name="s1_right_arm_initial" />
+        <Arm_H name="s1_left_arm_initial" />
+      </Parallel>
+      <Parallel name="parallel_init_action">
+        <GripperCmd1_H name="s1_right_hand_gripper_open" />
+        <GripperCmd0_H name="s1_left_hand_gripper_open" />
+        <!-- <Arm_H name="s1_right_arm_pre_cam_take_photo" />
+        <Arm_H name="s1_left_arm_pre_cam_take_photo" /> -->
+      </Parallel>
+      <RetryUntilSuccessful name="retry_top_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Right Arm Specific Subtrees -->
+  <BehaviorTree ID="right_arm_hand_vision_subtree">
+    <Sequence name="right_hand_vision_root">
+      <RetryUntilSuccessful name="retry_right_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_subtree"> 
+    <Sequence name="right_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_right_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s3_right_arm_vision_pre_grasp" />
+          <Arm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_right_hand_gripper_close" />
+          <Arm_H name="s4_right_arm_take_box" />
+          <Arm_H name="s5_right_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_putdown_subtree">
+    <Sequence name="right_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_right_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Arm_H name="s7_right_arm_grasp_box" /> -->
+          <Arm_H name="s7_right_arm_put_down_box" />
+          <GripperCmd1_H name="s8_right_hand_gripper_open" />
+          <Arm_H name="s9_right_arm_grasp_box" />
+          <!-- <Arm_H name="s11_right_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Left Arm Specific Subtrees -->
+  <BehaviorTree ID="left_arm_hand_vision_subtree">
+    <Sequence name="left_hand_vision_root">
+      <RetryUntilSuccessful name="retry_left_hand_cam_vision" num_attempts="5">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_subtree"> 
+    <Sequence name="left_arm_grasp_root">
+      <RetryUntilSuccessful name="retry_left_arm_grasp_action" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s3_left_arm_vision_pre_grasp" />
+          <Arm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_left_hand_gripper_close" />
+          <Arm_H name="s4_left_arm_take_box" />
+          <Arm_H name="s5_left_arm_take_box_off" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_putdown_subtree">
+    <Sequence name="left_arm_putdown_root">
+      <RetryUntilSuccessful name="retry_left_arm_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Arm_H name="s7_left_arm_grasp_box" /> -->
+          <Arm_H name="s7_left_arm_put_down_box" />
+          <GripperCmd0_H name="s8_left_hand_gripper_open" />
+          <Arm_H name="s9_left_arm_grasp_box" />
+          <!-- <Arm_H name="s11_left_arm_initial" /> -->
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Recovery Subtrees -->
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <Parallel name="overall_recovery_parallel">
+        <GripperCmd1_H name="s12_right_hand_gripper_open" />
+        <GripperCmd0_H name="s12_left_hand_gripper_open" />
+        <Arm_H name="s13_right_arm_initial" />
+        <Arm_H name="s13_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_overall" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <Parallel name="arm_initial2">
+        <Arm_H name="s14_right_arm_initial" />
+        <Arm_H name="s14_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_vision" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="right_arm_grasp_recovery_subtree">
+    <Sequence name="right_grasp_recovery_root">
+      <GripperCmd1_H name="s15_right_hand_gripper_open" />
+      <Arm_H name="s15_right_arm_pre_grasp" />
+      <Arm_H name="s15_right_arm_cam_take_photo" />
+      <Arm_H name="s15_right_arm_initial" />
+      <ClearDoneInstances_H name="clear_done_instances_right_grasp" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="left_arm_grasp_recovery_subtree">
+    <Sequence name="left_grasp_recovery_root">
+      <GripperCmd0_H name="s15_left_hand_gripper_open" />
+      <Arm_H name="s15_left_arm_pre_grasp" />
+      <Arm_H name="s15_left_arm_cam_take_photo" />
+      <Arm_H name="s15_left_arm_initial" />
+      <ClearDoneInstances_H name="clear_done_instances_left_grasp" />
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="arm_vision_recovery_subtree">
+    <Sequence name="arm_vision_recovery_root">
+      <Parallel name="arm_initial3">
+        <Arm_H name="s16_right_arm_initial" />
+        <Arm_H name="s16_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_arm_vision" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="left_arm_vision_recovery_subtree">
+    <Sequence name="left_arm_vision_recovery_root">
+      <Parallel name="left_arm_initial">
+        <Arm_H name="s17_left_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_left_arm_vision" />
+    </Sequence>
+  </BehaviorTree>
+  
+  <BehaviorTree ID="right_arm_vision_recovery_subtree">
+    <Sequence name="right_arm_vision_recovery_root">
+      <Parallel name="right_arm_initial">
+        <Arm_H name="s17_right_arm_initial" />
+      </Parallel>
+      <ClearDoneInstances_H name="clear_done_instances_right_arm_vision" />
+    </Sequence>
+  </BehaviorTree>
+
+
+<!-- 子树：双臂相机同步拍照位 -->
+<BehaviorTree ID="dual_arm_cam_subtree">
+  <Sequence name="cam_sync_root">
+    <Script code="right_cam_status:=false; left_cam_status:=false" />
+    <Parallel name="dual_arm_cam_take_photo" success_count="2" failure_count="2">
+      <!-- 右臂 -->
+      <Fallback>
+        <Sequence>
+          <Arm_H name="s2_right_arm_cam_take_photo" />
+          <Script code="right_cam_status:=true" />
+        </Sequence>
+        <Arm_H name="s15_right_arm_initial" />
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂 -->
+      <Fallback>
+        <Sequence>
+          <Arm_H name="s2_left_arm_cam_take_photo" />
+          <Script code="left_cam_status:=true" />
+        </Sequence>
+        <Arm_H name="s15_left_arm_initial" />
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_cam_status==true || left_cam_status==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：串行手眼视觉识别 -->
+<BehaviorTree ID="sequential_hand_vision_subtree">
+  <Sequence name="hand_vision_root">
+    <Script code="right_v_ok:=false; left_v_ok:=false" />
+    <!-- 右臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="right_done==false &amp;&amp; right_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="right_arm_hand_vision_subtree"/>
+            <Script code="right_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+    <!-- 左臂视觉 -->
+    <Fallback>
+      <Sequence>
+        <ScriptCondition code="left_done==false &amp;&amp; left_cam_ok==true" />
+        <Fallback>
+          <Sequence>
+            <SubTree ID="left_arm_hand_vision_subtree"/>
+            <Script code="left_v_ok:=true" />
+          </Sequence>
+          <AlwaysFailure/>
+        </Fallback>
+      </Sequence>
+      <AlwaysSuccess/>
+    </Fallback>
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：异步双臂抓取 -->
+<BehaviorTree ID="dual_arm_grasp_subtree_wrapper">
+  <Sequence name="grasp_sync_root">
+    <Parallel name="dual_arm_asynchronous_grasp" success_count="2" failure_count="2">
+      <!-- 右臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_subtree"/>
+          <Script code="right_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="right_v_ok==true" />
+          <SubTree ID="right_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <!-- 左臂抓取 -->
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_subtree"/>
+          <Script code="left_done:=true" />
+        </Sequence>
+        <Sequence>
+          <ScriptCondition code="left_v_ok==true" />
+          <SubTree ID="left_arm_grasp_recovery_subtree"/>
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="right_done==true || left_done==true" />
+  </Sequence>
+</BehaviorTree>
+
+<!-- 子树：双臂放置流程 -->
+<BehaviorTree ID="dual_arm_putdown_subtree_wrapper">
+  <Sequence name="putdown_sync_root">
+    <Script code="left_p_ok:=false; right_p_ok:=false" />
+    <Parallel name="dual_arm_putdown" success_count="2" failure_count="2">
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="left_done==true" />
+          <SubTree ID="left_arm_putdown_subtree"/>
+          <Script code="left_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+      <Fallback>
+        <Sequence>
+          <ScriptCondition code="right_done==true" />
+          <SubTree ID="right_arm_putdown_subtree"/>
+          <Script code="right_p_ok:=true" />
+        </Sequence>
+        <AlwaysSuccess/>
+      </Fallback>
+    </Parallel>
+    <ScriptCondition code="left_p_ok==true || right_p_ok==true" />
+  </Sequence>
+</BehaviorTree>
+
+</root>
--- a/src/brain/config/bt_vision_grasp_left_arm.params.yaml
+++ b/src/brain/config/bt_vision_grasp_left_arm.params.yaml
@@ -0,0 +1,218 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: s1_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s1_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_left_arm_pre_cam_take_photo
+  params: &pre_cam_take_photo 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 25, 90, 0, 68, 90, 95, -25, -90, 0, -68, 90]
+    
+    '
+- name: s1_wheel_move_to_origin_pickup_position
+  params: 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s2_left_arm_pre_cam_take_photo
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
+    
+    '
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    '
+- name: s2_left_arm_cam_take_photo
+  params: &arm_cam_take_photo 'body_id: 0
+
+    data_type: 100
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s3_left_camera_vision_recg
+  params: 'camera_position: left
+
+    '
+- name: s3_left_arm_vision_pre_grasp
+  params: &pre_grasp 'body_id: 0
+
+    data_type: 101
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_left_arm_vision_grasp
+  params: 'body_id: 0
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_gripper_close
+  params: 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_left_arm_take_box
+  params: 'body_id: 0
+
+    data_type: 110
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s5_left_arm_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [84, 88, 45, 22, 16, 68, 95, -25, -90, 0, -68, 90]
+    
+    '
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s7_left_arm_grasp_box
+  params: &grasp_box_s7_s9 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 88, -14, 22, 16, 68, 95, -25, -90, 0, -68, 90]
+    
+    '
+- name: s7_left_arm_put_down_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [95, 88, -14, -22, 16, 112, 95, -25, -90, 0, -68, 90]
+    
+    '
+- name: s8_gripper_open
+  params: *gripper_open
+- name: s9_left_arm_grasp_box
+  params: *grasp_box_s7_s9
+- name: s10_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s11_left_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s12_gripper_open
+  params: *gripper_open
+- name: s13_left_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s14_left_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s15_left_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s16_left_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s14_gripper_open
+  params: *gripper_open
+- name: s15_gripper_open
+  params: *gripper_open
+- name: s16_gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp
+- name: s15_left_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s16_left_arm_cam_take_photo
+  params: *arm_cam_take_photo
--- a/src/brain/config/bt_vision_grasp_left_arm.xml
+++ b/src/brain/config/bt_vision_grasp_left_arm.xml
@@ -0,0 +1,171 @@
+<!--
+  功能描述: 左臂独立视觉抓取流程（视觉/抓取/放置均带恢复与重试）。
+  流程: 左臂到手眼拍照位 -> 相机识别目标 -> 预抓取/抓取/抬起 -> 送到放置位并松爪 -> 回到初始位。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Vision with recovery -->
+            <Fallback name="vision_with_recovery">
+              <SubTree ID="vision_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Grasp with recovery -->
+            <Fallback name="grasp_with_recovery">
+              <SubTree ID="grasp_subtree"/>
+              <Sequence>
+                <SubTree ID="grasp_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Putdown with recovery -->
+            <Fallback name="putdown_with_recovery">
+              <SubTree ID="putdown_subtree"/>
+              <Sequence>
+                <SubTree ID="putdown_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+          </Sequence>
+          
+          <!-- Overall recovery if the entire sequence fails -->
+          <!-- <Sequence>
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence> -->
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_subtree"> 
+    <Sequence name="vision_root">
+      <RetryUntilSuccessful name="retry_vision" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s1_left_arm_pre_cam_take_photo" />
+          <Parallel name="parallel_action_1">
+            <!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
+            <GripperCmd0_H name="s1_gripper_open" />
+            <Arm_H name="s2_left_arm_pre_cam_take_photo" />
+          </Parallel>
+          <SubTree ID="vision_top_cam_subtree"/>
+
+          <Arm_H name="s2_left_arm_cam_take_photo" />
+          <SubTree ID="vision_hand_cam_subtree"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_top_cam_subtree"> 
+    <Sequence name="vision_top_cam_root">
+      <RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_hand_cam_subtree"> 
+    <Sequence name="vision_hand_cam_root">
+      <RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s3_left_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_subtree"> 
+    <Sequence name="grasp_root">
+      <RetryUntilSuccessful name="retry_grasp" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s3_left_arm_vision_pre_grasp" />
+          <Arm_H name="s4_left_arm_vision_grasp" />
+          <GripperCmd0_H name="s4_gripper_close" />
+          <Arm_H name="s4_left_arm_take_box" />
+          <Arm_H name="s5_left_arm_grasp_box" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_subtree">
+    <Sequence name="putdown_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+            <MoveWaist_H name="s6_move_waist_turn_left_90" />
+          </Parallel> -->
+          <Arm_H name="s7_left_arm_grasp_box" />
+          <Arm_H name="s7_left_arm_put_down_box" />
+          <GripperCmd0_H name="s8_gripper_open" />
+          <Arm_H name="s9_left_arm_grasp_box" />
+
+          <Parallel name="parallel_action_3">
+            <!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
+            <Arm_H name="s11_left_arm_pre_cam_take_photo" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <RetryUntilSuccessful name="retry_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s12_gripper_open" />
+          <Arm_H name="s13_left_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_overall" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
+        <Sequence> 
+          <!-- <GripperCmd0_H name="s14_gripper_open" /> -->
+          <Arm_H name="s14_left_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_vision" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_recovery_subtree">
+    <Sequence name="grasp_recovery_root">
+      <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s15_gripper_open" />
+          <Arm_H name="s15_arm_pre_grasp" />
+          <Arm_H name="s15_left_arm_cam_take_photo" />
+          <Arm_H name="s15_left_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_grasp" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_recovery_subtree">
+    <Sequence name="putdown_recovery_root">
+      <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd0_H name="s16_gripper_open" />
+          <Arm_H name="s16_arm_pre_grasp" />
+          <Arm_H name="s16_left_arm_cam_take_photo" />
+          <Arm_H name="s16_left_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_putdown" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>
--- a/src/brain/config/bt_vision_grasp_right_arm.params.yaml
+++ b/src/brain/config/bt_vision_grasp_right_arm.params.yaml
@@ -0,0 +1,218 @@
+- name: root
+  params: '{}
+
+    '
+- name: retry_vision_hand
+  params: '{}
+
+    '
+- name: s1_move_waist_turn_right_90
+  params: &waist_turn_right_90 'move_pitch_degree: 0.0
+
+    move_yaw_degree: -90.0
+
+    '
+- name: s1_gripper_open
+  params: &gripper_open 'loc: 0
+
+    speed: 150
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s1_right_arm_pre_cam_take_photo
+  params: &pre_cam_take_photo 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 25, 90, 0, 68, 90, 95, -25, -90, 0, -68, 90]
+    
+    '
+- name: s1_wheel_move_to_origin_pickup_position
+  params: 'move_distance: 0.5
+
+    move_angle: 0.0
+
+    '
+- name: s2_right_arm_pre_cam_take_photo
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [120, 20, 80, 0, 80, 120, 60, -20, -80, 0, -80, 60]
+    
+    '
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
+
+    '
+- name: s2_right_arm_cam_take_photo
+  params: &arm_cam_take_photo 'body_id: 1
+
+    data_type: 100
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    '
+- name: s3_right_arm_vision_pre_grasp
+  params: &pre_grasp 'body_id: 1
+
+    data_type: 101
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_right_arm_vision_grasp
+  params: 'body_id: 1
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s4_gripper_close
+  params: 'loc: 200
+
+    speed: 150
+
+    torque: 100
+
+    mode: 2
+
+    '
+- name: s4_right_arm_take_box
+  params: 'body_id: 1
+
+    data_type: 110
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
+    '
+- name: s5_right_arm_grasp_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 25, 90, 0, 68, 90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
+    
+    '
+- name: s6_move_waist_turn_left_90
+  params: 'move_pitch_degree: 0.0
+
+    move_yaw_degree: 90.0
+
+    '
+- name: s6_wheel_move_to_putdown_position
+  params: 'move_distance: 0.0
+
+    move_angle: 0.0
+
+    '
+- name: s7_right_arm_grasp_box
+  params: &grasp_box_s7_s9 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 25, 90, 0, 68, 90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
+    
+    '
+- name: s7_right_arm_put_down_box
+  params: 'body_id: 2
+
+    data_type: 2
+
+    data_length: 12
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: [85, 25, 90, 0, 68, 90, 85.78, -88.95, 13.97, 22.88, -16.29, 67.99]
+    
+    '
+- name: s8_gripper_open
+  params: *gripper_open
+- name: s9_right_arm_grasp_box
+  params: *grasp_box_s7_s9
+- name: s10_move_waist_turn_right_90
+  params: *waist_turn_right_90
+- name: s11_right_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s12_gripper_open
+  params: *gripper_open
+- name: s13_right_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s14_right_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s15_right_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s16_right_arm_pre_cam_take_photo
+  params: *pre_cam_take_photo
+- name: s14_gripper_open
+  params: *gripper_open
+- name: s15_gripper_open
+  params: *gripper_open
+- name: s16_gripper_open
+  params: *gripper_open
+- name: s15_arm_pre_grasp
+  params: *pre_grasp
+- name: s16_arm_pre_grasp
+  params: *pre_grasp
+- name: s15_right_arm_cam_take_photo
+  params: *arm_cam_take_photo
+- name: s16_right_arm_cam_take_photo
+  params: *arm_cam_take_photo
--- a/src/brain/config/bt_vision_grasp_right_arm.xml
+++ b/src/brain/config/bt_vision_grasp_right_arm.xml
@@ -0,0 +1,171 @@
+<!--
+  功能描述: 右臂独立视觉抓取流程（视觉/抓取/放置均带恢复与重试）。
+  流程: 右臂到手眼拍照位 -> 相机识别目标 -> 预抓取/抓取/抬起 -> 送到放置位并松爪 -> 回到初始位。
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <RetryUntilSuccessful name="retry_vision_grasp" num_attempts="3">
+        <Fallback name="fallback_main_action">
+          <Sequence name="complete_pick_and_place">
+            <!-- Vision with recovery -->
+            <Fallback name="vision_with_recovery">
+              <SubTree ID="vision_subtree"/>
+              <Sequence>
+                <SubTree ID="vision_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Grasp with recovery -->
+            <Fallback name="grasp_with_recovery">
+              <SubTree ID="grasp_subtree"/>
+              <Sequence>
+                <SubTree ID="grasp_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+            
+            <!-- Putdown with recovery -->
+            <Fallback name="putdown_with_recovery">
+              <SubTree ID="putdown_subtree"/>
+              <Sequence>
+                <SubTree ID="putdown_recovery_subtree"/>
+                <AlwaysFailure/>
+              </Sequence>
+            </Fallback>
+          </Sequence>
+          
+          <!-- Overall recovery if the entire sequence fails -->
+          <!-- <Sequence>
+            <SubTree ID="recovery_subtree"/>
+            <AlwaysFailure/>
+          </Sequence> -->
+        </Fallback>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_subtree"> 
+    <Sequence name="vision_root">
+      <RetryUntilSuccessful name="retry_vision" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s1_right_arm_pre_cam_take_photo" />
+          <Parallel name="parallel_action_1">
+            <!-- <MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" /> -->
+            <GripperCmd1_H name="s1_gripper_open" />
+            <Arm_H name="s2_right_arm_pre_cam_take_photo" />
+          </Parallel>
+          <SubTree ID="vision_top_cam_subtree"/>
+
+          <Arm_H name="s2_right_arm_cam_take_photo" />
+          <SubTree ID="vision_hand_cam_subtree"/>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_top_cam_subtree"> 
+    <Sequence name="vision_top_cam_root">
+      <RetryUntilSuccessful name="retry_top_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_hand_cam_subtree"> 
+    <Sequence name="vision_hand_cam_root">
+      <RetryUntilSuccessful name="retry_hand_cam_vision_recg_1" num_attempts="10">
+        <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_subtree"> 
+    <Sequence name="grasp_root">
+      <RetryUntilSuccessful name="retry_grasp" num_attempts="1">
+        <Sequence> 
+          <Arm_H name="s3_right_arm_vision_pre_grasp" />
+          <Arm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd1_H name="s4_gripper_close" />
+          <Arm_H name="s4_right_arm_take_box" />
+          <Arm_H name="s5_right_arm_grasp_box" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_subtree">
+    <Sequence name="putdown_root">
+      <RetryUntilSuccessful name="retry_putdown" num_attempts="1">
+        <Sequence> 
+          <!-- <Parallel name="parallel_action_2">
+            <MoveWheel_H name="s6_wheel_move_to_putdown_position" />
+            <MoveWaist_H name="s6_move_waist_turn_left_90" />
+          </Parallel> -->
+          <Arm_H name="s7_right_arm_grasp_box" />
+          <Arm_H name="s7_right_arm_put_down_box" />
+          <GripperCmd1_H name="s8_gripper_open" />
+          <Arm_H name="s9_right_arm_grasp_box" />
+
+          <Parallel name="parallel_action_3">
+            <!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
+            <Arm_H name="s11_right_arm_pre_cam_take_photo" />
+          </Parallel>
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="recovery_subtree">
+    <Sequence name="recovery_root">
+      <RetryUntilSuccessful name="retry_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s12_gripper_open" />
+          <Arm_H name="s13_right_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_overall" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="vision_recovery_subtree">
+    <Sequence name="vision_recovery_root">
+      <RetryUntilSuccessful name="retry_vision_recovery" num_attempts="1">
+        <Sequence> 
+          <!-- <GripperCmd1_H name="s14_gripper_open" /> -->
+          <Arm_H name="s14_right_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_vision" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="grasp_recovery_subtree">
+    <Sequence name="grasp_recovery_root">
+      <RetryUntilSuccessful name="retry_grasp_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s15_gripper_open" />
+          <Arm_H name="s15_arm_pre_grasp" />
+          <Arm_H name="s15_right_arm_cam_take_photo" />
+          <Arm_H name="s15_right_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_grasp" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="putdown_recovery_subtree">
+    <Sequence name="putdown_recovery_root">
+      <RetryUntilSuccessful name="retry_putdown_recovery" num_attempts="1">
+        <Sequence> 
+          <GripperCmd1_H name="s16_gripper_open" />
+          <Arm_H name="s16_arm_pre_grasp" />
+          <Arm_H name="s16_right_arm_cam_take_photo" />
+          <Arm_H name="s16_right_arm_pre_cam_take_photo" />
+          <ClearDoneInstances_H name="clear_done_instances_putdown" />
+        </Sequence>
+      </RetryUntilSuccessful>
+    </Sequence>
+  </BehaviorTree>
+</root>
--- a/src/brain/config/example_skill_config.yaml
+++ b/src/brain/config/example_skill_config.yaml
@@ -0,0 +1,55 @@
+# Example configuration file showing how to configure skill-specific parameters
+# This can be used as an additional parameters file in addition to the main brain_node_params.yaml
+
+cerebrum_node:
+  ros__parameters:
+    # Skill-specific parameters
+    cerebrum:
+      bt:
+        VisionObjectRecognition:
+          topic: "vision_object_recognition"
+          payload_yaml: ""
+          timeout_ms: "10000"
+        Arm:
+          topic: "arm_control"
+          payload_yaml: ""
+          timeout_ms: "30000"
+        CameraTakePhoto:
+          topic: "camera_take_photo"
+          payload_yaml: ""
+          timeout_ms: "5000"
+        HandControl:
+          topic: "hand_control"
+          payload_yaml: ""
+          timeout_ms: "5000"
+        MoveWaist:
+          topic: "move_waist"
+          payload_yaml: ""
+          timeout_ms: "10000"
+        MoveLeg:
+          topic: "move_leg"
+          payload_yaml: ""
+          timeout_ms: "15000"
+        MoveWheel:
+          topic: "move_wheel"
+          payload_yaml: ""
+          timeout_ms: "20000"
+        MoveHome:
+          topic: "move_home"
+          payload_yaml: ""
+          timeout_ms: "30000"
+        GripperCmd:
+          topic: "gripper_cmd0"
+          payload_yaml: ""
+          timeout_ms: "5000"
+        
+        # Example instance-specific overrides
+        Arm:
+          s1_arm_move_to_object:
+            topic: "arm_control_specific"
+            payload_yaml: "{target_pose: {x: 0.1, y: 0.2, z: 0.3}}"
+            timeout_ms: "45000"
+          s2_arm_grasp_object:
+            topic: "arm_control_grasp"
+            payload_yaml: "{target_pose: {x: 0.15, y: 0.25, z: 0.35}, grip_force: 10.0}"
+            timeout_ms: "50000"
--- a/src/brain/config/robot_config.yaml
+++ b/src/brain/config/robot_config.yaml
@@ -6,12 +6,18 @@
  version: 1.0.0
  config_params_path: [
    {config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
-    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml}
+    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
+    {config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
+    {config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
+    {config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
  ]

 - name: cerebellum_node
  version: 1.0.0
  config_params_path: [
    {config: /config/bt_carry_boxes_sch1.xml, param: /config/bt_carry_boxes_sch1.params.yaml},
-    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml}
+    {config: /config/bt_carry_boxes_sch2.xml, param: /config/bt_carry_boxes_sch2.params.yaml},
+    {config: /config/bt_vision_grasp_left_arm.xml, param: /config/bt_vision_grasp_left_arm.params.yaml},
+    {config: /config/bt_vision_grasp_right_arm.xml, param: /config/bt_vision_grasp_right_arm.params.yaml},
+    {config: /config/bt_vision_grasp_dual_arm.xml, param: /config/bt_vision_grasp_dual_arm.params.yaml}
  ]
--- a/src/brain/config/robot_skills.yaml
+++ b/src/brain/config/robot_skills.yaml
@@ -61,4 +61,18 @@
  description: "回到原点位姿"
  interfaces:
    - name: MoveHome.action
-      default_topic: "MoveHome"
+      default_topic: "MoveHome"
+
+- name: GripperCmd0
+  version: 1.0.0
+  description: "夹爪0控制"
+  interfaces:
+    - name: GripperCmd.action
+      default_topic: "gripper_cmd0"
+
+- name: GripperCmd1
+  version: 1.0.0
+  description: "夹爪1控制"
+  interfaces:
+    - name: GripperCmd.action
+      default_topic: "gripper_cmd1"
--- a/src/brain/config/voice/inFinished.mp3
+++ b/src/brain/config/voice/inFinished.mp3
--- a/src/brain/config/voice/inScan.mp3
+++ b/src/brain/config/voice/inScan.mp3
--- a/src/brain/config/voice/inStart.mp3
+++ b/src/brain/config/voice/inStart.mp3
--- a/src/brain/config/voice/outFinished.mp3
+++ b/src/brain/config/voice/outFinished.mp3
--- a/src/brain/config/voice/outScan.mp3
+++ b/src/brain/config/voice/outScan.mp3
--- a/src/brain/config/voice/outStart.mp3
+++ b/src/brain/config/voice/outStart.mp3
--- a/src/brain/config/voice/scanFinished.mp3
+++ b/src/brain/config/voice/scanFinished.mp3
--- a/src/brain/config/voice/scaning.mp3
+++ b/src/brain/config/voice/scaning.mp3
--- a/src/brain/include/brain/action_registry.hpp
+++ b/src/brain/include/brain/action_registry.hpp
@@ -249,6 +249,9 @@ public:
    std::function<void(const typename GoalHandle<ActionT>::WrappedResult &)> on_result,
    std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response = nullptr)
  {
+    if (entries_.find(name) != entries_.end()) {
+      RCLCPP_WARN(node_->get_logger(), "[ActionClientRegistry] Overwriting existing client for '%s'", name.c_str());
+    }
    entries_[name] =
      std::make_unique<Entry<ActionT>>(
      node_, name, std::move(make_goal), std::move(
@@ -406,6 +409,87 @@ public:
    return wrapped_result;
  }

+  template<typename ActionT>
+  std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait_with_goal(
+    const std::string & name,
+    rclcpp::Logger logger,
+    const typename ActionT::Goal & goal,
+    std::chrono::nanoseconds timeout = std::chrono::seconds(30))
+  {
+    auto it = entries_.find(name);
+    if (it == entries_.end()) {
+      RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
+      return std::nullopt;
+    }
+    auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
+    if (!e) {
+      RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
+      return std::nullopt;
+    }
+    if (!e->client) {return std::nullopt;}
+    if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
+      RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
+      return std::nullopt;
+    }
+    auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
+    if (e->on_feedback) {opts.feedback_callback = e->on_feedback;}
+    if (e->on_result) {opts.result_callback = e->on_result;}
+    if (e->on_goal_response) {opts.goal_response_callback = e->on_goal_response;}
+    auto goal_future = e->client->async_send_goal(goal, opts);
+    auto goal_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5);
+    if (goal_future.wait_until(goal_deadline) != std::future_status::ready) {
+      RCLCPP_ERROR(logger, "Timed out waiting for goal response on '%s'", name.c_str());
+      return std::nullopt;
+    }
+
+    typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle;
+    try {
+      goal_handle = goal_future.get();
+    } catch (const std::exception & ex) {
+      RCLCPP_ERROR(logger, "Failed to get goal handle for '%s': %s", name.c_str(), ex.what());
+      return std::nullopt;
+    }
+    if (!goal_handle) {
+      RCLCPP_WARN(logger, "Goal rejected for '%s'", name.c_str());
+      return std::nullopt;
+    }
+    e->last_goal_handle = goal_handle;
+    if (e->on_goal_response) {
+      try {
+        e->on_goal_response(goal_handle);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN(logger, "Goal response callback threw for '%s': %s", name.c_str(), ex.what());
+      }
+    }
+
+    auto result_future = e->client->async_get_result(goal_handle);
+    auto deadline = std::chrono::steady_clock::now() + timeout;
+    if (result_future.wait_until(deadline) != std::future_status::ready) {
+      RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
+      try {
+        e->client->async_cancel_goal(goal_handle);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN(logger, "Failed cancelling goal for '%s': %s", name.c_str(), ex.what());
+      }
+      return std::nullopt;
+    }
+    typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult wrapped_result;
+    try {
+      wrapped_result = result_future.get();
+    } catch (const std::exception & ex) {
+      RCLCPP_ERROR(logger, "Failed to get result for '%s': %s", name.c_str(), ex.what());
+      return std::nullopt;
+    }
+    if (e->on_result) {
+      try {
+        e->on_result(wrapped_result);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN(logger, "Result callback threw for '%s': %s", name.c_str(), ex.what());
+      }
+    }
+    return wrapped_result;
+  }
+
 private:
  rclcpp::Node * node_;
  std::unordered_map<std::string, std::unique_ptr<EntryBase>> entries_;
--- a/src/brain/include/brain/bt_registry.hpp
+++ b/src/brain/include/brain/bt_registry.hpp
@@ -86,9 +86,12 @@ public:
      node) {}

  /**
-   * @brief Declare static ports. Currently none (empty list) simplifying dynamic XML generation.
+   * @brief Declare static ports. Added "match" for selective clearing logic in ClearDoneInstances_H.
   */
-  static BT::PortsList providedPorts() {return {};}
+  static BT::PortsList providedPorts()
+  {
+    return {BT::InputPort<std::string>("match")};
+  }

  /**
   * @brief First tick transition handler.
@@ -274,8 +277,11 @@ private:
    std::ostringstream oss;
    oss <<
      "\n    <root BTCPP_format=\"4\" >\n\n        <BehaviorTree ID=\"MainTree\">\n            <Sequence name=\"root_sequence\">\n";
-    for (const auto & act : seq) {
-      oss << "                <" << act.type << " name=\"" << act.name << "\"";
+    auto timestamp = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+    for (size_t i = 0; i < seq.size(); ++i) {
+      const auto & act = seq[i];
+      // Append timestamp and sequence index to ensure global uniqueness of the instance name in the tree
+      oss << "                <" << act.type << " name=\"" << act.name << "_" << timestamp << "_" << i << "\"";
      for (const auto & kv : act.ports) {
        oss << " " << kv.first << "=\"" << kv.second << "\"";
      }
--- a/src/brain/include/brain/calc_grasp_pose.hpp
+++ b/src/brain/include/brain/calc_grasp_pose.hpp
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Transform.h>
+#include <string>
+#include <vector>
+#include <functional>
+
+namespace brain {
+
+struct Pose {
+    tf2::Vector3 position;
+    tf2::Quaternion orientation;
+};
+
+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
+    );
+
+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
+    );
+
+};
+
+} // namespace brain
--- a/src/brain/include/brain/cerebellum_hooks.hpp
+++ b/src/brain/include/brain/cerebellum_hooks.hpp
@@ -0,0 +1,35 @@
+#ifndef BRAIN_CEREBELLUM_HOOKS_HPP_
+#define BRAIN_CEREBELLUM_HOOKS_HPP_
+
+namespace brain
+{
+
+class CerebellumNode;
+
+class CerebellumHooks
+{
+public:
+  static void ConfigureActionHooks(CerebellumNode * node);
+  static void ConfigureArmSpaceControlHooks(CerebellumNode * node);
+  static void ConfigureArmHooks(CerebellumNode * node);
+  static void ConfigureHandControlHooks(CerebellumNode * node);
+  static void ConfigureCameraTakePhotoHooks(CerebellumNode * node);
+  static void ConfigureLegControlHooks(CerebellumNode * node);
+  static void ConfigureVisionGraspObjectHooks(CerebellumNode * node);
+  static void ConfigureSlamModeHooks(CerebellumNode * node);
+  static void ConfigureNavigateToPoseHooks(CerebellumNode * node);
+  static void ConfigureMoveWaistHooks(CerebellumNode * node);
+  static void ConfigureMoveLegHooks(CerebellumNode * node);
+  static void ConfigureMoveWheelHooks(CerebellumNode * node);
+  static void ConfigureMoveHomeHooks(CerebellumNode * node);
+  static void ConfigureGripperCmdControlHooks(CerebellumNode * node);
+
+  static void ConfigureServiceHooks(CerebellumNode * node);
+  static void ConfigureVisionObjectRecognitionHooks(CerebellumNode * node);
+  static void ConfigureMapLoadHooks(CerebellumNode * node);
+  static void ConfigureMapSaveHooks(CerebellumNode * node);
+};
+
+}  // namespace brain
+
+#endif  // BRAIN_CEREBELLUM_HOOKS_HPP_
--- a/src/brain/include/brain/cerebellum_node.hpp
+++ b/src/brain/include/brain/cerebellum_node.hpp
@@ -4,6 +4,8 @@
 #include <atomic>
 #include <limits>
 #include <memory>
+#include <vector>
+#include <unordered_map>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -18,8 +20,11 @@
 #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"
+#include "interfaces/srv/inverse_kinematics.hpp"

 namespace brain
 {
@@ -41,7 +46,34 @@ public:
   */
  explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

+  std::unordered_map<std::string, std::vector<geometry_msgs::msg::Pose>> arm_poses_;
+  std::unordered_map<std::string, std::vector<std::vector<double>>> arm_angles_;
+
+  std::vector<double> left_arm_joint_angles_;
+  std::vector<double> right_arm_joint_angles_;
+  std::mutex joint_state_mutex_;
+
+  std::string camera_position_;
+
+  std::unordered_map<std::string, std::string> gripper_info_;
+  
+  /**
+   * @brief Call Inverse Kinematics service
+   * 
+   * @param pos 
+   * @param quat 
+   * @param angles 
+   * @param arm_id 
+   * @return int 
+   */
+  int CallInverseKinematics(const tf2::Vector3& pos, const tf2::Quaternion& quat, std::vector<double>& angles, int arm_id);
+
 private:
+  struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
+  std::unordered_map<std::uintptr_t, std::shared_ptr<GoalExecutionContext>> goal_ctxs_;
+  std::mutex goal_ctxs_mutex_;
+  static thread_local const std::vector<interfaces::msg::SkillCall>* tls_current_calls_;
+  static thread_local int tls_arm_body_id;
  // Generic action registry to support multiple actions via registration
  std::unique_ptr<ActionServerRegistry> action_registry_;
  std::unique_ptr<ActionClientRegistry> action_clients_;
@@ -53,19 +85,64 @@ private:

  // SMACC2 execution context
  std::unique_ptr<smacc2::SmExecution> sm_exec_;
+  
+  // IK Client
+  rclcpp::Client<interfaces::srv::InverseKinematics>::SharedPtr ik_client_;

-  // Parameters: abort policy, default action timeout, specific grasp timeout
-  bool abort_on_failure_ {true};
-  double default_action_timeout_sec_ {45.0};
-  double vision_grasp_object_timeout_sec_ {120.0};
-  std::string map_save_url_ {"/tmp/humanoid_map"};
-  std::string map_save_image_format_ {"pgm"};
-  double map_save_free_thresh_ {0.25};
-  double map_save_occupied_thresh_ {0.65};
-  std::string map_load_url_ {};
-  double nav_goal_yaw_offset_ {0.0};
-  double nav_goal_distance_tolerance_ {0.5};
-  bool slam_enable_mapping_default_ {true};
+  // ROS parameters (replacing hardcoded defaults)
+  bool abort_on_failure_{true};
+  double default_action_timeout_sec_{45.0};
+  double vision_grasp_object_timeout_sec_{120.0};
+  std::string map_save_url_{"/tmp/humanoid_map"};
+  std::string map_save_image_format_{"pgm"};
+  double map_save_free_thresh_{0.25};
+  double map_save_occupied_thresh_{0.65};
+  std::string map_load_url_{};
+  double nav_goal_yaw_offset_{0.0};
+  double nav_goal_distance_tolerance_{0.5};
+  bool slam_enable_mapping_default_{true};
+  
+  // Grasp pose calculation parameters
+  double top_cam_right_arm_z_threshold_{0.05};
+  double top_cam_right_arm_x_offset_{0.10};
+  double top_cam_right_arm_y_{0.33};
+  std::vector<double> top_cam_right_arm_orientation_{-0.7071, 0.0, 0.0, 0.7071};
+  double top_cam_left_arm_z_threshold_{-0.10};
+  double top_cam_left_arm_x_offset_{0.10};
+  double top_cam_left_arm_y_{-0.33};
+  std::vector<double> top_cam_left_arm_orientation_{0.7071, 0.0, 0.0, 0.7071};
+  double side_cam_right_arm_y_offset_{0.02};
+  double side_cam_right_arm_z_offset_{0.02};
+  double side_cam_left_arm_y_offset_{0.03};
+  double side_cam_left_arm_z_offset_{0.01};
+  double take_object_arm_x_{-0.05};
+  double vision_object_recognition_wait_timeout_sec_{2.0};
+  double vision_object_recognition_call_timeout_sec_{5.0};
+  double map_load_wait_timeout_sec_{2.0};
+  double map_load_call_timeout_sec_{5.0};
+  
+  std::string left_camera_frame_{"LeftLink6"};
+  std::string right_camera_frame_{"RightLink6"};
+  std::vector<std::string> target_frames_{"apple", "bottle", "medicine_box"};
+  std::string target_frame_{"medicine_box"};
+  std::vector<std::string> grasp_palm_facings_{"down"};
+  std::vector<double> grasp_best_angles_{0.0};
+  std::string grasp_type_{"side"};
+  double grasp_safety_dist_{0.05};
+  double grasp_finger_length_{0.20};
+  
+  int hand_control_default_mode_{1};
+  int gripper_default_loc_{30};
+  int gripper_default_speed_{0};
+  int gripper_default_torque_{0};
+  int gripper_default_mode_{0};
+
+  int arm_cmd_type_take_photo_{100};
+  int arm_cmd_type_pre_grasp_{101};
+  int arm_cmd_type_grasp_{102};
+  int arm_cmd_type_take_object_{110};
+  int arm_cmd_type_custom_min_{100};
+  int arm_cmd_type_custom_max_{120};

  // Optional per-skill override timeout (skill_name -> seconds)
  std::unordered_map<std::string, double> skill_timeouts_;
@@ -73,19 +150,31 @@ private:
  // Statistics: per-skill success/failure counters & sequence totals
  std::unordered_map<std::string, size_t> skill_success_counts_;
  std::unordered_map<std::string, size_t> skill_failure_counts_;
-  size_t total_sequence_success_ {0};
-  size_t total_sequence_failure_ {0};
+  size_t total_sequence_success_{0};
+  size_t total_sequence_failure_{0};
+
+  // ROS parameters (new additions)
+  bool enable_stats_publisher_{true};
+  int stats_publish_interval_ms_{5000};
+  std::string robot_config_path_{""};
+
  rclcpp::TimerBase::SharedPtr stats_timer_;
  double stats_log_interval_sec_ {30.0};
  std::mutex stats_mutex_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
  std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
-  std::string target_frame_{"bottle"};
  int64_t command_id_{0};
+  int64_t command_id_left_arm_ {0};
+  int64_t command_id_right_arm_{0};
+  std::unordered_map<std::string, geometry_msgs::msg::Pose> final_arm_pose_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-  std::string arm_target_frame_{"base_link_leftarm"};
-  double arm_transform_timeout_sec_{0.2};
+  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);
+  std::vector<double> right_hand_grab_width_;

  // Current accepted sequence epoch (from client) used to filter/reject stale goals
  std::atomic<uint32_t> current_epoch_{0};
@@ -103,6 +192,8 @@ private:
  std::atomic<double> nav2_last_distance_remaining_{std::numeric_limits<double>::quiet_NaN()};
  std::atomic<bool> nav2_last_goal_succeeded_{false};

+  friend class CerebellumHooks;
+
  // ---- helpers ----
  /**
   * @brief Try to parse the current goal's per-call YAML payload into a typed object.
@@ -114,8 +205,9 @@ private:
  template<typename T>
  bool TryParseCallPayload(const std::string & skill_name, T & out) const
  {
-    if (last_calls_.empty()) {return false;}
-    for (const auto & c : last_calls_) {
+    const auto * calls = tls_current_calls_ ? tls_current_calls_ : &last_calls_;
+    if (!calls || calls->empty()) {return false;}
+    for (const auto & c : *calls) {
      if (c.name == skill_name && !c.payload_yaml.empty()) {
        try {
          auto node = YAML::Load(c.payload_yaml);
@@ -131,26 +223,6 @@ private:
    }
    return false;
  }
-  /** @brief Install application-specific action hook overrides for skill manager. */
-  void ConfigureActionHooks();
-  void ConfigureArmSpaceControlHooks();
-  void ConfigureArmHooks();
-  void ConfigureHandControlHooks();
-  void ConfigureCameraTakePhotoHooks();
-  void ConfigureLegControlHooks();
-  void ConfigureVisionGraspObjectHooks();
-  void ConfigureSlamModeHooks();
-  void ConfigureNavigateToPoseHooks();
-  void ConfigureMoveWaistHooks();
-  void ConfigureMoveLegHooks();
-  void ConfigureMoveWheelHooks();
-  void ConfigureMoveHomeHooks();
-
-  /** @brief Install application-specific service hook overrides. */
-  void ConfigureServiceHooks();
-  void ConfigureVisionObjectRecognitionHooks();
-  void ConfigureMapLoadHooks();
-  void ConfigureMapSaveHooks();

  /**
   * @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
@@ -258,6 +330,7 @@ private:
   * @brief Internal helper: run the action worker thread and emit RUN feedback until completion.
   * Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
   * @param skill Skill name.
+   * @param interface_name Interface base name for dispatching.
   * @param topic Action topic.
   * @param timeout_ns Timeout duration (nanoseconds).
   * @param index Skill index.
@@ -268,8 +341,11 @@ private:
   */
  bool RunActionSkillWithProgress(
    const std::string & skill,
+    const std::string & interface_name,
    const std::string & topic,
    std::chrono::nanoseconds timeout_ns,
+    const std::string & instance_name,
+    uint32_t timeout_ms_override,
    int index,
    int total_skills,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
@@ -279,6 +355,7 @@ private:
   * @brief Internal helper: run bilateral arm actions concurrently.
   * Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
   * @param skill Skill name.
+   * @param interface_name Interface base name for dispatching.
   * @param topic Action topic.
   * @param timeout Action timeout.
   * @param index Skill index.
@@ -289,8 +366,11 @@ private:
   */
  bool ExecuteBilateralArmAction(
    const std::string & skill,
+    const std::string & interface_name,
    const std::string & topic,
    std::chrono::nanoseconds timeout,
+    const std::string & instance_name,
+    uint32_t timeout_ms_override,
    int index,
    int total_skills,
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
@@ -303,7 +383,7 @@ private:
  * @param detail [out] Detail / diagnostics.
  * @param start_progress Sequence progress prior to executing the service.
  * @param goal_handle Goal handle (used for feedback publishing).
-   * @return true if successful.
+  * @return true if successful.
   */
  bool ExecuteServiceSkill(
    const std::string & skill,
@@ -355,6 +435,9 @@ private:
   * @return Topic / action client key.
   */
  std::string ResolveTopicForSkill(const std::string & skill) const;
+  std::string ResolveTopicForSkillFromCalls(
+    const std::vector<interfaces::msg::SkillCall> & calls,
+    const std::string & skill) const;
  /**
   * @brief Finalize goal with summary result (success or abort) and log message.
   * @param goal_handle Goal handle.
@@ -379,9 +462,17 @@ private:
  /** @brief Register ExecuteBtAction server with goal/execute handlers. */
  void SetupExecuteBtServer();

+  void RunExecuteBtGoalConcurrent(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
+
+  brain::CerebellumData::ExecResult ExecuteSequenceConcurrent(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
+    const std::vector<std::string> & skills,
+    const std::shared_ptr<GoalExecutionContext> & ctx);
+
  /** @brief 将检测到的姿态转换到机械臂参考坐标系。 */
  bool TransformPoseToArmFrame(
-    const geometry_msgs::msg::PoseStamped & source,
+    const geometry_msgs::msg::PoseStamped & source, const std::string & arm_target_frame,
    geometry_msgs::msg::PoseStamped & transformed) const;

  // NOTE: Additional future helpers (cancellation handling, concurrency) may be added here.
--- a/src/brain/include/brain/cerebrum_node.hpp
+++ b/src/brain/include/brain/cerebrum_node.hpp
@@ -118,6 +118,20 @@ public:
   */
  static std::vector<std::string> ParseListString(const std::string & raw);

+  /**
+   * @brief Split a string by comma, ignoring commas inside quotes (single or double).
+   * @param raw Input string.
+   * @return Vector of tokens.
+   */
+  static std::vector<std::string> SplitQuotedString(const std::string & raw);
+
+  /**
+   * @brief Split a string by comma, preserving order and duplicates.
+   * @param raw Input string.
+   * @return Vector of tokens.
+   */
+  static std::vector<std::string> SplitString(const std::string & raw);
+
  /**
   * @brief Classify textual result detail (prefix or code= token) into ExecResultCode.
   * @param detail Input detail string.
@@ -173,7 +187,10 @@ private:
  /** Declare per-skill instance parameter entries used to customize outgoing calls. */
  bool UpdateBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name, const std::string & instance_params);
  /** Declare per-instance parameter entries used to customize a specific BT node occurrence. */
-  bool DeclareBtActionParamsForSkillInstance(const std::string & skill_name, const std::string & instance_name);
+  bool DeclareBtActionParamsForSkillInstance(
+    const std::string & skill_name,
+    const std::string & instance_name,
+    std::string * out_instance_params = nullptr);
  /** Build a SkillCall message for given skill by consulting SkillSpec and per-skill params. */
  interfaces::msg::SkillCall BuildSkillCallForSkill(const std::string & skill_name) const;
  /** Build a SkillCall with optional per-instance override (cerebrum.bt.<Skill>.<Instance>.*). */
@@ -203,13 +220,25 @@ private:
  rclcpp::Service<interfaces::srv::BtRebuild>::SharedPtr rebuild_service_;
  std::atomic<bool> bt_updating_{false};
  std::atomic<bool> bt_pending_run_{false};
+  rclcpp::Time bt_execution_start_time_;
  // Encapsulated epoch+skill filter
  EpochSkillFilter epoch_filter_;
  // Current sequence skills (kept in original order for logging / statistics).
  std::vector<std::string> current_sequence_skills_;
+  // Map of sequence index to override params for the current sequence.
+  std::unordered_map<size_t, std::string> active_sequence_param_overrides_;
  // Mutex that protects both per_node_exec_ and epoch_skills_ updates.
  std::mutex exec_mutex_;

+  // Tracking currently running BT leaf instances (for serial/parallel reporting).
+  std::unordered_set<std::string> running_leaf_keys_;
+  // Index map for active RobotWorkInfo.action entries (key -> vector index).
+  std::unordered_map<std::string, std::size_t> action_index_by_key_;
+  // Parallel vector of action keys aligned with robot_work_info_.action.
+  std::vector<std::string> action_keys_;
+  // Map from BT node instance name to execution mode based on BT structure.
+  std::unordered_map<std::string, std::string> action_exec_by_node_name_;
+
  rclcpp::Publisher<interfaces::msg::RobotWorkInfo>::SharedPtr robot_work_info_pub_;
  interfaces::msg::RobotWorkInfo robot_work_info_;

@@ -237,6 +266,18 @@ private:
    std::optional<std::string> current_instance;
  };
  std::unordered_map<std::string, PerNodeExecInfo> per_node_exec_;
+
+  struct PerInstanceExecInfo
+  {
+    std::atomic<bool> in_flight {false};
+    std::atomic<bool> finished {false};
+    bool success {false};
+    ExecResultCode result_code {ExecResultCode::UNKNOWN};
+    rclcpp::Time start_time;
+    double last_progress {0.0};
+  };
+  std::unordered_map<std::string, PerInstanceExecInfo> per_instance_exec_;
+  std::unordered_set<std::string> done_instances_epoch_;
  // Set of already registered action client names (prevents duplicate registration & duplicate feedback).
  std::unordered_set<std::string> registered_actions_;
  // Per-BT-node execution timeout (seconds).
@@ -253,11 +294,24 @@ private:
  std::vector<std::string> allowed_action_skills_;
  // Fixed skill sequence (if set, random selection is skipped entirely).
  std::optional<std::vector<std::string>> fixed_skill_sequence_;
+  
+  // ROS parameters
+  double tick_rate_hz_{10.0};
+  int rebuild_interval_ms_{5000};
+  int stats_publish_interval_ms_{10000};
+  std::string robot_config_path_{""};
+  std::string skill_file_path_{""};
+  double default_bt_timeout_{300.0};
+  bool enable_sequence_rebuild_{true};
+
  /**
   * @brief Cancel the currently active ExecuteBtAction goal (if any).
   * @thread_safety Calls into ActionClientRegistry; assumed thread-safe for cancellation.
   */
  void CancelActiveExecuteBtGoal();
+
+  void StopBehaviorTree(bool cancel_goals);
+
  /**
   * @brief (Legacy) Update per-epoch allowed skills. Currently a no-op (handled by EpochSkillFilter).
   * @param skills Ignored.
@@ -366,16 +420,33 @@ private:
  
  /** Handle Trigger type rebuild request */
  void HandleTriggerRebuild(
+    const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
    const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
  
-  /** Handle Remote type rebuild request */
-  void HandleRemoteRebuild(
+  /** Handle Generic (Remote/Sequence) type rebuild request */
+  void HandleGenericRebuild(
    const std::shared_ptr<interfaces::srv::BtRebuild::Request> req,
    const std::shared_ptr<interfaces::srv::BtRebuild::Response> resp);
  
  /** Extract filename without extension from a path */
  std::string ExtractFilenameWithoutExtension(const std::string & path);

+  // Update robot_work_info_ leaf execution mode based on running_leaf_keys_.
+  void UpdateLeafParallelismStatusLocked();
+
+  // Manage RobotWorkInfo.action entries (guarded by exec_mutex_).
+  void AddActionInfoLocked(
+    const std::string & key,
+    const std::string & skill,
+    const std::string & action_name,
+    const std::string & params);
+  void UpdateActionInfoStateLocked(const std::string & key, const std::string & state);
+  void RemoveActionInfoLocked(const std::string & key);
+  void ClearActionInfoLocked();
+
+  // Build map of BT node instance name -> execution mode (serial/parallel).
+  void BuildActionExecutionMapLocked();
+
  // Temporary override used so the generic ExecuteBtAction goal builder can
  // send a single-skill goal when a per-skill BT node starts.
  std::optional<std::string> single_skill_goal_override_;
--- a/src/brain/include/brain/payload_converters.hpp
+++ b/src/brain/include/brain/payload_converters.hpp
@@ -9,6 +9,8 @@
 #include "interfaces/action/arm.hpp"
 #include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/gripper_cmd.hpp"
 #include "interfaces/action/hand_control.hpp"
 #include "interfaces/action/leg_control.hpp"
 #include "interfaces/action/move_home.hpp"
@@ -18,8 +20,12 @@
 #include "interfaces/action/move_wheel.hpp"
 #include "interfaces/action/slam_mode.hpp"
 #include "interfaces/action/vision_grasp_object.hpp"
+#include "interfaces/srv/bt_rebuild.hpp"
+#include "interfaces/srv/gripper_cmd.hpp"
 #include "interfaces/srv/map_load.hpp"
 #include "interfaces/srv/map_save.hpp"
+#include "interfaces/srv/motor_info.hpp"
+#include "interfaces/srv/motor_param.hpp"
 #include "interfaces/srv/vision_object_recognition.hpp"


@@ -153,6 +159,70 @@ inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome
  return g;
 }

+// GripperCmd::Goal: loc, speed, torque, mode
+template<>
+inline interfaces::action::GripperCmd::Goal from_yaml<interfaces::action::GripperCmd::Goal>(const YAML::Node & n)
+{
+  interfaces::action::GripperCmd::Goal g;
+  if (n && n.IsMap()) {
+    if (n["loc"]) g.loc = n["loc"].as<uint8_t>();
+    if (n["speed"]) g.speed = n["speed"].as<uint8_t>();
+    if (n["torque"]) g.torque = n["torque"].as<uint8_t>();
+    if (n["mode"]) g.mode = n["mode"].as<uint8_t>();
+  }
+  return g;
+}
+
+// ExecuteBtAction::Goal: epoch, action_name, calls
+template<>
+inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::ExecuteBtAction::Goal>(const YAML::Node & n)
+{
+  interfaces::action::ExecuteBtAction::Goal g;
+  if (n && n.IsMap()) {
+    if (n["epoch"]) g.epoch = n["epoch"].as<uint32_t>();
+    if (n["action_name"]) g.action_name = n["action_name"].as<std::string>();
+    // TODO: parse field `calls` of ROS type `interfaces/SkillCall[]` into g.calls (complex type)
+  }
+  return g;
+}
+
+// LegControl::Goal: target
+template<>
+inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
+{
+  interfaces::action::LegControl::Goal g;
+  if (n && n.IsMap()) {
+    // geometry_msgs::msg::TwistStamped
+    if (n) {
+      if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
+      if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
+      if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
+      if (auto lin = n["linear"]) {
+        if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
+        if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
+        if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
+      }
+      if (auto ang = n["angular"]) {
+        if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
+        if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
+        if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
+      }
+    }
+  }
+  return g;
+}
+
+// VisionGraspObject::Goal: object_id
+template<>
+inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
+{
+  interfaces::action::VisionGraspObject::Goal g;
+  if (n && n.IsMap()) {
+    if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
+  }
+  return g;
+}
+
 // MoveToPosition::Goal: target_x, target_y
 template<>
 inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
@@ -203,41 +273,61 @@ inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::A
  return g;
 }

-// LegControl::Goal: target
+// GripperCmd::Request: devid, loc, speed, torque
 template<>
-inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
+inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
 {
-  interfaces::action::LegControl::Goal g;
+  interfaces::srv::GripperCmd::Request r;
  if (n && n.IsMap()) {
-    // geometry_msgs::msg::TwistStamped
-    if (n) {
-      if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
-      if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
-      if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
-      if (auto lin = n["linear"]) {
-        if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
-        if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
-        if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
-      }
-      if (auto ang = n["angular"]) {
-        if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
-        if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
-        if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
-      }
-    }
+    if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
+    if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
+    if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
+    if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
  }
-  return g;
+  return r;
 }

-// VisionGraspObject::Goal: object_id
+// MotorInfo::Request: target, type, motor_ids
 template<>
-inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
+inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo::Request>(const YAML::Node & n)
 {
-  interfaces::action::VisionGraspObject::Goal g;
+  interfaces::srv::MotorInfo::Request r;
  if (n && n.IsMap()) {
-    if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
+    if (n["target"]) r.target = n["target"].as<std::string>();
+    if (n["type"]) r.type = n["type"].as<std::string>();
+    if (n["motor_ids"]) {
+      auto vec = n["motor_ids"].as<std::vector<uint8_t>>();
+      r.motor_ids.assign(vec.begin(), vec.end());
+    }
  }
-  return g;
+  return r;
+}
+
+// MotorParam::Request: motor_id, max_speed, add_acc, dec_acc
+template<>
+inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorParam::Request>(const YAML::Node & n)
+{
+  interfaces::srv::MotorParam::Request r;
+  if (n && n.IsMap()) {
+    if (n["motor_id"]) r.motor_id = n["motor_id"].as<uint16_t>();
+    if (n["max_speed"]) r.max_speed = n["max_speed"].as<uint16_t>();
+    if (n["add_acc"]) r.add_acc = n["add_acc"].as<uint16_t>();
+    if (n["dec_acc"]) r.dec_acc = n["dec_acc"].as<uint16_t>();
+  }
+  return r;
+}
+
+// BtRebuild::Request: type, config, param
+template<>
+inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
+{
+  interfaces::srv::BtRebuild::Request r;
+  if (n && n.IsMap()) {
+    if (n["type"]) r.type = n["type"].as<std::string>();
+    if (n["config"]) r.config = n["config"].as<std::string>();
+    if (n["param"]) r.param = n["param"].as<std::string>();
+  }
+  return r;
 }

 } // namespace brain
--- a/src/brain/include/brain/skill_config.hpp
+++ b/src/brain/include/brain/skill_config.hpp
@@ -6,6 +6,7 @@

 #include "interfaces/action/arm.hpp"
 #include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/gripper_cmd.hpp"
 #include "interfaces/action/hand_control.hpp"
 #include "interfaces/action/move_home.hpp"
 #include "interfaces/action/move_leg.hpp"
@@ -30,7 +31,8 @@ using SkillActionTypes = std::tuple<
  interfaces::action::MoveWaist,
  interfaces::action::MoveLeg,
  interfaces::action::MoveWheel,
-  interfaces::action::MoveHome
+  interfaces::action::MoveHome,
+  interfaces::action::GripperCmd
 >;

 using SkillServiceTypes = std::tuple<
@@ -44,7 +46,7 @@ struct SkillActionTrait<interfaces::action::Arm>
 {
  static constexpr const char * skill_name = "Arm";
  static constexpr const char * default_topic = "ArmAction";
-  static bool success(const interfaces::action::Arm::Result & r) {return (r.result == 0);}
+  static bool success(const interfaces::action::Arm::Result & r, const std::string & instance_name) {(void)instance_name; return (r.result == 0);}
  static std::string message(const interfaces::action::Arm::Result & r) {(void)r; return "completed";}
 };

@@ -53,7 +55,7 @@ struct SkillActionTrait<interfaces::action::CameraTakePhoto>
 {
  static constexpr const char * skill_name = "CameraTakePhoto";
  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::CameraTakePhoto::Result & r) {return r.success;}
+  static bool success(const interfaces::action::CameraTakePhoto::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
  static std::string message(const interfaces::action::CameraTakePhoto::Result & r) {return r.message;}
 };

@@ -62,7 +64,7 @@ struct SkillActionTrait<interfaces::action::HandControl>
 {
  static constexpr const char * skill_name = "HandControl";
  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
+  static bool success(const interfaces::action::HandControl::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
  static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
 };

@@ -70,8 +72,8 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveWaist>
 {
  static constexpr const char * skill_name = "MoveWaist";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveWaist::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveWaist";
+  static bool success(const interfaces::action::MoveWaist::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
  static std::string message(const interfaces::action::MoveWaist::Result & r) {return r.message;}
 };

@@ -79,8 +81,8 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveLeg>
 {
  static constexpr const char * skill_name = "MoveLeg";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveLeg::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveLeg";
+  static bool success(const interfaces::action::MoveLeg::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
  static std::string message(const interfaces::action::MoveLeg::Result & r) {return r.message;}
 };

@@ -88,8 +90,8 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveWheel>
 {
  static constexpr const char * skill_name = "MoveWheel";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveWheel::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveWheel";
+  static bool success(const interfaces::action::MoveWheel::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
  static std::string message(const interfaces::action::MoveWheel::Result & r) {return r.message;}
 };

@@ -97,11 +99,35 @@ template<>
 struct SkillActionTrait<interfaces::action::MoveHome>
 {
  static constexpr const char * skill_name = "MoveHome";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::MoveHome::Result & r) {return r.success;}
+  static constexpr const char * default_topic = "MoveHome";
+  static bool success(const interfaces::action::MoveHome::Result & r, const std::string & instance_name) {(void)instance_name; return r.success;}
  static std::string message(const interfaces::action::MoveHome::Result & r) {return r.message;}
 };

+template<>
+struct SkillActionTrait<interfaces::action::GripperCmd>
+{
+  static constexpr const char * skill_name = "GripperCmd";
+  static constexpr const char * default_topic = "gripper_cmd0";
+  static bool success(const interfaces::action::GripperCmd::Result & r, const std::string & instance_name) {
+    (void)instance_name; 
+
+    if (r.result == 0) return false;
+
+    //如果instance_name中带有"open"，则表示是打开手指的命令
+    if (instance_name.find("open") != std::string::npos) {
+      //如果r.state中包含"手指已到达指定的位置"，则认为成功
+      return r.state.find("手指已到达指定的位置") != std::string::npos;
+    } else if (instance_name.find("close") != std::string::npos) {
+      //如果r.state中包含"手指在闭合检测到物体"，则认为成功
+      return (r.state.find("手指在闭合检测到物体") != std::string::npos);
+    } else {
+      return false;
+    }
+  }
+  static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
+};
+
 template<>
 struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
 {
--- a/src/brain/include/brain/skill_manager.hpp
+++ b/src/brain/include/brain/skill_manager.hpp
@@ -287,10 +287,17 @@ private:
    const std::string & topic,
    const std::string & internal_skill)
  {
-    auto it = action_override_registrars_.find(base);
+    // PRIORITY 1: Match exact skill name (e.g. "GripperCmd0") for highly specific overrides
+    auto it = action_override_registrars_.find(internal_skill);
+    // PRIORITY 2: Match interface base name (e.g. "GripperCmd") for generic overrides
+    if (it == action_override_registrars_.end()) {
+      it = action_override_registrars_.find(base);
+    }
+
    if (it != action_override_registrars_.end()) {
      it->second(topic, internal_skill);
-      RCLCPP_INFO(node_->get_logger(), "Overridden action client registration: %s", base.c_str());
+      RCLCPP_INFO(node_->get_logger(), "Registered action client '%s' via Hook on '%s'", 
+        internal_skill.c_str(), it->first.c_str());
      return;
    }
    using GoalHandle = typename ActionClientRegistry::GoalHandle<ActionT>;
@@ -324,7 +331,7 @@ private:
    if (!result_cb) {
      result_cb = [this, internal_skill](const typename GoalHandle::WrappedResult & res) {
          if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
-            SkillActionTrait<ActionT>::success(*res.result)) {
+            SkillActionTrait<ActionT>::success(*res.result, "")) {
            RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
          } else if (res.code == rclcpp_action::ResultCode::CANCELED) {
            RCLCPP_WARN(node_->get_logger(), "action cancelled: %s", internal_skill.c_str());
@@ -365,7 +372,7 @@ private:
 template<size_t I = 0, typename TupleT, typename RegistryT>
 bool dispatch_skill_action(
  const std::string & skill, const std::string & topic, RegistryT * reg,
-  rclcpp::Logger logger, std::chrono::nanoseconds timeout, std::string & detail)
+  rclcpp::Logger logger, std::chrono::nanoseconds timeout, const std::string & instance_name, std::string & detail)
 {
  if constexpr (I == std::tuple_size<TupleT>::value) {
    return false;
@@ -374,7 +381,7 @@ bool dispatch_skill_action(
    if (skill == SkillActionTrait<ActionT>::skill_name) {
      auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
      if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
-        bool ok = SkillActionTrait<ActionT>::success(*opt->result);
+        bool ok = SkillActionTrait<ActionT>::success(*opt->result, instance_name);
        detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
          "result flag false");
        return ok;
@@ -382,7 +389,7 @@ bool dispatch_skill_action(
      detail = "action failed";
      return false;
    }
-    return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, detail);
+    return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, instance_name, detail);
  }
 }

--- a/src/brain/include/brain/sm_cerebellum.hpp
+++ b/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());
--- a/src/brain/launch/brain.launch.py
+++ b/src/brain/launch/brain.launch.py
@@ -0,0 +1,42 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('brain'),
+            'config',
+            'brain_node_params.yaml'
+        ]),
+        description='Full path to params file to load'
+    )
+
+    # Get the params file path
+    params_file = LaunchConfiguration('params_file')
+
+    # Create the brain node that runs both cerebrum and cerebellum
+    brain_node = Node(
+        package='brain',
+        executable='brain_node',
+        # name='brain_node',
+        parameters=[params_file],
+        output='screen',
+        emulate_tty=True
+    )
+
+    # Create the launch description
+    ld = LaunchDescription()
+
+    # Declare launch arguments
+    ld.add_action(declare_params_file_cmd)
+
+    # Add nodes
+    ld.add_action(brain_node)
+
+    return ld
--- a/src/brain/launch/brain_with_params.launch.py
+++ b/src/brain/launch/brain_with_params.launch.py
@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import os
+
+
+def launch_setup(context, *args, **kwargs):
+    # Get the params file path
+    params_file = LaunchConfiguration('params_file').perform(context)
+    
+    # Create the brain node that runs both cerebrum and cerebellum
+    brain_node = Node(
+        package='brain',
+        executable='brain_node',
+        name='brain_node',
+        parameters=[params_file],
+        output='screen',
+        emulate_tty=True,
+        # Override specific parameters if needed
+        arguments=[
+            # Example of overriding individual parameters via command line
+            # Uncomment and modify as needed
+            # "--ros-args", 
+            # "-p", "tick_rate_hz:=20.0",
+            # "-p", "default_action_timeout_sec:=60.0"
+        ]
+    )
+
+    return [brain_node]
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('brain'),
+            'config',
+            'brain_node_params.yaml'
+        ]),
+        description='Full path to params file to load'
+    )
+
+    # Create the launch description
+    ld = LaunchDescription()
+
+    # Declare launch arguments
+    ld.add_action(declare_params_file_cmd)
+
+    # Add the opaque function to set up the nodes
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld
--- a/src/brain/package.xml
+++ b/src/brain/package.xml
@@ -13,7 +13,6 @@
  <depend>rclcpp_components</depend>
  <depend>behaviortree_cpp</depend>
  <depend>interfaces</depend>
-  <depend>brain_interfaces</depend>
  <depend>rclcpp_lifecycle</depend>
  <depend>rclcpp_action</depend>
  <depend>smacc2</depend>
--- a/src/brain/src/brain_node.cpp
+++ b/src/brain/src/brain_node.cpp
@@ -25,8 +25,11 @@ int main(int argc, char ** argv)
  rclcpp::Logger logger = rclcpp::get_logger("brain_node");
  RCLCPP_INFO(logger, "Starting brain node...");

-  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  // Create node options with parameter overrides if provided
  rclcpp::NodeOptions options;
+  // If command line arguments are provided, they will be handled by rclcpp::init
+  
+  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();

  auto cerebellum_node = std::make_shared<brain::CerebellumNode>(options);
  auto cerebrum_node = std::make_shared<brain::CerebrumNode>(options);
--- a/src/brain/src/calc_grasp_pose.cpp
+++ b/src/brain/src/calc_grasp_pose.cpp
@@ -0,0 +1,202 @@
+#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>
+
+namespace brain {
+
+bool GraspPoseCalculator::rm_arm_algo_init_done_ = false;
+
+GraspPoseCalculator::GraspPoseCalculator()
+{
+}
+
+tf2::Vector3 normalize_vector(const tf2::Vector3& v) {
+    if (v.length() == 0) return v;
+    return v.normalized();
+}
+
+GraspResult GraspPoseCalculator::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
+) {
+    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;
+    T_pre.setOrigin(T_pre.getOrigin() - z_axis * safety_dist);
+    
+    GraspResult result;
+    result.grasp_pose.position = T_grasp.getOrigin();
+    result.grasp_pose.orientation = T_grasp.getRotation();
+    result.pre_grasp_pose.position = T_pre.getOrigin();
+    result.pre_grasp_pose.orientation = T_pre.getRotation();
+    
+    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;
+
+    bool is_left = (arm.find("left") != std::string::npos);
+
+    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);
+    if (V_horiz.length() < 1e-3) {
+        V_horiz = tf2::Vector3(0.0, 1.0, 0.0); // Default horizontal vector
+    } else {
+        V_horiz = normalize_vector(V_horiz);
+    }
+    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);
+        if (is_left) {
+            dy = -dy;
+        }
+        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);
+    // In this frame (Z is Right), angle > 0 moves towards Z (Right/Outward)
+    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;
+}
+
+} // namespace brain
--- a/src/brain/src/cerebellum_hooks.cpp
+++ b/src/brain/src/cerebellum_hooks.cpp
--- a/src/brain/src/cerebellum_node.cpp
+++ b/src/brain/src/cerebellum_node.cpp
--- a/src/brain/src/cerebrum_node.cpp
+++ b/src/brain/src/cerebrum_node.cpp
--- a/src/brain/src/skill_manager.cpp
+++ b/src/brain/src/skill_manager.cpp
@@ -311,6 +311,8 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
    return prefix + "/" + base;
  };

+  const std::string skill_snake = to_snake_case(s.name);
+
  for (const auto & iface : s.interfaces) {
    auto parsed = parse_interface_entry(iface);
    if (!parsed) {
@@ -323,8 +325,8 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
    const std::string base_snake = to_snake_case(parsed->base);

    auto resolve_topic_name = [&](const std::string & kind_suffix) {
-      // Compute the prior default (prefix + base_snake), but allow per-trait default topic when provided.
-      const std::string prefix_default = join_topic(prefix_value, base_snake);
+      // Compute the prior default (prefix + skill_snake), but allow per-trait default topic when provided.
+      const std::string prefix_default = join_topic(prefix_value, skill_snake);
      const auto & action_defaults = get_action_default_topics();
      const auto & service_defaults = get_service_default_topics();
      std::string trait_default;
@@ -342,8 +344,10 @@ void SkillManager::register_interfaces_(const SkillSpec & s)
        yaml_override = it_yaml->second;
      }
      const bool has_trait_default = !trait_default.empty();
-      const std::string computed_default = !yaml_override.empty() ? yaml_override : (has_trait_default ? trait_default : prefix_default);
-      const std::string param_name = base_snake + "." + kind_suffix + "_name";
+      // If skill name doesn't match interface base, it's a specific instance; prefer prefix_default (skill_snake) over generic trait_default.
+      const std::string computed_default = !yaml_override.empty() ? yaml_override : 
+                                           ((skill_snake == base_snake && has_trait_default) ? trait_default : prefix_default);
+      const std::string param_name = skill_snake + "." + kind_suffix + "_name";
      std::string topic_override;
      if (node_->get_parameter(param_name, topic_override)) {
        // If user explicitly sets an empty parameter, fall back to computed_default.
--- a/src/brain_interfaces/CMakeLists.txt
+++ b/src/brain_interfaces/CMakeLists.txt
@@ -1,56 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(brain_interfaces)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(builtin_interfaces REQUIRED)
-find_package(interfaces REQUIRED)
-
-set(msg_files
-  msg/Movej.msg
-)
-set(action_files
-)
-set(srv_files
-  srv/VisionDemo.srv
-)
-
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-rosidl_generate_interfaces(${PROJECT_NAME}
-  ${msg_files}
-  ${action_files}
-  ${srv_files}
-  
-  DEPENDENCIES
-    std_msgs
-    geometry_msgs
-    builtin_interfaces
-    interfaces
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_export_dependencies(rosidl_default_runtime)
-ament_package()
--- a/src/brain_interfaces/msg/Movej.msg
+++ b/src/brain_interfaces/msg/Movej.msg
@@ -1,5 +0,0 @@
-float32[] joint
-uint8 speed
-bool block
-uint8 trajectory_connect   #0 代表立即规划，1 代表和下一条轨迹一起规划，当为 1 时，轨迹不会立即执行
-uint8 dof
--- a/src/brain_interfaces/package.xml
+++ b/src/brain_interfaces/package.xml
@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>brain_interfaces</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="root@todo.todo">root</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>rosidl_default_generators</build_depend>
-  <depend>std_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>interfaces</depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <member_of_group>rosidl_interface_packages</member_of_group>
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
--- a/src/brain_interfaces/srv/VisionDemo.srv
+++ b/src/brain_interfaces/srv/VisionDemo.srv
@@ -1,8 +0,0 @@
-string camera_position
-
---
-
-std_msgs/Header header
-string info
-bool success
-interfaces/PoseClassAndID[] objects
--- a/src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
+++ b/src/modules/vision/vision_object_recg/src/vision_object_recg_node.cpp
@@ -44,64 +44,48 @@ private:

      // Fake two types of objects (example: cup / bottle), each type gives 1~2 Poses
      interfaces::msg::PoseClassAndID obj1;
-      obj1.class_name = "bottlex";
+      obj1.class_name = "medicine_box";
      obj1.class_id = 1;

-      //{-0.9758, 0, 0.42, -0.5, 0.5, 0.5, -0.5};
      geometry_msgs::msg::Pose pose1; // 基本位姿
-      // pose1.position.x = 0.9758;
-      // pose1.position.y = 0;
-      // pose1.position.z = 0.42;
-      // pose1.orientation.x = -0.5;
-      // pose1.orientation.y = 0.5;
-      // pose1.orientation.z = 0.5;
-      // pose1.orientation.w = -0.5;

-      //geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=-0.2851924786746129, y=-0.056353812689333635, z=1.073772448259523), 
-      //orientation=geometry_msgs.msg.Quaternion(x=-0.16713377464857188, y=-0.42460237568763715, z=-0.26706232441180955, w=0.8487972895879773))
-      // pose1.position.x = -0.2851924786746129;
-      // pose1.position.y = -0.056353812689333635;
-      // pose1.position.z = 1.073772448259523;
-      // pose1.orientation.x = -0.16713377464857188;
-      // pose1.orientation.y = -0.42460237568763715;
-      // pose1.orientation.z = -0.26706232441180955;
-      // pose1.orientation.w = 0.8487972895879773;
+      if (req->camera_position == "top") {
+        //0.1647, 0.7436, -0.2081, 0.4809, 0.7398, 0.4006, 0.2469
+        pose1.position.x = 0.1647;
+        pose1.position.y = 0.7436;
+        pose1.position.z = -0.2081;
+        pose1.orientation.x = 0.4809;
+        pose1.orientation.y = 0.7398;
+        pose1.orientation.z = 0.4006;
+        pose1.orientation.w = 0.2469;
+      } else if (req->camera_position == "left") {
+        //0.2320, -0.6137, 0.2141, 0.3670, 0.7705, -0.4314, 0.2924
+        pose1.position.x = 0.2320;
+        pose1.position.y = 0.6137;
+        pose1.position.z = 0.2141;
+        pose1.orientation.x = -0.3670;
+        pose1.orientation.y = 0.7705;
+        pose1.orientation.z = 0.4314;
+        pose1.orientation.w = 0.2924;
+      } else if (req->camera_position == "right") {
+        //-0.0165, 0.0246, 0.4166, 0.2939, -0.2219, -0.6882, 0.6251
+        pose1.position.x = -0.0165;
+        pose1.position.y = 0.0246;
+        pose1.position.z = 0.4166;
+        pose1.orientation.x = 0.2939;
+        pose1.orientation.y = -0.2219;
+        pose1.orientation.z = -0.6882;
+        pose1.orientation.w = 0.6251;
+      }

-      //222.853, 514.124, 261.742
-      //-0.65115316, 0.05180144, -0.19539139, 0.73153153
-      //-0.63142093, 0.18186004, -0.12407289, 0.74353241
-      pose1.position.x = 222.853;
-      pose1.position.y = 514.124;
-      pose1.position.z = 261.742;
-      pose1.orientation.x = -0.65115316;
-      pose1.orientation.y = 0.05180144;
-      pose1.orientation.z = -0.19539139;
-      pose1.orientation.w = 0.73153153;
-
-      obj1.pose_list.push_back(pose1);
-
-      // 第二个同类实例(示例)
-      geometry_msgs::msg::Pose pose1b = pose1;
-      pose1b.position.y += 0.05;
-      obj1.pose_list.push_back(pose1b);
-
-      interfaces::msg::PoseClassAndID obj2;
-      obj2.class_name = "cup";
-      obj2.class_id = 2;
-      geometry_msgs::msg::Pose pose2;
-      pose2.position.x = 0.5;
-      pose2.position.y = -0.2;
-      pose2.position.z = 0.78;
-      pose2.orientation.w = 1.0;
-      obj2.pose_list.push_back(pose2);
+      obj1.pose = pose1;
+      obj1.grab_width = {0.046393, 0.065393, 0.046393}; // Example grab widths

      res->objects.clear();
      res->objects.emplace_back(obj1);
-      res->objects.emplace_back(obj2);

      std::ostringstream oss;
-      oss << "Recognized " << res->objects.size() << " object classes (total instances: "
-          << (obj1.pose_list.size() + obj2.pose_list.size()) << ")";
+      oss << "Recognized " << res->objects.size() << " object classes (total instances: " << res->objects.size() << ")";
      res->info = oss.str();
    } else {
      res->success = false;
--- a/src/scripts/build.sh
+++ b/src/scripts/build.sh
@@ -1,4 +1,4 @@
 #!/bin/bash

-colcon build --packages-select brain_interfaces smacc2_msgs smacc2 brain
-#colcon build --packages-select brain_interfaces smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control
+colcon build --packages-select smacc2_msgs smacc2 brain
+#colcon build --packages-select smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control
--- a/src/scripts/euler_to_equa.py
+++ b/src/scripts/euler_to_equa.py
--- a/src/scripts/euler_to_quaternion.py
+++ b/src/scripts/euler_to_quaternion.py
--- a/src/scripts/gen_bt_params.py
+++ b/src/scripts/gen_bt_params.py
@@ -116,25 +116,25 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
    return fields


-def locate_action_file(interfaces_root: str, base: str) -> Optional[str]:
-    action_dir = resolve_interface_subdir(interfaces_root, 'action')
+def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[str]:
+    action_dir = resolve_interface_subdir(interfaces_root, type)
    # 1) Try as-is (CamelCase)
-    candidate = os.path.join(action_dir, f'{base}.action')
+    candidate = os.path.join(action_dir, f'{base}.{type}')
    if os.path.exists(candidate):
        return candidate
    # 2) Try CamelCase of snake
    camel = snake_to_camel(base)
    if camel != base:
-        candidate2 = os.path.join(action_dir, f'{camel}.action')
+        candidate2 = os.path.join(action_dir, f'{camel}.{type}')
        if os.path.exists(candidate2):
            return candidate2
    # 3) Try snake_case
    snake = to_snake(base)
-    candidate3 = os.path.join(action_dir, f'{snake}.action')
+    candidate3 = os.path.join(action_dir, f'{snake}.{type}')
    if os.path.exists(candidate3):
        return candidate3
    # 4) Case-insensitive lookup
-    found = find_file_case_insensitive(action_dir, f'{base}.action')
+    found = find_file_case_insensitive(action_dir, f'{base}.{type}')
    if found:
        return found
    # Not found
@@ -220,6 +220,7 @@ def main():
    ap = argparse.ArgumentParser(description='Generate YAML params for BT nodes.')
    ap.add_argument('--xml', default='src/brain/config/bt_carry_boxes.xml', help='Path to BehaviorTree XML')
    ap.add_argument('--interfaces', default='../hivecore_robot_interfaces', help='Path to hivecore_robot_interfaces root')
+    ap.add_argument('--skills', default='src/brain/config/robot_skills.yaml', help='Path to robot_skills.yaml')
    ap.add_argument('--out', default='src/brain/config/bt_carry_boxes.params.default.yaml', help='Output YAML path')
    args = ap.parse_args()

@@ -227,19 +228,49 @@ def main():
    repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
    xml_path = args.xml if os.path.isabs(args.xml) else os.path.join(repo_root, args.xml)
    interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
+    skills_path = args.skills if os.path.isabs(args.skills) else os.path.join(repo_root, args.skills)
    out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)

    if not os.path.exists(xml_path):
        print(f'ERROR: XML not found: {xml_path}', file=sys.stderr)
        sys.exit(2)

+    # Load mapping from skills.yaml if available
+    skill_to_interface: Dict[str, str] = {}
+    if os.path.exists(skills_path):
+        try:
+            with open(skills_path, 'r', encoding='utf-8') as f:
+                skills_data = yaml.safe_load(f)
+                if isinstance(skills_data, list):
+                    for skill in skills_data:
+                        name = skill.get('name')
+                        if not name: continue
+                        ifaces = skill.get('interfaces', [])
+                        for iface in ifaces:
+                            token = None
+                            if isinstance(iface, str):
+                                token = iface
+                            elif isinstance(iface, dict):
+                                token = iface.get('name')
+                            if token and '.' in token:
+                                base = token.split('.', 1)[0]
+                                skill_to_interface[name] = base
+                                break # Use first action/srv found
+        except Exception as e:
+            print(f"WARN: Failed to parse skills YAML {skills_path}: {e}", file=sys.stderr)
+
    nodes = collect_bt_nodes(xml_path)
    results = []

    for instance_name, node_tag in nodes:
        # Map tag to interface base by stripping trailing '_H' if present
        base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
-        action_file = locate_action_file(interfaces_root, base)
+        # If tag (like SkillName_H) refers to a skill in robot_skills.yaml, resolve to its interface
+        base = skill_to_interface.get(base, base)
+
+        action_file = locate_action_file(interfaces_root, base, "action")
+        if action_file == None:
+            action_file = locate_action_file(interfaces_root, base, "srv")
        fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
        yaml_params = build_yaml_params_from_fields(fields)
        results.append({'name': instance_name, 'params': yaml_params})
--- a/src/scripts/gen_payload_converters.py
+++ b/src/scripts/gen_payload_converters.py
@@ -348,6 +348,8 @@ def main():
            kind = kind.lower()
            header_snake = to_snake(base)
            if kind.startswith('action'):
+                if base in added_actions:
+                    continue
                includes.add(f'"interfaces/action/{header_snake}.hpp"')
                # locate .action file in common subdirs
                action_dir = resolve_interface_subdir(interfaces_root, 'action')
@@ -379,6 +381,8 @@ def main():
                converters.append(gen_converter_action(base, fields, header_snake))
                added_actions.add(base)
            elif kind.startswith('srv') or kind.startswith('service'):
+                if base in added_srvs:
+                    continue
                includes.add(f'"interfaces/srv/{header_snake}.hpp"')
                srv_dir = resolve_interface_subdir(interfaces_root, 'srv')
                tried_paths = []
--- a/src/scripts/gen_robot_config.py
+++ b/src/scripts/gen_robot_config.py
--- a/src/scripts/gen_skill_config.py
+++ b/src/scripts/gen_skill_config.py
@@ -95,6 +95,7 @@ def parse_iface_item(item):
        return None

 for entry in skills:
+    skill_name = entry.get('name', '')
    if 'interfaces' not in entry:
        continue
    for iface in entry['interfaces']:
@@ -105,13 +106,17 @@ for entry in skills:
        if kind == 'action':
            if base not in skill_action_types:
                skill_action_types.append(base)
+            # Prefer default_topic from a skill whose name matches the interface base,
+            # otherwise don't overwrite if we already have one from a prior matching or non-matching entry.
            if dtopic:
-                action_default_map[base] = dtopic
+                if base not in action_default_map or skill_name == base:
+                    action_default_map[base] = dtopic
        elif kind == 'srv':
            if base not in skill_service_types:
                skill_service_types.append(base)
            if dtopic:
-                service_default_map[base] = dtopic
+                if base not in service_default_map or skill_name == base:
+                    service_default_map[base] = dtopic

 # Helper to map base name to include path / C++ type name.
 # Assumption: interfaces live in package `interfaces` and types are under `interfaces::action::Name` or `interfaces::srv::Name`.
@@ -251,11 +256,11 @@ with OUT_HEADER.open('w') as out:
            result_fields = parse_action_result_fields(action_file_found) if action_file_found is not None else {}
            success_field, message_field, success_kind = pick_success_and_message_fields(result_fields)
            if success_kind == 'bool' and success_field is not None:
-                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{return r.{success_field};}}\n')
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; return r.{success_field};}}\n')
            elif success_kind == 'int_zero' and success_field is not None:
-                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{return (r.{success_field} == 0);}}\n')
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; return (r.{success_field} == 0);}}\n')
            else:
-                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r) {{(void)r; return true;}}\n')
+                out.write(f'  static bool success(const {action_cpp_type(a)}::Result & r, const std::string & instance_name) {{(void)instance_name; (void)r; return true;}}\n')
            if message_field is not None:
                out.write(f'  static std::string message(const {action_cpp_type(a)}::Result & r) {{return r.{message_field};}}\n')
            else:
--- a/src/scripts/hand_eye_cali.py
+++ b/src/scripts/hand_eye_cali.py
--- a/src/scripts/images_to_quaternions.py
+++ b/src/scripts/images_to_quaternions.py
--- a/src/scripts/rotate_pose.py
+++ b/src/scripts/rotate_pose.py
--- a/src/scripts/run.sh
+++ b/src/scripts/run.sh
@@ -63,11 +63,20 @@ while [[ $# -gt 0 ]]; do
 	esac
 done

-timestamp() { date +"%Y%m%d_%H%M%S"; }
+# 获取当前日期用于归档 (例如: 20260114)
+date_day() { date +"%Y%m%d"; }
+# 获取可读的时间戳用于文件名 (例如: 2026-01-14_15-30-05)
+readable_timestamp() { date +"%Y-%m-%d_%H-%M-%S"; }

-LOG_DIR="${WS_ROOT}/log/run/$(timestamp)"
+# 1. 统一存放路径到当天的目录下
+DAILY_LOG_ROOT="${WS_ROOT}/logs/$(date_day)"
+# 2. 为每次启动创建一个带时间的子目录 (保证多次运行不冲突)
+LOG_DIR="${DAILY_LOG_ROOT}/robot_logs_$(readable_timestamp)"
 mkdir -p "${LOG_DIR}"

+# 强制 ROS 2 自身的内部日志也记录到此目录，防止散落在 ~/.ros/log
+export ROS_LOG_DIR="${LOG_DIR}"
+
 pids=()
 names=()

@@ -75,12 +84,14 @@ start_node() {
 	local name="$1"; shift
 	local pkg="$1"; shift
 	local exe="$1"; shift
-	local log_file="${LOG_DIR}/${name}.log"
+	# 3. 日志文件名可配置：在此处修改格式
+	local node_ts=$(readable_timestamp)
+	local log_file="${LOG_DIR}/${name}_${node_ts}.log"

 	if (( LOG_TO_FILES )); then
 		echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} (logs: ${log_file})"
 		(
-			# Disable ANSI colors in log files for readability
+			# 禁用 ANSI 颜色以保持日志文件整洁
 			RCUTILS_COLORIZED_OUTPUT=0 exec ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}"
 		) >>"${log_file}" 2>&1 &
 	else
Author	SHA1	Message	Date
NuoDaJia02	9d19a88ac6	添加行为树xml注释	2026-02-03 09:21:44 +08:00
NuoDaJia02	ac4a1fc8f2	优化robot work info处理	2026-01-29 18:45:19 +08:00
NuoDaJia02	9047e71206	Refactor IK client: move from GraspPoseCalculator to CerebellumNode	2026-01-28 15:29:30 +08:00
NuoDaJia02	da61cf8709	add timestamp to work info	2026-01-27 18:51:29 +08:00
NuoDaJia02	c4f07c73fb	feat(brain): enhance BT sequence rebuild and parameter handling - Update BT registry to ensure unique node names with timestamps and indices. - Implement per-instance parameter overrides for sequence executions. - Add helpers for splitting quoted strings and handling generic rebuild requests. - update HandleGenericRebuild to support both Remote and Sequence types with parameter matching.	2026-01-22 17:31:06 +08:00
NuoDaJia02	3fc1de3a1a	optimize code	2026-01-19 17:55:49 +08:00
NuoDaJia02	02f7c103c7	update dual arm demo1 params	2026-01-15 16:05:04 +08:00
NuoDaJia02	c1a7205e10	优化代码和流程	2026-01-15 15:37:05 +08:00
NuoDaJia02	c929176625	参数可配置	2026-01-14 16:32:06 +08:00
NuoDaJia02	f191008b5a	optimize parameters	2026-01-13 18:26:08 +08:00
NuoDaJia02	a36903f467	modify parameters	2026-01-13 16:59:58 +08:00
NuoDaJia02	2d4a1edb10	optimize code	2026-01-12 17:55:58 +08:00
NuoDaJia02	88dceb1a1a	add wheel and waist action to sch	2026-01-12 10:13:19 +08:00
NuoDaJia02	0b6f18729c	optimize bt sch	2026-01-11 16:31:43 +08:00
NuoDaJia02	385527dc2a	test dual arm grasp sch ok	2026-01-11 14:26:00 +08:00
NuoDaJia02	2348200dc7	test left arm grasp	2026-01-09 16:57:33 +08:00
NuoDaJia02	79187e6645	update yaml	2026-01-08 17:31:49 +08:00
NuoDaJia02	1c7605e397	add left arm bt xml	2026-01-08 17:10:53 +08:00
NuoDaJia02	68563e2162	support left arm	2026-01-08 16:10:49 +08:00
NuoDaJia02	5c3994da9d	modify arm position	2026-01-08 10:08:47 +08:00
NuoDaJia02	ac6bf49e75	support two gripper	2026-01-07 13:46:55 +08:00
NuoDaJia02	4e0d53d41e	optimize code	2026-01-06 16:05:29 +08:00
NuoDaJia02	19d59c50db	update sch	2026-01-05 18:04:01 +08:00
NuoDaJia02	1ee0c96bb6	optimize sch&params	2026-01-05 17:20:21 +08:00
NuoDaJia02	4ec5ff7a05	add launch files	2026-01-04 16:40:30 +08:00
NuoDaJia02	c9fc59119e	update bt	2025-12-30 18:13:35 +08:00
NuoDaJia02	869e77d3c8	add fallback to bt	2025-12-30 15:21:24 +08:00
NuoDaJia02	5cf006a3e5	add subtree	2025-12-30 15:08:20 +08:00
NuoDaJia02	40b38dfd00	disable wheel and waist action	2025-12-30 14:23:11 +08:00
NuoDaJia02	b063044249	optimize code	2025-12-30 13:52:49 +08:00
NuoDaJia02	56ec283a44	huiyu demo 1	2025-12-30 08:55:45 +08:00
NuoDaJia02	acffa0e6db	new schedual	2025-12-29 15:14:54 +08:00
NuoDaJia02	f56490c5b1	optimize code	2025-12-27 13:29:39 +08:00
NuoDaJia02	994f2a574f	test oK	2025-12-25 19:01:01 +08:00
NuoDaJia02	7f05981c15	add gripper control	2025-12-24 16:26:33 +08:00
NuoDaJia02	34d4f4cce6	add take photo pose	2025-12-24 08:40:02 +08:00
NuoDaJia02	75b2dd5d3f	add cali	2025-12-19 10:20:52 +08:00
NuoDaJia02	0ced39b18d	test vision grasp	2025-12-16 10:02:15 +08:00
NuoDaJia02	180fbe4789	fix cal pose	2025-12-14 18:27:58 +08:00
david	afc4d983c8	modify rm lib	2025-12-04 16:46:41 +08:00
david	948785a1a8	test vision detect	2025-12-04 10:10:07 +08:00
david	9fe000911f	rm lib for ik cal	2025-12-01 13:36:18 +08:00
david	b4b3c3b80c	support srv default topic	2025-11-28 11:45:21 +08:00
david	1b69632c7b	fix error default_topic	2025-11-28 11:38:15 +08:00
david	f9992d8437	modify JzCmd -> GripperCmd	2025-11-27 10:36:20 +08:00
david	0992caae72	更新夹爪参数	2025-11-25 15:10:32 +08:00
david	4f679c77ae	add calc grasp pose	2025-11-24 18:34:12 +08:00
david	1951dd8276	move cerebellum hooks funcs to cerebellum_hooks.hpp/cpp	2025-11-24 14:39:07 +08:00
david	e8f1021af1	update grasp sch and data	2025-11-24 13:55:03 +08:00
david	79ade2d741	merge remote	2025-11-21 18:29:10 +08:00
david	53e62e2714	add JzCmd gripper support	2025-11-21 18:22:28 +08:00
david	3d79cf2eb8	add final_arm_pose_	2025-11-20 16:30:17 +08:00
david	9ecdb1be60	test vision grasp	2025-11-20 11:12:01 +08:00
NuoDaJia02	eed410e776	set timeout_ms to 500	2025-11-18 11:52:36 +08:00
NuoDaJia02	114ba598d0	transfer instance&timeout to cerebellum, disable action timeout	2025-11-12 16:41:53 +08:00
NuoDaJia02	7753f2f569	modify distance 1.0m	2025-11-12 08:41:08 +08:00
NuoDaJia02	0ba8bf79e6	new params	2025-11-11 15:39:45 +08:00
NuoDaJia02	0b915a0dee	new sch and params	2025-11-11 13:21:18 +08:00
NuoDaJia02	e2227cce31	add cancel task	2025-11-11 08:44:30 +08:00
NuoDaJia02	4721d7f7e6	demo show 1110 params	2025-11-10 10:56:22 +08:00
NuoDaJia02	212e652d72	0.5m version	2025-11-09 00:44:04 +08:00
david	a96384e8f3	optimize robot work info	2025-11-07 23:08:19 +08:00
NuoDaJia02	f0b847af96	调整stock out动作，增加扫码动作	2025-11-07 18:43:15 +08:00
NuoDaJia02	897597ae6d	sch1 left/right arm scan code	2025-11-07 13:31:16 +08:00
NuoDaJia02	37e650512e	sch1 right arm scan code	2025-11-07 13:15:37 +08:00
NuoDaJia02	9a2f17f8b4	增加准备抓取动作	2025-11-06 15:20:11 +08:00
NuoDaJia02	4ae6b84595	更新任务参数	2025-11-06 12:56:46 +08:00
NuoDaJia02	2e1fc55e47	update sch params	2025-11-05 19:05:26 +08:00
NuoDaJia02	100e06fe81	disable some logs	2025-11-05 15:18:34 +08:00
NuoDaJia02	893bca0d2e	Merge branch 'develop' of ssh://192.168.10.50:2222/HiveCoreRD/hivecore_robot_brain into develop	2025-11-04 22:03:11 +08:00
NuoDaJia02	948aba7e84	support stand alone sch	2025-11-04 21:59:07 +08:00
NuoDaJia02	0b21c86d31	modify sch & params	2025-11-04 20:30:12 +08:00
david	b6934ff229	delete brain_interfaces	2025-11-01 17:40:56 +08:00
david	ff116c9b72	modify sch1	2025-11-01 13:26:39 +08:00
david	967b7bdf9b	update sch & params	2025-11-01 13:02:29 +08:00
david	74c6ee0046	only retry failed action	2025-11-01 12:02:57 +08:00
david	4da85d16dc	support parallel action	2025-11-01 11:39:20 +08:00