328 lines
9.1 KiB
C++
328 lines
9.1 KiB
C++
|
//
|
|||
|
|
// Created by lgv on 2025/8/4.
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
|
|||
|
|
// #include "humanoid_robot.h"
|
|||
|
|
|
|||
|
|
#include "gtest/gtest.h"
|
|||
|
|
#include <glog/logging.h>
|
|||
|
|
#include "device_manager/device_manager.h"
|
|||
|
|
#include <libgen.h>
|
|||
|
|
|
|||
|
|
|
|||
|
|
// 定义一个命令行参数 --config_path
|
|||
|
|
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
|
|||
|
|
using namespace cmvr::device;
|
|||
|
|
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");
|
|||
|
|
|
|||
|
|
std::vector<JointPoint> cmd{};
|
|||
|
|
|
|||
|
|
std::vector<std::vector<JointPoint>> traj;
|
|||
|
|
|
|||
|
|
robot->calibrateZeroQ("R_WRIST_Y");
|
|||
|
|
//
|
|||
|
|
cmd = {
|
|||
|
|
// {"L_SHOULDER_P", 0.0},
|
|||
|
|
// {"L_SHOULDER_R", 0.0},
|
|||
|
|
// {"L_SHOULDER_Y", 0.0},
|
|||
|
|
// {"L_ELBOW_R", 0.0},
|
|||
|
|
// {"L_WRIST_P", 0.0},
|
|||
|
|
// {"L_WRIST_Y", 0.0},
|
|||
|
|
// {"L_WRIST_R", 0.0},
|
|||
|
|
//
|
|||
|
|
// {"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.0},
|
|||
|
|
|
|||
|
|
{"WAIST_P" ,0.0},
|
|||
|
|
{"WAIST_Y" ,0.0},
|
|||
|
|
};
|
|||
|
|
robot->moveJ(cmd,0.8);
|
|||
|
|
|
|||
|
|
|
|||
|
|
// cmd = {
|
|||
|
|
// {"L_SHOULDER_P", 0.0},
|
|||
|
|
// {"L_SHOULDER_R", -1.31873},
|
|||
|
|
// {"L_SHOULDER_Y", 0.0},
|
|||
|
|
// {"L_ELBOW_R", -0.537621},
|
|||
|
|
// {"L_WRIST_P", 0.0},
|
|||
|
|
// {"L_WRIST_Y", 0.000183204},
|
|||
|
|
// {"L_WRIST_R", 0.0225797},
|
|||
|
|
//
|
|||
|
|
// {"R_SHOULDER_P", 0.00203898},
|
|||
|
|
// {"R_SHOULDER_R", 1.34062},
|
|||
|
|
// {"R_SHOULDER_Y", 0.0},
|
|||
|
|
// {"R_ELBOW_R", 0.522261},
|
|||
|
|
// {"R_WRIST_P", 0.0},
|
|||
|
|
// {"R_WRIST_Y", -0.000210733},
|
|||
|
|
// {"R_WRIST_R", -0.0942364}
|
|||
|
|
// };
|
|||
|
|
|
|||
|
|
// robot->moveJ(cmd,0.8);
|
|||
|
|
|
|||
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|||
|
|
// robot->eStop();
|
|||
|
|
|
|||
|
|
// robot->torqueOff();
|
|||
|
|
|
|||
|
|
|
|||
|
|
while (true) {
|
|||
|
|
|
|||
|
|
// cmd = {{"L_SHOULDER_P",3.14},{"L_SHOULDER_R",3.14},{"L_SHOULDER_Y",3.14}};
|
|||
|
|
// robot->moveJ(cmd,1.0);
|
|||
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(8000));
|
|||
|
|
// cmd = {{"L_SHOULDER_P",-3.14},{"L_SHOULDER_R",-3.14},{"L_SHOULDER_Y",-3.14}};
|
|||
|
|
// robot->moveJ(cmd,1.0);
|
|||
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(8000));
|
|||
|
|
|
|||
|
|
// cmd = {{"L_SHOULDER_R",3.14}};
|
|||
|
|
// robot->moveJ(cmd,0.7);
|
|||
|
|
// // std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
|||
|
|
// cmd = {{"L_SHOULDER_R",-3.14}};
|
|||
|
|
// robot->moveJ(cmd,0.7);
|
|||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
TEST(HumanoidRobotTest,FollowJointTrajectoryTest) {
|
|||
|
|
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");
|
|||
|
|
|
|||
|
|
|
|||
|
|
// 读取轨迹
|
|||
|
|
std::ifstream file("/home/lgv/cmvr/cmvr-es/src/devices/robot/humanoid_robot/joint_positions_1.csv");
|
|||
|
|
if (!file.is_open()) {
|
|||
|
|
LOG(ERROR) << "无法打开文件" ;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::string line;
|
|||
|
|
|
|||
|
|
// 读取标题行，获取关节名称（除去第一列 idx）
|
|||
|
|
if (!std::getline(file, line)) {
|
|||
|
|
LOG(ERROR) << "文件为空或格式错误" ;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<std::string> joint_names;
|
|||
|
|
{
|
|||
|
|
std::stringstream ss(line);
|
|||
|
|
std::string cell;
|
|||
|
|
// 第一列是 idx，跳过
|
|||
|
|
std::getline(ss, cell, ',');
|
|||
|
|
// 读取关节名称列
|
|||
|
|
while (std::getline(ss, cell, ',')) {
|
|||
|
|
joint_names.push_back(cell);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<std::vector<JointPoint>> traj;
|
|||
|
|
|
|||
|
|
// 读取后续每行数据
|
|||
|
|
while (std::getline(file, line)) {
|
|||
|
|
std::stringstream ss(line);
|
|||
|
|
std::string cell;
|
|||
|
|
|
|||
|
|
// 读取第一列 idx，暂时不使用
|
|||
|
|
std::getline(ss, cell, ',');
|
|||
|
|
|
|||
|
|
std::vector<JointPoint> joints;
|
|||
|
|
|
|||
|
|
// 读取每个关节角度
|
|||
|
|
for (size_t i = 0; i < joint_names.size(); ++i) {
|
|||
|
|
if (!std::getline(ss, cell, ',')) {
|
|||
|
|
LOG(ERROR) << "数据列不足，格式错误";
|
|||
|
|
}
|
|||
|
|
double angle = std::stod(cell); // 字符串转 double
|
|||
|
|
|
|||
|
|
JointPoint cmd;
|
|||
|
|
cmd.joint_name = joint_names[i];
|
|||
|
|
cmd.rad = angle;
|
|||
|
|
joints.push_back(cmd);
|
|||
|
|
}
|
|||
|
|
traj.push_back(joints);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
file.close();
|
|||
|
|
|
|||
|
|
|
|||
|
|
// 计算速度
|
|||
|
|
double dt = 0.01; // 采样周期(s)
|
|||
|
|
size_t N = traj.size();
|
|||
|
|
|
|||
|
|
for (size_t i = 0; i < N; ++i) {
|
|||
|
|
if (i == 0) {
|
|||
|
|
// 第一个点没有前一帧，速度设为 0
|
|||
|
|
for (auto &cmd : traj[i]) {
|
|||
|
|
cmd.vel = 0.0;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
// 后向差分
|
|||
|
|
for (size_t j = 0; j < traj[i].size(); ++j) {
|
|||
|
|
double pos_prev = traj[i - 1][j].rad;
|
|||
|
|
double pos_curr = traj[i][j].rad;
|
|||
|
|
traj[i][j].vel = (pos_curr - pos_prev) / dt;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
// // 测试打印读取结果
|
|||
|
|
// for (size_t i = 0; i < traj.size(); ++i) {
|
|||
|
|
// std::cout << "Index " << i << ":" << std::endl;
|
|||
|
|
// for (const auto& cmd : traj[i]) {
|
|||
|
|
// std::cout << cmd.joint_name
|
|||
|
|
// << " = " << cmd.rad << " rad, "
|
|||
|
|
// << cmd.vel << " rad/s; ";
|
|||
|
|
// }
|
|||
|
|
// std::cout << std::endl;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
|
|||
|
|
// 1：
|
|||
|
|
|
|||
|
|
|
|||
|
|
//
|
|||
|
|
// std::vector<JointPoint> cmd{};
|
|||
|
|
// cmd = {
|
|||
|
|
// // {"L_SHOULDER_P", 0.0},
|
|||
|
|
// // {"L_SHOULDER_R", 0.0},
|
|||
|
|
// // {"L_SHOULDER_Y", 0.0},
|
|||
|
|
// // {"L_ELBOW_R", 0.0},
|
|||
|
|
// // {"L_WRIST_P", 0.0},
|
|||
|
|
// // {"L_WRIST_Y", 0.0},
|
|||
|
|
// // {"L_WRIST_R", 0.0},
|
|||
|
|
//
|
|||
|
|
// {"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.0}
|
|||
|
|
// };
|
|||
|
|
// robot->moveJ(cmd,0.8);
|
|||
|
|
|
|||
|
|
// // // 先到达轨迹起点
|
|||
|
|
// LOG(INFO) << "Moving to trajectory start...";
|
|||
|
|
// robot->moveJ(traj[0],0.8);
|
|||
|
|
//
|
|||
|
|
//
|
|||
|
|
//
|
|||
|
|
// // 再移动
|
|||
|
|
// LOG(INFO) << "Reached trajectory start point";
|
|||
|
|
// robot->followJointTrajectory(traj,dt * 1000);
|
|||
|
|
// LOG(INFO) << "Trajectory execution completed";
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
// std::vector<JointPoint> cmd1{
|
|||
|
|
// {"L_SHOULDER_P", -0.747573},
|
|||
|
|
// {"L_SHOULDER_R", -1.26911},
|
|||
|
|
// {"L_SHOULDER_Y", -1.20811},
|
|||
|
|
// {"L_ELBOW_R", -1.51221},
|
|||
|
|
// {"L_WRIST_P", 2.64099},
|
|||
|
|
// {"L_WRIST_Y", 0.417608},
|
|||
|
|
// {"L_WRIST_R", -0.518287},
|
|||
|
|
//
|
|||
|
|
// {"R_SHOULDER_P", -0.344938},
|
|||
|
|
// {"R_SHOULDER_R", 0.935147},
|
|||
|
|
// {"R_SHOULDER_Y", 2.27031},
|
|||
|
|
// {"R_ELBOW_R", 1.68959},
|
|||
|
|
// {"R_WRIST_P", -2.32841},
|
|||
|
|
// {"R_WRIST_Y", 0.460145},
|
|||
|
|
// {"R_WRIST_R", 0.300996}
|
|||
|
|
// };
|
|||
|
|
//
|
|||
|
|
// std::vector<JointPoint> cmd2{
|
|||
|
|
// {"L_SHOULDER_P", -0.747573},
|
|||
|
|
// {"L_SHOULDER_R", -1.26911},
|
|||
|
|
// {"L_SHOULDER_Y", -1.20811},
|
|||
|
|
// {"L_ELBOW_R", -1.51221},
|
|||
|
|
// {"L_WRIST_P", 2.64099},
|
|||
|
|
// {"L_WRIST_Y", 0.417608},
|
|||
|
|
// {"L_WRIST_R", -0.518287},
|
|||
|
|
//
|
|||
|
|
// {"R_SHOULDER_P", 0.239368},
|
|||
|
|
// {"R_SHOULDER_R", 0.871341},
|
|||
|
|
// {"R_SHOULDER_Y", 1.86052},
|
|||
|
|
// {"R_ELBOW_R", 1.28044},
|
|||
|
|
// {"R_WRIST_P", -2.49436},
|
|||
|
|
// {"R_WRIST_Y", 0.404731},
|
|||
|
|
// {"R_WRIST_R", 0.280016}
|
|||
|
|
// };
|
|||
|
|
|
|||
|
|
|
|||
|
|
std::vector<JointPoint> cmd1{
|
|||
|
|
{"L_SHOULDER_P", -0.747573},
|
|||
|
|
{"L_SHOULDER_R", -1.26911},
|
|||
|
|
{"L_SHOULDER_Y", -1.20811},
|
|||
|
|
{"L_ELBOW_R", -1.51221},
|
|||
|
|
{"L_WRIST_P", 2.64099},
|
|||
|
|
{"L_WRIST_Y", 0.417608},
|
|||
|
|
{"L_WRIST_R", -0.518287},
|
|||
|
|
|
|||
|
|
{"R_SHOULDER_P", -0.956276},
|
|||
|
|
{"R_SHOULDER_R", 1.0244},
|
|||
|
|
{"R_SHOULDER_Y", 2.70621},
|
|||
|
|
{"R_ELBOW_R", 2.02276},
|
|||
|
|
{"R_WRIST_P", -1.99653},
|
|||
|
|
{"R_WRIST_Y", 0.68523},
|
|||
|
|
{"R_WRIST_R", 0.477066}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
std::vector<JointPoint> cmd2{
|
|||
|
|
{"L_SHOULDER_P", -0.747573},
|
|||
|
|
{"L_SHOULDER_R", -1.26911},
|
|||
|
|
{"L_SHOULDER_Y", -1.20811},
|
|||
|
|
{"L_ELBOW_R", -1.51221},
|
|||
|
|
{"L_WRIST_P", 2.64099},
|
|||
|
|
{"L_WRIST_Y", 0.417608},
|
|||
|
|
{"L_WRIST_R", -0.518287},
|
|||
|
|
|
|||
|
|
{"R_SHOULDER_P", 0.226871},
|
|||
|
|
{"R_SHOULDER_R", 0.624717},
|
|||
|
|
{"R_SHOULDER_Y", 1.15086},
|
|||
|
|
{"R_ELBOW_R", 1.30365},
|
|||
|
|
{"R_WRIST_P", -2.14683},
|
|||
|
|
{"R_WRIST_Y", 0.196003},
|
|||
|
|
{"R_WRIST_R", 0.106678}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
while (true) {
|
|||
|
|
robot->moveJ(cmd1,0.8);
|
|||
|
|
robot->moveJ(cmd2,0.8);
|
|||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|