publishLoop的else分支是增量速度模式actions_里存的值直接当作rad/s持续下发靠时间积分得到位移不支持绝对位置。 要解析、运行绝对关节点位必须做两件事把actions_元素从「速度值」改为「目标绝对关节角度」在发布循环内做位置闭环实时读取当前关节角计算误差输出限速速度指令走到目标。下面给你完整改造后的publishLoop配套类成员变量、数据解析逻辑可直接替换。1. 先在类私有域新增绝对点位相关变量cpp运行private: // 原有变量省略... // 绝对点位模式新增 // 6轴关节名称固定映射顺序JM0, JM1-2, JM4-3, JM4, JM5, YB const std::vectorstd::string joint_name_list_ {JM0, JM1-2, JM4-3, JM4, JM5, YB}; std::vectordouble point_target_joints_ {0,0,0,0,0,0}; // 当前点位目标绝对角 const double pos_tolerance 0.005; // 到位误差阈值(rad) const double max_move_vel 0.3; // 单轴最大运动速度 rad/s const double pos_kp 1.2; // 位置闭环比例系数2. 完整改造后的 publishLoop支持绝对点位解析cpp运行void publishLoop() { rclcpp::Rate rate(100); while (rclcpp::ok()) { std::lock_guardstd::mutex lock(action_mutex_); auto msg control_msgs::msg::JointJog(); msg.header.stamp this-now(); if (cartesian_mode_active_ joint_states_received_) { // ---------- 笛卡尔模式速度 P 控制保留原有逻辑不变 ---------- const double Kp 0.5; const double VEL_LIMIT 0.5; msg.joint_names joint_name_list_; bool reached true; for (size_t i 0; i 6; i) { double error target_joints_[i] - current_joints_[i]; if (std::fabs(error) 0.005) reached false; double vel Kp * error; vel std::max(-VEL_LIMIT, std::min(VEL_LIMIT, vel)); msg.velocities.push_back(vel); } if (reached) { RCLCPP_INFO(this-get_logger(), ✅ 笛卡尔目标已到达自动退出模式); cartesian_mode_active_ false; msg.velocities.assign(6, 0.0); action_start_time_ this-now(); last_print_time_ 0.0; } double now this-now().seconds(); double elapsed now - action_start_time_.seconds(); if (elapsed - last_print_time_ 0.2 || elapsed 0.05) { last_print_time_ elapsed; auto current_pose kin_.fk(current_joints_); RCLCPP_INFO(this-get_logger(), 笛卡尔模式 | 当前末端: (%.3f, %.3f, %.3f), current_pose[0], current_pose[1], current_pose[2]); } } else { // // 【绝对点位模式】解析 actions_ 内的绝对关节位置 // const auto target_point_map actions_[current_action_idx_]; // 步骤1解析当前点位填充完整6轴目标绝对角度 // 初始化全部为0再用点位覆盖指定轴 std::fill(point_target_joints_.begin(), point_target_joints_.end(), 0.0); for (size_t axis_idx 0; axis_idx joint_name_list_.size(); axis_idx) { const std::string j_name joint_name_list_[axis_idx]; auto map_iter target_point_map.find(j_name); if (map_iter ! target_point_map.end()) { // 核心解析map中存储的值 目标绝对关节角 rad point_target_joints_[axis_idx] map_iter-second; } } // 步骤2位置闭环计算各轴速度指令 msg.joint_names joint_name_list_; msg.velocities.clear(); bool point_reached true; // 空点位代表停驻直接输出零速度 if (target_point_map.empty()) { msg.velocities.assign(6, 0.0); } else { for (size_t i 0; i 6; i) { // 位置误差 目标绝对角 - 当前实际关节角 double pos_error point_target_joints_[i] - current_joints_[i]; if (std::fabs(pos_error) pos_tolerance) { point_reached false; } // P控制生成速度 double vel_cmd pos_kp * pos_error; // 速度限幅 vel_cmd std::clamp(vel_cmd, -max_move_vel, max_move_vel); msg.velocities.push_back(vel_cmd); } } // ----- 打印绝对点位日志 ----- double now this-now().seconds(); double elapsed now - action_start_time_.seconds(); if (elapsed - last_print_time_ 0.2 || elapsed 0.05) { last_print_time_ elapsed; RCLCPP_INFO(this-get_logger(), → 绝对点位 %d | 运行时长: %.1fs | 到位:%s, current_action_idx_ 1, elapsed, point_reached ? 是 : 否); // 打印解析后的完整目标关节 RCLCPP_INFO(this-get_logger(), 目标关节[rad]: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f], point_target_joints_[0], point_target_joints_[1], point_target_joints_[2], point_target_joints_[3], point_target_joints_[4], point_target_joints_[5]); auto current_pose kin_.fk(current_joints_); RCLCPP_INFO(this-get_logger(), 当前末端 XYZ: (%.3f, %.3f, %.3f), current_pose[0], current_pose[1], current_pose[2]); } // 点位到达后自动切换下一个点位 if (point_reached) { current_action_idx_ (current_action_idx_ 1) % actions_.size(); action_start_time_ this-now(); last_print_time_ 0.0; RCLCPP_INFO(this-get_logger(), ✅ 到达绝对点位切换下一站); } } joint_pub_-publish(msg); rate.sleep(); } }二、绝对位置完整解析流程拆解流程 1点位数据存储格式actions_每个元素JointAction mapstring, doublekey 关节名value目标绝对关节角度rad示例cpp运行// 绝对点位定义无除法数值是最终关节位置 actions_.push_back({ {JM0, 1.610731}, {JM1-2, 0.553435}, {JM4-3, 1.073864}, {JM4, 0.0}, {JM5, 0.0}, {YB, 0.0} });流程 2解析映射核心代码cpp运行// 1. 先清空目标数组 std::fill(point_target_joints_.begin(), point_target_joints_.end(), 0.0); // 2. 遍历固定关节名列表从map匹配读取绝对角度 for (size_t axis_idx 0; axis_idx joint_name_list_.size(); axis_idx) { const std::string j_name joint_name_list_[axis_idx]; auto map_iter target_point_map.find(j_name); if (map_iter ! target_point_map.end()) { // 解析map的值赋值为该轴目标绝对位置 point_target_joints_[axis_idx] map_iter-second; } }作用保证 6 轴数组顺序固定和joint_states回调读取顺序对齐只填写 map 中存在的轴未定义轴默认 0。流程 3位置闭环控制实现走到绝对位置cpp运行// 误差 目标绝对位置 - 当前反馈实际位置 double pos_error point_target_joints_[i] - current_joints_[i]; // P控制输出速度 double vel_cmd pos_kp * pos_error; // 限速防止飞车 vel_cmd std::clamp(vel_cmd, -max_move_vel, max_move_vel);current_joints_从/joint_states实时订阅得到当前真实绝对关节角point_target_joints_从actions_解析出的目标绝对关节角持续输出速度指令误差缩小到阈值内判定到位自动切下一个点位。三、配套必须修改的地方删除定时切换点位的线程构造函数注释掉绝对点位靠「到位检测」切换不能固定时间cpp运行// std::thread(ArmAutoDemo::actionSwitchLoop, this).detach();切换 actions_定义注释带/1.0的增量速度序列打开注释块内纯绝对角度点位序列移除原有打印里错误的target_joints_[0]打印笛卡尔坐标代码已在新版中删掉。四、运行逻辑对比表格模式actions_存储值控制方式切换条件增量速度模式rad/s 速度持续输出固定速度积分位移固定 action_duration_ 时间绝对点位模式rad 绝对位置位置闭环 P 调速自动减速趋近关节误差小于阈值到位自动切换五、调参说明max_move_vel最大单轴速度数值越大走得越快pos_kp闭环刚度太小运动缓慢太大抖动pos_tolerance到位精度0.005rad ≈ 0.28°需要更高精度可改为 0.002。
