From ee9a8022d0da8de4e56bab22b7a4b9851ba0212b Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Fri, 19 Sep 2025 15:16:07 +0800 Subject: [PATCH] update --- config/cabin_robot.xml | 53 +- .../robot/humanoid_robot/humanoid_robot.cpp | 615 +++++++++--------- .../robot/humanoid_robot/humanoid_robot.h | 2 +- 3 files changed, 328 insertions(+), 342 deletions(-) diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 701c6335..13dd1646 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -14,9 +14,9 @@ - + - + @@ -36,19 +36,20 @@ urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf" baseLink="PELVIS_S" jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R" - linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S" - bufferSize="50"> + linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM" + bufferSize="50" + verbose="false"> - - - - - - - + + + + + + + - + @@ -68,25 +69,17 @@ + + + - - - - - - - - - - - - - - - - - + + + + + + @@ -139,7 +132,7 @@ - + diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 290c4085..46540788 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -550,268 +550,6 @@ void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vecto } } - - - -// template -// void HumanoidRobot::moveL(std::string &base_link, std::vector &targets, double vel, double acc) { -// try { -// if (rsm_.load() == ROBOT_RUNNING) { -// flash_cmd_.store(true); -// eStop(); -// return; -// } -// if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { -// rsm_.store(ROBOT_RUNNING); -// -// // 获取当前关节状态 -// Eigen::Vector q_init; -// auto q_map = getJointQ(); -// q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"], -// q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"], -// q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], -// q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; -// -// LOG(INFO) << "q_init: " << q_init; -// m_state_->SetQ(q_init); -// m_robot_->ComputeForwardKinematics(m_state_); -// -// // 获取基座链接索引 -// auto base_idx = m_robot_->GetLinkIdx(base_link); -// -// -// // 获取当前末端位姿 - 使用前向运动学计算 -// std::vector current_poses; -// for (const auto& target : targets) { -// auto ee_idx = m_robot_->GetLinkIdx(target.link_name); -// -// -// // 使用正向运动学计算当前位姿 -// Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); -// current_poses.push_back(T); -// -// // 打印当前末端执行器的 XYZ 和欧拉角 -// if (&target == &targets.front()) { -// Eigen::Vector3d position = T.block<3, 1>(0, 3); -// Eigen::Matrix3d rotation = T.block<3, 3>(0, 0); -// Eigen::Vector3d euler = rotationMatrixToEulerZYX(rotation); -// -// LOG(INFO) << "Starting point (Initial position): " -// << "X: " << position[0] << ", Y: " << position[1] << ", Z: " << position[2]; -// LOG(INFO) << "Starting orientation (Euler angles): " -// << "RX: " << euler[0] << ", RY: " << euler[1] << ", RZ: " << euler[2]; -// } -// } -// -// // 计算最大距离和插值点数 -// double max_distance = 0.0; -// for (size_t i = 0; i < targets.size(); i++) { -// Eigen::Vector3d current_pos = current_poses[i].block<3, 1>(0, 3); -// Eigen::Vector3d target_pos = targets[i].T_target.block<3, 1>(0, 3); -// double distance = (target_pos - current_pos).norm(); -// max_distance = std::max(max_distance, distance); -// } -// -// // 基于速度和距离计算插值点数 -// double move_time = max_distance / vel; -// int num_points = static_cast(move_time * 100); // 100Hz控制频率 -// -// // 存储所有插值点的关节角度 -// std::vector> joint_trajectory; -// joint_trajectory.reserve(num_points + 1); -// -// // 记录上一次成功的关节角度 -// Eigen::Vector last_success_q = q_init; -// -// // 预先计算所有插值点的逆运动学 -// for (int i = 0; i <= num_points; i++) { -// if (flash_cmd_.load()) { -// flash_cmd_.store(false); -// rsm_.store(ROBOT_READY); -// return; -// } -// -// double t = static_cast(i) / num_points; -// -// // 创建插值后的目标 -// std::vector interpolated_targets = targets; -// for (size_t j = 0; j < targets.size(); j++) { -// // 位置线性插值 -// Eigen::Vector3d current_pos = current_poses[j].block<3, 1>(0, 3); -// Eigen::Vector3d target_pos = targets[j].T_target.block<3, 1>(0, 3); -// Eigen::Vector3d interp_pos = current_pos + t * (target_pos - current_pos); -// -// // 旋转球面线性插值 -// Eigen::Matrix3d current_rot_matrix = current_poses[j].block<3, 3>(0, 0); -// Eigen::Matrix3d target_rot_matrix = targets[j].T_target.block<3, 3>(0, 0); -// Eigen::Quaterniond current_rot(current_rot_matrix); -// Eigen::Quaterniond target_rot(target_rot_matrix); -// Eigen::Quaterniond interp_rot = current_rot.slerp(t, target_rot); -// -// // 更新目标位姿 -// interpolated_targets[j].T_target.setIdentity(); -// interpolated_targets[j].T_target.block<3, 3>(0, 0) = interp_rot.toRotationMatrix(); -// interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos; -// } -// -// // 求解逆运动学 -// Eigen::Vector q_cmd; -// bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002, -// ctrl::CartesianController::Mode::Position, -// q_cmd, 10000, 1e-6); -// -// if (!ok) { -// LOG(WARNING) << "IK failed at point " << i << ", using last successful configuration"; -// q_cmd = last_success_q; -// } else { -// last_success_q = q_cmd; -// } -// -// joint_trajectory.push_back(q_cmd); -// -// // 获取当前末端执行器的位置 (通过正向运动学) -// m_state_->SetQ(q_cmd); -// m_robot_->ComputeForwardKinematics(m_state_); -// -// // 获取当前末端执行器的位姿 (变换矩阵 T) -// auto ee_idx = m_robot_->GetLinkIdx(targets[0].link_name); -// Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx); -// -// // 从变换矩阵中提取 XYZ 坐标 -// Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3); -// -// // 打印 IK 解算出的 XYZ 位置 -// if (i % 10 == 0) { // 每10个点打印一次，避免日志过多 -// LOG(INFO) << "IK solution at point " << i << " : " -// << "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos[2]; -// } -// } -// -// // 计算每个关节的最大角度变化 -// Eigen::Vector max_angle_change = Eigen::Vector::Zero(); -// for (int i = 1; i < joint_trajectory.size(); i++) { -// Eigen::Vector delta = joint_trajectory[i] - joint_trajectory[i-1]; -// for (int j = 0; j < DOF; j++) { -// if (std::abs(delta[j]) > std::abs(max_angle_change[j])) { -// max_angle_change[j] = delta[j]; -// } -// } -// } -// -// // 计算每个关节所需的时间比例因子 -// Eigen::Vector time_scale_factors = Eigen::Vector::Ones(); -// for (int j = 0; j < DOF; j++) { -// if (std::abs(max_angle_change[j]) > 1e-6) { -// // 根据关节的最大速度和加速度限制计算时间比例因子 -// double max_vel = 1.0; // 假设最大角速度 1 rad/s -// double max_acc = 2.0; // 假设最大角加速度 2 rad/s² -// -// double required_time_vel = std::abs(max_angle_change[j]) / max_vel; -// double required_time_acc = std::sqrt(std::abs(max_angle_change[j]) / max_acc); -// -// double required_time = std::max(required_time_vel, required_time_acc); -// time_scale_factors[j] = required_time / move_time; -// } -// } -// -// // 取最大的时间比例因子作为整体时间缩放因子 -// double max_time_scale = time_scale_factors.maxCoeff(); -// if (max_time_scale > 1.0) { -// // 需要延长运动时间 -// move_time *= max_time_scale; -// num_points = static_cast(move_time * 100); -// LOG(INFO) << "Adjusted move time: " << move_time << " seconds"; -// } -// -// // 执行轨迹 -// for (int i = 0; i <= num_points; i++) { -// if (flash_cmd_.load()) { -// flash_cmd_.store(false); -// break; -// } -// -// // 计算当前时间点的索引（考虑时间缩放） -// int idx = static_cast(i / max_time_scale); -// if (idx >= joint_trajectory.size()) { -// idx = joint_trajectory.size() - 1; -// } -// -// Eigen::Vector q_cmd = joint_trajectory[idx]; -// -// // 发送关节命令 - 控制所有7个关节 -// std::vector joint_points{ -// {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]}, -// {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]}, -// {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]}, -// {"R_WRIST_R", q_cmd[13]} -// }; -// -// // 计算每个关节的角度变化 -// std::vector angle_changes(joint_points.size(), 0.0); -// if (i > 0) { -// int prev_idx = static_cast((i-1) / max_time_scale); -// if (prev_idx >= joint_trajectory.size()) { -// prev_idx = joint_trajectory.size() - 1; -// } -// -// Eigen::Vector prev_q = joint_trajectory[prev_idx]; -// for (size_t j = 0; j < joint_points.size(); j++) { -// angle_changes[j] = std::abs(q_cmd[7 + j] - prev_q[7 + j]); -// } -// } -// -// // 设置每个关节的速度和位置 -// for (size_t j = 0; j < joint_points.size(); j++) { -// auto& joint_point = joint_points[j]; -// auto motor = motor_manager_->getMotor(joint_point.joint_name); -// if (motor != nullptr) { -// // 根据关节的角度变化计算实际速度 -// double actual_vel = vel; -// if (i > 0) { -// actual_vel = angle_changes[j] / (move_time / num_points); -// } -// -// motor->setQd(actual_vel); -// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { -// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); -// } -// motor->setQ(joint_point.rad); -// } -// } -// -// // 等待一段时间，控制频率 -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// } -// -// // 等待最终位置到达 - 检查所有关节 -// bool completion = true; -// do { -// completion = true; -// for (const auto& name : joint_names_) { -// auto motor = motor_manager_->getMotor(name); -// if (motor != nullptr && !motor->reachedTargetQ()) { -// completion = false; -// break; -// } -// } -// if (flash_cmd_.load()) { -// flash_cmd_.store(false); -// return; -// } -// std::this_thread::sleep_for(std::chrono::milliseconds(2)); -// } while (!completion); -// -// rsm_.store(ROBOT_READY); -// } else { -// throw std::runtime_error("rsm invalid"); -// } -// } catch (std::exception &e) { -// rsm_.store(ROBOT_ESTOP); -// throw std::runtime_error(e.what()); -// } -// } - - template void HumanoidRobot::moveL(std::string &base_link, std::vector &targets, double vel, double acc) { try { @@ -1006,7 +744,6 @@ void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirectio if (rsm_.load() == ROBOT_RUNNING) { flash_cmd_.store(true); eStop(); - LOG(ERROR) << "机器人正在运行，停止当前运动"; } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { rsm_.store(ROBOT_RUNNING); @@ -1549,27 +1286,88 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di template void HumanoidRobot::followJointTrajectory(std::vector > &traj, double dt) { try { - if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { - auto ok = check_joint_traj_(traj, dt); - if (!ok) { throw runtime_error("joint traj invalid"); } + // 1. 状态机检查：仅允许在 ESTOP/READY/TOROFF 状态启动 + if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY && rsm_.load() != ROBOT_TOROFF) { + throw runtime_error("followJointTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")"); + } - rsm_.store(ROBOT_RUNNING); - // TODO: need to optimize callback loop - for (auto i = 0; i < traj.size(); i++) { + // 2. 轨迹合法性检查 + if (!check_joint_traj_(traj, dt)) { + throw runtime_error("followJointTrajectory: invalid trajectory"); + } + + // 3. 初始化：切换电机模式为CSP，清空缓冲，更新状态机 + std::lock_guard exec_lock(exec_mtx_); // 防止多线程指令冲突 + CSP_buffer_->clear(); // 清空CSP模式缓冲 + rsm_.store(ROBOT_RUNNING); + + // 3.1 预配置所有电机为CSP模式（避免轨迹执行中切换模式导致延迟） + for (const auto& motor_pair : motor_manager_->motorsMap()) { + auto motor = motor_pair.second; + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + LOG(INFO) << "Motor " << motor->jointName() << " switched to CSP mode"; + } + } + + // 4. 轨迹执行：使用定时器按dt间隔发送轨迹点 + std::shared_ptr traj_timer = std::make_shared(); + std::atomic waypoint_idx(0); // 当前执行的轨迹点索引（原子变量防线程竞争） + std::atomic traj_completed(false); // 轨迹是否完成 + + // 4.1 定时器回调：发送当前轨迹点 + traj_timer->start( + std::chrono::nanoseconds(static_cast(dt * 1e9)), // dt转换为纳秒 + [this, &traj, &waypoint_idx, &traj_completed, traj_timer]() { + // 检查轨迹中断（外部指令触发） if (flash_cmd_.load()) { flash_cmd_.store(false); - LOG(INFO) << "followJointTrajectory is canceled"; + traj_timer->stop(); + traj_completed.store(true); + rsm_.store(ROBOT_ESTOP); + LOG(INFO) << "followJointTrajectory: interrupted by external command"; return; } - servoJ(traj[i], dt); - this_thread::sleep_for(chrono::milliseconds((int) dt)); + + // 检查轨迹是否完成 + size_t current_idx = waypoint_idx.load(); + if (current_idx >= traj.size()) { + traj_timer->stop(); + traj_completed.store(true); + rsm_.store(ROBOT_ESTOP); + LOG(INFO) << "followJointTrajectory: trajectory completed"; + return; + } + + // 4.2 发送当前轨迹点的关节指令 + const auto& current_waypoint = traj[current_idx]; + for (const auto& joint : current_waypoint) { + auto motor = motor_manager_->getMotor(joint.joint_name); + if (motor) { + // 优先使用轨迹点中的速度，若无则用默认速度（0.5 rad/s） + double target_vel = (joint.vel > 0) ? joint.vel : 0.5; + motor->setQd(target_vel); // 设置关节速度 + motor->setQ(joint.rad); // 设置关节目标位置 + LOG(INFO)<< "Joint " << joint.joint_name + << " -> pos=" << joint.rad << " rad, vel=" << target_vel << " rad/s"; + } + } + + // 4.3 推进轨迹点索引 + waypoint_idx.fetch_add(1); } - rsm_.store(ROBOT_ESTOP); - } else { - throw runtime_error("rsm invalid"); + ); + + // 4.4 等待轨迹完成或中断 + while (!traj_completed.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 } - } catch (exception &e) { - throw runtime_error(e.what()); + + } catch (std::exception &e) { + // 异常处理：停止轨迹，重置状态机 + rsm_.store(ROBOT_ESTOP); + LOG(ERROR) << "followJointTrajectory failed: " << e.what(); + throw runtime_error("followJointTrajectory error: " + std::string(e.what())); } } @@ -1577,25 +1375,214 @@ template void HumanoidRobot::followPoseTrajectory(std::string &base_link, std::vector > &targets, double dt) { try { + // 1. 基础校验：状态机与轨迹合法性 if (rsm_.load() == ROBOT_RUNNING) { flash_cmd_.store(true); - eStop(); - } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { - rsm_.store(ROBOT_RUNNING); - // TODO: - // 1. set Timer(dt) - // 2. for each timestamp, use Cyclic Synchronous Position (CSP) Mode to set joint position - // 3. if flash_cmd_ is set, set it to false and exit - // 3. join timer - rsm_.store(ROBOT_READY); - } else { - throw runtime_error("rsm invalid"); + eStop(); // 中断当前运动 + throw runtime_error("followPoseTrajectory: robot is running, interrupted"); } - } catch (exception &e) { - throw runtime_error(e.what()); + if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY) { + throw runtime_error("followPoseTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")"); + } + if (targets.empty()) { + throw runtime_error("followPoseTrajectory: pose trajectory is empty"); + } + if (dt <= 0 || dt > 0.1) { + throw runtime_error("followPoseTrajectory: invalid dt=" + std::to_string(dt) + " (must be 0 < dt ≤ 0.1)"); + } + // // 检查基座链接有效性（依赖机器人模型接口） + // int base_link_idx = m_robot_->GetLinkIdx(base_link); + // if (base_link_idx == -1) { + // throw runtime_error("followPoseTrajectory: invalid base link: " + base_link); + // } + + + // 2. 关键步骤1：获取当前关节状态，解算“轨迹第一个点”的关节配置（作为后续IK基准） + std::lock_guard exec_lock(exec_mtx_); + CSP_buffer_->clear(); // 清空CSP缓冲，避免指令冲突 + + // 2.1 获取当前关节角度（初始化机器人状态） + Eigen::Vector q_current; + auto q_map_current = getJointQ(); + q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"], + q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], + q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"], + q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; + m_state_->SetQ(q_current); + m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态 + + + // 2.2 解算“轨迹第一个点”的关节配置（q_first，作为后续所有IK的初始值） + const auto& first_pose_targets = targets[0]; // 轨迹第一个点的位姿目标 + Eigen::Vector q_first; // 轨迹第一个点的关节配置（IK基准） + bool ik_first_ok = m_cctrl_->compute( + m_state_, // 当前机器人状态（作为IK初始值） + base_link, // 基座链接 + first_pose_targets, // 第一个点的位姿目标 + dt, // 控制周期（用于速度限制） + ctrl::CartesianController::Mode::Position, // 位置控制模式 + q_first, // 输出：第一个点的关节配置 + 10000, // IK最大迭代次数（确保精度） + 1e-6 // IK位置精度（1mm/0.001°） + ); + if (!ik_first_ok) { + throw runtime_error("followPoseTrajectory: IK failed for the FIRST waypoint (unreachable target)"); + } + LOG(INFO) << "followPoseTrajectory: first waypoint IK solved successfully, q_first=" << q_first.transpose(); + + + // 3. 关键步骤2：从当前位置移动到“轨迹第一个点”（过渡运动） + rsm_.store(ROBOT_RUNNING); // 切换状态为运行中 + LOG(INFO) << "followPoseTrajectory: moving from current position to first waypoint..."; + + // 3.1 配置电机为CSP模式（用于过渡运动和后续轨迹） + for (const auto& motor_pair : motor_manager_->motorsMap()) { + auto motor = motor_pair.second; + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + LOG(INFO) << "followPoseTrajectory: motor " << motor->jointName() << " switched to CSP mode"; + } + } + + // 3.2 执行“当前→第一个点”的过渡运动（匀速逼近，避免冲击） + const double TRANSITION_VEL = 0.5; // 过渡运动速度（1rad/s，可根据需求调整） + bool transition_completed = false; + auto transition_start_time = std::chrono::high_resolution_clock::now(); + + while (!transition_completed && !flash_cmd_.load()) { + // 3.2.1 计算当前应到达的关节位置（匀速插值） + auto now = std::chrono::high_resolution_clock::now(); + double elapsed = std::chrono::duration(now - transition_start_time).count(); + Eigen::Vector q_transition = q_current + (q_first - q_current) * std::min(elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0); + + // 3.2.2 发送过渡运动关节指令 + for (size_t i = 0; i < DOF; ++i) { + const std::string& joint_name = joint_names_[i]; + auto motor = motor_manager_->getMotor(joint_name); + if (motor) { + motor->setQd(TRANSITION_VEL); // 过渡运动速度 + motor->setQ(q_transition[i]); // 当前过渡位置 + } + } + + // 3.2.3 检查过渡运动是否完成（所有关节到达目标） + transition_completed = true; + for (size_t i = 0; i < DOF; ++i) { + const std::string& joint_name = joint_names_[i]; + auto motor = motor_manager_->getMotor(joint_name); + if (motor && !motor->reachedTargetQ()) { // 精度阈值：0.0001rad（≈0.0057°） + transition_completed = false; + break; + } + } + + // 3.2.4 控制过渡运动频率（与后续轨迹一致） + std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast(dt * 1e9))); + } + + // 3.2.5 过渡运动中断处理 + if (flash_cmd_.load()) { + flash_cmd_.store(false); + rsm_.store(ROBOT_ESTOP); + throw runtime_error("followPoseTrajectory: transition to first waypoint interrupted"); + } + LOG(INFO) << "followPoseTrajectory: reached first waypoint, start trajectory execution"; + + + // 4. 关键步骤3：执行轨迹（所有点的IK均以q_first为初始值） + std::shared_ptr traj_timer = std::make_shared(); + std::atomic waypoint_idx(0); // 当前执行的轨迹点索引（从0开始，即第一个点） + std::atomic traj_completed(false); // 轨迹是否完成 + Eigen::Vector last_valid_q = q_first; // 上一次有效的关节配置（容错用） + + // 4.1 初始化机器人状态为第一个点（确保轨迹起始状态正确） + m_state_->SetQ(q_first); + m_robot_->ComputeForwardKinematics(m_state_); + + // 4.2 定时器回调：按dt间隔解算IK并发送指令（IK初始值固定为q_first） + traj_timer->start( + std::chrono::nanoseconds(static_cast(dt * 1e9)), // 定时器周期=控制周期dt + [this, &base_link, &targets, &waypoint_idx, &traj_completed, &last_valid_q, &q_first, traj_timer, dt]() { + // 4.2.1 检查外部中断 + if (flash_cmd_.load()) { + flash_cmd_.store(false); + traj_timer->stop(); + traj_completed.store(true); + rsm_.store(ROBOT_ESTOP); + LOG(INFO) << "followPoseTrajectory: trajectory interrupted by external command"; + return; + } + + // 4.2.2 检查轨迹是否完成 + size_t current_idx = waypoint_idx.load(); + if (current_idx >= targets.size()) { + traj_timer->stop(); + traj_completed.store(true); + rsm_.store(ROBOT_ESTOP); + LOG(INFO) << "followPoseTrajectory: trajectory executed completely"; + return; + } + + // 4.2.3 解算当前轨迹点的IK（关键：初始值固定为q_first） + const auto& current_pose_targets = targets[current_idx]; + Eigen::Vector q_cmd; // 当前点的关节目标 + + // 临时更新机器人状态为q_first（确保IK初始值固定） + m_state_->SetQ(q_first); + m_robot_->ComputeForwardKinematics(m_state_); + + bool ik_ok = m_cctrl_->compute( + m_state_, // IK初始值：固定为q_first + base_link, // 基座链接 + current_pose_targets, // 当前点的位姿目标 + dt, // 控制周期 + ctrl::CartesianController::Mode::Position, + q_cmd, // 输出：当前点的关节配置 + 5000, // 减少迭代次数（平衡精度与速度） + 5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽） + ); + + // 4.2.4 IK容错：失败时使用上一次有效配置 + if (!ik_ok) { + LOG(WARNING) << "followPoseTrajectory: IK failed at waypoint " << current_idx + << ", use last valid config (q_last_valid=" << last_valid_q.transpose() << ")"; + q_cmd = last_valid_q; + } else { + last_valid_q = q_cmd; // 更新有效配置 + } + + // 4.2.5 发送当前点的关节指令（固定速度，可根据需求调整） + const double TRAJ_VEL = 1.0; // 轨迹执行速度（1rad/s） + for (size_t i = 0; i < DOF; ++i) { + const std::string& joint_name = joint_names_[i]; + auto motor = motor_manager_->getMotor(joint_name); + if (motor) { + motor->setQd(TRAJ_VEL); // 轨迹执行速度 + motor->setQ(q_cmd[i]); // 关节目标位置 + LOG(INFO) << "followPoseTrajectory: waypoint " << current_idx + << ", joint " << joint_name << " -> pos=" << q_cmd[i] << " rad"; + } + } + + // 4.2.6 推进轨迹点索引 + waypoint_idx.fetch_add(1); + } + ); + + // 4.3 等待轨迹执行完成 + while (!traj_completed.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 + } + + } catch (std::exception &e) { + // 异常处理：重置状态机，确保机器人安全 + rsm_.store(ROBOT_ESTOP); + LOG(ERROR) << "followPoseTrajectory failed: " << e.what(); + throw runtime_error("followPoseTrajectory error: " + std::string(e.what())); } } + template void HumanoidRobot::servoJ(std::vector &joints, double dt) { for (const auto &j: joints) { @@ -1944,7 +1931,7 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & } try { - const double CONTROL_PERIOD = 1.0 / 100.0; // 控制周期保持不变 + const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变 msgs::Pose3d current_pose = fk(base_link, ee_link); // 1. 初始化当前和目标位姿矩阵 @@ -2032,26 +2019,6 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & cartesian_trajectory.push_back(T_interp); } - // 在这里保存轨迹点到文件 - std::ofstream trajectory_file("/home/tankaitao/cmvr-es/trajectory_points.csv"); - if (!trajectory_file.is_open()) { - throw std::runtime_error("Failed to open trajectory file"); - } - - // 写入 CSV 文件头 - trajectory_file << "x,y,z\n"; - - // 写入轨迹点位置 - for (size_t i = 0; i < cartesian_trajectory.size(); ++i) { - Eigen::Matrix4d T = cartesian_trajectory[i]; - Eigen::Vector3d position = T.block<3, 1>(0, 3); // 获取位置 - - trajectory_file << position.x() << "," << position.y() << "," << position.z() << "\n"; - } - - trajectory_file.close(); - LOG(INFO) << "Trajectory points saved to /home/tankaitao/cmvr-es/trajectory_points.csv"; - // 6. 预先计算所有轨迹点的关节位置 std::vector> joint_positions; @@ -2264,7 +2231,33 @@ void HumanoidRobot::generateSTrapezoidalProfile(double total_distance, doub } } +template +cmvr::math::Pose3d HumanoidRobot::getTransform(std::string &base_link, std::string &target_link) +{ + // 获取gRPC生成的Pose3d消息 + auto grpc_pose = fk(base_link, target_link); + // 转换为cmvr::math::Pose3d + cmvr::math::Pose3d math_pose; + + // 转换位置信息 + math_pose.position.x = grpc_pose.position().x(); + math_pose.position.y = grpc_pose.position().y(); + math_pose.position.z = grpc_pose.position().z(); + + // 转换四元数 + math_pose.quaternion.w = grpc_pose.quaternion().w(); + math_pose.quaternion.x = grpc_pose.quaternion().x(); + math_pose.quaternion.y = grpc_pose.quaternion().y(); + math_pose.quaternion.z = grpc_pose.quaternion().z(); + + // 转换欧拉角 + math_pose.euler.rx = grpc_pose.euler().rx(); + math_pose.euler.ry = grpc_pose.euler().ry(); + math_pose.euler.rz = grpc_pose.euler().rz(); + + return math_pose; +} template class cmvr::device::HumanoidRobot<7>; template class cmvr::device::HumanoidRobot<14>; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index 0764c8a2..c1b2d70e 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -63,7 +63,7 @@ namespace cmvr::device{ void getJointQ(std::unordered_map &joint_qs) const override; void getState(RobotState &state) override; - math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");} + math::Pose3d getTransform(std::string &bask_link, std::string &target_link) override; /* torque on and off */ void torqueOn() override;