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

update

Browse Source
This commit is contained in:
linbo 2025-09-25 16:01:01 +08:00
parent 83c52ac19b
commit 11f0097521
10 changed files with 512 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/devices/abstract_robot.h
View File

@ -105,9 +105,15 @@ namespace cmvr::device{
virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }
virtual void calibrateZeroQ(const std::string &joint_name) = 0;
void setToolFrame(const std::string& toolFrame)
{
toolFrame_ = toolFrame;
}
protected:
int dof_{};
RobotState state_{};
std::string toolFrame_;
};
}

14
include/hardware/io_interface.h Normal file
View File

@ -0,0 +1,14 @@
//
// Created by linbo on 2025/9/24.
//
#ifndef CMVR_ES_IO_INTERFACE_H
#define CMVR_ES_IO_INTERFACE_H
class io_interface
{
};
#endif //CMVR_ES_IO_INTERFACE_H

6
include/service/grpc_humanoid_robot_service.h
View File

@ -19,6 +19,12 @@ namespace cmvr {
const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override;
grpc::Status moveJ(grpc::ServerContext *context, const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) override;
grpc::Status moveL(grpc::ServerContext* context, const cmvr::api::MoveL_Request* request, cmvr::api::MoveL_Response* response) override;
grpc::Status speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response) override;
grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override;
private:
device::DeviceManager& dmgr_;
};

86
protos/cmvr/api/humanoid_robot.proto
View File

@ -5,12 +5,41 @@ package cmvr.api;
import "cmvr/api/common.proto";
message JointCmd {
string joint_name = 1; // 关节名称
double rad = 2; // 弧度
double vel = 3; // 角速度，rad/s
string joint_name = 1; // 关节名称
double rad = 2; // 弧度
double vel = 3; // 角速度，rad/s
}
message Pose3D{
double x = 1;
double y = 2;
double z = 3;
double rx = 4;
double ry = 5;
double rz = 6;
}
enum RobotCartesian{
X = 0;
Y = 1;
Z = 2;
RX = 3;
RY = 4;
RZ = 5;
} ;
enum RobotJointIndexDirection{
FORWARD = 0;
BACKWARD = 1;
X_POSITIVE = 2; // X轴正向
X_NEGATIVE = 3; // X轴负向
Y_POSITIVE = 4; // Y轴正向
Y_NEGATIVE = 5; // Y轴负向
Z_POSITIVE = 6; // Z轴正向
Z_NEGATIVE = 7; // Z轴负向
ROTATE_X = 8; // 绕X轴旋转
ROTATE_Y = 9; // 绕Y轴旋转
ROTATE_Z = 10; // 绕Z轴旋转
}
message MoveJ{
message Request{
CommandHeader.Request header = 1;
@ -25,8 +54,57 @@ message MoveJ{
}
message MoveL{
message Request{
CommandHeader.Request header = 1;
string ee_link = 2; //连杆名称
Pose3D targetPose = 3;
double vel = 4;
double acc = 5;
}
message Response{
CommandHeader.Feedback header= 1;
}
}
message SpeedJ{
message Request{
CommandHeader.Request header = 1;
string joint_name = 2;
double vel = 3;
double acc = 4;
RobotJointIndexDirection dir = 5;
}
message Response{
CommandHeader.Feedback header= 1;
}
}
message SpeedL{
message Request{
CommandHeader.Request header = 1;
string ee_link = 2;
double vel = 3;
double acc = 4;
RobotJointIndexDirection dir = 5;
RobotCartesian cart = 6;
}
message Response{
CommandHeader.Feedback header= 1;
}
}
service HumanoidRobotService{
rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback);
rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback);
rpc moveJ(MoveJ.Request) returns (MoveJ.Response);
}
rpc moveL(MoveL.Request) returns (MoveL.Response);
rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response);
rpc speedL(SpeedL.Request) returns (SpeedL.Response);
}

302
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -45,8 +45,6 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
left_toolFrame_ = l_can_cfg.getAttrString("toolFrame");
auto r_can_cfg = can_cfg.getChild("RightArmCan");
r_motors_cfg_ = r_can_cfg.getChildren("Motor");
@ -54,7 +52,6 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
right_toolFrame_ = r_can_cfg.getAttrString("toolFrame");
auto waist_can_cfg = can_cfg.getChild("WaistCan");
waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
@ -794,10 +791,11 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
try {
const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变
const size_t MAX_QUEUE_SIZE = 10; // 队列最大缓存的轨迹点数量，防止内存溢出
// 定义基座和末端执行器链接
std::string base_link = "PELVIS_S";
std::string ee_link = right_toolFrame_;
std::string ee_link = toolFrame_;
msgs::Pose3d current_pose = fk(base_link, ee_link);
@ -847,13 +845,11 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
is_rotation = true;
break;
case RobotJointIndexDirection::FORWARD:
// 保持向后兼容
if (cart == RobotCartesian::X) direction.x() = 1.0;
else if (cart == RobotCartesian::Y) direction.y() = 1.0;
else if (cart == RobotCartesian::Z) direction.z() = 1.0;
break;
case RobotJointIndexDirection::BACKWARD:
// 保持向后兼容
if (cart == RobotCartesian::X) direction.x() = -1.0;
else if (cart == RobotCartesian::Y) direction.y() = -1.0;
else if (cart == RobotCartesian::Z) direction.z() = -1.0;
@ -862,11 +858,11 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
throw std::runtime_error("speedL: unknown direction");
}
// 计算末端执行器的总运动时间
// 计算末端执行器的总运动时间和轨迹点数量
double move_time = calculateMoveTime(direction.norm(), vel, acc);
// 使用S曲线速度规划
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
// 生成S曲线速度规划的时间点和距离比例（主线程预计算）
std::vector<double> time_points;
std::vector<double> distance_ratios;
generateSTrapezoidalProfile(direction.norm(), vel, acc, move_time, num_points,
@ -875,137 +871,77 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
LOG(INFO) << "speedL: Planning trajectory - points=" << num_points
<< ", total distance=" << direction.norm() << "m, move time=" << move_time << "s";
// 生成轨迹点
std::vector<Eigen::Matrix4d> cartesian_trajectory;
for (size_t i = 0; i <= num_points; ++i) {
double s = distance_ratios[i]; // 使用S曲线规划的距离比例
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
if (is_rotation) {
// 旋转运动：保持位置不变，旋转姿态
T_interp.block<3, 1>(0, 3) = T_current.block<3, 1>(0, 3); // 位置不变
// 计算旋转矩阵
double angle = s * direction.norm(); // 旋转角度
Eigen::AngleAxisd rotation(angle, direction.normalized());
Eigen::Matrix3d R_current = T_current.block<3, 3>(0, 0);
Eigen::Matrix3d R_interp = rotation * R_current;
T_interp.block<3, 3>(0, 0) = R_interp;
} else {
// 平移运动：保持姿态不变，移动位置
T_interp.block<3, 3>(0, 0) = T_current.block<3, 3>(0, 0); // 保持姿态不变
T_interp(0, 3) = T_current(0, 3) + s * direction.x();
T_interp(1, 3) = T_current(1, 3) + s * direction.y();
T_interp(2, 3) = T_current(2, 3) + s * direction.z();
}
cartesian_trajectory.push_back(T_interp);
}
// 预先计算所有关节位置和速度
std::vector<Eigen::Vector<double, DOF>> joint_positions;
std::vector<Eigen::Vector<double, DOF>> joint_velocities;
// 获取当前关节位置
auto q_map_current = getJointQ();
Eigen::Vector<double, DOF> q_current;
// 根据你的关节名称填充q_current
for (int i = 0; i < DOF; ++i) {
q_current[i] = q_map_current[joint_names_[i]];
}
joint_positions.push_back(q_current);
joint_velocities.push_back(Eigen::Vector<double, DOF>::Zero()); // 起始速度为零
// 预先计算所有关节位置
for (size_t i = 1; i < cartesian_trajectory.size(); ++i) {
const auto& T_interp = cartesian_trajectory[i];
// 构造当前目标
cmvr::ctrl::PoseTarget current_target;
current_target.T_target = T_interp;
current_target.link_name = ee_link;
current_target.w_posrot = 0.5;
current_target.weight = 1.0;
// 使用前一点的位置作为初始值求解IK
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_next, 10000, 1e-6);
if (!ok) {
LOG(WARNING) << "IK计算失败，使用上一个有效点";
q_next = joint_positions.back();
}
LOG(INFO) << "IK计算结果 (q_next): " << q_next.transpose();
joint_positions.push_back(q_next);
}
// 计算每个点的关节速度
for (size_t i = 1; i < joint_positions.size(); ++i) {
double dt = time_points[i] - time_points[i-1];
Eigen::Vector<double, DOF> vel = (joint_positions[i] - joint_positions[i-1]) / dt;
joint_velocities.push_back(vel);
}
// 创建队列用于存储轨迹点
// 创建轨迹队列及同步机制
std::queue<std::pair<Eigen::Vector<double, DOF>, Eigen::Vector<double, DOF>>> trajectory_queue;
std::mutex queue_mutex;
std::condition_variable queue_cv;
std::atomic<bool> trajectory_completed{false};
std::atomic<bool> planning_completed{false}; // 规划是否完成
std::atomic<bool> execution_failed{false}; // 执行是否失败
std::atomic<size_t> planned_points{0}; // 已规划的点数
std::atomic<size_t> executed_points{0}; // 已执行的点数
// 轨迹点计算线程
std::thread trajectory_thread([&]() {
try {
// 计算并将轨迹点放入队列
for (size_t i = 0; i < joint_positions.size(); ++i) {
{
std::lock_guard<std::mutex> lock(queue_mutex);
trajectory_queue.push({joint_positions[i], joint_velocities[i]});
}
queue_cv.notify_one(); // 通知控制线程
// 获取当前关节位置（初始点）
auto q_map_current = getJointQ();
Eigen::Vector<double, DOF> q_current;
for (int i = 0; i < DOF; ++i) {
q_current[i] = q_map_current[joint_names_[i]];
}
// 控制节奏
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(CONTROL_PERIOD * 1000)));
}
// 先将初始点加入队列
{
std::lock_guard<std::mutex> lock(queue_mutex);
trajectory_queue.push({q_current, Eigen::Vector<double, DOF>::Zero()});
planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量
}
trajectory_completed.store(true);
queue_cv.notify_one();
} catch (const std::exception &e) {
LOG(ERROR) << "Trajectory thread error: " << e.what();
trajectory_completed.store(true);
queue_cv.notify_one();
}
});
// 控制执行线程
// 启动控制执行子线程（先启动子线程）
std::thread control_thread([&]() {
try {
while (!trajectory_completed.load() || !trajectory_queue.empty()) {
auto start_time = std::chrono::high_resolution_clock::now();
LOG(INFO) << "控制执行线程已启动";
// 循环条件使用load()读取原子变量
while (!planning_completed.load() || !trajectory_queue.empty() && !execution_failed.load()) {
// 从队列中获取轨迹点
std::pair<Eigen::Vector<double, DOF>, Eigen::Vector<double, DOF>> point;
bool has_point = false;
{
std::unique_lock<std::mutex> lock(queue_mutex);
if (queue_cv.wait_for(lock, std::chrono::milliseconds(100),
[&] { return !trajectory_queue.empty() || trajectory_completed.load(); })) {
// 等待队列中有数据或规划完成，使用load()读取原子变量
if (queue_cv.wait_for(lock, std::chrono::milliseconds(500),
[&] { return !trajectory_queue.empty() || planning_completed.load() || execution_failed.load(); })) {
if (!trajectory_queue.empty()) {
point = trajectory_queue.front();
trajectory_queue.pop();
has_point = true;
executed_points.store(executed_points.load() + 1); // 使用store和load操作原子变量
}
} else {
// 超时，可能规划线程出现问题
LOG(WARNING) << "控制线程等待轨迹点超时";
execution_failed.store(true); // 使用store设置原子变量
break;
}
}
if (has_point) {
// 计算当前点的期望执行时间，确保按时间规划执行
auto current_time = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed = current_time - start_time;
double expected_time = (executed_points.load() - 1) * CONTROL_PERIOD; // 使用load读取原子变量
// 如果执行过快，等待到期望时间
if (elapsed.count() < expected_time) {
std::this_thread::sleep_for(std::chrono::duration<double>(expected_time - elapsed.count()));
}
// 更新关节命令
m_state_->SetQ(point.first);
m_robot_->ComputeForwardKinematics(m_state_);
// 发送关节命令 - 为每个电机单独设置位置和速度
// 发送关节命令
for (int j = 0; j < DOF; ++j) {
auto motor = motor_manager_->getMotor(joint_names_[j]);
if (motor != nullptr) {
@ -1015,44 +951,147 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
motor->setQd(point.second[j]);
motor->setQ(point.first[j]);
// 打印发送的关节命令
LOG(INFO) << "Sending setQ: joint[" << joint_names_[j] << "] = "
<< point.first[j] << ", joint_velocity = "
<< point.second[j];
LOG(INFO) << "执行点 " << executed_points.load() // 使用load读取原子变量
<< ": joint[" << joint_names_[j] << "] = "
<< point.first[j];
}
}
}
// 控制时间节奏
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 控制周期
}
rsm_.store(ROBOT_READY); // 运动完成
if (execution_failed.load()) { // 使用load读取原子变量
LOG(ERROR) << "控制执行线程异常退出";
rsm_.store(ROBOT_ERROR);
} else {
LOG(INFO) << "控制执行线程完成，共执行 " << executed_points.load() // 使用load读取原子变量
<< " 个轨迹点";
rsm_.store(ROBOT_READY);
}
} catch (const std::exception &e) {
LOG(ERROR) << "Control thread error: " << e.what();
LOG(ERROR) << "控制线程错误: " << e.what();
execution_failed.store(true); // 使用store设置原子变量
rsm_.store(ROBOT_ERROR);
}
});
// 等待线程完成
if (trajectory_thread.joinable()) {
trajectory_thread.join();
// 主线程开始进行IK逆解和轨迹点规划（边规划边放入队列）
try {
Eigen::Vector<double, DOF> prev_q = q_current; // 上一个关节位置
double prev_time = 0.0;
// 生成并规划轨迹点（从1开始，因为0已经作为初始点）
for (size_t i = 1; i <= num_points; ++i) {
// 检查执行线程是否失败，如果失败则停止规划，使用load读取原子变量
if (execution_failed.load()) {
LOG(WARNING) << "执行线程失败，停止轨迹规划";
break;
}
// 生成笛卡尔空间轨迹点
double s = distance_ratios[i];
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
if (is_rotation) {
// 旋转运动
T_interp.block<3, 1>(0, 3) = T_current.block<3, 1>(0, 3);
double angle = s * direction.norm();
Eigen::AngleAxisd rotation(angle, direction.normalized());
Eigen::Matrix3d R_current = T_current.block<3, 3>(0, 0);
Eigen::Matrix3d R_interp = rotation * R_current;
T_interp.block<3, 3>(0, 0) = R_interp;
} else {
// 平移运动
T_interp.block<3, 3>(0, 0) = T_current.block<3, 3>(0, 0);
T_interp(0, 3) = T_current(0, 3) + s * direction.x();
T_interp(1, 3) = T_current(1, 3) + s * direction.y();
T_interp(2, 3) = T_current(2, 3) + s * direction.z();
}
// IK逆解计算
cmvr::ctrl::PoseTarget current_target;
current_target.T_target = T_interp;
current_target.link_name = ee_link;
current_target.w_posrot = 0.5;
current_target.weight = 1.0;
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_next, 10000, 1e-6);
if (!ok) {
LOG(WARNING) << "轨迹点 " << i << " IK求解失败，停止规划";
execution_failed.store(true); // 使用store设置原子变量
break;
}
// 计算时间差和关节速度
double dt = time_points[i] - prev_time;
Eigen::Vector<double, DOF> q_vel;
if (dt > 0) {
q_vel = (q_next - prev_q) / dt;
} else {
q_vel = Eigen::Vector<double, DOF>::Zero();
}
// 将计算好的轨迹点放入队列，如果队列满了则等待
{
std::unique_lock<std::mutex> lock(queue_mutex);
// 等待队列有空间，使用load读取原子变量
queue_cv.wait(lock, [&] {
return trajectory_queue.size() < MAX_QUEUE_SIZE || execution_failed.load();
});
if (execution_failed.load()) { // 使用load读取原子变量
break;
}
trajectory_queue.push({q_next, q_vel});
planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量
prev_q = q_next;
prev_time = time_points[i];
LOG(INFO) << "规划点 " << i << " 已加入队列，当前队列大小: " << trajectory_queue.size();
}
queue_cv.notify_one(); // 通知控制线程有新数据
// 简单的速率控制，避免规划过快，使用load读取原子变量
if (planned_points.load() - executed_points.load() > MAX_QUEUE_SIZE / 2) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
} catch (const std::exception &e) {
LOG(ERROR) << "轨迹规划错误: " << e.what();
execution_failed.store(true); // 使用store设置原子变量
}
// 规划完成，通知控制线程，使用store设置原子变量
planning_completed.store(true);
queue_cv.notify_one();
LOG(INFO) << "轨迹规划完成，共规划 " << planned_points.load() // 使用load读取原子变量
<< " 个轨迹点";
// 等待控制线程完成
if (control_thread.joinable()) {
control_thread.join();
}
LOG(INFO) << "speedL trajectory execution completed successfully.";
if (execution_failed.load()) { // 使用load读取原子变量
LOG(ERROR) << "speedL执行失败";
rsm_.store(ROBOT_ERROR);
throw std::runtime_error("speedL execution failed");
} else {
LOG(INFO) << "speedL轨迹执行成功完成";
}
} catch (const std::exception &e) {
LOG(ERROR) << "speedL failed: " << e.what();
LOG(ERROR) << "speedL失败: " << e.what();
rsm_.store(ROBOT_ERROR);
throw std::runtime_error(std::string("speedL error: ") + e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
try {
@ -1812,8 +1851,9 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
q_next, 10000, 1e-6);
if (!ok) {
LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point";
q_next = joint_positions.back();
throw std::runtime_error("Pre-computation IK failed");
// LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point";
// q_next = joint_positions.back();
}
joint_positions.push_back(q_next);

2
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -137,8 +137,6 @@ namespace cmvr::device{
private:
std::string id_;
std::string right_toolFrame_;
std::string left_toolFrame_;
int upd_freq_;
std::shared_ptr<FDTimer> upd_timer_;
std::atomic<RobotStateMachine> rsm_{ROBOT_DISABLED};

156
src/devices/robot/humanoid_robot/humanoid_robot_test.cpp
View File

@ -529,11 +529,45 @@ TEST(HumanoidRobotTest, SpeedLTest) {
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
try {
// std::unordered_map<std::string,double> joint_qs;
// joint_qs.insert(std::make_pair("R_SHOULDER_P",0));
// joint_qs.insert(std::make_pair("R_SHOULDER_R",0));
// joint_qs.insert(std::make_pair("R_SHOULDER_Y",0));
// joint_qs.insert(std::make_pair("R_ELBOW_R",0));
// joint_qs.insert(std::make_pair("R_WRIST_P",0));
// joint_qs.insert(std::make_pair("R_WRIST_Y",0));
// joint_qs.insert(std::make_pair("R_WRIST_R",0));
//
// robot->getJointQ(joint_qs);
// LOG(INFO) << "======================================";
// LOG(INFO) << "获取到的关节位置信息 (单位: 弧度)";
// LOG(INFO) << "--------------------------------------";
//
// for (const auto& pair : joint_qs) {
// LOG(INFO) << "关节 " << pair.first
// << ": " << pair.second
// << " (" << pair.second * 180 / M_PI << "°)";
// }
// LOG(INFO) << "======================================";
// return ;
// 先运动到一个合适的位置
std::vector<JointPoint> cmd{
{"R_SHOULDER_P", -0.348663},
{"R_SHOULDER_R", 1.1512},
{"R_SHOULDER_Y", 1.64856},
{"R_ELBOW_R", 1.8978},
{"R_WRIST_P", -2.8002},
{"R_WRIST_Y", -0.0262979},
{"R_WRIST_R", 0.0449828}
};
robot->moveJ(cmd);
// 定义运动方向（笛卡尔坐标系）
RobotCartesian cart_direction = RobotCartesian::Y; // 沿X轴移动
// 定义运动方向（正向或反向）
RobotJointIndexDirection move_direction = RobotJointIndexDirection::Y_NEGATIVE; // 正向
RobotJointIndexDirection move_direction = RobotJointIndexDirection::ROTATE_Y; // 正向
// 设置运动参数（速度单位：m/s，加速度单位：m/s²）
double vel = 0.02; // 速度 0.1m/s
@ -556,4 +590,124 @@ TEST(HumanoidRobotTest, SpeedLTest) {
// 捕获异常（如IK解算失败、状态非法等）
LOG(ERROR) << "speedL调用失败：" << e.what();
}
}
TEST(HumanoidRobotTest, followPoseTest) {
// 1. 配置文件与设备初始化
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found in config file";
return; // 配置错误，直接退出测试
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
// 获取机器人设备（hc01）
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
if (!robot) {
LOG(ERROR) << "Failed to get robot device 'hc01'";
return;
}
try {
// 2. 核心参数配置
std::string base_link = "PELVIS_S"; // 基座坐标系（轨迹相对此坐标系）
std::string ee_link = "R_WRIST_R_S"; // 待控制的末端执行器（如右手腕）
const double circle_radius = 0.03; // 圆形轨迹半径（3厘米，根据工作空间调整）
const double angle_start = 0.0; // 轨迹起始角度（0弧度）
const double angle_end = 2 * M_PI; // 轨迹结束角度（2π，完整圆形）
const int num_points = 100; // 轨迹总点数（点数越多，轨迹越平滑）
const double dt = 0.02; // 相邻轨迹点的时间间隔（20ms，控制运动速度）
// 选择轨迹所在平面（保持某一轴坐标不变）
// 0: XY平面（Z轴固定） | 1: XZ平面（Y轴固定） | 2: YZ平面（X轴固定）
const int plane_choice = 2;
const std::string plane_desc = (plane_choice == 0) ? "XY平面（Z轴不变）" :
(plane_choice == 1) ? "XZ平面（Y轴不变）" : "YZ平面（X轴不变）";
LOG(INFO) << "Trajectory plane selected: " << plane_desc;
// 3. 获取末端执行器当前位姿（作为圆形轨迹的圆心）
LOG(INFO) << "Getting current pose of end-effector (" << ee_link << ") relative to base (" << base_link << ")";
const cmvr::math::Pose3d current_pose = robot->getTransform(base_link, ee_link);
// 将当前位姿转换为Eigen::Matrix4d（圆心位姿矩阵，包含位置和姿态）
Eigen::Matrix4d center_T = Eigen::Matrix4d::Identity(); // 初始化单位矩阵（姿态为默认，位置待填充）
// 3.1 填充圆心位置（从当前位姿的position提取）
center_T(0, 3) = current_pose.position.x; // 圆心X坐标（当前末端X）
center_T(1, 3) = current_pose.position.y; // 圆心Y坐标（当前末端Y）
center_T(2, 3) = current_pose.position.z; // 圆心Z坐标（当前末端Z）
LOG(INFO) << "Circle center pose (XYZ): ["
<< center_T(0,3) << ", " << center_T(1,3) << ", " << center_T(2,3) << "] (m)";
// 3.2 填充圆心姿态（从当前位姿的四元数转换为旋转矩阵）
const double w = current_pose.quaternion.w;
const double x = current_pose.quaternion.x;
const double y = current_pose.quaternion.y;
const double z = current_pose.quaternion.z;
// 四元数转旋转矩阵（标准公式，确保末端姿态与当前一致）
center_T.block<3,3>(0,0) << 1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w,
2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w,
2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y;
// 4. 生成指定平面内的圆形轨迹（格式：std::vector<std::vector<cmvr::ctrl::PoseTarget>>）
std::vector<std::vector<cmvr::ctrl::PoseTarget>> trajectory;
const double angle_step = (angle_end - angle_start) / num_points; // 每步角度增量
for (int i = 0; i <= num_points; ++i) {
const double current_angle = angle_start + i * angle_step; // 当前轨迹点角度
Eigen::Matrix4d point_T = center_T; // 复制圆心位姿，在此基础上偏移生成轨迹点
// 根据选择的平面，计算位置偏移（保持对应轴不变）
switch (plane_choice) {
case 0: // XY平面：Z轴固定，X/Y随角度变化
point_T(0, 3) += circle_radius * cos(current_angle); // X方向偏移
point_T(1, 3) += circle_radius * sin(current_angle); // Y方向偏移
break;
case 1: // XZ平面：Y轴固定，X/Z随角度变化
point_T(0, 3) += circle_radius * cos(current_angle); // X方向偏移
point_T(2, 3) += circle_radius * sin(current_angle); // Z方向偏移
break;
case 2: // YZ平面：X轴固定，Y/Z随角度变化
point_T(1, 3) += circle_radius * cos(current_angle); // Y方向偏移
point_T(2, 3) += circle_radius * sin(current_angle); // Z方向偏移
break;
default:
LOG(WARNING) << "Invalid plane choice (" << plane_choice << "), use default XY plane";
point_T(0, 3) += circle_radius * cos(current_angle);
point_T(1, 3) += circle_radius * sin(current_angle);
}
// 构造当前时间步的PoseTarget（控制末端执行器）
cmvr::ctrl::PoseTarget ee_target;
ee_target.link_name = ee_link; // 目标控制的link（末端执行器）
ee_target.T_target = point_T; // 相对base_link的目标位姿矩阵
ee_target.w_posrot = 0.5; // 位置/姿态权重（0.5：位置和姿态同等重要）
ee_target.weight = 1.0; // 任务权重（1.0：硬约束，必须满足）
// 每个时间步可控制多个link（此处仅控制一个末端，故vector大小为1）
std::vector<cmvr::ctrl::PoseTarget> step_targets = {ee_target};
trajectory.push_back(step_targets); // 将当前时间步目标加入轨迹
}
// 输出轨迹信息（调试用）
LOG(INFO) << "Trajectory generated successfully: "
<< trajectory.size() << " time steps, radius: " << circle_radius << "m";
// 5. 执行轨迹（调用followPoseTrajectory接口）
LOG(INFO) << "Starting circular trajectory execution...";
robot->followPoseTrajectory(base_link,trajectory, dt); // 传入轨迹和时间间隔
LOG(INFO) << "Circular trajectory execution completed!";
} catch (const std::exception& e) {
// 捕获并打印异常（如机器人未就绪、逆解失败等）
LOG(ERROR) << "Trajectory execution failed: " << e.what();
throw; // 可选：抛出异常让测试框架捕获，标记测试失败
}
}

1
src/hardware/CMakeLists.txt
View File

@ -2,6 +2,7 @@ add_library(hardware SHARED
can_interface.cpp
serial_interface.cpp
esp32_serial_port.cpp
io_interface.cpp
)
target_include_directories(hardware PUBLIC

5
src/hardware/io_interface.cpp Normal file
View File

@ -0,0 +1,5 @@
//
// Created by linbo on 2025/9/24.
//
#include "../../include/hardware/io_interface.h"

73
src/service/grpc_humanoid_robot_service.cpp
View File

@ -6,7 +6,7 @@
#include "service/grpc_humanoid_robot_service.h"
#include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h"
#include "cmvr/msgs/geometry.pb.h"
using namespace cmvr::service;
using namespace cmvr::device;
@ -80,4 +80,75 @@ grpc::Status gRPCHumanoidRobotServiceImpl::moveJ(grpc::ServerContext *context,
return ret;
}
grpc::Status gRPCHumanoidRobotServiceImpl::moveL(grpc::ServerContext* context, const cmvr::api::MoveL_Request* request, cmvr::api::MoveL_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
auto ee_link = request->ee_link();
msgs::Pose3d pose;
pose.mutable_position()->set_x(request->targetpose().x());
pose.mutable_position()->set_y(request->targetpose().y());
pose.mutable_position()->set_z(request->targetpose().z());
pose.mutable_euler()->set_rx(request->targetpose().rx());
pose.mutable_euler()->set_ry(request->targetpose().ry());
pose.mutable_euler()->set_rz(request->targetpose().rz());
robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc());
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}
grpc::Status gRPCHumanoidRobotServiceImpl::speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try
{
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
auto joint_name = request->joint_name();
auto dir = static_cast<device::RobotJointIndexDirection>(request->dir());
auto vel = request->vel();
auto acc = request->acc();
robot->speedJ(joint_name,dir,vel,acc);
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}
grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try
{
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
auto ee_link = request->ee_link();
auto dir = static_cast<device::RobotJointIndexDirection>(request->dir());
auto vel = request->vel();
auto acc = request->acc();
auto cart = static_cast<device::RobotCartesian>(request->cart());
robot->setToolFrame(ee_link);
robot->speedL(cart,dir,vel,acc);
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}