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
48 changes: 48 additions & 0 deletions docs/humanoid/g1_costnet_grid_planner/index.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
# G1 CostNet 栅格规划对照

在独立栅格示例中比较 CostNet 辅助 A* 和曼哈顿 A* 的路径代价与搜索扩展数。

## 环境与运行

使用 Python 3.11。在模块目录运行 `python -m pip install -r requirements.txt`。

```bash
python main.py --output results
```

示例在一个合成栅格比较经典和神经辅助搜索,输出路线图和 JSON。网络仅处理相同 f 值的排序。可用 `--mode train_planner` 重训,用 `--mode plan_stdio` 接收占据栅格 JSON。

## ROS 1 Noetic 启动

本模块的 ROS 接口使用 ROS 1 Noetic(catkin、rospy)。在 Noetic 源码空间运行,将路径替换为实际值。

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

## 运行效果

![G1 CostNet 栅格规划对照](../../img/g1_costnet_grid_planner/planner.png)

## 可复核结果

| 方法或指标 | 路径代价(格) | 扩展节点 |
| --- | ---: | ---: |
| Manhattan A* | 48 | 401 |
| 随仓库提供的 CostNet 辅助 A* | 48 | 49 |
| 40 张合成测试图的 CostNet MAE | 0.756 | — |

扩展数仅对应演示地图。推理按栅格各轴尺寸归一化相对坐标,各轴尺度至少为 32,以保持 32×32 训练输入及已有权重兼容。大图的搜索扩展数会随布局变化。

```bash
python main.py --mode train_planner --epochs 5 --samples 100 --seed 91 --weights results/trained
python main.py --mode plan_stdio --robot-radius 0.3 --stride 3 < request.json
```

请求 JSON 包含 `map`(行优先的一维占据数组)、`width`、`height`、`resolution`(米/格)、`origin`、`start`、`goal`(二维世界坐标)。可用 `robot_radius`、`stride` 字段覆盖命令行默认值。重复在同一 Python 进程调用时复用权重,文件更新后重新加载。

## 大模型使用声明

使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
Binary file added docs/img/g1_costnet_grid_planner/planner.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 @@ -99,6 +99,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c
* [人形上下台阶步态仿真](./humanoid/humanoid_stair_climb.md)
* [ROS虚拟机器人圆形轨迹rviz可视化示例](./humanoid/ros_topic_demo/index.md)
* [人形机器人全身逆运动学与可操作性](./humanoid/whole_body_ik.md)
* [G1 CostNet 栅格规划对照](./humanoid/g1_costnet_grid_planner/index.md)
* [人形机器人双臂末端可达域分析](./humanoid/humanoid_reachability.md)
* [人形机器人跌倒预判与保护性倒地](./humanoid/humanoid_fall_protection.md)
* [人形机器人 3D 碰撞体边界检测可视化](./humanoid/humanoid_collision_boundary.md)
Expand Down
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ nav:
- ROS虚拟机器人圆形轨迹rviz可视化示例: './humanoid/ros_topic_demo/index.md'
- ROS2 Humble 路径点发布器: './waypoint_publisher.md'
- 人形机器人全身逆运动学与可操作性: './humanoid/whole_body_ik.md'
- G1 CostNet 栅格规划对照: './humanoid/g1_costnet_grid_planner/index.md'
- 人形机器人双臂末端可达域分析: './humanoid/humanoid_reachability.md'
- 人形机器人跌倒预判与保护性倒地: './humanoid/humanoid_fall_protection.md'
- 人形机器人 3D 碰撞体边界检测可视化: './humanoid/humanoid_collision_boundary.md'
Expand Down
2 changes: 2 additions & 0 deletions src/humanoid/g1_costnet_grid_planner/.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_costnet_grid_planner/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_costnet_grid_planner)
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})
44 changes: 44 additions & 0 deletions src/humanoid/g1_costnet_grid_planner/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
# G1 CostNet 栅格规划对照

在独立栅格示例中比较 CostNet 辅助 A* 和曼哈顿 A* 的路径代价与搜索扩展数。

## 环境与运行

使用 Python 3.11。在模块目录运行 `python -m pip install -r requirements.txt`。

```bash
python main.py --output results
```

示例在一个合成栅格比较经典和神经辅助搜索,输出路线图和 JSON。网络仅处理相同 f 值的排序。可用 `--mode train_planner` 重训,用 `--mode plan_stdio` 接收占据栅格 JSON。

## ROS 1 Noetic 启动

本模块的 ROS 接口使用 ROS 1 Noetic(catkin、rospy)。在 Noetic 源码空间运行,将路径替换为实际值。

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

## 可复核结果

| 方法或指标 | 路径代价(格) | 扩展节点 |
| --- | ---: | ---: |
| Manhattan A* | 48 | 401 |
| 随仓库提供的 CostNet 辅助 A* | 48 | 49 |
| 40 张合成测试图的 CostNet MAE | 0.756 | — |

扩展数仅对应演示地图。推理按栅格各轴尺寸归一化相对坐标,各轴尺度至少为 32,以保持 32×32 训练输入及已有权重兼容。大图的搜索扩展数会随布局变化。

```bash
python main.py --mode train_planner --epochs 5 --samples 100 --seed 91 --weights results/trained
python main.py --mode plan_stdio --robot-radius 0.3 --stride 3 < request.json
```

请求 JSON 包含 `map`(行优先的一维占据数组)、`width`、`height`、`resolution`(米/格)、`origin`、`start`、`goal`(二维世界坐标)。可用 `robot_radius`、`stride` 字段覆盖命令行默认值。重复在同一 Python 进程调用时复用权重,文件更新后重新加载。

## 大模型使用声明

使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
10 changes: 10 additions & 0 deletions src/humanoid/g1_costnet_grid_planner/launch/main.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
<launch>
<arg name="sim_python"/>
<arg name="sim_entry"/>
<arg name="weights" default="$(find g1_costnet_grid_planner)/models"/>
<arg name="output" default="results"/>
<node pkg="g1_costnet_grid_planner" 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="weights" value="$(arg weights)"/><param name="output" value="$(arg output)"/>
</node>
</launch>
35 changes: 35 additions & 0 deletions src/humanoid/g1_costnet_grid_planner/main.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
#!/usr/bin/env python3
"""在独立栅格示例中比较 CostNet 辅助 A* 和曼哈顿 A* 的路径代价与搜索扩展数。"""
import argparse
import json
from pathlib import Path


def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--mode', choices=['demo', 'train_planner', 'plan_stdio'], default='demo')
parser.add_argument('--output', type=Path)
parser.add_argument('--weights', type=Path, default=Path(__file__).resolve().parent / 'models')
parser.add_argument('--samples', type=int, default=240)
parser.add_argument('--epochs', type=int, default=20)
parser.add_argument('--seed', type=int, default=91)
parser.add_argument('--robot-radius', type=float, default=0.3)
parser.add_argument('--stride', type=int, default=3)
args = parser.parse_args()
if args.mode == 'train_planner' and (args.samples < 1 or args.epochs < 1 or args.seed < 0):
parser.error('training requires positive samples and epochs, and a nonnegative seed')
if args.mode == 'train_planner':
from patrol.planning import train_planner
print(json.dumps(train_planner(args.weights, epochs=args.epochs, sample_count=args.samples, seed=args.seed),indent=2))
return
if args.mode == 'plan_stdio':
import sys
from patrol.planning import plan_request
print(json.dumps(plan_request(json.load(sys.stdin),args.weights, robot_radius=args.robot_radius, stride=args.stride)))
return
from patrol.demo import run
print(json.dumps(run(args.weights, args.output or Path("results")), indent=2))


if __name__ == '__main__':
main()
Binary file not shown.
9 changes: 9 additions & 0 deletions src/humanoid/g1_costnet_grid_planner/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_costnet_grid_planner</name><version>0.1.0</version>
<description>MuJoCo G1 visual floor perception, image control and ROS indoor navigation.</description>
<maintainer email="usernamekkyy@users.noreply.github.com">usernamekkyy</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_costnet_grid_planner/patrol/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
"""Visual patrol on a physics simulated G1 humanoid."""
43 changes: 43 additions & 0 deletions src/humanoid/g1_costnet_grid_planner/patrol/demo.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
import json

import numpy as np
from PIL import Image, ImageDraw

from .planning import astar, predict_costs


def run(weights, output):
grid = np.zeros((32, 32), dtype=bool)
grid[5:26, 10:13] = True
grid[8:29, 21:24] = True
start, goal = (27, 3), (4, 28)
costs = predict_costs(grid, goal, weights)
baseline, baseline_expanded = astar(grid, start, goal)
assisted, assisted_expanded = astar(grid, start, goal, costs)
if len(assisted) != len(baseline):
raise RuntimeError('Search methods disagree on optimal path cost')
output.mkdir(parents=True, exist_ok=True)
report = {
'path_cost': len(assisted) - 1,
'baseline_expanded': baseline_expanded,
'assisted_expanded': assisted_expanded,
'baseline_route': baseline,
'assisted_route': assisted,
}
(output / 'planner.json').write_text(json.dumps(report, indent=2), encoding='utf-8')
image = Image.new('RGB', (640, 355), 'white')
draw = ImageDraw.Draw(image)
for panel, (route, label) in enumerate((
(baseline, f'Manhattan A* ({baseline_expanded} expanded)'),
(assisted, f'CostNet A* ({assisted_expanded} expanded)'),
)):
left = 16 + 320 * panel
draw.text((left, 9), label, fill='#13202a')
for y, x in np.argwhere(grid):
draw.rectangle((left+x*9, 30+y*9, left+x*9+8, 30+y*9+8), fill='#333b42')
draw.line([(left+x*9+4, 34+y*9) for y,x in route], fill='#087a61', width=3)
for y, x in (start, goal):
draw.ellipse((left+x*9, 30+y*9, left+x*9+8, 30+y*9+8), fill='#b04827')
draw.text((16, 330), f'Path cost: {report["path_cost"]} cells for both methods', fill='#13202a')
image.save(output / 'planner.png')
return report
Loading
Loading