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

433 lines
12 KiB
C++
Raw Normal View History

feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
//
// 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>
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#include "cmvr/msgs/geometry.pb.h"
#include "robot/humanoid_robot/humanoid_robot.h"
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
// 定义一个命令行参数 --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;
update vision servo
2025-08-23 15:51:07 +08:00
// robot->calibrateZeroQ("R_WRIST_P");
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
robot->calibrateZeroQ("R_WRIST_Y");
update vision servo
2025-08-23 15:51:07 +08:00
// robot->calibrateZeroQ("R_WRIST_R");
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
//
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}
// };
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
//
// 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));
// }
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
}
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);
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
pose.mutable_position()->set_z(0.01);
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
pose.mutable_euler()->set_rx(0);
pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(0);
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
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();
}
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
}
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);
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
// //
// 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));
// }
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
}
Reference in New Issue Copy Permalink