From 175ea87fed9444af013cba022b916f280bf80d0d Mon Sep 17 00:00:00 2001 From: lovez Date: Sat, 21 Feb 2026 10:33:09 +0800 Subject: [PATCH] release: v0.1 initial version --- MJDATA.TXT | 554 +++++++++++++++++++++++++++++++ scripts/run_simulation.py | 676 +++++++++++++++++++++----------------- 2 files changed, 936 insertions(+), 294 deletions(-) create mode 100644 MJDATA.TXT diff --git a/MJDATA.TXT b/MJDATA.TXT new file mode 100644 index 0000000..72a2963 --- /dev/null +++ b/MJDATA.TXT @@ -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 + diff --git a/scripts/run_simulation.py b/scripts/run_simulation.py index 23f6ae0..5ab3e1d 100644 --- a/scripts/run_simulation.py +++ b/scripts/run_simulation.py @@ -1,197 +1,303 @@ #!/usr/bin/env python3 """ -ROS1 + MuJoCo VLA 3D 导航 — 全向移动 + 前向扫描 + 避障 + 目标点导航 - -圆柱体小车: vx, vy, wz 全向移动 -前向范围扫描: mj_ray 射线检测 -避障: 基于距离的势场/向量场 +MuJoCo 全向小车导航（前向射线扫描 + 势场避障 + 目标点导航） +- vx, vy, wz 全向移动（车体系输出 -> 世界系执行器） +- mj_ray 扇形前向扫描 +- 避障：排斥 + 朝开阔方向绕行 +- 卡住检测：连续位移太小 -> 强制脱困策略 """ +from __future__ import annotations + +import os +from dataclasses import dataclass +from typing import Optional, Tuple, List + import mujoco import mujoco.viewer import numpy as np -import os + 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) -OBSTACLE_THRESHOLD = 0.45 # 障碍判定距离 (m) -SAFE_DISTANCE = 0.65 # 安全距离 -GOAL_THRESHOLD = 0.2 # 到达目标判定距离 -MAX_LINEAR = 0.6 # 最大线速度 -MAX_ANGULAR = 0.8 # 最大角速度 -K_ATTRACT = 1.0 # 目标吸引力 -K_REPEL = 0.6 # 障碍排斥力 -K_YAW_GOAL = 0.8 # 目标对准角速度增益 -MIN_LINEAR = 0.18 # 最小线速度，避免卡住 -STUCK_THRESH = 0.03 # 位移阈值，低于此认为卡住 (m) -STUCK_STEPS = 100 # 连续卡住步数触发绕行 -GOAL_POINTS = [] # 初始无目标 -GOAL_AHEAD_DIST = 2.0 # G 键：目标 = 车头前方 N 米 -GOAL_MAX_R = 5.0 # 目标点边界：距离原点不超过此值 (m) + +# ===================== 参数配置 ===================== +@dataclass +class NavCfg: + # ray scan + num_rays: int = 19 + ray_angle_span: float = np.pi # -span/2 ~ +span/2 + + # distances + obstacle_threshold: float = 0.45 + safe_distance: float = 0.65 + goal_threshold: float = 0.20 + + # limits + max_linear: float = 0.60 + max_angular: float = 0.80 + 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): - """目标点边界：限制在距离原点 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)) +CFG = NavCfg() -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 - x, y = qpos[0], qpos[1] - yaw = qpos[2] - return x, y, yaw + return float(qpos[0]), float(qpos[1]), float(qpos[2]) -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) - distances = np.full(num_rays, 10.0) + def __init__(self, num_rays: int, angle_span: float): + self.num_rays = int(num_rays) + self.angle_span = float(angle_span) - for i, theta in enumerate(angles): - c, s = np.cos(theta), np.sin(theta) - ray_dir = c * forward + s * xmat[:, 1] # 绕 body Z 旋转 - ray_dir = ray_dir / (np.linalg.norm(ray_dir) + 1e-8) + self.angles = np.linspace(-self.angle_span / 2, self.angle_span / 2, self.num_rays) + c = np.cos(self.angles) + s = np.sin(self.angles) - geomid = np.array([-1], dtype=np.int32) - d = mujoco.mj_ray(model, data, site_xpos, ray_dir, None, 1, body_id, geomid) - if d >= 0: - distances[i] = d + # body-frame ray directions: dir_b = c*[1,0,0] + s*[0,1,0] + # shape (N,3) + self.ray_dirs_body = np.stack([c, s, np.zeros_like(c)], axis=1).astype(np.float64) - 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): - """ - 避障速度 + 绕行分量。被挡时朝开阔方向加速，而非只排斥 - 返回: (vx_avoid, vy_avoid, wz_avoid) - """ - min_d = np.min(distances) - if min_d > OBSTACLE_THRESHOLD: - return 0.0, 0.0, 0.0 +# ===================== 速度融合与滤波 ===================== +class VelocityFilter: + """EMA + 速率限制，输出更平滑，避免 ctrl 抖动。""" - vx, vy, wz = 0.0, 0.0, 0.0 - safe_sector = np.argmax(distances) - best_theta = angles[safe_sector] + def __init__(self, alpha: float, max_dv: float, max_dw: float): + self.alpha = float(alpha) + 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): - if d < OBSTACLE_THRESHOLD: - 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 reset(self): + self.vx = self.vy = self.wz = 0.0 - # 绕行：朝最开阔方向加正向速度，避免只退不绕 + 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 vx += bypass * np.cos(best_theta) vy += bypass * np.sin(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): - """ - 目标吸引力: 在车体坐标系下计算朝目标的 vx, vy, wz - 优先保持线速度，角速度仅用于微调朝向 - """ - dx = goal_x - x - 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): - """融合避障与目标速度，限幅，卡住时强制最小速度""" +def blend_and_clamp( + vx_a: float, vy_a: float, wz_a: float, + vx_g: float, vy_g: float, wz_g: float, + dist_to_goal: float, + stuck: bool, + cfg: NavCfg +): vx = vx_a + vx_g vy = vy_a + vy_g wz = wz_a + wz_g - lin = np.sqrt(vx * vx + vy * vy) - # 未到目标且线速度过小：强制最小速度 - if dist_to_goal > GOAL_THRESHOLD and lin < MIN_LINEAR: + lin = float(np.hypot(vx, vy)) + + # 强制最小速度：未到目标且输出太小 + if dist_to_goal > cfg.goal_threshold and lin < cfg.min_linear: if lin > 1e-6: - vx *= MIN_LINEAR / lin - vy *= MIN_LINEAR / lin + s = cfg.min_linear / lin + vx *= s + vy *= s else: - # 融合后接近零：优先朝目标，被挡时朝避障方向 - ax, ay = vx_g + vx_a, vy_g + vy_a - anorm = np.sqrt(ax * ax + ay * ay) + 1e-8 - vx = MIN_LINEAR * ax / anorm - vy = MIN_LINEAR * ay / anorm - lin = np.sqrt(vx * vx + vy * vy) - if lin > MAX_LINEAR: - scale = MAX_LINEAR / lin - vx *= scale - vy *= scale - wz = np.clip(wz, -MAX_ANGULAR, MAX_ANGULAR) + # 彻底趋零时：优先用目标方向（若也为零就不动） + ax = vx_g + vx_a + ay = vy_g + vy_a + an = float(np.hypot(ax, ay)) + 1e-9 + vx = cfg.min_linear * ax / an + vy = cfg.min_linear * ay / an + + # 卡住脱困：额外加一点“侧向+偏航”，更容易摆脱夹角/凹障碍 + if stuck: + # 让它更愿意横移并转一点（避免只顶着障碍） + 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 -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(): model = mujoco.MjModel.from_xml_path(SCENE_PATH) data = mujoco.MjData(model) @@ -199,163 +305,145 @@ def main(): body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cylinder_agent") site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ray_origin") - # 执行器索引 - act_ids = [ - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_x"), - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_y"), - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "vel_yaw"), - ] + # actuators + act_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") + act_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 stuck_cnt = 0 - goal_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "goal_joint") - goal_qposadr = model.jnt_qposadr[goal_joint_id] - data.qpos[goal_qposadr : goal_qposadr + 3] = [-99, -99, 0.1] # 初始隐藏 + # key flags + add_goal_ahead = False + add_goal_camera = False - print("=" * 55) - print("VLA 3D 导航 - 全向移动 + 前向扫描 + 避障") - print("=" * 55) - print("G: 车头前方 {:.0f}m | C: 相机视线 | 目标边界 R<{:.0f}m".format(GOAL_AHEAD_DIST, GOAL_MAX_R)) + print("=" * 60) + print("MuJoCo 全向导航：射线扫描 + 避障 + 目标点") + print(f"G: 车头前方 {CFG.goal_ahead_dist:.1f}m | C: 相机视线落点 | 目标边界 R<{CFG.goal_max_r:.1f}m") print("ESC 退出") - print("=" * 55) + print("=" * 60) - add_goal_ahead = [False] - add_goal_camera = [False] + def key_cb(keycode: int): + 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): - if keycode == 71: # G - add_goal_ahead[0] = True - elif keycode == 67: # C: 相机视线与地面交点 - 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 + def stop_output(): + data.ctrl[act_vel_x] = 0.0 + data.ctrl[act_vel_y] = 0.0 + data.ctrl[act_vel_yaw] = 0.0 with mujoco.viewer.launch_passive(model, data, key_callback=key_cb) as viewer: while viewer.is_running(): 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) viewer.sync() if __name__ == "__main__": - main() + main() \ No newline at end of file