diff --git a/docs/humanoid/g1_gmapping_navigation/index.md b/docs/humanoid/g1_gmapping_navigation/index.md
new file mode 100644
index 0000000000..0992f27bdb
--- /dev/null
+++ b/docs/humanoid/g1_gmapping_navigation/index.md
@@ -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
+```
+
+## 运行效果
+
+
+
+## 无仿真环境的场景 XML 测试
+
+安装 NumPy 后在模块目录运行:
+
+```bash
+python tests/test_scene_xml_local.py
+```
+
+测试使用临时 G1 XML,覆盖有无 compiler 节点、场景几何和相机命名。
+图像使用 front_camera 光学坐标系,TF 每帧发布其相对 base_link 的姿态。
+
+## 大模型使用声明
+
+使用 OpenAI Codex 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
diff --git a/docs/img/g1_gmapping_navigation/slam_map.png b/docs/img/g1_gmapping_navigation/slam_map.png
new file mode 100644
index 0000000000..b250ad3126
Binary files /dev/null and b/docs/img/g1_gmapping_navigation/slam_map.png differ
diff --git a/docs/index.md b/docs/index.md
index 9f6f16ff53..175a932665 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -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)
diff --git a/mkdocs.yml b/mkdocs.yml
index edf15e5906..9e065a8ff9 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -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'
diff --git a/src/humanoid/g1_gmapping_navigation/.gitattributes b/src/humanoid/g1_gmapping_navigation/.gitattributes
new file mode 100644
index 0000000000..f13e51c0ec
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/.gitattributes
@@ -0,0 +1,2 @@
+*.py text eol=lf
+*.sh text eol=lf
diff --git a/src/humanoid/g1_gmapping_navigation/CMakeLists.txt b/src/humanoid/g1_gmapping_navigation/CMakeLists.txt
new file mode 100644
index 0000000000..80d6948af7
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/CMakeLists.txt
@@ -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})
diff --git a/src/humanoid/g1_gmapping_navigation/README.md b/src/humanoid/g1_gmapping_navigation/README.md
new file mode 100644
index 0000000000..150ad143da
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/README.md
@@ -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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。
diff --git a/src/humanoid/g1_gmapping_navigation/config/navigation.yaml b/src/humanoid/g1_gmapping_navigation/config/navigation.yaml
new file mode 100644
index 0000000000..908d5e04ba
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/config/navigation.yaml
@@ -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}
diff --git a/src/humanoid/g1_gmapping_navigation/launch/main.launch b/src/humanoid/g1_gmapping_navigation/launch/main.launch
new file mode 100644
index 0000000000..9d77644fa3
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/launch/main.launch
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/humanoid/g1_gmapping_navigation/main.py b/src/humanoid/g1_gmapping_navigation/main.py
new file mode 100644
index 0000000000..25f8235fc1
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/main.py
@@ -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()
diff --git a/src/humanoid/g1_gmapping_navigation/package.xml b/src/humanoid/g1_gmapping_navigation/package.xml
new file mode 100644
index 0000000000..9addc3c8fd
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/package.xml
@@ -0,0 +1,13 @@
+
+
+ g1_gmapping_navigation0.1.0
+ MuJoCo G1 visual floor perception, image control and ROS indoor navigation.
+ usernamekkyy
+ BSD-3-Clause
+ catkin
+ rospytf
+ sensor_msgsgeometry_msgs
+ nav_msgsrosgraph_msgs
+ gmappingmove_base
+ actionlibmove_base_msgsstd_msgs
+
diff --git a/src/humanoid/g1_gmapping_navigation/patrol/__init__.py b/src/humanoid/g1_gmapping_navigation/patrol/__init__.py
new file mode 100644
index 0000000000..bc4204b4fb
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/patrol/__init__.py
@@ -0,0 +1 @@
+"""Visual patrol on a physics simulated G1 humanoid."""
diff --git a/src/humanoid/g1_gmapping_navigation/patrol/bridge_server.py b/src/humanoid/g1_gmapping_navigation/patrol/bridge_server.py
new file mode 100644
index 0000000000..90182198ab
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/patrol/bridge_server.py
@@ -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()
diff --git a/src/humanoid/g1_gmapping_navigation/patrol/locomotion.py b/src/humanoid/g1_gmapping_navigation/patrol/locomotion.py
new file mode 100644
index 0000000000..d072059b3d
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/patrol/locomotion.py
@@ -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())
diff --git a/src/humanoid/g1_gmapping_navigation/patrol/scene.py b/src/humanoid/g1_gmapping_navigation/patrol/scene.py
new file mode 100644
index 0000000000..243830ba30
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/patrol/scene.py
@@ -0,0 +1,58 @@
+"""Generate indoor geometry without changing the upstream robot assets."""
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+
+
+def build_scene(assets, seed=0):
+ asset_dir = Path(assets).resolve() / 'resources/robots/g1_description'
+ tree = ET.parse(asset_dir/'g1_12dof.xml')
+ root = tree.getroot()
+ compiler = root.find('compiler')
+ if compiler is None:
+ compiler = ET.Element('compiler')
+ root.insert(0, compiler)
+ compiler.set('meshdir', (asset_dir / 'meshes').as_posix())
+ world = root.find('worldbody')
+ if world is None:
+ raise ValueError('G1 XML lacks worldbody')
+ pelvis = world.find("body[@name='pelvis']")
+ if pelvis is None:
+ raise ValueError('G1 XML lacks pelvis body')
+ for geom in pelvis.iter('geom'):
+ geom.set('group', '1')
+ ET.SubElement(pelvis, 'camera', name='front_camera', pos='0.16 0 0.35',
+ xyaxes='0 -1 0 0.35 0 0.94', fovy='75')
+ visual = root.find('visual')
+ if visual is None:
+ visual = ET.SubElement(root, 'visual')
+ global_settings = visual.find('global')
+ if global_settings is None:
+ global_settings = ET.SubElement(visual, 'global')
+ global_settings.attrib.update(offwidth='640', offheight='480')
+ headlight_settings = visual.find('headlight')
+ if headlight_settings is None:
+ headlight_settings = ET.SubElement(visual, 'headlight')
+ headlight_settings.attrib.update(ambient='0.4 0.4 0.4', diffuse='0.6 0.6 0.6')
+ rng = np.random.default_rng(seed)
+ brightness = float(rng.uniform(.6, .95))
+ ET.SubElement(world, 'light', pos='3 0 4', diffuse=f'{brightness} {brightness} {brightness}')
+ ET.SubElement(world, 'geom', name='floor', type='plane', size='15 15 .1', rgba='.65 .65 .60 1')
+ boxes = [('left_wall', [4, 2, 1], [6, .12, 1]),
+ ('right_wall', [4, -2, 1], [6, .12, 1]),
+ ('back_wall', [-2, 0, 1], [.12, 2, 1]),
+ ('end_wall', [10, 0, 1], [.12, 2, 1]),
+ ('door_left', [5, 1.65, .9], [.12, .35, .9]),
+ ('door_right', [5, -1.65, .9], [.12, .35, .9]),
+ ('cabinet', [3, .95 + float(rng.uniform(-.15, .15)), .45], [.3, .3, .45])]
+ for name, pos, size in boxes:
+ rgba = [*rng.uniform(.25, .75, 3), 1]
+ ET.SubElement(world, 'geom', name=name, type='box',
+ pos=' '.join(map(str, pos)), size=' '.join(map(str, size)),
+ rgba=' '.join(map(str, rgba)))
+ ET.SubElement(world, 'geom', name='inspection_marker', type='box',
+ pos='9.84 0 1', size='.02 .35 .35', rgba='.1 .7 .8 1',
+ contype='0', conaffinity='0')
+ return ET.tostring(root, encoding='unicode')
diff --git a/src/humanoid/g1_gmapping_navigation/patrol/simulator.py b/src/humanoid/g1_gmapping_navigation/patrol/simulator.py
new file mode 100644
index 0000000000..1b61670be1
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/patrol/simulator.py
@@ -0,0 +1,88 @@
+import tempfile
+from pathlib import Path
+
+import mujoco
+import numpy as np
+
+from .locomotion import Walker
+from .scene import build_scene
+
+
+class IndoorSimulator(Walker):
+ def __init__(self, assets, seed=0):
+ # MuJoCo 3.2 parses an XML file; generated files remain outside the repo.
+ with tempfile.TemporaryDirectory(prefix='humanoid_scene_') as folder:
+ path = Path(folder)/'scene.xml'
+ path.write_text(build_scene(assets, seed), encoding='utf-8')
+ super().__init__(assets, path)
+ self.renderer = None
+ self.floor_id = self.model.geom('floor').id
+ self.rng = np.random.default_rng(seed)
+ self.odom = np.zeros(3)
+ self._last_yaw = self.yaw
+
+ def step(self):
+ super().step()
+ # Simulated velocity/gyro measurements, integrated independently of position.
+ dt = self.model.opt.timestep*self.config['control_decimation']
+ yaw = self.yaw
+ rotation = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]])
+ body_velocity = rotation @ self.data.qvel[:2]
+ body_velocity += self.rng.normal(0, .003, 2)
+ dyaw = (yaw-self._last_yaw+np.pi)%(2*np.pi)-np.pi
+ self._last_yaw = yaw
+ self.odom[2] += dyaw + self.rng.normal(0, .0002)
+ c, s = np.cos(self.odom[2]), np.sin(self.odom[2])
+ self.odom[:2] += np.array([[c, -s], [s, c]])@body_velocity*dt
+
+ def camera_transform(self):
+ """Optical pose relative to the level, ground-projected base frame."""
+ camera = self.data.camera('front_camera')
+ yaw = self.yaw
+ c, s = np.cos(yaw), np.sin(yaw)
+ inverse_heading = np.array([[c, s, 0], [-s, c, 0], [0, 0, 1]])
+ base = np.array([*self.data.qpos[:2], 0.0])
+ translation = inverse_heading @ (camera.xpos - base)
+ optical = camera.xmat.reshape(3, 3) @ np.diag([1, -1, -1])
+ rotation = np.eye(4)
+ rotation[:3, :3] = inverse_heading @ optical
+ return translation.tolist(), rotation.tolist()
+
+ def image(self, labels=False, depth=False):
+ if self.renderer is None:
+ self.renderer = mujoco.Renderer(self.model, height=96, width=128)
+ r = self.renderer
+ r.disable_segmentation_rendering()
+ r.disable_depth_rendering()
+ r.update_scene(self.data, camera='front_camera')
+ rgb = r.render().copy()
+ if labels:
+ r.enable_segmentation_rendering()
+ segmentation = r.render().copy()
+ r.disable_segmentation_rendering()
+ return rgb, (segmentation[:, :, 0] == self.floor_id).astype(np.uint8)
+ if depth:
+ r.enable_depth_rendering()
+ result = r.render().copy()
+ r.disable_depth_rendering()
+ return rgb, result
+ return rgb
+
+ def scan(self, count=180):
+ origin = self.data.qpos[:3].copy()
+ origin[2] = .65
+ angles = np.linspace(-np.pi, np.pi, count, endpoint=False) + self.yaw
+ directions = np.column_stack((np.cos(angles), np.sin(angles), np.zeros(count)))
+ groups = np.array([1, 0, 0, 0, 0, 0], dtype=np.uint8)
+ geom_ids = np.empty(count, dtype=np.int32)
+ distances = np.empty(count)
+ # Exclude the robot and limit the query to the published scan range.
+ mujoco.mj_multiRay(self.model, self.data, origin, directions.ravel(), groups,
+ 1, self.model.body('pelvis').id, geom_ids, distances, count, 8.0)
+ distances[distances < 0] = 8.0
+ return distances.clip(0.05, 8.0).tolist()
+
+ def close(self):
+ if self.renderer:
+ self.renderer.close()
+ self.renderer = None
diff --git a/src/humanoid/g1_gmapping_navigation/requirements.txt b/src/humanoid/g1_gmapping_navigation/requirements.txt
new file mode 100644
index 0000000000..f55b4da02e
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/requirements.txt
@@ -0,0 +1,6 @@
+mujoco==3.2.0
+numpy>=1.24
+torch>=2.0
+PyYAML>=6
+Pillow>=10
+imageio>=2.30
diff --git a/src/humanoid/g1_gmapping_navigation/scripts/fetch_assets.py b/src/humanoid/g1_gmapping_navigation/scripts/fetch_assets.py
new file mode 100755
index 0000000000..2c5656e23c
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/scripts/fetch_assets.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+"""Fetch the exact upstream model/policy revision; never overwrite an existing path."""
+import argparse
+from pathlib import Path
+import subprocess
+
+REVISION = '276801e46c5d433564f24658bac64f254b7d2d4b'
+URL = 'https://github.com/unitreerobotics/unitree_rl_gym.git'
+
+
+def main():
+ parser = argparse.ArgumentParser(description=__doc__)
+ parser.add_argument('destination',type=Path)
+ args = parser.parse_args()
+ target = args.destination.resolve()
+ if target.exists():
+ raise SystemExit('Destination exists; use a new directory or validate the existing revision manually.')
+ subprocess.run(['git', 'init', str(target)],check=True)
+ subprocess.run(['git', '-C', str(target), 'remote', 'add', 'origin', URL], check=True)
+ subprocess.run(['git', '-C', str(target), 'fetch', '--depth', '1', 'origin', REVISION], check=True)
+ subprocess.run(['git','-C',str(target),'sparse-checkout','set','deploy','resources/robots/g1_description'],check=True)
+ subprocess.run(['git','-C',str(target),'checkout','--detach',REVISION],check=True)
+ print(target)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/g1_gmapping_navigation/scripts/ros_bridge.py b/src/humanoid/g1_gmapping_navigation/scripts/ros_bridge.py
new file mode 100755
index 0000000000..a2041d1a09
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/scripts/ros_bridge.py
@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+"""ROS Noetic bridge. Simulation uses an isolated interpreter over stdio.
+
+The odometry pose integrates simulated velocity and gyro measurements.
+On WSL, an explicit Windows Python executable can run the MuJoCo process.
+"""
+import json
+import math
+import subprocess
+import time
+import base64
+
+import rospy
+import tf
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan, JointState, Image
+from rosgraph_msgs.msg import Clock
+
+
+def main():
+ rospy.init_node('humanoid_sim_bridge')
+ interpreter = rospy.get_param('~sim_python')
+ entry = rospy.get_param('~sim_entry')
+ assets = rospy.get_param('~assets')
+ seed = str(rospy.get_param('~seed',7))
+ real_time_factor = float(rospy.get_param('~real_time_factor',1.0))
+ command = [0.,0.,0.]
+ last_command = [0.]
+ def callback(msg):
+ command[:] = [msg.linear.x,msg.linear.y,msg.angular.z]
+ last_command[0] = time.monotonic()
+ rospy.Subscriber('/cmd_vel',Twist,callback,queue_size=1)
+ scan_pub = rospy.Publisher('/scan',LaserScan,queue_size=2)
+ odom_pub = rospy.Publisher('/odom',Odometry,queue_size=2)
+ clock_pub = rospy.Publisher('/clock',Clock,queue_size=2)
+ joint_pub = rospy.Publisher('/joint_states',JointState,queue_size=2)
+ image_pub = rospy.Publisher('/camera/image_raw',Image,queue_size=1)
+ broadcaster = tf.TransformBroadcaster()
+ child = subprocess.Popen([interpreter,entry,'--mode','bridge','--assets',assets,'--seed',seed],
+ stdin=subprocess.PIPE,stdout=subprocess.PIPE,universal_newlines=True,bufsize=1)
+ try:
+ while not rospy.is_shutdown():
+ begin = time.monotonic()
+ if time.monotonic()-last_command[0] > .8:
+ command[:] = [0.,0.,0.]
+ child.stdin.write(json.dumps({'command':command})+'\n')
+ child.stdin.flush()
+ line = child.stdout.readline()
+ if not line:
+ raise RuntimeError('Simulator process exited')
+ state = json.loads(line)
+ stamp = rospy.Time.from_sec(state['time'])
+ clock_pub.publish(Clock(clock=stamp))
+ x,y,yaw = state['odom']
+ quaternion = tf.transformations.quaternion_from_euler(0,0,yaw)
+ broadcaster.sendTransform((x,y,0),quaternion,stamp,'base_link','odom')
+ broadcaster.sendTransform((0,0,.65),(0,0,0,1),stamp,'laser','base_link')
+ odom = Odometry()
+ odom.header.stamp,odom.header.frame_id,odom.child_frame_id = stamp,'odom','base_link'
+ odom.pose.pose.position.x,odom.pose.pose.position.y = x,y
+ odom.pose.pose.orientation.x,odom.pose.pose.orientation.y,odom.pose.pose.orientation.z,odom.pose.pose.orientation.w = quaternion
+ # Rotate MuJoCo world-frame linear velocity into the base_link frame.
+ vx,vy = state['velocity']
+ odom.twist.twist.linear.x = math.cos(yaw)*vx+math.sin(yaw)*vy
+ odom.twist.twist.linear.y = -math.sin(yaw)*vx+math.cos(yaw)*vy
+ odom.twist.twist.angular.z = state['angular_velocity']
+ odom_pub.publish(odom)
+ scan = LaserScan()
+ scan.header.stamp,scan.header.frame_id = stamp,'laser'
+ scan.angle_min = -math.pi
+ scan.angle_increment = 2*math.pi/len(state['scan'])
+ scan.angle_max = scan.angle_min+(len(state['scan'])-1)*scan.angle_increment
+ scan.range_min,scan.range_max,scan.scan_time = .05,8.,.2
+ scan.ranges = state['scan']
+ scan_pub.publish(scan)
+ joints=JointState()
+ joints.header.stamp=stamp
+ joints.name=state['joint_names']; joints.position=state['joints']
+ joint_pub.publish(joints)
+ camera_quaternion = tf.transformations.quaternion_from_matrix(state['camera_rotation'])
+ broadcaster.sendTransform(state['camera_position'], camera_quaternion,
+ stamp, 'front_camera', 'base_link')
+ image=Image()
+ image.header.stamp=stamp; image.header.frame_id='front_camera'
+ image.height,image.width=state['image_shape'][:2]
+ image.encoding='rgb8'; image.step=image.width*3
+ image.data=base64.b64decode(state['image'])
+ image_pub.publish(image)
+ if state['fallen']:
+ rospy.logerr('Robot fell; stopping simulator')
+ break
+ time.sleep(max(0,.2/real_time_factor-(time.monotonic()-begin)))
+ finally:
+ try:
+ child.stdin.write('{"stop":true}\n')
+ child.stdin.flush()
+ child.wait(timeout=5)
+ except (OSError,subprocess.TimeoutExpired):
+ child.terminate()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/g1_gmapping_navigation/scripts/run_navigation.sh b/src/humanoid/g1_gmapping_navigation/scripts/run_navigation.sh
new file mode 100755
index 0000000000..7520324720
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/scripts/run_navigation.sh
@@ -0,0 +1,16 @@
+#!/usr/bin/env bash
+# Usage: bash scripts/run_navigation.sh SIM_PYTHON ASSETS [SEED]
+set -e
+MODULE_DIR="$(cd -- "$(dirname -- "$0")/.." && pwd)"
+source /opt/ros/noetic/setup.bash
+export ROS_MASTER_URI="${ROS_MASTER_URI:-http://localhost:11331}"
+export ROS_PACKAGE_PATH="$MODULE_DIR:$ROS_PACKAGE_PATH"
+SIM_PYTHON="${1:?Provide the simulator Python executable}"
+ASSETS="${2:?Provide the unitree_rl_gym path}"
+ENTRY="$MODULE_DIR/main.py"
+if [[ "$SIM_PYTHON" == *.exe ]]; then
+ ENTRY="$(wslpath -w "$ENTRY")"
+ ASSETS="$(wslpath -w "$ASSETS")"
+fi
+exec roslaunch -p "${ROS_MASTER_URI##*:}" g1_gmapping_navigation main.launch \
+ sim_python:="$SIM_PYTHON" sim_entry:="$ENTRY" assets:="$ASSETS" seed:="${3:-7}"
diff --git a/src/humanoid/g1_gmapping_navigation/tests/test_scene_xml_local.py b/src/humanoid/g1_gmapping_navigation/tests/test_scene_xml_local.py
new file mode 100644
index 0000000000..f7abe3d4a3
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/tests/test_scene_xml_local.py
@@ -0,0 +1,36 @@
+import sys
+from pathlib import Path
+from tempfile import TemporaryDirectory
+import unittest
+import xml.etree.ElementTree as ET
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+from patrol.scene import build_scene
+
+
+class SceneXmlTest(unittest.TestCase):
+ def test_generates_scene_without_simulator_or_assets(self):
+ for compiler in ('', ''):
+ with self.subTest(compiler=compiler), TemporaryDirectory() as directory:
+ assets = Path(directory)
+ description = assets / 'resources/robots/g1_description'
+ description.mkdir(parents=True)
+ (description / 'g1_12dof.xml').write_text(
+ f'{compiler}',
+ encoding='utf-8',
+ )
+ xml = build_scene(assets, seed=7)
+ self.assertEqual(xml, build_scene(assets, seed=7))
+ root = ET.fromstring(xml)
+ self.assertEqual(root.find('compiler').get('meshdir'),
+ (description / 'meshes').as_posix())
+ if compiler:
+ self.assertEqual(root.find('compiler').get('angle'), 'radian')
+ names = {geom.get('name') for geom in root.findall('worldbody/geom')}
+ self.assertTrue({'floor', 'left_wall', 'right_wall', 'back_wall', 'end_wall',
+ 'door_left', 'door_right', 'inspection_marker'} <= names)
+ self.assertIsNotNone(root.find("worldbody/body/camera[@name='front_camera']"))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/src/humanoid/g1_gmapping_navigation/third_party_license.md b/src/humanoid/g1_gmapping_navigation/third_party_license.md
new file mode 100644
index 0000000000..9ad8f6dbb0
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/third_party_license.md
@@ -0,0 +1,23 @@
+BSD 3-Clause License
+Copyright (c) 2016-2023 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+3. Neither the name of the copyright holder nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/humanoid/g1_gmapping_navigation/third_party_notices.md b/src/humanoid/g1_gmapping_navigation/third_party_notices.md
new file mode 100644
index 0000000000..01dd8c97e2
--- /dev/null
+++ b/src/humanoid/g1_gmapping_navigation/third_party_notices.md
@@ -0,0 +1,20 @@
+# Third-party sources
+
+The G1 MJCF/STL assets, pretrained `motion.pt`, policy observation convention,
+PD gains and action scaling originate from
+[Unitree RL Gym](https://github.com/unitreerobotics/unitree_rl_gym), revision
+`276801e46c5d433564f24658bac64f254b7d2d4b`, BSD-3-Clause.
+The upstream copyright and license are preserved in [third_party_license.md](third_party_license.md).
+Assets are fetched separately and are not duplicated into this module.
+
+`patrol/locomotion.py` adapts the upstream deployment equations and observation
+ordering. The pretrained walking policy is not an original training result.
+
+MuJoCo supplies physical simulation, rendering and ray queries. ROS Noetic
+gmapping and move_base supply SLAM and classical navigation. These components
+retain their respective upstream licenses; no claim of a new SLAM algorithm is made.
+
+FloorNet, MaskController, ImageController and CostNet, the training data generator, visual corridor
+controller, indoor scene generator, ROS stdio bridge and evaluation code were
+created for this project with AI assistance. Training weights under `models/`
+were produced by this module's training script.