SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
34e349ce2a
cmvr-es/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp

445 lines
14 KiB
C++
Raw Blame History

//
// Created by lgv on 2025/8/4.
//
// #include "humanoid_robot.h"
#include <csignal>
#include <atomic>
#include <thread>
#include <chrono>
#include <iostream>
#include <unordered_map>
#include <array>
#include <algorithm>
#include <limits>
#include <cmath>
#include "gtest/gtest.h"
#include <glog/logging.h>
#include "../../../../device_manager/include/device_manager.h"
#include <libgen.h>
#include "controller/include/ibvs_controller.h"
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#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;
namespace {
constexpr std::array<const char*, 7> kRightArmJointNames = {
"R_SHOULDER_P",
"R_SHOULDER_R",
"R_SHOULDER_Y",
"R_ELBOW_R",
"R_WRIST_P",
"R_WRIST_Y",
"R_WRIST_R"
};
std::vector<JointPoint> buildRightArmJointCmd(const std::vector<double>& q_cmd,
double vel) {
std::vector<JointPoint> cmd;
cmd.reserve(kRightArmJointNames.size());
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
cmd.emplace_back(kRightArmJointNames[i], q_cmd[i], vel);
}
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());
for (const auto* name : kRightArmJointNames) {
q_map.emplace(name, 0.0);
}
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) {
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
robot->torqueOff();
std::vector<cmvr::device::JointState> joints;
while (true) {
joints.clear();
robot->getJointsState(joints);
for (const auto &js : joints) {
LOG(INFO) << "Joint: " << js.name
<< ", Position: " << js.position
<< ", Velocity: " << js.velocity
<< std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
}
TEST(HumanoidRobotTest,MyRobotTest) {
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")){
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
robot->calibrateZeroQ("R_WRIST_R");
// std::vector<JointPoint> cmd{};
}
TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto q_map_now = makeRightArmQMap();
std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
auto refreshRightArmQ = [&]() -> bool {
robot->getJointQ(q_map_now);
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
const auto it = q_map_now.find(kRightArmJointNames[i]);
if (it == q_map_now.end()) return false;
q_now[i] = it->second;
}
return true;
};
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
std::vector<JointPoint> cmd{};
cmd = {
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0.2},
};
// auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
ASSERT_NO_THROW(robot->servoJ(cmd, 0.8, 0.01));
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
}
}
TEST(HumanoidRobotTest,speedJTest) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
std::vector<JointVelocityCommand> cmd{};
cmd = {
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0.2},
};
robot->speedJ(cmd);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// robot->speedJ(0);
//
std::vector<JointPoint> cmd1{};
cmd1 = {
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0},
};
robot->moveJ(cmd1,2.0,10.0);
// robot->eStop();
//
}
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto camera = dmgr.getDevice<AbstractCamera>("cam2");
ASSERT_NE(robot, nullptr);
ASSERT_NE(camera, nullptr);
std::vector<JointPoint> init_cmd{};
init_cmd = {
{"R_SHOULDER_P", -0.2423},
{"R_SHOULDER_R", 1.2929},
{"R_SHOULDER_Y", 1.61},
{"R_ELBOW_R", 1.58},
{"R_WRIST_P", -2.8792},
{"R_WRIST_Y", 0.1150},
{"R_WRIST_R", -0.08},
};
robot->moveJ(init_cmd,1.0,2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_NO_THROW(camera->start());
struct RuntimeGuard {
std::shared_ptr<AbstractCamera> camera;
std::shared_ptr<AbstractRobot> robot;
~RuntimeGuard() {
if (camera) {
try {
camera->stop();
} catch (...) {
}
}
if (robot) {
try {
robot->eStop();
} catch (...) {
}
}
}
} guard{camera, robot};
cmvr::IbvsController ibvs_controller;
ibvs_controller.setMu(0.1);
ibvs_controller.setLambda(0.6);
ibvs_controller.setQdotMax(0.6);
ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
ibvs_controller.setTagSize(0.12);
ibvs_controller.setTrackedTagId(0);
ibvs_controller.setTarget(0.0, 0.0, 0.40);
ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR);
ibvs_controller.setDepthZGain(1.0);
Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
// ibvs_controller.setAlignCameraToVisp(I); // RealSense optical -> ViSP
// ibvs_controller.setAlignCameraToUrdf(I); // optical -> URDF 的 R_CAM
Eigen::Matrix3d RxPi;
RxPi << 1,0,0,
0,-1,0,
0,0,-1;
ibvs_controller.setAlignCameraToUrdf(RxPi);
ibvs_controller.setAlignCameraToVisp(RxPi);
// ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0});
// ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005});
ibvs_controller.setQdotMax(0.15);
ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
ASSERT_TRUE(ibvs_controller.init(camera,
"/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
"PELVIS_S",
"R_WRIST_R_S",
"R_CAM"))
<< "IbvsController init failed";
auto q_map_now = makeRightArmQMap();
robot->getJointQ(q_map_now);
std::vector<double> q_now(kRightArmJointNames.size(), 0.0);
auto refreshRightArmQ = [&]() -> bool {
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
const auto it = q_map_now.find(kRightArmJointNames[i]);
if (it == q_map_now.end()) {
return false;
}
q_now[i] = it->second;
// std::cout << kRightArmJointNames[i] << " " << q_now[i] << std::endl;
}
return true;
};
ASSERT_TRUE(refreshRightArmQ())
<< "Failed to extract right-arm 7 joints from robot state";
ibvs_controller.reset(q_now);
const int max_steps = 30000;
const int log_every = 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 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) {
robot->getJointQ(q_map_now);
if (!refreshRightArmQ()) {
++fail_steps;
if ((step % log_every) == 0) {
std::cout << "[IBVS_REAL] step=" << step << " joint extract failed\n";
}
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
continue;
}
const auto t_now = std::chrono::steady_clock::now();
double dt = std::chrono::duration<double>(t_now - t_prev).count();
t_prev = t_now;
dt = std::clamp(dt, 0.001, 0.1);
std::vector<double> qdot_next;
const bool ok = ibvs_controller.compute(q_now, qdot_next);
std::vector<JointVelocityCommand> qd_send;
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 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.push_back({kRightArmJointNames[i],v});
}
robot->speedJ(qd_send);
++ok_steps;
if ((step % log_every) == 0) {
const auto& t_co = ibvs_controller.lastTagPositionVisp();
const auto& v_c = ibvs_controller.lastCameraTwistVisp();
std::cout << "[IBVS_REAL] step=" << step
<< " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]"
<< " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2]
<< ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]"
<< " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage())
<< std::endl;
std::cout << "qd_cmd: " ;
for (const auto& it : qd_send) {
std::cout << it.vel << " ";
}
std::cout << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
}
std::cout << "[IBVS_REAL] finished steps=" << max_steps
<< " ok_steps=" << ok_steps
<< " fail_steps=" << fail_steps
<< std::endl;
robot->speedJ(0);
EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
}
Reference in New Issue View Git Blame Copy Permalink