From 8fcd1c1d8cc82244cc3e71171deb18893221f8c0 Mon Sep 17 00:00:00 2001
From: sladro <sladro@163.com>
Date: Sat, 13 Sep 2025 16:55:39 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20=E6=89=A9=E5=B1=95=E5=8F=AF=E8=BE=BE?=
 =?UTF-8?q?=E6=80=A7=E6=B5=8B=E8=AF=95=EF=BC=8C=E5=A2=9E=E5=8A=A0=E7=89=A9?=
 =?UTF-8?q?=E4=BD=93=E4=BD=8D=E7=BD=AE=E6=A3=80=E6=9F=A5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 修改 test_reachability 方法，增加transport_object初始位置的获取
- 更新 _check_reachability 方法支持三个关键位置测试：物体位置、A点、B点
- 优化状态显示，明确区分三个位置的可达性结果
- 确保完整任务流程的可达性验证（取物→A点→B点）

🤖 Generated with [Claude Code](https://claude.ai/code)

Co-Authored-By: Claude <noreply@anthropic.com>
---
 src/gui/main_window.py | 34 ++++++++++++++++++++++------------
 1 file changed, 22 insertions(+), 12 deletions(-)

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