diff --git a/CLAUDE.md b/CLAUDE.md index a2f4240..c08274c 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -146,4 +146,23 @@ wall_position = [2.0, 0.0, 1.0] # 错误！ - 确保与 `ik_solver` 有相同的生命周期 - 移除违反编码规范的回退方案 -**修复文件**：`src/robot/kinematics.py` \ No newline at end of file +**修复文件**：`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` \ No newline at end of file diff --git a/src/simulation/environment.py b/src/simulation/environment.py index fa853af..2f998a7 100644 --- a/src/simulation/environment.py +++ b/src/simulation/environment.py @@ -19,6 +19,7 @@ class Environment: self.wall_id: Optional[int] = None self.transport_object_id: Optional[int] = None self.ground_plane_id: Optional[int] = None + self._wall_part_ids: List[int] = [] # 记录所有墙体部件ID def setup_simulation(self) -> None: """Setup basic simulation environment""" @@ -48,80 +49,123 @@ class Environment: ) return self.ground_plane_id - def create_wall_with_hole(self) -> int: - """Create wall with hole obstacle""" - wall_pos = self.wall_config['position'] + def _clear_wall(self) -> None: + """Clear existing wall parts before creating new ones""" + if self._wall_part_ids: + for body_id in self._wall_part_ids: + try: + p.removeBody(body_id, physicsClientId=self.physics_client) + except: + pass # Body might already be removed + self._wall_part_ids = [] + self.wall_id = None + + def create_wall_with_hole(self) -> List[int]: + """Create wall with hole using: + - Top/Bottom: full wall width + - Left/Right: only over the hole height + """ + # Clear existing wall parts first + self._clear_wall() + + wall_pos = self.wall_config['position'] # [x, y, z] - 墙体底部中心 wall_dims = self.wall_config['dimensions'] hole_dims = self.hole_config['dimensions'] hole_pos_rel = self.hole_config['position_relative_to_wall'] # Calculate wall dimensions - wall_width = wall_dims['width'] - wall_height = wall_dims['height'] - wall_thickness = wall_dims['thickness'] + wall_width = float(wall_dims['width']) + wall_height = float(wall_dims['height']) + wall_thickness = float(wall_dims['thickness']) # Calculate hole dimensions - hole_width = hole_dims['width'] - hole_height = hole_dims['height'] + hole_width = float(hole_dims['width']) + hole_height = float(hole_dims['height']) - # Calculate absolute hole position - hole_pos = [ - wall_pos[0] + hole_pos_rel[0], - wall_pos[1] + hole_pos_rel[1], - wall_pos[2] + hole_pos_rel[2] - ] - - # Create wall with hole using multiple boxes - wall_parts = [] - - # Wall parameters + # Wall color wall_color = self.wall_config.get('material', {}).get('color', [0.8, 0.8, 0.8, 1.0]) - # Bottom part (below hole) - bottom_height = hole_pos[2] - wall_pos[2] - hole_height / 2 + # Calculate absolute hole center position + hole_center = [ + wall_pos[0] + hole_pos_rel[0], + wall_pos[1] + hole_pos_rel[1], + wall_pos[2] + wall_height/2 + hole_pos_rel[2] # 默认洞口在墙体中心高度 + ] + + # Calculate hole boundaries + hole_top = hole_center[2] + hole_height/2 + hole_bottom = hole_center[2] - hole_height/2 + + # Calculate wall boundaries + wall_top = wall_pos[2] + wall_height + wall_bottom = wall_pos[2] + + + # Calculate part dimensions + bottom_height = max(0.0, hole_bottom - wall_bottom) + top_height = max(0.0, wall_top - hole_top) + side_width = max(0.0, (wall_width - hole_width) / 2) + + wall_parts = [] + + # 1) Bottom part - FULL WIDTH if bottom_height > 0: - bottom_pos = [wall_pos[0], wall_pos[1], wall_pos[2] + bottom_height / 2] + bottom_pos = [ + wall_pos[0], + wall_pos[1], + wall_bottom + bottom_height/2 + ] bottom_id = self._create_box( - [wall_width / 2, wall_thickness / 2, bottom_height / 2], + [wall_width/2, wall_thickness/2, bottom_height/2], bottom_pos, wall_color ) wall_parts.append(bottom_id) - # Top part (above hole) - top_height = wall_pos[2] + wall_height - (hole_pos[2] + hole_height / 2) + # 2) Top part - FULL WIDTH if top_height > 0: - top_pos = [wall_pos[0], wall_pos[1], hole_pos[2] + hole_height / 2 + top_height / 2] + top_pos = [ + wall_pos[0], + wall_pos[1], + hole_top + top_height/2 + ] top_id = self._create_box( - [wall_width / 2, wall_thickness / 2, top_height / 2], + [wall_width/2, wall_thickness/2, top_height/2], top_pos, wall_color ) wall_parts.append(top_id) - # Left part (left of hole) - left_width = hole_pos[0] - wall_pos[0] - hole_width / 2 + wall_width / 2 - if left_width > 0: - left_pos = [wall_pos[0] - wall_width / 2 + left_width / 2, wall_pos[1], hole_pos[2]] + # 3) Left part - HOLE HEIGHT ONLY + if side_width > 0 and hole_height > 0: + left_pos = [ + wall_pos[0] - (hole_width/2 + side_width/2), + wall_pos[1], + hole_center[2] + ] left_id = self._create_box( - [left_width / 2, wall_thickness / 2, hole_height / 2], + [side_width/2, wall_thickness/2, hole_height/2], left_pos, wall_color ) wall_parts.append(left_id) - # Right part (right of hole) - right_width = wall_pos[0] + wall_width / 2 - (hole_pos[0] + hole_width / 2) - if right_width > 0: - right_pos = [hole_pos[0] + hole_width / 2 + right_width / 2, wall_pos[1], hole_pos[2]] + # 4) Right part - HOLE HEIGHT ONLY + if side_width > 0 and hole_height > 0: + right_pos = [ + wall_pos[0] + (hole_width/2 + side_width/2), + wall_pos[1], + hole_center[2] + ] right_id = self._create_box( - [right_width / 2, wall_thickness / 2, hole_height / 2], + [side_width/2, wall_thickness/2, hole_height/2], right_pos, wall_color ) wall_parts.append(right_id) - # Store the first wall part as main wall ID + # Store wall parts and set main wall ID + self._wall_part_ids = wall_parts if wall_parts: self.wall_id = wall_parts[0] @@ -297,12 +341,13 @@ class Environment: initial_pos = self.transport_object_config['initial_position'] self.set_transport_object_pose(initial_pos) + def cleanup(self) -> None: """Clean up environment objects""" - objects_to_remove = [] + # Clean up wall parts first + self._clear_wall() - if self.wall_id is not None: - objects_to_remove.append(self.wall_id) + objects_to_remove = [] if self.transport_object_id is not None: objects_to_remove.append(self.transport_object_id)