diff --git a/include/devices/abstract_robot.h b/include/devices/abstract_robot.h index 4f6e89ed..e9879e7e 100644 --- a/include/devices/abstract_robot.h +++ b/include/devices/abstract_robot.h @@ -77,6 +77,9 @@ namespace cmvr::device{ virtual void moveL(std::string &base_link, std::vector &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } + virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } + virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } + virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index b579d94e..36b9204a 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -929,6 +929,316 @@ cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const st return pose; } + +void printTrajectoryInfo( + const std::vector& trajectory, + const std::vector& times, + const std::vector& velocities, + double total_distance) { + + std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl; + std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl; + std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl; + std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl; + std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + std::cout << std::setw(4) << "序号" << " | " + << std::setw(8) << "时间(s)" << " | " + << std::setw(10) << "x(m)" << " | " + << std::setw(10) << "y(m)" << " | " + << std::setw(10) << "z(m)" << " | " + << std::setw(12) << "速度(m/s)" << " | " + << std::setw(16) << "到起点距离(m)" << std::endl; + std::cout << "-----------------------------------------------------------------------------------------" << std::endl; + + Eigen::Vector3d start_pos(trajectory[0](0,3), trajectory[0](1,3), trajectory[0](2,3)); + for (size_t idx = 0; idx < trajectory.size(); ++idx) { + const auto& T = trajectory[idx]; + Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3)); + double dist_from_start = (pos - start_pos).norm(); + + std::cout << std::setw(4) << idx << " | " + << std::fixed << std::setprecision(3) << std::setw(8) << times[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(12) << velocities[idx] << " | " + << std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl; + } + std::cout << "=========================================================================================\n" << std::endl; +} + + +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"]; + + 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(); + + // 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()); + + // 3. 调用moveL执行直线运动到目标位姿 + moveL(base_link, ee_link, target_pose, vel, acc); + + } catch (const std::exception &e) { + LOG(ERROR) << "moveDeltaL failed: " << e.what(); + throw std::runtime_error(std::string("moveDeltaL error: ") + e.what()); + } +} + +template +void HumanoidRobot::moveL(const std::string &base_link, const std::string &ee_link, + msgs::Pose3d target_pose, double vel, double acc) { + if (vel <= 0 || acc <= 0) { + throw std::runtime_error("moveL: vel and acc must be positive"); + } + + try { + const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变 + msgs::Pose3d current_pose = fk(base_link, ee_link); + + // 1. 初始化当前和目标位姿矩阵 + Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity(); + T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( + current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz() + ); + T_current(0, 3) = current_pose.position().x(); + T_current(1, 3) = current_pose.position().y(); + T_current(2, 3) = current_pose.position().z(); + + Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity(); + T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( + target_pose.euler().rx(), target_pose.euler().ry(), target_pose.euler().rz() + ); + T_target(0, 3) = target_pose.position().x(); + T_target(1, 3) = target_pose.position().y(); + T_target(2, 3) = target_pose.position().z(); + + // 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"]; + + 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.w_posrot = 0.5; + target_ik_check.weight = 1.0; + target_ik_check.link_name = ee_link; + + 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中的高迭代次数 + if (!ik_solvable) { + throw std::runtime_error("moveL: Target pose is unreachable"); + } + + // 3. 计算位置差值（与main一致，保持姿态不变） + Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3); + double total_distance = delta_pos.norm(); + + if (total_distance < 1e-6) { + LOG(INFO) << "moveL: Target is already reached"; + return; + } + + // 4. 基于路径长度的均匀插值（核心修改点） + // 计算所需的插值点数（根据速度和控制周期计算） + double move_time = total_distance / vel; // 总移动时间 + size_t num_points = std::max(2ul, static_cast(ceil(move_time / CONTROL_PERIOD))); + double step_distance = total_distance / num_points; // 每个点的距离间隔 + + LOG(INFO) << "moveL: Planning trajectory - points=" << num_points + << ", total distance=" << total_distance << "m, move time=" << move_time << "s"; + + // 5. 生成均匀分布的轨迹点（仅位置变化，姿态保持与起点一致） + std::vector cartesian_trajectory; + + for (size_t i = 0; i <= num_points; ++i) { + double s = static_cast(i) / num_points; // 基于距离的插值系数（0~1） + + Eigen::Matrix4d T_interp = T_current; // 复制起点姿态（保持不变） + // 仅位置按比例插值 + T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x(); + T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y(); + T_interp(2, 3) = T_current(2, 3) + s * delta_pos.z(); + + 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; + + 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) { + 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求解参数（使用main中的高迭代次数） + int max_iter = 10000; + double tolerance = 1e-6; + + // 求解IK，以上一个状态作为初始值（与main一致） + 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); + } + + 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"); + } + // 右臂第二个关节不能超过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; + + + // 更新状态（与main一致，保证连续性） + m_state_->SetQ(q_current); + 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; + joint_command.push_back(jp); + } + servoJ(joint_command, vel, CONTROL_PERIOD); + + // 检查中断 + if (flash_cmd_.load()) { + flash_cmd_.store(false); + LOG(INFO) << "moveL: Interrupted by external command"; + return; + } + + // 控制时间节奏 + auto expected_time = loop_start_time + std::chrono::nanoseconds( + static_cast(i * CONTROL_PERIOD * 1e9) + ); + auto now = std::chrono::high_resolution_clock::now(); + if (now < expected_time) { + std::this_thread::sleep_until(expected_time); + } + } + + // 最终状态更新 + m_state_->SetQ(q_current); + m_robot_->ComputeForwardKinematics(m_state_); + rsm_.store(ROBOT_READY); + LOG(INFO) << "moveL: Trajectory completed successfully"; + + } catch (const std::exception &e) { + LOG(ERROR) << "moveL failed: " << e.what(); + rsm_.store(ROBOT_ERROR); + throw std::runtime_error(std::string("moveL error: ") + e.what()); + } +} + + + 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 02315e22..1da238fa 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -63,7 +63,12 @@ namespace cmvr::device{ void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override; void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, double acc) override ; void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; + void moveL(std::string &base_link, std::vector &targets, double vel, double acc) override; + void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) override; + void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) override; + + void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override; void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index a278a90b..1e55b715 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -19,7 +19,7 @@ DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot confi using namespace cmvr::device; TEST(HumanoidRobotTest,MyRobotTest) { - std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; + std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml"; const XmlNode config(config_path); if (!config.hasChild("DeviceManager")){ @@ -47,7 +47,7 @@ TEST(HumanoidRobotTest,MyRobotTest) { // {"L_WRIST_Y", 0.0}, // {"L_WRIST_R", 0.0}, // - // {"R_SHOULDER_P", 0.0}, + // {"R_SHOULDER_P", 0.0} // {"R_SHOULDER_R", 0.0}, // {"R_SHOULDER_Y", 0.0}, // {"R_ELBOW_R", 0.0}, @@ -430,4 +430,45 @@ TEST(HumanoidRobotTest,MoveIKTest) { // std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // } +} + +TEST(HumanoidRobotTest,MoveLTest) { + 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"; + } + auto dmgr_cfg = config.getChild("DeviceManager"); + auto &dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot = dmgr.getDevice("hc01"); + + + try { + // 定义基座和末端链路名（需与URDF模型中的链路名一致） + std::string base_link = "PELVIS_S"; // 基座坐标系 + std::string ee_link = "R_WRIST_R_S"; // 末端执行器坐标系 + + // 定义目标位姿（直线运动的终点与当前的差值） + cmvr::msgs::Pose3d delta_pose; + delta_pose.mutable_position()->set_x(0.05); + delta_pose.mutable_position()->set_y(0.05); + delta_pose.mutable_position()->set_z(0.05); + delta_pose.mutable_euler()->set_rx(0); + delta_pose.mutable_euler()->set_ry(0); + delta_pose.mutable_euler()->set_rz(0); + + // 4. 设置运动参数（速度单位：m/s，加速度单位：m/s²） + double vel = 0.1; // 最大线速度 0.1m/s + double acc = 0.05; // 加速度 0.05m/s² + + // 调用moveL执行直线运动 + robot->moveDeltaL(base_link, ee_link, delta_pose, vel, acc); + LOG(INFO) << "直线运动完成！"; + + } catch (const std::exception& e) { + // 捕获异常（如IK解算失败、状态非法等） + LOG(ERROR) << "moveL调用失败：" << e.what(); + } } \ No newline at end of file