Skip to content
Open
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_corridor_teleop/index.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
# G1 室内走廊键盘遥控

通过键盘复现 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
```

使用 WASD 或方向键前进、后退和转向,空格停车,+/- 调速。持续按键维持命令,0.6 秒无按键后速度归零。无窗口时可运行 `--mode smoke` 验证步态。

## ROS launch

在 ROS Noetic 源码空间运行,将路径替换为实际值。

```bash
source /opt/ros/noetic/setup.bash
export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}"
roslaunch g1_corridor_teleop main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym
```

## 运行效果

![G1 室内走廊键盘遥控](../../img/g1_corridor_teleop/teleop.png)

## 大模型使用声明

使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
Binary file added docs/img/g1_corridor_teleop/teleop.png
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 @@ -86,6 +86,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c
## 2. 人形机器人 <span id='humanoid'></span>

* [人形机器人基础](./humanoid/humanoid_basics.md)
* [G1 室内走廊键盘遥控](./humanoid/g1_corridor_teleop/index.md)
* [人形机器人双臂搬运负载的神经网络平衡控制](./humanoid/humanoid_carry_payload_nn.md)
* [双足步态生成与 ZMP 稳定性](./humanoid/bipedal_gait_zmp.md)
* [基于三次样条插值的双足多步态平滑切换](./humanoid/biped_cubic_spline_gait_switch.md)
Expand Down
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ nav:

- 人形机器人:
- 人形机器人基础: './humanoid/humanoid_basics.md'
- G1 室内走廊键盘遥控: './humanoid/g1_corridor_teleop/index.md'
- 双臂搬运负载的神经网络平衡控制: './humanoid/humanoid_carry_payload_nn.md'
- 双足步态生成与 ZMP 稳定性: './humanoid/bipedal_gait_zmp.md'
- 基于三次样条插值的双足多步态平滑切换: './humanoid/biped_cubic_spline_gait_switch.md'
Expand Down
2 changes: 2 additions & 0 deletions src/humanoid/g1_corridor_teleop/.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_corridor_teleop/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_corridor_teleop)
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_corridor_teleop/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
# G1 室内走廊键盘遥控

通过键盘复现 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
```

使用 WASD 或方向键前进、后退和转向,空格停车,+/- 调速。持续按键维持命令,0.6 秒无按键后速度归零。无窗口时可运行 `--mode smoke` 验证步态。

## ROS launch

在 ROS Noetic 源码空间运行,将路径替换为实际值。

```bash
source /opt/ros/noetic/setup.bash
export ROS_PACKAGE_PATH="$PWD:${ROS_PACKAGE_PATH:-}"
roslaunch g1_corridor_teleop 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_corridor_teleop/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="teleop"/>
<arg name="weights" default="$(find g1_corridor_teleop)/models"/>
<arg name="output" default="results"/>
<arg name="seconds" default="45"/>
<node pkg="g1_corridor_teleop" 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>
38 changes: 38 additions & 0 deletions src/humanoid/g1_corridor_teleop/main.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
#!/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=['teleop', 'smoke'], default='teleop')
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 == 'teleop':
from patrol.runtime import teleop
teleop(args.assets,args.seed,args.seconds,args.output)
return
from patrol.locomotion import Walker
result = Walker(args.assets).smoke_test()
report = json.dumps(result, indent=2)
print(report)
if args.output:
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(report+'\n', encoding='utf-8')
if result['fallen'] or result['displacement_m'] < 0.5:
raise SystemExit(1)


if __name__ == '__main__':
main()
9 changes: 9 additions & 0 deletions src/humanoid/g1_corridor_teleop/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_corridor_teleop</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_corridor_teleop/patrol/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
"""Visual patrol on a physics simulated G1 humanoid."""
85 changes: 85 additions & 0 deletions src/humanoid/g1_corridor_teleop/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())
46 changes: 46 additions & 0 deletions src/humanoid/g1_corridor_teleop/patrol/runtime.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
import json
from pathlib import Path
import time

import numpy as np

from .scene import IndoorSimulator


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<seconds:
start = time.monotonic()
if time.monotonic()-last_key[0]>.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()
Loading
Loading