add gripper control

src/brain/config/bt_vision_grasp.params.yaml

@@ -65,6 +65,26 @@
 
     data_array: []
 
+    '
+- name: s4_gripper_close
+  params: 'loc: 90
+
+    speed: 20
+
+    torque: 20
+
+    mode: 2
+
+    '
+- name: s4_gripper_open
+  params: 'loc: 0
+
+    speed: 20
+
+    torque: 20
+
+    mode: 2
+
     '
 - name: s5_right_arm_vision_pre_grasp
   params: 'body_id: 1

src/brain/config/bt_vision_grasp.xml

@@ -4,12 +4,14 @@
       <!-- Retry vision + hand until GripperControl_H succeeds -->
       <RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
         <Sequence>
-          <!-- <GripperCmd_H name="s1_gripper_open" /> -->
+          <GripperCmd_H name="s1_gripper_open" />
           <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
           <Arm_H name="s2_right_arm_cam_take_photo" />
           <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
           <Arm_H name="s3_right_arm_vision_pre_grasp" />
           <Arm_H name="s4_right_arm_vision_grasp" />
+          <GripperCmd_H name="s4_gripper_close" />
+          <GripperCmd_H name="s4_gripper_open" />
           <Arm_H name="s5_right_arm_vision_pre_grasp" />
           <Arm_H name="s6_right_arm_cam_take_photo" />
 

src/brain/src/cerebellum_hooks.cpp

@@ -589,7 +589,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
         pose.orientation.y, pose.orientation.z, pose.orientation.w);
 
       node->right_hand_take_photo_pose_.position.x = pose.position.x + 0.04f;
-      node->right_hand_take_photo_pose_.position.y = pose.position.y - 0.35f;
+      node->right_hand_take_photo_pose_.position.y = pose.position.y - 0.40f;
       node->right_hand_take_photo_pose_.position.z = pose.position.z;
       node->right_hand_take_photo_pose_.orientation.x = -0.7071;
       node->right_hand_take_photo_pose_.orientation.y = 0.0000;
@@ -603,7 +603,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
         node->right_hand_take_photo_pose_.orientation.w);
 
       if (node->right_hand_take_photo_pose_.position.x < 0.1 || 
-        node->right_hand_take_photo_pose_.position.y < 0.1 || node->right_hand_take_photo_pose_.position.z < 0.1) {
+        node->right_hand_take_photo_pose_.position.y < 0.1 || node->right_hand_take_photo_pose_.position.z < 0.15) {
           RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
           return false;
       }