diff --git a/docs/humanoid/g1_costnet_grid_planner/index.md b/docs/humanoid/g1_costnet_grid_planner/index.md new file mode 100644 index 0000000000..c4d14643e7 --- /dev/null +++ b/docs/humanoid/g1_costnet_grid_planner/index.md @@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。 diff --git a/docs/img/g1_costnet_grid_planner/planner.png b/docs/img/g1_costnet_grid_planner/planner.png new file mode 100644 index 0000000000..36c4c600f4 Binary files /dev/null and b/docs/img/g1_costnet_grid_planner/planner.png differ diff --git a/docs/index.md b/docs/index.md index 9f6f16ff53..4551cacf5a 100644 --- a/docs/index.md +++ b/docs/index.md @@ -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) diff --git a/mkdocs.yml b/mkdocs.yml index edf15e5906..dabd97e228 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -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' diff --git a/src/humanoid/g1_costnet_grid_planner/.gitattributes b/src/humanoid/g1_costnet_grid_planner/.gitattributes new file mode 100644 index 0000000000..f13e51c0ec --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/.gitattributes @@ -0,0 +1,2 @@ +*.py text eol=lf +*.sh text eol=lf diff --git a/src/humanoid/g1_costnet_grid_planner/CMakeLists.txt b/src/humanoid/g1_costnet_grid_planner/CMakeLists.txt new file mode 100644 index 0000000000..084cb87281 --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/CMakeLists.txt @@ -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}) diff --git a/src/humanoid/g1_costnet_grid_planner/README.md b/src/humanoid/g1_costnet_grid_planner/README.md new file mode 100644 index 0000000000..77510e61b3 --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/README.md @@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。 diff --git a/src/humanoid/g1_costnet_grid_planner/launch/main.launch b/src/humanoid/g1_costnet_grid_planner/launch/main.launch new file mode 100644 index 0000000000..9adf983528 --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/launch/main.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/humanoid/g1_costnet_grid_planner/main.py b/src/humanoid/g1_costnet_grid_planner/main.py new file mode 100644 index 0000000000..981f457f0a --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/main.py @@ -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() diff --git a/src/humanoid/g1_costnet_grid_planner/models/planner.pt b/src/humanoid/g1_costnet_grid_planner/models/planner.pt new file mode 100644 index 0000000000..97c8a1cad3 Binary files /dev/null and b/src/humanoid/g1_costnet_grid_planner/models/planner.pt differ diff --git a/src/humanoid/g1_costnet_grid_planner/package.xml b/src/humanoid/g1_costnet_grid_planner/package.xml new file mode 100644 index 0000000000..92c0df4c7b --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/package.xml @@ -0,0 +1,9 @@ + + + g1_costnet_grid_planner0.1.0 + MuJoCo G1 visual floor perception, image control and ROS indoor navigation. + usernamekkyy + BSD-3-Clause + catkin + rospy + diff --git a/src/humanoid/g1_costnet_grid_planner/patrol/__init__.py b/src/humanoid/g1_costnet_grid_planner/patrol/__init__.py new file mode 100644 index 0000000000..bc4204b4fb --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/patrol/__init__.py @@ -0,0 +1 @@ +"""Visual patrol on a physics simulated G1 humanoid.""" diff --git a/src/humanoid/g1_costnet_grid_planner/patrol/demo.py b/src/humanoid/g1_costnet_grid_planner/patrol/demo.py new file mode 100644 index 0000000000..c118766e4b --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/patrol/demo.py @@ -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 diff --git a/src/humanoid/g1_costnet_grid_planner/patrol/planning.py b/src/humanoid/g1_costnet_grid_planner/patrol/planning.py new file mode 100644 index 0000000000..c48189657a --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/patrol/planning.py @@ -0,0 +1,184 @@ +"""Learned cost-to-go tie breaker for admissible grid A*. + +Manhattan f is the primary priority, so learned estimates cannot invalidate +optimality on the four-connected grid. The network resolves equal-f plateaus. +This is neural-assisted search, not an end-to-end replacement of graph search. +""" +import heapq +import json +from pathlib import Path +from functools import lru_cache +import math + +import numpy as np +import torch +from torch import nn + + +class CostNet(nn.Module): + def __init__(self): + super().__init__() + self.net=nn.Sequential(nn.Conv2d(3,16,3,padding=1),nn.ReLU(), + nn.Conv2d(16,24,3,padding=2,dilation=2),nn.ReLU(), + nn.Conv2d(24,24,3,padding=4,dilation=4),nn.ReLU(), + nn.Conv2d(24,1,1),nn.Softplus()) + + def forward(self,x): + return self.net(x) + + +def features(grid,goal): + yy,xx=np.indices(grid.shape) + height, width = grid.shape + return np.stack([grid, (xx - goal[1]) / max(32, width), + (yy - goal[0]) / max(32, height)]).astype(np.float32) + + +def distances(grid,goal): + result=np.full(grid.shape,np.inf,dtype=np.float32) + result[goal]=0 + queue=[(0,goal)] + while queue: + cost,point=heapq.heappop(queue) + if cost!=result[point]: continue + for dy,dx in [(1,0),(-1,0),(0,1),(0,-1)]: + q=(point[0]+dy,point[1]+dx) + if 0<=q[0]50 + radius=int(np.ceil(robot_radius/resolution)) + inflated=blocked.copy() + for dy in range(-min(radius, len(blocked)-1), min(radius, len(blocked)-1)+1): + for dx in range(-min(radius, blocked.shape[1]-1), min(radius, blocked.shape[1]-1)+1): + if dx*dx+dy*dy>radius*radius: continue + srcy=slice(max(0,-dy),min(len(blocked),len(blocked)-dy)) + srcx=slice(max(0,-dx),min(blocked.shape[1],blocked.shape[1]-dx)) + dsty=slice(max(0,dy),min(len(blocked),len(blocked)+dy)) + dstx=slice(max(0,dx),min(blocked.shape[1],blocked.shape[1]+dx)) + inflated[dsty,dstx]|=blocked[srcy,srcx] + hh,ww=inflated.shape[0]//stride,inflated.shape[1]//stride + grid=inflated[:hh*stride,:ww*stride].reshape(hh,stride,ww,stride).any(axis=(1,3)) + cell=resolution*stride;ox,oy=request['origin'] + def index(xy): + p=(math.floor((xy[1]-oy)/cell), math.floor((xy[0]-ox)/cell)) + if not(0<=p[0]=1.5: + keep.append(route[i]) + keep.append(route[-1]) + world=[[ox+(p[1]+.5)*cell,oy+(p[0]+.5)*cell] for p in keep[1:]] + world[-1]=request['goal'] + return dict(waypoints=world,expanded=expanded,baseline_expanded=baseline_expanded, + path_cost=len(route)-1,baseline_path_cost=len(baseline)-1, + unknown_policy='provisionally traversable; live local obstacle avoidance required', + method='Manhattan A* with neural equal-f tie breaking') diff --git a/src/humanoid/g1_costnet_grid_planner/requirements.txt b/src/humanoid/g1_costnet_grid_planner/requirements.txt new file mode 100644 index 0000000000..09a1ba72d0 --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/requirements.txt @@ -0,0 +1,3 @@ +numpy>=1.26,<3 +torch>=2.5 +Pillow>=10 diff --git a/src/humanoid/g1_costnet_grid_planner/scripts/task_runner.py b/src/humanoid/g1_costnet_grid_planner/scripts/task_runner.py new file mode 100755 index 0000000000..a48aee8ebd --- /dev/null +++ b/src/humanoid/g1_costnet_grid_planner/scripts/task_runner.py @@ -0,0 +1,27 @@ +#!/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'), + '--weights', rospy.get_param('~weights'), + '--output', rospy.get_param('~output'), + ] + 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()