From ed6593a4f36f89771943ca4bd36969b7bce276f9 Mon Sep 17 00:00:00 2001 From: sladro Date: Sat, 13 Sep 2025 16:49:30 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8Dexecute=5Fpath?= =?UTF-8?q?=E6=96=B9=E6=B3=95=E4=BD=BF=E7=94=A8=E5=B7=B2=E8=A7=84=E5=88=92?= =?UTF-8?q?=E8=B7=AF=E5=BE=84=E8=80=8C=E9=9D=9E=E9=87=8D=E6=96=B0=E8=A7=84?= =?UTF-8?q?=E5=88=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 修改execute_path()方法使其直接使用self.planned_path中的已优化路径 - 通过self.object_index和self.point_a_index正确分段执行三个阶段 - 消除重复规划导致的性能浪费和逻辑不一致问题 - 确保执行的路径与用户看到的可视化路径完全一致 - 保留原有的夹爪控制和碰撞检测逻辑 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude --- src/gui/main_window.py | 73 +++++++++++++++--------------------------- 1 file changed, 26 insertions(+), 47 deletions(-) diff --git a/src/gui/main_window.py b/src/gui/main_window.py index 3f630e7..a3e54b4 100644 --- a/src/gui/main_window.py +++ b/src/gui/main_window.py @@ -561,82 +561,61 @@ class MainWindow: self.start_simulation() def execute_path(self): - """Execute three independent path segments""" - self.update_status("Starting three-stage independent execution...") - + """Execute the planned path using pre-planned segments""" + self.update_status("Starting three-stage execution using planned path...") + try: - # Get task points and object position - task_points = self.config_loader.get_task_points() - point_a = task_points['point_A']['position'] - point_b = task_points['point_B']['position'] - transport_object_config = self.config_loader.get_full_config()['transport_object'] - object_position = transport_object_config['initial_position'] - - # Stage 1: Current position to object (independent planning & execution) - self.update_status("Stage 1: Planning path to object...") - current_config = self.arm_controller.get_current_joint_positions() - - config_object = self.arm_controller.inverse_kinematics(object_position, seed_angles=current_config) - if config_object is None: - raise RuntimeError("Cannot find joint configuration for object position") - + # Check if planned path exists + if self.planned_path is None: + raise RuntimeError("No planned path available. Please run path planning first.") + + # Stage 1: Execute path to object using pre-planned segment + self.update_status("Stage 1: Moving to object...") + path_to_object = self.planned_path[:self.object_index + 1] + self.collision_checker.ignore_transport_object = True try: - path_to_object = self.path_planner.plan_auto(current_config, config_object) + success = self.path_executor.execute_path(path_to_object, gripper_action=None) finally: self.collision_checker.ignore_transport_object = False - - self.update_status("Stage 1: Moving to object...") - success = self.path_executor.execute_path(path_to_object, gripper_action=None) + if not success: raise RuntimeError("Stage 1 failed: Could not reach object") - + # Close gripper self.update_status("Closing gripper at object...") if hasattr(self.arm_controller, 'close_gripper'): self.arm_controller.close_gripper() import time time.sleep(0.5) - + # After gripping, ignore transport object collision self.collision_checker.ignore_transport_object = True - - # Stage 2: Object to A (independent planning & execution) - self.update_status("Stage 2: Planning path to point A...") - current_config = self.arm_controller.get_current_joint_positions() - config_a = self.arm_controller.inverse_kinematics(point_a, seed_angles=current_config) - if config_a is None: - raise RuntimeError("Cannot find joint configuration for point A") - - path_to_a = self.path_planner.plan_auto(current_config, config_a) - + + # Stage 2: Execute path from object to A using pre-planned segment self.update_status("Stage 2: Moving to point A...") + path_to_a = self.planned_path[self.object_index:self.point_a_index + 1] + success = self.path_executor.execute_path(path_to_a, gripper_action=None) if not success: raise RuntimeError("Stage 2 failed: Could not reach point A") - - # Stage 3: A to B (independent planning & execution) - self.update_status("Stage 3: Planning path to point B...") - current_config = self.arm_controller.get_current_joint_positions() - config_b = self.arm_controller.inverse_kinematics(point_b, seed_angles=current_config) - if config_b is None: - raise RuntimeError("Cannot find joint configuration for point B") - - path_to_b = self.path_planner.plan_auto(current_config, config_b) - + + # Stage 3: Execute path from A to B using pre-planned segment self.update_status("Stage 3: Moving to point B...") + path_to_b = self.planned_path[self.point_a_index:] + success = self.path_executor.execute_path(path_to_b, gripper_action=None) if not success: raise RuntimeError("Stage 3 failed: Could not reach point B") - + # Open gripper at B self.update_status("Opening gripper at point B...") if hasattr(self.arm_controller, 'open_gripper'): self.arm_controller.open_gripper() time.sleep(0.5) - + self.update_status("All three stages completed successfully!") - + except Exception as e: import traceback self.update_status(f"Path execution error: {str(e)}")