feat: use qd cmd

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp

@@ -26,6 +26,7 @@
 #include "../include/humanoid_robot.h"
 #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
 #include "../../../../utils/base/include/abstract_interpolation.h"
+#include "data_center/include/motors_info.h"
 
 using namespace cmvr::device;
 
@@ -318,6 +319,20 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
         << "Failed to extract right-arm 7 joints from robot state";
     ibvs_controller.reset(q_now);
 
+    auto motor_manager = cmvr::MotorsInfo::getInstance()->getMotorManager();
+    ASSERT_TRUE(motor_manager != nullptr) << "MotorManager is null";
+    std::vector<std::shared_ptr<AbstractMotor>> right_arm_motors;
+    right_arm_motors.reserve(kRightArmJointNames.size());
+    for (const auto* name : kRightArmJointNames) {
+        auto motor = motor_manager->getMotor(name);
+        ASSERT_TRUE(motor != nullptr) << "getMotor failed for " << name;
+        if (motor->getMode() !=cmvr::msgs::RUN_MODE_PROFILE_VELOCITY) {
+            motor->setMode(cmvr::msgs::RUN_MODE_PROFILE_VELOCITY);
+        }
+        motor->setQd(0.0);
+        right_arm_motors.push_back(motor);
+    }
+
     const int max_steps = 30000;
     const int log_every = 1;
     const int cycle_ms = 10;
@@ -325,13 +340,13 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
     const double hard_margin = 0.04;   // rad
     const double qdot_deadband = 0.01; // rad/s
     const double qdot_lpf_alpha = 0.75;
-    const double q_step_max = 0.015;   // rad per cycle
+    const double qdot_acc_limit = 1.5; // rad/s^2
-    const double vel_min = 0.05;       // rad/s
+    const double qdot_max_send = 0.25; // rad/s
-    const double vel_max = 0.35;       // rad/s
 
     int ok_steps = 0;
     int fail_steps = 0;
     std::vector<double> qdot_lpf(kRightArmJointNames.size(), 0.0);
+    std::vector<double> qdot_cmd_prev(kRightArmJointNames.size(), 0.0);
     auto t_prev = std::chrono::steady_clock::now();
 
     for (int step = 0; step < max_steps; ++step) {
@@ -353,6 +368,10 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
         std::vector<double> qdot_next;
         const bool ok = ibvs_controller.compute(q_now, qdot_next);
         if (!ok || qdot_next.size() != kRightArmJointNames.size()) {
+            for (auto& motor : right_arm_motors) {
+                motor->setQd(0.0);
+            }
+            std::fill(qdot_cmd_prev.begin(), qdot_cmd_prev.end(), 0.0);
             ++fail_steps;
             if ((step % log_every) == 0) {
                 std::cout << "[IBVS_REAL] step=" << step
@@ -364,8 +383,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
             continue;
         }
 
-        std::vector<double> q_cmd_next(kRightArmJointNames.size(), 0.0);
+        std::vector<double> qd_send(kRightArmJointNames.size(), 0.0);
-        std::vector<double> qd_send(kRightArmJointNames.size(), vel_min);
         for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
             qdot_lpf[i] = qdot_lpf_alpha * qdot_lpf[i] + (1.0 - qdot_lpf_alpha) * qdot_next[i];
             double v = applySoftJointLimitVelocity(q_now[i], qdot_lpf[i],
@@ -374,14 +392,16 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
             if (std::abs(v) < qdot_deadband) {
                 v = 0.0;
             }
-
+            const double dv_max = qdot_acc_limit * dt;
-            const double dq = std::clamp(v * dt, -q_step_max, q_step_max);
+            v = std::clamp(v, qdot_cmd_prev[i] - dv_max, qdot_cmd_prev[i] + dv_max);
-            q_cmd_next[i] = std::clamp(q_now[i] + dq, kRightArmQMin[i], kRightArmQMax[i]);
+            v = std::clamp(v, -qdot_max_send, qdot_max_send);
-            qd_send[i] = std::clamp(std::abs(v), vel_min, vel_max);
+            qdot_cmd_prev[i] = v;
+            qd_send[i] = v;
         }
 
-        auto joint_cmd = buildRightArmJointCmdWithVel(q_cmd_next, qd_send, vel_min);
+        for (size_t i = 0; i < right_arm_motors.size(); ++i) {
-        robot->servoJ(joint_cmd, dt);
+            right_arm_motors[i]->setQd(qd_send[i]);
+        }
         ++ok_steps;
 
         if ((step % log_every) == 0) {
@@ -393,8 +413,8 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
                       << ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]"
                       << " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage())
                       << std::endl;
-            std::cout << "q_cmd: " ;
+            std::cout << "qd_cmd: " ;
-            for (const auto& it : q_cmd_next) {
+            for (const auto& it : qd_send) {
                 std::cout << it << "  ";
             }
             std::cout << std::endl;
@@ -408,7 +428,10 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
               << " fail_steps=" << fail_steps
               << std::endl;
 
+    for (auto& motor : right_arm_motors) {
+        motor->setQd(0.0);
+    }
+
     EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
 }
 
-