From 019807eb74a58761f5f98602424b5d7a371686e5 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Mon, 9 Mar 2026 16:11:45 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8A=8A=E4=BD=8D=E7=BD=AE=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E5=99=A8=E6=8D=A2=E6=88=90=E5=85=B3=E8=8A=82=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E5=99=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_servo_control/ik_controller.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 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 e7ecd60..f4533bd 100644 --- a/src/openarm_servo_control/openarm_servo_control/ik_controller.py +++ b/src/openarm_servo_control/openarm_servo_control/ik_controller.py @@ -85,8 +85,8 @@ class ArucoIKController(Node): self.control_period = 0.05 # 20 ms self.joint_limiter = JointLimiter( dof=7, - max_velocity=0.2, # 建议 0.1~0.3，降低以减小速度 - max_acceleration=0.5, # 建议 0.2~0.6，降低以减小加速度 + max_velocity=0.01, # 建议 0.1~0.3，降低以减小速度 + max_acceleration=0.01, # 建议 0.2~0.6，降低以减小加速度 dt=self.control_period ) self.pose_queue = queue.Queue(maxsize=10) # 用于存储最近的目标位姿，避免处理过旧的指令 @@ -310,17 +310,17 @@ class ArucoIKController(Node): jt.joint_names = self.joint_names # 计算受限后的关节目标 q_sol = np.array(res.solution.joint_state.position[:len(self.joint_names)]) - q_limited = self.joint_limiter.limit(q_sol) + # q_limited = self.joint_limiter.limit(q_sol) - # 计算速度（基于上一帧的 q），并限幅 + # # 计算速度（基于上一帧的 q），并限幅 if self.prev_q is None: - vel = np.zeros_like(q_limited) + vel = np.zeros_like(q_sol) else: - vel = (q_limited - self.prev_q) / self.control_period + 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: @@ -329,26 +329,26 @@ class ArucoIKController(Node): acc = np.clip(acc, -self.joint_limiter.max_acceleration, self.joint_limiter.max_acceleration) # 保存为下一次使用 - self.prev_q = q_limited.copy() + self.prev_q = q_sol.copy() self.prev_vel = vel.copy() # 构造轨迹点：把 time_from_start 设大一些（例如 0.1s）让控制器有更长时间插值，减少瞬时加速度 - # point = JointTrajectoryPoint() - msg = Float64MultiArray() + point = JointTrajectoryPoint() + # msg = Float64MultiArray() # 在 velocity-only 模式下，不设置 positions，只填 velocities（取决于控制器实现是否支持） # if not self.publish_velocity_only: # point.positions = q_limited.tolist() - - # point.velocities = vel.tolist() - # point.accelerations = acc.tolist() + point.positions = q_sol.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 - msg.data = (q_sol).tolist() + # 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()}") - self.float64MultiArrayPublisher_.publish(msg) - # jt.points.append(point) - # self.traj_pub.publish(jt) + # self.float64MultiArrayPublisher_.publish(msg) + jt.points.append(point) + self.traj_pub.publish(jt) def main(): rclpy.init()