修复：固定底座、无副作用碰撞检测、通用避障与执行收敛等待
This commit is contained in:
parent
e04c0a9ca5
commit
e617a7c28e
@ -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,
|
||||
@ -134,4 +135,4 @@
|
||||
"position_tolerance": 0.01
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
self.update_status("Planning path through hole...")
|
||||
raw_path = self.path_planner.plan_through_hole(config_a, config_b)
|
||||
# Plan path (通用避障；若配置要求强制穿洞，内部会选择穿洞策略)
|
||||
self.update_status("Planning path...")
|
||||
raw_path = self.path_planner.plan_auto(config_a, config_b)
|
||||
|
||||
# Optimize path
|
||||
self.update_status("Optimizing path...")
|
||||
@ -480,7 +484,7 @@ class MainWindow:
|
||||
|
||||
# Visualize path (optional)
|
||||
self._visualize_path(self.planned_path)
|
||||
|
||||
|
||||
except Exception as e:
|
||||
import traceback
|
||||
self.update_status(f"Path planning failed: {str(e)}")
|
||||
@ -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}")
|
||||
@ -615,4 +618,4 @@ class MainWindow:
|
||||
if __name__ == "__main__":
|
||||
# Test the main window directly
|
||||
app = MainWindow()
|
||||
app.run()
|
||||
app.run()
|
||||
|
||||
@ -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)
|
||||
@ -409,4 +411,10 @@ class AIRRTStarPlanner:
|
||||
# 恢复原始偏向率
|
||||
self.hole_strategy.hole_bias_rate = original_rate
|
||||
|
||||
return path
|
||||
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)
|
||||
|
||||
@ -2,230 +2,160 @@
|
||||
"""
|
||||
Collision Checker Module
|
||||
|
||||
碰撞检测封装模块，提供高效的碰撞检测接口。
|
||||
封装PyBullet碰撞检测功能，支持批量检测和缓存优化。
|
||||
|
||||
所有参数从config.json读取，无硬编码。
|
||||
路径规划阶段的碰撞检测工具。
|
||||
核心目标：任何碰撞检查都不改变真实仿真世界（无副作用）。
|
||||
做到这一点的方法是：在 save/restore 的状态下用 resetJointState 设置“试配置”，
|
||||
只触发碰撞检测（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()
|
||||
collision_config = config['path_planning']['collision']
|
||||
self.check_resolution = collision_config['check_resolution']
|
||||
self.safety_margin = collision_config['safety_margin']
|
||||
|
||||
# 获取机器人和环境信息
|
||||
|
||||
# 配置
|
||||
cfg = config_loader.get_full_config()
|
||||
c_cfg = cfg['path_planning']['collision']
|
||||
self.check_resolution = c_cfg['check_resolution']
|
||||
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 # 无效配置视为碰撞
|
||||
|
||||
# 保存当前配置
|
||||
original_config = self.arm_controller.get_current_joint_positions()
|
||||
|
||||
# 设置机器人到指定配置
|
||||
self.robot_loader.set_joint_positions(joint_config)
|
||||
|
||||
# 执行一步仿真以更新碰撞信息
|
||||
p.stepSimulation(physicsClientId=self.physics_client)
|
||||
|
||||
# 检测自碰撞
|
||||
has_self_collision = self.robot_loader.check_self_collision()
|
||||
|
||||
# 检测与环境碰撞
|
||||
has_env_collision = False
|
||||
|
||||
# 检测与墙体的碰撞
|
||||
if self.environment.wall_id is not None:
|
||||
contact_points = p.getContactPoints(
|
||||
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
|
||||
|
||||
return True
|
||||
|
||||
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
|
||||
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
|
||||
|
||||
# 自碰撞（严格接触）
|
||||
if len(p.getContactPoints(self.robot_id, self.robot_id, physicsClientId=self.physics_client)) > 0:
|
||||
return True
|
||||
|
||||
# 环境碰撞（含安全边界，使用最近点）
|
||||
# 与墙体各分段/其他障碍
|
||||
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
|
||||
|
||||
# 与地面（排除基座）
|
||||
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
|
||||
finally:
|
||||
p.restoreState(state_id, physicsClientId=self.physics_client)
|
||||
|
||||
def check_collision_batch(self, joint_configs: List[List[float]]) -> List[bool]:
|
||||
"""
|
||||
批量检测多个关节配置的碰撞状态
|
||||
|
||||
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
|
||||
|
||||
"""批量检查配置是否碰撞（无副作用）"""
|
||||
return [self.check_collision(cfg) for cfg in joint_configs]
|
||||
|
||||
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)
|
||||
|
||||
if self.check_collision(interpolated.tolist()):
|
||||
# 恢复原始配置
|
||||
self.robot_loader.set_joint_positions(original_config)
|
||||
interpolated = (start + t * (end - start)).tolist()
|
||||
if self.check_collision(interpolated):
|
||||
return True
|
||||
|
||||
# 恢复原始配置
|
||||
self.robot_loader.set_joint_positions(original_config)
|
||||
return False
|
||||
|
||||
|
||||
def get_obstacle_distance(self, joint_config: List[float]) -> float:
|
||||
"""
|
||||
获取机械臂到最近障碍物的距离
|
||||
|
||||
Args:
|
||||
joint_config: 关节配置
|
||||
|
||||
Returns:
|
||||
最小距离值
|
||||
"""
|
||||
# 保存原始配置
|
||||
original_config = self.arm_controller.get_current_joint_positions()
|
||||
|
||||
# 设置机器人配置
|
||||
self.robot_loader.set_joint_positions(joint_config)
|
||||
p.stepSimulation(physicsClientId=self.physics_client)
|
||||
|
||||
min_distance = float('inf')
|
||||
|
||||
# 计算到墙体的最近距离
|
||||
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
|
||||
|
||||
"""估算到墙体的最近距离（米）"""
|
||||
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
|
||||
|
||||
min_distance = float('inf')
|
||||
for obs_id in self._iter_obstacles():
|
||||
cps = p.getClosestPoints(
|
||||
self.robot_id,
|
||||
int(obs_id),
|
||||
distance=1.0,
|
||||
physicsClientId=self.physics_client,
|
||||
)
|
||||
for cp in cps:
|
||||
d = float(cp[8])
|
||||
if d < min_distance:
|
||||
min_distance = d
|
||||
return min_distance
|
||||
finally:
|
||||
p.restoreState(state_id, physicsClientId=self.physics_client)
|
||||
|
||||
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, ""
|
||||
return True, ""
|
||||
|
||||
@ -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(
|
||||
np.array(target_config) - np.array(final_config)
|
||||
)
|
||||
|
||||
if final_distance > self.position_tolerance:
|
||||
raise RuntimeError(f"Failed to reach target, error: {final_distance}")
|
||||
|
||||
return True
|
||||
final_distance = np.linalg.norm(np.array(target_config) - np.array(final_config))
|
||||
raise RuntimeError(f"Failed to reach target, error: {final_distance}")
|
||||
|
||||
def _close_gripper(self):
|
||||
"""关闭夹爪"""
|
||||
@ -188,4 +193,4 @@ class PathExecutor:
|
||||
self._open_gripper()
|
||||
time.sleep(self.timestep * 10) # 等待夹爪打开
|
||||
|
||||
return True
|
||||
return True
|
||||
|
||||
@ -141,6 +141,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:
|
||||
@ -536,4 +543,4 @@ class ArmController:
|
||||
|
||||
def create_arm_controller(config_loader: ConfigLoader, physics_client: int) -> ArmController:
|
||||
"""Create ArmController instance from configuration"""
|
||||
return ArmController(config_loader, physics_client)
|
||||
return ArmController(config_loader, physics_client)
|
||||
|
||||
@ -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
|
||||
)
|
||||
|
||||
@ -320,4 +321,4 @@ class RobotLoader:
|
||||
current_positions = [state['position'] for state in current_states.values()]
|
||||
|
||||
# Set position control with current positions as targets
|
||||
self.set_joint_positions(current_positions)
|
||||
self.set_joint_positions(current_positions)
|
||||
|
||||
@ -328,12 +328,15 @@ class Environment:
|
||||
wall_pos = self.wall_config['position']
|
||||
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 {
|
||||
@ -430,4 +433,4 @@ class Environment:
|
||||
try:
|
||||
p.removeBody(obj_id, physicsClientId=self.physics_client)
|
||||
except:
|
||||
pass # Object might already be removed
|
||||
pass # Object might already be removed
|
||||
|
||||
Loading…
Reference in New Issue
Block a user