HiveCoreRD/hivecore_robot_brain
6
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Refresh grasp BT params and hooks

Browse Source
This commit is contained in:
NuoDaJia02
2026-02-05 10:18:48 +08:00
parent 32e15a7d9a
commit 527c243617
5 changed files with 76 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
src/brain/config/bt_vision_grasp_dual_arm.params.yaml
View File

@@ -18,32 +18,32 @@
params: &arm_inittial_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_left_arm_initial
params: &arm_inittial_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_left_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_right_arm_cam_take_photo
params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_left_arm_cam_take_photo
params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_top_camera_vision_recg
params: 'camera_position: top
@@ -60,22 +60,22 @@
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_right_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_right_hand_gripper_close
params: &gripper_close 'loc: 200
@@ -92,42 +92,42 @@
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_take_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_grasp_box
params: &grasp_box_s7_s9_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.28, -103.86, 19.46, 40.34, -7.37, 49.94]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s8_right_hand_gripper_open
params: *gripper_open
- name: s8_left_hand_gripper_open

36
src/brain/config/bt_vision_grasp_dual_arm_demo1.params.yaml
View File

@@ -18,32 +18,32 @@
params: &arm_inittial_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [80, -5, -112, -12, -62, 175]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_left_arm_initial
params: &arm_inittial_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [100, 5, 112, 12, 62, 5]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [14.29, -34.07, -122.53, -76.89, 84.40, 112.89]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_left_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [165.80, 34.07, 122.53, 76.89, -84.40, 67.20]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_right_arm_cam_take_photo
params: &right_arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_left_arm_cam_take_photo
params: &left_arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_top_camera_vision_recg
params: 'camera_position: top
@@ -60,22 +60,22 @@
params: &pre_grasp_right "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s3_left_arm_vision_pre_grasp
params: &pre_grasp_left "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_right_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_right_hand_gripper_close
params: &gripper_close 'loc: 200
@@ -92,42 +92,42 @@
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_take_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_take_box_off
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9_right "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.31, -103.06, 17.69, -53.40, -7.81, 143.19]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_grasp_box
params: &grasp_box_s7_s9_left "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [83.78, 103.06, -17.69, 53.40, 7.81, 36.90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.28, -103.86, 19.46, 40.34, -7.37, 49.94]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [94.81, 103.86, -19.46, -40.33, 7.37, 130.15]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s8_right_hand_gripper_open
params: *gripper_open
- name: s8_left_hand_gripper_open

32
src/brain/config/bt_vision_grasp_left_arm.params.yaml
View File

@@ -26,12 +26,12 @@
params: &pre_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_pre_cam_take_photo
params: &pre_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
@@ -42,12 +42,12 @@
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [120, 20, 80, 0, 80, 120]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [60, -20, -80, 0, -80, 60]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_top_camera_vision_recg
params: 'camera_position: top
@@ -56,7 +56,7 @@
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s3_left_camera_vision_recg
params: 'camera_position: left
@@ -65,12 +65,12 @@
params: &pre_grasp "arm_motion_params:\n- arm_id: 0\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_left_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_gripper_close
params: 'loc: 200
@@ -85,17 +85,17 @@
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [84, 88, 45, 22, 16, 68]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s6_move_waist_turn_left_90
params: 'move_pitch_degree: 0.0
@@ -112,22 +112,22 @@
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 88, -14, 22, 16, 68]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, 88, -14, -22, 16, 112]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s8_gripper_open
params: *gripper_open
- name: s9_left_arm_grasp_box

34
src/brain/config/bt_vision_grasp_right_arm.params.yaml
View File

@@ -23,15 +23,15 @@
'
- name: s1_left_arm_pre_cam_take_photo
params: &pre_cam_take_photo "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_right_arm_pre_cam_take_photo
params: &pre_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [95, -25, -90, 0, -68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
@@ -42,12 +42,12 @@
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [120, 20, 80, 0, 80, 120]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s2_right_arm_pre_cam_take_photo
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [960, -20, -80, 0, -80, 60]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [60, -20, -80, 0, -80, 60]\n blend_radius:\
\ 0\nvelocity_scaling: 40\n"
- name: s2_top_camera_vision_recg
params: 'camera_position: top
@@ -56,7 +56,7 @@
params: &arm_cam_take_photo "arm_motion_params:\n- arm_id: 1\n motion_type: 100\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s3_right_camera_vision_recg
params: 'camera_position: right
@@ -65,12 +65,12 @@
params: &pre_grasp "arm_motion_params:\n- arm_id: 1\n motion_type: 101\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_right_arm_vision_grasp
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 102\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s4_gripper_close
params: 'loc: 200
@@ -85,17 +85,17 @@
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 110\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [0, 0, 0, 0, 0, 0]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_left_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s5_right_arm_grasp_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, -45, -22.88, -16.30, 112.10]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s6_move_waist_turn_left_90
params: 'move_pitch_degree: 0.0
@@ -109,25 +109,25 @@
'
- name: s7_left_arm_grasp_box
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_grasp_box
params: &grasp_box_s7_s9 "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [96.30, -88.95, 13.98, -22.88, -16.30, 112.10]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_left_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 0\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85, 25, 90, 0, 68, 90]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s7_right_arm_put_down_box
params: "arm_motion_params:\n- arm_id: 1\n motion_type: 0\n target_pose:\n \
\ position:\n x: 0.0\n y: 0.0\n z: 0.0\n orientation:\n \
\ x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0\n target_joints: [85.78, -88.95, 13.97, 22.88, -16.29, 67.99]\n blend_radius:\
\ 0\nvelocity_scaling: 50\n"
\ 0\nvelocity_scaling: 40\n"
- name: s8_gripper_open
params: *gripper_open
- name: s9_right_arm_grasp_box

13
src/brain/src/cerebellum_hooks.cpp
View File

@@ -248,13 +248,14 @@ void CerebellumHooks::ConfigureDualArmHooks(CerebellumNode * node)
if (!angle.empty()) {
p.target_joints = angle;
}
p.motion_type = interfaces::msg::ArmMotionParams::MOVEJ; //MOVEJ or MOVEL
}
RCLCPP_INFO(node->get_logger(),
"[%s] param[%zu]: arm_id=%u motion_type=%u blend_radius=%d pose=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) joints=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f)",
"[%s] param[%zu]: arm_id=%u motion_type=%u blend_radius=%d pose=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) joint_size=%zu joints=(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f)",
skill_name.c_str(), i, p.arm_id, p.motion_type, p.blend_radius,
p.target_pose.position.x, p.target_pose.position.y, p.target_pose.position.z,
p.target_pose.orientation.x, p.target_pose.orientation.y, p.target_pose.orientation.z, p.target_pose.orientation.w,
p.target_pose.orientation.x, p.target_pose.orientation.y, p.target_pose.orientation.z, p.target_pose.orientation.w, p.target_joints.size(),
p.target_joints[0], p.target_joints[1], p.target_joints[2], p.target_joints[3], p.target_joints[4], p.target_joints[5]);
}
@@ -762,7 +763,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// return false;
// }
std::vector<double> joint_angles(7, 0.0);
std::vector<double> joint_angles(6, 0.0);
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
int arm_id = (target_arm == "left_arm") ? 0 : 1;
@@ -857,7 +858,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
//pre grasp
std::vector<double> pre_grasp_joint_angles(7, 0.0);
std::vector<double> pre_grasp_joint_angles(6, 0.0);
if (node->CallInverseKinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
@@ -865,7 +866,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
continue;
}
//grasp
std::vector<double> grasp_joint_angles(7, 0.0);
std::vector<double> grasp_joint_angles(6, 0.0);
if (node->CallInverseKinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
@@ -876,7 +877,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
auto take_object_pose = result.grasp_pose;
// take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
take_object_pose.position.setX(node->take_object_arm_x_); //向上拿起物体
std::vector<double> take_object_joint_angles(7, 0.0);
std::vector<double> take_object_joint_angles(6, 0.0);
if (node->CallInverseKinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",