bismih520/daohang
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

release: v0.1 initial version

Browse Source
This commit is contained in:
lovez
2026-02-21 10:33:09 +08:00
parent 32a6c49bbe
commit 175ea87fed
2 changed files with 936 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files

554
MJDATA.TXT Normal file
View File

@@ -0,0 +1,554 @@
SIZES
narena 13631488
nbuffer 13952
nplugin 0
maxuse_stack 2388
maxuse_arena 29552
maxuse_con 1
maxuse_efc 4
solver_nisland 1
ne 0
nf 0
nl 0
nefc 0
nnzJ 0
ncon 0
nisland 0
threadpool 0
TIMER
0: duration = 0.0029 number = 1
1: duration = 0.0055 number = 2
2: duration = 0 number = 0
3: duration = 0.0039 number = 2
4: duration = 0.0009 number = 2
5: duration = 0.00017 number = 2
6: duration = 6e-05 number = 2
7: duration = 8e-05 number = 1
8: duration = 0.001 number = 2
9: duration = 0.00032 number = 2
10: duration = 0.0018 number = 2
11: duration = 0.00029 number = 2
12: duration = 6e-05 number = 2
13: duration = 0.0014 number = 2
14: duration = 0.00031 number = 2
ENERGY = 0 0
TIME = 3.7e+04
QPOS
0.6
0.51
-27
-99
-99
-7.3e+02
1
0
0
0
QVEL
-1.1e-322
-1.1e-322
-2e-323
0
0
-3.7e+05
0
0
0
QACC_WARMSTART
1.2e-321
1.2e-321
8.8e-322
0
0
-9.8
0
0
0
CTRL
0
0
0
QFRC_APPLIED
0
0
0
0
0
0
0
0
0
XFRC_APPLIED
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
QACC
1.2e-321
1.2e-321
8.8e-322
0
0
-9.8
0
0
0
XPOS
0 0 0
0.6 0.51 0.35
-99 -99 0.1
1.5 0 0.25
-1.2 1 0.2
0.8 -1 0.15
-0.5 -0.8 0.2
1 0.8 0.25
XQUAT
1 0 0 0
0.74 0 0 -0.67
1 0 0 0
1 0 0 0
1 0 0 0
1 0 0 0
1 0 0 0
1 0 0 0
XMAT
1 0 0 0 1 0 0 0 1
0.096 1 0 -1 0.096 -0 -0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
XIPOS
0 0 0
0.61 0.48 0.36
-99 -99 0.1
1.5 0 0.25
-1.2 1 0.2
0.8 -1 0.15
-0.5 -0.8 0.2
1 0.8 0.25
XIMAT
1 0 0 0 1 0 0 0 1
0.024 -1 0.049 -0.021 -0.05 -1 1 0.023 -0.022
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
XANCHOR
0 0 0.35
0.6 0 0.35
0.6 0.51 0.35
-99 -99 0.1
XAXIS
1 0 0
0 1 0
0 0 1
0 0 1
GEOM_XPOS
0 0 0
0.6 0.51 0.35
0.62 0.32 0.35
0.73 0.52 0.35
0.6 0.51 0.45
-99 -99 0.1
1.5 0 0.25
-1.2 1 0.2
0.8 -1 0.15
-0.5 -0.8 0.2
1 0.8 0.25
GEOM_XMAT
1 0 0 0 1 0 0 0 1
0.096 1 0 -1 0.096 -0 -0 0 1
-2.8e-17 1 -0.096 -5.6e-17 0.096 1 1 5.6e-17 5.6e-17
0.096 5.6e-17 -1 -1 -5.6e-17 -0.096 -5.6e-17 1 5.6e-17
0.096 -1 -1.2e-16 -1 -0.096 -1.2e-17 0 1.2e-16 -1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
SITE_XPOS
0.63 0.26 0.35
SITE_XMAT
0.096 1 0 -1 0.096 -0 -0 0 1
CAM_XPOS
0 0 5
CAM_XMAT
1 0 0 0 1 0 0 0 1
SUBTREE_COM
-1.4 -2.1 0.23
0.61 0.48 0.36
-99 -99 0.1
1.5 0 0.25
-1.2 1 0.2
0.8 -1 0.15
-0.5 -0.8 0.2
1 0.8 0.25
CDOF
0 0 0 1 0 0
0 0 0 0 1 0
0 0 1 0.037 0.012 -0
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
CINERT
0 0 0 0 0 0 0 0 0 0
0.15 0.1 0.17 0.0025 0.00047 -0.0014 0 0 0 7.3
0.042 0.042 0.042 0 0 0 0 0 0 7.2
9.2 9.2 11 0 0 0 0 0 0 1.8e+02
0.54 0.54 0.32 0 0 0 0 0 0 28
0.13 0.13 0.13 0 0 0 0 0 -3.9e-16 14
3.4 3.4 4.2 0 0 0 0 0 0 1e+02
0.55 0.55 0.16 0 0 0 0 0 0 23
ACTUATOR_LENGTH
0.6
0.51
-27
ACTUATOR_MOMENT
1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0
CRB
0 0 0 0 0 0 0 0 0 0
0.15 0.1 0.17 0.0025 0.00047 -0.0014 0 0 0 7.3
0.042 0.042 0.042 0 0 0 0 0 0 7.2
9.2 9.2 11 0 0 0 0 0 0 1.8e+02
0.54 0.54 0.32 0 0 0 0 0 0 28
0.13 0.13 0.13 0 0 0 0 0 -3.9e-16 14
3.4 3.4 4.2 0 0 0 0 0 0 1e+02
0.55 0.55 0.16 0 0 0 0 0 0 23
QM
7.3 0 0.27 0 0 0 0 0 0
0 7.3 0.088 0 0 0 0 0 0
0.27 0.088 0.18 0 0 0 0 0 0
0 0 0 7.2 0 0 0 0 0
0 0 0 0 7.2 0 0 0 0
0 0 0 0 0 7.2 0 0 0
0 0 0 0 0 0 0.042 0 0
0 0 0 0 0 0 0 0.042 0
0 0 0 0 0 0 0 0 0.042
QLD
6.9 -0.018 1.5 0 0 0 0 0 0
-0.018 7.3 0.48 0 0 0 0 0 0
1.5 0.48 0.18 0 0 0 0 0 0
0 0 0 7.2 0 0 0 0 0
0 0 0 0 7.2 0 0 0 0
0 0 0 0 0 7.2 0 0 0
0 0 0 0 0 0 0.042 0 0
0 0 0 0 0 0 0 0.042 0
0 0 0 0 0 0 0 0 0.042
QLDIAGINV
0.14
0.14
5.5
0.14
0.14
0.14
24
24
24
QLDIAGSQRTINV
0.38
0.37
2.3
0.37
0.37
0.37
4.9
4.9
4.9
B_rownnz 9 3 6 0 0 0 0 0
B_rowadr 0 9 12 18 18 18 18 18
B_colind 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8
C_rownnz 3 3 3 1 1 1 1 1 1
C_rowadr 0 3 6 9 10 11 12 13 14
C_colind 0 1 2 0 1 2 0 1 2 3 4 5 6 7 8
mapM2C 0 2 5 2 1 4 5 4 3 6 7 9 12 16 21
D_rownnz 3 3 3 6 6 6 6 6 6
D_rowadr 0 3 6 9 15 21 27 33 39
D_colind 0 1 2 0 1 2 0 1 2 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8 3 4 5 6 7 8
mapM2D 0 2 5 2 1 4 5 4 3 6 8 11 15 20 26 8 7 10 14 19 25 11 10 9 13 18 24 15 14 13 12 17 23 20 19 18 17 16 22 26 25 24 23 22 21
mapD2M 0 4 3 8 7 6 9 16 15 23 22 21 30 29 28 27 37 36 35 34 33 44 43 42 41 40 39
QDERIV
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
QLU
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
CONTACT
ACTUATOR_VELOCITY
-1.1e-322
-1.1e-322
-2e-323
CVEL
0 0 0 0 0 0
0 0 -2e-323 -1.1e-322 -1.1e-322 0
0 0 0 0 0 -3.7e+05
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
CDOF_DOT
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 -1.1e-322 1.1e-322 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 -3.7e+05 0
0 0 0 3.7e+05 0 0
0 0 0 0 0 0
QFRC_BIAS
0
0
0
0
0
71
0
0
0
QFRC_SPRING
0
0
0
0
0
0
0
0
0
QFRC_DAMPER
0
0
0
0
0
0
0
0
0
QFRC_GRAVCOMP
0
0
0
0
0
0
0
0
0
QFRC_FLUID
0
0
0
0
0
0
0
0
0
QFRC_PASSIVE
0
0
0
0
0
0
0
0
0
SUBTREE_LINVEL
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
SUBTREE_ANGMOM
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
ACTUATOR_FORCE
9.1e-321
9.1e-321
5.9e-322
QFRC_ACTUATOR
9.1e-321
9.1e-321
5.9e-322
0
0
0
0
0
0
QFRC_SMOOTH
9.1e-321
9.1e-321
5.9e-322
0
0
-71
0
0
0
QACC_SMOOTH
1.2e-321
1.2e-321
8.8e-322
0
0
-9.8
0
0
0
QFRC_CONSTRAINT
0
0
0
0
0
0
0
0
0
QFRC_INVERSE
0
0
0
0
0
0
0
0
0
CACC
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
CFRC_INT
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
CFRC_EXT
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0

676
scripts/run_simulation.py
View File

@@ -1,197 +1,303 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
""" """
ROS1 + MuJoCo VLA 3D 导航 — 全向移动 + 前向扫描 + 避障 + 目标点导航 MuJoCo 全向小车导航（前向射线扫描 + 势场避障 + 目标点导航）
- vx, vy, wz 全向移动（车体系输出 -> 世界系执行器）
圆柱体小车: vx, vy, wz 全向移动 - mj_ray 扇形前向扫描
前向范围扫描: mj_ray 射线检测 - 避障：排斥 + 朝开阔方向绕行
避障: 基于距离的势场/向量场 - 卡住检测：连续位移太小 -> 强制脱困策略
""" """
from __future__ import annotations
import os
from dataclasses import dataclass
from typing import Optional, Tuple, List
import mujoco import mujoco
import mujoco.viewer import mujoco.viewer
import numpy as np import numpy as np
import os
SCENE_PATH = os.path.join(os.path.dirname(__file__), "..", "mujoco_scenes", "cylinder_obstacles.xml") SCENE_PATH = os.path.join(os.path.dirname(__file__), "..", "mujoco_scenes", "cylinder_obstacles.xml")
# ============ 可配置参数 ============
NUM_RAYS = 19 # 前向扫描射线数量（-90 ~ +90 deg） # ===================== 参数配置 =====================
RAY_ANGLE_SPAN = np.pi # 扫描角度跨度 (rad) @dataclass
OBSTACLE_THRESHOLD = 0.45 # 障碍判定距离 (m) class NavCfg:
SAFE_DISTANCE = 0.65 # 安全距离 # ray scan
GOAL_THRESHOLD = 0.2 # 到达目标判定距离 num_rays: int = 19
MAX_LINEAR = 0.6 # 最大线速度 ray_angle_span: float = np.pi # -span/2 ~ +span/2
MAX_ANGULAR = 0.8 # 最大角速度
K_ATTRACT = 1.0 # 目标吸引力 # distances
K_REPEL = 0.6 # 障碍排斥力 obstacle_threshold: float = 0.45
K_YAW_GOAL = 0.8 # 目标对准角速度增益 safe_distance: float = 0.65
MIN_LINEAR = 0.18 # 最小线速度，避免卡住 goal_threshold: float = 0.20
STUCK_THRESH = 0.03 # 位移阈值，低于此认为卡住 (m)
STUCK_STEPS = 100 # 连续卡住步数触发绕行 # limits
GOAL_POINTS = [] # 初始无目标 max_linear: float = 0.60
GOAL_AHEAD_DIST = 2.0 # G 键：目标 = 车头前方 N 米 max_angular: float = 0.80
GOAL_MAX_R = 5.0 # 目标点边界：距离原点不超过此值 (m) min_linear: float = 0.18 # 防止“只转不走”或输出趋零卡死
# gains
k_attract: float = 1.0
k_repel: float = 0.6
k_yaw_goal: float = 0.8
# stuck detect
stuck_thresh: float = 0.03
stuck_steps: int = 100
# goal set
goal_ahead_dist: float = 2.0
goal_max_r: float = 5.0
# output smoothing
ema_alpha: float = 0.70
max_dv: float = 0.18
max_dw: float = 0.20
max_dv_stuck: float = 0.25
max_dw_stuck: float = 0.28
def _clamp_goal(gx, gy): CFG = NavCfg()
"""目标点边界：限制在距离原点 GOAL_MAX_R 以内"""
r = np.sqrt(gx * gx + gy * gy) + 1e-8
if r > GOAL_MAX_R:
scale = GOAL_MAX_R / r
return (float(gx * scale), float(gy * scale))
return (float(gx), float(gy))
def get_agent_state(data, body_id): # ===================== 工具函数 =====================
"""获取小车位姿: x, y, yaw""" def clamp_goal(gx: float, gy: float, max_r: float) -> Tuple[float, float]:
r = float(np.hypot(gx, gy)) + 1e-9
if r > max_r:
s = max_r / r
return float(gx * s), float(gy * s)
return float(gx), float(gy)
def wrap_pi(a: float) -> float:
# 更稳定的 [-pi, pi]
return float(np.arctan2(np.sin(a), np.cos(a)))
def get_agent_state(data: mujoco.MjData) -> Tuple[float, float, float]:
# 假设 qpos: x, y, yaw 在 0,1,2
qpos = data.qpos qpos = data.qpos
x, y = qpos[0], qpos[1] return float(qpos[0]), float(qpos[1]), float(qpos[2])
yaw = qpos[2]
return x, y, yaw
def forward_ray_scan(model, data, body_id, site_id, num_rays, angle_span): def goal_from_camera(viewer_handle) -> Optional[Tuple[float, float]]:
"""相机视线与 z=0 平面交点（旋转相机后按 C）。"""
cam = viewer_handle.cam
lookat = np.array(cam.lookat, dtype=float)
dist = float(cam.distance)
az = np.deg2rad(float(cam.azimuth))
el = np.deg2rad(float(cam.elevation))
# MuJoCo viewer 的相机方向约定：这里延续你原实现
fx = np.cos(el) * np.sin(az)
fy = -np.cos(el) * np.cos(az)
fz = np.sin(el)
cam_pos = lookat - dist * np.array([fx, fy, fz], dtype=float)
if abs(fz) < 1e-8:
return None
t = -cam_pos[2] / fz
if t < 0:
return None
pt = cam_pos + t * np.array([fx, fy, fz], dtype=float)
return float(pt[0]), float(pt[1])
# ===================== 射线扫描（预计算优化） =====================
class RayScanner:
""" """
前向范围扫描: 在车体前方扇形区域内发射射线 预计算扇形扫描的 body-frame 方向（基于 forward=[1,0,0], left=[0,1,0]），
返回: (距离数组, 角度数组) 每帧用 body rotation (xmat) 转到 world，减少每帧 cos/sin + 临时数组。
""" """
site_xpos = data.site_xpos[site_id].copy()
xmat = np.array(data.xmat[body_id]).reshape(3, 3)
forward = xmat[:, 0] # 前向 (body +X)
angles = np.linspace(-angle_span / 2, angle_span / 2, num_rays) def __init__(self, num_rays: int, angle_span: float):
distances = np.full(num_rays, 10.0) self.num_rays = int(num_rays)
self.angle_span = float(angle_span)
for i, theta in enumerate(angles): self.angles = np.linspace(-self.angle_span / 2, self.angle_span / 2, self.num_rays)
c, s = np.cos(theta), np.sin(theta) c = np.cos(self.angles)
ray_dir = c * forward + s * xmat[:, 1] # 绕 body Z 旋转 s = np.sin(self.angles)
ray_dir = ray_dir / (np.linalg.norm(ray_dir) + 1e-8)
geomid = np.array([-1], dtype=np.int32) # body-frame ray directions: dir_b = c*[1,0,0] + s*[0,1,0]
d = mujoco.mj_ray(model, data, site_xpos, ray_dir, None, 1, body_id, geomid) # shape (N,3)
if d >= 0: self.ray_dirs_body = np.stack([c, s, np.zeros_like(c)], axis=1).astype(np.float64)
distances[i] = d
return distances, angles # buffers to avoid per-step alloc
self.distances = np.full(self.num_rays, 10.0, dtype=np.float64)
self._geomid = np.array([-1], dtype=np.int32)
def scan(
self,
model: mujoco.MjModel,
data: mujoco.MjData,
body_id: int,
site_id: int,
) -> Tuple[np.ndarray, np.ndarray]:
site_xpos = data.site_xpos[site_id] # view, no copy
xmat = np.array(data.xmat[body_id], dtype=np.float64).reshape(3, 3)
# world dirs: dir_w = xmat @ dir_b
# shape (N,3)
ray_dirs_world = (xmat @ self.ray_dirs_body.T).T
# normalize (理论上已单位，但数值上更稳一点)
norms = np.linalg.norm(ray_dirs_world, axis=1) + 1e-9
ray_dirs_world /= norms[:, None]
self.distances.fill(10.0)
for i in range(self.num_rays):
d = mujoco.mj_ray(
model, data,
site_xpos, ray_dirs_world[i],
None, 1, body_id, self._geomid
)
if d >= 0:
self.distances[i] = float(d)
return self.distances, self.angles
def obstacle_avoidance_vel(distances, angles): # ===================== 速度融合与滤波 =====================
""" class VelocityFilter:
避障速度 + 绕行分量。被挡时朝开阔方向加速，而非只排斥 """EMA + 速率限制，输出更平滑，避免 ctrl 抖动。"""
返回: (vx_avoid, vy_avoid, wz_avoid)
"""
min_d = np.min(distances)
if min_d > OBSTACLE_THRESHOLD:
return 0.0, 0.0, 0.0
vx, vy, wz = 0.0, 0.0, 0.0 def __init__(self, alpha: float, max_dv: float, max_dw: float):
safe_sector = np.argmax(distances) self.alpha = float(alpha)
best_theta = angles[safe_sector] self.max_dv = float(max_dv)
self.max_dw = float(max_dw)
self.vx = 0.0
self.vy = 0.0
self.wz = 0.0
for d, theta in zip(distances, angles): def reset(self):
if d < OBSTACLE_THRESHOLD: self.vx = self.vy = self.wz = 0.0
ratio = 1.0 - d / OBSTACLE_THRESHOLD
strength = K_REPEL * (ratio ** 2)
vx -= strength * np.cos(theta)
vy -= strength * np.sin(theta)
elif d < SAFE_DISTANCE:
ratio = (SAFE_DISTANCE - d) / (SAFE_DISTANCE - OBSTACLE_THRESHOLD)
strength = K_REPEL * 0.15 * ratio
wz += strength * np.clip(best_theta - theta, -1, 1)
# 绕行：朝最开阔方向加正向速度，避免只退不绕 def update(self, vx_cmd: float, vy_cmd: float, wz_cmd: float) -> Tuple[float, float, float]:
vx_f = self.alpha * self.vx + (1.0 - self.alpha) * vx_cmd
vy_f = self.alpha * self.vy + (1.0 - self.alpha) * vy_cmd
wz_f = self.alpha * self.wz + (1.0 - self.alpha) * wz_cmd
dvx = float(np.clip(vx_f - self.vx, -self.max_dv, self.max_dv))
dvy = float(np.clip(vy_f - self.vy, -self.max_dv, self.max_dv))
dwz = float(np.clip(wz_f - self.wz, -self.max_dw, self.max_dw))
self.vx += dvx
self.vy += dvy
self.wz += dwz
return self.vx, self.vy, self.wz
def goal_attraction_vel(x: float, y: float, yaw: float, goal_x: float, goal_y: float, cfg: NavCfg):
dx = goal_x - x
dy = goal_y - y
dist = float(np.hypot(dx, dy)) + 1e-6
# 世界系单位方向
gx_w = dx / dist
gy_w = dy / dist
# 转车体系（只关心 yaw）
c = np.cos(-yaw)
s = np.sin(-yaw)
gx_b = c * gx_w - s * gy_w
gy_b = s * gx_w + c * gy_w
# 线速度（随距离平滑）
scale = float(np.tanh(dist) * 0.8 + 0.2)
vx = cfg.k_attract * scale * gx_b
vy = cfg.k_attract * scale * gy_b
# 角速度（弱对准）
target_yaw = float(np.arctan2(dy, dx))
yaw_err = wrap_pi(target_yaw - yaw)
wz = cfg.k_yaw_goal * float(np.tanh(yaw_err))
return vx, vy, wz
def obstacle_avoidance_vel(distances: np.ndarray, angles: np.ndarray, cfg: NavCfg):
min_d = float(np.min(distances))
if min_d > cfg.obstacle_threshold:
return 0.0, 0.0, 0.0, min_d
# 最开阔方向
safe_i = int(np.argmax(distances))
best_theta = float(angles[safe_i])
vx = 0.0
vy = 0.0
wz = 0.0
# 排斥 + 轻微“扭转”朝开阔方向
# （这里保持你原逻辑，但数值上更稳一点）
for d, th in zip(distances, angles):
d = float(d)
th = float(th)
if d < cfg.obstacle_threshold:
ratio = 1.0 - d / cfg.obstacle_threshold
strength = cfg.k_repel * (ratio * ratio)
vx -= strength * np.cos(th)
vy -= strength * np.sin(th)
elif d < cfg.safe_distance:
ratio = (cfg.safe_distance - d) / (cfg.safe_distance - cfg.obstacle_threshold)
strength = cfg.k_repel * 0.15 * ratio
wz += strength * float(np.clip(best_theta - th, -1.0, 1.0))
# 绕行：朝最开阔方向“推一把”
bypass = 0.35 bypass = 0.35
vx += bypass * np.cos(best_theta) vx += bypass * np.cos(best_theta)
vy += bypass * np.sin(best_theta) vy += bypass * np.sin(best_theta)
wz += 0.4 * best_theta wz += 0.4 * best_theta
return vx, vy, wz return vx, vy, wz, min_d
def goal_attraction_vel(x, y, yaw, goal_x, goal_y): def blend_and_clamp(
""" vx_a: float, vy_a: float, wz_a: float,
目标吸引力: 在车体坐标系下计算朝目标的 vx, vy, wz vx_g: float, vy_g: float, wz_g: float,
优先保持线速度，角速度仅用于微调朝向 dist_to_goal: float,
""" stuck: bool,
dx = goal_x - x cfg: NavCfg
dy = goal_y - y ):
dist = np.sqrt(dx * dx + dy * dy) + 1e-6
# 世界系下期望方向
gx_w = dx / dist
gy_w = dy / dist
# 转到车体系
c, s = np.cos(-yaw), np.sin(-yaw)
gx_b = c * gx_w - s * gy_w
gy_b = s * gx_w + c * gy_w
# 线速度：朝目标，随距离平滑
scale = np.tanh(dist) * 0.8 + 0.2
vx = K_ATTRACT * scale * gx_b
vy = K_ATTRACT * scale * gy_b
# 角速度：弱增益，避免只转不走
target_yaw = np.arctan2(dy, dx)
yaw_err = np.arctan2(np.sin(target_yaw - yaw), np.cos(target_yaw - yaw))
wz = K_YAW_GOAL * np.tanh(yaw_err)
return vx, vy, wz
def blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, min_d, stuck):
"""融合避障与目标速度，限幅，卡住时强制最小速度"""
vx = vx_a + vx_g vx = vx_a + vx_g
vy = vy_a + vy_g vy = vy_a + vy_g
wz = wz_a + wz_g wz = wz_a + wz_g
lin = np.sqrt(vx * vx + vy * vy) lin = float(np.hypot(vx, vy))
# 未到目标且线速度过小：强制最小速度
if dist_to_goal > GOAL_THRESHOLD and lin < MIN_LINEAR: # 强制最小速度：未到目标且输出太小
if dist_to_goal > cfg.goal_threshold and lin < cfg.min_linear:
if lin > 1e-6: if lin > 1e-6:
vx *= MIN_LINEAR / lin s = cfg.min_linear / lin
vy *= MIN_LINEAR / lin vx *= s
vy *= s
else: else:
# 融合后接近零：优先朝目标，被挡时朝避障方向 # 彻底趋零时：优先用目标方向（若也为零就不动）
ax, ay = vx_g + vx_a, vy_g + vy_a ax = vx_g + vx_a
anorm = np.sqrt(ax * ax + ay * ay) + 1e-8 ay = vy_g + vy_a
vx = MIN_LINEAR * ax / anorm an = float(np.hypot(ax, ay)) + 1e-9
vy = MIN_LINEAR * ay / anorm vx = cfg.min_linear * ax / an
lin = np.sqrt(vx * vx + vy * vy) vy = cfg.min_linear * ay / an
if lin > MAX_LINEAR:
scale = MAX_LINEAR / lin # 卡住脱困：额外加一点“侧向+偏航”，更容易摆脱夹角/凹障碍
vx *= scale if stuck:
vy *= scale # 让它更愿意横移并转一点（避免只顶着障碍）
wz = np.clip(wz, -MAX_ANGULAR, MAX_ANGULAR) vx *= 0.85
vy = vy * 1.15 + 0.06 * np.sign(vy if abs(vy) > 1e-3 else 1.0)
wz += 0.10 * np.sign(wz if abs(wz) > 1e-3 else 1.0)
# 限幅
lin = float(np.hypot(vx, vy))
if lin > cfg.max_linear:
s = cfg.max_linear / lin
vx *= s
vy *= s
wz = float(np.clip(wz, -cfg.max_angular, cfg.max_angular))
return vx, vy, wz return vx, vy, wz
class VelocityFilter: # ===================== 主程序 =====================
"""EMA 滤波 + 速率限制，平滑控制输出"""
def __init__(self, alpha=0.75, max_dv=0.15, max_dw=0.2):
self.alpha = alpha # 滤波系数，越大越平滑
self.max_dv = max_dv # 每步最大线速度变化
self.max_dw = max_dw # 每步最大角速度变化
self.vx, self.vy, self.wz = 0.0, 0.0, 0.0
def update(self, vx_cmd, vy_cmd, wz_cmd):
# 1. EMA 滤波
vx_f = self.alpha * self.vx + (1 - self.alpha) * vx_cmd
vy_f = self.alpha * self.vy + (1 - self.alpha) * vy_cmd
wz_f = self.alpha * self.wz + (1 - self.alpha) * wz_cmd
# 2. 速率限制
dvx = np.clip(vx_f - self.vx, -self.max_dv, self.max_dv)
dvy = np.clip(vy_f - self.vy, -self.max_dv, self.max_dv)
dwz = np.clip(wz_f - self.wz, -self.max_dw, self.max_dw)
self.vx += dvx
self.vy += dvy
self.wz += dwz
return self.vx, self.vy, self.wz
def main(): def main():
model = mujoco.MjModel.from_xml_path(SCENE_PATH) model = mujoco.MjModel.from_xml_path(SCENE_PATH)
data = mujoco.MjData(model) data = mujoco.MjData(model)
@@ -199,163 +305,145 @@ def main():
body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cylinder_agent") body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cylinder_agent")
site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ray_origin") site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ray_origin")
# 执行器索引 # actuators
act_ids = [ act_vel_x = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_x")
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_x"), act_vel_y = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_y")
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_y"), act_vel_yaw = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_yaw")
mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_yaw"),
] # goal marker joint
goal_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "goal_joint")
goal_qposadr = int(model.jnt_qposadr[goal_joint_id])
def hide_goal_marker():
data.qpos[goal_qposadr: goal_qposadr + 3] = [-99.0, -99.0, 0.1]
def set_goal_marker(gx: float, gy: float):
data.qpos[goal_qposadr] = gx
data.qpos[goal_qposadr + 1] = gy
data.qpos[goal_qposadr + 2] = 0.15
hide_goal_marker()
scanner = RayScanner(CFG.num_rays, CFG.ray_angle_span)
vel_filter = VelocityFilter(alpha=CFG.ema_alpha, max_dv=CFG.max_dv, max_dw=CFG.max_dw)
goals: List[Tuple[float, float]] = []
goal_active = False
goals = GOAL_POINTS.copy()
goal_idx = 0 if goals else -1
vel_filter = VelocityFilter(alpha=0.70, max_dv=0.18, max_dw=0.2)
last_x, last_y = 0.0, 0.0 last_x, last_y = 0.0, 0.0
stuck_cnt = 0 stuck_cnt = 0
goal_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "goal_joint") # key flags
goal_qposadr = model.jnt_qposadr[goal_joint_id] add_goal_ahead = False
data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1] # 初始隐藏 add_goal_camera = False
print("=" * 55) print("=" * 60)
print("VLA 3D 导航 - 全向移动 + 前向扫描 + 避障") print("MuJoCo 全向导航：射线扫描 + 避障 + 目标点")
print("=" * 55) print(f"G: 车头前方 {CFG.goal_ahead_dist:.1f}m | C: 相机视线落点 | 目标边界 R<{CFG.goal_max_r:.1f}m")
print("G: 车头前方 {:.0f}m | C: 相机视线 | 目标边界 R<{:.0f}m".format(GOAL_AHEAD_DIST, GOAL_MAX_R))
print("ESC 退出") print("ESC 退出")
print("=" * 55) print("=" * 60)
add_goal_ahead = [False] def key_cb(keycode: int):
add_goal_camera = [False] nonlocal add_goal_ahead, add_goal_camera
if keycode == 71: # G
add_goal_ahead = True
elif keycode == 67: # C
add_goal_camera = True
def key_cb(keycode): def stop_output():
if keycode == 71: # G data.ctrl[act_vel_x] = 0.0
add_goal_ahead[0] = True data.ctrl[act_vel_y] = 0.0
elif keycode == 67: # C: 相机视线与地面交点 data.ctrl[act_vel_yaw] = 0.0
add_goal_camera[0] = True
def _goal_from_camera(viewer_handle):
"""相机视线与 z=0 地面交点（旋转相机后按 C）"""
cam = viewer_handle.cam
lookat = np.array(cam.lookat)
dist = float(cam.distance)
az = np.deg2rad(float(cam.azimuth))
el = np.deg2rad(float(cam.elevation))
# 相机位置 = lookat - dist * 前向单位向量
fx = np.cos(el) * np.sin(az)
fy = -np.cos(el) * np.cos(az)
fz = np.sin(el)
cam_pos = lookat - dist * np.array([fx, fy, fz])
# 射线与 z=0 交点: cam_pos + t*[fx,fy,fz], 令 z=0
if abs(fz) < 1e-6:
return None
t = -cam_pos[2] / fz
if t < 0:
return None
pt = cam_pos + t * np.array([fx, fy, fz])
return (float(pt[0]), float(pt[1]))
def control_callback():
nonlocal goal_idx, last_x, last_y, stuck_cnt
if add_goal_ahead[0]:
add_goal_ahead[0] = False
x, y, yaw = get_agent_state(data, body_id)
gx = x + GOAL_AHEAD_DIST * np.cos(yaw)
gy = y + GOAL_AHEAD_DIST * np.sin(yaw)
gx, gy = _clamp_goal(gx, gy)
goals.clear()
goals.append((gx, gy))
goal_idx = 0
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
print(" 目标: ({:.2f}, {:.2f}) [G=车头前方]".format(gx, gy))
elif add_goal_camera[0]:
add_goal_camera[0] = False
g = _goal_from_camera(viewer)
if g is not None:
g = _clamp_goal(g[0], g[1])
goals.clear()
goals.append(g)
goal_idx = 0
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
print(" 目标: ({:.2f}, {:.2f}) [C=相机视线]".format(g[0], g[1]))
else:
print(" [C] 相机未指向地面，请调整视角后重试")
if goal_idx < 0 or not goals:
data.ctrl[act_ids[0]] = 0
data.ctrl[act_ids[1]] = 0
data.ctrl[act_ids[2]] = 0
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1] # 藏起标记
return
x, y, yaw = get_agent_state(data, body_id)
goal_x, goal_y = goals[goal_idx]
dist_to_goal = np.sqrt((goal_x - x) ** 2 + (goal_y - y) ** 2)
if dist_to_goal < GOAL_THRESHOLD:
goals.clear()
goal_idx = -1
vel_filter.vx = vel_filter.vy = vel_filter.wz = 0.0
print(" 已到达，停止。按 G 设置新目标")
data.ctrl[act_ids[0]] = 0
data.ctrl[act_ids[1]] = 0
data.ctrl[act_ids[2]] = 0
data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1]
return
# 更新目标点标记位置
data.qpos[goal_qposadr] = goal_x
data.qpos[goal_qposadr + 1] = goal_y
data.qpos[goal_qposadr + 2] = 0.15
# 1. 前向范围扫描
distances, angles = forward_ray_scan(
model, data, body_id, site_id, NUM_RAYS, RAY_ANGLE_SPAN
)
min_d = np.min(distances)
# 2. 卡住检测
moved = np.sqrt((x - last_x) ** 2 + (y - last_y) ** 2)
if moved < STUCK_THRESH and dist_to_goal > GOAL_THRESHOLD:
stuck_cnt += 1
else:
stuck_cnt = 0
last_x, last_y = x, y
stuck = stuck_cnt > STUCK_STEPS
# 3. 避障速度（含绕行分量）
vx_a, vy_a, wz_a = obstacle_avoidance_vel(distances, angles)
# 4. 目标吸引速度
vx_g, vy_g, wz_g = goal_attraction_vel(x, y, yaw, goal_x, goal_y)
# 5. 融合、最小速度、限幅
vx, vy, wz = blend_and_clamp(
vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, min_d, stuck
)
# 6. 滤波 + 速率限制（卡住时放宽）
max_dv = 0.25 if stuck else 0.18
max_dw = 0.28 if stuck else 0.2
vel_filter.max_dv = max_dv
vel_filter.max_dw = max_dw
vx, vy, wz = vel_filter.update(vx, vy, wz)
# 车体系 -> 世界系（slide_x/y 沿世界轴）
c, s = np.cos(yaw), np.sin(yaw)
vx_w = vx * c - vy * s
vy_w = vx * s + vy * c
data.ctrl[act_ids[0]] = vx_w
data.ctrl[act_ids[1]] = vy_w
data.ctrl[act_ids[2]] = wz
with mujoco.viewer.launch_passive(model, data, key_callback=key_cb) as viewer: with mujoco.viewer.launch_passive(model, data, key_callback=key_cb) as viewer:
while viewer.is_running(): while viewer.is_running():
mujoco.mj_forward(model, data) mujoco.mj_forward(model, data)
control_callback()
# ========== goal set ==========
nonlocal_vars = None # 仅为了强调这里不用闭包 nonlocal
if add_goal_ahead:
add_goal_ahead = False
x, y, yaw = get_agent_state(data)
gx = x + CFG.goal_ahead_dist * float(np.cos(yaw))
gy = y + CFG.goal_ahead_dist * float(np.sin(yaw))
gx, gy = clamp_goal(gx, gy, CFG.goal_max_r)
goals[:] = [(gx, gy)]
goal_active = True
vel_filter.reset()
print(f" 目标: ({gx:.2f}, {gy:.2f}) [G=车头前方]")
elif add_goal_camera:
add_goal_camera = False
g = goal_from_camera(viewer)
if g is None:
print(" [C] 相机未指向地面，请调整视角后重试")
else:
gx, gy = clamp_goal(g[0], g[1], CFG.goal_max_r)
goals[:] = [(gx, gy)]
goal_active = True
vel_filter.reset()
print(f" 目标: ({gx:.2f}, {gy:.2f}) [C=相机视线]")
if not goal_active or not goals:
stop_output()
vel_filter.reset()
hide_goal_marker()
mujoco.mj_step(model, data)
viewer.sync()
continue
# ========== state ==========
x, y, yaw = get_agent_state(data)
goal_x, goal_y = goals[0]
dist_to_goal = float(np.hypot(goal_x - x, goal_y - y))
if dist_to_goal < CFG.goal_threshold:
print(" 已到达，停止。按 G / C 设置新目标")
goal_active = False
goals.clear()
vel_filter.reset()
stop_output()
hide_goal_marker()
mujoco.mj_step(model, data)
viewer.sync()
continue
set_goal_marker(goal_x, goal_y)
# ========== scan ==========
distances, angles = scanner.scan(model, data, body_id, site_id)
# ========== stuck detect ==========
moved = float(np.hypot(x - last_x, y - last_y))
if moved < CFG.stuck_thresh and dist_to_goal > CFG.goal_threshold:
stuck_cnt += 1
else:
stuck_cnt = 0
last_x, last_y = x, y
stuck = stuck_cnt > CFG.stuck_steps
# ========== velocities ==========
vx_a, vy_a, wz_a, _min_d = obstacle_avoidance_vel(distances, angles, CFG)
vx_g, vy_g, wz_g = goal_attraction_vel(x, y, yaw, goal_x, goal_y, CFG)
vx_b, vy_b, wz_b = blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, stuck, CFG)
# ========== filter ==========
vel_filter.max_dv = CFG.max_dv_stuck if stuck else CFG.max_dv
vel_filter.max_dw = CFG.max_dw_stuck if stuck else CFG.max_dw
vx, vy, wz = vel_filter.update(vx_b, vy_b, wz_b)
# ========== body -> world for actuator ==========
c = float(np.cos(yaw))
s = float(np.sin(yaw))
vx_w = vx * c - vy * s
vy_w = vx * s + vy * c
data.ctrl[act_vel_x] = vx_w
data.ctrl[act_vel_y] = vy_w
data.ctrl[act_vel_yaw] = wz
mujoco.mj_step(model, data) mujoco.mj_step(model, data)
viewer.sync() viewer.sync()
if __name__ == "__main__": if __name__ == "__main__":
main() main()