diff --git a/docs/humanoid/g1_rgb_velocity_patrol/index.md b/docs/humanoid/g1_rgb_velocity_patrol/index.md new file mode 100644 index 0000000000..7eff55cbb7 --- /dev/null +++ b/docs/humanoid/g1_rgb_velocity_patrol/index.md @@ -0,0 +1,35 @@ +# 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 +``` + +默认 `end2end` 模式直接读取模块自带的 `models/end2end.pt`(1,637,701 字节),拿到仓库和固定版本的 Unitree 仿真资产后即可运行。该模式只加载图像速度模型。`models/mask_controller.pt`(61,173 字节)提供给 `vision.py` 的感知分支复用。 + +闭环默认使用留出种子 7。运行 `python scripts/evaluate_near_obstacle.py --help` 查看近墙评估参数。策略输出高层速度,由预训练步态执行。 + +## ROS launch + +在 ROS Noetic 源码空间运行,将路径替换为实际值。 + +```bash +source /opt/ros/noetic/setup.bash +export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}" +roslaunch g1_rgb_velocity_patrol main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym +``` + +## 运行效果 + +![G1 图像速度巡检](../../img/g1_rgb_velocity_patrol/end2end_7.gif) + +## 大模型使用声明 + +使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。 diff --git a/docs/img/g1_rgb_velocity_patrol/end2end_7.gif b/docs/img/g1_rgb_velocity_patrol/end2end_7.gif new file mode 100644 index 0000000000..a12b32683f Binary files /dev/null and b/docs/img/g1_rgb_velocity_patrol/end2end_7.gif differ diff --git a/docs/index.md b/docs/index.md index 9f6f16ff53..1f92796ea7 100644 --- a/docs/index.md +++ b/docs/index.md @@ -93,6 +93,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c * [人形机器人平衡控制与外力抗扰恢复](./humanoid/humanoid_push_recovery.md) * [人形机器人 PPO 神经网络平衡控制](./humanoid/humanoid_ppo_locomotion.md) * [人形机器人 2D 激光雷达 CNN 感知与避障](./humanoid/humanoid_lidar_nn_perception.md) +* [G1 图像速度巡检](./humanoid/g1_rgb_velocity_patrol/index.md) * [人形足迹下的局部避障算法对比](./humanoid/humanoid_local_planner_bench.md) * [MuJoCo单足弹跳机器人简单仿真](./humanoid/mujoco_hopper_demo/index.md) * [人形机器人关节轨迹录制与回放工具](./humanoid/humanoid_joint_tools.md) diff --git a/mkdocs.yml b/mkdocs.yml index edf15e5906..3dae94ccb5 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -89,6 +89,7 @@ nav: - 人形机器人平衡控制与外力抗扰恢复: './humanoid/humanoid_push_recovery.md' - 人形机器人 PPO 神经网络平衡控制: './humanoid/humanoid_ppo_locomotion.md' - 人形机器人 2D 激光雷达 CNN 感知与避障: './humanoid/humanoid_lidar_nn_perception.md' + - G1 图像速度巡检: './humanoid/g1_rgb_velocity_patrol/index.md' - 人形足迹下的局部避障算法对比: './humanoid/humanoid_local_planner_bench.md' - 人形机器人 H1/H1_2 神经网络行走控制: './humanoid/humanoid_h1_walk.md' - MuJoCo单足弹跳机器人简单仿真: './humanoid/mujoco_hopper_demo/index.md' diff --git a/src/humanoid/g1_rgb_velocity_patrol/.gitattributes b/src/humanoid/g1_rgb_velocity_patrol/.gitattributes new file mode 100644 index 0000000000..723794f831 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/.gitattributes @@ -0,0 +1,3 @@ +*.py text eol=lf +*.sh text eol=lf +*.pt binary -filter diff --git a/src/humanoid/g1_rgb_velocity_patrol/CMakeLists.txt b/src/humanoid/g1_rgb_velocity_patrol/CMakeLists.txt new file mode 100644 index 0000000000..3df6cba50c --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.0.2) +project(g1_rgb_velocity_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_velocity_patrol/README.md b/src/humanoid/g1_rgb_velocity_patrol/README.md new file mode 100644 index 0000000000..aa09cece79 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/README.md @@ -0,0 +1,31 @@ +# 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 +``` + +默认 `end2end` 模式直接读取模块自带的 `models/end2end.pt`(1,637,701 字节),拿到仓库和固定版本的 Unitree 仿真资产后即可运行。该模式只加载图像速度模型。`models/mask_controller.pt`(61,173 字节)提供给 `vision.py` 的感知分支复用。 + +闭环默认使用留出种子 7。运行 `python scripts/evaluate_near_obstacle.py --help` 查看近墙评估参数。策略输出高层速度,由预训练步态执行。 + +## ROS launch + +在 ROS Noetic 源码空间运行,将路径替换为实际值。 + +```bash +source /opt/ros/noetic/setup.bash +export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}" +roslaunch g1_rgb_velocity_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_velocity_patrol/launch/main.launch b/src/humanoid/g1_rgb_velocity_patrol/launch/main.launch new file mode 100644 index 0000000000..eaabd43ab8 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/launch/main.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/humanoid/g1_rgb_velocity_patrol/main.py b/src/humanoid/g1_rgb_velocity_patrol/main.py new file mode 100644 index 0000000000..0ac2eba6f5 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/main.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +"""验证直接由 RGB 图像预测速度的 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=['end2end', 'collect', 'train', 'teacher'], default='end2end') + 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(args.dataset, args.weights, args.epochs) + 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_velocity_patrol/models/end2end.pt b/src/humanoid/g1_rgb_velocity_patrol/models/end2end.pt new file mode 100644 index 0000000000..871eac4fb1 Binary files /dev/null and b/src/humanoid/g1_rgb_velocity_patrol/models/end2end.pt differ diff --git a/src/humanoid/g1_rgb_velocity_patrol/models/mask_controller.pt b/src/humanoid/g1_rgb_velocity_patrol/models/mask_controller.pt new file mode 100644 index 0000000000..5bcc069e20 Binary files /dev/null and b/src/humanoid/g1_rgb_velocity_patrol/models/mask_controller.pt differ diff --git a/src/humanoid/g1_rgb_velocity_patrol/package.xml b/src/humanoid/g1_rgb_velocity_patrol/package.xml new file mode 100644 index 0000000000..6574056aa3 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/package.xml @@ -0,0 +1,9 @@ + + + g1_rgb_velocity_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_velocity_patrol/patrol/__init__.py b/src/humanoid/g1_rgb_velocity_patrol/patrol/__init__.py new file mode 100644 index 0000000000..bc4204b4fb --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/patrol/__init__.py @@ -0,0 +1 @@ +"""Visual patrol on a physics simulated G1 humanoid.""" diff --git a/src/humanoid/g1_rgb_velocity_patrol/patrol/learning.py b/src/humanoid/g1_rgb_velocity_patrol/patrol/learning.py new file mode 100644 index 0000000000..ff53e0690c --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/patrol/learning.py @@ -0,0 +1,98 @@ +"""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 [('end2end', ImageController(), 2)]: + 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 diff --git a/src/humanoid/g1_rgb_velocity_patrol/patrol/locomotion.py b/src/humanoid/g1_rgb_velocity_patrol/patrol/locomotion.py new file mode 100644 index 0000000000..d072059b3d --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/patrol/locomotion.py @@ -0,0 +1,85 @@ +"""G1 velocity-command adapter using Unitree's pretrained locomotion policy. + +Observation ordering and PD gains follow unitree_rl_gym/deploy/deploy_mujoco +(BSD-3-Clause). This module does not train the underlying walking policy. +""" +from pathlib import Path +import math + +import mujoco +import numpy as np +import torch +import yaml + + +class Walker: + def __init__(self, assets, xml_path=None): + assets = Path(assets).resolve() + config_path = assets / 'deploy/deploy_mujoco/configs/g1.yaml' + self.config = yaml.safe_load(config_path.read_text(encoding='utf-8')) + def resolve(value): + return value.replace('{LEGGED_GYM_ROOT_DIR}', assets.as_posix()) + self.model = mujoco.MjModel.from_xml_path(str(xml_path or resolve(self.config['xml_path']))) + self.data = mujoco.MjData(self.model) + self.model.opt.timestep = self.config['simulation_dt'] + torch.set_num_threads(2) + self.policy = torch.jit.load(resolve(self.config['policy_path']), map_location='cpu').eval() + self.default = np.asarray(self.config['default_angles'], dtype=np.float32) + self.kp = np.asarray(self.config['kps']) + self.kd = np.asarray(self.config['kds']) + self.command = np.zeros(3, dtype=np.float32) + self.action = np.zeros(self.config['num_actions'], dtype=np.float32) + self.target = self.default.copy() + self.counter = 0 + mujoco.mj_forward(self.model, self.data) + + def set_command(self, vx=0.0, vy=0.0, wz=0.0): + value = np.array([vx, vy, wz], dtype=np.float32) + if not np.isfinite(value).all(): + raise ValueError('Command must be finite') + self.command[:] = np.clip(value, [-0.3, -0.2, -0.5], [0.5, 0.2, 0.5]) + + @property + def yaw(self): + w, x, y, z = self.data.qpos[3:7] + return math.atan2(2*(w*z+x*y), 1-2*(y*y+z*z)) + + @property + def fallen(self): + return self.data.qpos[2] < 0.40 or not np.isfinite(self.data.qpos).all() + + def step(self): + c, d = self.config, self.data + for _ in range(c['control_decimation']): + d.ctrl[:] = (self.target-d.qpos[7:])*self.kp-d.qvel[6:]*self.kd + mujoco.mj_step(self.model, d) + self.counter += 1 + w, x, y, z = d.qpos[3:7] + gravity = np.array([2*(-z*x+w*y), -2*(z*y+w*x), 1-2*(w*w+z*z)]) + phase = 2*np.pi*(d.time % 0.8)/0.8 + observation = np.concatenate([ + d.qvel[3:6]*c['ang_vel_scale'], gravity, + self.command*np.asarray(c['cmd_scale']), + (d.qpos[7:]-self.default)*c['dof_pos_scale'], + d.qvel[6:]*c['dof_vel_scale'], self.action, + [np.sin(phase), np.cos(phase)], + ]).astype(np.float32) + with torch.inference_mode(): + self.action = self.policy(torch.from_numpy(observation)[None]).numpy().reshape(-1) + self.target = self.action*c['action_scale']+self.default + + def smoke_test(self, seconds=12.0): + start = self.data.qpos[:3].copy() + min_height = float(start[2]) + while self.data.time < seconds: + t = self.data.time + self.set_command(0.0 if t < 2 or t >= 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_velocity_patrol/patrol/runtime.py b/src/humanoid/g1_rgb_velocity_patrol/patrol/runtime.py new file mode 100644 index 0000000000..7f381f483b --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/patrol/scene.py b/src/humanoid/g1_rgb_velocity_patrol/patrol/scene.py new file mode 100644 index 0000000000..df750e8a32 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/patrol/vision.py b/src/humanoid/g1_rgb_velocity_patrol/patrol/vision.py new file mode 100644 index 0000000000..b0d706f474 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/requirements.txt b/src/humanoid/g1_rgb_velocity_patrol/requirements.txt new file mode 100644 index 0000000000..f55b4da02e --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/scripts/evaluate_near_obstacle.py b/src/humanoid/g1_rgb_velocity_patrol/scripts/evaluate_near_obstacle.py new file mode 100755 index 0000000000..3759f3493a --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_patrol/scripts/evaluate_near_obstacle.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +"""Measure the deployed image controller near the end wall in MuJoCo.""" +import argparse +import json +from pathlib import Path +import sys + +import mujoco +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) +from patrol.scene import IndoorSimulator +from patrol.vision import VisionPolicy + + +def run(assets, weights, distance): + sim = IndoorSimulator(assets, 7) + try: + # End wall's near surface is x=9.88. Distance is pelvis to wall, + # not foot or body clearance. Settle before enabling the policy. + sim.data.qpos[0] = 9.88 - distance + mujoco.mj_forward(sim.model, sim.data) + policy = VisionPolicy(Path(weights) / 'end2end.pt', 'end2end') + for _ in range(100): + sim.set_command(0, 0, 0) + sim.step() + if sim.fallen: + return dict(distance_m=distance, settled=False, fallen=True) + start = sim.data.qpos[:3].copy() + commands = [] + contacts = 0 + previous = np.zeros(2) + for tick in range(150): + if tick % 10 == 0: + command, _ = policy.command(sim.image()) + previous = .6 * previous + .4 * command + sim.set_command(float(previous[0]), wz=float(previous[1])) + commands.append(command.tolist()) + 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 'end_wall' in names: + contacts += 1 + break + if sim.fallen: + break + return dict(distance_m=distance, settled=True, start_position=start.tolist(), + final_position=sim.data.qpos[:3].tolist(), fallen=bool(sim.fallen), + wall_contact_steps=contacts, commands=commands) + finally: + sim.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--assets', required=True) + parser.add_argument('--weights', default=str(Path(__file__).resolve().parents[1] / 'models')) + parser.add_argument('--output', required=True) + args = parser.parse_args() + results = [run(args.assets, args.weights, d) for d in (1.5, 1.0, .6)] + Path(args.output).write_text(json.dumps(results, indent=2) + '\n', encoding='utf-8') + print(json.dumps(results, indent=2)) diff --git a/src/humanoid/g1_rgb_velocity_patrol/scripts/fetch_assets.py b/src/humanoid/g1_rgb_velocity_patrol/scripts/fetch_assets.py new file mode 100755 index 0000000000..71f10aa86b --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/scripts/task_runner.py b/src/humanoid/g1_rgb_velocity_patrol/scripts/task_runner.py new file mode 100755 index 0000000000..c1112df063 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/third_party_license.md b/src/humanoid/g1_rgb_velocity_patrol/third_party_license.md new file mode 100644 index 0000000000..9ad8f6dbb0 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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_velocity_patrol/third_party_notices.md b/src/humanoid/g1_rgb_velocity_patrol/third_party_notices.md new file mode 100644 index 0000000000..01dd8c97e2 --- /dev/null +++ b/src/humanoid/g1_rgb_velocity_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.