feat: use qdot

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

@@ -14,6 +14,7 @@
 #include <array>
 #include <algorithm>
 #include <limits>
+#include <cmath>
 
 #include "gtest/gtest.h"
 #include <glog/logging.h>
@@ -50,6 +51,21 @@ std::vector<JointPoint> buildRightArmJointCmd(const std::vector<double>& q_cmd,
     return cmd;
 }
 
+std::vector<JointPoint> buildRightArmJointCmdWithVel(const std::vector<double>& q_cmd,
+                                                     const std::vector<double>& qd_abs,
+                                                     double vel_fallback = 0.1) {
+    std::vector<JointPoint> cmd;
+    cmd.reserve(kRightArmJointNames.size());
+    for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
+        double vel = vel_fallback;
+        if (i < qd_abs.size() && std::isfinite(qd_abs[i]) && qd_abs[i] > 0.0) {
+            vel = qd_abs[i];
+        }
+        cmd.emplace_back(kRightArmJointNames[i], q_cmd[i], vel);
+    }
+    return cmd;
+}
+
 std::unordered_map<std::string, double> makeRightArmQMap() {
     std::unordered_map<std::string, double> q_map;
     q_map.reserve(kRightArmJointNames.size());
@@ -59,6 +75,56 @@ std::unordered_map<std::string, double> makeRightArmQMap() {
     return q_map;
 }
 
+constexpr std::array<double, 7> kRightArmQMin = {
+    -3.14, // R_SHOULDER_P
+    -0.78, // R_SHOULDER_R
+    -3.14, // R_SHOULDER_Y
+     0.00, // R_ELBOW_R
+    -3.14, // R_WRIST_P
+    -0.78, // R_WRIST_Y
+    -0.55  // R_WRIST_R
+};
+
+constexpr std::array<double, 7> kRightArmQMax = {
+     3.14, // R_SHOULDER_P
+     1.57, // R_SHOULDER_R
+     3.14, // R_SHOULDER_Y
+     2.05, // R_ELBOW_R
+     3.14, // R_WRIST_P
+     0.78, // R_WRIST_Y
+     1.57  // R_WRIST_R
+};
+
+double applySoftJointLimitVelocity(double q,
+                                   double v,
+                                   double q_min,
+                                   double q_max,
+                                   double soft_margin,
+                                   double hard_margin) {
+    if (q_max <= q_min) return 0.0;
+    const double hard = std::max(1e-4, hard_margin);
+    const double soft = std::max(hard + 1e-4, soft_margin);
+
+    if (v < 0.0) {
+        const double q_hard = q_min + hard;
+        const double q_soft = q_min + soft;
+        if (q <= q_hard) return 0.0;
+        if (q < q_soft) {
+            const double s = std::clamp((q - q_hard) / (q_soft - q_hard), 0.0, 1.0);
+            return v * s;
+        }
+    } else if (v > 0.0) {
+        const double q_hard = q_max - hard;
+        const double q_soft = q_max - soft;
+        if (q >= q_hard) return 0.0;
+        if (q > q_soft) {
+            const double s = std::clamp((q_hard - q) / (q_hard - q_soft), 0.0, 1.0);
+            return v * s;
+        }
+    }
+    return v;
+}
+
 } // namespace
 
 TEST(HumanoidRobotTest,GetState) {
@@ -254,10 +320,18 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
 
     const int max_steps = 30000;
     const int log_every = 1;
-    const int cycle_ms = 1;
+    const int cycle_ms = 10;
+    const double soft_margin = 0.20;   // rad
+    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 vel_min = 0.05;       // 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);
     auto t_prev = std::chrono::steady_clock::now();
 
     for (int step = 0; step < max_steps; ++step) {
@@ -276,9 +350,9 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
         t_prev = t_now;
         dt = std::clamp(dt, 0.001, 0.1);
 
-        std::vector<double> q_cmd_next;
+        std::vector<double> qdot_next;
-        const bool ok = ibvs_controller.compute(q_now, dt, q_cmd_next);
+        const bool ok = ibvs_controller.compute(q_now, qdot_next);
-        if (!ok || q_cmd_next.size() != kRightArmJointNames.size()) {
+        if (!ok || qdot_next.size() != kRightArmJointNames.size()) {
             ++fail_steps;
             if ((step % log_every) == 0) {
                 std::cout << "[IBVS_REAL] step=" << step
@@ -290,8 +364,24 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
             continue;
         }
 
-        auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.5);
+        std::vector<double> q_cmd_next(kRightArmJointNames.size(), 0.0);
-        robot->servoJ(joint_cmd, 0.5, 0.5);
+        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],
+                                                   kRightArmQMin[i], kRightArmQMax[i],
+                                                   soft_margin, hard_margin);
+            if (std::abs(v) < qdot_deadband) {
+                v = 0.0;
+            }
+
+            const double dq = std::clamp(v * dt, -q_step_max, q_step_max);
+            q_cmd_next[i] = std::clamp(q_now[i] + dq, kRightArmQMin[i], kRightArmQMax[i]);
+            qd_send[i] = std::clamp(std::abs(v), vel_min, vel_max);
+        }
+
+        auto joint_cmd = buildRightArmJointCmdWithVel(q_cmd_next, qd_send, vel_min);
+        robot->servoJ(joint_cmd, dt);
         ++ok_steps;
 
         if ((step % log_every) == 0) {
@@ -322,4 +412,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
 }
 
 
-