修复：固定底座、无副作用碰撞检测、通用避障与执行收敛等待

config.json

@@ -119,7 +119,8 @@
       "step_size": 0.1,
       "goal_sample_rate": 0.15,
       "search_radius": 0.5,
-      "hole_bias_rate": 0.3
+      "hole_bias_rate": 0.3,
+      "enforce_hole_crossing": false
     },
     "collision": {
       "check_resolution": 0.05,

src/gui/main_window.py

@@ -443,6 +443,10 @@ class MainWindow:
     def plan_path(self):
         """Plan path from A to B through hole using AI RRT*"""
         self.update_status("Starting AI RRT* path planning...")
+        # 暂停仿真线程，避免规划期间并发 step 引入副作用
+        was_running = self.simulation_running
+        if was_running:
+            self.pause_simulation()
         
         try:
             # Get task points
@@ -463,9 +467,9 @@ class MainWindow:
             if config_b is None:
                 raise RuntimeError("Cannot find joint configuration for point B")
             
-            # Plan path through hole
+            # Plan path (通用避障；若配置要求强制穿洞，内部会选择穿洞策略)
-            self.update_status("Planning path through hole...")
+            self.update_status("Planning path...")
-            raw_path = self.path_planner.plan_through_hole(config_a, config_b)
+            raw_path = self.path_planner.plan_auto(config_a, config_b)
             
             # Optimize path
             self.update_status("Optimizing path...")
@@ -488,6 +492,10 @@ class MainWindow:
             traceback.print_exc()
             self.planned_path = None
             self.execute_btn.config(state=tk.DISABLED)
+        finally:
+            # 规划完成后恢复仿真运行状态
+            if was_running:
+                self.start_simulation()
     
     def execute_path(self):
         """Execute the planned path"""
@@ -522,13 +530,9 @@ class MainWindow:
             
             self.path_visualization = []
             
-            # Draw path as lines between waypoints
+            # Draw path as lines between waypoints（仅用 KDL FK 计算，不改仿真状态）
             for i in range(len(path) - 1):
-                # Set robot to configuration to get end-effector position
-                self.arm_controller.set_joint_positions(path[i])
                 start_pos, _ = self.arm_controller.forward_kinematics(path[i])
-                
-                self.arm_controller.set_joint_positions(path[i + 1])
                 end_pos, _ = self.arm_controller.forward_kinematics(path[i + 1])
                 
                 # Draw line
@@ -540,8 +544,7 @@ class MainWindow:
                 )
                 self.path_visualization.append(line_id)
             
-            # Restore original configuration
+            # 可选：不恢复任何机器人状态（可视化不改状态）
-            self.arm_controller.set_joint_positions(path[0])
             
         except Exception as e:
             self.update_status(f"Path visualization failed: {e}")

src/planning/ai_rrt_star.py

@@ -106,6 +106,8 @@ class AIRRTStarPlanner:
         # 初始化参数
         self.max_iterations = self.rrt_config['max_iterations']
         self.params = AdaptiveParameters(self.rrt_config)
+        # 是否强制穿洞（默认否）
+        self.enforce_hole = self.rrt_config.get('enforce_hole_crossing', False)
         
         # 初始化组件
         self.collision_checker = CollisionChecker(arm_controller, environment, config_loader)
@@ -410,3 +412,9 @@ class AIRRTStarPlanner:
             self.hole_strategy.hole_bias_rate = original_rate
         
         return path
+
+    def plan_auto(self, start_config: List[float], goal_config: List[float]) -> List[List[float]]:
+        """根据配置选择是否强制穿洞；默认进行通用避障路径规划。"""
+        if self.enforce_hole:
+            return self.plan_through_hole(start_config, goal_config)
+        return self.plan(start_config, goal_config)

src/planning/collision_checker.py

@@ -2,230 +2,160 @@
 """
 Collision Checker Module
 
-碰撞检测封装模块，提供高效的碰撞检测接口。
+路径规划阶段的碰撞检测工具。
-封装PyBullet碰撞检测功能，支持批量检测和缓存优化。
+核心目标：任何碰撞检查都不改变真实仿真世界（无副作用）。
-
+做到这一点的方法是：在 save/restore 的状态下用 resetJointState 设置“试配置”，
-所有参数从config.json读取，无硬编码。
+只触发碰撞检测（performCollisionDetection），而不 stepSimulation。
 """
 
 import numpy as np
-from typing import List, Tuple, Dict, Any, Optional
+from typing import List, Tuple
 import pybullet as p
 
 
 class CollisionChecker:
-    """碰撞检测器"""
+    """碰撞检测器（规划期无副作用）"""
 
     def __init__(self, arm_controller, environment, config_loader):
-        """
+        # 依赖
-        初始化碰撞检测器
-        
-        Args:
-            arm_controller: 机械臂控制器
-            environment: 环境管理器
-            config_loader: 配置加载器
-        """
         self.arm_controller = arm_controller
         self.environment = environment
         self.config_loader = config_loader
 
-        # 从配置读取碰撞检测参数
+        # 配置
-        config = config_loader.get_full_config()
+        cfg = config_loader.get_full_config()
-        collision_config = config['path_planning']['collision']
+        c_cfg = cfg['path_planning']['collision']
-        self.check_resolution = collision_config['check_resolution']
+        self.check_resolution = c_cfg['check_resolution']
-        self.safety_margin = collision_config['safety_margin']
+        self.safety_margin = c_cfg['safety_margin']
 
-        # 获取机器人和环境信息
+        # 资源句柄
         self.robot_loader = arm_controller.robot_loader
         self.robot_id = self.robot_loader.get_robot_id()
         self.physics_client = self.robot_loader.physics_client
 
+    def _iter_obstacles(self):
+        """遍历需要考虑的环境障碍物 body ids（墙体各分段、可选 transport 物体）。"""
+        # 墙体可能由多个 body 组成
+        wall_parts = getattr(self.environment, '_wall_part_ids', []) or []
+        for wid in wall_parts:
+            if wid is not None:
+                yield wid
+        # 可选地加入运输物体（若要把它也视为障碍）
+        if getattr(self.environment, 'transport_object_id', None) is not None:
+            yield self.environment.transport_object_id
+
     def check_collision(self, joint_config: List[float]) -> bool:
-        """
+        """检测给定关节配置是否发生碰撞（含安全边界）"""
-        检测给定关节配置是否发生碰撞
+        # 基本有效性
-        
-        Args:
-            joint_config: 关节配置
-            
-        Returns:
-            True if collision detected, False otherwise
-        """
-        # 验证关节配置
         if not self.arm_controller.validate_joint_configuration(joint_config):
-            return True  # 无效配置视为碰撞
+            return True
 
-        # 保存当前配置
+        state_id = p.saveState(physicsClientId=self.physics_client)
-        original_config = self.arm_controller.get_current_joint_positions()
+        try:
+            # 将机器人打到“试配置”（仅状态，不经电机/动力学）
+            joint_indices = self.robot_loader.joint_indices
+            if len(joint_config) != len(joint_indices):
+                return True
+            for i, jid in enumerate(joint_indices):
+                p.resetJointState(self.robot_id, jid, float(joint_config[i]), physicsClientId=self.physics_client)
 
-        # 设置机器人到指定配置
+            # 触发碰撞检测（不推进世界时间）
-        self.robot_loader.set_joint_positions(joint_config)
+            try:
+                p.performCollisionDetection(physicsClientId=self.physics_client)
+            except Exception:
+                pass
 
-        # 执行一步仿真以更新碰撞信息
+            # 自碰撞（严格接触）
-        p.stepSimulation(physicsClientId=self.physics_client)
+            if len(p.getContactPoints(self.robot_id, self.robot_id, physicsClientId=self.physics_client)) > 0:
+                return True
 
-        # 检测自碰撞
+            # 环境碰撞（含安全边界，使用最近点）
-        has_self_collision = self.robot_loader.check_self_collision()
+            # 与墙体各分段/其他障碍
+            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 len(cps) > 0:
+                    return True
 
-        # 检测与环境碰撞
+            # 与地面（排除基座）
-        has_env_collision = False
+            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:
+                    # cp[3]: link index on robot
+                    if int(cp[3]) > 0:
+                        return True
 
-        # 检测与墙体的碰撞
+            return False
-        if self.environment.wall_id is not None:
+        finally:
-            contact_points = p.getContactPoints(
+            p.restoreState(state_id, physicsClientId=self.physics_client)
-                bodyA=self.robot_id,
-                bodyB=self.environment.wall_id,
-                physicsClientId=self.physics_client
-            )
-            if len(contact_points) > 0:
-                # 检查接触距离是否超过安全边界
-                for contact in contact_points:
-                    contact_distance = contact[8]  # Contact distance
-                    if contact_distance < -self.safety_margin:
-                        has_env_collision = True
-                        break
-        
-        # 检测与地面的碰撞（排除基座）
-        if self.environment.ground_plane_id is not None:
-            contact_points = p.getContactPoints(
-                bodyA=self.robot_id,
-                bodyB=self.environment.ground_plane_id,
-                physicsClientId=self.physics_client
-            )
-            # 过滤掉基座的接触点
-            for contact in contact_points:
-                link_index = contact[3]  # Link index on robot
-                if link_index > 0:  # 非基座链接
-                    contact_distance = contact[8]
-                    if contact_distance < -self.safety_margin:
-                        has_env_collision = True
-                        break
-        
-        # 恢复原始配置
-        self.robot_loader.set_joint_positions(original_config)
-        
-        return has_self_collision or has_env_collision
 
     def check_collision_batch(self, joint_configs: List[List[float]]) -> List[bool]:
-        """
+        """批量检查配置是否碰撞（无副作用）"""
-        批量检测多个关节配置的碰撞状态
+        return [self.check_collision(cfg) for cfg in joint_configs]
-        
-        Args:
-            joint_configs: 关节配置列表
-            
-        Returns:
-            碰撞检测结果列表
-        """
-        results = []
-        
-        # 保存原始配置
-        original_config = self.arm_controller.get_current_joint_positions()
-        
-        for config in joint_configs:
-            has_collision = self.check_collision(config)
-            results.append(has_collision)
-        
-        # 恢复原始配置
-        self.robot_loader.set_joint_positions(original_config)
-        
-        return results
 
     def check_path_collision(self, path: List[List[float]], resolution: float = None) -> bool:
-        """
+        """检测整条路径是否存在碰撞"""
-        检测整条路径是否存在碰撞
-        
-        Args:
-            path: 路径点列表
-            resolution: 检测分辨率
-            
-        Returns:
-            True if any collision detected along path
-        """
         if len(path) < 2:
             return False
-        
         if resolution is None:
             resolution = self.check_resolution
 
-        # 保存原始配置
-        original_config = self.arm_controller.get_current_joint_positions()
-        
-        # 检测路径上的每个段
         for i in range(len(path) - 1):
             start = np.array(path[i])
             end = np.array(path[i + 1])
-            
-            # 计算需要检测的中间点数量
             distance = np.linalg.norm(end - start)
             num_checks = max(2, int(distance / resolution))
-            
-            # 检测路径段上的点
             for j in range(num_checks):
                 t = j / (num_checks - 1)
-                interpolated = start + t * (end - start)
+                interpolated = (start + t * (end - start)).tolist()
-                
+                if self.check_collision(interpolated):
-                if self.check_collision(interpolated.tolist()):
-                    # 恢复原始配置
-                    self.robot_loader.set_joint_positions(original_config)
                     return True
-        
-        # 恢复原始配置
-        self.robot_loader.set_joint_positions(original_config)
         return False
 
     def get_obstacle_distance(self, joint_config: List[float]) -> float:
-        """
+        """估算到墙体的最近距离（米）"""
-        获取机械臂到最近障碍物的距离
+        state_id = p.saveState(physicsClientId=self.physics_client)
+        try:
+            joint_indices = self.robot_loader.joint_indices
+            if len(joint_config) != len(joint_indices):
+                return float('inf')
+            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
 
-        Args:
+            min_distance = float('inf')
-            joint_config: 关节配置
+            for obs_id in self._iter_obstacles():
-            
+                cps = p.getClosestPoints(
-        Returns:
+                    self.robot_id,
-            最小距离值
+                    int(obs_id),
-        """
+                    distance=1.0,
-        # 保存原始配置
+                    physicsClientId=self.physics_client,
-        original_config = self.arm_controller.get_current_joint_positions()
+                )
-        
+                for cp in cps:
-        # 设置机器人配置
+                    d = float(cp[8])
-        self.robot_loader.set_joint_positions(joint_config)
+                    if d < min_distance:
-        p.stepSimulation(physicsClientId=self.physics_client)
+                        min_distance = d
-        
+            return min_distance
-        min_distance = float('inf')
+        finally:
-        
+            p.restoreState(state_id, physicsClientId=self.physics_client)
-        # 计算到墙体的最近距离
-        if self.environment.wall_id is not None:
-            closest_points = p.getClosestPoints(
-                bodyA=self.robot_id,
-                bodyB=self.environment.wall_id,
-                distance=1.0,  # Maximum search distance
-                physicsClientId=self.physics_client
-            )
-            
-            for point in closest_points:
-                distance = point[8]  # Distance between points
-                min_distance = min(min_distance, distance)
-        
-        # 恢复原始配置
-        self.robot_loader.set_joint_positions(original_config)
-        
-        return min_distance
 
     def validate_configuration(self, joint_config: List[float]) -> Tuple[bool, str]:
-        """
+        """验证配置是否在限位内且无碰撞"""
-        验证关节配置的有效性和安全性
-        
-        Args:
-            joint_config: 关节配置
-            
-        Returns:
-            (是否有效, 错误信息)
-        """
-        # 检查关节限位
         is_valid, violations = self.arm_controller.check_joint_limits(joint_config)
         if not is_valid:
             return False, f"Joint limit violations: {', '.join(violations)}"
-        
-        # 检查碰撞
         if self.check_collision(joint_config):
             return False, "Collision detected"
-        
         return True, ""

src/planning/path_executor.py

@@ -118,19 +118,24 @@ class PathExecutor:
             # 等待一个时间步
             time.sleep(self.timestep)
         
-        # 确保到达精确位置
+        # 设定目标末点，并等待收敛到容差内（超时则失败，暴露问题）
         self.arm_controller.set_joint_positions(target_config)
 
-        # 验证是否到达
+        wait_start = time.time()
+        max_wait = max(self.timestep, move_time)
+        while time.time() - wait_start < max_wait:
+            final_config = self.arm_controller.get_current_joint_positions()
+            final_distance = np.linalg.norm(
+                np.array(target_config) - np.array(final_config)
+            )
+            if final_distance <= self.position_tolerance:
+                return True
+            time.sleep(self.timestep)
+
+        # 超时仍未到达，抛错暴露问题
         final_config = self.arm_controller.get_current_joint_positions()
-        final_distance = np.linalg.norm(
+        final_distance = np.linalg.norm(np.array(target_config) - np.array(final_config))
-            np.array(target_config) - np.array(final_config)
+        raise RuntimeError(f"Failed to reach target, error: {final_distance}")
-        )
-        
-        if final_distance > self.position_tolerance:
-            raise RuntimeError(f"Failed to reach target, error: {final_distance}")
-        
-        return True
     
     def _close_gripper(self):
         """关闭夹爪"""

src/robot/arm_controller.py

@@ -142,6 +142,13 @@ class ArmController:
         # Use KDL engine for primary validation (includes joint limits from URDF)
         return self.kinematics_engine.validate_joint_configuration(joint_positions)
 
+    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(joint_positions)
+    
     def move_to_joint_positions(self, joint_positions: List[float], 
                                timeout: float = 5.0) -> bool:
         """Move robot to specified joint positions"""

src/robot/robot_loader.py

@@ -42,11 +42,12 @@ class RobotLoader:
         # Convert RPY to quaternion for pybullet
         base_orientation_quat = p.getQuaternionFromEuler(self.base_orientation)
         
-        # Load URDF model
+        # Load URDF model (fixed base avoids floating/"flying" during planning)
         self.robot_id = p.loadURDF(
             model_path,
             basePosition=self.base_position,
             baseOrientation=base_orientation_quat,
+            useFixedBase=True,
             physicsClientId=self.physics_client
         )
         

src/simulation/environment.py

@@ -329,11 +329,14 @@ class Environment:
         hole_pos_rel = self.hole_config['position_relative_to_wall']
         hole_dims = self.hole_config['dimensions']
 
-        # Calculate absolute hole position
+        # Align with create_wall_with_hole(): hole center sits at wall mid-height by default
+        wall_height = float(self.wall_config['dimensions']['height'])
+
+        # Calculate absolute hole position (center)
         hole_center = [
             wall_pos[0] + hole_pos_rel[0],
             wall_pos[1] + hole_pos_rel[1],
-            wall_pos[2] + hole_pos_rel[2]
+            wall_pos[2] + wall_height / 2.0 + hole_pos_rel[2]
         ]
         
         return {