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
33 changes: 33 additions & 0 deletions docs/humanoid/g1_rgb_floor_patrol/index.md
Original file line number Diff line number Diff line change
@@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
Binary file added docs/img/g1_rgb_floor_patrol/perception_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 @@ -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)
Expand Down
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
2 changes: 2 additions & 0 deletions src/humanoid/g1_rgb_floor_patrol/.gitattributes
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
*.py text eol=lf
*.sh text eol=lf
6 changes: 6 additions & 0 deletions src/humanoid/g1_rgb_floor_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_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})
29 changes: 29 additions & 0 deletions src/humanoid/g1_rgb_floor_patrol/README.md
Original file line number Diff line number Diff line change
@@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
17 changes: 17 additions & 0 deletions src/humanoid/g1_rgb_floor_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="perception"/>
<arg name="weights" default="$(find g1_rgb_floor_patrol)/models"/>
<arg name="output" default="results"/>
<arg name="seconds" default="45"/>
<node pkg="g1_rgb_floor_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>
40 changes: 40 additions & 0 deletions src/humanoid/g1_rgb_floor_patrol/main.py
Original file line number Diff line number Diff line change
@@ -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()
Binary file not shown.
Binary file not shown.
9 changes: 9 additions & 0 deletions src/humanoid/g1_rgb_floor_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_floor_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_floor_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."""
134 changes: 134 additions & 0 deletions src/humanoid/g1_rgb_floor_patrol/patrol/learning.py
Original file line number Diff line number Diff line change
@@ -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<best:
best=val
torch.save(model.state_dict(),output/'mask_controller.pt')
model.load_state_dict(torch.load(output/'mask_controller.pt',weights_only=True))
with torch.inference_mode():
mae=(model(xt)-yt).abs().mean(0).tolist()
metrics=json.loads((output/'metrics.json').read_text())
metrics['mask_controller']=dict(epochs=epochs,validation_mse=best,
test_speed_mae_mps=mae[0],test_yaw_rate_mae_radps=mae[1])
(output/'metrics.json').write_text(json.dumps(metrics,indent=2),encoding='utf-8')
print(metrics['mask_controller'],flush=True)
85 changes: 85 additions & 0 deletions src/humanoid/g1_rgb_floor_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