433 lines
12 KiB
C++
433 lines
12 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>
|
||
|
||
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
||
#include "cmvr/msgs/geometry.pb.h"
|
||
#include "robot/humanoid_robot/humanoid_robot.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_P");
|
||
robot->calibrateZeroQ("R_WRIST_Y");
|
||
// robot->calibrateZeroQ("R_WRIST_R");
|
||
//
|
||
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));
|
||
// }
|
||
|
||
}
|
||
|
||
|
||
TEST(HumanoidRobotTest,MoveDeltaTest) {
|
||
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");
|
||
|
||
cmvr::msgs::Pose3d pose;
|
||
pose.mutable_position()->set_x( 0);
|
||
pose.mutable_position()->set_y(0);
|
||
pose.mutable_position()->set_z(0.01);
|
||
|
||
pose.mutable_euler()->set_rx(0);
|
||
pose.mutable_euler()->set_ry(0);
|
||
pose.mutable_euler()->set_rz(0);
|
||
|
||
try {
|
||
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1);
|
||
// robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01);
|
||
// robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose);
|
||
}catch (std::exception &e) {
|
||
robot->torqueOff();
|
||
LOG(INFO) << e.what();
|
||
}
|
||
}
|
||
|
||
TEST(HumanoidRobotTest,AngleToTest) {
|
||
double rx = M_PI / 6; // 30°
|
||
double ry = M_PI / 4; // 45°
|
||
double rz = M_PI / 3; // 60°
|
||
|
||
Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz);
|
||
Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R);
|
||
|
||
std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n";
|
||
std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n";
|
||
}
|
||
|
||
|
||
TEST(HumanoidRobotTest,MoveIKTest) {
|
||
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{
|
||
{"R_SHOULDER_P", -0.08882},
|
||
{"R_SHOULDER_R", 1.50297},
|
||
{"R_SHOULDER_Y", 1.85922},
|
||
{"R_ELBOW_R", 0.214816},
|
||
{"R_WRIST_P", -1.69504},
|
||
{"R_WRIST_Y", 0.0363855},
|
||
{"R_WRIST_R", 0.0519807}
|
||
};
|
||
|
||
|
||
robot->moveJ(cmd);
|
||
|
||
|
||
// //
|
||
// cmvr::ctrl::PoseTarget target;
|
||
// std::vector<cmvr::ctrl::PoseTarget> targets;
|
||
//
|
||
//
|
||
// cmvr::msgs::Pose3d pose;
|
||
//
|
||
// pose.mutable_position()->set_x( 1.49969573e-01);
|
||
// pose.mutable_position()->set_y(-4.00100001e-01);
|
||
// pose.mutable_position()->set_z(-1.00102800e-01);
|
||
//
|
||
// pose.mutable_euler()->set_rx(0);
|
||
// pose.mutable_euler()->set_ry(0);
|
||
// pose.mutable_euler()->set_rz(1.57);
|
||
//
|
||
//
|
||
//
|
||
// while (true) {
|
||
// robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
|
||
//
|
||
//
|
||
//
|
||
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||
//
|
||
//
|
||
// robot->moveJ(cmd);
|
||
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||
// }
|
||
|
||
} |