抓取demo除移动A点B点，其他阶段提速1.5倍

coordinated_actions/coordinated_actions.cpp

@@ -859,17 +859,33 @@ bool perform_grasp_task(RobotController &rc)
     std::cout<<"开始执行抓握"<<std::endl;
     const config::AppConfig &cfg = config::get_default_app_config();
     const auto &grasp_cfg = cfg.grasp;
+    const auto &hand_cfg = cfg.hand;
 
     double descend = grasp_cfg.grasp_descend_mm;
     if (descend <= 1.0) {
         descend = 30.0;
     }
 
-    //加速
+    // A/B 点关节速度保持 grasp_joint_* 不变；升降与灵巧手提速约 1.5 倍
+    constexpr double kGraspOtherStageBoost = 1.5;
     rc.setJointSpeedDeg(grasp_cfg.grasp_joint_speed_deg);
     rc.setJointAccDeg(grasp_cfg.grasp_joint_acc_deg);
-    const double line_speed = rc.getLineSpeed();
-    const double line_acc = rc.getLineAcc();
+    const double line_speed = rc.getLineSpeed() * kGraspOtherStageBoost;
+    const double line_acc = rc.getLineAcc() * kGraspOtherStageBoost;
+
+    auto clamp_hand_speed = [](int v) {
+        if (v < 0) return 0;
+        if (v > 1000) return 1000;
+        return v;
+    };
+    const int boost_hand_speed[6] = {
+        clamp_hand_speed(static_cast<int>(hand_cfg.speed[0] * kGraspOtherStageBoost)),
+        clamp_hand_speed(static_cast<int>(hand_cfg.speed[1] * kGraspOtherStageBoost)),
+        clamp_hand_speed(static_cast<int>(hand_cfg.speed[2] * kGraspOtherStageBoost)),
+        clamp_hand_speed(static_cast<int>(hand_cfg.speed[3] * kGraspOtherStageBoost)),
+        clamp_hand_speed(static_cast<int>(hand_cfg.speed[4] * kGraspOtherStageBoost)),
+        clamp_hand_speed(static_cast<int>(hand_cfg.speed[5] * kGraspOtherStageBoost)),
+    };
 
     std::cout<<"A点位置:";
     for (const auto &v : grasp_cfg.grasp_a.joint_deg) {
@@ -882,6 +898,7 @@ bool perform_grasp_task(RobotController &rc)
     }
     std::cout<<std::endl;
     std::cout<<"抓取下降距离:"<<descend<<" mm"<<std::endl;
+    std::cout<<"升降线速度:"<<line_speed<<" mm/s, 灵巧手提速:"<<boost_hand_speed[0]<<std::endl;
 
 
     // A/B 点采用多策略运动：关节 -> 直线 -> 圆弧，避免奇异点导致直接失败
@@ -898,6 +915,10 @@ bool perform_grasp_task(RobotController &rc)
     // hand_actions::action_grasp_test();
     // sleep_ms(3000);
 
+    hand_actions::set_speed_service(
+        boost_hand_speed[0], boost_hand_speed[1], boost_hand_speed[2],
+        boost_hand_speed[3], boost_hand_speed[4], boost_hand_speed[5]
+    );
     bool grasp_ok = hand_actions::action_quick_grasp_once();
     sleep_ms(500);
 
@@ -917,6 +938,10 @@ bool perform_grasp_task(RobotController &rc)
         return false;
     }
 
+    hand_actions::set_speed_service(
+        boost_hand_speed[0], boost_hand_speed[1], boost_hand_speed[2],
+        boost_hand_speed[3], boost_hand_speed[4], boost_hand_speed[5]
+    );
     hand_actions::action_full_extend();
     sleep_ms(2000);
     rc.moveZRelative(descend, line_speed, line_acc);
@@ -925,6 +950,10 @@ bool perform_grasp_task(RobotController &rc)
     //速度改回去
     rc.setJointSpeedDeg(cfg.robot.joint_speed_deg_s);
     rc.setJointAccDeg(cfg.robot.joint_acc_deg_s2);
+    hand_actions::set_speed_service(
+        hand_cfg.speed[0], hand_cfg.speed[1], hand_cfg.speed[2],
+        hand_cfg.speed[3], hand_cfg.speed[4], hand_cfg.speed[5]
+    );
     return true;
 }