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

feat: 添加碰撞可视化功能

Browse Source 
- 添加详细碰撞检测方法，返回碰撞对信息
- 碰撞的机器人link和障碍物自动显示为红色
- 路径执行时允许碰撞，让物理引擎自然处理
- 功能完全通用，不依赖特定机械臂型号

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

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
sladro 2025-09-14 15:13:39 +08:00
parent 161edce061
commit 831e104429
3 changed files with 196 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
CLAUDE.md
View File

@ -385,4 +385,32 @@ self.arm_controller.set_joint_positions(saved_joint_positions)
**主要修复**（2025-09-14）：
- 修复回放时路径不显示问题
- 新增 `get_planned_path()` 方法
- 改进回放时路径和标记显示
- 改进回放时路径和标记显示
## 碰撞可视化功能 ✅（2025-01-14）
**功能描述**：当路径规划检测到碰撞时，将碰撞的双方（机器人link和障碍物）显示为红色
**实现方案**：
1. **碰撞检测增强**：
- 添加`check_collision_detailed()`方法返回碰撞对信息
- 记录碰撞的机器人link索引和障碍物ID
2. **可视化实现**：
- 碰撞双方自动变红色标记
- 保存原始颜色以便恢复
- 不使用闪烁或动画（MVP原则）
3. **路径执行改进**：
- 即使有碰撞也尝试执行路径
- 让物理引擎自然处理碰撞
- 机器人会停在碰撞位置
**通用性保证**：
- 不依赖特定机械臂型号
- 使用PyBullet标准API
- 配置驱动，换机械臂只需修改config.json
**修复文件**：
- `src/planning/collision_checker.py`（添加详细碰撞检测）
- `src/gui/main_window.py`（添加碰撞可视化和改进执行逻辑）

105
src/gui/main_window.py
View File

@ -577,10 +577,36 @@ class MainWindow:
# 规划完整路径（忽略物体碰撞，因为要抓取它）
self.collision_checker.ignore_transport_object = True
# 分段规划然后合并
path1 = self.path_planner.plan_auto(saved_joint_positions, config_object)
path2 = self.path_planner.plan_auto(config_object, config_a)
path3 = self.path_planner.plan_auto(config_a, config_b)
# 分段规划然后合并（允许部分路径失败）
try:
path1 = self.path_planner.plan_auto(saved_joint_positions, config_object)
except Exception as e:
self.update_status(f"Warning: Cannot plan path to object: {e}")
# 检查碰撞对
has_collision, collision_pairs = self.collision_checker.check_collision_detailed(config_object)
if has_collision and collision_pairs:
self._visualize_collision_points(collision_pairs)
path1 = [saved_joint_positions, config_object] # 直接尝试移动
try:
path2 = self.path_planner.plan_auto(config_object, config_a)
except Exception as e:
self.update_status(f"Warning: Cannot plan path to A: {e}")
# 检查碰撞对
has_collision, collision_pairs = self.collision_checker.check_collision_detailed(config_a)
if has_collision and collision_pairs:
self._visualize_collision_points(collision_pairs)
path2 = [config_object, config_a] # 直接尝试移动
try:
path3 = self.path_planner.plan_auto(config_a, config_b)
except Exception as e:
self.update_status(f"Warning: Cannot plan path to B: {e}")
# 检查碰撞对
has_collision, collision_pairs = self.collision_checker.check_collision_detailed(config_b)
if has_collision and collision_pairs:
self._visualize_collision_points(collision_pairs)
path3 = [config_a, config_b] # 直接尝试移动
# 合并路径（去除重复点）
full_path = path1[:-1] + path2[:-1] + path3
@ -628,6 +654,7 @@ class MainWindow:
# 执行路径，在关键点停留
object_attached = False # 标记物体是否已吸附
for i, config in enumerate(optimized_path):
# 直接尝试移动，物理引擎会自动处理碰撞
self.arm_controller.set_joint_positions(config)
# 执行仿真步进
@ -785,6 +812,62 @@ class MainWindow:
self.update_status(f"Visualized {label} path: {len(path)} waypoints")
def _visualize_collision_points(self, collision_pairs):
"""将碰撞的物体双方显示为红色"""
if not hasattr(self, 'original_colors'):
self.original_colors = {}
# 获取机器人ID
robot_id = self.arm_controller.robot_loader.robot_id
# 遍历碰撞对
for robot_link, obstacle_id, obstacle_link in collision_pairs:
# 标记机器人link为红色
if robot_link >= 0:
# 保存原始颜色
key = (robot_id, robot_link)
if key not in self.original_colors:
visual_data = p.getVisualShapeData(robot_id, physicsClientId=self.physics_client)
for data in visual_data:
if data[1] == robot_link: # data[1]是linkIndex
self.original_colors[key] = data[7] # data[7]是rgbaColor
break
# 设置为红色
p.changeVisualShape(
robot_id,
robot_link,
rgbaColor=[1, 0, 0, 1], # 红色
physicsClientId=self.physics_client
)
# 标记障碍物为红色
if obstacle_id >= 0:
# 保存原始颜色
key = (obstacle_id, obstacle_link)
if key not in self.original_colors:
visual_data = p.getVisualShapeData(obstacle_id, physicsClientId=self.physics_client)
if visual_data:
self.original_colors[key] = visual_data[0][7] # 保存基体颜色
# 设置为红色
if obstacle_link < 0: # 基体
p.changeVisualShape(
obstacle_id,
-1,
rgbaColor=[1, 0, 0, 1], # 红色
physicsClientId=self.physics_client
)
else: # 特定link
p.changeVisualShape(
obstacle_id,
obstacle_link,
rgbaColor=[1, 0, 0, 1], # 红色
physicsClientId=self.physics_client
)
self.update_status(f"Highlighted {len(collision_pairs)} collision pairs in red")
def _clear_visualizations(self):
"""清除所有可视化元素"""
# 清除路径线条
@ -799,6 +882,20 @@ class MainWindow:
p.removeBody(marker_id)
self.point_markers = []
# 恢复原始颜色
if hasattr(self, 'original_colors'):
for (body_id, link_index), color in self.original_colors.items():
try:
p.changeVisualShape(
body_id,
link_index,
rgbaColor=color,
physicsClientId=self.physics_client
)
except:
pass # 物体可能已被删除
self.original_colors = {}
def _add_point_markers(self, object_pos, point_a_pos, point_b_pos):
"""添加任务点标记"""
if not hasattr(self, 'point_markers'):

67
src/planning/collision_checker.py
View File

@ -56,7 +56,7 @@ class CollisionChecker:
state_id = p.saveState(physicsClientId=self.physics_client)
try:
# 将机器人打到“试配置”（仅状态，不经电机/动力学）
# 将机器人打到"试配置"（仅状态，不经电机/动力学）
joint_indices = self.robot_loader.joint_indices
if len(joint_config) != len(joint_indices):
return True
@ -103,6 +103,71 @@ class CollisionChecker:
finally:
p.restoreState(state_id, physicsClientId=self.physics_client)
def check_collision_detailed(self, joint_config: List[float]) -> Tuple[bool, List[Tuple[int, int, int]]]:
"""检测碰撞并返回碰撞的物体对（机器人link索引，障碍物ID，障碍物link索引）"""
collision_pairs = []
if not self.arm_controller.validate_joint_configuration(joint_config):
return True, collision_pairs
state_id = p.saveState(physicsClientId=self.physics_client)
try:
# 设置关节配置
joint_indices = self.robot_loader.joint_indices
if len(joint_config) != len(joint_indices):
return True, collision_pairs
for i, jid in enumerate(joint_indices):
p.resetJointState(self.robot_id, jid, float(joint_config[i]), physicsClientId=self.physics_client)
# 触发碰撞检测
try:
p.performCollisionDetection(physicsClientId=self.physics_client)
except Exception:
pass
has_collision = False
# 自碰撞
contact_points = p.getContactPoints(self.robot_id, self.robot_id, physicsClientId=self.physics_client)
if contact_points:
has_collision = True
for cp in contact_points:
# cp[3]是linkA索引, cp[4]是linkB索引
collision_pairs.append((int(cp[3]), self.robot_id, int(cp[4])))
# 环境碰撞
for obs_id in self._iter_obstacles():
cps = p.getClosestPoints(
self.robot_id,
int(obs_id),
distance=float(max(0.0, self.safety_margin)),
physicsClientId=self.physics_client,
)
if cps:
has_collision = True
for cp in cps:
# cp[3]是机器人link索引
collision_pairs.append((int(cp[3]), int(obs_id), -1))
# 地面碰撞
if self.environment.ground_plane_id is not None:
cps = p.getClosestPoints(
self.robot_id,
self.environment.ground_plane_id,
distance=float(max(0.0, self.safety_margin)),
physicsClientId=self.physics_client,
)
for cp in cps:
if int(cp[3]) > 0: # 排除基座
has_collision = True
collision_pairs.append((int(cp[3]), self.environment.ground_plane_id, -1))
return has_collision, collision_pairs
finally:
p.restoreState(state_id, physicsClientId=self.physics_client)
def check_collision_batch(self, joint_configs: List[List[float]]) -> List[bool]:
"""批量检查配置是否碰撞（无副作用）"""
return [self.check_collision(cfg) for cfg in joint_configs]