From 823e235ee0c9e6b3f9f694c39b2a85904e9ef2a1 Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Thu, 11 Sep 2025 16:36:14 +0800 Subject: [PATCH] update --- .../robot/humanoid_robot/humanoid_robot.cpp | 763 +++++++++++++++--- .../robot/humanoid_robot/humanoid_robot.h | 12 +- 2 files changed, 643 insertions(+), 132 deletions(-) diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 36b9204a..a69d9109 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -5,6 +5,7 @@ #include "humanoid_robot.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" #include "motor/ti5_motor/ti5_motor.h" +#include "utils/base/abstract_interpolation.h" using namespace std; @@ -246,7 +247,7 @@ HumanoidRobot::~HumanoidRobot() { {"WAIST_P", 0} }; - this->moveJ(cmd,0.8); + // this->moveJ(cmd,0.8); this->torqueOff(); } @@ -549,35 +550,456 @@ 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) { +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(); - } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { + return; + } + if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) { rsm_.store(ROBOT_RUNNING); - // TODO: - // 1. interpolate line waypoint by vel and acc - // 2. for each waypoint, call cartesian controller to solve joint positions - // 3. for each waypoint, call motor Cyclic Synchronous Position (CSP) command with Timer - // 4. in the loop, check flash_cmd_, if it is true, set it false then exit - // Eigen::Vector q_cmd; - // m_state_->SetQ(state_.joint_positions); - // bool ok = m_cctrl_.compute(m_state_, base_link, targets, 1, ctrl::CartesianController::Mode::Position, q_cmd, 60, 1e-4); - // if (!ok) { - // throw runtime_error("solve IK failed"); - // } + + // 获取当前关节状态 + 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); + interpolated_targets[j].T_target.setIdentity(); + interpolated_targets[j].T_target.block<3, 3>(0, 0) = current_rot_matrix; + 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]; + } + } + + // 执行轨迹 + for (int i = 0; i <= num_points; i++) { + if (flash_cmd_.load()) { + flash_cmd_.store(false); + break; + } + + // 获取当前时间点的关节角度 + Eigen::Vector q_cmd = joint_trajectory[i]; + + // 发送关节命令 + 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]} + }; + + // 设置每个关节的速度和位置 + 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) { + 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 runtime_error("rsm invalid"); + throw std::runtime_error("rsm invalid"); } - } catch (exception &e) { - throw runtime_error(e.what()); + } catch (std::exception &e) { + rsm_.store(ROBOT_ESTOP); + throw std::runtime_error(e.what()); } } + + + template void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) { try { @@ -972,29 +1394,27 @@ template void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) { try { - Eigen::Vector q_current_for_ik; auto q_map_current = getJointQ(); q_current_for_ik << 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"]; + 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"]; LOG(INFO) << "q_current_for_ik: " << q_current_for_ik; // 1. 计算末端当前位姿（通过FK） msgs::Pose3d current_pose = fk(base_link, ee_link); - LOG(INFO)<< current_pose.mutable_position()->x() << " " << current_pose.mutable_position()->y() << " " << current_pose.mutable_position()->z() - << " " << current_pose.mutable_euler()->rx() << " " << current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz(); + LOG(INFO) << current_pose.mutable_position()->x() << " " << current_pose.mutable_position()->y() << " " + << current_pose.mutable_position()->z() << " " << current_pose.mutable_euler()->rx() << " " + << current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz(); // 2. 计算目标位姿 = 当前位姿 + 相对偏移（位置/姿态分别叠加） msgs::Pose3d target_pose; - // 位置偏移（米） target_pose.mutable_position()->set_x(current_pose.position().x() + delta_pose.position().x()); target_pose.mutable_position()->set_y(current_pose.position().y() + delta_pose.position().y()); target_pose.mutable_position()->set_z(current_pose.position().z() + delta_pose.position().z()); - // 姿态偏移（弧度，ZYX欧拉角） target_pose.mutable_euler()->set_rx(current_pose.euler().rx() + delta_pose.euler().rx()); target_pose.mutable_euler()->set_ry(current_pose.euler().ry() + delta_pose.euler().ry()); target_pose.mutable_euler()->set_rz(current_pose.euler().rz() + delta_pose.euler().rz()); @@ -1036,21 +1456,25 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & T_target(1, 3) = target_pose.position().y(); T_target(2, 3) = target_pose.position().z(); + // 保存起始姿态，确保整个运动过程中姿态保持不变 + Eigen::Matrix3d start_orientation = T_current.block<3, 3>(0, 0); + // 2. 获取当前关节配置并验证目标可达性 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"]; + 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"]; LOG(INFO) << "Current joint configuration: " << q_current; m_state_->SetQ(q_current); m_robot_->ComputeForwardKinematics(m_state_); - // 验证目标点可达性 + // 验证目标点可达性 - 使用起始姿态，确保姿态不变 cmvr::ctrl::PoseTarget target_ik_check; target_ik_check.T_target = T_target; + target_ik_check.T_target.block<3, 3>(0, 0) = start_orientation; // 使用起始姿态 target_ik_check.w_posrot = 0.5; target_ik_check.weight = 1.0; target_ik_check.link_name = ee_link; @@ -1058,12 +1482,12 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & Eigen::Vector q_cmd_check; bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002, ctrl::CartesianController::Mode::Position, - q_cmd_check, 10000, 1e-6); // 使用main中的高迭代次数 + q_cmd_check, 10000, 1e-6); if (!ik_solvable) { - throw std::runtime_error("moveL: Target pose is unreachable"); + throw std::runtime_error("moveL: Target pose is unreachable with constant orientation"); } - // 3. 计算位置差值（与main一致，保持姿态不变） + // 3. 计算位置差值（保持姿态不变） Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3); double total_distance = delta_pos.norm(); @@ -1072,22 +1496,27 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & return; } - // 4. 基于路径长度的均匀插值（核心修改点） - // 计算所需的插值点数（根据速度和控制周期计算） - double move_time = total_distance / vel; // 总移动时间 + // 4. 基于S曲线速度规划的时间规划 + // 计算总时间和插值点数 + double move_time = calculateMoveTime(total_distance, vel, acc); size_t num_points = std::max(2ul, static_cast(ceil(move_time / CONTROL_PERIOD))); - double step_distance = total_distance / num_points; // 每个点的距离间隔 + + // 生成时间轴和距离比例 + std::vector time_points; + std::vector distance_ratios; + generateSTrapezoidalProfile(total_distance, vel, acc, move_time, num_points, + time_points, distance_ratios); LOG(INFO) << "moveL: Planning trajectory - points=" << num_points << ", total distance=" << total_distance << "m, move time=" << move_time << "s"; - // 5. 生成均匀分布的轨迹点（仅位置变化，姿态保持与起点一致） + // 5. 生成轨迹点（位置线性插值，姿态保持不变） std::vector cartesian_trajectory; - for (size_t i = 0; i <= num_points; ++i) { - double s = static_cast(i) / num_points; // 基于距离的插值系数（0~1） + double s = distance_ratios[i]; // 使用S曲线规划的距离比例 - Eigen::Matrix4d T_interp = T_current; // 复制起点姿态（保持不变） + Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity(); + T_interp.block<3, 3>(0, 0) = start_orientation; // 保持起始姿态不变 // 仅位置按比例插值 T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x(); T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y(); @@ -1096,45 +1525,12 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & cartesian_trajectory.push_back(T_interp); } - // 6. 打印所有轨迹点信息 - std::cout << "\n===================================== 所有轨迹点信息 =====================================" << std::endl; - std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl; - std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; - std::cout << "每段距离: " << std::fixed << std::setprecision(6) << step_distance << "m" << std::endl; - std::cout << "起点位置: (x=" << T_current(0,3) << ", y=" << T_current(1,3) << ", z=" << T_current(2,3) << ")" << std::endl; - std::cout << "终点位置: (x=" << T_target(0,3) << ", y=" << T_target(1,3) << ", z=" << T_target(2,3) << ")" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << std::endl; - std::cout << std::setw(4) << "序号" << " | " - << std::setw(10) << "x(m)" << " | " - << std::setw(10) << "y(m)" << " | " - << std::setw(10) << "z(m)" << " | " - << std::setw(16) << "到起点距离(m)" << " | " - << std::setw(16) << "与上一点距离(m)" << std::endl; - std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + // 6. 预先计算所有轨迹点的关节位置 + std::vector> joint_positions; + joint_positions.push_back(q_current); // 起始位置 - double prev_distance = 0.0; - for (size_t idx = 0; idx < cartesian_trajectory.size(); ++idx) { - const auto& T = cartesian_trajectory[idx]; - Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3)); - Eigen::Vector3d delta_from_start = pos - T_current.block<3,1>(0,3); - double current_distance = delta_from_start.norm(); - double segment_distance = (idx == 0) ? current_distance : current_distance - prev_distance; - - std::cout << std::setw(4) << idx << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | " - << std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | " - << std::fixed << std::setprecision(6) << std::setw(16) << current_distance << " | " - << std::fixed << std::setprecision(6) << std::setw(16) << segment_distance << std::endl; - - prev_distance = current_distance; - } - std::cout << "=========================================================================================\n" << std::endl; - - // 7. 执行IK求解（与main逻辑一致，逐步更新状态） - auto loop_start_time = std::chrono::high_resolution_clock::now(); - - for (size_t i = 0; i < cartesian_trajectory.size(); ++i) { + // 预先计算所有关节位置 + for (size_t i = 1; i < cartesian_trajectory.size(); ++i) { const auto& T_interp = cartesian_trajectory[i]; // 构造当前目标 @@ -1144,68 +1540,78 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & current_target.w_posrot = 0.5; current_target.weight = 1.0; - // IK求解参数（使用main中的高迭代次数） - int max_iter = 10000; - double tolerance = 1e-6; - - // 求解IK，以上一个状态作为初始值（与main一致） + // 使用前一点的位置作为初始值求解IK + Eigen::Vector q_next; bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, - q_current, max_iter, tolerance); - - // 失败重试机制 - if (!ok) { - LOG(WARNING) << "Retrying IK for point " << i; - ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, ctrl::CartesianController::Mode::Position, - q_current, max_iter * 2, tolerance * 10); - } + q_next, 10000, 1e-6); if (!ok) { - LOG(ERROR) << "IK failed at point " << i; - LOG(ERROR) << "Target pos: (" << T_interp(0,3) << "," - << T_interp(1,3) << "," << T_interp(2,3) << ")"; - throw std::runtime_error("moveL: IK failed during trajectory execution"); + LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point"; + q_next = joint_positions.back(); } - // 右臂第二个关节不能超过90° - if (q_current[8] > 1.5708) - q_current[8] = 1.5708; - // 新增：打印IK求解得到的关节角度 - std::cout << "\n===================================== 关节角度信息 (点 " << i << ") =====================================" << std::endl; - std::cout << "左手臂关节角度（弧度）：" << std::endl; - std::cout << " L_SHOULDER_P: " << std::fixed << std::setprecision(6) << q_current[0] << std::endl; - std::cout << " L_SHOULDER_R: " << std::fixed << std::setprecision(6) << q_current[1] << std::endl; - std::cout << " L_SHOULDER_Y: " << std::fixed << std::setprecision(6) << q_current[2] << std::endl; - std::cout << " L_ELBOW_R: " << std::fixed << std::setprecision(6) << q_current[3] << std::endl; - std::cout << " L_WRIST_P: " << std::fixed << std::setprecision(6) << q_current[4] << std::endl; - std::cout << " L_WRIST_Y: " << std::fixed << std::setprecision(6) << q_current[5] << std::endl; - std::cout << " L_WRIST_R: " << std::fixed << std::setprecision(6) << q_current[6] << std::endl; - std::cout << "\n右手臂关节角度（弧度）：" << std::endl; - std::cout << " R_SHOULDER_P: " << std::fixed << std::setprecision(6) << q_current[7] << std::endl; - std::cout << " R_SHOULDER_R: " << std::fixed << std::setprecision(6) << q_current[8] << std::endl; - std::cout << " R_SHOULDER_Y: " << std::fixed << std::setprecision(6) << q_current[9] << std::endl; - std::cout << " R_ELBOW_R: " << std::fixed << std::setprecision(6) << q_current[10] << std::endl; - std::cout << " R_WRIST_P: " << std::fixed << std::setprecision(6) << q_current[11] << std::endl; - std::cout << " R_WRIST_Y: " << std::fixed << std::setprecision(6) << q_current[12] << std::endl; - std::cout << " R_WRIST_R: " << std::fixed << std::setprecision(6) << q_current[13] << std::endl; - std::cout << "====================================================================================================\n" << std::endl; + joint_positions.push_back(q_next); + } + // 7. 计算每个点的关节速度 + std::vector> joint_velocities; + joint_velocities.push_back(Eigen::Vector::Zero()); // 起始速度为零 - // 更新状态（与main一致，保证连续性） - m_state_->SetQ(q_current); + 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); + } + + // 8. 打印轨迹信息 + std::cout << "\n===================================== 轨迹规划信息 =====================================" << std::endl; + std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl; + std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; + std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl; + std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl; + std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl; + std::cout << "起点位置: (x=" << T_current(0,3) << ", y=" << T_current(1,3) << ", z=" << T_current(2,3) << ")" << std::endl; + std::cout << "终点位置: (x=" << T_target(0,3) << ", y=" << T_target(1,3) << ", z=" << T_target(2,3) << ")" << std::endl; + std::cout << "保持姿态不变" << std::endl; + std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + + // 9. 执行轨迹 + auto loop_start_time = std::chrono::high_resolution_clock::now(); + + for (size_t i = 0; i < cartesian_trajectory.size(); ++i) { + // 获取当前点的关节位置和速度 + Eigen::Vector q_cmd = joint_positions[i]; + Eigen::Vector q_vel = joint_velocities[i]; + + // 更新状态 + m_state_->SetQ(q_cmd); m_robot_->ComputeForwardKinematics(m_state_); - // 发送关节命令 + // 发送关节命令 - 为每个电机单独设置位置和速度 std::vector joint_command; for (size_t j = 0; j < DOF; ++j) { JointPoint jp; jp.joint_name = joint_names_[j]; - jp.rad = q_current[j]; - jp.vel = vel; + jp.rad = q_cmd[j]; + jp.vel = std::abs(q_vel[j]); // 使用计算出的关节速度 joint_command.push_back(jp); } - servoJ(joint_command, vel, CONTROL_PERIOD); + + // 计算当前点应该执行的时间 + double expected_time = time_points[i]; + // servoJ(joint_command, vel, expected_time); + for (const auto &j: joint_command) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + motor->setQd(j.vel); + motor->setQ(j.rad); + } + } // 检查中断 if (flash_cmd_.load()) { @@ -1214,18 +1620,21 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & return; } - // 控制时间节奏 - auto expected_time = loop_start_time + std::chrono::nanoseconds( - static_cast(i * CONTROL_PERIOD * 1e9) + // 控制时间节奏 - 使用精确的时间规划 + auto expected_time_point = loop_start_time + std::chrono::nanoseconds( + static_cast(expected_time * 1e9) ); auto now = std::chrono::high_resolution_clock::now(); - if (now < expected_time) { - std::this_thread::sleep_until(expected_time); + if (now < expected_time_point) { + std::this_thread::sleep_until(expected_time_point); + } else { + LOG(WARNING) << "moveL: Behind schedule at point " << i + << " by " << std::chrono::duration_cast(now - expected_time_point).count() << "ms"; } } // 最终状态更新 - m_state_->SetQ(q_current); + m_state_->SetQ(joint_positions.back()); m_robot_->ComputeForwardKinematics(m_state_); rsm_.store(ROBOT_READY); LOG(INFO) << "moveL: Trajectory completed successfully"; @@ -1238,7 +1647,99 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & } +// 辅助函数：计算运动时间 +template +double HumanoidRobot::calculateMoveTime(double distance, double vel, double acc) { + // 计算加速和减速所需的时间和距离 + double acc_time = vel / acc; + double acc_distance = 0.5 * acc * acc_time * acc_time; + + // 如果加速距离超过总距离的一半，需要调整最大速度 + if (2 * acc_distance > distance) { + // 三角形速度曲线：加速然后直接减速 + double max_reachable_vel = std::sqrt(acc * distance); + return 2 * max_reachable_vel / acc; + } else { + // 梯形速度曲线：加速-匀速-减速 + double constant_distance = distance - 2 * acc_distance; + double constant_time = constant_distance / vel; + return 2 * acc_time + constant_time; + } +} + +// 辅助函数：生成S曲线轨迹规划 +template +void HumanoidRobot::generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc, + double total_time, size_t num_points, + std::vector& time_points, + std::vector& distance_ratios) { + time_points.clear(); + distance_ratios.clear(); + + // 计算加速和减速阶段的时间 + double acc_time = max_vel / max_acc; + double acc_distance = 0.5 * max_acc * acc_time * acc_time; + + // 确定实际的速度曲线形状 + if (2 * acc_distance > total_distance) { + // 三角形速度曲线 + double actual_max_vel = std::sqrt(max_acc * total_distance); + acc_time = actual_max_vel / max_acc; + acc_distance = 0.5 * max_acc * acc_time * acc_time; + + double dec_time = acc_time; + + // 生成时间点和距离比例 + for (size_t i = 0; i <= num_points; ++i) { + double t = static_cast(i) / num_points * total_time; + time_points.push_back(t); + + if (t <= acc_time) { + // 加速阶段 + double s = 0.5 * max_acc * t * t; + distance_ratios.push_back(s / total_distance); + } else { + // 减速阶段 + double dec_start_time = total_time - dec_time; + double dec_elapsed = t - dec_start_time; + double s = acc_distance + actual_max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed; + distance_ratios.push_back(s / total_distance); + } + } + } else { + // 梯形速度曲线 + double constant_time = (total_distance - 2 * acc_distance) / max_vel; + double dec_time = acc_time; + + // 生成时间点和距离比例 + for (size_t i = 0; i <= num_points; ++i) { + double t = static_cast(i) / num_points * total_time; + time_points.push_back(t); + + if (t <= acc_time) { + // 加速阶段 + double s = 0.5 * max_acc * t * t; + distance_ratios.push_back(s / total_distance); + } else if (t <= acc_time + constant_time) { + // 匀速阶段 + double s = acc_distance + max_vel * (t - acc_time); + distance_ratios.push_back(s / total_distance); + } else { + // 减速阶段 + double dec_start_time = acc_time + constant_time; + double dec_elapsed = t - dec_start_time; + double s = acc_distance + max_vel * constant_time + + max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed; + distance_ratios.push_back(s / total_distance); + } + } + } +} + + template class cmvr::device::HumanoidRobot<7>; template class cmvr::device::HumanoidRobot<14>; template class cmvr::device::HumanoidRobot<20>; + + diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index 1da238fa..96dd4289 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -90,10 +90,20 @@ namespace cmvr::device{ int exec_CSP_cmd_(std::vector &ids, std::vector &pos, int dt); int exec_CSV_cmd_(std::vector &ids, std::vector &vel, int dt); int exec_CSC_cmd_(std::vector &ids, std::vector &cur, int dt); + + + public: // 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz); - static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R); + static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);\ + + + double calculateMoveTime(double distance, double vel, double acc); + void generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc, + double total_time, size_t num_points, + std::vector& time_points, + std::vector& distance_ratios); msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link); private: