diff --git a/CLAUDE.md b/CLAUDE.md index d5e78db..5a9c76d 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -385,4 +385,32 @@ self.arm_controller.set_joint_positions(saved_joint_positions) **主要修复**（2025-09-14）： - 修复回放时路径不显示问题 - 新增 `get_planned_path()` 方法 -- 改进回放时路径和标记显示 \ No newline at end of file +- 改进回放时路径和标记显示 + +## 碰撞可视化功能 ✅（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`（添加碰撞可视化和改进执行逻辑） \ No newline at end of file diff --git a/src/gui/main_window.py b/src/gui/main_window.py index 4671804..a1108c7 100644 --- a/src/gui/main_window.py +++ b/src/gui/main_window.py @@ -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'): diff --git a/src/planning/collision_checker.py b/src/planning/collision_checker.py index 2c710b8..05976c8 100644 --- a/src/planning/collision_checker.py +++ b/src/planning/collision_checker.py @@ -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]