diff --git a/src/gui/main_window.py b/src/gui/main_window.py index a3e54b4..bb046d6 100644 --- a/src/gui/main_window.py +++ b/src/gui/main_window.py @@ -379,16 +379,20 @@ class MainWindow: def test_reachability(self): """Test reachability of task points""" self.update_status("Testing reachability...") - + try: # Get task points task_points = self.config_loader.get_task_points() point_a = task_points['point_A']['position'] point_b = task_points['point_B']['position'] - - # Perform reachability check - this will expose the KDL problem - self._check_reachability(point_a, point_b) - + + # Get transport object position + transport_object = self.config_loader.get_transport_object_config() + object_position = transport_object['initial_position'] + + # Perform reachability check - now testing three points + self._check_reachability(object_position, point_a, point_b) + except Exception as e: import traceback error_msg = f"Reachability test failed: {str(e)}" @@ -397,32 +401,38 @@ class MainWindow: print("Full traceback:") traceback.print_exc() - def _check_reachability(self, point_a, point_b): + def _check_reachability(self, object_position, point_a, point_b): """Check if points are reachable by the robot using KDL kinematics""" try: # Use ArmController's precise reachability checking with KDL # This will call KDL inverse kinematics and expose any problems + object_reachable = self.arm_controller.check_workspace_reachability(object_position) a_reachable = self.arm_controller.check_workspace_reachability(point_a) b_reachable = self.arm_controller.check_workspace_reachability(point_b) - + # Get detailed results results = [] + if object_reachable: + results.append("Object: Reachable") + else: + results.append("Object: Not reachable") + if a_reachable: results.append("Point A: Reachable") else: results.append("Point A: Not reachable") - + if b_reachable: results.append("Point B: Reachable") else: results.append("Point B: Not reachable") - + # Update status based on results - if a_reachable and b_reachable: - self.update_status("Both points are reachable") + if object_reachable and a_reachable and b_reachable: + self.update_status("All three points are reachable") else: self.update_status(f"Reachability: {', '.join(results)}") - + except Exception as e: import traceback error_msg = f"Reachability check failed: {str(e)}"