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 +``` + +## 运行效果 + +![G1 RGB 地面分割巡检](../../img/g1_rgb_floor_patrol/perception_7.gif) + +## 大模型使用声明 + +使用 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.