绝对位置模式与相对位置模式

publishLoop的else分支是增量速度模式：actions_里存的值直接当作rad/s持续下发，靠时间积分得到位移，不支持绝对位置。 要解析、运行绝对关节点位，必须做两件事：

1. 把actions_元素从「速度值」改为「目标绝对关节角度」；
2. 在发布循环内做位置闭环：实时读取当前关节角，计算误差，输出限速速度指令走到目标。

下面给你完整改造后的publishLoop，配套类成员变量、数据解析逻辑，可直接替换。

1. 先在类私有域新增绝对点位相关变量

cpp

运行

private: // 原有变量省略... // ========== 绝对点位模式新增 ========== // 6轴关节名称固定映射，顺序：JM0, JM1-2, JM4-3, JM4, JM5, YB const std::vector<std::string> joint_name_list_ = {"JM0", "JM1-2", "JM4-3", "JM4", "JM5", "YB"}; std::vector<double> 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_guard<std::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 = map<string, double>，key= 关节名，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_解析出的目标绝对关节角；
• 持续输出速度指令，误差缩小到阈值内判定到位，自动切下一个点位。

三、配套必须修改的地方

1. 删除定时切换点位的线程构造函数注释掉，绝对点位靠「到位检测」切换，不能固定时间：

   cpp

   运行

   // std::thread(&ArmAutoDemo::actionSwitchLoop, this).detach();

2. 切换 actions_定义注释带/1.0的增量速度序列，打开注释块内纯绝对角度点位序列；
3. 移除原有打印里错误的target_joints_[0]打印笛卡尔坐标代码（已在新版中删掉）。

四、运行逻辑对比

表格

五、调参说明

1. max_move_vel：最大单轴速度，数值越大走得越快；
2. pos_kp：闭环刚度，太小运动缓慢，太大抖动；
3. pos_tolerance：到位精度，0.005rad ≈ 0.28°，需要更高精度可改为 0.002。