From 04ec70ae577dc5264dc629717dfeb399b01b84a3 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Mon, 9 Mar 2026 17:01:02 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=80=9F=E5=BA=A6=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_servo_control/ik_controller.py | 49 +++++++++++-------- 1 file changed, 29 insertions(+), 20 deletions(-) diff --git a/src/openarm_servo_control/openarm_servo_control/ik_controller.py b/src/openarm_servo_control/openarm_servo_control/ik_controller.py index f4533bd..9aa3092 100644 --- a/src/openarm_servo_control/openarm_servo_control/ik_controller.py +++ b/src/openarm_servo_control/openarm_servo_control/ik_controller.py @@ -89,6 +89,10 @@ class ArucoIKController(Node): max_acceleration=0.01, # 建议 0.2~0.6，降低以减小加速度 dt=self.control_period ) + self.Kp = 20.0 # 比例增益（需调试） + self.max_vel = 3.0 # rad/s 每关节上限 + self.max_acc = 1.0 # rad/s^2 加速度限幅 + self.pose_queue = queue.Queue(maxsize=10) # 用于存储最近的目标位姿，避免处理过旧的指令 self.ik_ready = False self.prev_filtered_pos = None @@ -312,25 +316,30 @@ class ArucoIKController(Node): q_sol = np.array(res.solution.joint_state.position[:len(self.joint_names)]) # q_limited = self.joint_limiter.limit(q_sol) - # # 计算速度（基于上一帧的 q），并限幅 - if self.prev_q is None: - vel = np.zeros_like(q_sol) - else: - vel = (q_sol - self.prev_q) / self.control_period - - vel = np.clip(vel, -self.joint_limiter.max_velocity, self.joint_limiter.max_velocity) - - # # 计算加速度并限幅 - if self.prev_vel is None: - acc = np.zeros_like(vel) - else: - acc = (vel - self.prev_vel) / self.control_period - - acc = np.clip(acc, -self.joint_limiter.max_acceleration, self.joint_limiter.max_acceleration) # 保存为下一次使用 self.prev_q = q_sol.copy() - self.prev_vel = vel.copy() + # 得到当前关节（按 self.joint_names 顺序） + if self.latest_joint_state is None: + return + q_curr = np.array([self.latest_joint_state.position[self.latest_joint_state.name.index(j)] + for j in self.joint_names]) + + # q_sol 是 IK 求得的目标位置（numpy） + q_target = q_sol.copy() + + # 1) 计算速度（比例律） + v_des = self.Kp * (q_target - q_curr) + + # 2) 限幅速度与加速度（基于 self.prev_vel） + v_des = np.clip(v_des, -self.max_vel, self.max_vel) + if getattr(self, 'prev_vel', None) is not None: + dv = (v_des - self.prev_vel) / self.control_period + dv = np.clip(dv, -self.max_acc, self.max_acc) + v_des = self.prev_vel + dv * self.control_period + + # 3) 积分得到下一个位置目标 + q_cmd = q_curr + v_des * self.control_period # 构造轨迹点：把 time_from_start 设大一些（例如 0.1s）让控制器有更长时间插值，减少瞬时加速度 point = JointTrajectoryPoint() @@ -338,11 +347,11 @@ class ArucoIKController(Node): # 在 velocity-only 模式下，不设置 positions，只填 velocities（取决于控制器实现是否支持） # if not self.publish_velocity_only: # point.positions = q_limited.tolist() - point.positions = q_sol.tolist() - point.velocities = vel.tolist() - point.accelerations = acc.tolist() + point.positions = q_cmd.tolist() + # point.velocities = vel.tolist() + # point.accelerations = acc.tolist() # point.time_from_start.sec = 0 - # point.time_from_start.nanosec = int(0.1 * 1e9) # 0.1s + # point.time_from_start.nanosec = int(0.05 * 1e9) # 0.1s # msg.data = (q_sol).tolist() # self.get_logger().info(f"msg.data: {msg.data}") # self.get_logger().info(f"Publishing joint command: vel={np.round(vel,3).tolist()}, acc={np.round(acc,3).tolist()}, pos={np.round(q_limited,3).tolist()}")