diff --git a/src/gui/main_window.py b/src/gui/main_window.py index e196c65..d6bb14f 100644 --- a/src/gui/main_window.py +++ b/src/gui/main_window.py @@ -561,71 +561,89 @@ class MainWindow: self.start_simulation() def execute_path(self): - """Execute the planned path""" - if not self.planned_path: - self.update_status("No path planned yet") - return - - self.update_status("Executing planned path...") + """Execute three independent path segments""" + self.update_status("Starting three-stage independent execution...") try: - # Split path into three segments - if hasattr(self, 'object_index') and hasattr(self, 'point_a_index'): - # Segment 1: Initial to object (no gripper action) - path_to_object = self.planned_path[:self.object_index + 1] - # Segment 2: Object to A (gripper closed) - path_object_to_a = self.planned_path[self.object_index:self.point_a_index + 1] - # Segment 3: A to B (gripper closed) - path_a_to_b = self.planned_path[self.point_a_index:] - - # Execute path to object - self.update_status("Moving to object...") - success = self.path_executor.execute_path(path_to_object, gripper_action=None) - if not success: - self.update_status("Failed to reach object") - return - - # Close gripper at object - 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) # Wait for gripper to close - - # Execute path from object to A - self.update_status("Moving object to point A...") - success = self.path_executor.execute_path(path_object_to_a, gripper_action=None) - if not success: - self.update_status("Failed to reach point A") - return - - # Execute path from A to B - self.update_status("Moving from A to B through hole...") - success = self.path_executor.execute_path(path_a_to_b, gripper_action=None) - if not success: - self.update_status("Failed to reach point B") - return - - # 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) # Wait for gripper to open - - self.update_status("Path execution completed successfully!") - else: - # Fallback to old behavior if no segment information - success = self.path_executor.execute_path(self.planned_path, gripper_action=None) - if success: - self.update_status("Path execution completed!") - else: - self.update_status("Path execution failed") + # 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") + + self.collision_checker.ignore_transport_object = True + try: + path_to_object = self.path_planner.plan_auto(current_config, config_object) + 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) + + self.update_status("Stage 2: Moving to point A...") + 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) + + self.update_status("Stage 3: Moving to point B...") + 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)}") print("Full traceback:") traceback.print_exc() + finally: + # Restore collision checking + self.collision_checker.ignore_transport_object = False def _visualize_path(self, path): """Visualize the planned path in PyBullet""" diff --git a/src/planning/path_executor.py b/src/planning/path_executor.py index 5f10762..8a33503 100644 --- a/src/planning/path_executor.py +++ b/src/planning/path_executor.py @@ -118,7 +118,7 @@ class PathExecutor: p.stepSimulation(physicsClientId=self.arm_controller.physics_client) time.sleep(self.timestep) - # final_config = self.arm_controller.get_current_joint_positions() + final_config = self.arm_controller.get_current_joint_positions() final_distance = np.linalg.norm(np.array(target_config) - np.array(final_config)) raise RuntimeError(f"Failed to reach target, error: {final_distance}") diff --git a/src/planning/path_optimizer.py b/src/planning/path_optimizer.py index c4f0270..444f681 100644 --- a/src/planning/path_optimizer.py +++ b/src/planning/path_optimizer.py @@ -202,8 +202,7 @@ class PathOptimizer: # 检查插值点是否有碰撞 if not collision_checker.check_collision(interpolated.tolist()): densified.append(interpolated.tolist()) - else: - raise RuntimeError(f"Densification created collision at segment {i}") + # 如果插值点有碰撞，跳过该点，保持原始路径连接 densified.append(path[i + 1]) diff --git a/src/robot/kinematics.py b/src/robot/kinematics.py index af19a9e..686fa27 100644 --- a/src/robot/kinematics.py +++ b/src/robot/kinematics.py @@ -314,11 +314,21 @@ class KinematicsEngine: if result < 0: return None # No solution found - # Return joint angles + # Extract joint angles joint_angles = [] for i in range(q_out.rows()): joint_angles.append(q_out[i]) + # Verify solution accuracy + actual_pos, _ = self.forward_kinematics(joint_angles) + error = ((actual_pos[0] - target_position[0])**2 + + (actual_pos[1] - target_position[1])**2 + + (actual_pos[2] - target_position[2])**2) ** 0.5 + + # If error is too large, return None + if error > 0.01: # 1cm tolerance + return None + return joint_angles def compute_jacobian(self, joint_positions: List[float]) -> np.ndarray: