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)}")