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

feat: use qd cmd

Browse Source
This commit is contained in:
lgv 2026-02-28 17:32:44 +08:00
parent 02504e9534
commit 774e8a16e5
1 changed files with 37 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 vel_min = 0.05; // rad/s
const double vel_max = 0.35; // rad/s
const double qdot_acc_limit = 1.5; // rad/s^2
const double qdot_max_send = 0.25; // 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(), vel_min);
std::vector<double> qd_send(kRightArmJointNames.size(), 0.0);
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 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);
const double dv_max = qdot_acc_limit * dt;
v = std::clamp(v, qdot_cmd_prev[i] - dv_max, qdot_cmd_prev[i] + dv_max);
v = std::clamp(v, -qdot_max_send, qdot_max_send);
qdot_cmd_prev[i] = v;
qd_send[i] = v;
}
auto joint_cmd = buildRightArmJointCmdWithVel(q_cmd_next, qd_send, vel_min);
robot->servoJ(joint_cmd, dt);
for (size_t i = 0; i < right_arm_motors.size(); ++i) {
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: " ;
for (const auto& it : q_cmd_next) {
std::cout << "qd_cmd: " ;
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.";
}