diff --git a/docs/humanoid/g1_rgb_floor_patrol/index.md b/docs/humanoid/g1_rgb_floor_patrol/index.md
new file mode 100644
index 0000000000..4d6d2757b8
--- /dev/null
+++ b/docs/humanoid/g1_rgb_floor_patrol/index.md
@@ -0,0 +1,33 @@
+# G1 RGB 地面分割巡检
+
+用地面分割和语义控制网络为 G1 生成通行速度,并在地面掩码为空时停车。
+
+## 环境与运行
+
+使用 Python 3.11。在模块目录运行 `python -m pip install -r requirements.txt`。
+
+在模块目录运行 `python scripts/fetch_assets.py /path/to/unitree_rl_gym` 获取固定版本的 G1 资产。底层步态来自 Unitree 预训练策略。
+
+```bash
+python main.py --assets /path/to/unitree_rl_gym --record --output results
+```
+
+闭环默认使用留出种子 7,输出轨迹及到达、碰撞和跌倒指标。可用 `--mode collect` 采样,随后用 `--mode train` 复训。无地面停车由规则触发。
+
+## ROS launch
+
+在 ROS Noetic 源码空间运行,将路径替换为实际值。
+
+```bash
+source /opt/ros/noetic/setup.bash
+export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}"
+roslaunch g1_rgb_floor_patrol main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym
+```
+
+## 运行效果
+
+
+
+## 大模型使用声明
+
+使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
diff --git a/docs/img/g1_rgb_floor_patrol/perception_7.gif b/docs/img/g1_rgb_floor_patrol/perception_7.gif
new file mode 100644
index 0000000000..ae5f1cbdc1
Binary files /dev/null and b/docs/img/g1_rgb_floor_patrol/perception_7.gif differ
diff --git a/docs/index.md b/docs/index.md
index 9f6f16ff53..e5ae6df2ad 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -90,6 +90,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c
* [双足步态生成与 ZMP 稳定性](./humanoid/bipedal_gait_zmp.md)
* [基于三次样条插值的双足多步态平滑切换](./humanoid/biped_cubic_spline_gait_switch.md)
* [人形机器人键盘运动控制](./humanoid/humanoid_keyboard_control.md)
+* [G1 RGB 地面分割巡检](./humanoid/g1_rgb_floor_patrol/index.md)
* [人形机器人平衡控制与外力抗扰恢复](./humanoid/humanoid_push_recovery.md)
* [人形机器人 PPO 神经网络平衡控制](./humanoid/humanoid_ppo_locomotion.md)
* [人形机器人 2D 激光雷达 CNN 感知与避障](./humanoid/humanoid_lidar_nn_perception.md)
diff --git a/mkdocs.yml b/mkdocs.yml
index edf15e5906..5f388ae4c4 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -84,6 +84,7 @@ nav:
- 双足步态生成与 ZMP 稳定性: './humanoid/bipedal_gait_zmp.md'
- 基于三次样条插值的双足多步态平滑切换: './humanoid/biped_cubic_spline_gait_switch.md'
- 人形机器人键盘运动控制: './humanoid/humanoid_keyboard_control.md'
+ - G1 RGB 地面分割巡检: './humanoid/g1_rgb_floor_patrol/index.md'
- 键盘操控仿真机械臂: './humanoid/panda_keyboard_control.md'
- 机械臂手眼标定仿真: './humanoid/hand_eye_calibration.md'
- 人形机器人平衡控制与外力抗扰恢复: './humanoid/humanoid_push_recovery.md'
diff --git a/src/humanoid/g1_rgb_floor_patrol/.gitattributes b/src/humanoid/g1_rgb_floor_patrol/.gitattributes
new file mode 100644
index 0000000000..f13e51c0ec
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/.gitattributes
@@ -0,0 +1,2 @@
+*.py text eol=lf
+*.sh text eol=lf
diff --git a/src/humanoid/g1_rgb_floor_patrol/CMakeLists.txt b/src/humanoid/g1_rgb_floor_patrol/CMakeLists.txt
new file mode 100644
index 0000000000..b3c02a0895
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/CMakeLists.txt
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(g1_rgb_floor_patrol)
+find_package(catkin REQUIRED)
+catkin_package()
+catkin_install_python(PROGRAMS scripts/task_runner.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/src/humanoid/g1_rgb_floor_patrol/README.md b/src/humanoid/g1_rgb_floor_patrol/README.md
new file mode 100644
index 0000000000..969aa4dd6d
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/README.md
@@ -0,0 +1,29 @@
+# G1 RGB 地面分割巡检
+
+用地面分割和语义控制网络为 G1 生成通行速度,并在地面掩码为空时停车。
+
+## 环境与运行
+
+使用 Python 3.11。在模块目录运行 `python -m pip install -r requirements.txt`。
+
+在模块目录运行 `python scripts/fetch_assets.py /path/to/unitree_rl_gym` 获取固定版本的 G1 资产。底层步态来自 Unitree 预训练策略。
+
+```bash
+python main.py --assets /path/to/unitree_rl_gym --record --output results
+```
+
+闭环默认使用留出种子 7,输出轨迹及到达、碰撞和跌倒指标。可用 `--mode collect` 采样,随后用 `--mode train` 复训。无地面停车由规则触发。
+
+## ROS launch
+
+在 ROS Noetic 源码空间运行,将路径替换为实际值。
+
+```bash
+source /opt/ros/noetic/setup.bash
+export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}"
+roslaunch g1_rgb_floor_patrol main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym
+```
+
+## 大模型使用声明
+
+使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
diff --git a/src/humanoid/g1_rgb_floor_patrol/launch/main.launch b/src/humanoid/g1_rgb_floor_patrol/launch/main.launch
new file mode 100644
index 0000000000..127c168847
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/launch/main.launch
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/humanoid/g1_rgb_floor_patrol/main.py b/src/humanoid/g1_rgb_floor_patrol/main.py
new file mode 100644
index 0000000000..8ea9ab689a
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/main.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""用地面分割和语义控制网络为 G1 生成通行速度,并在地面掩码为空时停车。"""
+import argparse
+import json
+from pathlib import Path
+
+
+def main():
+ parser = argparse.ArgumentParser(description=__doc__)
+ parser.add_argument('--assets', required=True, help='Path to unitree_rl_gym checkout')
+ parser.add_argument('--mode', choices=['perception', 'collect', 'train', 'teacher'], default='perception')
+ parser.add_argument('--output', type=Path)
+ parser.add_argument('--dataset', type=Path, default=Path('data/visual_patrol'))
+ parser.add_argument('--weights', type=Path, default=Path(__file__).resolve().parent / 'models')
+ parser.add_argument('--samples', type=int, default=100)
+ parser.add_argument('--epochs', type=int, default=15)
+ parser.add_argument('--seed', type=int, default=7)
+ parser.add_argument('--seconds', type=float, default=45)
+ parser.add_argument('--record', action='store_true')
+ parser.add_argument('--controller',choices=['navigation','perception','end2end'],default='navigation')
+ args = parser.parse_args()
+ if args.mode == 'collect':
+ from patrol.learning import collect
+ collect(args.assets, args.dataset, args.samples)
+ return
+ if args.mode == 'train':
+ from patrol.learning import train, train_mask_controller
+ train(args.dataset, args.weights, args.epochs)
+ train_mask_controller(args.dataset,args.weights)
+ return
+ if args.mode in ('teacher','perception','end2end'):
+ from patrol.runtime import rollout
+ result = rollout(args.assets,args.mode,args.weights,args.output or Path('results'),
+ args.seed,args.seconds,args.record)
+ print(json.dumps(result,indent=2))
+ return
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/g1_rgb_floor_patrol/models/mask_controller.pt b/src/humanoid/g1_rgb_floor_patrol/models/mask_controller.pt
new file mode 100644
index 0000000000..5bcc069e20
Binary files /dev/null and b/src/humanoid/g1_rgb_floor_patrol/models/mask_controller.pt differ
diff --git a/src/humanoid/g1_rgb_floor_patrol/models/perception.pt b/src/humanoid/g1_rgb_floor_patrol/models/perception.pt
new file mode 100644
index 0000000000..1c8b1565dd
Binary files /dev/null and b/src/humanoid/g1_rgb_floor_patrol/models/perception.pt differ
diff --git a/src/humanoid/g1_rgb_floor_patrol/package.xml b/src/humanoid/g1_rgb_floor_patrol/package.xml
new file mode 100644
index 0000000000..5de9337cf2
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/package.xml
@@ -0,0 +1,9 @@
+
+
+ g1_rgb_floor_patrol0.1.0
+ MuJoCo G1 visual floor perception, image control and ROS indoor navigation.
+ OpenHUTB contributor
+ BSD-3-Clause
+ catkin
+ rospy
+
diff --git a/src/humanoid/g1_rgb_floor_patrol/patrol/__init__.py b/src/humanoid/g1_rgb_floor_patrol/patrol/__init__.py
new file mode 100644
index 0000000000..bc4204b4fb
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/patrol/__init__.py
@@ -0,0 +1 @@
+"""Visual patrol on a physics simulated G1 humanoid."""
diff --git a/src/humanoid/g1_rgb_floor_patrol/patrol/learning.py b/src/humanoid/g1_rgb_floor_patrol/patrol/learning.py
new file mode 100644
index 0000000000..3f040c8c2c
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/patrol/learning.py
@@ -0,0 +1,134 @@
+"""Reproducible simulator data and held-out-scene evaluation."""
+import json
+from pathlib import Path
+import time
+
+import mujoco
+import numpy as np
+import torch
+from torch import nn
+from torch.utils.data import DataLoader, TensorDataset
+
+from .scene import IndoorSimulator
+from .vision import FloorNet, ImageController, MaskController, mask_command
+
+
+def collect(assets, destination, samples=100):
+ destination = Path(destination)
+ destination.mkdir(parents=True, exist_ok=True)
+ for split, seeds in [('train', range(6)), ('validation', [6]), ('test', [7, 8])]:
+ images, masks, actions, scene_ids = [], [], [], []
+ for seed in seeds:
+ sim = IndoorSimulator(assets, seed)
+ rng = np.random.default_rng(1000+seed)
+ try:
+ for _ in range(samples):
+ # Independent static camera sampling, NOT a runtime walking method.
+ mujoco.mj_resetData(sim.model, sim.data)
+ sim.data.qpos[:2] = [rng.uniform(.0, 8.8), rng.uniform(-.9, .9)]
+ yaw = rng.uniform(-.65, .65)
+ sim.data.qpos[3:7] = [np.cos(yaw/2), 0, 0, np.sin(yaw/2)]
+ sim.data.qpos[7:] = sim.default
+ mujoco.mj_forward(sim.model, sim.data)
+ rgb, mask = sim.image(labels=True)
+ images.append(rgb)
+ masks.append(mask)
+ actions.append(mask_command(mask))
+ scene_ids.append(seed)
+ finally:
+ sim.close()
+ print(f'collected {split} scene={seed}', flush=True)
+ np.savez_compressed(destination/f'{split}.npz', images=np.asarray(images),
+ masks=np.asarray(masks), actions=np.asarray(actions), scene_ids=scene_ids)
+ metadata = dict(split_by='scene_seed', train=list(range(6)), validation=[6], test=[7,8],
+ samples_per_scene=samples, labels='MuJoCo geometry-ID floor masks',
+ teacher='reactive floor-mask corridor following',
+ limitations='Static camera poses; appearance/layout randomization is limited. Closed-loop testing required.')
+ (destination/'dataset.json').write_text(json.dumps(metadata, indent=2), encoding='utf-8')
+
+
+def train(dataset, output, epochs=15):
+ torch.manual_seed(42)
+ torch.set_num_threads(4)
+ dataset, output = Path(dataset), Path(output)
+ output.mkdir(parents=True, exist_ok=True)
+ def read(split):
+ with np.load(dataset/f'{split}.npz') as d:
+ return (torch.from_numpy(d['images']).permute(0,3,1,2).float()/255,
+ torch.from_numpy(d['masks']).float()[:,None],
+ torch.from_numpy(d['actions']).float())
+ train_data, val_data, test_data = read('train'), read('validation'), read('test')
+ metrics = {}
+ for mode, model, idx in [('perception', FloorNet(), 1)]:
+ loader = DataLoader(TensorDataset(train_data[0],train_data[idx]), batch_size=16, shuffle=True)
+ optimizer = torch.optim.Adam(model.parameters(), lr=.001)
+ loss_fn = nn.BCEWithLogitsLoss() if idx == 1 else nn.MSELoss()
+ best = float('inf')
+ history = []
+ start = time.time()
+ for epoch in range(epochs):
+ model.train()
+ total = 0.
+ for x,y in loader:
+ optimizer.zero_grad()
+ loss = loss_fn(model(x),y)
+ loss.backward()
+ optimizer.step()
+ total += float(loss.detach())*len(x)
+ model.eval()
+ with torch.inference_mode():
+ val = float(loss_fn(model(val_data[0]),val_data[idx]))
+ history.append(dict(epoch=epoch+1, train_loss=total/len(train_data[0]), validation_loss=val))
+ if val < best:
+ best = val
+ torch.save(model.state_dict(),output/f'{mode}.pt')
+ print(mode,history[-1],flush=True)
+ model.load_state_dict(torch.load(output/f'{mode}.pt',weights_only=True))
+ with torch.inference_mode():
+ pred = model(test_data[0])
+ if idx == 1:
+ pred = pred.sigmoid()>.5
+ truth = test_data[idx]>.5
+ score = dict(floor_iou=float((pred & truth).sum()/(pred | truth).sum().clamp(min=1)))
+ else:
+ error = (pred-test_data[idx]).abs().mean(0)
+ score = dict(speed_mae_mps=float(error[0]), yaw_rate_mae_radps=float(error[1]))
+ metrics[mode] = dict(test=score, history=history, training_seconds=time.time()-start)
+ (output/'metrics.json').write_text(json.dumps(metrics,indent=2),encoding='utf-8')
+ return metrics
+
+
+def train_mask_controller(dataset, output, epochs=100):
+ """Train on scene-disjoint semantic masks; test only after model selection."""
+ torch.manual_seed(73)
+ torch.set_num_threads(2)
+ dataset,output=Path(dataset),Path(output)
+ def load(split):
+ with np.load(dataset/f'{split}.npz') as d:
+ return torch.from_numpy(d['masks']).float()[:,None],torch.from_numpy(d['actions'])
+ x,y=load('train')
+ xv,yv=load('validation')
+ xt,yt=load('test')
+ model=MaskController()
+ optimizer=torch.optim.Adam(model.parameters(),lr=.002)
+ loader=DataLoader(TensorDataset(x,y),batch_size=32,shuffle=True)
+ best=float('inf')
+ for epoch in range(epochs):
+ model.train()
+ for xb,yb in loader:
+ loss=nn.functional.mse_loss(model(xb),yb)
+ optimizer.zero_grad(); loss.backward(); optimizer.step()
+ model.eval()
+ with torch.inference_mode():
+ val=float(nn.functional.mse_loss(model(xv),yv))
+ if val= 10 else 0.3,
+ wz=0.25 if 6 <= t < 10 else 0.0)
+ self.step()
+ min_height = min(min_height, float(self.data.qpos[2]))
+ if self.fallen:
+ break
+ return dict(simulated_seconds=float(self.data.time), fallen=bool(self.fallen),
+ displacement_m=float(np.linalg.norm(self.data.qpos[:2]-start[:2])),
+ min_height_m=min_height, final_yaw_rad=float(self.yaw),
+ final_position=self.data.qpos[:3].tolist())
diff --git a/src/humanoid/g1_rgb_floor_patrol/patrol/runtime.py b/src/humanoid/g1_rgb_floor_patrol/patrol/runtime.py
new file mode 100644
index 0000000000..7f381f483b
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/patrol/runtime.py
@@ -0,0 +1,110 @@
+"""Closed-loop rollouts, keyboard control, and actual simulator recordings."""
+import json
+from pathlib import Path
+import sys
+import time
+
+import imageio.v2 as imageio
+import mujoco
+import numpy as np
+from PIL import Image, ImageDraw
+
+from .scene import IndoorSimulator
+from .vision import VisionPolicy, mask_command
+
+
+def rollout(assets, mode, weights, output, seed=7, seconds=45, record=False):
+ output = Path(output)
+ output.mkdir(parents=True, exist_ok=True)
+ sim = IndoorSimulator(assets,seed)
+ policy = VisionPolicy(Path(weights)/f'{mode}.pt', mode) if mode in ('perception','end2end') else None
+ trajectory, frames = [], []
+ overview = mujoco.Renderer(sim.model,height=240,width=320) if record else None
+ camera = mujoco.MjvCamera()
+ camera.azimuth, camera.elevation, camera.distance = 140, -65, 3.5
+ collision_steps = 0
+ previous = np.zeros(2)
+ start = time.time()
+ try:
+ for tick in range(int(seconds/.02)):
+ if tick%10 == 0:
+ rgb, mask = sim.image(labels=True)
+ command = policy.command(rgb)[0] if policy else mask_command(mask)
+ if tick<100:
+ command[:] = 0
+ previous = .6*previous+.4*command
+ sim.set_command(float(previous[0]),wz=float(previous[1]))
+ trajectory.append([float(sim.data.time), *sim.data.qpos[:3].tolist(), sim.yaw,
+ *previous.tolist(), *sim.odom.tolist()])
+ if overview and tick%20 == 0:
+ camera.lookat[:] = sim.data.qpos[:3]
+ overview.update_scene(sim.data,camera=camera)
+ frame = Image.fromarray(overview.render())
+ inset = Image.fromarray(rgb).resize((128,96))
+ frame.paste(inset,(192,0))
+ ImageDraw.Draw(frame).text((8,8),f'{mode} t={sim.data.time:.1f}s',fill='white')
+ frames.append(np.asarray(frame))
+ sim.step()
+ for contact in sim.data.contact:
+ names = [mujoco.mj_id2name(sim.model,mujoco.mjtObj.mjOBJ_GEOM,int(g)) or ''
+ for g in (contact.geom1,contact.geom2)]
+ if any(n.endswith('wall') or n.startswith('door_') or n=='cabinet' for n in names):
+ collision_steps += 1
+ break
+ if sim.fallen or sim.data.qpos[0]>=7.5:
+ break
+ result = dict(mode=mode,scene_seed=seed,simulated_seconds=float(sim.data.time),
+ wall_seconds=time.time()-start,fallen=bool(sim.fallen),
+ reached=bool(sim.data.qpos[0]>=7.5 and not sim.fallen),
+ final_position=sim.data.qpos[:3].tolist(),collision_steps=collision_steps,
+ odometry_error_m=float(np.linalg.norm(sim.odom[:2]-sim.data.qpos[:2])))
+ (output/f'{mode}_{seed}.json').write_text(json.dumps(result,indent=2),encoding='utf-8')
+ np.savetxt(output/f'{mode}_{seed}.csv',trajectory,delimiter=',',
+ header='time,x,y,z,yaw,vx,wz,odom_x,odom_y,odom_yaw',comments='')
+ if frames:
+ imageio.mimsave(output/f'{mode}_{seed}.gif',frames,duration=.4,loop=0)
+ Image.fromarray(frames[-1]).save(output/f'{mode}_{seed}.png')
+ return result
+ finally:
+ if overview:
+ overview.close()
+ sim.close()
+
+
+def teleop(assets,seed=0,seconds=45,output=None):
+ """Interactive MuJoCo viewer; arrow/WASD commands expire after 0.6s."""
+ import mujoco.viewer
+ sim = IndoorSimulator(assets,seed)
+ command = np.zeros(2)
+ speed = [.25]
+ last_key = [0.]
+ events = []
+ def key(code):
+ if code in (87,265): command[:] = [speed[0],0]
+ elif code in (83,264): command[:] = [-speed[0],0]
+ elif code in (65,263): command[:] = [0,.35]
+ elif code in (68,262): command[:] = [0,-.35]
+ elif code == 32: command[:] = 0
+ elif code in (61,334): speed[0] = min(.4,speed[0]+.05)
+ elif code in (45,333): speed[0] = max(.1,speed[0]-.05)
+ last_key[0] = time.monotonic()
+ events.append(dict(key=code,command=command.tolist(),time=float(sim.data.time)))
+ print('WASD/arrows: move, space: stop, +/-: speed; repeat presses to keep moving.',flush=True)
+ try:
+ with mujoco.viewer.launch_passive(sim.model,sim.data,key_callback=key) as viewer:
+ while viewer.is_running() and not sim.fallen and sim.data.time.6: command[:] = 0
+ sim.set_command(command[0],wz=command[1])
+ sim.step()
+ viewer.sync()
+ time.sleep(max(0,.02-(time.monotonic()-start)))
+ finally:
+ sim.set_command()
+ if output:
+ output=Path(output)
+ output.mkdir(parents=True,exist_ok=True)
+ (output/'teleop_check.json').write_text(json.dumps(dict(events=events,
+ final_position=sim.data.qpos[:3].tolist(),fallen=bool(sim.fallen),
+ simulated_seconds=float(sim.data.time)),indent=2),encoding='utf-8')
+ sim.close()
diff --git a/src/humanoid/g1_rgb_floor_patrol/patrol/scene.py b/src/humanoid/g1_rgb_floor_patrol/patrol/scene.py
new file mode 100644
index 0000000000..df750e8a32
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/patrol/scene.py
@@ -0,0 +1,112 @@
+"""Generate indoor geometry without changing the upstream robot assets."""
+from pathlib import Path
+import tempfile
+import xml.etree.ElementTree as ET
+
+import mujoco
+import numpy as np
+
+from .locomotion import Walker
+
+
+def build_scene(assets, seed=0):
+ asset_dir = Path(assets).resolve() / 'resources/robots/g1_description'
+ tree = ET.parse(asset_dir/'g1_12dof.xml')
+ root = tree.getroot()
+ root.find('compiler').set('meshdir', (asset_dir/'meshes').as_posix())
+ world = root.find('worldbody')
+ pelvis = world.find("body[@name='pelvis']")
+ for geom in pelvis.iter('geom'):
+ geom.set('group', '1')
+ ET.SubElement(pelvis, 'camera', name='front', pos='0.16 0 0.35',
+ xyaxes='0 -1 0 0.35 0 0.94', fovy='75')
+ visual = ET.SubElement(root, 'visual')
+ ET.SubElement(visual, 'global', offwidth='640', offheight='480')
+ ET.SubElement(visual, 'headlight', ambient='0.4 0.4 0.4', diffuse='0.6 0.6 0.6')
+ rng = np.random.default_rng(seed)
+ brightness = float(rng.uniform(.6, .95))
+ ET.SubElement(world, 'light', pos='3 0 4', diffuse=f'{brightness} {brightness} {brightness}')
+ ET.SubElement(world, 'geom', name='floor', type='plane', size='15 15 .1', rgba='.65 .65 .60 1')
+ boxes = [('left_wall', [4, 2, 1], [6, .12, 1]),
+ ('right_wall', [4, -2, 1], [6, .12, 1]),
+ ('back_wall', [-2, 0, 1], [.12, 2, 1]),
+ ('end_wall', [10, 0, 1], [.12, 2, 1]),
+ ('door_left', [5, 1.65, .9], [.12, .35, .9]),
+ ('door_right', [5, -1.65, .9], [.12, .35, .9]),
+ ('cabinet', [3, .95 + float(rng.uniform(-.15, .15)), .45], [.3, .3, .45])]
+ for name, pos, size in boxes:
+ rgba = [*rng.uniform(.25, .75, 3), 1]
+ ET.SubElement(world, 'geom', name=name, type='box',
+ pos=' '.join(map(str, pos)), size=' '.join(map(str, size)),
+ rgba=' '.join(map(str, rgba)))
+ ET.SubElement(world, 'geom', name='inspection_marker', type='box',
+ pos='9.84 0 1', size='.02 .35 .35', rgba='.1 .7 .8 1',
+ contype='0', conaffinity='0')
+ return ET.tostring(root, encoding='unicode')
+
+
+class IndoorSimulator(Walker):
+ def __init__(self, assets, seed=0):
+ # MuJoCo 3.2 parses an XML file; generated files remain outside the repo.
+ with tempfile.TemporaryDirectory(prefix='humanoid_scene_') as folder:
+ path = Path(folder)/'scene.xml'
+ path.write_text(build_scene(assets, seed), encoding='utf-8')
+ super().__init__(assets, path)
+ self.renderer = None
+ self.floor_id = self.model.geom('floor').id
+ self.rng = np.random.default_rng(seed)
+ self.odom = np.zeros(3)
+ self._last_yaw = self.yaw
+
+ def step(self):
+ super().step()
+ # Simulated velocity/gyro measurements, integrated independently of position.
+ dt = self.model.opt.timestep*self.config['control_decimation']
+ yaw = self.yaw
+ rotation = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]])
+ body_velocity = rotation @ self.data.qvel[:2]
+ body_velocity += self.rng.normal(0, .003, 2)
+ dyaw = (yaw-self._last_yaw+np.pi)%(2*np.pi)-np.pi
+ self._last_yaw = yaw
+ self.odom[2] += dyaw + self.rng.normal(0, .0002)
+ c, s = np.cos(self.odom[2]), np.sin(self.odom[2])
+ self.odom[:2] += np.array([[c, -s], [s, c]])@body_velocity*dt
+
+ def image(self, labels=False, depth=False):
+ if self.renderer is None:
+ self.renderer = mujoco.Renderer(self.model, height=96, width=128)
+ r = self.renderer
+ r.disable_segmentation_rendering()
+ r.disable_depth_rendering()
+ r.update_scene(self.data, camera='front')
+ rgb = r.render().copy()
+ if labels:
+ r.enable_segmentation_rendering()
+ segmentation = r.render().copy()
+ r.disable_segmentation_rendering()
+ return rgb, (segmentation[:, :, 0] == self.floor_id).astype(np.uint8)
+ if depth:
+ r.enable_depth_rendering()
+ result = r.render().copy()
+ r.disable_depth_rendering()
+ return rgb, result
+ return rgb
+
+ def scan(self, count=180):
+ origin = self.data.qpos[:3].copy()
+ origin[2] = .65
+ ranges = []
+ geom_id = np.zeros(1, dtype=np.int32)
+ # Only static geometry: no robot self returns. Angle frame is level base_link.
+ for angle in np.linspace(-np.pi, np.pi, count, endpoint=False):
+ a = angle + self.yaw
+ distance = mujoco.mj_ray(self.model, self.data, origin,
+ np.array([np.cos(a), np.sin(a), 0.]), np.array([1,0,0,0,0,0], dtype=np.uint8), 1,
+ self.model.body('pelvis').id, geom_id)
+ ranges.append(float(np.clip(distance if distance >= 0 else 8., .05, 8.)))
+ return ranges
+
+ def close(self):
+ if self.renderer:
+ self.renderer.close()
+ self.renderer = None
diff --git a/src/humanoid/g1_rgb_floor_patrol/patrol/vision.py b/src/humanoid/g1_rgb_floor_patrol/patrol/vision.py
new file mode 100644
index 0000000000..b0d706f474
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/patrol/vision.py
@@ -0,0 +1,89 @@
+"""Separate perception and direct-image control models."""
+import numpy as np
+import torch
+from torch import nn
+
+
+class FloorNet(nn.Module):
+ def __init__(self):
+ super().__init__()
+ self.net = nn.Sequential(nn.Conv2d(3, 16, 5, padding=2), nn.ReLU(),
+ nn.Conv2d(16, 16, 3, padding=1), nn.ReLU(), nn.Conv2d(16, 1, 1))
+
+ def forward(self, image):
+ return self.net(image)
+
+
+class ImageController(nn.Module):
+ def __init__(self):
+ super().__init__()
+ self.net = nn.Sequential(nn.Conv2d(3, 16, 5, stride=2, padding=2), nn.ReLU(),
+ nn.Conv2d(16, 32, 3, stride=2, padding=1), nn.ReLU(),
+ nn.Conv2d(32, 32, 3, stride=2, padding=1), nn.ReLU(),
+ nn.Flatten(), nn.Linear(32*12*16, 64), nn.ReLU(), nn.Linear(64, 2))
+
+ def forward(self, image):
+ return self.net(image)
+
+
+class MaskController(nn.Module):
+ """Learned velocity policy operating on a semantic floor mask."""
+ def __init__(self):
+ super().__init__()
+ self.net = nn.Sequential(nn.AdaptiveAvgPool2d((12,16)),nn.Flatten(),
+ nn.Linear(192,64),nn.ReLU(),nn.Linear(64,32),nn.ReLU(),nn.Linear(32,2))
+
+ def forward(self, mask):
+ return self.net(mask)
+
+
+def mask_command(mask):
+ """Reactive floor-following teacher, with no global route/position input.
+
+ Scores candidate image corridors; low front clearance reduces speed.
+ This is a heuristic teacher, not a globally optimal navigation planner.
+ """
+ h, w = mask.shape
+ band = mask[int(h*.48):int(h*.82)]
+ scores = np.array([band[:, max(0, x-10):min(w, x+11)].mean()
+ for x in range(12, w-12, 4)])
+ centers = np.arange(12, w-12, 4)
+ scores -= .20*np.abs(centers-w/2)/(w/2)
+ best = centers[np.argmax(scores)]
+ yaw = float(np.clip((w/2-best)/(w/2)*1.2, -.45, .45))
+ clearance = float(mask[int(h*.55):int(h*.85), int(w*.4):int(w*.6)].mean())
+ speed = .28*float(np.clip((clearance-.15)/.6, 0, 1))*(1-.6*abs(yaw))
+ return np.array([speed, yaw], dtype=np.float32)
+
+
+def tensor_image(image):
+ return torch.from_numpy(image.copy()).permute(2, 0, 1).float().unsqueeze(0)/255.
+
+
+class VisionPolicy:
+ def __init__(self, weights, mode):
+ self.mode = mode
+ self.net = FloorNet() if mode == 'perception' else ImageController()
+ self.net.load_state_dict(torch.load(weights, map_location='cpu', weights_only=True))
+ self.net.eval()
+ if mode == 'perception':
+ from pathlib import Path
+ self.controller = MaskController()
+ self.controller.load_state_dict(torch.load(Path(weights).with_name('mask_controller.pt'),
+ map_location='cpu',weights_only=True))
+ self.controller.eval()
+
+ def command(self, image):
+ with torch.inference_mode():
+ output = self.net(tensor_image(image))
+ if self.mode == 'perception':
+ mask = (output.sigmoid()[0, 0].numpy()>.5).astype(np.float32)
+ with torch.inference_mode():
+ command = self.controller(torch.from_numpy(mask)[None,None])[0].numpy()
+ # A learned controller can extrapolate a forward command even when
+ # segmentation detects no floor. Stop on this unambiguous failure.
+ if not mask.any():
+ command = np.zeros(2, dtype=np.float32)
+ return np.clip(command,[0,-.45],[.3,.45]), mask
+ command = output[0].numpy()
+ return np.clip(command, [0, -.45], [.3, .45]), None
diff --git a/src/humanoid/g1_rgb_floor_patrol/requirements.txt b/src/humanoid/g1_rgb_floor_patrol/requirements.txt
new file mode 100644
index 0000000000..f55b4da02e
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/requirements.txt
@@ -0,0 +1,6 @@
+mujoco==3.2.0
+numpy>=1.24
+torch>=2.0
+PyYAML>=6
+Pillow>=10
+imageio>=2.30
diff --git a/src/humanoid/g1_rgb_floor_patrol/scripts/fetch_assets.py b/src/humanoid/g1_rgb_floor_patrol/scripts/fetch_assets.py
new file mode 100755
index 0000000000..71f10aa86b
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/scripts/fetch_assets.py
@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+"""Fetch the exact upstream model/policy revision; never overwrite an existing path."""
+import argparse
+from pathlib import Path
+import subprocess
+
+REVISION = '276801e46c5d433564f24658bac64f254b7d2d4b'
+URL = 'https://github.com/unitreerobotics/unitree_rl_gym.git'
+
+
+def main():
+ parser = argparse.ArgumentParser(description=__doc__)
+ parser.add_argument('destination',type=Path)
+ args = parser.parse_args()
+ target = args.destination.resolve()
+ if target.exists():
+ raise SystemExit('Destination exists; use a new directory or validate the existing revision manually.')
+ subprocess.run(['git','clone','--filter=blob:none','--no-checkout',URL,str(target)],check=True)
+ subprocess.run(['git','-C',str(target),'sparse-checkout','set','deploy','resources/robots/g1_description'],check=True)
+ subprocess.run(['git','-C',str(target),'checkout','--detach',REVISION],check=True)
+ print(target)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/g1_rgb_floor_patrol/scripts/task_runner.py b/src/humanoid/g1_rgb_floor_patrol/scripts/task_runner.py
new file mode 100755
index 0000000000..c1112df063
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/scripts/task_runner.py
@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+"""Launch standalone simulation/learning tasks through a ROS launch file."""
+import subprocess
+import rospy
+
+
+def main():
+ rospy.init_node('humanoid_task_runner')
+ args = [rospy.get_param('~sim_python'),rospy.get_param('~sim_entry'),
+ '--assets',rospy.get_param('~assets'),'--mode',rospy.get_param('~task'),
+ '--weights',rospy.get_param('~weights'),'--output',rospy.get_param('~output'),
+ '--seconds',str(rospy.get_param('~seconds',45))]
+ child = subprocess.Popen(args)
+ try:
+ while child.poll() is None and not rospy.is_shutdown():
+ rospy.rostime.wallsleep(.1)
+ if child.poll() not in (None,0):
+ raise RuntimeError('Simulation task failed with exit code %s'%child.returncode)
+ finally:
+ if child.poll() is None:
+ child.terminate()
+ child.wait(timeout=10)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/g1_rgb_floor_patrol/third_party_license.md b/src/humanoid/g1_rgb_floor_patrol/third_party_license.md
new file mode 100644
index 0000000000..9ad8f6dbb0
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/third_party_license.md
@@ -0,0 +1,23 @@
+BSD 3-Clause License
+Copyright (c) 2016-2023 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+3. Neither the name of the copyright holder nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/humanoid/g1_rgb_floor_patrol/third_party_notices.md b/src/humanoid/g1_rgb_floor_patrol/third_party_notices.md
new file mode 100644
index 0000000000..01dd8c97e2
--- /dev/null
+++ b/src/humanoid/g1_rgb_floor_patrol/third_party_notices.md
@@ -0,0 +1,20 @@
+# Third-party sources
+
+The G1 MJCF/STL assets, pretrained `motion.pt`, policy observation convention,
+PD gains and action scaling originate from
+[Unitree RL Gym](https://github.com/unitreerobotics/unitree_rl_gym), revision
+`276801e46c5d433564f24658bac64f254b7d2d4b`, BSD-3-Clause.
+The upstream copyright and license are preserved in [third_party_license.md](third_party_license.md).
+Assets are fetched separately and are not duplicated into this module.
+
+`patrol/locomotion.py` adapts the upstream deployment equations and observation
+ordering. The pretrained walking policy is not an original training result.
+
+MuJoCo supplies physical simulation, rendering and ray queries. ROS Noetic
+gmapping and move_base supply SLAM and classical navigation. These components
+retain their respective upstream licenses; no claim of a new SLAM algorithm is made.
+
+FloorNet, MaskController, ImageController and CostNet, the training data generator, visual corridor
+controller, indoor scene generator, ROS stdio bridge and evaluation code were
+created for this project with AI assistance. Training weights under `models/`
+were produced by this module's training script.