Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions docs/humanoid/g1_rgb_velocity_patrol/index.md
Original file line number Diff line number Diff line change
@@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
Binary file added docs/img/g1_rgb_velocity_patrol/end2end_7.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 1 addition & 0 deletions docs/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
3 changes: 3 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/.gitattributes
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
*.py text eol=lf
*.sh text eol=lf
*.pt binary -filter
6 changes: 6 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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})
31 changes: 31 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/README.md
Original file line number Diff line number Diff line change
@@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
17 changes: 17 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/launch/main.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<launch>
<arg name="sim_python"/>
<arg name="sim_entry"/>
<arg name="assets"/>
<arg name="seed" default="7"/>
<arg name="real_time_factor" default="1.0"/>
<arg name="task" default="end2end"/>
<arg name="weights" default="$(find g1_rgb_velocity_patrol)/models"/>
<arg name="output" default="results"/>
<arg name="seconds" default="45"/>
<node pkg="g1_rgb_velocity_patrol" type="task_runner.py" name="humanoid_task_runner" output="screen" required="true">
<param name="sim_python" value="$(arg sim_python)"/><param name="sim_entry" value="$(arg sim_entry)"/>
<param name="assets" value="$(arg assets)"/><param name="task" value="$(arg task)"/>
<param name="weights" value="$(arg weights)"/><param name="output" value="$(arg output)"/>
<param name="seconds" value="$(arg seconds)"/>
</node>
</launch>
39 changes: 39 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/main.py
Original file line number Diff line number Diff line change
@@ -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()
Binary file not shown.
Binary file not shown.
9 changes: 9 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<package format="2">
<name>g1_rgb_velocity_patrol</name><version>0.1.0</version>
<description>MuJoCo G1 visual floor perception, image control and ROS indoor navigation.</description>
<maintainer email="maintainer@example.com">OpenHUTB contributor</maintainer>
<license>BSD-3-Clause</license>
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>rospy</exec_depend>
</package>
1 change: 1 addition & 0 deletions src/humanoid/g1_rgb_velocity_patrol/patrol/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
"""Visual patrol on a physics simulated G1 humanoid."""
98 changes: 98 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/patrol/learning.py
Original file line number Diff line number Diff line change
@@ -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
85 changes: 85 additions & 0 deletions src/humanoid/g1_rgb_velocity_patrol/patrol/locomotion.py
Original file line number Diff line number Diff line change
@@ -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())
Loading
Loading