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
44 changes: 44 additions & 0 deletions docs/humanoid/g1_gmapping_navigation/index.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
# G1 gmapping 室内建图导航

通过仿真雷达和里程计为 G1 同步建图,并用 move_base 执行地图目标。

## 环境与运行

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

在模块目录运行 `python scripts/fetch_assets.py /path/to/unitree_rl_gym` 获取固定版本的 G1 资产。底层步态来自 Unitree 预训练策略。

```bash
python3 main.py --sim-python /path/to/simulation/python --assets /path/to/unitree_rl_gym
```

需要 ROS Noetic、gmapping、move_base。等待 /map 后,在 RViz 选择 map 坐标系,用 2D Nav Goal 向 /move_base_simple/goal 发送目标。Windows 仿真环境与 WSL 组合时使用启动脚本转换路径。

## 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_gmapping_navigation main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym
```

## 运行效果

![G1 gmapping 室内建图导航](../../img/g1_gmapping_navigation/slam_map.png)

## 无仿真环境的场景 XML 测试

安装 NumPy 后在模块目录运行:

```bash
python tests/test_scene_xml_local.py
```

测试使用临时 G1 XML,覆盖有无 compiler 节点、场景几何和相机命名。
图像使用 front_camera 光学坐标系,TF 每帧发布其相对 base_link 的姿态。

## 大模型使用声明

使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
Binary file added docs/img/g1_gmapping_navigation/slam_map.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 @@ -97,6 +97,7 @@ ROS 2 服务与参数基础实验(无人机模式切换与在线调参):[c
* [MuJoCo单足弹跳机器人简单仿真](./humanoid/mujoco_hopper_demo/index.md)
* [人形机器人关节轨迹录制与回放工具](./humanoid/humanoid_joint_tools.md)
* [人形上下台阶步态仿真](./humanoid/humanoid_stair_climb.md)
* [G1 gmapping 室内建图导航](./humanoid/g1_gmapping_navigation/index.md)
* [ROS虚拟机器人圆形轨迹rviz可视化示例](./humanoid/ros_topic_demo/index.md)
* [人形机器人全身逆运动学与可操作性](./humanoid/whole_body_ik.md)
* [人形机器人双臂末端可达域分析](./humanoid/humanoid_reachability.md)
Expand Down
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,7 @@ nav:
- MuJoCo单足弹跳机器人简单仿真: './humanoid/mujoco_hopper_demo/index.md'
- 人形机器人关节轨迹录制与回放工具: './humanoid/humanoid_joint_tools.md'
- 人形上下台阶步态仿真: './humanoid/humanoid_stair_climb.md'
- G1 gmapping 室内建图导航: './humanoid/g1_gmapping_navigation/index.md'
- ROS虚拟机器人圆形轨迹rviz可视化示例: './humanoid/ros_topic_demo/index.md'
- ROS2 Humble 路径点发布器: './waypoint_publisher.md'
- 人形机器人全身逆运动学与可操作性: './humanoid/whole_body_ik.md'
Expand Down
2 changes: 2 additions & 0 deletions src/humanoid/g1_gmapping_navigation/.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_gmapping_navigation/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_gmapping_navigation)
find_package(catkin REQUIRED)
catkin_package()
catkin_install_python(PROGRAMS scripts/ros_bridge.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
40 changes: 40 additions & 0 deletions src/humanoid/g1_gmapping_navigation/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
# G1 gmapping 室内建图导航

通过仿真雷达和里程计为 G1 同步建图,并用 move_base 执行地图目标。

## 环境与运行

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

在模块目录运行 `python scripts/fetch_assets.py /path/to/unitree_rl_gym` 获取固定版本的 G1 资产。底层步态来自 Unitree 预训练策略。

```bash
python3 main.py --sim-python /path/to/simulation/python --assets /path/to/unitree_rl_gym
```

需要 ROS Noetic、gmapping、move_base。等待 /map 后,在 RViz 选择 map 坐标系,用 2D Nav Goal 向 /move_base_simple/goal 发送目标。Windows 仿真环境与 WSL 组合时使用启动脚本转换路径。

## 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_gmapping_navigation main.launch sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym
```

## 无仿真环境的场景 XML 测试

安装 NumPy 后在模块目录运行:

```bash
python tests/test_scene_xml_local.py
```

测试使用临时 G1 XML,覆盖有无 compiler 节点、场景几何和相机命名。
图像使用 front_camera 光学坐标系,TF 每帧发布其相对 base_link 的姿态。

## 大模型使用声明

使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
57 changes: 57 additions & 0 deletions src/humanoid/g1_gmapping_navigation/config/navigation.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
controller_frequency: 5.0
planner_frequency: 1.0
controller_patience: 15.0
planner_patience: 10.0
recovery_behavior_enabled: false
base_local_planner: base_local_planner/TrajectoryPlannerROS
TrajectoryPlannerROS:
max_vel_x: 0.3
min_vel_x: 0.05
max_vel_theta: 0.35
min_vel_theta: -0.35
min_in_place_vel_theta: 0.15
acc_lim_x: 0.3
acc_lim_y: 0.0
acc_lim_theta: 0.4
holonomic_robot: false
# Patrol goals constrain position; no final heading is prescribed.
yaw_goal_tolerance: 3.14159
xy_goal_tolerance: 0.5
latch_xy_goal_tolerance: true
sim_time: 2.0
vx_samples: 8
vtheta_samples: 16
meter_scoring: true
pdist_scale: 0.8
gdist_scale: 0.6
occdist_scale: 0.05
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 3.0
publish_frequency: 1.0
transform_tolerance: 1.0
robot_radius: 0.3
plugins:
- {name: static_layer, type: 'costmap_2d::StaticLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
static_layer: {subscribe_to_updates: true}
inflation_layer: {inflation_radius: 0.6, cost_scaling_factor: 3.0}
local_costmap:
global_frame: odom
robot_base_frame: base_link
rolling_window: true
width: 5.0
height: 5.0
resolution: 0.05
update_frequency: 5.0
publish_frequency: 1.0
transform_tolerance: 1.0
robot_radius: 0.3
plugins:
- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
obstacle_layer:
observation_sources: laser
laser: {sensor_frame: laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true, obstacle_range: 7.5, raytrace_range: 8.0}
inflation_layer: {inflation_radius: 0.6, cost_scaling_factor: 3.0}
28 changes: 28 additions & 0 deletions src/humanoid/g1_gmapping_navigation/launch/main.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
<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"/>
<param name="use_sim_time" value="true"/>
<node pkg="g1_gmapping_navigation" type="ros_bridge.py" name="humanoid_sim_bridge" 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="seed" value="$(arg seed)"/>
<param name="real_time_factor" value="$(arg real_time_factor)"/>
</node>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="base_link"/><param name="odom_frame" value="odom"/>
<param name="map_frame" value="map"/><param name="map_update_interval" value="1.0"/>
<param name="tf_delay" value="0.5"/><param name="transform_publish_period" value="0.2"/>
<param name="maxUrange" value="7.8"/><param name="maxRange" value="8.0"/>
<param name="delta" value="0.05"/><param name="particles" value="20"/>
<param name="linearUpdate" value="0.1"/><param name="angularUpdate" value="0.1"/>
<param name="xmin" value="-3"/><param name="ymin" value="-3"/>
<param name="xmax" value="12"/><param name="ymax" value="3"/>
</node>
<node pkg="move_base" type="move_base" name="move_base" output="screen">
<rosparam command="load" file="$(find g1_gmapping_navigation)/config/navigation.yaml"/>
</node>
</launch>
24 changes: 24 additions & 0 deletions src/humanoid/g1_gmapping_navigation/main.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
#!/usr/bin/env python3
import argparse
from pathlib import Path
import subprocess
import sys


def main():
parser = argparse.ArgumentParser(description='Run G1 gmapping and move_base in ROS Noetic.')
parser.add_argument('--assets', required=True)
parser.add_argument('--sim-python', default=sys.executable)
parser.add_argument('--mode', choices=['navigation', 'bridge'], default='navigation')
parser.add_argument('--seed', type=int, default=7)
args = parser.parse_args()
if args.mode == 'bridge':
from patrol.bridge_server import serve
serve(args.assets, args.seed)
return
script = Path(__file__).resolve().parent / 'scripts/run_navigation.sh'
subprocess.run(['bash', str(script), args.sim_python, args.assets, str(args.seed)], check=True)


if __name__ == '__main__':
main()
13 changes: 13 additions & 0 deletions src/humanoid/g1_gmapping_navigation/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<package format="2">
<name>g1_gmapping_navigation</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><exec_depend>tf</exec_depend>
<exec_depend>sensor_msgs</exec_depend><exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend><exec_depend>rosgraph_msgs</exec_depend>
<exec_depend>gmapping</exec_depend><exec_depend>move_base</exec_depend>
<exec_depend>actionlib</exec_depend><exec_depend>move_base_msgs</exec_depend><exec_depend>std_msgs</exec_depend>
</package>
1 change: 1 addition & 0 deletions src/humanoid/g1_gmapping_navigation/patrol/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
"""Visual patrol on a physics simulated G1 humanoid."""
36 changes: 36 additions & 0 deletions src/humanoid/g1_gmapping_navigation/patrol/bridge_server.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
"""JSON lines over stdio: a process boundary between ROS and simulator Python."""
import json
import sys
import base64

from .simulator import IndoorSimulator


def serve(assets, seed=0):
sim = IndoorSimulator(assets, seed)
try:
for line in sys.stdin:
request = json.loads(line)
if request.get('stop'):
break
command = request.get('command',[0,0,0])
sim.set_command(*command)
previous_yaw = sim.odom[2]
previous_time = sim.data.time
for _ in range(10):
sim.step()
if sim.fallen:
break
rgb=sim.image()
camera_position, camera_rotation = sim.camera_transform()
result = dict(time=float(sim.data.time), odom=sim.odom.tolist(),
scan=sim.scan(), velocity=sim.data.qvel[:2].tolist(),
angular_velocity=float((sim.odom[2] - previous_yaw) / (sim.data.time - previous_time)),
camera_position=camera_position, camera_rotation=camera_rotation,
fallen=bool(sim.fallen), joints=sim.data.qpos[7:].tolist(),
joint_names=[sim.model.joint(i).name for i in range(1,sim.model.njnt)],
image=base64.b64encode(rgb.tobytes()).decode('ascii'),
image_shape=list(rgb.shape))
print(json.dumps(result),flush=True)
finally:
sim.close()
85 changes: 85 additions & 0 deletions src/humanoid/g1_gmapping_navigation/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