Doni/RoboticArmTest
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: 修复墙体洞口创建和重建问题

Browse Source 
主要修复：
- 改进墙体分块方式：上下全宽 + 左右洞口高度，避免四角空缺
- 添加墙体清理机制：重建前清理所有旧墙体部件，防止叠加
- 修复cleanup函数：确保清理所有墙体部件而非仅主ID
- 记录所有墙体部件ID用于正确清理

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

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
sladro 2025-09-11 19:44:45 +08:00
parent 70cb0019d8
commit 4429a4a7e4
2 changed files with 106 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
CLAUDE.md
View File

@ -146,4 +146,23 @@ wall_position = [2.0, 0.0, 1.0] # 错误！
- 确保与 `ik_solver` 有相同的生命周期
- 移除违反编码规范的回退方案
**修复文件**：`src/robot/kinematics.py`
**修复文件**：`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`

127
src/simulation/environment.py
View File

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