SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: use qdot

Browse Source
This commit is contained in:
lgv 2026-02-28 17:13:34 +08:00
parent 720d851cd0
commit 02504e9534
1 changed files with 96 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
View File

@ -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;
const bool ok = ibvs_controller.compute(q_now, dt, q_cmd_next);
if (!ok || q_cmd_next.size() != kRightArmJointNames.size()) {
std::vector<double> qdot_next;
const bool ok = ibvs_controller.compute(q_now, qdot_next);
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);
robot->servoJ(joint_cmd, 0.5, 0.5);
std::vector<double> q_cmd_next(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],
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) {
}