fix: 修复KDL逆运动学崩溃问题

- 修复ik_vel_solver对象生命周期问题，改为实例变量避免过早销毁
- 添加kinematics配置支持(max_iterations, epsilon)
- 移除违反编码规范的回退方案
- 清理调试代码保持代码简洁

解决了ChainIkSolverPos_NR_JL访问已销毁局部变量导致的Windows平台崩溃

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

Co-Authored-By: Claude <noreply@anthropic.com>

.claude/settings.local.json

@@ -1,9 +1,13 @@
 {
   "permissions": {
     "allow": [
-      "Bash(git init:*)"
+      "Bash(git init:*)",
+      "mcp__sequential-thinking__sequentialthinking",
+      "Bash(python -m pytest tests/ -v)",
+      "Bash(python:*)"
     ],
     "deny": [],
     "ask": []
-  }
+  },
+  "outputStyle": "code-compliance-checker"
 }

.claude/tdd-guard/data/test.json

@@ -0,0 +1,34 @@
+{
+  "testModules": [
+    {
+      "moduleId": "tests/test_config_loader.py",
+      "tests": [
+        {
+          "name": "test_config_loader",
+          "fullName": "tests/test_config_loader.py::test_config_loader",
+          "state": "passed"
+        }
+      ]
+    },
+    {
+      "moduleId": "tests/test_environment.py",
+      "tests": [
+        {
+          "name": "test_environment",
+          "fullName": "tests/test_environment.py::test_environment",
+          "state": "passed"
+        }
+      ]
+    },
+    {
+      "moduleId": "tests/test_robot_loader.py",
+      "tests": [
+        {
+          "name": "test_robot_loader",
+          "fullName": "tests/test_robot_loader.py::test_robot_loader",
+          "state": "passed"
+        }
+      ]
+    }
+  ]
+}

CLAUDE.md

@@ -133,9 +133,17 @@ wall_position = [2.0, 0.0, 1.0]          # 错误！
 
 ## 已知问题
 
-### KDL逆运动学崩溃（2025-09-11）
+### KDL逆运动学崩溃（2025-09-11）✅ 已解决
 **现象**：点击"Test Reachability"按钮后程序直接退出，无错误信息。
 
-**崩溃点**：`KinematicsEngine.inverse_kinematics()` → `self.ik_solver.CartToJnt()` 
+**根本原因**：C++对象生命周期问题
+- `ik_vel_solver` 作为局部变量在函数结束后被销毁
+- `ChainIkSolverPos_NR_JL` 内部持有已销毁对象的引用
+- 调用 `CartToJnt` 时访问无效内存导致崩溃
 
-**推测**：PyKDL库在Windows上进行逆运动学计算时发生内部错误导致进程终止。
+**解决方案**：
+- 将 `ik_vel_solver` 改为实例变量 `self.ik_vel_solver`
+- 确保与 `ik_solver` 有相同的生命周期
+- 移除违反编码规范的回退方案
+
+**修复文件**：`src/robot/kinematics.py`

config.json

@@ -107,5 +107,9 @@
   },
   "gui": {
     "window_size": "1200x800"
+  },
+  "kinematics": {
+    "max_iterations": 1500,
+    "epsilon": 1e-5
   }
 }

src/robot/arm_controller.py

@@ -101,37 +101,25 @@ class ArmController:
                           target_orientation: List[float] = None,
                           seed_angles: List[float] = None) -> Optional[List[float]]:
         """Calculate inverse kinematics from target pose to joint positions"""
-        print(f"DEBUG IK: target_position={target_position}, target_orientation={target_orientation}")
         self._ensure_initialized()
         
         # Use current joint angles as seed if not provided
         if seed_angles is None:
             seed_angles = self.get_current_joint_positions()
         
-        print(f"DEBUG IK: seed_angles={seed_angles}")
-        
         try:
-            print("DEBUG IK: Calling kinematics_engine.inverse_kinematics...")
             joint_angles = self.kinematics_engine.inverse_kinematics(
                 target_position, target_orientation, seed_angles
             )
-            print(f"DEBUG IK: Raw result from KDL: {joint_angles}")
             
             # Validate solution if found
             if joint_angles is not None:
-                print("DEBUG IK: Validating joint configuration...")
                 if not self.validate_joint_configuration(joint_angles):
-                    print("DEBUG IK: Joint configuration validation failed")
                     return None
-                print("DEBUG IK: Joint configuration validation passed")
             
-            print(f"DEBUG IK: Final result: {joint_angles}")
             return joint_angles
             
         except Exception as e:
-            print(f"DEBUG IK: Exception caught: {e}")
-            import traceback
-            traceback.print_exc()
             raise RuntimeError(f"Inverse kinematics calculation failed: {e}")
     
     def compute_jacobian(self, joint_positions: List[float]) -> np.ndarray:

src/robot/kinematics.py

@@ -18,7 +18,7 @@ class KinematicsEngine:
     """KDL-based kinematics engine for robot arm calculations"""
     
     def __init__(self, urdf_path: str, base_position: List[float] = None, 
-                 base_orientation: List[float] = None):
+                 base_orientation: List[float] = None, kinematics_config: dict = None):
         """Initialize KDL kinematics engine from URDF file"""
         self.urdf_path = urdf_path
         self.base_link: Optional[str] = None
@@ -30,10 +30,17 @@ class KinematicsEngine:
         self._base_to_world_frame = self._create_transform_frame(self.base_position, self.base_orientation)
         self._world_to_base_frame = self._base_to_world_frame.Inverse()
         
+        # Kinematics configuration - 必须提供
+        if kinematics_config is None:
+            raise ValueError("kinematics_config is required")
+        self.max_iterations = kinematics_config['max_iterations']
+        self.epsilon = kinematics_config['epsilon']
+        
         # KDL components
         self.chain: Optional[kdl.Chain] = None
         self.fk_solver: Optional[kdl.ChainFkSolverPos_recursive] = None
         self.ik_solver: Optional[kdl.ChainIkSolverPos_NR_JL] = None
+        self.ik_vel_solver: Optional[kdl.ChainIkSolverVel_pinv] = None
         self.jac_solver: Optional[kdl.ChainJntToJacSolver] = None
         self.joint_limits: List[Tuple[float, float]] = []
         
@@ -217,18 +224,15 @@ class KinematicsEngine:
                     q_max[i] = upper
                 
                 # Create velocity IK solver first
-                ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain)
+                self.ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain)
                 
                 # Create position IK solver with joint limits
                 self.ik_solver = kdl.ChainIkSolverPos_NR_JL(
-                    self.chain, q_min, q_max, self.fk_solver, ik_vel_solver
+                    self.chain, q_min, q_max, self.fk_solver, self.ik_vel_solver, 
+                    self.max_iterations, self.epsilon
                 )
             else:
-                # Fallback without joint limits
-                ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain)
-                self.ik_solver = kdl.ChainIkSolverPos_NR(
-                    self.chain, self.fk_solver, ik_vel_solver
-                )
+                raise RuntimeError("Joint limits not available for IK solver initialization")
                 
         except Exception as e:
             raise RuntimeError(f"Failed to create KDL solvers: {e}")
@@ -385,4 +389,7 @@ def create_kinematics_engine(config_loader) -> KinematicsEngine:
     base_position = robot_config.get('base_position', [0.0, 0.0, 0.0])
     base_orientation = robot_config.get('base_orientation', [0.0, 0.0, 0.0])
     
-    return KinematicsEngine(urdf_path, base_position, base_orientation)
+    # Get kinematics config
+    kinematics_config = config_loader.get_full_config()['kinematics']
+    
+    return KinematicsEngine(urdf_path, base_position, base_orientation, kinematics_config)