From 11f009752124a2463b292f8939e19a23e5188676 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Thu, 25 Sep 2025 16:01:01 +0800 Subject: [PATCH] update --- include/devices/abstract_robot.h | 6 + include/hardware/io_interface.h | 14 + include/service/grpc_humanoid_robot_service.h | 6 + protos/cmvr/api/humanoid_robot.proto | 86 ++++- .../robot/humanoid_robot/humanoid_robot.cpp | 302 ++++++++++-------- .../robot/humanoid_robot/humanoid_robot.h | 2 - .../humanoid_robot/humanoid_robot_test.cpp | 156 ++++++++- src/hardware/CMakeLists.txt | 1 + src/hardware/io_interface.cpp | 5 + src/service/grpc_humanoid_robot_service.cpp | 73 ++++- 10 files changed, 512 insertions(+), 139 deletions(-) create mode 100644 include/hardware/io_interface.h create mode 100644 src/hardware/io_interface.cpp diff --git a/include/devices/abstract_robot.h b/include/devices/abstract_robot.h index e9879e7e..5cbd5ad0 100644 --- a/include/devices/abstract_robot.h +++ b/include/devices/abstract_robot.h @@ -105,9 +105,15 @@ namespace cmvr::device{ virtual void servoL(std::string &base_link, std::vector &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_; }; } diff --git a/include/hardware/io_interface.h b/include/hardware/io_interface.h new file mode 100644 index 00000000..4f04daf5 --- /dev/null +++ b/include/hardware/io_interface.h @@ -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 \ No newline at end of file diff --git a/include/service/grpc_humanoid_robot_service.h b/include/service/grpc_humanoid_robot_service.h index 45ffed4d..cbb3c5c3 100644 --- a/include/service/grpc_humanoid_robot_service.h +++ b/include/service/grpc_humanoid_robot_service.h @@ -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_; }; diff --git a/protos/cmvr/api/humanoid_robot.proto b/protos/cmvr/api/humanoid_robot.proto index e9221157..910d9053 100644 --- a/protos/cmvr/api/humanoid_robot.proto +++ b/protos/cmvr/api/humanoid_robot.proto @@ -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); -} \ No newline at end of file + rpc moveL(MoveL.Request) returns (MoveL.Response); + rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response); + rpc speedL(SpeedL.Request) returns (SpeedL.Response); +} diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 4b5b8bd9..bd12ff94 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -45,8 +45,6 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { l_can_sender_ = std::make_shared >(); l_can_receiver_ = std::make_shared >(); l_message_manager_ = std::make_shared >(); - 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::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { r_can_sender_ = std::make_shared >(); r_can_receiver_ = std::make_shared >(); r_message_manager_ = std::make_shared >(); - 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::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::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::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(ceil(move_time / CONTROL_PERIOD))); + + // 生成S曲线速度规划的时间点和距离比例（主线程预计算） std::vector time_points; std::vector distance_ratios; generateSTrapezoidalProfile(direction.norm(), vel, acc, move_time, num_points, @@ -875,137 +871,77 @@ void HumanoidRobot::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 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> joint_positions; - std::vector> joint_velocities; - - // 获取当前关节位置 - auto q_map_current = getJointQ(); - Eigen::Vector 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::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 q_next; - bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::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 vel = (joint_positions[i] - joint_positions[i-1]) / dt; - joint_velocities.push_back(vel); - } - - // 创建队列用于存储轨迹点 + // 创建轨迹队列及同步机制 std::queue, Eigen::Vector>> trajectory_queue; std::mutex queue_mutex; std::condition_variable queue_cv; - std::atomic trajectory_completed{false}; + std::atomic planning_completed{false}; // 规划是否完成 + std::atomic execution_failed{false}; // 执行是否失败 + std::atomic planned_points{0}; // 已规划的点数 + std::atomic executed_points{0}; // 已执行的点数 - // 轨迹点计算线程 - std::thread trajectory_thread([&]() { - try { - // 计算并将轨迹点放入队列 - for (size_t i = 0; i < joint_positions.size(); ++i) { - { - std::lock_guard lock(queue_mutex); - trajectory_queue.push({joint_positions[i], joint_velocities[i]}); - } - queue_cv.notify_one(); // 通知控制线程 + // 获取当前关节位置（初始点） + auto q_map_current = getJointQ(); + Eigen::Vector 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(CONTROL_PERIOD * 1000))); - } + // 先将初始点加入队列 + { + std::lock_guard lock(queue_mutex); + trajectory_queue.push({q_current, Eigen::Vector::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> point; bool has_point = false; { std::unique_lock 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 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(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::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 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 q_next; + bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, + ctrl::CartesianController::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 q_vel; + if (dt > 0) { + q_vel = (q_next - prev_q) / dt; + } else { + q_vel = Eigen::Vector::Zero(); + } + + // 将计算好的轨迹点放入队列，如果队列满了则等待 + { + std::unique_lock 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 void HumanoidRobot::followJointTrajectory(std::vector > &traj, double dt) { try { @@ -1812,8 +1851,9 @@ void HumanoidRobot::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); diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index cd9bafd9..c1b2d70e 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -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 upd_timer_; std::atomic rsm_{ROBOT_DISABLED}; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index 7d317126..dce81562 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -529,11 +529,45 @@ TEST(HumanoidRobotTest, SpeedLTest) { auto robot = dmgr.getDevice("hc01"); try { + + // std::unordered_map 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 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("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> 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 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; // 可选：抛出异常让测试框架捕获，标记测试失败 + } } \ No newline at end of file diff --git a/src/hardware/CMakeLists.txt b/src/hardware/CMakeLists.txt index 5e3c8c66..f7ef8250 100644 --- a/src/hardware/CMakeLists.txt +++ b/src/hardware/CMakeLists.txt @@ -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 diff --git a/src/hardware/io_interface.cpp b/src/hardware/io_interface.cpp new file mode 100644 index 00000000..3360857b --- /dev/null +++ b/src/hardware/io_interface.cpp @@ -0,0 +1,5 @@ +// +// Created by linbo on 2025/9/24. +// + +#include "../../include/hardware/io_interface.h" diff --git a/src/service/grpc_humanoid_robot_service.cpp b/src/service/grpc_humanoid_robot_service.cpp index 7e78fc21..e2464fdc 100644 --- a/src/service/grpc_humanoid_robot_service.cpp +++ b/src/service/grpc_humanoid_robot_service.cpp @@ -6,7 +6,7 @@ #include "service/grpc_humanoid_robot_service.h" #include #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(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(request->header().device_id()); + auto joint_name = request->joint_name(); + auto dir = static_cast(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(request->header().device_id()); + auto ee_link = request->ee_link(); + auto dir = static_cast(request->dir()); + auto vel = request->vel(); + auto acc = request->acc(); + auto cart = static_cast(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; +} \ No newline at end of file