diff --git a/CLAUDE.md b/CLAUDE.md index 8bd43b7..fdd62b2 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -26,8 +26,7 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co ## 技术栈 -- **pybullet**: 物理仿真引擎 -- **kdl**: 运动学/动力学库 +- **PyBullet**: 物理仿真引擎（统一使用PyBullet进行运动学计算和仿真） - **AI RRT***: 路径规划算法 ## 路径规划设计理念 @@ -149,55 +148,18 @@ wall_position = [2.0, 0.0, 1.0] # 错误！ - 墙体和洞口参数变化应反映在仿真中 - A、B点位置调整应影响路径规划结果 -## 已知问题 +## 系统架构更新（2025-01-14） -### 配置更新后路径规划组件未重新初始化（2025-09-13）✅ 已解决 -**现象**：修改配置文件后运行路径规划，报错"Joint index out-of-range"。 +### PyBullet统一架构 +系统已完全迁移到PyBullet架构，移除了KDL依赖： +- **运动学计算**：全部使用PyBullet的IK/FK +- **碰撞检测**：使用PyBullet原生功能 +- **关节顺序**：统一使用PyBullet顺序，无需转换 -**根本原因**： -- `on_config_applied`方法重新创建了`ArmController`但未重新初始化路径规划组件 -- `CollisionChecker`仍持有旧的`arm_controller`引用，其`robot_id`已无效 -- 使用无效的`robot_id`调用`p.resetJointState`导致索引越界错误 - -**解决方案**： -- 在重新创建`ArmController`后立即调用`_initialize_planning_components()` -- 确保路径规划组件使用新的`arm_controller`实例 - -**修复文件**：`src/gui/main_window.py`（第282行） - -### KDL逆运动学崩溃（2025-09-11）✅ 已解决 -**现象**：点击"Test Reachability"按钮后程序直接退出，无错误信息。 - -**根本原因**：C++对象生命周期问题 -- `ik_vel_solver` 作为局部变量在函数结束后被销毁 -- `ChainIkSolverPos_NR_JL` 内部持有已销毁对象的引用 -- 调用 `CartToJnt` 时访问无效内存导致崩溃 - -**解决方案**： -- 将 `ik_vel_solver` 改为实例变量 `self.ik_vel_solver` -- 确保与 `ik_solver` 有相同的生命周期 -- 移除违反编码规范的回退方案 - -**修复文件**：`src/robot/kinematics.py` - -### 墙体洞口创建问题（2025-09-11）✅ 已解决 -**现象**： -1. 修改洞口尺寸后墙体形状异常，呈现"凸"字形 -2. 洞口尺寸变化不生效 -3. 多次修改后墙体形状越来越怪异 - -**根本原因**： -1. **墙体分块设计错误**：上下墙体只有洞口宽度，左右墙体是全高，导致四角空缺 -2. **未清理旧墙体**：每次重建墙体时新旧叠加，形状混乱 -3. **cleanup函数不完整**：只清理了主墙体ID，未清理所有墙体部件 - -**解决方案**： -- 改为"上下全宽 + 左右洞口高度"的分块方式，避免四角空缺 -- 添加 `_clear_wall()` 函数清理所有墙体部件 -- 修复 `cleanup()` 函数调用 `_clear_wall()` -- 使用 `_wall_part_ids` 列表记录所有墙体部件ID - -**修复文件**：`src/simulation/environment.py` +### 主要改进 +1. **精度提升**：IK residualThreshold从0.001降至0.0001 +2. **容差调整**：position_tolerance从0.02增至0.05 +3. **代码简化**：移除了所有关节顺序转换代码 ### RRT*算法路径终点精度问题（2025-09-13）🚧 进行中 **现象**：路径规划生成的路径终点不是精确的目标配置，存在数厘米的偏差。 @@ -309,17 +271,14 @@ wall_position = [2.0, 0.0, 1.0] # 错误！ - **保持谦卑**：我是傻逼，所以要听话 - **简单执行**：傻逼就该简单执行，不要多想 -### 当前已确认的关键BUG（2025-09-14） +### 当前待解决问题（2025-01-14） -1. **KDL和PyBullet关节顺序映射严重错误** - - 初始状态（全0关节）：误差0 - - 复杂配置：误差30-88cm - - 测试结果：目标[1.0, 0.8, 0.6]，KDL认为到达[1.0007, 0.7978, 0.5985]，PyBullet实际[0.7246, 0.9538, 0.5085] +1. **路径执行精度问题** + - 第二阶段执行失败，误差约6.2cm + - 超过position_tolerance设置的5cm容差 + - 可能与物体抓取后的动力学变化有关 -2. **get_current_pose()方法问题** - - 返回的是KDL计算值，不是PyBullet实际值 - - 导致路径执行器使用错误的位置反馈 - -3. **根本原因** - - `_to_pb_order()`和`_to_kdl_order()`转换函数有严重错误 - - 需要修复关节顺序映射 \ No newline at end of file +2. **潜在优化点** + - 考虑增加等待时间让控制器充分收敛 + - 可能需要调整控制器增益参数 + - 物体重量对精度的影响需要进一步研究 \ No newline at end of file diff --git a/config.json b/config.json index 3feb3f8..8c705a4 100644 --- a/config.json +++ b/config.json @@ -102,7 +102,7 @@ }, "execution": { "velocity_scaling": 1.0, - "position_tolerance": 0.02, + "position_tolerance": 0.05, "motor_max_force": 150.0 } } diff --git a/src/gui/main_window.py b/src/gui/main_window.py index cee9e93..59b8b06 100644 --- a/src/gui/main_window.py +++ b/src/gui/main_window.py @@ -4,6 +4,8 @@ import pybullet as p import pybullet_data import threading import time +import numpy as np +import traceback from typing import Dict, Any import sys import os @@ -387,7 +389,6 @@ class MainWindow: self._check_reachability(object_position, point_a, point_b) except Exception as e: - import traceback error_msg = f"Reachability test failed: {str(e)}" self.update_status(error_msg) print(f"ERROR: {error_msg}") @@ -427,7 +428,6 @@ class MainWindow: self.update_status(f"Reachability: {', '.join(results)}") except Exception as e: - import traceback error_msg = f"Reachability check failed: {str(e)}" self.update_status(error_msg) print(f"ERROR in _check_reachability: {error_msg}") @@ -547,6 +547,9 @@ class MainWindow: # 可视化第一段路径（紫色） self._visualize_path_segment(optimized_path_to_object, [0.8, 0, 0.8], "To Object") + # 可视化后添加小延时，让控制器稳定 + time.sleep(0.1) + # 执行第一段路径 self.update_status("Stage 1: Moving to object...") self.collision_checker.ignore_transport_object = True @@ -562,7 +565,6 @@ class MainWindow: self.update_status("Closing gripper at object...") if hasattr(self.arm_controller, 'close_gripper'): self.arm_controller.close_gripper() - import time time.sleep(0.5) # 从现在开始忽略物体碰撞（因为已经抓取） @@ -588,6 +590,9 @@ class MainWindow: # 可视化第二段路径（绿色） self._visualize_path_segment(optimized_path_object_to_a, [0, 1, 0], "To A") + # 可视化后添加小延时，让控制器稳定 + time.sleep(0.1) + # 执行第二段路径 self.update_status("Stage 2: Moving to point A...") success = self.path_executor.execute_path(optimized_path_object_to_a, gripper_action=None) @@ -611,6 +616,9 @@ class MainWindow: # 可视化第三段路径（蓝色） self._visualize_path_segment(optimized_path_a_to_b, [0, 0, 1], "To B") + # 可视化后添加小延时，让控制器稳定 + time.sleep(0.1) + # 执行第三段路径 self.update_status("Stage 3: Moving to point B...") success = self.path_executor.execute_path(optimized_path_a_to_b, gripper_action=None) @@ -629,7 +637,6 @@ class MainWindow: self.update_status("All three stages completed successfully!") except Exception as e: - import traceback self.update_status(f"Execution error: {str(e)}") print("Full traceback:") traceback.print_exc() @@ -645,9 +652,6 @@ class MainWindow: if not hasattr(self, 'path_visualization'): self.path_visualization = [] - # 导入numpy用于插值计算 - import numpy as np - # 直接使用路径点的末端位置来绘制，避免移动机械臂 # 使用PyBullet IK来获取每个配置的末端位置 positions = [] @@ -798,7 +802,6 @@ class MainWindow: def update_status(self, message: str): """Update status display with copyable text""" - import time if hasattr(self, 'log_text'): # Enable text widget to add content self.log_text.config(state=tk.NORMAL) diff --git a/src/planning/collision_checker.py b/src/planning/collision_checker.py index 1621170..2c710b8 100644 --- a/src/planning/collision_checker.py +++ b/src/planning/collision_checker.py @@ -60,10 +60,9 @@ class CollisionChecker: joint_indices = self.robot_loader.joint_indices if len(joint_config) != len(joint_indices): return True - # KDL 顺序 -> PyBullet 顺序 - q_pb = self.arm_controller._to_pb_order(joint_config) + # 现在已经统一使用PyBullet顺序，不需要转换 for i, jid in enumerate(joint_indices): - p.resetJointState(self.robot_id, jid, float(q_pb[i]), physicsClientId=self.physics_client) + p.resetJointState(self.robot_id, jid, float(joint_config[i]), physicsClientId=self.physics_client) # 触发碰撞检测（不推进世界时间） try: @@ -134,9 +133,9 @@ class CollisionChecker: joint_indices = self.robot_loader.joint_indices if len(joint_config) != len(joint_indices): return float('inf') - q_pb = self.arm_controller._to_pb_order(joint_config) + # 现在已经统一使用PyBullet顺序，不需要转换 for i, jid in enumerate(joint_indices): - p.resetJointState(self.robot_id, jid, float(q_pb[i]), physicsClientId=self.physics_client) + p.resetJointState(self.robot_id, jid, float(joint_config[i]), physicsClientId=self.physics_client) try: p.performCollisionDetection(physicsClientId=self.physics_client) except Exception: diff --git a/src/robot/arm_controller.py b/src/robot/arm_controller.py index 947fa77..02107b2 100644 --- a/src/robot/arm_controller.py +++ b/src/robot/arm_controller.py @@ -10,6 +10,7 @@ import numpy as np from typing import List, Tuple, Optional, Dict, Any import time import math +import pybullet as p from .kinematics import KinematicsEngine, create_kinematics_engine from .robot_loader import RobotLoader @@ -39,7 +40,6 @@ class ArmController: # Initialize components self._initialize_components() self._validate_consistency() - self._build_joint_mappings() def _initialize_components(self) -> None: """Initialize KDL kinematics engine and PyBullet robot loader""" @@ -84,54 +84,43 @@ class ArmController: if len(kdl_limits) != len(pybullet_limits): raise RuntimeError("Joint limits count mismatch between KDL and PyBullet") - def _build_joint_mappings(self) -> None: - """建立 KDL 顺序 与 PyBullet 顺序 的双向映射（基于关节名称）""" - kdl_names = self.kinematics_engine.get_joint_names() - pb_names = self.robot_loader.get_joint_names() - if len(kdl_names) != len(pb_names): - raise RuntimeError(f"DOF name count mismatch: KDL={len(kdl_names)}, PyBullet={len(pb_names)}") - name_to_pb = {n: i for i, n in enumerate(pb_names)} - kdl_to_pb = [] - for n in kdl_names: - if n not in name_to_pb: - raise RuntimeError(f"Joint name not found in PyBullet: {n}") - kdl_to_pb.append(name_to_pb[n]) - - pb_to_kdl = [0] * len(kdl_to_pb) - for kdl_i, pb_i in enumerate(kdl_to_pb): - pb_to_kdl[pb_i] = kdl_i - - self._kdl_to_pb = kdl_to_pb - self._pb_to_kdl = pb_to_kdl - - def _to_pb_order(self, q_kdl: List[float]) -> List[float]: - if len(q_kdl) != len(self._kdl_to_pb): - raise ValueError("Input joint vector size mismatch (KDL->PB)") - q_pb = [0.0] * len(q_kdl) - for kdl_i, pb_i in enumerate(self._kdl_to_pb): - q_pb[pb_i] = q_kdl[kdl_i] - return q_pb - - def _to_kdl_order(self, q_pb: List[float]) -> List[float]: - if len(q_pb) != len(self._pb_to_kdl): - raise ValueError("Input joint vector size mismatch (PB->KDL)") - q_kdl = [0.0] * len(q_pb) - for pb_i, kdl_i in enumerate(self._pb_to_kdl): - q_kdl[kdl_i] = q_pb[pb_i] - return q_kdl def forward_kinematics(self, joint_positions: List[float]) -> Tuple[List[float], List[float]]: """Calculate forward kinematics from joint positions to end effector pose""" self._ensure_initialized() - + if not self.validate_joint_configuration(joint_positions): raise ValueError("Joint positions exceed limits or invalid configuration") - + try: - position, orientation = self.kinematics_engine.forward_kinematics(joint_positions) + # 使用PyBullet计算正向运动学 + # 保存当前状态 + saved_states = [] + for i in range(len(joint_positions)): + state = p.getJointState(self.robot_loader.robot_id, i, physicsClientId=self.physics_client) + saved_states.append(state[0]) + + # 设置关节位置 + for i, pos in enumerate(joint_positions): + p.resetJointState(self.robot_loader.robot_id, i, pos, physicsClientId=self.physics_client) + + # 获取末端执行器位置和姿态 + link_state = p.getLinkState( + self.robot_loader.robot_id, + self.robot_loader.end_effector_index, + physicsClientId=self.physics_client + ) + + position = list(link_state[0]) # 世界坐标系中的位置 + orientation = list(link_state[1]) # 四元数姿态 + + # 恢复原始状态 + for i, pos in enumerate(saved_states): + p.resetJointState(self.robot_loader.robot_id, i, pos, physicsClientId=self.physics_client) + return position, orientation - + except Exception as e: raise RuntimeError(f"Forward kinematics calculation failed: {e}") @@ -147,15 +136,13 @@ class ArmController: try: # 使用PyBullet的IK求解器以确保与仿真环境的一致性 - import pybullet as p - pb_solution = p.calculateInverseKinematics( self.robot_loader.robot_id, self.robot_loader.end_effector_index, target_position, targetOrientation=target_orientation, - maxNumIterations=100, - residualThreshold=0.001, + maxNumIterations=200, + residualThreshold=0.0001, physicsClientId=self.physics_client ) @@ -175,29 +162,74 @@ class ArmController: def compute_jacobian(self, joint_positions: List[float]) -> np.ndarray: """Compute Jacobian matrix for given joint configuration""" self._ensure_initialized() - + if not self.validate_joint_configuration(joint_positions): raise ValueError("Invalid joint configuration for Jacobian computation") - + try: - return self.kinematics_engine.compute_jacobian(joint_positions) - + # 使用PyBullet计算雅可比矩阵 + # 保存当前状态 + saved_states = [] + for i in range(len(joint_positions)): + state = p.getJointState(self.robot_loader.robot_id, i, physicsClientId=self.physics_client) + saved_states.append(state[0]) + + # 设置关节位置 + for i, pos in enumerate(joint_positions): + p.resetJointState(self.robot_loader.robot_id, i, pos, physicsClientId=self.physics_client) + + # 计算雅可比矩阵 + # PyBullet返回线速度和角速度的雅可比矩阵 + zero_vec = [0.0] * len(joint_positions) + jac_trans, jac_rot = p.calculateJacobian( + self.robot_loader.robot_id, + self.robot_loader.end_effector_index, + [0, 0, 0], # 末端执行器局部坐标系中的点 + joint_positions, + zero_vec, # 关节速度（计算雅可比不需要） + zero_vec, # 关节加速度（计算雅可比不需要） + physicsClientId=self.physics_client + ) + + # 恢复原始状态 + for i, pos in enumerate(saved_states): + p.resetJointState(self.robot_loader.robot_id, i, pos, physicsClientId=self.physics_client) + + # 组合成6xN的雅可比矩阵 + jacobian = np.vstack([np.array(jac_trans), np.array(jac_rot)]) + return jacobian + except Exception as e: raise RuntimeError(f"Jacobian computation failed: {e}") def validate_joint_configuration(self, joint_positions: List[float]) -> bool: """Validate if joint configuration is within limits and feasible""" self._ensure_initialized() - - # Use KDL engine for primary validation (includes joint limits from URDF) - return self.kinematics_engine.validate_joint_configuration(joint_positions) + + # 使用PyBullet检查关节限制 + if len(joint_positions) != self.kinematics_engine.get_num_joints(): + return False + + # 检查每个关节是否在限制范围内 + for i, pos in enumerate(joint_positions): + joint_info = p.getJointInfo(self.robot_loader.robot_id, i, physicsClientId=self.physics_client) + lower_limit = joint_info[8] # 关节下限 + upper_limit = joint_info[9] # 关节上限 + + # 如果关节有限制（不是连续关节） + if lower_limit > -1e10 and upper_limit < 1e10: + if pos < lower_limit or pos > upper_limit: + return False + + return True def set_joint_positions(self, joint_positions: List[float]) -> None: """Convenience: set robot joint targets in PyBullet with validation.""" self._ensure_initialized() if not self.validate_joint_configuration(joint_positions): raise ValueError("Target joint positions are invalid or exceed limits") - self.robot_loader.set_joint_positions(self._to_pb_order(joint_positions)) + # 直接使用PyBullet顺序，因为我们现在使用PyBullet IK + self.robot_loader.set_joint_positions(joint_positions) def move_to_joint_positions(self, joint_positions: List[float], timeout: float = 5.0) -> bool: @@ -208,8 +240,8 @@ class ArmController: raise ValueError("Target joint positions are invalid or exceed limits") try: - # Set joint positions in PyBullet（按 PyBullet 顺序） - self.robot_loader.set_joint_positions(self._to_pb_order(joint_positions)) + # Set joint positions in PyBullet（已经是 PyBullet 顺序） + self.robot_loader.set_joint_positions(joint_positions) # Wait for movement completion or timeout start_time = time.time() @@ -269,7 +301,7 @@ class ArmController: def get_current_joint_positions(self) -> List[float]: """Get current joint positions from PyBullet simulation""" self._ensure_initialized() - + try: joint_states = self.robot_loader.get_joint_states() pb_names = self.robot_loader.get_joint_names() @@ -278,7 +310,8 @@ class ArmController: if joint_name not in joint_states: raise RuntimeError(f"Joint {joint_name} not found in joint states") q_pb.append(joint_states[joint_name]['position']) - return self._to_kdl_order(q_pb) + # 直接返回PyBullet顺序，因为我们现在使用PyBullet IK + return q_pb except Exception as e: raise RuntimeError(f"Failed to get current joint positions: {e}") @@ -309,17 +342,22 @@ class ArmController: def check_joint_limits(self, joint_positions: List[float]) -> Tuple[bool, List[str]]: """Check joint limits and return violations""" self._ensure_initialized() - + violations = [] - joint_limits = self.kinematics_engine.get_joint_limits() joint_names = self.robot_loader.get_joint_names() - - for i, (pos, (lower, upper), name) in enumerate(zip(joint_positions, joint_limits, joint_names)): - if pos < lower: - violations.append(f"{name}: {pos:.3f} < {lower:.3f}") - elif pos > upper: - violations.append(f"{name}: {pos:.3f} > {upper:.3f}") - + + for i, (pos, name) in enumerate(zip(joint_positions, joint_names)): + joint_info = p.getJointInfo(self.robot_loader.robot_id, i, physicsClientId=self.physics_client) + lower_limit = joint_info[8] + upper_limit = joint_info[9] + + # 如果关节有限制（不是连续关节） + if lower_limit > -1e10 and upper_limit < 1e10: + if pos < lower_limit: + violations.append(f"{name}: {pos:.3f} < {lower_limit:.3f}") + elif pos > upper_limit: + violations.append(f"{name}: {pos:.3f} > {upper_limit:.3f}") + is_valid = len(violations) == 0 return is_valid, violations