From b2f178c4098e25c301cb54a70c1088040dce579e Mon Sep 17 00:00:00 2001 From: sladro Date: Thu, 11 Sep 2025 11:28:05 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8DKDL=E9=80=86=E8=BF=90?= =?UTF-8?q?=E5=8A=A8=E5=AD=A6=E5=B4=A9=E6=BA=83=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 修复ik_vel_solver对象生命周期问题，改为实例变量避免过早销毁 - 添加kinematics配置支持(max_iterations, epsilon) - 移除违反编码规范的回退方案 - 清理调试代码保持代码简洁 解决了ChainIkSolverPos_NR_JL访问已销毁局部变量导致的Windows平台崩溃 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude --- .claude/settings.local.json | 8 ++++++-- .claude/tdd-guard/data/test.json | 34 ++++++++++++++++++++++++++++++++ CLAUDE.md | 14 ++++++++++--- config.json | 4 ++++ src/robot/arm_controller.py | 12 ----------- src/robot/kinematics.py | 25 ++++++++++++++--------- 6 files changed, 71 insertions(+), 26 deletions(-) create mode 100644 .claude/tdd-guard/data/test.json diff --git a/.claude/settings.local.json b/.claude/settings.local.json index b2bc9f8..586e6ad 100644 --- a/.claude/settings.local.json +++ b/.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" } \ No newline at end of file diff --git a/.claude/tdd-guard/data/test.json b/.claude/tdd-guard/data/test.json new file mode 100644 index 0000000..67d2eeb --- /dev/null +++ b/.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" + } + ] + } + ] +} \ No newline at end of file diff --git a/CLAUDE.md b/CLAUDE.md index 0a7f5e9..a2f4240 100644 --- a/CLAUDE.md +++ b/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上进行逆运动学计算时发生内部错误导致进程终止。 \ No newline at end of file +**解决方案**： +- 将 `ik_vel_solver` 改为实例变量 `self.ik_vel_solver` +- 确保与 `ik_solver` 有相同的生命周期 +- 移除违反编码规范的回退方案 + +**修复文件**：`src/robot/kinematics.py` \ No newline at end of file diff --git a/config.json b/config.json index 712dd40..f832e8c 100644 --- a/config.json +++ b/config.json @@ -107,5 +107,9 @@ }, "gui": { "window_size": "1200x800" + }, + "kinematics": { + "max_iterations": 1500, + "epsilon": 1e-5 } } \ No newline at end of file diff --git a/src/robot/arm_controller.py b/src/robot/arm_controller.py index a3ce9e4..d9466cf 100644 --- a/src/robot/arm_controller.py +++ b/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: diff --git a/src/robot/kinematics.py b/src/robot/kinematics.py index 087993e..878b560 100644 --- a/src/robot/kinematics.py +++ b/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) \ No newline at end of file + # Get kinematics config + kinematics_config = config_loader.get_full_config()['kinematics'] + + return KinematicsEngine(urdf_path, base_position, base_orientation, kinematics_config) \ No newline at end of file