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 +``` + +## 运行效果 + +![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 辅助拆分、编程、验证和文档整理,提交者复核并对提交内容负责。 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.