release: v0.1 initial version
This commit is contained in:
lovez
79
OUTLINE.md
Normal file
79
OUTLINE.md
Normal file
@@ -0,0 +1,79 @@
|
||||
# ROS1 + MuJoCo VLA 视觉避障 3D 导航 — 项目大纲
|
||||
|
||||
## 一、项目概述
|
||||
|
||||
基于 ROS1 与 MuJoCo 搭建一个 **VLA (Vision-Language-Action)** 视觉避障 3D 导航系统，实现圆柱形机器人在仿真环境中的自主避障与路径规划。
|
||||
|
||||
---
|
||||
|
||||
## 二、里程碑目标
|
||||
|
||||
### 🎯 目标一：基础仿真场景（当前阶段）✅
|
||||
- [x] 创建 MuJoCo 仿真环境
|
||||
- [x] 添加圆柱体机器人/代理
|
||||
- [x] 放置若干障碍物（盒子、圆柱、球体）
|
||||
- [x] 运行基础物理仿真
|
||||
|
||||
### 目标二：相机与视觉感知
|
||||
- [ ] 在圆柱体上添加相机传感器
|
||||
- [ ] 实现 RGB/深度图像发布
|
||||
- [ ] ROS1 `/camera/image_raw` 话题发布
|
||||
|
||||
### 目标三：运动控制接口
|
||||
- [ ] 圆柱体自由关节（freejoint）或差速模型
|
||||
- [ ] 发布 `/cmd_vel` 接收速度指令
|
||||
- [ ] 订阅 `/odom` 里程计
|
||||
|
||||
### 目标四：VLA 视觉避障核心
|
||||
- [ ] 集成视觉语言模型（VLM/VLA）接口
|
||||
- [ ] 基于视觉的障碍物检测与语义理解
|
||||
- [ ] 语言条件避障策略（如 "避开红色障碍物"）
|
||||
|
||||
### 目标五：3D 导航与路径规划
|
||||
- [ ] 3D 栅格地图或点云表示
|
||||
- [ ] 路径规划算法（A* / RRT 等）
|
||||
- [ ] 与 VLA 决策融合
|
||||
|
||||
### 目标六：ROS1 完整集成
|
||||
- [ ] ROS1 包结构、launch、参数服务器
|
||||
- [ ] TF 变换、传感器消息标准格式
|
||||
- [ ] 可视化（RViz）
|
||||
|
||||
---
|
||||
|
||||
## 三、技术栈
|
||||
|
||||
| 组件 | 技术 |
|
||||
|------|------|
|
||||
| 仿真 | MuJoCo 3.x |
|
||||
| 机器人中间件 | ROS1 Noetic |
|
||||
| 视觉 | OpenCV, 相机传感器 |
|
||||
| VLA/VLM | 待选（如 LLaVA, RT-2, OpenVLA 等） |
|
||||
| 语言 | Python 3.8+ |
|
||||
|
||||
---
|
||||
|
||||
## 四、目录结构（规划）
|
||||
|
||||
```
|
||||
/home/pc/test/
|
||||
├── OUTLINE.md # 本大纲
|
||||
├── requirements.txt
|
||||
├── mujoco_scenes/ # MuJoCo 场景
|
||||
│ └── cylinder_obstacles.xml
|
||||
├── scripts/ # 仿真与 ROS 节点
|
||||
│ ├── run_simulation.py
|
||||
│ └── ros_bridge.py # 后续
|
||||
└── ros_ws/ # ROS1 工作空间（后续）
|
||||
└── src/
|
||||
└── vla_nav/
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 五、依赖安装
|
||||
|
||||
```bash
|
||||
pip install mujoco
|
||||
# ROS1 Noetic 需单独安装
|
||||
```
|
||||
71
mujoco_scenes/cylinder_obstacles.xml
Normal file
71
mujoco_scenes/cylinder_obstacles.xml
Normal file
@@ -0,0 +1,71 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<mujoco model="VLA 3D 导航 - 圆柱体与障碍物">
|
||||
<option timestep="0.002" gravity="0 0 -9.81"/>
|
||||
|
||||
<default>
|
||||
<geom friction="1 0.5 0.5" condim="3"/>
|
||||
</default>
|
||||
|
||||
<worldbody>
|
||||
<!-- 地面 -->
|
||||
<geom name="floor" type="plane" size="10 10 0.1" rgba="0.7 0.7 0.75 1"/>
|
||||
|
||||
<!-- 圆柱体小车（全向移动：vx, vy, wz） -->
|
||||
<body name="cylinder_agent" pos="0 0 0.35">
|
||||
<joint name="slide_x" type="slide" axis="1 0 0"/>
|
||||
<joint name="slide_y" type="slide" axis="0 1 0"/>
|
||||
<joint name="hinge_yaw" type="hinge" axis="0 0 1"/>
|
||||
<geom name="agent_body" type="cylinder" size="0.2 0.15" rgba="0.2 0.6 0.9 1"
|
||||
mass="5"/>
|
||||
<!-- 前进方向箭头 X/Y/Z（contype/conaffinity=0 不参与碰撞） -->
|
||||
<geom name="axis_x" type="cylinder" fromto="0 0 0 0.38 0 0" size="0.035"
|
||||
rgba="1 0 0 0.95" contype="0" conaffinity="0"/>
|
||||
<geom name="axis_y" type="cylinder" fromto="0 0 0 0 0.25 0" size="0.025"
|
||||
rgba="0 0.9 0 0.9" contype="0" conaffinity="0"/>
|
||||
<geom name="axis_z" type="cylinder" fromto="0 0 0 0 0 0.2" size="0.025"
|
||||
rgba="0 0 1 0.9" contype="0" conaffinity="0"/>
|
||||
<site name="ray_origin" pos="0.25 0 0" size="0.01"/>
|
||||
</body>
|
||||
|
||||
<!-- 目标点标记（用 freejoint 可动态更新位置） -->
|
||||
<body name="goal_marker" pos="0 0 0.15">
|
||||
<freejoint name="goal_joint"/>
|
||||
<geom name="goal_geom" type="sphere" size="0.12" rgba="1 1 0 0.6"
|
||||
contype="0" conaffinity="0"/>
|
||||
</body>
|
||||
|
||||
<!-- 障碍物 1: 红色盒子 -->
|
||||
<body name="obstacle_box1" pos="1.5 0 0.25">
|
||||
<geom name="obs_box1" type="box" size="0.3 0.3 0.25" rgba="0.9 0.2 0.2 1"/>
|
||||
</body>
|
||||
|
||||
<!-- 障碍物 2: 绿色圆柱 -->
|
||||
<body name="obstacle_cyl1" pos="-1.2 1.0 0.2">
|
||||
<geom name="obs_cyl1" type="cylinder" size="0.15 0.2" rgba="0.2 0.8 0.3 1"/>
|
||||
</body>
|
||||
|
||||
<!-- 障碍物 3: 蓝色球体 -->
|
||||
<body name="obstacle_sphere1" pos="0.8 -1.0 0.15">
|
||||
<geom name="obs_sphere1" type="sphere" size="0.15" rgba="0.2 0.3 0.9 1"/>
|
||||
</body>
|
||||
|
||||
<!-- 障碍物 4: 黄色盒子 -->
|
||||
<body name="obstacle_box2" pos="-0.5 -0.8 0.2">
|
||||
<geom name="obs_box2" type="box" size="0.25 0.25 0.2" rgba="0.95 0.85 0.2 1"/>
|
||||
</body>
|
||||
|
||||
<!-- 障碍物 5: 紫色圆柱 -->
|
||||
<body name="obstacle_cyl2" pos="1.0 0.8 0.25">
|
||||
<geom name="obs_cyl2" type="cylinder" size="0.12 0.25" rgba="0.6 0.2 0.8 1"/>
|
||||
</body>
|
||||
|
||||
<camera name="top_down" pos="0 0 5" xyaxes="1 0 0 0 1 0"/>
|
||||
</worldbody>
|
||||
|
||||
<!-- 全向 velocity 执行器 -->
|
||||
<actuator>
|
||||
<velocity name="vel_x" joint="slide_x" kv="80"/>
|
||||
<velocity name="vel_y" joint="slide_y" kv="80"/>
|
||||
<velocity name="vel_yaw" joint="hinge_yaw" kv="30"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
1
requirements.txt
Normal file
1
requirements.txt
Normal file
@@ -0,0 +1 @@
|
||||
mujoco>=3.0.0
|
||||
361
scripts/run_simulation.py
Normal file
361
scripts/run_simulation.py
Normal file
@@ -0,0 +1,361 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
ROS1 + MuJoCo VLA 3D 导航 — 全向移动 + 前向扫描 + 避障 + 目标点导航
|
||||
|
||||
圆柱体小车: vx, vy, wz 全向移动
|
||||
前向范围扫描: mj_ray 射线检测
|
||||
避障: 基于距离的势场/向量场
|
||||
"""
|
||||
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)
|
||||
|
||||
|
||||
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))
|
||||
|
||||
|
||||
def get_agent_state(data, body_id):
|
||||
"""获取小车位姿: x, y, yaw"""
|
||||
qpos = data.qpos
|
||||
x, y = qpos[0], qpos[1]
|
||||
yaw = qpos[2]
|
||||
return x, y, yaw
|
||||
|
||||
|
||||
def forward_ray_scan(model, data, body_id, site_id, num_rays, angle_span):
|
||||
"""
|
||||
前向范围扫描: 在车体前方扇形区域内发射射线
|
||||
返回: (距离数组, 角度数组)
|
||||
"""
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
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
|
||||
|
||||
return distances, 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
|
||||
|
||||
vx, vy, wz = 0.0, 0.0, 0.0
|
||||
safe_sector = np.argmax(distances)
|
||||
best_theta = angles[safe_sector]
|
||||
|
||||
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)
|
||||
|
||||
# 绕行：朝最开阔方向加正向速度，避免只退不绕
|
||||
bypass = 0.35
|
||||
vx += bypass * np.cos(best_theta)
|
||||
vy += bypass * np.sin(best_theta)
|
||||
wz += 0.4 * best_theta
|
||||
|
||||
return vx, vy, wz
|
||||
|
||||
|
||||
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):
|
||||
"""融合避障与目标速度，限幅，卡住时强制最小速度"""
|
||||
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:
|
||||
if lin > 1e-6:
|
||||
vx *= MIN_LINEAR / lin
|
||||
vy *= MIN_LINEAR / lin
|
||||
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)
|
||||
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)
|
||||
|
||||
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"),
|
||||
]
|
||||
|
||||
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] # 初始隐藏
|
||||
|
||||
print("=" * 55)
|
||||
print("VLA 3D 导航 - 全向移动 + 前向扫描 + 避障")
|
||||
print("=" * 55)
|
||||
print("G: 车头前方 {:.0f}m | C: 相机视线 | 目标边界 R<{:.0f}m".format(GOAL_AHEAD_DIST, GOAL_MAX_R))
|
||||
print("ESC 退出")
|
||||
print("=" * 55)
|
||||
|
||||
add_goal_ahead = [False]
|
||||
add_goal_camera = [False]
|
||||
|
||||
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
|
||||
|
||||
with mujoco.viewer.launch_passive(model, data, key_callback=key_cb) as viewer:
|
||||
while viewer.is_running():
|
||||
mujoco.mj_forward(model, data)
|
||||
control_callback()
|
||||
mujoco.mj_step(model, data)
|
||||
viewer.sync()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user