feat: 扩展可达性测试，增加物体位置检查

- 修改 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

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