diff --git a/.github/workflows/ros2_rov_pipeline_follow.yml b/.github/workflows/ros2_rov_pipeline_follow.yml index df98556677..7ae47a3341 100644 --- a/.github/workflows/ros2_rov_pipeline_follow.yml +++ b/.github/workflows/ros2_rov_pipeline_follow.yml @@ -37,7 +37,10 @@ jobs: - name: colcon build run: | source /opt/ros/humble/setup.bash - colcon build --packages-select rov_pipeline_follow --symlink-install + # The repository also has src/water/package.xml; start discovery at + # the nested package so colcon does not stop at the parent package. + colcon build --base-paths src/water/rov_pipeline_follow \ + --packages-select rov_pipeline_follow --symlink-install # ---- 3) install 布局硬断言:少一项就说明 data_files 又漏了 - name: 校验 install 布局 diff --git a/.gitignore b/.gitignore index 4e193d427e..89337bc380 100644 --- a/.gitignore +++ b/.gitignore @@ -95,6 +95,13 @@ src/air/drl_drone_path_planning/models/train_summary.json # 例外:Autoencoder 雷达修复模块的离线训练权重必须入库(约 96 KB,免重训) !src/ground/lidar_autoencoder_repair/models/*.pt +# 视觉通行模块的四个部署权重(约 17 KB、40 KB、61 KB、1.64 MB) +!src/humanoid/humanoid_visual_patrol/models/perception.pt +!src/humanoid/humanoid_visual_patrol/models/end2end.pt +!src/humanoid/humanoid_visual_patrol/models/mask_controller.pt +!src/humanoid/humanoid_visual_patrol/models/planner.pt +src/humanoid/humanoid_visual_patrol/results/ + # 例外:人形 H1/H1_2 行走模块的 TorchScript 策略必须入库(137/143 KB,免重训) !src/humanoid/humanoid_h1_walk/weights/*.pt diff --git a/docs/humanoid/humanoid_visual_patrol/evaluation.json b/docs/humanoid/humanoid_visual_patrol/evaluation.json new file mode 100644 index 0000000000..e2ace2eab2 --- /dev/null +++ b/docs/humanoid/humanoid_visual_patrol/evaluation.json @@ -0,0 +1,1093 @@ +{ + "end2end_7": { + "mode": "end2end", + "scene_seed": 7, + "simulated_seconds": 29.45999999999455, + "wall_seconds": 2.1050050258636475, + "fallen": false, + "reached": true, + "final_position": [ + 7.500928685799143, + -0.4098442664875997, + 0.7759206563914889 + ], + "collision_steps": 0, + "odometry_error_m": 0.012438045118886324 + }, + "end2end_8": { + "mode": "end2end", + "scene_seed": 8, + "simulated_seconds": 31.579999999993376, + "wall_seconds": 2.3076367378234863, + "fallen": false, + "reached": true, + "final_position": [ + 7.503404718295236, + -1.0501545513112895, + 0.7742367789168679 + ], + "collision_steps": 0, + "odometry_error_m": 0.03067057421028003 + }, + "locomotion_smoke": { + "simulated_seconds": 12.000000000000677, + "fallen": false, + "displacement_m": 2.287985076372512, + "min_height_m": 0.7661107569028477, + "final_yaw_rad": 0.7960921896645273, + "final_position": [ + 2.272985425640397, + 0.26155872099716265, + 0.778752585346743 + ] + }, + "navigation_result": { + "server_ready": true, + "map_updates": 21, + "first_map": { + "stamp": 5.199999999, + "width": 288, + "height": 320, + "known": 11143, + "occupied": 352 + }, + "last_map": { + "stamp": 30.599999999, + "width": 288, + "height": 320, + "known": 18328, + "occupied": 1275 + }, + "goals": [ + { + "goal": [ + 2.0, + -0.3 + ], + "finished": true, + "state": 3, + "odom": [ + 1.6131431608545903, + -0.37145751631762564 + ] + }, + { + "goal": [ + 4.0, + -0.3 + ], + "finished": true, + "state": 3, + "odom": [ + 3.6526522289419505, + -0.4557591917484762 + ] + }, + { + "goal": [ + 7.0, + 0.0 + ], + "finished": true, + "state": 3, + "odom": [ + 6.73615725296174, + -0.2689369038850545 + ] + } + ], + "camera": { + "width": 128, + "height": 96, + "bytes": 36864 + }, + "joint_count": 12 + }, + "neural_navigation_result": { + "waypoints": [ + [ + 1.5750000681728125, + -0.07499995641410351 + ], + [ + 3.0750000905245543, + -0.07499995641410351 + ], + [ + 4.575000112876296, + -0.07499995641410351 + ], + [ + 6.075000135228038, + -0.07499995641410351 + ], + [ + 7.0, + 0.0 + ] + ], + "expanded": 47, + "baseline_expanded": 47, + "path_cost": 46, + "baseline_path_cost": 46, + "unknown_policy": "provisionally traversable; live local obstacle avoidance required", + "method": "Manhattan A* with neural equal-f tie breaking", + "request_id": 7799999999, + "execution": [ + { + "goal": [ + 1.5750000681728125, + -0.07499995641410351 + ], + "state": 3 + }, + { + "goal": [ + 3.0750000905245543, + -0.07499995641410351 + ], + "state": 3 + }, + { + "goal": [ + 4.575000112876296, + -0.07499995641410351 + ], + "state": 3 + }, + { + "goal": [ + 6.075000135228038, + -0.07499995641410351 + ], + "state": 3 + }, + { + "goal": [ + 7.0, + 0.0 + ], + "state": 3 + } + ], + "success": true + }, + "perception_7": { + "mode": "perception", + "scene_seed": 7, + "simulated_seconds": 30.41999999999402, + "wall_seconds": 2.4995839595794678, + "fallen": false, + "reached": true, + "final_position": [ + 7.503956924401511, + -1.1252807956239923, + 0.7736514554436829 + ], + "collision_steps": 0, + "odometry_error_m": 0.014016393594219575 + }, + "perception_8": { + "mode": "perception", + "scene_seed": 8, + "simulated_seconds": 32.099999999993265, + "wall_seconds": 2.5015172958374023, + "fallen": false, + "reached": true, + "final_position": [ + 7.503228414520496, + -0.6834990817441247, + 0.7741088336048865 + ], + "collision_steps": 0, + "odometry_error_m": 0.027739362518145388 + }, + "teacher_7": { + "mode": "teacher", + "scene_seed": 7, + "simulated_seconds": 30.359999999994052, + "wall_seconds": 2.1397719383239746, + "fallen": false, + "reached": true, + "final_position": [ + 7.500330321965385, + -1.2885485142101345, + 0.7769777181711114 + ], + "collision_steps": 0, + "odometry_error_m": 0.014627753766444346 + }, + "teleop_check": { + "events": [ + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 0.24000000000000019 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 0.4200000000000003 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 0.6400000000000005 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 0.8600000000000007 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 1.0400000000000007 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 1.2200000000000009 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 1.380000000000001 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 1.5800000000000012 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 1.7800000000000014 + }, + { + "key": 87, + "command": [ + 0.25, + 0.0 + ], + "time": 1.9600000000000015 + }, + { + "key": 68, + "command": [ + 0.0, + -0.35 + ], + "time": 2.139999999999986 + }, + { + "key": 68, + "command": [ + 0.0, + -0.35 + ], + "time": 2.339999999999964 + }, + { + "key": 68, + "command": [ + 0.0, + -0.35 + ], + "time": 2.539999999999942 + }, + { + "key": 68, + "command": [ + 0.0, + -0.35 + ], + "time": 2.73999999999992 + }, + { + "key": 68, + "command": [ + 0.0, + -0.35 + ], + "time": 2.939999999999898 + }, + { + "key": 65, + "command": [ + 0.0, + 0.35 + ], + "time": 3.139999999999876 + }, + { + "key": 65, + "command": [ + 0.0, + 0.35 + ], + "time": 3.3399999999998538 + }, + { + "key": 65, + "command": [ + 0.0, + 0.35 + ], + "time": 3.519999999999834 + }, + { + "key": 65, + "command": [ + 0.0, + 0.35 + ], + "time": 3.699999999999814 + }, + { + "key": 65, + "command": [ + 0.0, + 0.35 + ], + "time": 3.899999999999792 + }, + { + "key": 83, + "command": [ + -0.25, + 0.0 + ], + "time": 4.0999999999997705 + }, + { + "key": 83, + "command": [ + -0.25, + 0.0 + ], + "time": 4.279999999999751 + }, + { + "key": 83, + "command": [ + -0.25, + 0.0 + ], + "time": 4.499999999999726 + }, + { + "key": 83, + "command": [ + -0.25, + 0.0 + ], + "time": 4.659999999999709 + }, + { + "key": 83, + "command": [ + -0.25, + 0.0 + ], + "time": 4.859999999999687 + }, + { + "key": 61, + "command": [ + -0.25, + 0.0 + ], + "time": 5.039999999999667 + }, + { + "key": 61, + "command": [ + -0.25, + 0.0 + ], + "time": 5.239999999999645 + }, + { + "key": 61, + "command": [ + -0.25, + 0.0 + ], + "time": 5.439999999999623 + }, + { + "key": 87, + "command": [ + 0.39999999999999997, + 0.0 + ], + "time": 5.599999999999605 + }, + { + "key": 87, + "command": [ + 0.39999999999999997, + 0.0 + ], + "time": 5.799999999999583 + }, + { + "key": 45, + "command": [ + 0.39999999999999997, + 0.0 + ], + "time": 5.999999999999561 + }, + { + "key": 45, + "command": [ + 0.39999999999999997, + 0.0 + ], + "time": 6.199999999999539 + }, + { + "key": 45, + "command": [ + 0.39999999999999997, + 0.0 + ], + "time": 6.379999999999519 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 6.579999999999497 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 6.779999999999475 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 6.979999999999453 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 7.165999999999433 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 7.3599999999994115 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 7.559999999999389 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 7.73999999999937 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 7.939999999999348 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 8.139999999999388 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 8.319999999999448 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 8.519999999999515 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 8.719999999999581 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 8.879999999999635 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 9.079999999999702 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 9.299999999999775 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 9.479999999999835 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 9.659999999999895 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 9.879999999999969 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 10.060000000000029 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 10.260000000000096 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 10.440000000000156 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 10.640000000000223 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 10.860000000000296 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 11.060000000000363 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 11.26000000000043 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 11.460000000000496 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 11.660000000000563 + }, + { + "key": 32, + "command": [ + 0.0, + 0.0 + ], + "time": 11.880000000000637 + } + ], + "final_position": [ + 0.6022900755554784, + -0.24782982735759396, + 0.778801981361132 + ], + "fallen": false, + "simulated_seconds": 12.000000000000677 + }, + "training": { + "perception": { + "test": { + "floor_iou": 0.7144569158554077 + }, + "history": [ + { + "epoch": 1, + "train_loss": 0.6515593886375427, + "validation_loss": 0.5862979888916016 + }, + { + "epoch": 2, + "train_loss": 0.5322169105211894, + "validation_loss": 0.41666868329048157 + }, + { + "epoch": 3, + "train_loss": 0.3731572171052297, + "validation_loss": 0.31106099486351013 + }, + { + "epoch": 4, + "train_loss": 0.2773292029897372, + "validation_loss": 0.2671062648296356 + }, + { + "epoch": 5, + "train_loss": 0.2249623293677966, + "validation_loss": 0.23184753954410553 + }, + { + "epoch": 6, + "train_loss": 0.19123106648524602, + "validation_loss": 0.20464134216308594 + }, + { + "epoch": 7, + "train_loss": 0.1699126695593198, + "validation_loss": 0.1904342621564865 + }, + { + "epoch": 8, + "train_loss": 0.15063215494155885, + "validation_loss": 0.17608746886253357 + }, + { + "epoch": 9, + "train_loss": 0.13404261047641436, + "validation_loss": 0.1671636402606964 + }, + { + "epoch": 10, + "train_loss": 0.12165957614779473, + "validation_loss": 0.15130828320980072 + }, + { + "epoch": 11, + "train_loss": 0.10908799370129903, + "validation_loss": 0.13740822672843933 + }, + { + "epoch": 12, + "train_loss": 0.10004713286956152, + "validation_loss": 0.13193221390247345 + }, + { + "epoch": 13, + "train_loss": 0.09509985347588858, + "validation_loss": 0.12277023494243622 + }, + { + "epoch": 14, + "train_loss": 0.08908139442404112, + "validation_loss": 0.11898733675479889 + }, + { + "epoch": 15, + "train_loss": 0.08403473868966102, + "validation_loss": 0.1125747337937355 + }, + { + "epoch": 16, + "train_loss": 0.08217032079895338, + "validation_loss": 0.11868017166852951 + }, + { + "epoch": 17, + "train_loss": 0.07921109373370806, + "validation_loss": 0.105145663022995 + }, + { + "epoch": 18, + "train_loss": 0.07703785821795464, + "validation_loss": 0.10191230475902557 + }, + { + "epoch": 19, + "train_loss": 0.07324557105700175, + "validation_loss": 0.0991356372833252 + }, + { + "epoch": 20, + "train_loss": 0.07076147037247817, + "validation_loss": 0.10474512726068497 + } + ], + "training_seconds": 18.795533418655396 + }, + "end2end": { + "test": { + "speed_mae_mps": 0.02873709797859192, + "yaw_rate_mae_radps": 0.07807976007461548 + }, + "history": [ + { + "epoch": 1, + "train_loss": 0.019927604349019627, + "validation_loss": 0.015247656032443047 + }, + { + "epoch": 2, + "train_loss": 0.013338427625906963, + "validation_loss": 0.012845995835959911 + }, + { + "epoch": 3, + "train_loss": 0.009953329991549253, + "validation_loss": 0.010511630214750767 + }, + { + "epoch": 4, + "train_loss": 0.008802419520604114, + "validation_loss": 0.013564219698309898 + }, + { + "epoch": 5, + "train_loss": 0.007528269515993695, + "validation_loss": 0.009634070098400116 + }, + { + "epoch": 6, + "train_loss": 0.005717495433054864, + "validation_loss": 0.009699787944555283 + }, + { + "epoch": 7, + "train_loss": 0.004978025514477243, + "validation_loss": 0.008820238523185253 + }, + { + "epoch": 8, + "train_loss": 0.0043911014023857815, + "validation_loss": 0.009981256909668446 + }, + { + "epoch": 9, + "train_loss": 0.003916206141002476, + "validation_loss": 0.009927424602210522 + }, + { + "epoch": 10, + "train_loss": 0.004110249632503837, + "validation_loss": 0.010983279906213284 + }, + { + "epoch": 11, + "train_loss": 0.003298697933011378, + "validation_loss": 0.009419215843081474 + }, + { + "epoch": 12, + "train_loss": 0.0026648445830990872, + "validation_loss": 0.007939483039081097 + }, + { + "epoch": 13, + "train_loss": 0.0024159081862308086, + "validation_loss": 0.008272224105894566 + }, + { + "epoch": 14, + "train_loss": 0.002422941313125193, + "validation_loss": 0.00868251733481884 + }, + { + "epoch": 15, + "train_loss": 0.0019108354560254763, + "validation_loss": 0.008090991526842117 + }, + { + "epoch": 16, + "train_loss": 0.0017981273510182898, + "validation_loss": 0.008004545234143734 + }, + { + "epoch": 17, + "train_loss": 0.0016011812549550086, + "validation_loss": 0.009002798236906528 + }, + { + "epoch": 18, + "train_loss": 0.001546536212360176, + "validation_loss": 0.008609442971646786 + }, + { + "epoch": 19, + "train_loss": 0.0014207457948941737, + "validation_loss": 0.008610133081674576 + }, + { + "epoch": 20, + "train_loss": 0.0012522446893854066, + "validation_loss": 0.009489416144788265 + } + ], + "training_seconds": 4.659215688705444 + }, + "mask_controller": { + "epochs": 100, + "validation_mse": 0.0027787438593804836, + "test_speed_mae_mps": 0.01168871857225895, + "test_yaw_rate_mae_radps": 0.048268433660268784 + } + }, + "planner_training": { + "train_maps": 240, + "validation_maps": 40, + "test_maps": 40, + "test_mae_cells": 0.7555620074272156, + "validation_mse": [ + 0.07789694517850876, + 0.02313491329550743, + 0.011575024574995041, + 0.00888085551559925, + 0.006188783794641495, + 0.005088148172944784, + 0.004368642810732126, + 0.003886079415678978, + 0.0029025787953287363, + 0.0028561274521052837, + 0.0027689868584275246, + 0.00214683311060071, + 0.0019310235511511564, + 0.001588753191754222, + 0.001514676958322525, + 0.0014852900058031082, + 0.001386171323247254, + 0.0013086803955957294, + 0.0012405538000166416, + 0.0011569717898964882 + ], + "scope": "synthetic occupancy maps; not visual training scenes" + }, + "perception_9": { + "mode": "perception", + "scene_seed": 9, + "simulated_seconds": 31.439999999993454, + "wall_seconds": 4.430427312850952, + "fallen": false, + "reached": true, + "final_position": [ + 7.5020529612010725, + -0.9344328852960936, + 0.7773054296349473 + ], + "collision_steps": 0, + "odometry_error_m": 0.03544455567901597 + }, + "perception_10": { + "mode": "perception", + "scene_seed": 10, + "simulated_seconds": 30.41999999999402, + "wall_seconds": 3.4013195037841797, + "fallen": false, + "reached": true, + "final_position": [ + 7.502263401781641, + -1.1874273503767911, + 0.7735630895421056 + ], + "collision_steps": 0, + "odometry_error_m": 0.05658640509787874 + }, + "perception_11": { + "mode": "perception", + "scene_seed": 11, + "simulated_seconds": 30.819999999993797, + "wall_seconds": 3.7954225540161133, + "fallen": false, + "reached": true, + "final_position": [ + 7.50444187888058, + -0.9793791840700157, + 0.7678394853311956 + ], + "collision_steps": 0, + "odometry_error_m": 0.021790147097236014 + }, + "perception_12": { + "mode": "perception", + "scene_seed": 12, + "simulated_seconds": 30.699999999993864, + "wall_seconds": 3.201212167739868, + "fallen": false, + "reached": true, + "final_position": [ + 7.501021051329815, + -1.1233923591479789, + 0.7770555982312694 + ], + "collision_steps": 0, + "odometry_error_m": 0.04592932339707591 + }, + "end2end_9": { + "mode": "end2end", + "scene_seed": 9, + "simulated_seconds": 29.559999999994496, + "wall_seconds": 2.964218854904175, + "fallen": false, + "reached": true, + "final_position": [ + 7.505344022030739, + -0.5925750182237367, + 0.7767791604811192 + ], + "collision_steps": 0, + "odometry_error_m": 0.03564252525587454 + }, + "end2end_10": { + "mode": "end2end", + "scene_seed": 10, + "simulated_seconds": 29.99999999999425, + "wall_seconds": 15.170369863510132, + "fallen": false, + "reached": true, + "final_position": [ + 7.502644446115033, + -0.21897350089130785, + 0.771064878556972 + ], + "collision_steps": 0, + "odometry_error_m": 0.05675195153087234 + }, + "end2end_11": { + "mode": "end2end", + "scene_seed": 11, + "simulated_seconds": 30.619999999993908, + "wall_seconds": 3.099447011947632, + "fallen": false, + "reached": true, + "final_position": [ + 7.504029707618278, + -0.7136290023136964, + 0.7776695657480839 + ], + "collision_steps": 0, + "odometry_error_m": 0.026391199870822925 + }, + "end2end_12": { + "mode": "end2end", + "scene_seed": 12, + "simulated_seconds": 31.13999999999362, + "wall_seconds": 2.786536693572998, + "fallen": false, + "reached": true, + "final_position": [ + 7.504242986277308, + -0.8676467751623445, + 0.7774885119970814 + ], + "collision_steps": 0, + "odometry_error_m": 0.0451717146363518 + } +} diff --git a/docs/humanoid/humanoid_visual_patrol/index.md b/docs/humanoid/humanoid_visual_patrol/index.md new file mode 100644 index 0000000000..b571335855 --- /dev/null +++ b/docs/humanoid/humanoid_visual_patrol/index.md @@ -0,0 +1,118 @@ +# 人形机器人视觉通行感知与室内导航 + +本示例在 MuJoCo 3.2.0 中驱动 Unitree G1,通过视觉通行识别和图像速度策略完成走廊通行,通过 ROS Noetic 完成同步建图导航。**当前为可复现原型,不是经过真实机器人验证的部署方案。** + +## 四项任务 + +| 项目 | 实现 | 当前证据 | +|---|---|---| +| 键盘运动 | WASD/方向键、调速、超时停止,经官方步态输出关节力矩 | 真实 GLFW 窗口收到自动发送的前进/转向/停止按键,12秒未跌倒 | +| 感知控制 | RGB 图像 → FloorNet → MaskController → 速度 | 两个留出场景闭环通过 | +| 建图导航 | 模拟雷达+带噪积分里程计 → gmapping 与 move_base 同时运行 | 神经辅助规划生成的五个航点均返回 SUCCEEDED | +| 端到端 | RGB 图像 → ImageController → 速度 → 步态底座 | 两个留出场景闭环通过 | + +任务二的感知和控制网络均经过训练;任务三采用经典 SLAM、神经网络辅助全局搜索及经典局部避障的混合方案,并非纯神经 SLAM 或纯神经规划。任务四学习图像到高层速度,并不直接输出全身关节力矩。 + +## 运行效果 + +以下为真实 MuJoCo 物理步进的离屏渲染,右上角是机载相机画面。 + +### 视觉感知控制 + +![感知控制](../../img/humanoid_visual_patrol/perception_7.gif) + +### 图像直接控制 + +![端到端控制](../../img/humanoid_visual_patrol/end2end_7.gif) + +### SLAM 地图 + +![gmapping 输出地图](../../img/humanoid_visual_patrol/slam_map.png) + +地图来自运行中的 `/map`,不是场景几何直接绘制。仿真真实位置仅用于离线评价;导航接收的是对带噪速度与航向增量积分的里程计。 + +## 系统原理 + +底层采用官方预训练 G1 12 腿部关节步态策略,固定上半身。物理频率 500 Hz,策略频率 50 Hz。观测包括角速度、重力投影、速度命令、关节位置/速度、前次动作及步态相位。策略输出关节位置残差,PD 控制产生力矩: + +$$\tau=K_p(q_{target}-q)-K_d\dot q,\qquad q_{target}=q_0+0.25a.$$ + +底层预训练网络没有在本项目重新训练。独立训练的三个视觉相关模型如下: + +| 模型 | 输入 | 结构 | 输出与损失 | +|---|---|---|---| +| FloorNet | 96×128 RGB | 5×5 Conv(16)、3×3 Conv(16)、1×1 Conv(1) | 逐像素地面概率;BCE | +| MaskController | 地面语义图 | 12×16池化、64/32维MLP | 前进速度、偏航角速度;MSE | +| ImageController | 96×128 RGB | 三层步长2卷积+64维全连接 | 前进速度、偏航角速度;MSE | + +示范策略在图像中部至下部比较候选通行条带,对偏离中心的条带施加惩罚,并按前方通行像素比例限制速度。MaskController 学习此策略,任务二在线阶段使用学习后的网络输出速度。针对分割结果完全没有地面像素的情况,部署端增加规则保护并将速度和转向置零;保护不是网络能力,也不能处理所有误分割或障碍。当前测试用固定的无地面分割输出验证保护逻辑;真实 FloorNet 在空白图上的误分割仍未单独测试。它是局部反应式控制器,不等价于全局最短路规划。 + +任务四以同一个视觉规则控制器产生示范标签,网络推理时只接收 RGB。两者形成“显式分割+语义控制网络”和“图像直接回归控制”的对照。速度限幅和缓动用于接口保护,不能掩盖模型失败。 + +## 数据与训练 + +MuJoCo 几何 ID 生成地面分割标签,无人工涂改。训练使用种子 0–5 共480张图,验证种子6共80张图,测试种子7–8共160张图。每个种子改变光照、墙面颜色及柜体位置,**属于同一走廊模板的变化,不是多个独立建筑**。 + +样本通过静态相机位置/朝向采样获得。只在数据采集和回合重置时设置根节点位置,闭环测试中机器人由关节控制与接触动力学移动。以验证损失选择模型,训练20轮,Adam学习率0.001,批量16,随机种子42。MaskController 只保留最佳验证 MSE 和测试误差,没有保存逐轮曲线;perception、end2end 和 planner 的验证历史保存在 [evaluation.json](evaluation.json)。 + +| 留出图像指标 | 结果 | +|---|---:| +| 地面分割 IoU | 0.7145 | +| 端到端速度 MAE | 0.0287 m/s | +| 端到端转向速度 MAE | 0.0781 rad/s | +| 语义控制网络速度 MAE | 0.0117 m/s | +| 语义控制网络转向速度 MAE | 0.0483 rad/s | + +种子9–12为追加的闭环测试,不参与训练或验证;它们仍然是同一走廊模板。训练图像误差不能代替闭环效果。下面是每个留出场景各一次物理运行,没有宣称统计显著性或大样本成功率。 + +| 模式 | 场景种子 | 到达走廊测试终线 | 仿真耗时 | 环境碰撞步数 | 跌倒 | +|---|---:|---|---:|---:|---| +| 规则教师 | 7 | 是 | 30.36 s | 0 | 否 | +| 感知控制 | 7 | 是 | 30.42 s | 0 | 否 | +| 感知控制 | 8 | 是 | 32.10 s | 0 | 否 | +| 端到端 | 7 | 是 | 29.46 s | 0 | 否 | +| 端到端 | 8 | 是 | 31.58 s | 0 | 否 | +| 感知控制 | 9 | 是 | 31.44 s | 0 | 否 | +| 端到端 | 9 | 是 | 29.56 s | 0 | 否 | +| 感知控制 | 10 | 是 | 30.42 s | 0 | 否 | +| 端到端 | 10 | 是 | 30.00 s | 0 | 否 | +| 感知控制 | 11 | 是 | 30.82 s | 0 | 否 | +| 端到端 | 11 | 是 | 30.62 s | 0 | 否 | +| 感知控制 | 12 | 是 | 30.70 s | 0 | 否 | +| 端到端 | 12 | 是 | 31.14 s | 0 | 否 | + +终线定义为机器人根节点 x≥7.5m;这是测试终止条件,并非网络的输入或控制依据。碰撞只统计墙、门框、柜体接触;正常足地接触不计为障碍碰撞。原始试验摘要见 [evaluation.json](evaluation.json)。 + +### 端到端近墙测试 + +使用实际交付的 ImageController 权重,将骨盆初始位置分别设在距走廊端墙近表面 1.5、1.0、0.6 m 处,先零速稳定 2 秒,再运行控制 3 秒。三次均未跌倒,未记录与端墙的接触;最后一种情况下控制器最初输出零前进速度并转向。记录见 [near_obstacle_20261111.json](near_obstacle_20261111.json),复现使用源码目录的 `scripts/evaluate_near_obstacle.py --assets <资产路径> --output <结果路径>`。距离是初始骨盆到墙面的距离,不是机器人轮廓的间距;初始零速期间可能有漂移。仅测试单个静态墙面、朝向和种子,不能据此声称覆盖复杂障碍或保证安全。 + +## 同步 SLAM 与导航 + +180束平面雷达,5Hz,量程8m,排除机器人自体几何。ROS 桥发布 `/clock`、`/scan`、`/odom` 以及 `odom→base_link→laser`,gmapping 发布 `map→odom` 和 `/map`。图像消息的 `front_camera` 坐标系目前未发布对应 TF,因此不能直接用于跨坐标系融合。move_base 读取持续变化的地图,向 `/cmd_vel` 发布速度。 + +测试顺序目标为 (2,-0.3)、(4,-0.3)、(7,0),三次均返回 action 状态3。位置容差 **0.5m**,终点朝向不约束。测试期间记录21次地图更新,已知栅格由11,143增至18,328。不能将该结果表述为厘米级导航。 + +另一次神经辅助导航测试从实时地图和 TF 位置生成5个航点,最终目标为 (7,0),五个航点均返回 SUCCEEDED。CostNet 在240个随机障碍栅格上训练,验证与测试各40个独立种子,Dijkstra 距离为监督标签,训练20轮;测试距离 MAE 为0.7556格。 + +CostNet 输入障碍图与相对目标的 x/y 坐标,使用空洞卷积预测剩余代价。A* 的主排序保持 `g + Manhattan`,仅在同一主代价时使用预测值打破平局,因此是神经辅助搜索。当前实测神经辅助与普通 A* 均扩展47个节点、路径代价均为46格,**没有观察到本回合的搜索加速**。未知栅格暂按可通行处理以支持探索,实际执行仍依赖实时雷达局部避障;不保证未知区域安全或任意地图都能到达。规划路径发布在 `/neural_patrol/path`,执行结果在 `/neural_patrol/result`。 + +首次测试发现 TF 延迟与里程计角速度遗漏,修正后重新运行。步态零速仍有小幅漂移,长时间停车和精确停靠尚需进一步评价。 + +## 安装与复现 + +完整命令、环境、资产下载、四种模式和退出步骤见 [模块 README](https://github.com/OpenHUTB/ros2/tree/master/src/humanoid/humanoid_visual_patrol)。本页面待 PR 合并前,上述上游代码链接可能尚不存在,请使用本分支同路径。 + +运行顺序:安装独立仿真环境 → 获取固定版本官方资产 → smoke → 遥控 → collect/train → perception/end2end → 启动 ROS 同步导航 → 保存地图 → 检查输出证据。 + +四个模型权重随模块提供(分割约17KB,语义控制约61KB,图像控制约1.64MB,规划约40KB)。无需先训练即可测试,训练步骤可独立复现。大量数据和录像只保留本地;文档展示 GIF 小于5MB,PNG小于3MB。 + +## 与已有模块的区别及限制 + +已有 `humanoid_lidar_nn_perception` 以雷达扫描预测通行距离并用 MLP 避障。本模块学习 RGB 地面分割与图像速度回归,并集成同步 SLAM 多目标导航;它不是对原模块改名。共享的 G1 模型、官方步态和 ROS 软件明确归属于上游。 + +目前缺少动态人群、复杂光照、不同建筑、大规模重复试验,以及跌倒后的恢复。视觉训练数据规模小,IoU仍有提升空间。外部机器复现和正式维护者信息尚待补齐,当前不能直接勾选全部 PR 验收项。 + +## 大模型使用声明 + +本项目使用 OpenAI Codex 辅助编程、调试、实验和文档整理。训练及仿真结果由实际程序产生;提交者需复核并对提交内容负责。官方模型、预训练策略及许可证详见源码 `third_party_notices.md`。 diff --git a/docs/humanoid/humanoid_visual_patrol/near_obstacle_20261111.json b/docs/humanoid/humanoid_visual_patrol/near_obstacle_20261111.json new file mode 100644 index 0000000000..6cf05d96f8 --- /dev/null +++ b/docs/humanoid/humanoid_visual_patrol/near_obstacle_20261111.json @@ -0,0 +1,236 @@ +[ + { + "distance_m": 1.5, + "settled": true, + "start_position": [ + 8.417243170839143, + -0.01578553768493515, + 0.775756219577456 + ], + "final_position": [ + 8.730576156913775, + -0.16362254449528404, + 0.7793329966064625 + ], + "fallen": false, + "wall_contact_steps": 0, + "commands": [ + [ + 0.14146247506141663, + -0.17015895247459412 + ], + [ + 0.11891498416662216, + -0.24590933322906494 + ], + [ + 0.1121121346950531, + -0.22971871495246887 + ], + [ + 0.11599573493003845, + -0.21102428436279297 + ], + [ + 0.13836979866027832, + -0.2381228506565094 + ], + [ + 0.1183127835392952, + -0.33010783791542053 + ], + [ + 0.10987158119678497, + -0.3273693919181824 + ], + [ + 0.12709695100784302, + -0.2385319620370865 + ], + [ + 0.13229939341545105, + -0.25760507583618164 + ], + [ + 0.10650484263896942, + -0.35021722316741943 + ], + [ + 0.11009601503610611, + -0.26059627532958984 + ], + [ + 0.1392822563648224, + -0.13386747241020203 + ], + [ + 0.1288258582353592, + -0.20020955801010132 + ], + [ + 0.10298807919025421, + -0.2853042483329773 + ], + [ + 0.12290354818105698, + -0.17758265137672424 + ] + ] + }, + { + "distance_m": 1.0, + "settled": true, + "start_position": [ + 8.917243170839134, + -0.015785537684923127, + 0.7757562195774552 + ], + "final_position": [ + 9.090908049428544, + -0.10127337209597133, + 0.781319078402443 + ], + "fallen": false, + "wall_contact_steps": 0, + "commands": [ + [ + 0.08929011225700378, + -0.09286505728960037 + ], + [ + 0.08064748346805573, + -0.1253059059381485 + ], + [ + 0.07548756152391434, + -0.11916515976190567 + ], + [ + 0.07235558331012726, + -0.1257079541683197 + ], + [ + 0.06656993180513382, + -0.21493399143218994 + ], + [ + 0.03834439069032669, + -0.3157784938812256 + ], + [ + 0.025117628276348114, + -0.3475883901119232 + ], + [ + 0.02754291146993637, + -0.3555821180343628 + ], + [ + 0.01775287836790085, + -0.4294828176498413 + ], + [ + 0.013274461030960083, + -0.45 + ], + [ + 0.029487960040569305, + -0.42537784576416016 + ], + [ + 0.058489810675382614, + -0.33308491110801697 + ], + [ + 0.06443822383880615, + -0.3198435604572296 + ], + [ + 0.05350431799888611, + -0.36891812086105347 + ], + [ + 0.07223033905029297, + -0.2864035665988922 + ] + ] + }, + { + "distance_m": 0.6, + "settled": true, + "start_position": [ + 9.317243170839143, + -0.0157855376849174, + 0.7757562195774554 + ], + "final_position": [ + 9.386478171746983, + -0.07180124879056617, + 0.7820914707217922 + ], + "fallen": false, + "wall_contact_steps": 0, + "commands": [ + [ + 0.0, + -0.07231006026268005 + ], + [ + 0.0, + -0.13426412642002106 + ], + [ + 0.0, + -0.12162493169307709 + ], + [ + 0.0, + -0.1387452930212021 + ], + [ + 0.0, + -0.2208845317363739 + ], + [ + 0.0, + -0.33562785387039185 + ], + [ + 0.0, + -0.35593006014823914 + ], + [ + 0.0, + -0.39087188243865967 + ], + [ + 0.0, + -0.45 + ], + [ + 0.0, + -0.45 + ], + [ + 0.0, + -0.45 + ], + [ + 0.0, + -0.45 + ], + [ + 0.0, + -0.41778644919395447 + ], + [ + 0.041777290403842926, + -0.35124245285987854 + ], + [ + 0.07274018228054047, + -0.26592519879341125 + ] + ] + } +] diff --git a/docs/img/humanoid_visual_patrol/end2end_7.gif b/docs/img/humanoid_visual_patrol/end2end_7.gif new file mode 100644 index 0000000000..de0550a204 Binary files /dev/null and b/docs/img/humanoid_visual_patrol/end2end_7.gif differ diff --git a/docs/img/humanoid_visual_patrol/end2end_7.png b/docs/img/humanoid_visual_patrol/end2end_7.png new file mode 100644 index 0000000000..b2b5756e7a Binary files /dev/null and b/docs/img/humanoid_visual_patrol/end2end_7.png differ diff --git a/docs/img/humanoid_visual_patrol/perception_7.gif b/docs/img/humanoid_visual_patrol/perception_7.gif new file mode 100644 index 0000000000..421110ed17 Binary files /dev/null and b/docs/img/humanoid_visual_patrol/perception_7.gif differ diff --git a/docs/img/humanoid_visual_patrol/slam_map.png b/docs/img/humanoid_visual_patrol/slam_map.png new file mode 100644 index 0000000000..e88d3ac6ea Binary files /dev/null and b/docs/img/humanoid_visual_patrol/slam_map.png differ diff --git a/docs/index.md b/docs/index.md index af2b8fd076..9e3a1dd814 100644 --- a/docs/index.md +++ b/docs/index.md @@ -6,6 +6,8 @@ title: 主页 文档环境初始化:[Ubuntu 一键安装与本地预览](./ubuntu_docs_setup.md)。 +人形机器人示例:[视觉通行感知与室内巡检](./humanoid/humanoid_visual_patrol/index.md),包含 MuJoCo G1、RGB 分割控制、同步 SLAM 导航和图像速度策略。 + **提交项目之前**,请按以下指引自查,并阅读[仓库提交约定](https://github.com/OpenHUTB/ros2#注意事项)。PR 模板只列常见检查项;具体任务、模拟器和验收要求以仓库约定及维护者说明为准。 diff --git a/mkdocs.yml b/mkdocs.yml index 58a39225e1..d787bec5d7 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -75,6 +75,7 @@ nav: - 10. Turtlebot3 Gazebo键盘控制: 'chapter/turtlebot3_gazebo_control.md' - 人形机器人: + - 人形机器人视觉通行感知与室内巡检: './humanoid/humanoid_visual_patrol/index.md' - 人形机器人基础: './humanoid/humanoid_basics.md' - 双臂搬运负载的神经网络平衡控制: './humanoid/humanoid_carry_payload_nn.md' - 双足步态生成与 ZMP 稳定性: './humanoid/bipedal_gait_zmp.md' diff --git a/src/humanoid/humanoid_visual_patrol/.gitattributes b/src/humanoid/humanoid_visual_patrol/.gitattributes new file mode 100644 index 0000000000..61d299b53f --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/.gitattributes @@ -0,0 +1,2 @@ +*.py text eol=lf +*.sh text eol=lf diff --git a/src/humanoid/humanoid_visual_patrol/CMakeLists.txt b/src/humanoid/humanoid_visual_patrol/CMakeLists.txt new file mode 100644 index 0000000000..d23806fcda --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.0.2) +project(humanoid_visual_patrol) +find_package(catkin REQUIRED) +catkin_package() +catkin_install_python(PROGRAMS scripts/ros_bridge.py scripts/task_runner.py scripts/neural_planner.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/humanoid/humanoid_visual_patrol/README.md b/src/humanoid/humanoid_visual_patrol/README.md new file mode 100644 index 0000000000..946fc81218 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/README.md @@ -0,0 +1,104 @@ +# 人形机器人视觉通行感知与室内导航 + +在 MuJoCo 3.2.0 中使用 Unitree G1 的预训练步态,开发 RGB 地面分割、图像到速度控制及 ROS Noetic 同步 SLAM 导航。模块名:`humanoid_visual_patrol`。 + +完整原理、实验和限制见 [课程文档](../../../docs/humanoid/humanoid_visual_patrol/index.md)。 + +## 功能与边界 + +| 任务 | 入口 | 实现 | +|---|---|---| +| 键盘控制 | `main.py --mode teleop` | WASD/方向键、空格停止、加减速度,0.6秒未按键归零 | +| 感知控制 | `--mode perception` | RGB → FloorNet 地面分割 → MaskController 语义控制网络 → 行走速度 | +| 建图导航 | `launch/main.launch` 默认 navigation | 模拟雷达及积分里程计 → gmapping → CostNet 辅助 A* → move_base → G1 | +| 端到端 | `--mode end2end` | RGB → ImageController → 前进/转向速度 → 预训练步态 | + +任务二的分割与语义控制网络分别训练,规则通道选择只用于生成示范及对照;部署时若分割图完全没有地面像素,额外的规则保护会将速度和转向置零,并不代表网络自身能处理这种情况。该保护不覆盖少量误分割或所有障碍。任务三使用经典 gmapping、CostNet 辅助全局 A* 和 move_base 局部避障,是混合规划方案,并非神经网络 SLAM。任务四直接输出高层速度,不是图像到关节力矩。底层步态是第三方预训练策略,不计作本项目训练成果。 + +本示例的终线是通行测试终点,场景标记不触发巡检事件。 + +## 已验证环境 + +- 当前复现步骤仅验证源码空间运行,不保证 catkin install 空间运行。 +- `catkin_make` 源码空间构建通过;`catkin_make install` 构建通过,但安装空间只有 scripts/launch/config,不含 `models/` 与 `patrol` 包,因此仍按源码空间运行。 +- Windows 11,Python 3.11.15,MuJoCo 3.2.0,PyTorch 2.15.0+cpu:物理仿真、图像采集、训练、离屏录像。 +- WSL2 Ubuntu 20.04.5,ROS Noetic,Python 3.8.10:gmapping、move_base 和 ROS 桥。 +- Windows 和 WSL 用子进程标准输入输出连接,不需要开放网络端口给仿真器。 +- 本次使用 CPU 训练;未声明 GPU 性能。原生 Linux 仿真解释器路径受支持,但本次未验证该部署组合。 + +## 安装与获取资产 + +在本模块目录运行(以下 `python` 指独立仿真环境解释器): + +```bash +python -m pip install -r requirements.txt +python scripts/fetch_assets.py /path/to/unitree_rl_gym +``` + +资产固定到官方 `unitree_rl_gym` 提交 `276801e46c5d433564f24658bac64f254b7d2d4b`,含 G1 12 关节腿部模型、网格和 `deploy/pre_train/g1/motion.pt`。上半身模型固定,不能声称实现全身灵巧操作。许可证见 `third_party_notices.md`。 + +```bash +python main.py --assets /path/to/unitree_rl_gym --mode smoke --output results/smoke.json +python main.py --assets /path/to/unitree_rl_gym --mode teleop +python main.py --assets /path/to/unitree_rl_gym --mode perception --weights models --record --output results +python main.py --assets /path/to/unitree_rl_gym --mode end2end --weights models --record --output results +``` + +关闭可视窗口停止遥控,默认45仿真秒后退出,可用 `--seconds` 调整。需连续按键维持运动;已通过向真实 GLFW 窗口发送 Windows 按键事件验证前进、转向和停止,仍建议审核者实际操作复核手感。 + +## 独立训练与测试 + +```bash +python main.py --assets /path/to/unitree_rl_gym --mode collect --dataset data/visual_patrol --samples 80 +python main.py --assets /path/to/unitree_rl_gym --mode train --dataset data/visual_patrol --weights models --epochs 20 +python main.py --assets /path/to/unitree_rl_gym --mode train_planner --weights models +python main.py --assets /path/to/unitree_rl_gym --mode perception --weights models --seed 8 --output results --record +python main.py --assets /path/to/unitree_rl_gym --mode end2end --weights models --seed 8 --output results --record +``` + +训练/验证/测试分别使用场景种子 0–5、6、7–8,每个场景 80 张图像。随机化涉及光照、颜色和柜体侧向位置;两套测试场景并不代表不同建筑。标签由仿真几何 ID 产生,训练图像来自静态相机姿态采样,闭环测试中才使用真实步态运动。测试结果不能外推到楼梯、人群或真实机器人。 + +## ROS 启动(本次验证方式) + +在 WSL 中,以本模块目录为当前目录: + +```bash +bash scripts/run_navigation.sh \ + /mnt/f/path/to/.venv_humanoid/Scripts/python.exe \ + /mnt/f/path/to/unitree_rl_gym navigation +``` + +脚本自动转换 Windows 子进程使用的路径。`ROS_MASTER_URI` 默认为 `http://localhost:11331`;其他终端也需设置这个值。手动设置路径时: + +```bash +source /opt/ros/noetic/setup.bash +export ROS_MASTER_URI=http://localhost:11331 +export ROS_PACKAGE_PATH="$PWD:$ROS_PACKAGE_PATH" +roslaunch humanoid_visual_patrol main.launch \ + sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym +``` + +先等待 `/map` 更新,再通过 RViz 的 2D Nav Goal 发送地图坐标目标,由神经辅助规划节点生成并执行航点;直接调用 `/move_base` action 则用于经典规划基线对照。导航与 SLAM 同时运行;位置容差 0.5m,终点朝向不约束。不要同时运行独立遥控和导航去控制同一个仿真实例。 + +`task:=teleop`、`task:=perception`、`task:=end2end` 可以经相同 launch 调用对应任务。后两项需传入 `weights`,输出路径通过 `output` 指定。 + +```bash +rosrun map_server map_saver -f /path/to/saved_map +``` + +Ctrl+C 退出 launch 后桥接进程会关闭其仿真子进程。 + +## 本地验证 + +```bash +python -m pip install -r requirements-dev.txt +# Linux: export HUMANOID_ASSETS=/path/to/unitree_rl_gym +# PowerShell: $env:HUMANOID_ASSETS='F:/path/to/unitree_rl_gym' +python -m pytest tests -q +``` + +包括无地面时停止、左右通行方向、神经平局排序不改变最优栅格路径代价、无路径报错,以及真实动力学行走和雷达自体回波排除。没有资产路径时,物理测试明确跳过。 + +## 大模型使用声明 + +本模块使用 OpenAI Codex 辅助方案、编程、调试、文档和实验整理。发布者需对内容复核并负责;预训练模型和第三方来源已单独说明。 diff --git a/src/humanoid/humanoid_visual_patrol/config/navigation.yaml b/src/humanoid/humanoid_visual_patrol/config/navigation.yaml new file mode 100644 index 0000000000..d4b73c4910 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/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/humanoid_visual_patrol/launch/main.launch b/src/humanoid/humanoid_visual_patrol/launch/main.launch new file mode 100644 index 0000000000..1c45d50d5e --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/launch/main.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/humanoid/humanoid_visual_patrol/main.py b/src/humanoid/humanoid_visual_patrol/main.py new file mode 100644 index 0000000000..2827fe4a73 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/main.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +"""Train and run humanoid visual patrol, teleoperation and ROS bridge tasks.""" +import argparse +import json +from pathlib import Path + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--assets', required=True, help='Path to unitree_rl_gym checkout') + parser.add_argument('--mode', choices=['smoke', 'collect', 'train', 'teacher', 'perception', 'end2end', 'teleop', 'bridge', 'train_planner', 'plan_stdio'], default='smoke') + parser.add_argument('--output', type=Path) + parser.add_argument('--dataset', type=Path, default=Path('data/visual_patrol')) + parser.add_argument('--weights', type=Path, default=Path(__file__).resolve().parent / 'models') + parser.add_argument('--samples', type=int, default=100) + parser.add_argument('--epochs', type=int, default=15) + parser.add_argument('--seed', type=int, default=7) + parser.add_argument('--seconds', type=float, default=45) + parser.add_argument('--record', action='store_true') + parser.add_argument('--controller',choices=['navigation','perception','end2end'],default='navigation') + args = parser.parse_args() + if args.mode == 'train_planner': + from patrol.planning import train_planner + print(json.dumps(train_planner(args.weights),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))) + return + if args.mode == 'bridge': + from patrol.bridge_server import serve + serve(args.assets,args.seed,args.controller,args.weights) + return + if args.mode == 'collect': + from patrol.learning import collect + collect(args.assets, args.dataset, args.samples) + return + if args.mode == 'train': + from patrol.learning import train, train_mask_controller + train(args.dataset, args.weights, args.epochs) + train_mask_controller(args.dataset,args.weights) + return + if args.mode == 'teleop': + from patrol.runtime import teleop + teleop(args.assets,args.seed,args.seconds,args.output) + return + if args.mode in ('teacher','perception','end2end'): + from patrol.runtime import rollout + result = rollout(args.assets,args.mode,args.weights,args.output or Path('results'), + args.seed,args.seconds,args.record) + print(json.dumps(result,indent=2)) + return + from patrol.locomotion import Walker + result = Walker(args.assets).smoke_test() + report = json.dumps(result, indent=2) + print(report) + if args.output: + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(report+'\n', encoding='utf-8') + if result['fallen'] or result['displacement_m'] < 0.5: + raise SystemExit(1) + + +if __name__ == '__main__': + main() diff --git a/src/humanoid/humanoid_visual_patrol/models/end2end.pt b/src/humanoid/humanoid_visual_patrol/models/end2end.pt new file mode 100644 index 0000000000..871eac4fb1 Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/end2end.pt differ diff --git a/src/humanoid/humanoid_visual_patrol/models/mask_controller.pt b/src/humanoid/humanoid_visual_patrol/models/mask_controller.pt new file mode 100644 index 0000000000..5bcc069e20 Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/mask_controller.pt differ diff --git a/src/humanoid/humanoid_visual_patrol/models/perception.pt b/src/humanoid/humanoid_visual_patrol/models/perception.pt new file mode 100644 index 0000000000..1c8b1565dd Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/perception.pt differ diff --git a/src/humanoid/humanoid_visual_patrol/models/planner.pt b/src/humanoid/humanoid_visual_patrol/models/planner.pt new file mode 100644 index 0000000000..97c8a1cad3 Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/planner.pt differ diff --git a/src/humanoid/humanoid_visual_patrol/package.xml b/src/humanoid/humanoid_visual_patrol/package.xml new file mode 100644 index 0000000000..53c60223be --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/package.xml @@ -0,0 +1,13 @@ + + + humanoid_visual_patrol0.1.0 + MuJoCo G1 visual floor perception, image control and ROS indoor navigation. + OpenHUTB contributor + BSD-3-Clause + catkin + rospytf + sensor_msgsgeometry_msgs + nav_msgsrosgraph_msgs + gmappingmove_base + actionlibmove_base_msgsstd_msgs + diff --git a/src/humanoid/humanoid_visual_patrol/patrol/__init__.py b/src/humanoid/humanoid_visual_patrol/patrol/__init__.py new file mode 100644 index 0000000000..bc4204b4fb --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/__init__.py @@ -0,0 +1 @@ +"""Visual patrol on a physics simulated G1 humanoid.""" diff --git a/src/humanoid/humanoid_visual_patrol/patrol/bridge_server.py b/src/humanoid/humanoid_visual_patrol/patrol/bridge_server.py new file mode 100644 index 0000000000..c358ab2ac3 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/bridge_server.py @@ -0,0 +1,43 @@ +"""JSON lines over stdio: a process boundary between ROS and simulator Python.""" +import json +import sys +import base64 +from pathlib import Path + +from .scene import IndoorSimulator + + +def serve(assets, seed=0, controller='navigation',weights=None): + sim = IndoorSimulator(assets,seed) + if controller in ('perception','end2end'): + from .vision import VisionPolicy + policy=VisionPolicy(Path(weights)/f'{controller}.pt',controller) + else: + policy=None + try: + for line in sys.stdin: + request = json.loads(line) + if request.get('stop'): + break + command = request.get('command',[0,0,0]) + if policy and sim.data.time>=2: + prediction,_=policy.command(sim.image()) + command=[float(prediction[0]),0,float(prediction[1])] + 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() + result = dict(time=float(sim.data.time), odom=sim.odom.tolist(), + scan=sim.scan(), velocity=[float(sim.data.qvel[0]),float(sim.data.qvel[1])], + angular_velocity=float((sim.odom[2]-previous_yaw)/(sim.data.time-previous_time)), + 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/humanoid_visual_patrol/patrol/learning.py b/src/humanoid/humanoid_visual_patrol/patrol/learning.py new file mode 100644 index 0000000000..0c1d12a45b --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/learning.py @@ -0,0 +1,134 @@ +"""Reproducible simulator data and held-out-scene evaluation.""" +import json +from pathlib import Path +import time + +import mujoco +import numpy as np +import torch +from torch import nn +from torch.utils.data import DataLoader, TensorDataset + +from .scene import IndoorSimulator +from .vision import FloorNet, ImageController, MaskController, mask_command + + +def collect(assets, destination, samples=100): + destination = Path(destination) + destination.mkdir(parents=True, exist_ok=True) + for split, seeds in [('train', range(6)), ('validation', [6]), ('test', [7, 8])]: + images, masks, actions, scene_ids = [], [], [], [] + for seed in seeds: + sim = IndoorSimulator(assets, seed) + rng = np.random.default_rng(1000+seed) + try: + for _ in range(samples): + # Independent static camera sampling, NOT a runtime walking method. + mujoco.mj_resetData(sim.model, sim.data) + sim.data.qpos[:2] = [rng.uniform(.0, 8.8), rng.uniform(-.9, .9)] + yaw = rng.uniform(-.65, .65) + sim.data.qpos[3:7] = [np.cos(yaw/2), 0, 0, np.sin(yaw/2)] + sim.data.qpos[7:] = sim.default + mujoco.mj_forward(sim.model, sim.data) + rgb, mask = sim.image(labels=True) + images.append(rgb) + masks.append(mask) + actions.append(mask_command(mask)) + scene_ids.append(seed) + finally: + sim.close() + print(f'collected {split} scene={seed}', flush=True) + np.savez_compressed(destination/f'{split}.npz', images=np.asarray(images), + masks=np.asarray(masks), actions=np.asarray(actions), scene_ids=scene_ids) + metadata = dict(split_by='scene_seed', train=list(range(6)), validation=[6], test=[7,8], + samples_per_scene=samples, labels='MuJoCo geometry-ID floor masks', + teacher='reactive floor-mask corridor following', + limitations='Static camera poses; appearance/layout randomization is limited. Closed-loop testing required.') + (destination/'dataset.json').write_text(json.dumps(metadata, indent=2), encoding='utf-8') + + +def train(dataset, output, epochs=15): + torch.manual_seed(42) + torch.set_num_threads(4) + dataset, output = Path(dataset), Path(output) + output.mkdir(parents=True, exist_ok=True) + def read(split): + with np.load(dataset/f'{split}.npz') as d: + return (torch.from_numpy(d['images']).permute(0,3,1,2).float()/255, + torch.from_numpy(d['masks']).float()[:,None], + torch.from_numpy(d['actions']).float()) + train_data, val_data, test_data = read('train'), read('validation'), read('test') + metrics = {} + for mode, model, idx in [('perception', FloorNet(), 1), ('end2end', ImageController(), 2)]: + loader = DataLoader(TensorDataset(train_data[0],train_data[idx]), batch_size=16, shuffle=True) + optimizer = torch.optim.Adam(model.parameters(), lr=.001) + loss_fn = nn.BCEWithLogitsLoss() if idx == 1 else nn.MSELoss() + best = float('inf') + history = [] + start = time.time() + for epoch in range(epochs): + model.train() + total = 0. + for x,y in loader: + optimizer.zero_grad() + loss = loss_fn(model(x),y) + loss.backward() + optimizer.step() + total += float(loss.detach())*len(x) + model.eval() + with torch.inference_mode(): + val = float(loss_fn(model(val_data[0]),val_data[idx])) + history.append(dict(epoch=epoch+1, train_loss=total/len(train_data[0]), validation_loss=val)) + if val < best: + best = val + torch.save(model.state_dict(),output/f'{mode}.pt') + print(mode,history[-1],flush=True) + model.load_state_dict(torch.load(output/f'{mode}.pt',weights_only=True)) + with torch.inference_mode(): + pred = model(test_data[0]) + if idx == 1: + pred = pred.sigmoid()>.5 + truth = test_data[idx]>.5 + score = dict(floor_iou=float((pred & truth).sum()/(pred | truth).sum().clamp(min=1))) + else: + error = (pred-test_data[idx]).abs().mean(0) + score = dict(speed_mae_mps=float(error[0]), yaw_rate_mae_radps=float(error[1])) + metrics[mode] = dict(test=score, history=history, training_seconds=time.time()-start) + (output/'metrics.json').write_text(json.dumps(metrics,indent=2),encoding='utf-8') + return metrics + + +def train_mask_controller(dataset, output, epochs=100): + """Train on scene-disjoint semantic masks; test only after model selection.""" + torch.manual_seed(73) + torch.set_num_threads(2) + dataset,output=Path(dataset),Path(output) + def load(split): + with np.load(dataset/f'{split}.npz') as d: + return torch.from_numpy(d['masks']).float()[:,None],torch.from_numpy(d['actions']) + x,y=load('train') + xv,yv=load('validation') + xt,yt=load('test') + model=MaskController() + optimizer=torch.optim.Adam(model.parameters(),lr=.002) + loader=DataLoader(TensorDataset(x,y),batch_size=32,shuffle=True) + best=float('inf') + for epoch in range(epochs): + model.train() + for xb,yb in loader: + loss=nn.functional.mse_loss(model(xb),yb) + optimizer.zero_grad(); loss.backward(); optimizer.step() + model.eval() + with torch.inference_mode(): + val=float(nn.functional.mse_loss(model(xv),yv)) + if val= 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/humanoid_visual_patrol/patrol/planning.py b/src/humanoid/humanoid_visual_patrol/patrol/planning.py new file mode 100644 index 0000000000..ee38a57ac0 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/planning.py @@ -0,0 +1,159 @@ +"""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 + +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) + return np.stack([grid.astype(np.float32),(xx-goal[1])/32.,(yy-goal[0])/32.]).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(.3/resolution)) + inflated=blocked.copy() + for dy in range(-radius,radius+1): + for dx in range(-radius,radius+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] + stride=3 + 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=(int((xy[1]-oy)/cell),int((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/humanoid_visual_patrol/patrol/runtime.py b/src/humanoid/humanoid_visual_patrol/patrol/runtime.py new file mode 100644 index 0000000000..7f381f483b --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/runtime.py @@ -0,0 +1,110 @@ +"""Closed-loop rollouts, keyboard control, and actual simulator recordings.""" +import json +from pathlib import Path +import sys +import time + +import imageio.v2 as imageio +import mujoco +import numpy as np +from PIL import Image, ImageDraw + +from .scene import IndoorSimulator +from .vision import VisionPolicy, mask_command + + +def rollout(assets, mode, weights, output, seed=7, seconds=45, record=False): + output = Path(output) + output.mkdir(parents=True, exist_ok=True) + sim = IndoorSimulator(assets,seed) + policy = VisionPolicy(Path(weights)/f'{mode}.pt', mode) if mode in ('perception','end2end') else None + trajectory, frames = [], [] + overview = mujoco.Renderer(sim.model,height=240,width=320) if record else None + camera = mujoco.MjvCamera() + camera.azimuth, camera.elevation, camera.distance = 140, -65, 3.5 + collision_steps = 0 + previous = np.zeros(2) + start = time.time() + try: + for tick in range(int(seconds/.02)): + if tick%10 == 0: + rgb, mask = sim.image(labels=True) + command = policy.command(rgb)[0] if policy else mask_command(mask) + if tick<100: + command[:] = 0 + previous = .6*previous+.4*command + sim.set_command(float(previous[0]),wz=float(previous[1])) + trajectory.append([float(sim.data.time), *sim.data.qpos[:3].tolist(), sim.yaw, + *previous.tolist(), *sim.odom.tolist()]) + if overview and tick%20 == 0: + camera.lookat[:] = sim.data.qpos[:3] + overview.update_scene(sim.data,camera=camera) + frame = Image.fromarray(overview.render()) + inset = Image.fromarray(rgb).resize((128,96)) + frame.paste(inset,(192,0)) + ImageDraw.Draw(frame).text((8,8),f'{mode} t={sim.data.time:.1f}s',fill='white') + frames.append(np.asarray(frame)) + sim.step() + for contact in sim.data.contact: + names = [mujoco.mj_id2name(sim.model,mujoco.mjtObj.mjOBJ_GEOM,int(g)) or '' + for g in (contact.geom1,contact.geom2)] + if any(n.endswith('wall') or n.startswith('door_') or n=='cabinet' for n in names): + collision_steps += 1 + break + if sim.fallen or sim.data.qpos[0]>=7.5: + break + result = dict(mode=mode,scene_seed=seed,simulated_seconds=float(sim.data.time), + wall_seconds=time.time()-start,fallen=bool(sim.fallen), + reached=bool(sim.data.qpos[0]>=7.5 and not sim.fallen), + final_position=sim.data.qpos[:3].tolist(),collision_steps=collision_steps, + odometry_error_m=float(np.linalg.norm(sim.odom[:2]-sim.data.qpos[:2]))) + (output/f'{mode}_{seed}.json').write_text(json.dumps(result,indent=2),encoding='utf-8') + np.savetxt(output/f'{mode}_{seed}.csv',trajectory,delimiter=',', + header='time,x,y,z,yaw,vx,wz,odom_x,odom_y,odom_yaw',comments='') + if frames: + imageio.mimsave(output/f'{mode}_{seed}.gif',frames,duration=.4,loop=0) + Image.fromarray(frames[-1]).save(output/f'{mode}_{seed}.png') + return result + finally: + if overview: + overview.close() + sim.close() + + +def teleop(assets,seed=0,seconds=45,output=None): + """Interactive MuJoCo viewer; arrow/WASD commands expire after 0.6s.""" + import mujoco.viewer + sim = IndoorSimulator(assets,seed) + command = np.zeros(2) + speed = [.25] + last_key = [0.] + events = [] + def key(code): + if code in (87,265): command[:] = [speed[0],0] + elif code in (83,264): command[:] = [-speed[0],0] + elif code in (65,263): command[:] = [0,.35] + elif code in (68,262): command[:] = [0,-.35] + elif code == 32: command[:] = 0 + elif code in (61,334): speed[0] = min(.4,speed[0]+.05) + elif code in (45,333): speed[0] = max(.1,speed[0]-.05) + last_key[0] = time.monotonic() + events.append(dict(key=code,command=command.tolist(),time=float(sim.data.time))) + print('WASD/arrows: move, space: stop, +/-: speed; repeat presses to keep moving.',flush=True) + try: + with mujoco.viewer.launch_passive(sim.model,sim.data,key_callback=key) as viewer: + while viewer.is_running() and not sim.fallen and sim.data.time.6: command[:] = 0 + sim.set_command(command[0],wz=command[1]) + sim.step() + viewer.sync() + time.sleep(max(0,.02-(time.monotonic()-start))) + finally: + sim.set_command() + if output: + output=Path(output) + output.mkdir(parents=True,exist_ok=True) + (output/'teleop_check.json').write_text(json.dumps(dict(events=events, + final_position=sim.data.qpos[:3].tolist(),fallen=bool(sim.fallen), + simulated_seconds=float(sim.data.time)),indent=2),encoding='utf-8') + sim.close() diff --git a/src/humanoid/humanoid_visual_patrol/patrol/scene.py b/src/humanoid/humanoid_visual_patrol/patrol/scene.py new file mode 100644 index 0000000000..df750e8a32 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/scene.py @@ -0,0 +1,112 @@ +"""Generate indoor geometry without changing the upstream robot assets.""" +from pathlib import Path +import tempfile +import xml.etree.ElementTree as ET + +import mujoco +import numpy as np + +from .locomotion import Walker + + +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() + root.find('compiler').set('meshdir', (asset_dir/'meshes').as_posix()) + world = root.find('worldbody') + pelvis = world.find("body[@name='pelvis']") + for geom in pelvis.iter('geom'): + geom.set('group', '1') + ET.SubElement(pelvis, 'camera', name='front', pos='0.16 0 0.35', + xyaxes='0 -1 0 0.35 0 0.94', fovy='75') + visual = ET.SubElement(root, 'visual') + ET.SubElement(visual, 'global', offwidth='640', offheight='480') + ET.SubElement(visual, 'headlight', 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') + + +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 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') + 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 + ranges = [] + geom_id = np.zeros(1, dtype=np.int32) + # Only static geometry: no robot self returns. Angle frame is level base_link. + for angle in np.linspace(-np.pi, np.pi, count, endpoint=False): + a = angle + self.yaw + distance = mujoco.mj_ray(self.model, self.data, origin, + np.array([np.cos(a), np.sin(a), 0.]), np.array([1,0,0,0,0,0], dtype=np.uint8), 1, + self.model.body('pelvis').id, geom_id) + ranges.append(float(np.clip(distance if distance >= 0 else 8., .05, 8.))) + return ranges + + def close(self): + if self.renderer: + self.renderer.close() + self.renderer = None diff --git a/src/humanoid/humanoid_visual_patrol/patrol/vision.py b/src/humanoid/humanoid_visual_patrol/patrol/vision.py new file mode 100644 index 0000000000..b0d706f474 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/patrol/vision.py @@ -0,0 +1,89 @@ +"""Separate perception and direct-image control models.""" +import numpy as np +import torch +from torch import nn + + +class FloorNet(nn.Module): + def __init__(self): + super().__init__() + self.net = nn.Sequential(nn.Conv2d(3, 16, 5, padding=2), nn.ReLU(), + nn.Conv2d(16, 16, 3, padding=1), nn.ReLU(), nn.Conv2d(16, 1, 1)) + + def forward(self, image): + return self.net(image) + + +class ImageController(nn.Module): + def __init__(self): + super().__init__() + self.net = nn.Sequential(nn.Conv2d(3, 16, 5, stride=2, padding=2), nn.ReLU(), + nn.Conv2d(16, 32, 3, stride=2, padding=1), nn.ReLU(), + nn.Conv2d(32, 32, 3, stride=2, padding=1), nn.ReLU(), + nn.Flatten(), nn.Linear(32*12*16, 64), nn.ReLU(), nn.Linear(64, 2)) + + def forward(self, image): + return self.net(image) + + +class MaskController(nn.Module): + """Learned velocity policy operating on a semantic floor mask.""" + def __init__(self): + super().__init__() + self.net = nn.Sequential(nn.AdaptiveAvgPool2d((12,16)),nn.Flatten(), + nn.Linear(192,64),nn.ReLU(),nn.Linear(64,32),nn.ReLU(),nn.Linear(32,2)) + + def forward(self, mask): + return self.net(mask) + + +def mask_command(mask): + """Reactive floor-following teacher, with no global route/position input. + + Scores candidate image corridors; low front clearance reduces speed. + This is a heuristic teacher, not a globally optimal navigation planner. + """ + h, w = mask.shape + band = mask[int(h*.48):int(h*.82)] + scores = np.array([band[:, max(0, x-10):min(w, x+11)].mean() + for x in range(12, w-12, 4)]) + centers = np.arange(12, w-12, 4) + scores -= .20*np.abs(centers-w/2)/(w/2) + best = centers[np.argmax(scores)] + yaw = float(np.clip((w/2-best)/(w/2)*1.2, -.45, .45)) + clearance = float(mask[int(h*.55):int(h*.85), int(w*.4):int(w*.6)].mean()) + speed = .28*float(np.clip((clearance-.15)/.6, 0, 1))*(1-.6*abs(yaw)) + return np.array([speed, yaw], dtype=np.float32) + + +def tensor_image(image): + return torch.from_numpy(image.copy()).permute(2, 0, 1).float().unsqueeze(0)/255. + + +class VisionPolicy: + def __init__(self, weights, mode): + self.mode = mode + self.net = FloorNet() if mode == 'perception' else ImageController() + self.net.load_state_dict(torch.load(weights, map_location='cpu', weights_only=True)) + self.net.eval() + if mode == 'perception': + from pathlib import Path + self.controller = MaskController() + self.controller.load_state_dict(torch.load(Path(weights).with_name('mask_controller.pt'), + map_location='cpu',weights_only=True)) + self.controller.eval() + + def command(self, image): + with torch.inference_mode(): + output = self.net(tensor_image(image)) + if self.mode == 'perception': + mask = (output.sigmoid()[0, 0].numpy()>.5).astype(np.float32) + with torch.inference_mode(): + command = self.controller(torch.from_numpy(mask)[None,None])[0].numpy() + # A learned controller can extrapolate a forward command even when + # segmentation detects no floor. Stop on this unambiguous failure. + if not mask.any(): + command = np.zeros(2, dtype=np.float32) + return np.clip(command,[0,-.45],[.3,.45]), mask + command = output[0].numpy() + return np.clip(command, [0, -.45], [.3, .45]), None diff --git a/src/humanoid/humanoid_visual_patrol/requirements-dev.txt b/src/humanoid/humanoid_visual_patrol/requirements-dev.txt new file mode 100644 index 0000000000..26b77f68be --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/requirements-dev.txt @@ -0,0 +1,2 @@ +-r requirements.txt +pytest diff --git a/src/humanoid/humanoid_visual_patrol/requirements.txt b/src/humanoid/humanoid_visual_patrol/requirements.txt new file mode 100644 index 0000000000..f55b4da02e --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/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/humanoid_visual_patrol/scripts/evaluate_near_obstacle.py b/src/humanoid/humanoid_visual_patrol/scripts/evaluate_near_obstacle.py new file mode 100644 index 0000000000..3759f3493a --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/scripts/evaluate_near_obstacle.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +"""Measure the deployed image controller near the end wall in MuJoCo.""" +import argparse +import json +from pathlib import Path +import sys + +import mujoco +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) +from patrol.scene import IndoorSimulator +from patrol.vision import VisionPolicy + + +def run(assets, weights, distance): + sim = IndoorSimulator(assets, 7) + try: + # End wall's near surface is x=9.88. Distance is pelvis to wall, + # not foot or body clearance. Settle before enabling the policy. + sim.data.qpos[0] = 9.88 - distance + mujoco.mj_forward(sim.model, sim.data) + policy = VisionPolicy(Path(weights) / 'end2end.pt', 'end2end') + for _ in range(100): + sim.set_command(0, 0, 0) + sim.step() + if sim.fallen: + return dict(distance_m=distance, settled=False, fallen=True) + start = sim.data.qpos[:3].copy() + commands = [] + contacts = 0 + previous = np.zeros(2) + for tick in range(150): + if tick % 10 == 0: + command, _ = policy.command(sim.image()) + previous = .6 * previous + .4 * command + sim.set_command(float(previous[0]), wz=float(previous[1])) + commands.append(command.tolist()) + sim.step() + for contact in sim.data.contact: + names = [mujoco.mj_id2name(sim.model, mujoco.mjtObj.mjOBJ_GEOM, int(g)) or '' + for g in (contact.geom1, contact.geom2)] + if 'end_wall' in names: + contacts += 1 + break + if sim.fallen: + break + return dict(distance_m=distance, settled=True, start_position=start.tolist(), + final_position=sim.data.qpos[:3].tolist(), fallen=bool(sim.fallen), + wall_contact_steps=contacts, commands=commands) + finally: + sim.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--assets', required=True) + parser.add_argument('--weights', default=str(Path(__file__).resolve().parents[1] / 'models')) + parser.add_argument('--output', required=True) + args = parser.parse_args() + results = [run(args.assets, args.weights, d) for d in (1.5, 1.0, .6)] + Path(args.output).write_text(json.dumps(results, indent=2) + '\n', encoding='utf-8') + print(json.dumps(results, indent=2)) diff --git a/src/humanoid/humanoid_visual_patrol/scripts/fetch_assets.py b/src/humanoid/humanoid_visual_patrol/scripts/fetch_assets.py new file mode 100644 index 0000000000..71f10aa86b --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/scripts/fetch_assets.py @@ -0,0 +1,25 @@ +#!/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','clone','--filter=blob:none','--no-checkout',URL,str(target)],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/humanoid_visual_patrol/scripts/neural_planner.py b/src/humanoid/humanoid_visual_patrol/scripts/neural_planner.py new file mode 100755 index 0000000000..3990a8c109 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/scripts/neural_planner.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Plan on the live SLAM map with a learned tie breaker; execute via move_base.""" +import json +import subprocess +import threading + +import actionlib +import rospy +import tf +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import OccupancyGrid, Path +from std_msgs.msg import String +from move_base_msgs.msg import MoveBaseAction,MoveBaseGoal + + +def main(): + rospy.init_node('neural_patrol_planner') + latest_map=[None];pending=[None] + rospy.Subscriber('/map',OccupancyGrid,lambda msg:latest_map.__setitem__(0,msg),queue_size=1) + rospy.Subscriber('/move_base_simple/goal',PoseStamped,lambda msg:pending.__setitem__(0,msg),queue_size=1) + result_pub=rospy.Publisher('/neural_patrol/result',String,queue_size=1,latch=True) + path_pub=rospy.Publisher('/neural_patrol/path',Path,queue_size=1,latch=True) + listener=tf.TransformListener() + client=actionlib.SimpleActionClient('/move_base',MoveBaseAction) + while not rospy.is_shutdown(): + if pending[0] is None or latest_map[0] is None: + rospy.rostime.wallsleep(.1);continue + goal=pending[0];pending[0]=None + try: + if goal.header.frame_id!='map': raise ValueError('Goal must use map frame') + listener.waitForTransform('map','base_link',rospy.Time(0),rospy.Duration(5)) + position,_=listener.lookupTransform('map','base_link',rospy.Time(0)) + grid=latest_map[0] + request=dict(map=list(grid.data),width=grid.info.width,height=grid.info.height, + resolution=grid.info.resolution,origin=[grid.info.origin.position.x,grid.info.origin.position.y], + start=list(position[:2]),goal=[goal.pose.position.x,goal.pose.position.y]) + command=[rospy.get_param('~sim_python'),rospy.get_param('~sim_entry'), + '--mode','plan_stdio','--assets',rospy.get_param('~assets'), + '--weights',rospy.get_param('~weights')] + output=subprocess.run(command,input=json.dumps(request),stdout=subprocess.PIPE, + stderr=subprocess.PIPE,universal_newlines=True,timeout=30,check=True) + report=json.loads(output.stdout) + report['request_id']=goal.header.stamp.to_nsec() + path=Path();path.header.frame_id='map';path.header.stamp=rospy.Time.now() + for x,y in report['waypoints']: + pose=PoseStamped();pose.header=path.header + pose.pose.position.x=x;pose.pose.position.y=y;pose.pose.orientation.w=1 + path.poses.append(pose) + path_pub.publish(path) + if not client.wait_for_server(rospy.Duration(10)):raise RuntimeError('move_base unavailable') + report['execution']=[] + for pose in path.poses: + target=MoveBaseGoal();target.target_pose=pose + target.target_pose.header.stamp=rospy.Time.now() + client.send_goal(target) + completed=client.wait_for_result(rospy.Duration(45)) + state=client.get_state() + report['execution'].append(dict(goal=[pose.pose.position.x,pose.pose.position.y],state=state)) + if not completed or state!=3: + client.cancel_goal();break + report['success']=len(report['execution'])==len(path.poses) and all(p['state']==3 for p in report['execution']) + result_pub.publish(String(data=json.dumps(report))) + except Exception as exc: + client.cancel_goal() + detail=getattr(exc,'stderr',None) or str(exc) + rospy.logerr('Neural planning failed: %s',detail) + result_pub.publish(String(data=json.dumps(dict(success=False,error=detail,request_id=goal.header.stamp.to_nsec())))) + + +if __name__=='__main__': + main() diff --git a/src/humanoid/humanoid_visual_patrol/scripts/ros_bridge.py b/src/humanoid/humanoid_visual_patrol/scripts/ros_bridge.py new file mode 100755 index 0000000000..5dd2c0c016 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/scripts/ros_bridge.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 +"""ROS Noetic bridge. Simulation uses an isolated interpreter over stdio. + +The bridge only consumes integrated noisy odometry, never ground-truth position. +On WSL, an explicit Windows Python executable can run the MuJoCo process. +""" +import json +import math +import os +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)) + controller=rospy.get_param('~task','navigation') + weights=rospy.get_param('~weights','models') + duration=float(rospy.get_param('~seconds',45)) + 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, + '--controller',controller,'--weights',weights], + 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 + # Velocity from successive integrated odometry is in the body 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) + 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 + if controller!='navigation' and state['time']>=duration: + 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/humanoid_visual_patrol/scripts/run_navigation.sh b/src/humanoid/humanoid_visual_patrol/scripts/run_navigation.sh new file mode 100644 index 0000000000..30b6ed3192 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/scripts/run_navigation.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +# Usage: bash scripts/run_navigation.sh SIM_PYTHON ASSETS [TASK] [WEIGHTS] [OUTPUT] +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}" +TASK="${3:-navigation}" +WEIGHTS="${4:-$MODULE_DIR/models}" +OUTPUT="${5:-$MODULE_DIR/results}" +ENTRY="$MODULE_DIR/main.py" +if [[ "$SIM_PYTHON" == *.exe ]]; then + ENTRY="$(wslpath -w "$ENTRY")" + ASSETS="$(wslpath -w "$ASSETS")" + WEIGHTS="$(wslpath -w "$WEIGHTS")" + OUTPUT="$(wslpath -w "$OUTPUT")" +fi +exec roslaunch -p "${ROS_MASTER_URI##*:}" humanoid_visual_patrol main.launch \ + sim_python:="$SIM_PYTHON" sim_entry:="$ENTRY" assets:="$ASSETS" \ + task:="$TASK" weights:="$WEIGHTS" output:="$OUTPUT" diff --git a/src/humanoid/humanoid_visual_patrol/scripts/task_runner.py b/src/humanoid/humanoid_visual_patrol/scripts/task_runner.py new file mode 100755 index 0000000000..c1112df063 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/scripts/task_runner.py @@ -0,0 +1,26 @@ +#!/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'), + '--assets',rospy.get_param('~assets'),'--mode',rospy.get_param('~task'), + '--weights',rospy.get_param('~weights'),'--output',rospy.get_param('~output'), + '--seconds',str(rospy.get_param('~seconds',45))] + 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() diff --git a/src/humanoid/humanoid_visual_patrol/tests/test_contracts.py b/src/humanoid/humanoid_visual_patrol/tests/test_contracts.py new file mode 100644 index 0000000000..4f39435f20 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/tests/test_contracts.py @@ -0,0 +1,68 @@ +import os +from pathlib import Path +import sys + +import numpy as np +import pytest + +sys.path.insert(0,str(Path(__file__).resolve().parents[1])) +from patrol.vision import mask_command + + +def test_deployed_perception_stops_when_segmentation_finds_no_floor(): + import torch + from patrol.vision import VisionPolicy + policy = VisionPolicy(Path(__file__).resolve().parents[1] / 'models/perception.pt', 'perception') + class NoFloor(torch.nn.Module): + def forward(self, image): + return torch.full((1, 1, 96, 128), -100.0) + policy.net = NoFloor() + command, mask = policy.command(np.zeros((96,128,3), dtype=np.uint8)) + assert not mask.any() + assert np.array_equal(command, [0, 0]) + + +def test_no_floor_does_not_command_forward_motion(): + assert mask_command(np.zeros((96,128),dtype=np.uint8))[0] == 0 + + +def test_unilateral_floor_steers_into_visible_free_space(): + left = np.zeros((96,128),dtype=np.uint8) + left[:,:60] = 1 + assert mask_command(left)[1] > 0 + assert mask_command(left[:,::-1])[1] < 0 + + +def test_bad_neural_estimates_cannot_change_primary_astar_optimal_cost(): + from patrol.planning import astar,distances + rng=np.random.default_rng(93) + grid=np.zeros((16,16),dtype=bool) + grid[2:14,7]=True + grid[10,7]=False + start,goal=(4,3),(4,12) + expected=distances(grid,goal)[start] + route,_=astar(grid,start,goal,rng.normal(0,10000,grid.shape)) + assert len(route)-1==expected + assert all(not grid[p] for p in route) + + +def test_planner_reports_unreachable_goal(): + from patrol.planning import astar + grid=np.zeros((10,10),dtype=bool);grid[5,:]=True + with pytest.raises(ValueError,match='No path'): + astar(grid,(2,2),(8,8)) + + +@pytest.mark.skipif(not os.environ.get('HUMANOID_ASSETS'),reason='set HUMANOID_ASSETS for real physics checks') +def test_real_robot_stays_upright_and_scan_excludes_body(): + from patrol.scene import IndoorSimulator + sim = IndoorSimulator(os.environ['HUMANOID_ASSETS']) + try: + # Robot starts 1.88m from side walls; there must be no 7cm self-return. + assert min(sim.scan()) > 1.0 + result = sim.smoke_test() + assert not result['fallen'] + assert result['displacement_m'] > 1 + assert result['final_yaw_rad'] > .3 + finally: + sim.close() diff --git a/src/humanoid/humanoid_visual_patrol/third_party_license.md b/src/humanoid/humanoid_visual_patrol/third_party_license.md new file mode 100644 index 0000000000..9ad8f6dbb0 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/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/humanoid_visual_patrol/third_party_notices.md b/src/humanoid/humanoid_visual_patrol/third_party_notices.md new file mode 100644 index 0000000000..01dd8c97e2 --- /dev/null +++ b/src/humanoid/humanoid_visual_patrol/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.