[software] 添加16DOF早期训练仿真与Sim2Real闭环

.gitignore

@@ -0,0 +1,48 @@
+# Local reference material
+/00_reference/
+
+# Python
+__pycache__/
+*.py[cod]
+.venv/
+venv/
+*.egg-info/
+.pytest_cache/
+.ruff_cache/
+.mypy_cache/
+.uv-cache/
+
+# Build outputs
+build/
+install/
+devel/
+log/
+*.o
+*.so
+*.a
+*.elf
+*.hex
+*.bin
+*.axf
+
+# Required vendored Odin runtime libraries in the early Sim2Real release
+!05_software/real/sim2real/vendored/odin1_imu/build/
+!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
+!05_software/real/sim2real/vendored/odin1_imu/lib/*.a
+
+# Training outputs
+logs/
+checkpoints/
+wandb/
+sim2sim_log_*.txt
+
+# IDE and operating system files
+.idea/
+.vscode/
+.trae/
+.DS_Store
+Thumbs.db
+
+# Temporary and backup files
+*.Bak
+~$*

01_doc/README.md

@@ -0,0 +1,14 @@
+# 项目文档
+
+本目录用于保存 16DOF 轮足项目自身的技术文档和使用说明。
+
+后续建议按主题组织：
+
+```text
+01_doc/
+├─ architecture/   # 系统架构和数据流
+├─ control/        # 控制与强化学习原理
+├─ deployment/     # Sim2Sim 和 Sim2Real 部署
+├─ hardware/       # 接线、标定和硬件兼容性
+└─ user_guide/     # 安装、运行和调试说明
+```

01_doc/architecture/early_software_stack.md

@@ -0,0 +1,22 @@
+# 第一代软件闭环
+
+第一代 16DOF 软件的目标是先打通“训练、仿真验证、真机执行”闭环，而不是在初期建立复杂的分布式系统。
+
+## 训练与策略
+
+`05_software/train/rc_mjlab` 使用本地修改的 mjlab 和 MuJoCo 模型训练轮腿混合策略。训练任务包括 Flat、Rough 和 Crawl，腿部 12 个关节输出位置目标，4 个轮子输出速度目标。
+
+## MuJoCo 与 Sim2Sim
+
+- `mujoco_sim` 用于不加载 RL 策略时的模型、动力学和控制调试。
+- `sim2sim` 加载训练策略，在独立 MuJoCo 环境中验证观测、动作、地形和导航行为。
+- 两者与训练任务共同使用 `rc_mjlab/mjcf`，避免早期模型定义不一致。
+
+## 真机控制
+
+- `ik_real` 先以逆运动学和轨迹插值验证电机控制链路。
+- `sim2real` 再将训练策略部署到 Python 真机运行时，加入 IMU、站立、安全和 Web 调试。
+
+## 阶段特点
+
+这一版本的优势是链路完整、模块直观，便于快速验证；局限是训练、仿真和部署仍存在重复资源，Python 真机运行时的实时性和系统集成能力有限。这些问题推动了后续统一训练版本和 ROS 2/C++ 部署架构。

02_mechanical/CAD/README.md

@@ -0,0 +1,5 @@
+# SolidWorks 源文件
+
+`solidworks/` 保留原始装配层级和文件名。
+
+建议使用 `solidworks/WEEKDOG.SLDASM` 作为整机入口。`26版sw单件/` 包含组成整机的单件和子装配体，不应随意改名或拆散。

02_mechanical/README.md

@@ -0,0 +1,40 @@
+# 16DOF 串联轮足机械
+
+本目录保存山东华宇工学院 16DOF 串联轮足机器人的机械设计资料。
+
+机器人由 12 个腿部关节和 4 个驱动轮组成，共 16 个执行器。源资料说明其使用 SolidWorks 26 版本完成设计、装配和 URDF 导出准备。
+
+## 目录结构
+
+```text
+02_mechanical/
+├─ CAD/
+│  └─ solidworks/
+│     ├─ WEEKDOG.SLDASM       # 整机装配体入口
+│     ├─ 轮子完整.SLDASM        # 轮组装配体
+│     ├─ 轮毂垫片.SLDPRT        # 独立零件
+│     └─ 26版sw单件/            # 单件与子装配体
+├─ STEP/
+│  ├─ WEEKDOG.STEP            # 整机通用交换文件
+│  └─ *.STEP                  # 身体、关节、腿部和轮组
+└─ source_notes.txt           # 原始资料说明
+```
+
+## 文件统计
+
+- SolidWorks 装配体：4 个 `*.SLDASM`
+- SolidWorks 零件：26 个 `*.SLDPRT`
+- STEP 文件：10 个
+
+## 使用方式
+
+- 需要继续编辑设计时，从 `CAD/solidworks/WEEKDOG.SLDASM` 打开整机。
+- 只需查看、测量或导入其他 CAD 软件时，使用 `STEP/WEEKDOG.STEP`。
+- 子部件调试可以使用身体、髋关节、大腿、轮小腿和轮组 STEP。
+- 为避免装配引用丢失，不要单独移动或重命名 `CAD/solidworks` 内部文件。
+
+## 当前缺项
+
+- 尚未提供关键零件二维 PDF 工程图。
+- 尚未形成 BOM、装配步骤和干涉检查报告。
+- 尚未核对 CAD 参数与训练使用的 URDF/MJCF 是否完全一致。

02_mechanical/STEP/README.md

@@ -0,0 +1,11 @@
+# STEP 交换文件
+
+本目录提供不依赖 SolidWorks 的通用机械交换文件。
+
+- `WEEKDOG.STEP`：整机
+- `DOG身体.STEP`：机器人身体
+- `髋关节END.STEP`：髋关节
+- `大腿.STEP`：大腿组件
+- `轮小腿.STEP`：轮足小腿组件
+- `轮子完整.STEP`：完整轮组
+- 其余文件：轮子、轮毂垫片和连接零件

02_mechanical/STEP/轮毂垫片.STEP

@@ -0,0 +1,660 @@
+ISO-10303-21;
+HEADER;
+FILE_DESCRIPTION (( 'STEP AP214' ),
+    '1' );
+FILE_NAME ('ÂÖì±µæƬ.STEP',
+    '2026-04-27T05:39:26',
+    ( '' ),
+    ( '' ),
+    'SwSTEP 2.0',
+    'SolidWorks 2026',
+    '' );
+FILE_SCHEMA (( 'AUTOMOTIVE_DESIGN' ));
+ENDSEC;
+
+DATA;
+#1 = ADVANCED_FACE ( 'NONE', ( #454 ), #386, .F. ) ;
+#2 = PRODUCT_DEFINITION_CONTEXT ( 'detailed design', #631, 'design' ) ;
+#3 = CYLINDRICAL_SURFACE ( 'NONE', #580, 7.000000000000000000 ) ;
+#4 = CARTESIAN_POINT ( 'NONE',  ( -8.242304845413723768, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#5 = FACE_BOUND ( 'NONE', #375, .T. ) ;
+#6 = CARTESIAN_POINT ( 'NONE',  ( 19.00000000000000000, -1.487354696433177415, 8.000000000000000000 ) ) ;
+#7 = EDGE_CURVE ( 'NONE', #419, #33, #450, .T. ) ;
+#8 = CARTESIAN_POINT ( 'NONE',  ( 1.469576158976869898E-15, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#9 = EDGE_CURVE ( 'NONE', #83, #573, #139, .T. ) ;
+#10 = AXIS2_PLACEMENT_3D ( 'NONE', #50, #65, #101 ) ;
+#11 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#12 = VERTEX_POINT ( 'NONE', #91 ) ;
+#13 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#14 = ORIENTED_EDGE ( 'NONE', *, *, #582, .F. ) ;
+#15 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#16 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541302957, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#17 = VERTEX_POINT ( 'NONE', #548 ) ;
+#18 = AXIS2_PLACEMENT_3D ( 'NONE', #216, #64, #11 ) ;
+#19 = CYLINDRICAL_SURFACE ( 'NONE', #263, 2.149999999999539391 ) ;
+#20 = LINE ( 'NONE', #116, #596 ) ;
+#21 = ORIENTED_EDGE ( 'NONE', *, *, #473, .T. ) ;
+#22 = VERTEX_POINT ( 'NONE', #58 ) ;
+#23 = AXIS2_PLACEMENT_3D ( 'NONE', #570, #275, #180 ) ;
+#24 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541344879, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#25 = CIRCLE ( 'NONE', #186, 2.150000000000000355 ) ;
+#26 = ORIENTED_EDGE ( 'NONE', *, *, #169, .T. ) ;
+#27 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#28 = CARTESIAN_POINT ( 'NONE',  ( 2.150000000000000355, 10.51264530356682236, 0.000000000000000000 ) ) ;
+#29 = VERTEX_POINT ( 'NONE', #488 ) ;
+#30 = CARTESIAN_POINT ( 'NONE',  ( -12.54230484541302992, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#31 = VECTOR ( 'NONE', #168, 1000.000000000000000 ) ;
+#32 = ORIENTED_EDGE ( 'NONE', *, *, #241, .F. ) ;
+#33 = VERTEX_POINT ( 'NONE', #244 ) ;
+#34 = VECTOR ( 'NONE', #522, 1000.000000000000000 ) ;
+#35 = CARTESIAN_POINT ( 'NONE',  ( -2.150000000000000355, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#36 = ORIENTED_EDGE ( 'NONE', *, *, #190, .F. ) ;
+#37 = SURFACE_SIDE_STYLE ('',( #482 ) ) ;
+#38 = EDGE_CURVE ( 'NONE', #573, #33, #556, .T. ) ;
+#39 = CARTESIAN_POINT ( 'NONE',  ( -8.242304845413029213, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#40 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#41 = VECTOR ( 'NONE', #251, 1000.000000000000000 ) ;
+#42 = VECTOR ( 'NONE', #144, 1000.000000000000000 ) ;
+#43 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541326405, -7.487354696433185630, 0.000000000000000000 ) ) ;
+#44 = VERTEX_POINT ( 'NONE', #304 ) ;
+#45 = LINE ( 'NONE', #594, #73 ) ;
+#46 = ADVANCED_FACE ( 'NONE', ( #253 ), #588, .F. ) ;
+#47 = VECTOR ( 'NONE', #87, 1000.000000000000000 ) ;
+#48 = SHAPE_DEFINITION_REPRESENTATION ( #95, #280 ) ;
+#49 = APPLICATION_CONTEXT ( 'automotive_design' ) ;
+#50 = CARTESIAN_POINT ( 'NONE',  ( 1.469576158976869898E-15, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#51 = ADVANCED_FACE ( 'NONE', ( #192 ), #530, .F. ) ;
+#52 = CARTESIAN_POINT ( 'NONE',  ( -2.149999999999433697, -13.48735469643318119, 0.000000000000000000 ) ) ;
+#53 = VERTEX_POINT ( 'NONE', #293 ) ;
+#54 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#55 = EDGE_CURVE ( 'NONE', #53, #17, #643, .T. ) ;
+#56 = EDGE_CURVE ( 'NONE', #418, #620, #364, .T. ) ;
+#57 = CARTESIAN_POINT ( 'NONE',  ( 2.149999999999436362, -13.48735469643318119, 0.000000000000000000 ) ) ;
+#58 = CARTESIAN_POINT ( 'NONE',  ( 12.54230484541344914, 4.512645303567090593, 0.000000000000000000 ) ) ;
+#59 = CIRCLE ( 'NONE', #425, 2.149999999999435030 ) ;
+#60 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#61 = FACE_BOUND ( 'NONE', #360, .T. ) ;
+#62 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#63 = EDGE_LOOP ( 'NONE', ( #142, #640, #372, #268 ) ) ;
+#64 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#65 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#66 = ORIENTED_EDGE ( 'NONE', *, *, #563, .F. ) ;
+#67 = ORIENTED_EDGE ( 'NONE', *, *, #55, .T. ) ;
+#68 = EDGE_CURVE ( 'NONE', #614, #418, #322, .T. ) ;
+#69 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#70 = ORIENTED_EDGE ( 'NONE', *, *, #221, .T. ) ;
+#71 = CIRCLE ( 'NONE', #401, 2.150000000000000799 ) ;
+#72 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#73 = VECTOR ( 'NONE', #436, 1000.000000000000000 ) ;
+#74 = CARTESIAN_POINT ( 'NONE',  ( -8.242304845413029213, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#75 = ORIENTED_EDGE ( 'NONE', *, *, #7, .T. ) ;
+#76 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#77 = LINE ( 'NONE', #30, #247 ) ;
+#78 = ORIENTED_EDGE ( 'NONE', *, *, #7, .F. ) ;
+#79 = CIRCLE ( 'NONE', #327, 19.00000000000000000 ) ;
+#80 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#81 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#82 = AXIS2_PLACEMENT_3D ( 'NONE', #525, #122, #471 ) ;
+#83 = VERTEX_POINT ( 'NONE', #284 ) ;
+#84 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#85 = ADVANCED_FACE ( 'NONE', ( #134 ), #433, .F. ) ;
+#86 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#87 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#88 = VERTEX_POINT ( 'NONE', #28 ) ;
+#89 = AXIS2_PLACEMENT_3D ( 'NONE', #521, #564, #416 ) ;
+#90 = CYLINDRICAL_SURFACE ( 'NONE', #319, 19.00000000000000000 ) ;
+#91 = CARTESIAN_POINT ( 'NONE',  ( 12.54230484541344914, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#92 = ORIENTED_EDGE ( 'NONE', *, *, #515, .F. ) ;
+#93 = AXIS2_PLACEMENT_3D ( 'NONE', #405, #248, #447 ) ;
+#94 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 10.51264530356682236, 0.000000000000000000 ) ) ;
+#95 = PRODUCT_DEFINITION_SHAPE ( 'NONE', 'NONE',  #223 ) ;
+#96 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#97 = ADVANCED_FACE ( 'NONE', ( #385 ), #477, .F. ) ;
+#98 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#99 = AXIS2_PLACEMENT_3D ( 'NONE', #602, #107, #403 ) ;
+#100 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#101 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#102 = EDGE_CURVE ( 'NONE', #17, #44, #618, .T. ) ;
+#103 = AXIS2_PLACEMENT_3D ( 'NONE', #246, #86, #391 ) ;
+#104 = PLANE ( 'NONE',  #99 ) ;
+#105 = EDGE_CURVE ( 'NONE', #371, #620, #77, .T. ) ;
+#106 = CARTESIAN_POINT ( 'NONE',  ( -7.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#107 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#108 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#109 = LINE ( 'NONE', #106, #151 ) ;
+#110 = ORIENTED_EDGE ( 'NONE', *, *, #635, .F. ) ;
+#111 = CYLINDRICAL_SURFACE ( 'NONE', #344, 19.00000000000000000 ) ;
+#112 = FACE_OUTER_BOUND ( 'NONE', #63, .T. ) ;
+#113 = ORIENTED_EDGE ( 'NONE', *, *, #445, .F. ) ;
+#114 = AXIS2_PLACEMENT_3D ( 'NONE', #422, #434, #128 ) ;
+#115 = LINE ( 'NONE', #310, #205 ) ;
+#116 = CARTESIAN_POINT ( 'NONE',  ( -2.150000000000000355, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#117 = EDGE_LOOP ( 'NONE', ( #167, #611 ) ) ;
+#118 = EDGE_CURVE ( 'NONE', #22, #432, #299, .T. ) ;
+#119 = FACE_OUTER_BOUND ( 'NONE', #626, .T. ) ;
+#120 = VERTEX_POINT ( 'NONE', #633 ) ;
+#121 = ORIENTED_EDGE ( 'NONE', *, *, #474, .T. ) ;
+#122 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#123 = ORIENTED_EDGE ( 'NONE', *, *, #343, .T. ) ;
+#124 = ADVANCED_FACE ( 'NONE', ( #377 ), #380, .F. ) ;
+#125 = VECTOR ( 'NONE', #298, 1000.000000000000000 ) ;
+#126 = CARTESIAN_POINT ( 'NONE',  ( 1.469576158976869898E-15, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#127 = AXIS2_PLACEMENT_3D ( 'NONE', #547, #394, #533 ) ;
+#128 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#129 = ORIENTED_EDGE ( 'NONE', *, *, #397, .T. ) ;
+#130 = EDGE_LOOP ( 'NONE', ( #129, #415 ) ) ;
+#131 = CARTESIAN_POINT ( 'NONE',  ( 8.242304845413610082, -7.487354696433178525, 0.000000000000000000 ) ) ;
+#132 = FACE_BOUND ( 'NONE', #442, .T. ) ;
+#133 = CIRCLE ( 'NONE', #381, 7.000000000000000000 ) ;
+#134 = FACE_OUTER_BOUND ( 'NONE', #478, .T. ) ;
+#135 = AXIS2_PLACEMENT_3D ( 'NONE', #472, #581, #527 ) ;
+#136 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#137 = VERTEX_POINT ( 'NONE', #35 ) ;
+#138 = CARTESIAN_POINT ( 'NONE',  ( 12.54230484541292512, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#139 = LINE ( 'NONE', #486, #413 ) ;
+#140 = AXIS2_PLACEMENT_3D ( 'NONE', #24, #72, #27 ) ;
+#141 = CARTESIAN_POINT ( 'NONE',  ( -12.54230484541302992, 4.512645303566650057, 0.000000000000000000 ) ) ;
+#142 = ORIENTED_EDGE ( 'NONE', *, *, #610, .F. ) ;
+#143 = UNCERTAINTY_MEASURE_WITH_UNIT (LENGTH_MEASURE( 1.000000000000000082E-05 ), #537, 'distance_accuracy_value', 'NONE');
+#144 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#145 = ORIENTED_EDGE ( 'NONE', *, *, #296, .F. ) ;
+#146 = PRODUCT_CONTEXT ( 'NONE', #49, 'mechanical' ) ;
+#147 = ADVANCED_FACE ( 'NONE', ( #237, #132, #326, #628, #279, #183, #234, #431 ), #330, .T. ) ;
+#148 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#149 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#150 = CARTESIAN_POINT ( 'NONE',  ( -12.54230484541280255, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#151 = VECTOR ( 'NONE', #497, 1000.000000000000000 ) ;
+#152 = VECTOR ( 'NONE', #638, 1000.000000000000000 ) ;
+#153 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#154 = EDGE_LOOP ( 'NONE', ( #209, #255, #475, #276 ) ) ;
+#155 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#156 = MANIFOLD_SOLID_BREP ( '\X2\51f853f0\X0\-\X2\62c94f38\X0\1', #196 ) ;
+#157 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#158 = CARTESIAN_POINT ( 'NONE',  ( 2.149999999999436362, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#159 = CIRCLE ( 'NONE', #440, 2.149999999999657518 ) ;
+#160 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#161 = LINE ( 'NONE', #456, #125 ) ;
+#162 = ADVANCED_FACE ( 'NONE', ( #559 ), #622, .F. ) ;
+#163 = EDGE_LOOP ( 'NONE', ( #469, #566, #378, #338 ) ) ;
+#164 = AXIS2_PLACEMENT_3D ( 'NONE', #404, #499, #639 ) ;
+#165 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#166 = ORIENTED_EDGE ( 'NONE', *, *, #351, .T. ) ;
+#167 = ORIENTED_EDGE ( 'NONE', *, *, #169, .F. ) ;
+#168 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#169 = EDGE_CURVE ( 'NONE', #29, #12, #194, .T. ) ;
+#170 = ORIENTED_EDGE ( 'NONE', *, *, #531, .F. ) ;
+#171 = EDGE_LOOP ( 'NONE', ( #218, #291, #368, #513 ) ) ;
+#172 = ORIENTED_EDGE ( 'NONE', *, *, #38, .F. ) ;
+#173 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541302957, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#174 = AXIS2_PLACEMENT_3D ( 'NONE', #514, #225, #369 ) ;
+#175 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#176 = ORIENTED_EDGE ( 'NONE', *, *, #328, .T. ) ;
+#177 = CIRCLE ( 'NONE', #290, 7.000000000000000000 ) ;
+#178 = ORIENTED_EDGE ( 'NONE', *, *, #448, .F. ) ;
+#179 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#180 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#181 = ORIENTED_EDGE ( 'NONE', *, *, #397, .F. ) ;
+#182 = ORIENTED_EDGE ( 'NONE', *, *, #448, .T. ) ;
+#183 = FACE_BOUND ( 'NONE', #117, .T. ) ;
+#184 = AXIS2_PLACEMENT_3D ( 'NONE', #595, #398, #546 ) ;
+#185 = EDGE_CURVE ( 'NONE', #429, #259, #361, .T. ) ;
+#186 = AXIS2_PLACEMENT_3D ( 'NONE', #94, #345, #239 ) ;
+#187 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#188 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#189 = ORIENTED_EDGE ( 'NONE', *, *, #335, .F. ) ;
+#190 = EDGE_CURVE ( 'NONE', #466, #465, #59, .T. ) ;
+#191 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#192 = FACE_OUTER_BOUND ( 'NONE', #154, .T. ) ;
+#193 = ORIENTED_EDGE ( 'NONE', *, *, #560, .T. ) ;
+#194 = CIRCLE ( 'NONE', #140, 2.150000000000000799 ) ;
+#195 = VERTEX_POINT ( 'NONE', #222 ) ;
+#196 = CLOSED_SHELL ( 'NONE', ( #285, #470, #85, #46, #97, #1, #584, #577, #147, #483, #505, #124, #162, #383, #512, #212, #51, #355 ) ) ;
+#197 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#198 = PRODUCT_RELATED_PRODUCT_CATEGORY ( 'part', '', ( #601 ) ) ;
+#199 = FACE_OUTER_BOUND ( 'NONE', #315, .T. ) ;
+#200 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541302957, 4.512645303566650057, 0.000000000000000000 ) ) ;
+#201 = FACE_OUTER_BOUND ( 'NONE', #365, .T. ) ;
+#202 = ORIENTED_EDGE ( 'NONE', *, *, #68, .F. ) ;
+#203 = CARTESIAN_POINT ( 'NONE',  ( -8.242304845413723768, -7.487354696433185630, 0.000000000000000000 ) ) ;
+#204 = CIRCLE ( 'NONE', #10, 2.149999999999435030 ) ;
+#205 = VECTOR ( 'NONE', #506, 1000.000000000000000 ) ;
+#206 = FACE_BOUND ( 'NONE', #625, .T. ) ;
+#207 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#208 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#209 = ORIENTED_EDGE ( 'NONE', *, *, #393, .F. ) ;
+#210 = PRODUCT_DEFINITION_FORMATION_WITH_SPECIFIED_SOURCE ( 'ä»', '', #601, .NOT_KNOWN. ) ;
+#211 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#212 = ADVANCED_FACE ( 'NONE', ( #119 ), #510, .F. ) ;
+#213 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#214 = AXIS2_PLACEMENT_3D ( 'NONE', #592, #552, #302 ) ;
+#215 = ORIENTED_EDGE ( 'NONE', *, *, #105, .T. ) ;
+#216 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541326760, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#217 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#218 = ORIENTED_EDGE ( 'NONE', *, *, #306, .T. ) ;
+#219 = ORIENTED_EDGE ( 'NONE', *, *, #424, .T. ) ;
+#220 = VECTOR ( 'NONE', #153, 1000.000000000000000 ) ;
+#221 = EDGE_CURVE ( 'NONE', #314, #465, #609, .T. ) ;
+#222 = CARTESIAN_POINT ( 'NONE',  ( 12.54230484541292512, -7.487354696433178525, 0.000000000000000000 ) ) ;
+#223 = PRODUCT_DEFINITION ( 'æœ', '', #210, #2 ) ;
+#224 = ORIENTED_EDGE ( 'NONE', *, *, #583, .T. ) ;
+#225 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#226 = SURFACE_STYLE_USAGE ( .BOTH. , #37 ) ;
+#227 = ORIENTED_EDGE ( 'NONE', *, *, #632, .T. ) ;
+#228 = ORIENTED_EDGE ( 'NONE', *, *, #334, .T. ) ;
+#229 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#230 = AXIS2_PLACEMENT_3D ( 'NONE', #617, #476, #568 ) ;
+#231 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541344879, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#232 = ORIENTED_EDGE ( 'NONE', *, *, #306, .F. ) ;
+#233 = EDGE_LOOP ( 'NONE', ( #504, #75, #219, #370 ) ) ;
+#234 = FACE_BOUND ( 'NONE', #238, .T. ) ;
+#235 = ORIENTED_EDGE ( 'NONE', *, *, #445, .T. ) ;
+#236 = EDGE_CURVE ( 'NONE', #12, #29, #553, .T. ) ;
+#237 = FACE_BOUND ( 'NONE', #518, .T. ) ;
+#238 = EDGE_LOOP ( 'NONE', ( #113, #621 ) ) ;
+#239 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#240 = CIRCLE ( 'NONE', #318, 2.150000000000000799 ) ;
+#241 = EDGE_CURVE ( 'NONE', #264, #195, #441, .T. ) ;
+#242 = CIRCLE ( 'NONE', #350, 7.000000000000000000 ) ;
+#243 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#244 = CARTESIAN_POINT ( 'NONE',  ( 19.00000000000000000, -1.487354696433177415, 0.000000000000000000 ) ) ;
+#245 = AXIS2_PLACEMENT_3D ( 'NONE', #211, #496, #160 ) ;
+#246 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#247 = VECTOR ( 'NONE', #480, 1000.000000000000000 ) ;
+#248 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#249 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#250 = EDGE_LOOP ( 'NONE', ( #333, #558 ) ) ;
+#251 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#252 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#253 = FACE_OUTER_BOUND ( 'NONE', #629, .T. ) ;
+#254 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#255 = ORIENTED_EDGE ( 'NONE', *, *, #529, .T. ) ;
+#256 = AXIS2_PLACEMENT_3D ( 'NONE', #80, #316, #217 ) ;
+#257 = FACE_BOUND ( 'NONE', #354, .T. ) ;
+#258 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#259 = VERTEX_POINT ( 'NONE', #467 ) ;
+#260 = AXIS2_PLACEMENT_3D ( 'NONE', #438, #197, #191 ) ;
+#261 = FACE_OUTER_BOUND ( 'NONE', #562, .T. ) ;
+#262 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#263 = AXIS2_PLACEMENT_3D ( 'NONE', #453, #157, #362 ) ;
+#264 = VERTEX_POINT ( 'NONE', #131 ) ;
+#265 = EDGE_CURVE ( 'NONE', #419, #83, #376, .T. ) ;
+#266 = EDGE_LOOP ( 'NONE', ( #523, #636, #181, #324 ) ) ;
+#267 = CARTESIAN_POINT ( 'NONE',  ( -2.149999999999433697, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#268 = ORIENTED_EDGE ( 'NONE', *, *, #118, .F. ) ;
+#269 = AXIS2_PLACEMENT_3D ( 'NONE', #407, #605, #555 ) ;
+#270 = FACE_OUTER_BOUND ( 'NONE', #171, .T. ) ;
+#271 = PRESENTATION_STYLE_ASSIGNMENT (( #226 ) ) ;
+#272 = ORIENTED_EDGE ( 'NONE', *, *, #335, .T. ) ;
+#273 = ORIENTED_EDGE ( 'NONE', *, *, #185, .T. ) ;
+#274 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#275 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#276 = ORIENTED_EDGE ( 'NONE', *, *, #185, .F. ) ;
+#277 = ORIENTED_EDGE ( 'NONE', *, *, #593, .T. ) ;
+#278 = AXIS2_PLACEMENT_3D ( 'NONE', #126, #321, #69 ) ;
+#279 = FACE_BOUND ( 'NONE', #250, .T. ) ;
+#280 = ADVANCED_BREP_SHAPE_REPRESENTATION ( '\X2\8f6e6bc257ab7247\X0\', ( #156, #260 ), #539 ) ;
+#281 = EDGE_CURVE ( 'NONE', #432, #22, #623, .T. ) ;
+#282 = ORIENTED_EDGE ( 'NONE', *, *, #473, .F. ) ;
+#283 = FILL_AREA_STYLE ('',( #578 ) ) ;
+#284 = CARTESIAN_POINT ( 'NONE',  ( -19.00000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#285 = ADVANCED_FACE ( 'NONE', ( #270 ), #420, .F. ) ;
+#286 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#287 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#288 = EDGE_LOOP ( 'NONE', ( #590, #277, #123, #189 ) ) ;
+#289 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#290 = AXIS2_PLACEMENT_3D ( 'NONE', #402, #544, #557 ) ;
+#291 = ORIENTED_EDGE ( 'NONE', *, *, #68, .T. ) ;
+#292 = ORIENTED_EDGE ( 'NONE', *, *, #56, .T. ) ;
+#293 = CARTESIAN_POINT ( 'NONE',  ( -7.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#294 = AXIS2_PLACEMENT_3D ( 'NONE', #503, #62, #258 ) ;
+#295 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#296 = EDGE_CURVE ( 'NONE', #465, #466, #516, .T. ) ;
+#297 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#298 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#299 = CIRCLE ( 'NONE', #23, 2.150000000000000799 ) ;
+#300 = CIRCLE ( 'NONE', #269, 2.149999999999539391 ) ;
+#301 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#302 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#303 = FACE_OUTER_BOUND ( 'NONE', #406, .T. ) ;
+#304 = CARTESIAN_POINT ( 'NONE',  ( 7.000000000000000000, -1.487354696433178969, 0.000000000000000000 ) ) ;
+#305 = CARTESIAN_POINT ( 'NONE',  ( 2.150000000000000355, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#306 = EDGE_CURVE ( 'NONE', #371, #614, #240, .T. ) ;
+#307 = AXIS2_PLACEMENT_3D ( 'NONE', #81, #187, #572 ) ;
+#308 = FACE_BOUND ( 'NONE', #367, .T. ) ;
+#309 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#310 = CARTESIAN_POINT ( 'NONE',  ( 8.242304845413448433, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#311 = CARTESIAN_POINT ( 'NONE',  ( -7.000000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#312 = EDGE_LOOP ( 'NONE', ( #232, #400 ) ) ;
+#313 =( NAMED_UNIT ( * ) SI_UNIT ( $, .STERADIAN. ) SOLID_ANGLE_UNIT ( ) );
+#314 = VERTEX_POINT ( 'NONE', #356 ) ;
+#315 = EDGE_LOOP ( 'NONE', ( #67, #535, #524, #479 ) ) ;
+#316 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#317 = ORIENTED_EDGE ( 'NONE', *, *, #296, .T. ) ;
+#318 = AXIS2_PLACEMENT_3D ( 'NONE', #606, #309, #13 ) ;
+#319 = AXIS2_PLACEMENT_3D ( 'NONE', #100, #396, #446 ) ;
+#320 = VERTEX_POINT ( 'NONE', #311 ) ;
+#321 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#322 = LINE ( 'NONE', #74, #493 ) ;
+#323 = ORIENTED_EDGE ( 'NONE', *, *, #221, .F. ) ;
+#324 = ORIENTED_EDGE ( 'NONE', *, *, #328, .F. ) ;
+#325 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#326 = FACE_BOUND ( 'NONE', #607, .T. ) ;
+#327 = AXIS2_PLACEMENT_3D ( 'NONE', #349, #54, #254 ) ;
+#328 = EDGE_CURVE ( 'NONE', #137, #120, #20, .T. ) ;
+#329 = EDGE_LOOP ( 'NONE', ( #202, #567, #215, #340 ) ) ;
+#330 = PLANE ( 'NONE',  #619 ) ;
+#331 = AXIS2_PLACEMENT_3D ( 'NONE', #287, #207, #439 ) ;
+#332 =( LENGTH_UNIT ( ) NAMED_UNIT ( * ) SI_UNIT ( .MILLI., .METRE. ) );
+#333 = ORIENTED_EDGE ( 'NONE', *, *, #55, .F. ) ;
+#334 = EDGE_CURVE ( 'NONE', #616, #264, #495, .T. ) ;
+#335 = EDGE_CURVE ( 'NONE', #44, #320, #177, .T. ) ;
+#336 = CARTESIAN_POINT ( 'NONE',  ( 1.469576158976869898E-15, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#337 = ORIENTED_EDGE ( 'NONE', *, *, #583, .F. ) ;
+#338 = ORIENTED_EDGE ( 'NONE', *, *, #412, .F. ) ;
+#339 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#340 = ORIENTED_EDGE ( 'NONE', *, *, #56, .F. ) ;
+#341 = LINE ( 'NONE', #585, #42 ) ;
+#342 = EDGE_LOOP ( 'NONE', ( #317, #615 ) ) ;
+#343 = EDGE_CURVE ( 'NONE', #53, #320, #109, .T. ) ;
+#344 = AXIS2_PLACEMENT_3D ( 'NONE', #301, #502, #399 ) ;
+#345 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#346 = FACE_OUTER_BOUND ( 'NONE', #329, .T. ) ;
+#347 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#348 = AXIS2_PLACEMENT_3D ( 'NONE', #427, #165, #366 ) ;
+#349 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#350 = AXIS2_PLACEMENT_3D ( 'NONE', #347, #295, #98 ) ;
+#351 = EDGE_CURVE ( 'NONE', #417, #314, #204, .T. ) ;
+#352 = CARTESIAN_POINT ( 'NONE',  ( 12.54230484541292512, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#353 = EDGE_CURVE ( 'NONE', #620, #418, #71, .T. ) ;
+#354 = EDGE_LOOP ( 'NONE', ( #561, #182 ) ) ;
+#355 = ADVANCED_FACE ( 'NONE', ( #346 ), #600, .F. ) ;
+#356 = CARTESIAN_POINT ( 'NONE',  ( -2.149999999999433697, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#357 = FACE_OUTER_BOUND ( 'NONE', #163, .T. ) ;
+#358 = CIRCLE ( 'NONE', #245, 2.150000000000000355 ) ;
+#359 = CARTESIAN_POINT ( 'NONE',  ( 19.00000000000000000, -1.487354696433177415, 8.000000000000000000 ) ) ;
+#360 = EDGE_LOOP ( 'NONE', ( #637, #426 ) ) ;
+#361 = CIRCLE ( 'NONE', #174, 2.149999999999539391 ) ;
+#362 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#363 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541326760, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#364 = CIRCLE ( 'NONE', #114, 2.150000000000000799 ) ;
+#365 = EDGE_LOOP ( 'NONE', ( #78, #460, #519, #574 ) ) ;
+#366 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#367 = EDGE_LOOP ( 'NONE', ( #121, #272 ) ) ;
+#368 = ORIENTED_EDGE ( 'NONE', *, *, #353, .F. ) ;
+#369 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#370 = ORIENTED_EDGE ( 'NONE', *, *, #9, .F. ) ;
+#371 = VERTEX_POINT ( 'NONE', #500 ) ;
+#372 = ORIENTED_EDGE ( 'NONE', *, *, #531, .T. ) ;
+#373 = CARTESIAN_POINT ( 'NONE',  ( 1.469576158976869898E-15, -13.48735469643318119, 0.000000000000000000 ) ) ;
+#374 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#375 = EDGE_LOOP ( 'NONE', ( #508, #273 ) ) ;
+#376 = CIRCLE ( 'NONE', #256, 19.00000000000000000 ) ;
+#377 = FACE_OUTER_BOUND ( 'NONE', #288, .T. ) ;
+#378 = ORIENTED_EDGE ( 'NONE', *, *, #627, .F. ) ;
+#379 = ORIENTED_EDGE ( 'NONE', *, *, #424, .F. ) ;
+#380 = CYLINDRICAL_SURFACE ( 'NONE', #307, 7.000000000000000000 ) ;
+#381 = AXIS2_PLACEMENT_3D ( 'NONE', #252, #551, #249 ) ;
+#382 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#383 = ADVANCED_FACE ( 'NONE', ( #112 ), #507, .F. ) ;
+#384 = CIRCLE ( 'NONE', #103, 2.150000000000000355 ) ;
+#385 = FACE_OUTER_BOUND ( 'NONE', #509, .T. ) ;
+#386 = CYLINDRICAL_SURFACE ( 'NONE', #331, 2.150000000000000355 ) ;
+#387 = VERTEX_POINT ( 'NONE', #4 ) ;
+#388 = APPLICATION_PROTOCOL_DEFINITION ( 'draft international standard', 'automotive_design', 1998, #631 ) ;
+#389 = MECHANICAL_DESIGN_GEOMETRIC_PRESENTATION_REPRESENTATION (  '', ( #641 ), #452 ) ;
+#390 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541326405, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#391 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#392 = AXIS2_PLACEMENT_3D ( 'NONE', #231, #374, #428 ) ;
+#393 = EDGE_CURVE ( 'NONE', #387, #429, #161, .T. ) ;
+#394 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#395 = CARTESIAN_POINT ( 'NONE',  ( -8.242304845413029213, 4.512645303566650057, 0.000000000000000000 ) ) ;
+#396 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#397 = EDGE_CURVE ( 'NONE', #120, #88, #569, .T. ) ;
+#398 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#399 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#400 = ORIENTED_EDGE ( 'NONE', *, *, #421, .F. ) ;
+#401 = AXIS2_PLACEMENT_3D ( 'NONE', #200, #148, #262 ) ;
+#402 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#403 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, -0.000000000000000000 ) ) ;
+#404 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541326760, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#405 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541344879, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#406 = EDGE_LOOP ( 'NONE', ( #550, #21, #228, #178 ) ) ;
+#407 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541326405, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#408 = CIRCLE ( 'NONE', #18, 2.149999999999657518 ) ;
+#409 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#410 = CYLINDRICAL_SURFACE ( 'NONE', #526, 2.149999999999657518 ) ;
+#411 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541344879, 4.512645303567090593, 0.000000000000000000 ) ) ;
+#412 = EDGE_CURVE ( 'NONE', #430, #259, #603, .T. ) ;
+#413 = VECTOR ( 'NONE', #243, 1000.000000000000000 ) ;
+#414 = VERTEX_POINT ( 'NONE', #305 ) ;
+#415 = ORIENTED_EDGE ( 'NONE', *, *, #563, .T. ) ;
+#416 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#417 = VERTEX_POINT ( 'NONE', #158 ) ;
+#418 = VERTEX_POINT ( 'NONE', #395 ) ;
+#419 = VERTEX_POINT ( 'NONE', #359 ) ;
+#420 = CYLINDRICAL_SURFACE ( 'NONE', #634, 2.150000000000000799 ) ;
+#421 = EDGE_CURVE ( 'NONE', #614, #371, #604, .T. ) ;
+#422 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541302957, 4.512645303566650057, 0.000000000000000000 ) ) ;
+#423 = ORIENTED_EDGE ( 'NONE', *, *, #610, .T. ) ;
+#424 = EDGE_CURVE ( 'NONE', #33, #573, #79, .T. ) ;
+#425 = AXIS2_PLACEMENT_3D ( 'NONE', #373, #325, #613 ) ;
+#426 = ORIENTED_EDGE ( 'NONE', *, *, #118, .T. ) ;
+#427 = CARTESIAN_POINT ( 'NONE',  ( 1.469576158976869898E-15, -13.48735469643318119, 0.000000000000000000 ) ) ;
+#428 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#429 = VERTEX_POINT ( 'NONE', #203 ) ;
+#430 = VERTEX_POINT ( 'NONE', #451 ) ;
+#431 = FACE_BOUND ( 'NONE', #312, .T. ) ;
+#432 = VERTEX_POINT ( 'NONE', #549 ) ;
+#433 = CYLINDRICAL_SURFACE ( 'NONE', #579, 2.149999999999435030 ) ;
+#434 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#435 = CIRCLE ( 'NONE', #494, 2.149999999999539391 ) ;
+#436 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#437 = ORIENTED_EDGE ( 'NONE', *, *, #463, .F. ) ;
+#438 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#439 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#440 = AXIS2_PLACEMENT_3D ( 'NONE', #363, #586, #84 ) ;
+#441 = CIRCLE ( 'NONE', #89, 2.149999999999657518 ) ;
+#442 = EDGE_LOOP ( 'NONE', ( #437, #282 ) ) ;
+#443 = LINE ( 'NONE', #138, #152 ) ;
+#444 = AXIS2_PLACEMENT_3D ( 'NONE', #390, #538, #96 ) ;
+#445 = EDGE_CURVE ( 'NONE', #314, #417, #575, .T. ) ;
+#446 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#447 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#448 = EDGE_CURVE ( 'NONE', #195, #264, #462, .T. ) ;
+#449 = FACE_BOUND ( 'NONE', #342, .T. ) ;
+#450 = LINE ( 'NONE', #6, #47 ) ;
+#451 = CARTESIAN_POINT ( 'NONE',  ( -12.54230484541280255, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#452 =( GEOMETRIC_REPRESENTATION_CONTEXT ( 3 ) GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT ( ( #540 ) ) GLOBAL_UNIT_ASSIGNED_CONTEXT ( ( #332, #484, #313 ) ) REPRESENTATION_CONTEXT ( 'NONE', 'WORKASPACE' ) );
+#453 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541326405, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#454 = FACE_OUTER_BOUND ( 'NONE', #266, .T. ) ;
+#455 = AXIS2_PLACEMENT_3D ( 'NONE', #16, #213, #208 ) ;
+#456 = CARTESIAN_POINT ( 'NONE',  ( -8.242304845413723768, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#457 = ORIENTED_EDGE ( 'NONE', *, *, #334, .F. ) ;
+#458 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541326760, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#459 = CARTESIAN_POINT ( 'NONE',  ( 8.242304845413610082, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#460 = ORIENTED_EDGE ( 'NONE', *, *, #560, .F. ) ;
+#461 = CARTESIAN_POINT ( 'NONE',  ( 2.150000000000000355, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#462 = CIRCLE ( 'NONE', #294, 2.149999999999657518 ) ;
+#463 = EDGE_CURVE ( 'NONE', #616, #554, #159, .T. ) ;
+#464 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#465 = VERTEX_POINT ( 'NONE', #52 ) ;
+#466 = VERTEX_POINT ( 'NONE', #57 ) ;
+#467 = CARTESIAN_POINT ( 'NONE',  ( -12.54230484541280255, -7.487354696433185630, 0.000000000000000000 ) ) ;
+#468 = CIRCLE ( 'NONE', #127, 19.00000000000000000 ) ;
+#469 = ORIENTED_EDGE ( 'NONE', *, *, #635, .T. ) ;
+#470 = ADVANCED_FACE ( 'NONE', ( #357 ), #19, .F. ) ;
+#471 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#472 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#473 = EDGE_CURVE ( 'NONE', #554, #616, #408, .T. ) ;
+#474 = EDGE_CURVE ( 'NONE', #320, #44, #133, .T. ) ;
+#475 = ORIENTED_EDGE ( 'NONE', *, *, #412, .T. ) ;
+#476 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#477 = CYLINDRICAL_SURFACE ( 'NONE', #392, 2.150000000000000799 ) ;
+#478 = EDGE_LOOP ( 'NONE', ( #235, #224, #145, #323 ) ) ;
+#479 = ORIENTED_EDGE ( 'NONE', *, *, #343, .F. ) ;
+#480 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#481 = ORIENTED_EDGE ( 'NONE', *, *, #265, .T. ) ;
+#482 = SURFACE_STYLE_FILL_AREA ( #283 ) ;
+#483 = ADVANCED_FACE ( 'NONE', ( #261, #308, #545, #61, #257, #449, #5, #206 ), #104, .F. ) ;
+#484 =( NAMED_UNIT ( * ) PLANE_ANGLE_UNIT ( ) SI_UNIT ( $, .RADIAN. ) );
+#485 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541326405, -7.487354696433185630, 8.000000000000000000 ) ) ;
+#486 = CARTESIAN_POINT ( 'NONE',  ( -19.00000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#487 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#488 = CARTESIAN_POINT ( 'NONE',  ( 8.242304845413448433, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#489 = CARTESIAN_POINT ( 'NONE',  ( 8.242304845413610082, -7.487354696433178525, 8.000000000000000000 ) ) ;
+#490 = ORIENTED_EDGE ( 'NONE', *, *, #281, .F. ) ;
+#491 = VECTOR ( 'NONE', #464, 1000.000000000000000 ) ;
+#492 =( NAMED_UNIT ( * ) PLANE_ANGLE_UNIT ( ) SI_UNIT ( $, .RADIAN. ) );
+#493 = VECTOR ( 'NONE', #576, 1000.000000000000000 ) ;
+#494 = AXIS2_PLACEMENT_3D ( 'NONE', #43, #286, #487 ) ;
+#495 = LINE ( 'NONE', #459, #41 ) ;
+#496 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#497 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#498 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#499 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#500 = CARTESIAN_POINT ( 'NONE',  ( -12.54230484541302992, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#501 = PRESENTATION_LAYER_ASSIGNMENT (  '', '', ( #641 ) ) ;
+#502 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#503 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541326760, -7.487354696433178525, 0.000000000000000000 ) ) ;
+#504 = ORIENTED_EDGE ( 'NONE', *, *, #265, .F. ) ;
+#505 = ADVANCED_FACE ( 'NONE', ( #201 ), #111, .T. ) ;
+#506 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#507 = CYLINDRICAL_SURFACE ( 'NONE', #93, 2.150000000000000799 ) ;
+#508 = ORIENTED_EDGE ( 'NONE', *, *, #627, .T. ) ;
+#509 = EDGE_LOOP ( 'NONE', ( #26, #423, #490, #170 ) ) ;
+#510 = CYLINDRICAL_SURFACE ( 'NONE', #278, 2.149999999999435030 ) ;
+#511 = ORIENTED_EDGE ( 'NONE', *, *, #582, .T. ) ;
+#512 = ADVANCED_FACE ( 'NONE', ( #303 ), #410, .F. ) ;
+#513 = ORIENTED_EDGE ( 'NONE', *, *, #105, .F. ) ;
+#514 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541326405, -7.487354696433185630, 0.000000000000000000 ) ) ;
+#515 = EDGE_CURVE ( 'NONE', #137, #414, #358, .T. ) ;
+#516 = CIRCLE ( 'NONE', #348, 2.149999999999435030 ) ;
+#517 = CIRCLE ( 'NONE', #599, 2.149999999999539391 ) ;
+#518 = EDGE_LOOP ( 'NONE', ( #110, #565 ) ) ;
+#519 = ORIENTED_EDGE ( 'NONE', *, *, #9, .T. ) ;
+#520 = ORIENTED_EDGE ( 'NONE', *, *, #353, .T. ) ;
+#521 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541326760, -7.487354696433178525, 0.000000000000000000 ) ) ;
+#522 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#523 = ORIENTED_EDGE ( 'NONE', *, *, #515, .T. ) ;
+#524 = ORIENTED_EDGE ( 'NONE', *, *, #474, .F. ) ;
+#525 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541344879, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#526 = AXIS2_PLACEMENT_3D ( 'NONE', #458, #498, #15 ) ;
+#527 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#528 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#529 = EDGE_CURVE ( 'NONE', #387, #430, #300, .T. ) ;
+#530 = CYLINDRICAL_SURFACE ( 'NONE', #444, 2.149999999999539391 ) ;
+#531 = EDGE_CURVE ( 'NONE', #29, #432, #115, .T. ) ;
+#532 = COLOUR_RGB ( '',0.7921568627450980005, 0.8196078431372548767, 0.9333333333333333481 ) ;
+#533 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#534 = EDGE_CURVE ( 'NONE', #414, #88, #597, .T. ) ;
+#535 = ORIENTED_EDGE ( 'NONE', *, *, #102, .T. ) ;
+#536 = AXIS2_PLACEMENT_3D ( 'NONE', #411, #179, #76 ) ;
+#537 =( LENGTH_UNIT ( ) NAMED_UNIT ( * ) SI_UNIT ( .MILLI., .METRE. ) );
+#538 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#539 =( GEOMETRIC_REPRESENTATION_CONTEXT ( 3 ) GLOBAL_UNCERTAINTY_ASSIGNED_CONTEXT ( ( #143 ) ) GLOBAL_UNIT_ASSIGNED_CONTEXT ( ( #537, #492, #642 ) ) REPRESENTATION_CONTEXT ( 'NONE', 'WORKASPACE' ) );
+#540 = UNCERTAINTY_MEASURE_WITH_UNIT (LENGTH_MEASURE( 1.000000000000000082E-05 ), #332, 'distance_accuracy_value', 'NONE');
+#541 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#542 = AXIS2_PLACEMENT_3D ( 'NONE', #8, #155, #60 ) ;
+#543 = CARTESIAN_POINT ( 'NONE',  ( -19.00000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#544 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#545 = FACE_BOUND ( 'NONE', #130, .T. ) ;
+#546 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#547 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 8.000000000000000000 ) ) ;
+#548 = CARTESIAN_POINT ( 'NONE',  ( 7.000000000000000000, -1.487354696433178969, 8.000000000000000000 ) ) ;
+#549 = CARTESIAN_POINT ( 'NONE',  ( 8.242304845413448433, 4.512645303567090593, 0.000000000000000000 ) ) ;
+#550 = ORIENTED_EDGE ( 'NONE', *, *, #632, .F. ) ;
+#551 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#552 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#553 = CIRCLE ( 'NONE', #82, 2.150000000000000799 ) ;
+#554 = VERTEX_POINT ( 'NONE', #352 ) ;
+#555 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#556 = CIRCLE ( 'NONE', #608, 19.00000000000000000 ) ;
+#557 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#558 = ORIENTED_EDGE ( 'NONE', *, *, #593, .F. ) ;
+#559 = FACE_OUTER_BOUND ( 'NONE', #587, .T. ) ;
+#560 = EDGE_CURVE ( 'NONE', #83, #419, #468, .T. ) ;
+#561 = ORIENTED_EDGE ( 'NONE', *, *, #241, .T. ) ;
+#562 = EDGE_LOOP ( 'NONE', ( #172, #379 ) ) ;
+#563 = EDGE_CURVE ( 'NONE', #88, #120, #25, .T. ) ;
+#564 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#565 = ORIENTED_EDGE ( 'NONE', *, *, #529, .F. ) ;
+#566 = ORIENTED_EDGE ( 'NONE', *, *, #393, .T. ) ;
+#567 = ORIENTED_EDGE ( 'NONE', *, *, #421, .T. ) ;
+#568 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#569 = CIRCLE ( 'NONE', #184, 2.150000000000000355 ) ;
+#570 = CARTESIAN_POINT ( 'NONE',  ( 10.39230484541344879, 4.512645303567090593, 0.000000000000000000 ) ) ;
+#571 = EDGE_LOOP ( 'NONE', ( #193, #481 ) ) ;
+#572 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#573 = VERTEX_POINT ( 'NONE', #543 ) ;
+#574 = ORIENTED_EDGE ( 'NONE', *, *, #38, .T. ) ;
+#575 = CIRCLE ( 'NONE', #542, 2.149999999999435030 ) ;
+#576 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#577 = ADVANCED_FACE ( 'NONE', ( #589 ), #90, .T. ) ;
+#578 = FILL_AREA_STYLE_COLOUR ( '', #532 ) ;
+#579 = AXIS2_PLACEMENT_3D ( 'NONE', #336, #175, #136 ) ;
+#580 = AXIS2_PLACEMENT_3D ( 'NONE', #541, #289, #149 ) ;
+#581 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#582 = EDGE_CURVE ( 'NONE', #414, #137, #384, .T. ) ;
+#583 = EDGE_CURVE ( 'NONE', #417, #466, #341, .T. ) ;
+#584 = ADVANCED_FACE ( 'NONE', ( #199 ), #3, .F. ) ;
+#585 = CARTESIAN_POINT ( 'NONE',  ( 2.149999999999436362, -13.48735469643318119, 8.000000000000000000 ) ) ;
+#586 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#587 = EDGE_LOOP ( 'NONE', ( #598, #511, #176, #66 ) ) ;
+#588 = CYLINDRICAL_SURFACE ( 'NONE', #164, 2.149999999999657518 ) ;
+#589 = FACE_OUTER_BOUND ( 'NONE', #233, .T. ) ;
+#590 = ORIENTED_EDGE ( 'NONE', *, *, #102, .F. ) ;
+#591 = APPLICATION_PROTOCOL_DEFINITION ( 'draft international standard', 'automotive_design', 1998, #49 ) ;
+#592 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541302957, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#593 = EDGE_CURVE ( 'NONE', #17, #53, #242, .T. ) ;
+#594 = CARTESIAN_POINT ( 'NONE',  ( 12.54230484541344914, 4.512645303567090593, 8.000000000000000000 ) ) ;
+#595 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 10.51264530356682236, 0.000000000000000000 ) ) ;
+#596 = VECTOR ( 'NONE', #409, 1000.000000000000000 ) ;
+#597 = LINE ( 'NONE', #461, #220 ) ;
+#598 = ORIENTED_EDGE ( 'NONE', *, *, #534, .F. ) ;
+#599 = AXIS2_PLACEMENT_3D ( 'NONE', #485, #188, #297 ) ;
+#600 = CYLINDRICAL_SURFACE ( 'NONE', #455, 2.150000000000000799 ) ;
+#601 = PRODUCT ( '\X2\8f6e6bc257ab7247\X0\', '\X2\8f6e6bc257ab7247\X0\', '', ( #146 ) ) ;
+#602 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, -1.487354696433179857, 0.000000000000000000 ) ) ;
+#603 = LINE ( 'NONE', #150, #491 ) ;
+#604 = CIRCLE ( 'NONE', #214, 2.150000000000000799 ) ;
+#605 = DIRECTION ( 'NONE',  ( 0.000000000000000000, 0.000000000000000000, 1.000000000000000000 ) ) ;
+#606 = CARTESIAN_POINT ( 'NONE',  ( -10.39230484541302957, 4.512645303566650057, 8.000000000000000000 ) ) ;
+#607 = EDGE_LOOP ( 'NONE', ( #92, #14 ) ) ;
+#608 = AXIS2_PLACEMENT_3D ( 'NONE', #339, #40, #108 ) ;
+#609 = LINE ( 'NONE', #267, #31 ) ;
+#610 = EDGE_CURVE ( 'NONE', #12, #22, #45, .T. ) ;
+#611 = ORIENTED_EDGE ( 'NONE', *, *, #236, .F. ) ;
+#612 = ORIENTED_EDGE ( 'NONE', *, *, #463, .T. ) ;
+#613 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#614 = VERTEX_POINT ( 'NONE', #39 ) ;
+#615 = ORIENTED_EDGE ( 'NONE', *, *, #190, .T. ) ;
+#616 = VERTEX_POINT ( 'NONE', #489 ) ;
+#617 = CARTESIAN_POINT ( 'NONE',  ( 0.000000000000000000, 10.51264530356682236, 8.000000000000000000 ) ) ;
+#618 = LINE ( 'NONE', #624, #34 ) ;
+#619 = AXIS2_PLACEMENT_3D ( 'NONE', #382, #528, #630 ) ;
+#620 = VERTEX_POINT ( 'NONE', #141 ) ;
+#621 = ORIENTED_EDGE ( 'NONE', *, *, #351, .F. ) ;
+#622 = CYLINDRICAL_SURFACE ( 'NONE', #230, 2.150000000000000355 ) ;
+#623 = CIRCLE ( 'NONE', #536, 2.150000000000000799 ) ;
+#624 = CARTESIAN_POINT ( 'NONE',  ( 7.000000000000000000, -1.487354696433178969, 8.000000000000000000 ) ) ;
+#625 = EDGE_LOOP ( 'NONE', ( #520, #292 ) ) ;
+#626 = EDGE_LOOP ( 'NONE', ( #337, #166, #70, #36 ) ) ;
+#627 = EDGE_CURVE ( 'NONE', #259, #429, #435, .T. ) ;
+#628 = FACE_OUTER_BOUND ( 'NONE', #571, .T. ) ;
+#629 = EDGE_LOOP ( 'NONE', ( #612, #227, #32, #457 ) ) ;
+#630 = DIRECTION ( 'NONE',  ( 1.000000000000000000, 0.000000000000000000, -0.000000000000000000 ) ) ;
+#631 = APPLICATION_CONTEXT ( 'automotive_design' ) ;
+#632 = EDGE_CURVE ( 'NONE', #554, #195, #443, .T. ) ;
+#633 = CARTESIAN_POINT ( 'NONE',  ( -2.150000000000000355, 10.51264530356682236, 0.000000000000000000 ) ) ;
+#634 = AXIS2_PLACEMENT_3D ( 'NONE', #173, #274, #229 ) ;
+#635 = EDGE_CURVE ( 'NONE', #430, #387, #517, .T. ) ;
+#636 = ORIENTED_EDGE ( 'NONE', *, *, #534, .T. ) ;
+#637 = ORIENTED_EDGE ( 'NONE', *, *, #281, .T. ) ;
+#638 = DIRECTION ( 'NONE',  ( -0.000000000000000000, -0.000000000000000000, -1.000000000000000000 ) ) ;
+#639 = DIRECTION ( 'NONE',  ( -1.000000000000000000, 0.000000000000000000, 0.000000000000000000 ) ) ;
+#640 = ORIENTED_EDGE ( 'NONE', *, *, #236, .T. ) ;
+#641 = STYLED_ITEM ( 'NONE', ( #271 ), #156 ) ;
+#642 =( NAMED_UNIT ( * ) SI_UNIT ( $, .STERADIAN. ) SOLID_ANGLE_UNIT ( ) );
+#643 = CIRCLE ( 'NONE', #135, 7.000000000000000000 ) ;
+ENDSEC;
+END-ISO-10303-21;

02_mechanical/source_notes.txt

@@ -0,0 +1 @@
+机械设计采用26版本sw设计，装配，以及urdf导出，为减少sw装配出现问题，采用单件装配小件各个关节装配体，转step关节文件，step各关节装配总装配体方案

03_hardware/README.md

@@ -0,0 +1,5 @@
+# 硬件
+
+本目录用于保存 16DOF 轮足机器人的自研电路、接线图、BOM、传感器与计算平台说明。
+
+第三方硬件资料不作为自研成果提交。

04_firmware/README.md

@@ -0,0 +1,5 @@
+# 嵌入式固件
+
+本目录用于保存运行在 MCU 或其他嵌入式控制器上的固件源码和构建工程。
+
+编译生成的 `.hex`、`.bin`、`.elf`、`.axf` 等文件不进入源码目录，可在需要时作为 Release 附件发布。

05_software/README.md

@@ -0,0 +1,37 @@
+# 软件
+
+本目录当前保存 16DOF 轮足机器人的第一代完整软件闭环。
+
+```text
+05_software/
+├─ train/
+│  └─ rc_mjlab/       # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
+└─ real/
+   ├─ ik_real/        # IK 轨迹与早期真机控制
+   └─ sim2real/       # 第一代 Python 策略真机部署
+```
+
+## 数据流
+
+```text
+MJCF + mjlab task
+        |
+        v
+   PPO 训练策略
+        |
+        +----> MuJoCo 独立模型调试
+        |
+        +----> Sim2Sim 策略验证
+        |
+        +----> Python Sim2Real ----> 电机 / IMU
+
+IK real --------------------------------> 电机
+```
+
+`rc_mjlab` 在早期版本中是自包含工程。训练、MJCF、独立 MuJoCo、Sim2Sim 和策略权重通过相对路径绑定，因此本次保留其原始内部布局，没有为了目录外观拆散。
+
+详细说明见：
+
+- [`train/README.md`](train/README.md)
+- [`real/README.md`](real/README.md)
+- [`../01_doc/architecture/early_software_stack.md`](../01_doc/architecture/early_software_stack.md)

05_software/real/README.md

@@ -0,0 +1,21 @@
+# 第一代真机控制
+
+本目录保存 16DOF 轮足机器人的早期真机控制实现。
+
+## `ik_real`
+
+基于几何逆运动学和轨迹插值的真机控制探索，不依赖强化学习策略。主要用于验证电机接口、关节映射和姿态轨迹。
+
+## `sim2real`
+
+第一代 Python 策略部署栈，包含：
+
+- 53D 观测到 16D 动作的策略运行时
+- 电机映射和真机 IO
+- IMU 接入
+- 站立初始化与平衡
+- 运行时安全检查和阻尼刹车
+- Web 调试界面
+- 对齐、标定和独立检查工具
+
+部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。

05_software/real/ik_real/README.md

@@ -0,0 +1,9 @@
+# IK 真机控制探索
+
+该目录保存强化学习部署前的逆运动学真机控制代码。
+
+- `sim2real_control_api.py`：真机控制接口
+- `trajectory_interpolator.py`：关节/姿态轨迹插值
+- `sim_to_real_deploy_beifen.py`：早期部署脚本备份
+
+文件名中的 `beifen` 来自原始资料。为保持早期版本可追溯性，本次归档不修改源码和文件名。

05_software/real/ik_real/sim2real_control_api.py

@@ -0,0 +1,274 @@
+"""Sim-to-real control/IK API extracted from mature mujoco_sim controller.
+
+This module provides a deployment-friendly wrapper around:
+- wheel mode posture control
+- trot swing-leg IK + wheel assist
+- differential wheel speed mapping
+
+No MuJoCo runtime is required for using the API itself.
+For trot IK, Pinocchio model is used via Dynamics.
+"""
+
+from dataclasses import dataclass
+from typing import Dict, Optional, Tuple
+import numpy as np
+
+from config import (
+    LEG_NAMES,
+    LEG_JOINTS,
+    WHEEL_JOINT,
+    DEFAULT_JOINT_ANGLES,
+    WHEEL_RADIUS,
+    WHEEL_TRACK,
+    WHEEL_VEL_MAX,
+    KP_ROLL,
+    KP_PITCH,
+    GAIT_FREQ,
+    GAIT_DUTY,
+    SWING_HEIGHT,
+    PHASE_OFFSETS,
+)
+from dynamics import Dynamics
+
+
+@dataclass
+class DeployState:
+    """Minimal state for deployment control."""
+
+    rpy: np.ndarray  # (3,) roll, pitch, yaw
+
+
+class Sim2RealControlAPI:
+    """Deployment-friendly control and IK API.
+
+    Supported modes:
+    - wheel: wheel differential drive + leg posture hold
+    - trot: swing foot IK + stance posture + wheel assist
+    """
+
+    def __init__(self):
+        self.mode = "wheel"
+        self.prone = False
+
+        self.vel_x = 0.0
+        self.vel_y = 0.0
+        self.yaw_rate = 0.0
+        self.height = 0.33
+
+        self._gait_phase = 0.0
+        self._smooth_vx = 0.0
+        self._smooth_vy = 0.0
+        self._smooth_yaw = 0.0
+
+        self._default_q = np.array([
+            DEFAULT_JOINT_ANGLES["hip_abduction"],
+            DEFAULT_JOINT_ANGLES["hip_pitch"],
+            DEFAULT_JOINT_ANGLES["knee"],
+        ])
+
+        self.dynamics = Dynamics()
+        self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
+        self._last_contact = {leg: True for leg in LEG_NAMES}
+
+    def set_mode(self, mode: str):
+        if mode not in ("wheel", "trot"):
+            raise ValueError("mode must be one of: wheel, trot")
+        self.mode = mode
+
+    def set_command(self, vel_x: float, vel_y: float, yaw_rate: float, height: Optional[float] = None):
+        self.vel_x = float(vel_x)
+        self.vel_y = float(vel_y)
+        self.yaw_rate = float(yaw_rate)
+        if height is not None:
+            self.height = float(height)
+
+    def compute(
+        self,
+        state: DeployState,
+        dt: float,
+        q_pin: Optional[np.ndarray] = None,
+        dq_pin: Optional[np.ndarray] = None,
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """Compute leg and wheel commands.
+
+        Returns:
+            leg_targets: (12,) [fl(3), fr(3), rl(3), rr(3)]
+            wheel_targets: (4,) [fl, fr, rl, rr] in rad/s
+
+        Notes:
+            - wheel mode does not require q_pin/dq_pin
+            - trot mode requires q_pin/dq_pin for IK/FK through Pinocchio
+        """
+        alpha = min(float(dt) * 3.0, 1.0)
+        self._smooth_vx += alpha * (self.vel_x - self._smooth_vx)
+        self._smooth_vy += alpha * (self.vel_y - self._smooth_vy)
+        self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw)
+
+        if self.prone:
+            return self._prone_mode()
+
+        if self.mode == "wheel":
+            return self._wheel_mode(state)
+
+        if q_pin is None or dq_pin is None:
+            raise ValueError("trot mode requires q_pin and dq_pin")
+        return self._trot_mode(state, float(dt), q_pin, dq_pin)
+
+    def to_joint_dict(self, leg_targets: np.ndarray, wheel_targets: np.ndarray) -> Dict[str, float]:
+        """Convert array commands to named joint-command dictionary."""
+        out: Dict[str, float] = {}
+        for i, leg in enumerate(LEG_NAMES):
+            out[f"{leg}_{LEG_JOINTS[0]}"] = float(leg_targets[i * 3 + 0])
+            out[f"{leg}_{LEG_JOINTS[1]}"] = float(leg_targets[i * 3 + 1])
+            out[f"{leg}_{LEG_JOINTS[2]}"] = float(leg_targets[i * 3 + 2])
+            out[f"{leg}_{WHEEL_JOINT}"] = float(wheel_targets[i])
+        return out
+
+    def _prone_mode(self):
+        leg_targets = np.zeros(12)
+        for i, leg in enumerate(LEG_NAMES):
+            side = 1.0 if leg[1] == "l" else -1.0
+            leg_targets[i * 3 + 0] = side * 0.3
+            leg_targets[i * 3 + 1] = 1.5
+            leg_targets[i * 3 + 2] = -2.65
+        return leg_targets, np.zeros(4)
+
+    def _wheel_mode(self, state: DeployState):
+        wheel_targets = self._differential_drive(self._smooth_vx, self._smooth_yaw)
+        leg_targets = self._posture_control(state)
+        return leg_targets, wheel_targets
+
+    def _posture_control(self, state: DeployState) -> np.ndarray:
+        leg_targets = np.zeros(12)
+
+        _H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
+        _HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
+        _KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
+
+        h_clamp = np.clip(self.height, _H[0], _H[-1])
+        q_hip_base = float(np.interp(h_clamp, _H, _HIP))
+        q_knee_base = float(np.interp(h_clamp, _H, _KNEE))
+
+        roll_corr = -KP_ROLL * float(state.rpy[0])
+        pitch_corr = -KP_PITCH * float(state.rpy[1])
+        lateral_lean = 0.3 * self.vel_y
+
+        for i, leg in enumerate(LEG_NAMES):
+            side = 1.0 if leg[1] == "l" else -1.0
+            leg_targets[i * 3 + 0] = np.clip(side * roll_corr + lateral_lean, -0.5, 0.5)
+            leg_targets[i * 3 + 1] = np.clip(q_hip_base + pitch_corr, -1.0, 2.5)
+            leg_targets[i * 3 + 2] = np.clip(q_knee_base, -2.6, -0.3)
+
+        return leg_targets
+
+    def _trot_mode(self, state: DeployState, dt: float, q_pin: np.ndarray, dq_pin: np.ndarray):
+        self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
+
+        contacts: Dict[str, bool] = {}
+        for leg in LEG_NAMES:
+            phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+            contacts[leg] = bool(phase < GAIT_DUTY)
+
+        self.dynamics.update(q_pin, dq_pin)
+
+        leg_targets = np.zeros(12)
+        wheel_targets = np.zeros(4)
+
+        for i, leg in enumerate(LEG_NAMES):
+            if contacts[leg]:
+                leg_targets[i * 3:(i + 1) * 3] = self._stance_leg_target(state, leg)
+                self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
+                self._last_contact[leg] = True
+                wheel_targets[i] = self._differential_drive_single(self._smooth_vx, self._smooth_yaw, leg)
+            else:
+                swing_phase = self._get_swing_phase(leg)
+                target_foot = self._compute_swing_target(leg, state, swing_phase)
+                q_ik = self.dynamics.inverse_kinematics(leg, target_foot, q_pin)
+                leg_targets[i * 3:(i + 1) * 3] = q_ik
+                self._last_contact[leg] = False
+                wheel_targets[i] = 0.0
+
+        return leg_targets, wheel_targets
+
+    def _stance_leg_target(self, state: DeployState, leg: str) -> np.ndarray:
+        _H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
+        _HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
+        _KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
+
+        h_clamp = np.clip(self.height, _H[0], _H[-1])
+        q_hip = float(np.interp(h_clamp, _H, _HIP))
+        q_knee = float(np.interp(h_clamp, _H, _KNEE))
+
+        roll_corr = -KP_ROLL * float(state.rpy[0])
+        pitch_corr = -KP_PITCH * float(state.rpy[1])
+        side = 1.0 if leg[1] == "l" else -1.0
+        lateral_lean = 0.3 * self.vel_y
+
+        return np.array([
+            np.clip(side * roll_corr + lateral_lean, -0.5, 0.5),
+            np.clip(q_hip + pitch_corr, -1.0, 2.5),
+            np.clip(q_knee, -2.6, -0.3),
+        ])
+
+    def _differential_drive(self, vel_x: float, yaw_rate: float) -> np.ndarray:
+        vel_left = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        vel_right = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        targets = np.zeros(4)
+        for i, leg in enumerate(LEG_NAMES):
+            targets[i] = vel_left if leg[1] == "l" else vel_right
+        return np.clip(targets, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
+
+    def _differential_drive_single(self, vel_x: float, yaw_rate: float, leg: str) -> float:
+        if leg[1] == "l":
+            v = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        else:
+            v = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        return float(np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX))
+
+    def _get_swing_phase(self, leg: str) -> float:
+        phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+        if phase < GAIT_DUTY:
+            return 0.0
+        return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY)
+
+    def _compute_swing_target(self, leg: str, state: DeployState, swing_phase: float) -> np.ndarray:
+        p_start = self._swing_start_foot[leg]
+        p_end = self._compute_touchdown(leg, state)
+
+        s = swing_phase
+        s_mj = 10 * s**3 - 15 * s**4 + 6 * s**5
+
+        pos = p_start + (p_end - p_start) * s_mj
+
+        z_lift = 64.0 * s**3 * (1.0 - s)**3
+        pos[2] = p_start[2] + SWING_HEIGHT * z_lift
+
+        return pos
+
+    def _compute_touchdown(self, leg: str, state: DeployState) -> np.ndarray:
+        td = self._swing_start_foot[leg].copy()
+
+        t_stance = (1.0 / GAIT_FREQ) * GAIT_DUTY
+        yaw = float(state.rpy[2])
+        c, s = np.cos(yaw), np.sin(yaw)
+        R_z = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
+        cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
+
+        td[0] += cmd_vel_world[0] * t_stance * 0.5
+        td[1] += cmd_vel_world[1] * t_stance * 0.5
+        td[2] = WHEEL_RADIUS
+        return td
+
+
+if __name__ == "__main__":
+    api = Sim2RealControlAPI()
+    api.set_mode("wheel")
+    api.set_command(vel_x=0.3, vel_y=0.0, yaw_rate=0.0, height=0.33)
+
+    state = DeployState(rpy=np.array([0.0, 0.0, 0.0]))
+    leg, wheel = api.compute(state=state, dt=0.004)
+    cmd = api.to_joint_dict(leg, wheel)
+
+    print("Example wheel-mode command:")
+    for k, v in sorted(cmd.items()):
+        print(f"{k}: {v:.6f}")

05_software/real/ik_real/sim_to_real_deploy_beifen.py

@@ -0,0 +1,234 @@
+#!/usr/bin/env python3
+import argparse, sys, threading, time
+from dataclasses import dataclass
+from pathlib import Path
+import numpy as np
+
+sys.path.append('/home/rc2/work/rcwork/control')
+from drivers.motor_driver import RobStrideDriver
+
+ROOT = Path('/home/rc2/work/rcwork/wheelleg_deploy_swj/wheelleg_deploy/wheelleg_mjlab/beifen')
+sys.path += [str(ROOT), str(ROOT / 'mujoco_sim')]
+from sim2real_control_api import DeployState, Sim2RealControlAPI  # type: ignore
+
+sys.path.append('/home/rc2/work/rcwork')
+from trajectory_interpolator import TrajectoryInterpolator
+
+LEGS = ('fl','fr','rl','rr')
+LJ = ('hip_abduction_joint','hip_pitch_joint','knee_joint')
+WJ = 'wheel_joint'
+JNS = [f'{l}_{j}' for l in LEGS for j in (*LJ, WJ)]
+
+@dataclass
+class Cfg:
+    mid:int; model:str; sign:float; off:float; bus:str
+
+class Deploy:
+    def __init__(self, can1, can2, hz=100.0, use_interpolation=True, interp_method='quintic', interp_time=1.5):
+        self.d1, self.d2 = RobStrideDriver(can1, False), RobStrideDriver(can2, False)
+        self.api = Sim2RealControlAPI(); self.dt = 1.0/hz; self.lk = threading.Lock()
+        self.run = False; self.enabled = False; self.estop = True
+        self.mode='stand'; self.vx=0.0; self.vy=0.0; self.yaw=0.0; self.h=0.33; self.roll=0.0; self.pitch=0.0
+        self.prone = False
+        self.kp_leg, self.kd_leg, self.kd_wheel = 80.0, 2.5, 2.0
+        self.cfg = self._cfg(); self.q=np.zeros(23); self.dq=np.zeros(22)
+        
+        self.use_interpolation = use_interpolation
+        if self.use_interpolation:
+            self.interpolator = TrajectoryInterpolator(method=interp_method, transition_time=interp_time)
+            print(f'[Interpolation] Enabled: method={interp_method}, transition_time={interp_time}s')
+        else:
+            self.interpolator = None
+            print('[Interpolation] Disabled')
+
+    def _cfg(self):
+        sign={'fl_hip_abduction_joint':-1,'fl_hip_pitch_joint':-1,'fl_knee_joint':-1,'fl_wheel_joint':-1,'fr_hip_abduction_joint':-1,'fr_hip_pitch_joint':1,'fr_knee_joint':1,'fr_wheel_joint':1,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':-1,'rl_knee_joint':-1,'rl_wheel_joint':-1,'rr_hip_abduction_joint':1,'rr_hip_pitch_joint':1,'rr_knee_joint':1,'rr_wheel_joint':1}
+        off={'fl_hip_abduction_joint':0.003,'fl_hip_pitch_joint':0.030,'fl_knee_joint':0.028,'fl_wheel_joint':0.0,'fr_hip_abduction_joint':0.004,'fr_hip_pitch_joint':0.038,'fr_knee_joint':0.011,'fr_wheel_joint':0.0,'rl_hip_abduction_joint':0.019,'rl_hip_pitch_joint':-0.034,'rl_knee_joint':0.025,'rl_wheel_joint':0.0,'rr_hip_abduction_joint':-0.001,'rr_hip_pitch_joint':0.039,'rr_knee_joint':0.018,'rr_wheel_joint':0.0}
+        ids={'fl_hip_abduction_joint':1,'fl_hip_pitch_joint':2,'fl_knee_joint':3,'fl_wheel_joint':4,'fr_hip_abduction_joint':5,'fr_hip_pitch_joint':6,'fr_knee_joint':7,'fr_wheel_joint':8,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':2,'rl_knee_joint':3,'rl_wheel_joint':4,'rr_hip_abduction_joint':5,'rr_hip_pitch_joint':6,'rr_knee_joint':7,'rr_wheel_joint':8}
+        bus={k:('can1' if k.startswith('f') else 'can2') for k in JNS}
+        return {jn:Cfg(ids[jn],'rs-06',float(sign[jn]),float(off[jn]),bus[jn]) for jn in JNS}
+
+    def _drv(self, jn): return self.d1 if self.cfg[jn].bus=='can1' else self.d2
+
+    def connect(self):
+        self.d1.connect(); self.d2.connect()
+        for jn,c in self.cfg.items(): self._drv(jn).add_motor(jn,c.mid,c.model)
+
+    def enable_all(self):
+        for jn in JNS: self._drv(jn).enable(jn)
+        with self.lk: self.enabled=True; self.estop=False; self.vx=self.vy=self.yaw=0.0
+
+    def disable_all(self):
+        for jn in JNS: self._drv(jn).disable(jn)
+        with self.lk: self.enabled=False
+
+    def clear(self):
+        for jn in JNS: self._drv(jn).clear_warnings(jn)
+
+    def set_estop(self,on):
+        with self.lk:
+            self.estop=on
+            if on: self.vx=self.vy=self.yaw=0.0
+        if on: self.disable_all()
+
+    def set_prone(self,on):
+        with self.lk: self.prone=on; self.api.prone=on
+
+    def _update_pin(self,leg,wheel):
+        q=np.zeros(23); dq=np.zeros(22); q[2]=self.h; q[6]=1.0
+        for i,_ in enumerate(LEGS):
+            b=7+i*4; q[b:b+3]=leg[i*3:i*3+3]; dq[6+i*4+3]=wheel[i]
+        self.q,self.dq=q,dq
+
+    def step(self):
+        with self.lk:
+            if self.estop or (not self.enabled): return
+            m=self.mode; vx=float(np.clip(self.vx,-0.8,0.8)); vy=float(np.clip(self.vy,-0.5,0.5)); yaw=float(np.clip(self.yaw,-3,3)); h=float(np.clip(self.h,0.157,0.448)); r=float(np.clip(self.roll,-0.4,0.4)); p=float(np.clip(self.pitch,-0.4,0.4)); prone=self.prone
+        cm='trot' if m=='trot' else 'wheel'
+        if m=='stand': vx=vy=yaw=0.0
+        self.api.prone=prone; self.api.set_mode(cm); self.api.set_command(vx,vy,yaw,height=h)
+        st=DeployState(rpy=np.array([r,p,0.0]))
+        leg,wheel = self.api.compute(st,self.dt,self.q,self.dq) if cm=='trot' else self.api.compute(st,self.dt)
+        self._update_pin(leg,wheel); cmd=self.api.to_joint_dict(leg,wheel)
+        
+        if self.use_interpolation and self.interpolator is not None:
+            leg_cmd = {jn: cmd[jn] for jn in JNS if not jn.endswith(WJ)}
+            self.interpolator.set_target(leg_cmd)
+            smooth_cmd = self.interpolator.update(self.dt)
+            for jn in leg_cmd:
+                cmd[jn] = smooth_cmd[jn]
+        
+        for jn in JNS:
+            d=self._drv(jn); c=self.cfg[jn]
+            if jn.endswith(WJ): d.control_mit(jn,0.0,c.sign*float(cmd.get(jn,0.0)),0.0,self.kd_wheel,0.0)
+            else: d.control_mit(jn,c.sign*float(cmd[jn])+c.off,0.0,self.kp_leg,self.kd_leg,0.0)
+
+    def loop(self):
+        while self.run:
+            t=time.time()
+            try: self.step()
+            except Exception as e: print('[control]',e)
+            time.sleep(max(0.0,self.dt-(time.time()-t)))
+
+    def start(self): self.run=True; threading.Thread(target=self.loop,daemon=True).start()
+    def stop(self):
+        self.run=False; time.sleep(0.05)
+        try: self.disable_all()
+        finally: self.d1.disconnect(); self.d2.disconnect()
+
+    def status(self):
+        with self.lk: return f'mode={self.mode} en={self.enabled} estop={self.estop} prone={self.prone} vx={self.vx:.2f} vy={self.vy:.2f} yaw={self.yaw:.2f} h={self.h:.3f}'
+
+class CLI:
+    def __init__(self,d): self.d=d
+    def run(self):
+        print('enable disable clear estop_on estop_off prone_on prone_off status')
+        print('mode stand|wheel|trot, vx vy yaw h roll pitch, stop, quit')
+        print('interp_on interp_off interp_time <sec>, interp_method linear|cubic|quintic|cosine')
+        while True:
+            try: s=input('cmd> ').strip().lower()
+            except (EOFError,KeyboardInterrupt): s='quit'
+            if s in ('quit','exit'): break
+            if s=='enable': self.d.enable_all(); continue
+            if s=='disable': self.d.disable_all(); continue
+            if s=='clear': self.d.clear(); continue
+            if s=='estop_on': self.d.set_estop(True); continue
+            if s=='estop_off': self.d.set_estop(False); continue
+            if s=='prone_on': self.d.set_prone(True); continue
+            if s=='prone_off': self.d.set_prone(False); continue
+            if s=='status': print(self.d.status()); continue
+            if s=='stop':
+                with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0
+                continue
+            if s=='interp_on':
+                with self.d.lk: self.d.use_interpolation=True
+                print('Interpolation enabled'); continue
+            if s=='interp_off':
+                with self.d.lk: self.d.use_interpolation=False
+                print('Interpolation disabled'); continue
+            if s.startswith('interp_time '):
+                try:
+                    t=float(s.split()[1])
+                    if self.d.interpolator: self.d.interpolator.set_transition_time(t)
+                    print(f'Interpolation time set to {t}s')
+                except Exception as e: print(f'Error: {e}')
+                continue
+            if s.startswith('interp_method '):
+                try:
+                    method=s.split()[1]
+                    if self.d.interpolator: self.d.interpolator.set_method(method)
+                    print(f'Interpolation method set to {method}')
+                except Exception as e: print(f'Error: {e}')
+                continue
+            if s.startswith('mode '):
+                m=s.split()[1]
+                if m in ('stand','wheel','trot'):
+                    with self.d.lk: self.d.mode=m
+                else: print('bad mode')
+                continue
+            try:
+                k,v=s.split()[0],float(s.split()[1])
+                with self.d.lk:
+                    if k=='vx': self.d.vx=v
+                    elif k=='vy': self.d.vy=v
+                    elif k=='yaw': self.d.yaw=v
+                    elif k=='h': self.d.h=v
+                    elif k=='roll': self.d.roll=v
+                    elif k=='pitch': self.d.pitch=v
+                    else: print('unknown')
+            except Exception: print('unknown/bad')
+
+class GUI:
+    def __init__(self,d):
+        import tkinter as tk
+        from tkinter import ttk
+        self.d=d; self.root=tk.Tk(); self.root.title('WheelLeg Deploy')
+        f=ttk.Frame(self.root,padding=8); f.grid(row=0,column=0,sticky='nsew')
+        self.state=tk.StringVar(value='E-STOP ON'); ttk.Label(f,textvariable=self.state).grid(row=0,column=0,columnspan=4,sticky='w')
+        ttk.Button(f,text='Enable',command=self.en).grid(row=1,column=0)
+        ttk.Button(f,text='Disable',command=self.dis).grid(row=1,column=1)
+        ttk.Button(f,text='E-STOP ON',command=lambda:self.es(True)).grid(row=1,column=2)
+        ttk.Button(f,text='E-STOP OFF',command=lambda:self.es(False)).grid(row=1,column=3)
+        ttk.Button(f,text='Prone ON',command=lambda:self.pr(True)).grid(row=2,column=2)
+        ttk.Button(f,text='Prone OFF',command=lambda:self.pr(False)).grid(row=2,column=3)
+        self.mode=tk.StringVar(value='stand'); self.vx=tk.DoubleVar(value=0.0); self.vy=tk.DoubleVar(value=0.0); self.yaw=tk.DoubleVar(value=0.0); self.h=tk.DoubleVar(value=0.33)
+        self.roll=tk.DoubleVar(value=0.0); self.pitch=tk.DoubleVar(value=0.0)
+        cb=ttk.Combobox(f,textvariable=self.mode,values=['stand','wheel','trot'],state='readonly'); cb.grid(row=3,column=0,columnspan=2,sticky='ew'); cb.bind('<<ComboboxSelected>>',lambda _:self.sync())
+        ttk.Button(f,text='Stop',command=self.stp).grid(row=3,column=3)
+        self.sl(f,4,'vx',self.vx,-0.8,0.8); self.sl(f,5,'vy',self.vy,-0.5,0.5); self.sl(f,6,'yaw',self.yaw,-3,3); self.sl(f,7,'height',self.h,0.157,0.448); self.sl(f,8,'roll',self.roll,-0.4,0.4); self.sl(f,9,'pitch',self.pitch,-0.4,0.4)
+        self.info=tk.StringVar(value=''); ttk.Label(f,textvariable=self.info).grid(row=10,column=0,columnspan=4,sticky='w'); self.tick()
+    def sl(self,f,r,n,v,lo,hi):
+        from tkinter import ttk
+        ttk.Label(f,text=n).grid(row=r,column=0,sticky='w'); ttk.Scale(f,from_=lo,to=hi,variable=v,command=lambda _:self.sync()).grid(row=r,column=1,columnspan=3,sticky='ew')
+    def sync(self):
+        with self.d.lk:
+            self.d.mode=self.mode.get(); self.d.vx=float(self.vx.get()); self.d.vy=float(self.vy.get()); self.d.yaw=float(self.yaw.get()); self.d.h=float(self.h.get()); self.d.roll=float(self.roll.get()); self.d.pitch=float(self.pitch.get())
+    def en(self): self.d.enable_all(); self.state.set('Enabled')
+    def dis(self): self.d.disable_all(); self.state.set('Disabled')
+    def es(self,on): self.d.set_estop(on); self.state.set('E-STOP ON' if on else 'E-STOP OFF')
+    def pr(self,on): self.d.set_prone(on)
+    def stp(self):
+        with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0
+        self.vx.set(0.0); self.vy.set(0.0); self.yaw.set(0.0)
+    def tick(self): self.info.set(self.d.status()); self.root.after(150,self.tick)
+    def run(self): self.root.mainloop()
+
+def main():
+    ap=argparse.ArgumentParser()
+    ap.add_argument('--port-can1',default='/dev/can1')
+    ap.add_argument('--port-can2',default='/dev/can2')
+    ap.add_argument('--hz',type=float,default=100.0)
+    ap.add_argument('--no-gui',action='store_true')
+    ap.add_argument('--no-interp',action='store_true',help='Disable trajectory interpolation')
+    ap.add_argument('--interp-method',default='quintic',choices=['linear','cubic','quintic','cosine'],help='Interpolation method')
+    ap.add_argument('--interp-time',type=float,default=0.3,help='Interpolation transition time (seconds)')
+    a=ap.parse_args()
+    d=Deploy(a.port_can1,a.port_can2,a.hz,use_interpolation=not a.no_interp,interp_method=a.interp_method,interp_time=a.interp_time); d.connect(); d.start()
+    try:
+        cli=CLI(d); t=threading.Thread(target=cli.run,daemon=True); t.start()
+        if a.no_gui:
+            while t.is_alive(): time.sleep(0.2)
+        else: GUI(d).run()
+    finally: d.stop()
+
+if __name__=='__main__': main()

05_software/real/ik_real/trajectory_interpolator.py

@@ -0,0 +1,237 @@
+#!/usr/bin/env python3
+"""
+轨迹插值模块 - 用于平滑关节角度过渡，避免突变和冲击
+
+支持多种插值方法：
+- linear: 线性插值
+- cubic: 三次多项式（速度连续）
+- quintic: 五次多项式（速度和加速度连续，最平滑）
+- cosine: 余弦 S 曲线
+
+使用示例：
+    interp = TrajectoryInterpolator(method='quintic', transition_time=0.5)
+    
+    # 设置新目标
+    interp.set_target({'joint1': 1.5, 'joint2': 0.8})
+    
+    # 每个控制周期调用
+    smooth_q = interp.update(dt=0.01, current_q={'joint1': 0.5, 'joint2': 0.3})
+"""
+
+import time
+from typing import Dict, Optional
+import numpy as np
+
+
+class TrajectoryInterpolator:
+    def __init__(self, method: str = 'quintic', transition_time: float = 0.5):
+        """
+        初始化轨迹插值器
+        
+        Args:
+            method: 插值方法 ('linear', 'cubic', 'quintic', 'cosine')
+            transition_time: 过渡时间（秒）
+        """
+        self.method = method
+        self.transition_time = transition_time
+        
+        self.q_start: Dict[str, float] = {}
+        self.q_target: Dict[str, float] = {}
+        self.q_current: Dict[str, float] = {}
+        
+        self.transition_start_time: Optional[float] = None
+        self.is_transitioning = False
+        
+        self._interpolation_funcs = {
+            'linear': self._linear,
+            'cubic': self._cubic,
+            'quintic': self._quintic,
+            'cosine': self._cosine,
+        }
+        
+        if method not in self._interpolation_funcs:
+            raise ValueError(f"Unknown interpolation method: {method}. "
+                           f"Available: {list(self._interpolation_funcs.keys())}")
+    
+    def set_target(self, q_target: Dict[str, float], force_restart: bool = False):
+        """
+        设置新的目标角度，开始新的过渡
+        
+        Args:
+            q_target: 目标关节角度字典 {joint_name: angle}
+            force_restart: 是否强制重新开始过渡（即使已经在过渡中）
+        """
+        if not self.q_current:
+            self.q_current = q_target.copy()
+            self.q_target = q_target.copy()
+            self.q_start = q_target.copy()
+            self.is_transitioning = False
+            return
+        
+        if not force_restart and self.is_transitioning:
+            self.q_target = q_target.copy()
+            return
+        
+        self.q_start = self.q_current.copy()
+        self.q_target = q_target.copy()
+        self.transition_start_time = time.time()
+        self.is_transitioning = True
+    
+    def update(self, dt: float, current_q: Optional[Dict[str, float]] = None) -> Dict[str, float]:
+        """
+        更新插值状态，返回当前应该下发的平滑角度
+        
+        Args:
+            dt: 时间步长（秒）
+            current_q: 可选的当前实际角度（用于初始化或同步）
+        
+        Returns:
+            平滑后的关节角度字典
+        """
+        if current_q is not None and not self.q_current:
+            self.q_current = current_q.copy()
+            self.q_start = current_q.copy()
+            self.q_target = current_q.copy()
+            return self.q_current.copy()
+        
+        if not self.is_transitioning:
+            return self.q_target.copy()
+        
+        elapsed = time.time() - self.transition_start_time
+        
+        if elapsed >= self.transition_time:
+            self.q_current = self.q_target.copy()
+            self.is_transitioning = False
+            return self.q_current.copy()
+        
+        s = elapsed / self.transition_time
+        alpha = self._interpolation_funcs[self.method](s)
+        
+        self.q_current = {}
+        for joint_name in self.q_target:
+            start_val = self.q_start.get(joint_name, 0.0)
+            target_val = self.q_target[joint_name]
+            self.q_current[joint_name] = start_val + (target_val - start_val) * alpha
+        
+        return self.q_current.copy()
+    
+    def reset(self, q_init: Optional[Dict[str, float]] = None):
+        """
+        重置插值器状态
+        
+        Args:
+            q_init: 初始角度，如果为 None 则清空所有状态
+        """
+        if q_init is None:
+            self.q_start = {}
+            self.q_target = {}
+            self.q_current = {}
+        else:
+            self.q_start = q_init.copy()
+            self.q_target = q_init.copy()
+            self.q_current = q_init.copy()
+        
+        self.transition_start_time = None
+        self.is_transitioning = False
+    
+    def is_done(self) -> bool:
+        """返回是否已完成当前过渡"""
+        return not self.is_transitioning
+    
+    def set_transition_time(self, t: float):
+        """动态修改过渡时间"""
+        self.transition_time = max(0.01, t)
+    
+    def set_method(self, method: str):
+        """动态修改插值方法"""
+        if method not in self._interpolation_funcs:
+            raise ValueError(f"Unknown method: {method}")
+        self.method = method
+    
+    @staticmethod
+    def _linear(s: float) -> float:
+        """线性插值：alpha = s"""
+        return np.clip(s, 0.0, 1.0)
+    
+    @staticmethod
+    def _cubic(s: float) -> float:
+        """三次多项式：alpha = 3s² - 2s³"""
+        s = np.clip(s, 0.0, 1.0)
+        return 3.0 * s**2 - 2.0 * s**3
+    
+    @staticmethod
+    def _quintic(s: float) -> float:
+        """五次多项式：alpha = 10s³ - 15s⁴ + 6s⁵"""
+        s = np.clip(s, 0.0, 1.0)
+        return 10.0 * s**3 - 15.0 * s**4 + 6.0 * s**5
+    
+    @staticmethod
+    def _cosine(s: float) -> float:
+        """余弦 S 曲线：alpha = (1 - cos(πs)) / 2"""
+        s = np.clip(s, 0.0, 1.0)
+        return (1.0 - np.cos(np.pi * s)) / 2.0
+
+
+if __name__ == '__main__':
+    import matplotlib.pyplot as plt
+    
+    print("轨迹插值模块测试")
+    print("=" * 60)
+    
+    methods = ['linear', 'cubic', 'quintic', 'cosine']
+    colors = ['blue', 'green', 'red', 'purple']
+    
+    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 5))
+    
+    for method, color in zip(methods, colors):
+        interp = TrajectoryInterpolator(method=method, transition_time=1.0)
+        
+        interp.reset({'joint1': 0.0})
+        interp.set_target({'joint1': 1.0})
+        
+        times = []
+        positions = []
+        velocities = []
+        
+        t = 0.0
+        dt = 0.01
+        last_pos = 0.0
+        
+        while t <= 1.0:
+            q = interp.update(dt)
+            pos = q['joint1']
+            vel = (pos - last_pos) / dt if t > 0 else 0.0
+            
+            times.append(t)
+            positions.append(pos)
+            velocities.append(vel)
+            
+            last_pos = pos
+            t += dt
+        
+        ax1.plot(times, positions, label=method, color=color, linewidth=2)
+        ax2.plot(times, velocities, label=method, color=color, linewidth=2)
+    
+    ax1.set_xlabel('时间 (s)')
+    ax1.set_ylabel('位置 (rad)')
+    ax1.set_title('不同插值方法的位置曲线')
+    ax1.legend()
+    ax1.grid(True, alpha=0.3)
+    
+    ax2.set_xlabel('时间 (s)')
+    ax2.set_ylabel('速度 (rad/s)')
+    ax2.set_title('不同插值方法的速度曲线')
+    ax2.legend()
+    ax2.grid(True, alpha=0.3)
+    
+    plt.tight_layout()
+    plt.savefig('/home/rc2/work/rcwork/trajectory_interpolation_comparison.png', dpi=150)
+    print("已保存对比图到: trajectory_interpolation_comparison.png")
+    
+    print("\n测试完成")
+    print("=" * 60)
+    print("推荐使用:")
+    print("  - quintic: 最平滑，速度和加速度连续")
+    print("  - cosine: 平滑且计算简单")
+    print("  - cubic: 速度连续，比 quintic 稍快")
+    print("  - linear: 最简单但速度会突变")

05_software/real/sim2real/DEPLOYMENT.md

@@ -0,0 +1,50 @@
+# `sim2real` 部署说明
+
+## 模型
+
+当前只使用：
+
+- `sim2real/policies/model_rough.pt`
+
+## 模型契约
+
+- `obs_dim = 53`
+- `action_dim = 16`
+- 单帧输入
+- 无 `base_lin_vel`
+- 无 `height_scan`
+
+## 启动流程
+
+1. 连接硬件
+2. 使能电机
+3. 从当前实测姿态起立
+4. 进入 `stand_balance` 闭环站立
+5. prime 当前观测
+6. 进入 `50Hz` runtime
+
+## 为什么这样改
+
+- 之前版本在 `startup` 后只维持固定 `STAND_POSE`
+- 实机上纯 PD 不足以持续抗姿态扰动
+- 现在增加独立站立闭环，先保证身体支撑，再进入策略
+
+## 运行开关
+
+- `config.yaml > policy.enable_zero_cmd_suppression`
+- `config.yaml > stand_balance.enabled`
+- `config.yaml > policy.hold_zero_command_pose`
+- `config.yaml > policy.command_release_s`
+
+## 纯 Python 命令
+
+默认前提：当前目录就是 `sim2real/`
+
+```bash
+python -m pip install -r requirements-orin.txt
+python tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
+python tools/standalone_check.py
+python main.py --dry-run
+python main.py
+python web/server.py --host 0.0.0.0 --port 8080
+```

05_software/real/sim2real/FACTS_AND_ASSUMPTIONS.md

@@ -0,0 +1,48 @@
+# `FACTS_AND_ASSUMPTIONS`
+
+## 已确认
+
+- 当前部署模型：`sim2real/policies/model_rough.pt`
+- 源模型：`model_2000.pt`
+- actor 输入：`53D`
+- actor 输出：`16D`
+- 当前 actor 不吃 `base_lin_vel`
+- 当前 actor 不吃 `height_scan`
+
+## 当前观测顺序
+
+1. `base_ang_vel * 0.25`
+2. `projected_gravity`
+3. `command`
+4. `joint_pos_rel`（12）
+5. `joint_vel_rel * 0.05`（12）
+6. `wheel_vel * 0.05`（4）
+7. `last_actions`（16）
+
+## 当前控制定义
+
+- 控制频率：`50Hz`
+- 腿缩放：`0.125 / 0.25`
+- 轮缩放：`5.0`
+- 腿 LPF：`5Hz`
+- 轮 LPF：`15Hz`
+
+## 当前仍依赖现场一致的部分
+
+- IMU 安装方向与上一版校正一致
+- 当前 MJCF / 电机参数对应这次重新训练后的模型
+- 电机零位、方向、接线已按当前硬件修正
+
+## 本次实现边界
+
+不再支持：
+
+- `crawl` 模型
+- 多策略切换
+- `318D` 历史输入
+- 旧版 `startup.start_pose`
+
+## 本次排查结论
+
+代码应只围绕当前 rough 模型运行。  
+如果后续模型结构再改，必须重新核对观测、动作缩放、控制频率和部署文档。

05_software/real/sim2real/ORIN_NANO_DEPLOYMENT.md

@@ -0,0 +1,45 @@
+# `Orin Nano` 部署说明
+
+## 是否必须转 ONNX
+
+不必须。
+
+当前优先级仍然是：
+
+1. 先保证观测、动作、站立控制对齐
+2. 再测 `50Hz` 实际环路稳定性
+3. 最后才决定是否转 `ONNX/TensorRT`
+
+## 当前代码重点
+
+- `stand_balance` 已加入 `main.py` 和 `web/session.py`
+- 启动后先站稳，再允许策略接管
+- `PolicyRunner.step()` 仍保留零命令抑制开关，默认开启
+
+## Orin 上先测什么
+
+- 机器人能否在不启动策略时，仅靠 `startup + stand_balance` 稳定站住
+- `loop_dt_ms`
+- `imu_age_ms`
+- 电机 stale
+- policy forward 耗时
+
+## 纯 Python 部署命令
+
+默认前提：当前目录就是 `sim2real/`
+
+```bash
+python3 -m pip install -r requirements-orin.txt
+python3 tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
+python3 tools/standalone_check.py
+python3 main.py --dry-run
+python3 main.py
+python3 web/server.py --host 0.0.0.0 --port 8080
+```
+
+## 首轮实机建议
+
+1. 先不启动策略
+2. 只验证 `startup -> stand_balance`
+3. 站稳后再启动策略
+4. 只给很小的 `vx / vy / yaw`

05_software/real/sim2real/README.md

@@ -0,0 +1,77 @@
+# `sim2real`
+
+当前版本只部署现在这套 `53D -> 16D` 模型，不再兼容旧版 `crawl`、多策略和历史观测。
+
+## 当前部署模型
+
+- 使用文件：`sim2real/policies/model_rough.pt`
+- 来源文件：`model_2000.pt`
+
+## 当前 actor 输入
+
+- 单帧 `53D`
+- 顺序：
+  - `base_ang_vel * 0.25`
+  - `projected_gravity`
+  - `command`
+  - `joint_pos_rel`（12）
+  - `joint_vel_rel * 0.05`（12）
+  - `wheel_vel * 0.05`（4）
+  - `last_actions`（16）
+
+不包含：
+
+- `base_lin_vel`
+- `height_scan`
+
+## 当前控制参数
+
+- 控制频率：`50Hz`
+- 站立保持：`startup/stand_balance.py`
+- `hip_abduction` 缩放：`0.125`
+- 其他腿关节缩放：`0.25`
+- 轮速缩放：`5.0`
+- 腿 LPF：`5Hz`
+- 轮 LPF：`15Hz`
+- 零命令抑制：默认开启，可通过 `config.yaml > policy.enable_zero_cmd_suppression` 关闭
+- 零命令保持：默认开启，可通过 `config.yaml > policy.hold_zero_command_pose` 控制
+- 首次命令解锁：默认开启，可通过 `config.yaml > policy.require_active_command_to_release` 控制
+
+## 当前站立逻辑
+
+- `startup`：从实测姿态过渡到默认站姿
+- `stand_balance`：根据 `IMU roll/pitch + gyro` 动态修正四条腿目标
+- `runtime`：只有站立稳定后才进入策略控制
+
+这次改动的重点是：策略不再承担“先把身体撑住”的职责。
+
+另外当前部署逻辑改成：
+
+- 零命令时默认不让策略直接接管腿和轮，保持站立目标
+- 命令从零变为非零时，策略输出在 `command_release_s` 内平滑放开
+
+## 启动命令
+
+默认前提：当前目录就是 `sim2real/`
+
+纯 `python`：
+
+```bash
+python -m pip install -r requirements-orin.txt
+python tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
+python tools/standalone_check.py
+python main.py --dry-run
+python main.py
+python web/server.py --host 0.0.0.0 --port 8080
+```
+
+Windows 本机：
+
+```bash
+D:\Minicoda3\envs\py10\python.exe -m pip install -r requirements-orin.txt
+D:\Minicoda3\envs\py10\python.exe tools\alignment_check.py --policy policies\model_rough.pt --manifest deployment_manifest.yaml
+D:\Minicoda3\envs\py10\python.exe tools\standalone_check.py
+D:\Minicoda3\envs\py10\python.exe main.py
+D:\Minicoda3\envs\py10\python.exe web\server.py --host 0.0.0.0 --port 8080
+```
+#sim2real/policies/model_rough.pt

05_software/real/sim2real/config.yaml

@@ -0,0 +1,74 @@
+can1_port: "/dev/can1"
+can2_port: "/dev/can2"
+motor_model: "rs-02"
+debug: false
+
+control_freq: 50
+imu_lib_path: null
+
+controller:
+  kp_leg: 80.0
+  kd_leg: 2.5
+  kd_wheel: 2.0
+  max_vx: 0.8
+  max_vy: 0.3
+  max_yaw_rate: 0.5
+
+policy:
+  enable_zero_cmd_suppression: true
+  hold_zero_command_pose: true
+  command_release_s: 0.35
+  require_active_command_to_release: true
+  zero_cmd_use_yaw_rate: false
+  action_scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
+  release_command_hold_s: 0.12
+  release_posture_max_err: 0.35
+  release_target_blend_s: 0.30
+
+stand_balance:
+  enabled: true
+  height: 0.33
+  kp_roll: 0.85
+  kp_pitch: 0.70
+  kd_roll_rate: 0.03
+  kd_pitch_rate: 0.025
+  lateral_lean_gain: 0.0
+  hip_abduction_clip: 0.45
+  hip_pitch_clip: [-1.0, 2.5]
+  knee_clip: [-2.6, -0.3]
+  stable_roll_deg: 6.0
+  stable_pitch_deg: 8.0
+  stable_gyro_deg_s: 45.0
+  enter_hold_s: 1.0
+  profile_h: [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
+  profile_hip: [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
+  profile_knee: [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
+
+startup:
+  enabled: true
+  wait_for_enter_before_rise: false
+  soft_hold_duration: 1.0
+  ramp_kp_time: 1.0
+  transition_time_min: 2.0
+  transition_time_max: 6.0
+  transition_seconds_per_rad: 1.5
+  timeout_extra: 3.0
+  hold_time: 1.0
+  settle_pos_threshold: 0.30
+  settle_vel_threshold: 0.6
+  progress_log_interval: 0.5
+  max_dev_warn: 1.5
+  max_dev_abort: 3.0
+  require_user_confirm: true
+
+safety:
+  enabled: true
+  max_target_offset: 0.6
+  max_ang_vel: 10.0
+  max_tilt_z: -0.3
+  clip_to_brake: 3
+  imu_age_warn_ms: 60.0
+  imu_age_stop_ms: 200.0
+
+log_dir: "logs"
+log_every: 1

05_software/real/sim2real/deployment_manifest.yaml

@@ -0,0 +1,71 @@
+model:
+  path: "/home/rc2/work/rcwork/real/rc_mjlab/rc_mjlab/model_rough.pt"
+  obs_dim: 53
+  action_dim: 16
+  enable_zero_cmd_suppression: true
+
+observation:
+  terms:
+    - name: base_ang_vel
+      dim: 3
+      scale: 0.25
+    - name: projected_gravity
+      dim: 3
+    - name: command
+      dim: 3
+    - name: joint_pos_rel
+      dim: 12
+    - name: joint_vel_rel
+      dim: 12
+      scale: 0.05
+    - name: wheel_vel
+      dim: 4
+      scale: 0.05
+    - name: last_actions
+      dim: 16
+
+action:
+  scale:
+    - 0.125
+    - 0.25
+    - 0.25
+    - 0.125
+    - 0.25
+    - 0.25
+    - 0.125
+    - 0.25
+    - 0.25
+    - 0.125
+    - 0.25
+    - 0.25
+    - 5.0
+    - 5.0
+    - 5.0
+    - 5.0
+  default_dof_pos:
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+
+control:
+  control_freq_hz: 50
+  leg_lpf_hz: 5
+  wheel_lpf_hz: 15
+
+safety:
+  zero_cmd_lin_thresh: 0.05
+  zero_cmd_yaw_thresh: 0.05
+  zero_yaw_rate_thresh: 0.10

05_software/real/sim2real/input_dev/keyboard.py

@@ -0,0 +1,89 @@
+"""键盘控制器 — 兼容 sim2sim/input_dev/keyboard.py 的接口与平滑参数。"""
+import numpy as np
+
+try:
+    from pynput import keyboard
+    PYNPUT_AVAILABLE = True
+except ImportError:
+    PYNPUT_AVAILABLE = False
+    keyboard = None  # type: ignore
+
+
+class KeyboardCommandController:
+    """方向键 + AD 键的键盘指令源。
+    指令: [vx, vy, yaw_rate]，平滑加减速；空格触发急停标志。
+    """
+
+    def __init__(self,
+                 max_x_vel: float = 0.8,
+                 max_y_vel: float = 0.3,
+                 max_yaw_vel: float = 0.5,
+                 acc_step: float = 0.05,
+                 dec_step: float = 0.1):
+        if not PYNPUT_AVAILABLE:
+            raise RuntimeError("pynput 不可用，无法使用键盘控制；改用其他输入源。")
+
+        self.current_cmd = np.zeros(3, dtype=np.float32)
+        self.max_x_vel = max_x_vel
+        self.max_y_vel = max_y_vel
+        self.max_yaw_vel = max_yaw_vel
+        self.acc_step = acc_step
+        self.dec_step = dec_step
+
+        self._pressed = set()
+        self._estop = False
+        self.listener = keyboard.Listener(
+            on_press=self._on_press, on_release=self._on_release
+        )
+
+    def start(self):
+        self.listener.start()
+        print("[Keyboard] 启动。↑↓ 前后, ←→ 转向, A/D 横移, SPACE 急停")
+
+    def stop(self):
+        try:
+            self.listener.stop()
+        except Exception:
+            pass
+
+    def _on_press(self, key):
+        self._pressed.add(key)
+        if key == keyboard.Key.space:
+            self._estop = True
+
+    def _on_release(self, key):
+        self._pressed.discard(key)
+
+    def is_estop_triggered(self) -> bool:
+        return self._estop
+
+    def reset_estop(self):
+        self._estop = False
+
+    def get_command(self) -> np.ndarray:
+        target = np.zeros(3, dtype=np.float32)
+        if keyboard.Key.up in self._pressed:
+            target[0] += self.max_x_vel
+        if keyboard.Key.down in self._pressed:
+            target[0] -= self.max_x_vel
+        if keyboard.Key.left in self._pressed:
+            target[2] += self.max_yaw_vel
+        if keyboard.Key.right in self._pressed:
+            target[2] -= self.max_yaw_vel
+        try:
+            if keyboard.KeyCode.from_char('a') in self._pressed:
+                target[1] += self.max_y_vel
+            if keyboard.KeyCode.from_char('d') in self._pressed:
+                target[1] -= self.max_y_vel
+        except Exception:
+            pass
+
+        for i, max_v in enumerate((self.max_x_vel, self.max_y_vel, self.max_yaw_vel)):
+            step = self.acc_step if target[i] != 0 else self.dec_step
+            if i == 2:
+                step *= 2.0
+            if self.current_cmd[i] < target[i]:
+                self.current_cmd[i] = min(self.current_cmd[i] + step, target[i])
+            else:
+                self.current_cmd[i] = max(self.current_cmd[i] - step, target[i])
+        return self.current_cmd.copy()

05_software/real/sim2real/interface/imu_client.py

@@ -0,0 +1,136 @@
+"""Odin1 IMU 客户端封装。
+
+核心改动相对 sim_rl/odin1/python/odin1_imu.py：
+- 自动加载默认 .so 路径，调用方只需要 IMUClient(lib_path=...)
+- 启动后做一次"重力对齐" — 用静止时的加速度计读数初始化 Mahony 滤波器，
+  把首步姿态偏差从可能的 5°+ 降到 0.3° 内。这是方法论 D4 的关键一步。
+- 数据老化检测：若 imu_age_ms > stale_threshold 则报警（不阻塞）。
+"""
+import sys
+import time
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+
+
+class IMUClient:
+    """Odin1 IMU 包装。
+
+    Args:
+        lib_path: libodin1_imu_bridge.so 的绝对路径；None 则按方法论 1.2 中
+                  约定的相对位置寻找。
+        gravity_align_samples: 启动时取多少帧加速度计平均值用于姿态初始化
+        stale_threshold_ms: 单帧数据超过该 age 视为陈旧
+    """
+
+    def __init__(self, lib_path: Optional[str] = None, gravity_align_samples: int = 50,
+                 stale_threshold_ms: float = 50.0):
+        # 优先级 1: vendored/odin1_imu（独立部署模式）
+        # 优先级 2: ../../odin1/odin1/python（开发模式，即 sim_rl/odin1/odin1/python）
+        sim2real_root = Path(__file__).resolve().parents[1]
+        candidates = [
+            sim2real_root / "vendored" / "odin1_imu",
+            sim2real_root.parents[1] / "odin1" / "odin1" / "python",
+        ]
+        for cand in candidates:
+            if cand.exists() and str(cand) not in sys.path:
+                sys.path.insert(0, str(cand))
+                break
+        try:
+            from odin1_imu import Odin1ImuClient  # type: ignore
+        except ImportError as e:
+            raise ImportError(
+                f"无法导入 Odin1ImuClient，已尝试的路径: {[str(c) for c in candidates]}: {e}"
+            )
+
+        # lib_path 默认查找：vendored/odin1_imu/build/libodin1_imu_bridge.so → 开发路径
+        if lib_path is None:
+            so_candidates = [
+                sim2real_root / "vendored" / "odin1_imu" / "build" / "libodin1_imu_bridge.so",
+                sim2real_root / "vendored" / "odin1_imu" / "libodin1_imu_bridge.so",
+                sim2real_root.parents[1] / "odin1" / "odin1" / "build" / "libodin1_imu_bridge.so",
+            ]
+            for so in so_candidates:
+                if so.exists():
+                    lib_path = str(so)
+                    break
+
+        self._client = Odin1ImuClient(lib_path=lib_path)
+        self._gravity_align_samples = gravity_align_samples
+        self._stale_threshold_ms = stale_threshold_ms
+        self._initial_gravity: Optional[np.ndarray] = None
+        # 用本机时钟追踪数据新鲜度（stamp_ns 是设备单调时钟，不能和 time.time 混算）
+        self._last_seq: int = -1
+        self._last_fresh_time: float = 0.0
+
+    def version(self) -> str:
+        return self._client.version()
+
+    def start(self, timeout_ms: int = 8000):
+        """启动 IMU 流，并采集若干帧用于重力对齐。"""
+        self._client.start(timeout_ms=timeout_ms)
+        self._wait_for_stream()
+        self._initial_gravity = self._collect_gravity_samples()
+        self._last_fresh_time = time.time()
+
+    def stop(self):
+        try:
+            self._client.stop()
+        except Exception:
+            pass
+
+    @property
+    def initial_gravity(self) -> Optional[np.ndarray]:
+        """启动后的初始重力向量（机身坐标系），用于初始化 Mahony 四元数。"""
+        return self._initial_gravity
+
+    def get_latest(self):
+        """返回 (gyro[3], accel[3], age_ms, fresh)；fresh=False 表示无新数据。"""
+        sample = self._client.get_latest()
+        if sample is None:
+            return (np.zeros(3, dtype=np.float32),
+                    np.array([0.0, 0.0, 9.81], dtype=np.float32),
+                    -1.0, False)
+        gyro = np.array([sample.gyro_x, sample.gyro_y, sample.gyro_z], dtype=np.float32)
+        accel = np.array([sample.accel_x, sample.accel_y, sample.accel_z], dtype=np.float32)
+        # 用 stamp_ns 判断是否有新数据，因为 sequence 字段在 C++ 中可能没有赋值，导致永远为 0
+        stamp = getattr(sample, "stamp_ns", 0)
+        now = time.time()
+        if stamp != self._last_seq:
+            self._last_seq = stamp
+            self._last_fresh_time = now
+            fresh = True
+        else:
+            fresh = False
+        age_ms = (now - self._last_fresh_time) * 1000.0
+        return gyro, accel, age_ms, fresh
+
+    # ---- 内部方法 ----
+    def _wait_for_stream(self, timeout: float = 3.0):
+        deadline = time.time() + timeout
+        while time.time() < deadline:
+            if self._client.wait_for_data(timeout_ms=200):
+                # 有数据进来后清空一次队列以保证后续 get_latest 拿到的都是最新
+                while self._client.pop_sample() is not None:
+                    pass
+                return
+        raise RuntimeError("IMU 启动超时，未收到任何样本")
+
+    def _collect_gravity_samples(self) -> np.ndarray:
+        accels = []
+        for _ in range(self._gravity_align_samples):
+            sample = self._client.pop_sample()
+            if sample is None:
+                if not self._client.wait_for_data(timeout_ms=100):
+                    continue
+                sample = self._client.pop_sample()
+                if sample is None:
+                    continue
+            accels.append([sample.accel_x, sample.accel_y, sample.accel_z])
+        if not accels:
+            print("[IMU] 警告: 重力对齐期间未收到样本，使用默认重力 [0,0,-9.81]")
+            return np.array([0.0, 0.0, -9.81], dtype=np.float32)
+        gravity = np.mean(accels, axis=0).astype(np.float32)
+        print(f"[IMU] 重力对齐完成: g_body = {gravity}")
+        return gravity

05_software/real/sim2real/interface/motor_driver.py

@@ -0,0 +1,310 @@
+"""RobStride 电机驱动包装。
+
+职责：
+- 封装 ik_real 中 RobStrideDriver 的 enable/disable/clear/control_mit 调用
+- **真实的丢包检测**：旧版用「value=0 启发式」会误判（电机回机械零位时也是 0）。
+  新方案：
+    1. 调用 process_messages 前快照所有电机的 (pos, vel, torque)
+    2. 调用后比较：状态变了 → 这一帧有新反馈；状态完全没变 → 累计 stale_count
+    3. stale_count 超过阈值才沿用上一帧（方法论 3.4.2）
+  仍然不完美（电机长时间静止确实会有连续多帧 state 不变），但比 0 启发式可靠。
+- 通过 driver_factory 由调用方注入：远程 Linux 主机用 RobStrideDriver，
+  本地 Windows 调试可用 Mock。
+"""
+from dataclasses import dataclass
+import threading
+from typing import Callable, Dict, List, Optional, Tuple
+
+import numpy as np
+
+from interface.motor_mapping import MotorMapping
+
+
+@dataclass
+class MotorReading:
+    position: float
+    velocity: float
+    torque: float = 0.0
+    fresh: bool = False  # True 表示本帧驱动板有新反馈
+
+
+class HardwareIO:
+    """统一的电机+IMU总线接口（不含策略），主控调用这一层。
+
+    Args:
+        driver_factory: () -> (drv1, drv2)，由调用方注入；返回的对象需要满足：
+            connect()/disconnect()/disable(name)/enable(name)/clear_warnings(name)
+            add_motor(name, mid, model)/process_messages()
+            control_mit(name, q, dq, kp, kd, tau)
+            .motors: dict[name -> motor], motor.state.position / .velocity / .torque
+        config: yaml 解析后的字典
+    """
+
+    def __init__(self, driver_factory: Callable[[str, str, bool], Tuple[object, object]],
+                 motor_model: str, can1_port: str, can2_port: str, debug: bool = False,
+                 stale_frames_to_holdover: int = 2):
+        self.mapper = MotorMapping()
+        drv1, drv2 = driver_factory(can1_port, can2_port, debug)
+        self.driver_can1 = drv1
+        self.driver_can2 = drv2
+        self.motor_model = motor_model
+        self.stale_frames_to_holdover = stale_frames_to_holdover
+
+        # 上一帧反馈（按 (bus, can_id) 索引），用于丢包兜底
+        self._last_pos: Dict[Tuple[int, int], float] = {}
+        self._last_vel: Dict[Tuple[int, int], float] = {}
+        self._last_torque: Dict[Tuple[int, int], float] = {}
+        # 每个电机连续多少帧没收到新反馈
+        self._stale_counts: Dict[Tuple[int, int], int] = {}
+        # 第一次必须读到才能解锁，避免初始化时直接用零位发送大力矩
+        self._initialized = False
+
+        self.lock = threading.Lock()
+
+        # 累计诊断
+        self.holdover_total = 0  # 累计被沿用上一帧的次数
+
+    # ---- 总线管理 ----
+    def connect(self):
+        self.driver_can1.connect()
+        self.driver_can2.connect()
+        for jk in self.mapper.SIM_JOINT_ORDER:
+            leg, joint = jk
+            bus, mid = self.mapper.CAN_ID_MAP[jk]
+            name = f"{leg}_{joint}"
+            drv = self.driver_can1 if bus == 1 else self.driver_can2
+            drv.add_motor(name, mid, self.motor_model)
+            self._stale_counts[(bus, mid)] = 0
+
+    def disconnect(self):
+        try:
+            self.driver_can1.disconnect()
+        finally:
+            self.driver_can2.disconnect()
+
+    def enable_all(self):
+        for drv in (self.driver_can1, self.driver_can2):
+            for name in drv.motors:
+                drv.clear_warnings(name)
+                drv.enable(name)
+
+    def disable_all(self):
+        for drv in (self.driver_can1, self.driver_can2):
+            for name in drv.motors:
+                drv.disable(name)
+
+    # ---- 状态读取 ----
+    def _snapshot_state(self) -> Dict[Tuple[int, int], Tuple[float, float, float, int]]:
+        """快照所有电机的 (pos, vel, torque, update_count)，process_messages 前后比较即可判 fresh。"""
+        snap: Dict[Tuple[int, int], Tuple[float, float, float, int]] = {}
+        for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)):
+            bus = drv_idx + 1
+            for name, motor in drv.motors.items():
+                parts = name.split("_", 1)
+                if len(parts) != 2:
+                    continue
+                key = (parts[0], parts[1])
+                if key not in self.mapper.CAN_ID_MAP:
+                    continue
+                _, mid = self.mapper.CAN_ID_MAP[key]
+                s = motor.state
+                snap[(bus, mid)] = (s.position, s.velocity, s.torque, getattr(s, "update_count", 0))
+        return snap
+
+    def read_state(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, Dict[str, object]]:
+        """返回 (sim_joint_pos[16], sim_joint_vel[16], sim_joint_torque[16], debug_info)。"""
+        with self.lock:
+            # 1) 抓取上一次的状态作为「pre」快照（基线）
+            pre = self._snapshot_state()
+
+            # 2) 拉取本帧反馈
+            self.driver_can1.process_messages()
+            self.driver_can2.process_messages()
+
+            # 3) 抓取「post」快照
+            post = self._snapshot_state()
+
+            # 4) 比较：state 元组变了 → 本帧有新反馈，stale_count 清零；否则 stale_count++
+            per_motor_fresh: Dict[Tuple[int, int], bool] = {}
+            for key in post:
+                fresh = (pre.get(key) != post[key])
+                per_motor_fresh[key] = fresh
+                if fresh:
+                    self._stale_counts[key] = 0
+                else:
+                    self._stale_counts[key] += 1
+
+            # 5) 取出本帧 pos/vel；若该电机连续多帧没刷新，沿用上一帧（方法论 3.4.2）
+            real_pos: Dict[Tuple[int, int], float] = {}
+            real_vel: Dict[Tuple[int, int], float] = {}
+            real_torque: Dict[Tuple[int, int], float] = {}
+            holdover_this_frame = 0
+            for key, (pos, vel, tor, _) in post.items():
+                if (not per_motor_fresh[key]) and self._stale_counts[key] >= self.stale_frames_to_holdover:
+                    # 长时间不刷新视作丢包：沿用上一帧
+                    if key in self._last_pos:
+                        real_pos[key] = self._last_pos[key]
+                        real_vel[key] = self._last_vel[key]
+                        real_torque[key] = self._last_torque[key]
+                        holdover_this_frame += 1
+                    else:
+                        real_pos[key] = pos
+                        real_vel[key] = vel
+                        real_torque[key] = tor
+                else:
+                    real_pos[key] = pos
+                    real_vel[key] = vel
+                    real_torque[key] = tor
+
+            self.holdover_total += holdover_this_frame
+            # 缓存本帧（即便部分是 holdover 也缓存）
+            self._last_pos = real_pos.copy()
+            self._last_vel = real_vel.copy()
+            self._last_torque = real_torque.copy()
+            if not self._initialized:
+                self._initialized = True
+
+            cur_pos = self.mapper.real_to_sim(real_pos)
+            cur_vel = self.mapper.real_vel_to_sim(real_vel)
+            cur_torque = self.mapper.real_vel_to_sim(real_torque)
+
+            # 诊断信息
+            stale_max = max(self._stale_counts.values()) if self._stale_counts else 0
+            n_stale_motors = sum(1 for c in self._stale_counts.values()
+                                 if c >= self.stale_frames_to_holdover)
+            # 按 SIM_JOINT_ORDER 排列的每个电机连续丢帧数
+            per_motor_stale = [
+                self._stale_counts.get(self.mapper.CAN_ID_MAP[jk], 99)
+                for jk in self.mapper.SIM_JOINT_ORDER
+            ]
+            return cur_pos, cur_vel, cur_torque, {
+                "holdover_this_frame": holdover_this_frame,
+                "stale_max": stale_max,
+                "n_stale_motors": n_stale_motors,
+                "fresh_count": sum(1 for v in per_motor_fresh.values() if v),
+                "per_motor_stale": per_motor_stale,
+            }
+        
+    def passive_poll(self):
+        """发送全 0 (0刚度0阻尼0力矩) 的 MIT 指令给所有电机。
+        目的：在 ENABLED 状态下，不产生力矩地索要反馈（因为 RobStride 在 MIT 模式下必须有指令才反馈）。"""
+        with self.lock:
+            for jk in self.mapper.SIM_JOINT_ORDER:
+                bus, mid = self.mapper.CAN_ID_MAP[jk]
+                name = f"{jk[0]}_{jk[1]}"
+                drv = self.driver_can1 if bus == 1 else self.driver_can2
+                if name in drv.motors:
+                    drv.control_mit(name, 0.0, 0.0, 0.0, 0.0, 0.0)
+
+    # ---- 控制下发 ----
+    def send_control(self, target_angles: np.ndarray, kp_leg: float, kd_leg: float,
+                     kd_wheel: float):
+        """与 sim2sim 的 PD 模型对齐：
+        - 腿: position 控制，目标角度由 target_angles[:12] 给出，kp/kd 来自配置
+        - 轮: velocity 控制，目标速度由 target_angles[12:] 给出，kd 阻尼
+        """
+        with self.lock:
+            if target_angles.shape != (16,):
+                raise ValueError("target_angles must be (16,)")
+
+            real_targets = self.mapper.sim_to_real(target_angles.astype(np.float32))
+            
+            # 轮毂速度目标暂且用 0，如果 target_angles 里包含了速度，就在 policy 那里处理，
+            # 这里的 target_angles 是 pose 目标，轮毂作为连续旋转关节其实位置控制没有意义。
+            # 为了兼容旧代码，这里构造一个 16 维的 velocity array，只有后 4 个是目标（如果当作速度的话）。
+            vel_targets = np.zeros(16, dtype=np.float32)
+            vel_targets[12:] = target_angles[12:].astype(np.float32)
+            real_wheel = self.mapper.sim_vel_to_real(vel_targets)
+
+            for jk in self.mapper.SIM_JOINT_ORDER:
+                leg, joint = jk
+                bus, mid = self.mapper.CAN_ID_MAP[jk]
+                name = f"{leg}_{joint}"
+                drv = self.driver_can1 if bus == 1 else self.driver_can2
+                if name not in drv.motors:
+                    continue
+
+                if joint == "wheel":
+                    v = real_wheel[(bus, mid)]
+                    drv.control_mit(name, 0.0, v, 0.0, kd_wheel, 0.0)
+                else:
+                    q = real_targets[(bus, mid)]
+                    drv.control_mit(name, q, 0.0, kp_leg, kd_leg, 0.0)
+
+    def damping_brake(self, kd_leg: float, kd_wheel: float):
+        """急停模式：所有关节卸载刚度，仅保留阻尼。
+        对应 270_SimToReal 方法论 97.11 Level 2 "刹车"。
+        """
+        with self.lock:
+            for jk in self.mapper.SIM_JOINT_ORDER:
+                leg, joint = jk
+                bus, _ = self.mapper.CAN_ID_MAP[jk]
+                name = f"{leg}_{joint}"
+                drv = self.driver_can1 if bus == 1 else self.driver_can2
+                if name not in drv.motors:
+                    continue
+                kd = kd_wheel if joint == "wheel" else kd_leg
+                drv.control_mit(name, 0.0, 0.0, 0.0, kd, 0.0)
+
+    def wait_feedback_ready(self, max_attempts: int = 20,
+                            poll_interval: float = 0.05) -> Tuple[bool, list]:
+        """enable 后调用：尝试 max_attempts 次读总线，等所有 16 个电机
+        都至少给出一帧反馈。
+        返回 (all_ready, missing_motors)；missing_motors 是 (bus, mid, name) 列表。
+        """
+        import time
+        seen: Dict[Tuple[int, int], bool] = {
+            self.mapper.CAN_ID_MAP[jk]: False for jk in self.mapper.SIM_JOINT_ORDER
+        }
+        # 用第一次读到的 (pos, vel, torque) 三元组的"非零"或"已变化"作为反馈到达的判据。
+        # 启动瞬间所有 motor.state 默认全 0，要么收到反馈让其变化，要么收到反馈但值确实是 0。
+        # 退化情况下电机静止时 vel=0 且 pos=机械零位也=0，那种情况只能等多帧确认。
+        snap_prev = self._snapshot_state()
+        for attempt in range(max_attempts):
+            with self.lock:
+                self.driver_can1.process_messages()
+                self.driver_can2.process_messages()
+                snap_cur = self._snapshot_state()
+            for key, fields_cur in snap_cur.items():
+                if seen[key]:
+                    continue
+                fields_prev = snap_prev.get(key)
+                # 任一字段不为 0 → 一定有反馈（因为初始值都是 0）
+                if any(v != 0.0 for v in fields_cur):
+                    seen[key] = True
+                # 与上一次快照不同 → 一定有反馈（即便都很小）
+                elif fields_prev is not None and fields_cur != fields_prev:
+                    seen[key] = True
+            snap_prev = snap_cur
+            if all(seen.values()):
+                return True, []
+            time.sleep(poll_interval)
+
+        # 超时：列出仍未反馈的电机
+        missing = []
+        rev_can = {v: k for k, v in self.mapper.CAN_ID_MAP.items()}
+        for key, ok in seen.items():
+            if not ok:
+                leg, joint = rev_can[key]
+                missing.append((key[0], key[1], f"{leg}_{joint}"))
+        return False, missing
+
+    def read_measured_pose(self) -> np.ndarray:
+        """返回 (16,) 当前实测 sim 坐标系下的关节位置。
+        会先 process_messages 一次保证拿到本帧。
+        """
+        self.driver_can1.process_messages()
+        self.driver_can2.process_messages()
+        real_pos: Dict[Tuple[int, int], float] = {}
+        for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)):
+            bus = drv_idx + 1
+            for name, motor in drv.motors.items():
+                parts = name.split("_", 1)
+                if len(parts) != 2:
+                    continue
+                key = (parts[0], parts[1])
+                if key not in self.mapper.CAN_ID_MAP:
+                    continue
+                _, mid = self.mapper.CAN_ID_MAP[key]
+                real_pos[(bus, mid)] = motor.state.position
+        return self.mapper.real_to_sim(real_pos)

05_software/real/sim2real/interface/motor_mapping.py

@@ -0,0 +1,99 @@
+"""仿真→实机电机映射。
+
+数据来源：sim_rl/ik_real/sim_to_real_deploy_beifen.py 和
+sim_rl/sim2real/motor_mapping.py 中的 sign / offset / can_id 表（已在实机上验证）。
+关节顺序与 rc_mjlab/sim2sim 完全一致：[12 个腿关节] + [4 个轮子]。
+"""
+from typing import Dict, Tuple
+
+import numpy as np
+
+
+class MotorMapping:
+    LEG_NAMES = ("fl", "fr", "rl", "rr")
+    JOINT_NAMES = ("hip_abduction", "hip_pitch", "knee", "wheel")
+
+    SIM_JOINT_ORDER = (
+        ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"),
+        ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"),
+        ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"),
+        ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"),
+        ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"),
+    )
+    SIM_INDEX_MAP = {jk: i for i, jk in enumerate(SIM_JOINT_ORDER)}
+
+    CAN_ID_MAP: Dict[Tuple[str, str], Tuple[int, int]] = {
+        ("fl", "hip_abduction"): (1, 1), ("fl", "hip_pitch"): (1, 2),
+        ("fl", "knee"): (1, 3),          ("fl", "wheel"): (1, 4),
+        ("fr", "hip_abduction"): (1, 5), ("fr", "hip_pitch"): (1, 6),
+        ("fr", "knee"): (1, 7),          ("fr", "wheel"): (1, 8),
+        ("rl", "hip_abduction"): (2, 1), ("rl", "hip_pitch"): (2, 2),
+        ("rl", "knee"): (2, 3),          ("rl", "wheel"): (2, 4),
+        ("rr", "hip_abduction"): (2, 5), ("rr", "hip_pitch"): (2, 6),
+        ("rr", "knee"): (2, 7),          ("rr", "wheel"): (2, 8),
+    }
+
+    DIRECTION_MAP: Dict[Tuple[str, str], int] = {
+        ("fl", "hip_abduction"): -1, ("fl", "hip_pitch"): -1,
+        ("fl", "knee"): -1,          ("fl", "wheel"): -1,
+        ("fr", "hip_abduction"): -1, ("fr", "hip_pitch"):  1,
+        ("fr", "knee"):  1,          ("fr", "wheel"):  1,
+        ("rl", "hip_abduction"):  1, ("rl", "hip_pitch"): -1,
+        ("rl", "knee"): -1,          ("rl", "wheel"): -1,
+        ("rr", "hip_abduction"):  1, ("rr", "hip_pitch"):  1,
+        ("rr", "knee"):  1,          ("rr", "wheel"):  1,
+    }
+
+    ZERO_OFFSET_MAP: Dict[Tuple[str, str], float] = {
+        ("fl", "hip_abduction"):  0.003, ("fl", "hip_pitch"):  0.030,
+        ("fl", "knee"):           0.028, ("fl", "wheel"):      0.000,
+        ("fr", "hip_abduction"):  0.004, ("fr", "hip_pitch"):  0.038,
+        ("fr", "knee"):           0.011, ("fr", "wheel"):      0.000,
+        ("rl", "hip_abduction"):  0.019, ("rl", "hip_pitch"): -0.034,
+        ("rl", "knee"):           0.025, ("rl", "wheel"):      0.000,
+        ("rr", "hip_abduction"): -0.001, ("rr", "hip_pitch"):  0.039,
+        ("rr", "knee"):           0.018, ("rr", "wheel"):      0.000,
+    }
+
+    def __init__(self):
+        self.num_motors = len(self.SIM_JOINT_ORDER)
+        self._sign = np.array([self.DIRECTION_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32)
+        self._offset = np.array([self.ZERO_OFFSET_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32)
+
+    def sim_to_real(self, sim_angles: np.ndarray) -> Dict[Tuple[int, int], float]:
+        if len(sim_angles) != 16:
+            raise ValueError(f"expected 16 sim angles, got {len(sim_angles)}")
+        out: Dict[Tuple[int, int], float] = {}
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            real = float(self._sign[i] * sim_angles[i] + self._offset[i])
+            out[self.CAN_ID_MAP[jk]] = real
+        return out
+
+    def sim_vel_to_real(self, sim_vels: np.ndarray) -> Dict[Tuple[int, int], float]:
+        # 速度只受方向影响，不应用 offset。
+        out: Dict[Tuple[int, int], float] = {}
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            out[self.CAN_ID_MAP[jk]] = float(self._sign[i] * sim_vels[i])
+        return out
+
+    def real_to_sim(self, real_pos: Dict[Tuple[int, int], float]) -> np.ndarray:
+        out = np.zeros(16, dtype=np.float32)
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            v = real_pos.get(self.CAN_ID_MAP[jk])
+            if v is None:
+                continue
+            out[i] = (v - self._offset[i]) / self._sign[i]
+        return out
+
+    def real_vel_to_sim(self, real_vel: Dict[Tuple[int, int], float]) -> np.ndarray:
+        out = np.zeros(16, dtype=np.float32)
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            v = real_vel.get(self.CAN_ID_MAP[jk])
+            if v is None:
+                continue
+            out[i] = v / self._sign[i]
+        return out
+
+    def joint_name_at(self, idx: int) -> str:
+        leg, joint = self.SIM_JOINT_ORDER[idx]
+        return f"{leg}_{joint}_joint"

05_software/real/sim2real/interface/real_io.py

@@ -0,0 +1,135 @@
+import time
+from typing import Callable, Dict, Tuple
+
+import numpy as np
+
+from interface.imu_client import IMUClient
+from interface.motor_driver import HardwareIO
+from tools.math_utils import LowPassFilter, MahonyFilter, get_gravity_orientation
+
+
+class RealIO:
+    def __init__(
+        self,
+        driver_factory: Callable[[str, str, bool], Tuple[object, object]],
+        motor_model: str,
+        can1_port: str,
+        can2_port: str,
+        imu_lib_path: str,
+        control_dt: float = 0.02,
+        kp_leg: float = 80.0,
+        kd_leg: float = 2.5,
+        kd_wheel: float = 2.0,
+        debug: bool = False,
+    ):
+        self.control_dt = control_dt
+        self.kp_leg = kp_leg
+        self.kd_leg = kd_leg
+        self.kd_wheel = kd_wheel
+
+        print("[RealIO] 初始化电机驱动...")
+        self.hw = HardwareIO(driver_factory, motor_model, can1_port, can2_port, debug)
+        print("[RealIO] 初始化 IMU...")
+        self.imu = IMUClient(lib_path=imu_lib_path)
+
+        self.imu_filter = MahonyFilter(kp=2.0, ki=0.0, dt=control_dt)
+        self.quat_wxyz = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
+
+        self.lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=control_dt, dim=12)
+        self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=control_dt, dim=4)
+
+        self._last_imu_age_ms = -1.0
+        self._last_imu_fresh = False
+
+    def connect(self, imu_timeout_ms: int = 8000):
+        self.hw.connect()
+        self.imu.start(timeout_ms=imu_timeout_ms)
+        if self.imu.initial_gravity is not None:
+            self.imu_filter.reset_with_accel(self.imu.initial_gravity)
+            self.quat_wxyz = self.imu_filter.q.copy()
+
+    def disconnect(self):
+        try:
+            self.hw.disable_all()
+        finally:
+            self.imu.stop()
+            self.hw.disconnect()
+
+    def enable_motors(self):
+        self.hw.enable_all()
+
+    def disable_motors(self):
+        self.hw.disable_all()
+
+    def damping_brake(self):
+        self.hw.damping_brake(self.kd_leg, self.kd_wheel)
+
+    def wait_feedback_ready(self, max_attempts: int = 20, poll_interval: float = 0.05):
+        return self.hw.wait_feedback_ready(max_attempts=max_attempts, poll_interval=poll_interval)
+
+    def read_measured_pose(self) -> np.ndarray:
+        return self.hw.read_measured_pose()
+
+    def read_state(self) -> Dict[str, object]:
+        joint_pos, joint_vel, joint_torque, motor_diag = self.hw.read_state()
+        gyro, accel, age_ms, fresh = self.imu.get_latest()
+        self._last_imu_age_ms = age_ms
+        self._last_imu_fresh = fresh
+        self.quat_wxyz = self.imu_filter.update(accel, gyro)
+        projected_gravity = get_gravity_orientation(self.quat_wxyz)
+
+        return {
+            "joint_pos": joint_pos,
+            "joint_vel": joint_vel,
+            "joint_torque": joint_torque,
+            "imu_gyro": gyro,
+            "imu_accel": accel,
+            "quat_wxyz": self.quat_wxyz.copy(),
+            "projected_gravity": projected_gravity,
+            "imu_age_ms": age_ms,
+            "imu_fresh": fresh,
+            "motor_stale": motor_diag,
+        }
+
+    def get_obs_policy(
+        self,
+        state: Dict[str, object],
+        command: np.ndarray,
+        default_dof_pos: np.ndarray,
+        last_actions_raw: np.ndarray,
+    ) -> np.ndarray:
+        gyro = state["imu_gyro"]
+        joint_pos = state["joint_pos"]
+        joint_vel = state["joint_vel"]
+        projected_gravity = state["projected_gravity"]
+
+        base_ang_vel = (gyro * 0.25).astype(np.float32)
+        joint_pos_rel = (joint_pos[:12] - default_dof_pos[:12]).astype(np.float32)
+        joint_vel_leg = (joint_vel[:12] * 0.05).astype(np.float32)
+        wheel_vel = (joint_vel[12:] * 0.05).astype(np.float32)
+
+        return np.concatenate(
+            [
+                base_ang_vel,
+                projected_gravity,
+                command.astype(np.float32),
+                joint_pos_rel,
+                joint_vel_leg,
+                wheel_vel,
+                last_actions_raw,
+            ]
+        ).astype(np.float32)
+
+    def send_actions(self, scaled_actions: np.ndarray, default_dof_pos: np.ndarray):
+        act = (scaled_actions + default_dof_pos).astype(np.float32)
+        act = np.clip(act, -100.0, 100.0)
+        act[:12] = self.lpf_legs.filter(act[:12])
+        act[12:] = self.lpf_wheels.filter(act[12:])
+        self.hw.send_control(act, self.kp_leg, self.kd_leg, self.kd_wheel)
+        return act
+
+    def hold_pose(self, sim_target_pose: np.ndarray, kp_scale: float = 1.0):
+        target = np.clip(sim_target_pose.astype(np.float32), -100.0, 100.0)
+        kp_scale = float(np.clip(kp_scale, 0.0, 1.0))
+        self.hw.send_control(target, self.kp_leg * kp_scale, self.kd_leg, self.kd_wheel)
+        return target

05_software/real/sim2real/main.py

@@ -0,0 +1,725 @@
+"""CLI entrypoint for current sim2real deployment."""
+
+import argparse
+import os
+import sys
+import threading
+import time
+from pathlib import Path
+
+import numpy as np
+import yaml
+
+sys.path.insert(0, str(Path(__file__).resolve().parent))
+
+from input_dev.keyboard import KeyboardCommandController
+from interface.real_io import RealIO
+from policy.policy_runner import PolicyRunner
+from safety.runtime_guard import GuardLevel, RuntimeGuard
+from safety.safety_monitor import SafetyLevel, SafetyMonitor
+from startup.pose_initializer import PoseInitFailed, PoseInitializer, STAND_POSE
+from startup.stand_balance import StandBalanceController
+from tools.logger import LogBundle
+from tools.math_utils import get_gravity_orientation
+
+JOINT_LABELS = LogBundle.JOINT_LABELS
+
+
+def make_real_driver_factory():
+    def factory(can1_port, can2_port, debug):
+        sim2real_root = Path(__file__).resolve().parent
+        for path in (
+            sim2real_root / "vendored",
+            "/home/rc2/work/rcwork/control",
+            "/home/rc2/work/rcwork",
+        ):
+            path_str = str(path)
+            if path_str not in sys.path and Path(path).exists():
+                sys.path.append(path_str)
+        from drivers.motor_driver import RobStrideDriver  # type: ignore
+
+        return RobStrideDriver(can1_port, debug), RobStrideDriver(can2_port, debug)
+
+    return factory
+
+
+def make_dry_driver_factory():
+    class MockMotor:
+        def __init__(self):
+            class State:
+                position = 0.0
+                velocity = 0.0
+                torque = 0.0
+
+            self.state = State()
+
+    class MockDriver:
+        def __init__(self, port, debug):
+            self.port = port
+            self.motors = {}
+
+        def connect(self): ...
+        def disconnect(self): ...
+        def add_motor(self, name, motor_id, model): self.motors[name] = MockMotor()
+        def enable(self, name): ...
+        def disable(self, name): ...
+        def clear_warnings(self, name): ...
+        def process_messages(self): ...
+        def control_mit(self, *args, **kwargs): ...
+
+    def factory(can1_port, can2_port, debug):
+        return MockDriver(can1_port, debug), MockDriver(can2_port, debug)
+
+    return factory
+
+
+def _sleep_to(next_exec: float) -> float:
+    slack = next_exec - time.perf_counter()
+    if slack > 0:
+        time.sleep(slack)
+        return next_exec + 0.0
+    return time.perf_counter()
+
+
+def build_action_diag(
+    *,
+    joint_pos: np.ndarray,
+    default_pose: np.ndarray,
+    raw: np.ndarray,
+    scaled: np.ndarray,
+    tentative: np.ndarray,
+    cmd: np.ndarray,
+    zero_command: bool,
+    runtime_released: bool,
+    release_alpha: float,
+    safety_details: dict | None = None,
+) -> dict:
+    details = dict(safety_details or {})
+    joint_indices = list(details.get("joint_indices", []))
+    pos_err = tentative - joint_pos
+    leg_offset = tentative[:12] - default_pose[:12]
+    diag = {
+        "joint_indices": joint_indices,
+        "joint_names": [JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(JOINT_LABELS)],
+        "cmd": cmd.tolist(),
+        "zero_command": bool(zero_command),
+        "runtime_released": bool(runtime_released),
+        "release_alpha": float(release_alpha),
+        "max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0,
+        "max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0,
+        "max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0,
+    }
+    if joint_indices:
+        primary = int(joint_indices[0])
+        diag.update(
+            {
+                "primary_joint_index": primary,
+                "primary_joint_name": JOINT_LABELS[primary],
+                "primary_target": float(tentative[primary]),
+                "primary_default": float(default_pose[primary]),
+                "primary_measured": float(joint_pos[primary]),
+                "primary_pos_err": float(pos_err[primary]),
+                "primary_raw": float(raw[primary]),
+                "primary_scaled": float(scaled[primary]),
+            }
+        )
+        if primary < 12:
+            diag["primary_leg_offset"] = float(leg_offset[primary])
+    details.update(diag)
+    return details
+
+
+def policy_release_cfg(cfg: dict) -> dict[str, float]:
+    policy_cfg = cfg.get("policy", {})
+    return {
+        "command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0),
+        "posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0),
+        "target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3),
+    }
+
+
+def compute_release_metrics(runner: PolicyRunner, state: dict, hold_target: np.ndarray, cmd: np.ndarray) -> dict:
+    joint_pos = np.asarray(state["joint_pos"], dtype=np.float32)
+    default_pose = np.asarray(runner.default_dof_pos, dtype=np.float32)
+    hold_target = np.asarray(hold_target, dtype=np.float32)
+    planar_cmd, yaw_cmd = runner.command_activation_metrics(cmd)
+    return {
+        "planar_cmd": float(planar_cmd),
+        "yaw_cmd": float(yaw_cmd),
+        "max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))),
+        "max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))),
+        "max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))),
+    }
+
+
+def blend_runtime_target(
+    runner: PolicyRunner,
+    hold_target: np.ndarray,
+    policy_target: np.ndarray,
+    release_alpha: float,
+    target_blend_s: float,
+    control_dt: float,
+) -> np.ndarray:
+    blend = min(1.0, release_alpha * (runner.command_release_s / max(target_blend_s, control_dt)))
+    return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32)
+
+
+def compute_target_error_metrics(
+    state: dict,
+    hold_target: np.ndarray,
+    policy_target: np.ndarray,
+) -> dict[str, float]:
+    joint_pos = np.asarray(state["joint_pos"], dtype=np.float32)
+    hold_target = np.asarray(hold_target, dtype=np.float32)
+    policy_target = np.asarray(policy_target, dtype=np.float32)
+    return {
+        "hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))),
+        "policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))),
+        "hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))),
+    }
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--config", default=str(Path(__file__).parent / "config.yaml"))
+    parser.add_argument("--policy", default=None)
+    parser.add_argument("--dry-run", action="store_true")
+    args = parser.parse_args()
+
+    with open(args.config, "r", encoding="utf-8") as file_obj:
+        cfg = yaml.safe_load(file_obj)
+
+    sim2real_root = Path(__file__).resolve().parent
+    policy_path = Path(args.policy) if args.policy else sim2real_root / "policies" / "model_rough.pt"
+    if not policy_path.exists():
+        print(f"[Main] policy not found: {policy_path}")
+        sys.exit(1)
+
+    control_dt = 1.0 / float(cfg["control_freq"])
+    driver_factory = make_dry_driver_factory() if args.dry_run else make_real_driver_factory()
+
+    logger = LogBundle(cfg["log_dir"])
+    logger.event(
+        "CONFIG_LOADED",
+        config_path=args.config,
+        policy=str(policy_path),
+        dry_run=args.dry_run,
+        control_freq=cfg["control_freq"],
+        motor_model=cfg["motor_model"],
+    )
+
+    io = RealIO(
+        driver_factory=driver_factory,
+        motor_model=cfg["motor_model"],
+        can1_port=cfg["can1_port"],
+        can2_port=cfg["can2_port"],
+        imu_lib_path=cfg.get("imu_lib_path"),
+        control_dt=control_dt,
+        kp_leg=cfg["controller"]["kp_leg"],
+        kd_leg=cfg["controller"]["kd_leg"],
+        kd_wheel=cfg["controller"]["kd_wheel"],
+        debug=cfg.get("debug", False),
+    )
+    runner = PolicyRunner(
+        policy_path,
+        enable_zero_cmd_suppression=cfg.get("policy", {}).get("enable_zero_cmd_suppression", True),
+        hold_zero_command_pose=cfg.get("policy", {}).get("hold_zero_command_pose", True),
+        command_release_s=cfg.get("policy", {}).get("command_release_s", 0.35),
+        action_scale=np.asarray(
+            cfg.get("policy", {}).get(
+                "action_scale",
+                [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0],
+            ),
+            dtype=np.float32,
+        ),
+        zero_cmd_use_yaw_rate=cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False),
+    )
+    require_active_command = cfg.get("policy", {}).get("require_active_command_to_release", True)
+    keyboard = KeyboardCommandController(
+        max_x_vel=cfg["controller"]["max_vx"],
+        max_y_vel=cfg["controller"]["max_vy"],
+        max_yaw_vel=cfg["controller"]["max_yaw_rate"],
+    )
+    safety = SafetyMonitor(
+        max_target_offset=cfg["safety"]["max_target_offset"],
+        max_ang_vel=cfg["safety"]["max_ang_vel"],
+        max_tilt_z=cfg["safety"]["max_tilt_z"],
+        clip_to_brake=cfg["safety"]["clip_to_brake"],
+    )
+    safety.reset()
+    guard = RuntimeGuard(
+        max_ang_vel=cfg["safety"]["max_ang_vel"],
+        max_tilt_z=cfg["safety"]["max_tilt_z"],
+        imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0),
+        imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0),
+    )
+    initializer = PoseInitializer(
+        io,
+        control_dt=control_dt,
+        transition_time_min=cfg["startup"].get("transition_time_min", 2.0),
+        transition_time_max=cfg["startup"].get("transition_time_max", 6.0),
+        transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5),
+        hold_time=cfg["startup"]["hold_time"],
+        settle_pos_threshold=cfg["startup"]["settle_pos_threshold"],
+        settle_vel_threshold=cfg["startup"]["settle_vel_threshold"],
+        timeout_extra=cfg["startup"].get("timeout_extra", 3.0),
+        progress_log_interval=cfg["startup"]["progress_log_interval"],
+        ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0),
+        soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0),
+        max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5),
+        max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0),
+    )
+    initializer.attach(logger=logger, guard=guard, keyboard=keyboard)
+    stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt)
+
+    print("\n[Main] connecting hardware...")
+    keyboard.start()
+    try:
+        io.connect()
+        logger.event("CAN_IMU_CONNECTED", initial_gravity=io.imu.initial_gravity)
+    except Exception as exc:
+        logger.event("HARDWARE_CONNECT_FAILED", error=str(exc))
+        keyboard.stop()
+        logger.close()
+        raise
+
+    try:
+        io.enable_motors()
+        logger.event("MOTORS_ENABLED")
+        time.sleep(0.5)
+
+        target_pose = initializer.transition_to_stand_from_current(target_pose=STAND_POSE) if cfg["startup"]["enabled"] else STAND_POSE.copy()
+
+        if stand_balance.enabled:
+            logger.event("STAND_BALANCE_BEGIN")
+            print("[Main] waiting for stand-balance to settle...")
+            stand_balance.reset()
+            next_exec = time.perf_counter()
+            while True:
+                state = io.read_state()
+                target_pose = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32))
+                io.hold_pose(target_pose, kp_scale=1.0)
+                debug = stand_balance.last_debug
+                if stand_balance.is_stable():
+                    logger.event(
+                        "STAND_BALANCE_STABLE",
+                        roll_deg=float(np.degrees(debug.roll)),
+                        pitch_deg=float(np.degrees(debug.pitch)),
+                    )
+                    break
+                next_exec += control_dt
+                next_exec = _sleep_to(next_exec)
+            logger.event("STAND_BALANCE_END")
+
+        if cfg["startup"]["require_user_confirm"]:
+            print("[Main] standing complete. Press Enter to release policy control...")
+            done = threading.Event()
+
+            def _wait():
+                try:
+                    input()
+                except EOFError:
+                    pass
+                done.set()
+
+            threading.Thread(target=_wait, daemon=True).start()
+            if not initializer.hold_until_user_confirm(target_pose, done):
+                raise PoseInitFailed("WAIT_USER interrupted")
+
+        print("[Main] priming current observation...")
+        logger.event("PRIME_BEGIN")
+        zero_cmd = np.zeros(3, dtype=np.float32)
+        next_exec = time.perf_counter()
+        for index in range(1):
+            if stand_balance.enabled:
+                state = io.read_state()
+                target_pose = stand_balance.compute_target(state, zero_cmd)
+                io.hold_pose(target_pose, kp_scale=1.0)
+            else:
+                io.hold_pose(target_pose, kp_scale=1.0)
+                state = io.read_state()
+            obs = io.get_obs_policy(state, zero_cmd, runner.default_dof_pos, runner.last_actions)
+            if index == 0:
+                runner.reset(prime_obs=obs)
+            logger.state(
+                phase="PRIME",
+                joint_pos=state["joint_pos"],
+                joint_vel=state["joint_vel"],
+                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                target_pose=target_pose,
+                raw_action=None,
+                gyro=state["imu_gyro"],
+                accel=state["imu_accel"],
+                quat=state["quat_wxyz"],
+                proj_gravity=state["projected_gravity"],
+                command=zero_cmd,
+                imu_age_ms=float(state["imu_age_ms"]),
+                loop_dt_ms=0.0,
+                kp_scale=1.0,
+            )
+            next_exec += control_dt
+            next_exec = _sleep_to(next_exec)
+        logger.event("PRIME_END")
+
+        print("[Main] entering 50Hz control loop... (space = estop)")
+        logger.event("RUNTIME_BEGIN")
+        next_exec = time.perf_counter()
+        loop_count = 0
+        last_print = next_exec
+        log_every = int(cfg.get("log_every", 1))
+        recent_dt_ms = []
+        runtime_released = not require_active_command
+        release_cfg = policy_release_cfg(cfg)
+        release_active_time = 0.0
+
+        while True:
+            loop_t0 = time.perf_counter()
+            cmd = keyboard.get_command()
+            state = io.read_state()
+            obs = io.get_obs_policy(state, cmd, runner.default_dof_pos, runner.last_actions)
+            zero_command = runner._is_zero_command(cmd, state["imu_gyro"])
+
+            obs_nan = bool(np.any(np.isnan(obs)) or np.any(np.isinf(obs)))
+            if obs_nan:
+                logger.event("OBS_NAN", obs_max=float(np.nanmax(obs)))
+                io.damping_brake()
+                break
+
+            if not runtime_released and zero_command:
+                raw = np.zeros(16, dtype=np.float32)
+                scaled = np.zeros(16, dtype=np.float32)
+                target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy()
+                actual_target = io.hold_pose(target_hold, kp_scale=1.0)
+                policy_target = runner.default_dof_pos.copy()
+                release_metrics = compute_release_metrics(runner, state, target_hold, cmd)
+                target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
+                release_active_time = 0.0
+                safety_decision = SafetyMonitor().check(
+                    target_pose=target_hold,
+                    default_pose=runner.default_dof_pos,
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=state["projected_gravity"],
+                    estop_triggered=keyboard.is_estop_triggered(),
+                )
+                guard_decision = guard.check(
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=state["projected_gravity"],
+                    imu_age_ms=float(state["imu_age_ms"]),
+                    estop_triggered=keyboard.is_estop_triggered(),
+                    extra_nan_arrays=(target_hold,),
+                )
+            else:
+                target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy()
+                release_metrics = compute_release_metrics(runner, state, target_hold, cmd)
+                if not runtime_released:
+                    release_active_time += control_dt if runner.is_command_active(cmd) else 0.0
+                    active_ready = release_active_time >= release_cfg["command_hold_s"]
+                    posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"]
+                    if active_ready and posture_ready:
+                        runtime_released = True
+                        logger.event(
+                            "RUNTIME_COMMAND_RELEASED",
+                            cmd=cmd.tolist(),
+                            active_hold_s=release_active_time,
+                            max_hold_err=release_metrics["max_hold_err"],
+                            max_default_err=release_metrics["max_default_err"],
+                            max_hold_default_gap=release_metrics["max_hold_default_gap"],
+                        )
+                    else:
+                        reasons = []
+                        if not active_ready:
+                            reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s")
+                        if not posture_ready:
+                            reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}")
+                        logger.event(
+                            "RUNTIME_RELEASE_BLOCKED",
+                            reason=",".join(reasons),
+                            cmd=cmd.tolist(),
+                            active_hold_s=release_active_time,
+                            max_hold_err=release_metrics["max_hold_err"],
+                            max_default_err=release_metrics["max_default_err"],
+                            max_hold_default_gap=release_metrics["max_hold_default_gap"],
+                        )
+                        raw = np.zeros(16, dtype=np.float32)
+                        scaled = np.zeros(16, dtype=np.float32)
+                        actual_target = io.hold_pose(target_hold, kp_scale=1.0)
+                        policy_target = runner.default_dof_pos.copy()
+                        target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
+                        safety_decision = SafetyMonitor().check(
+                            target_pose=target_hold,
+                            default_pose=runner.default_dof_pos,
+                            imu_gyro=state["imu_gyro"],
+                            projected_gravity=state["projected_gravity"],
+                            estop_triggered=keyboard.is_estop_triggered(),
+                        )
+                        guard_decision = guard.check(
+                            imu_gyro=state["imu_gyro"],
+                            projected_gravity=state["projected_gravity"],
+                            imu_age_ms=float(state["imu_age_ms"]),
+                            estop_triggered=keyboard.is_estop_triggered(),
+                            extra_nan_arrays=(target_hold,),
+                        )
+                        loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0
+                        if log_every and (loop_count % log_every == 0):
+                            motor_diag = state.get("motor_stale", {})
+                            logger.state(
+                                phase="RUNTIME",
+                                joint_pos=state["joint_pos"],
+                                joint_vel=state["joint_vel"],
+                                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                                target_pose=actual_target,
+                                raw_action=raw,
+                                gyro=state["imu_gyro"],
+                                accel=state["imu_accel"],
+                                quat=state["quat_wxyz"],
+                                proj_gravity=state["projected_gravity"],
+                                command=cmd,
+                                imu_age_ms=float(state["imu_age_ms"]),
+                                loop_dt_ms=loop_dt_ms,
+                                safety_level=int(safety_decision.level),
+                                guard_level=int(guard_decision.level),
+                                holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                                stale_max=int(motor_diag.get("stale_max", 0)),
+                                fresh_count=int(motor_diag.get("fresh_count", 16)),
+                                kp_scale=1.0,
+                                nan_flag=0,
+                                kp_leg_cmd=float(io.kp_leg),
+                                kd_leg_cmd=float(io.kd_leg),
+                                kd_wheel_cmd=float(io.kd_wheel),
+                                runtime_release_alpha=0.0,
+                                runtime_release_hold_s=release_active_time,
+                                runtime_blend_ratio=0.0,
+                                hold_target_max_err=target_metrics["hold_target_max_err"],
+                                policy_target_max_err=target_metrics["policy_target_max_err"],
+                                hold_policy_max_gap=target_metrics["hold_policy_max_gap"],
+                                target_source="runtime_hold",
+                                clip_primary_joint="",
+                                safety_reason=f"release_blocked:{','.join(reasons)}",
+                                guard_reason=guard_decision.reason,
+                            )
+                        next_exec += control_dt
+                        next_exec = _sleep_to(next_exec)
+                        loop_count += 1
+                        continue
+                scaled, raw = runner.step(obs)
+                act_nan = bool(np.any(np.isnan(raw)) or np.any(np.isinf(raw)))
+                if act_nan:
+                    logger.event("ACTION_NAN")
+                    io.damping_brake()
+                    break
+
+                policy_target = (scaled + runner.default_dof_pos).astype(np.float32)
+                tentative = blend_runtime_target(
+                    runner,
+                    target_hold,
+                    policy_target,
+                    float(getattr(runner, "_command_release_alpha", 0.0)),
+                    release_cfg["target_blend_s"],
+                    control_dt,
+                )
+                scaled = tentative - runner.default_dof_pos
+                target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
+                runtime_blend_ratio = min(
+                    1.0,
+                    float(getattr(runner, "_command_release_alpha", 0.0))
+                    * (runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)),
+                )
+                projected_gravity = get_gravity_orientation(state["quat_wxyz"])
+
+                guard_decision = guard.check(
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=projected_gravity,
+                    imu_age_ms=float(state["imu_age_ms"]),
+                    estop_triggered=keyboard.is_estop_triggered(),
+                    extra_nan_arrays=(raw, tentative),
+                )
+                if guard_decision.level == GuardLevel.STOP:
+                    logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason)
+                    io.damping_brake()
+                    break
+
+                safety_decision = safety.check(
+                    target_pose=tentative,
+                    default_pose=runner.default_dof_pos,
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=projected_gravity,
+                    estop_triggered=keyboard.is_estop_triggered(),
+                )
+                if safety_decision.level == SafetyLevel.ESTOP:
+                    logger.event("SAFETY_ESTOP", reason=safety_decision.message)
+                    io.damping_brake()
+                    break
+                if safety_decision.level == SafetyLevel.BRAKE:
+                    safety_diag = build_action_diag(
+                        joint_pos=state["joint_pos"],
+                        default_pose=runner.default_dof_pos,
+                        raw=raw,
+                        scaled=scaled,
+                        tentative=tentative,
+                        cmd=cmd,
+                        zero_command=zero_command,
+                        runtime_released=runtime_released,
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                        safety_details=safety_decision.details,
+                    )
+                    logger.event(
+                        "SAFETY_BRAKE",
+                        reason=safety_decision.message,
+                        details=safety_diag,
+                        primary_joint=safety_diag.get("primary_joint_name"),
+                        primary_offset=safety_diag.get("primary_leg_offset"),
+                        primary_target=safety_diag.get("primary_target"),
+                        primary_measured=safety_diag.get("primary_measured"),
+                        primary_raw=safety_diag.get("primary_raw"),
+                        primary_scaled=safety_diag.get("primary_scaled"),
+                        cmd=cmd.tolist(),
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                    )
+                    io.damping_brake()
+                    break
+                if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None:
+                    scaled = safety_decision.clipped_target - runner.default_dof_pos
+                    safety_diag = build_action_diag(
+                        joint_pos=state["joint_pos"],
+                        default_pose=runner.default_dof_pos,
+                        raw=raw,
+                        scaled=scaled,
+                        tentative=tentative,
+                        cmd=cmd,
+                        zero_command=zero_command,
+                        runtime_released=runtime_released,
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                        safety_details=safety_decision.details,
+                    )
+                    logger.event(
+                        "SAFETY_CLIP",
+                        reason=safety_decision.message,
+                        details=safety_diag,
+                        primary_joint=safety_diag.get("primary_joint_name"),
+                        primary_offset=safety_diag.get("primary_leg_offset"),
+                        primary_target=safety_diag.get("primary_target"),
+                        primary_measured=safety_diag.get("primary_measured"),
+                        primary_raw=safety_diag.get("primary_raw"),
+                        primary_scaled=safety_diag.get("primary_scaled"),
+                        max_raw=float(np.max(np.abs(raw))),
+                        cmd=cmd.tolist(),
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                    )
+
+                actual_target = io.send_actions(scaled, runner.default_dof_pos)
+            loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0
+
+            if log_every and (loop_count % log_every == 0):
+                motor_diag = state.get("motor_stale", {})
+                logger.state(
+                    phase="RUNTIME",
+                    joint_pos=state["joint_pos"],
+                    joint_vel=state["joint_vel"],
+                    joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                    target_pose=actual_target,
+                    raw_action=raw,
+                    gyro=state["imu_gyro"],
+                    accel=state["imu_accel"],
+                    quat=state["quat_wxyz"],
+                    proj_gravity=projected_gravity,
+                    command=cmd,
+                    imu_age_ms=float(state["imu_age_ms"]),
+                    loop_dt_ms=loop_dt_ms,
+                    safety_level=int(safety_decision.level),
+                    guard_level=int(guard_decision.level),
+                    holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                    stale_max=int(motor_diag.get("stale_max", 0)),
+                    fresh_count=int(motor_diag.get("fresh_count", 16)),
+                    kp_scale=1.0,
+                    nan_flag=int(obs_nan or act_nan),
+                    kp_leg_cmd=float(io.kp_leg),
+                    kd_leg_cmd=float(io.kd_leg),
+                    kd_wheel_cmd=float(io.kd_wheel),
+                    runtime_release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                    runtime_release_hold_s=release_active_time,
+                    runtime_blend_ratio=runtime_blend_ratio,
+                    hold_target_max_err=target_metrics["hold_target_max_err"],
+                    policy_target_max_err=target_metrics["policy_target_max_err"],
+                    hold_policy_max_gap=target_metrics["hold_policy_max_gap"],
+                    target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy",
+                    clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")),
+                    clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0),
+                    clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0),
+                    clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0),
+                    clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0),
+                    clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0),
+                    clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0),
+                    safety_reason=(
+                        f"{safety_decision.message};zero_cmd={int(zero_command)};"
+                        f"released={int(runtime_released)};alpha={getattr(runner, '_command_release_alpha', 0.0):.2f};"
+                        f"max_raw={float(np.max(np.abs(raw))):.2f};"
+                        f"clip={((safety_decision.details or {}).get('joint_indices', []))}"
+                    ),
+                    guard_reason=guard_decision.reason,
+                )
+
+            next_exec += control_dt
+            slack = next_exec - time.perf_counter()
+            if slack > 0:
+                coarse = slack - 0.002
+                if coarse > 0:
+                    time.sleep(coarse)
+                while time.perf_counter() < next_exec:
+                    pass
+            elif slack < -control_dt:
+                logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0)
+                next_exec = time.perf_counter()
+
+            recent_dt_ms.append(loop_dt_ms)
+            if len(recent_dt_ms) > 50:
+                recent_dt_ms.pop(0)
+            if len(recent_dt_ms) == 50:
+                median_dt = float(np.median(recent_dt_ms))
+                if median_dt > 22.0:
+                    logger.event("SLOW_LOOP_TREND", median_dt_ms=median_dt)
+                    recent_dt_ms.clear()
+
+            loop_count += 1
+            if time.perf_counter() - last_print > 1.0:
+                print(
+                    f"[Loop] cmd=[{cmd[0]:+.2f},{cmd[1]:+.2f},{cmd[2]:+.2f}] "
+                    f"|raw|={float(np.max(np.abs(raw))):.2f} "
+                    f"zero={int(zero_command)} rel={int(runtime_released)} "
+                    f"alpha={getattr(runner, '_command_release_alpha', 0.0):.2f} "
+                    f"imu_age={state['imu_age_ms']:.1f}ms "
+                    f"holdover={io.hw.holdover_total} "
+                    f"safety={int(safety_decision.level)}"
+                )
+                last_print = time.perf_counter()
+
+    except PoseInitFailed as exc:
+        print(f"[Main] startup aborted: {exc}")
+        logger.event("POSE_INIT_FAILED", error=str(exc))
+    except KeyboardInterrupt:
+        print("\n[Main] Ctrl+C received, stopping...")
+        logger.event("KEYBOARD_INTERRUPT")
+    except Exception as exc:
+        import traceback
+
+        print(f"\n[Main] exception: {exc}")
+        traceback.print_exc()
+        logger.event("UNEXPECTED_ERROR", error=str(exc), traceback=traceback.format_exc())
+    finally:
+        print("[Main] cleaning up...")
+        try:
+            io.damping_brake()
+            time.sleep(0.05)
+            logger.event("DAMPING_BRAKE_APPLIED")
+        except Exception as exc:
+            logger.event("DAMPING_BRAKE_FAILED", error=str(exc))
+        try:
+            io.disconnect()
+            logger.event("HARDWARE_DISCONNECTED")
+        finally:
+            keyboard.stop()
+            logger.close()
+        os._exit(0)
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real/mjcf/scene.xml

@@ -0,0 +1,22 @@
+<mujoco model="wheelleg_scene">
+  <include file="wheelleg.xml"/>
+
+  <option timestep="0.002" gravity="0 0 -9.81" integrator="implicitfast"/>
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3"/>
+    <global azimuth="120" elevation="-20"/>
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge"
+      rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 3" dir="0 0 -1" directional="true"/>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" friction="0.8 0.05 0.01"/>
+  </worldbody>
+</mujoco>

05_software/real/sim2real/mjcf/sim2sim_temp.xml

@@ -0,0 +1,327 @@
+<mujoco model="go2w scene">
+<include file="C:/Users/31560/Documents/00_legged/new_rl/rc_mjlab/mjcf/wheelleg.xml"/>
+<statistic center="3.7 -9.0 0.4" extent="5.0"/>
+<visual>
+<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+<rgba haze="0.15 0.25 0.35 1"/>
+<global azimuth="90" elevation="-20"/>
+</visual>
+<asset>
+<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+<hfield name="perlin_hfield" size="1.0 0.75 0.2 0.2" file="C:/Users/31560/Documents/00_legged/new_rl/rc_mjlab/sim2sim/terrain/height_field.png"/>
+<hfield name="image_hfield" size="1.0 1.0 0.02 0.1" file="C:/Users/31560/Documents/00_legged/new_rl/rc_mjlab/sim2sim/terrain/unitree_hfield.png"/>
+</asset>
+<worldbody>
+<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
+<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+
+<!-- 30cm高墙：旋转90度，沿x轴方向放置，并与T型楼梯中心线 y=-3.50 对齐 -->
+<geom pos="1.8 -7.0 0.15"
+      type="box"
+      size="0.025 0.5 0.15"
+      quat="0.7071068 0.0 0.0 0.7071068" 
+      rgba="1.0 0.9 0.4 1.0"/>
+
+<!-- 沙砾碎木坑：x正方向边界与10度斜坡+x边界对齐，y正边界距斜坡y负边界4m -->
+
+<geom pos="4.8361 -12.5 0.075"
+      type="box"
+      size="0.5 0.5 0.075"
+      quat="0.0 0.0 0.0 1.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+
+<geom pos="5.8361 -12.0 0.075"
+      type="box"
+      size="0.5 1.0 0.075"
+      quat="0.0 0.0 0.0 1.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+
+
+
+
+<!-- 限高杆 -->
+<geom pos="6.2 -9.0 0.155"
+      type="cylinder"
+      size="0.025 0.155"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.8 0.1 0.1 1.0" />
+
+<geom pos="5.2 -9.0 0.155"
+      type="cylinder"
+      size="0.025 0.155"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.8 0.1 0.1 1.0" />
+<geom pos="5.7 -9.0 0.325"
+      type="cylinder"
+      size="0.015 0.5"
+      quat="0.7071068 0.0 0.7071068 0.0" 
+      rgba="1.0 0.9 0.4 1.0"/>
+
+
+<!-- 1m × 1m 正方形颜色块，出发区-->
+<geom pos="3.7 -9.0 0.0"
+      type="box"
+      size="0.5 0.5 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+
+
+<!-- 10cm梯形台阶：T型楼梯，最高平台与 x=5.7 y=-3.5 平台在y轴方向对齐 -->
+
+<geom pos="1.80 -4.75 0.05" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0" />
+<geom pos="1.80 -4.45 0.15" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.80 -4.15 0.25" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+
+<!-- 最高平台：y = -3.50，与目标平台y轴对齐 -->
+<geom pos="1.80 -3.50 0.35" type="box" size="0.5 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+
+<geom pos="1.80 -2.85 0.25" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.80 -2.55 0.15" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.80 -2.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+
+
+<!-- 顶部平台向 +x 方向连接地面的10cm台阶，同样y轴移动到 -3.50 -->
+
+<geom pos="2.45 -3.50 0.25" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="2.75 -3.50 0.15" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="3.05 -3.50 0.05" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+
+
+
+
+<!-- 斜坡木桥A木桥B -->
+<geom pos="1.8 -0.88 0.1" type="box" size="0.40 0.5 0.005" quat="0.701836 0.086175 -0.086175 0.701836" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.8 0.0 0.10" type="box" size="0.5 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="2.65 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="3.2 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="3.75 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="4.3 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="4.85 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.7 -0.5 0.10" type="box" size="0.5 1.0 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="4.8002 -1.0 0.0951"
+      type="box"
+      size="0.4133 0.5 0.005"
+      quat="0.992546 0.0 -0.121869 0.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.35 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.65 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.95 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.7 -3.5 0.10" type="box" size="0.5 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<!-- 10度斜坡：宽4m，斜坡 y正方向边缘 与平台 y正方向边缘 对齐 -->
+<geom pos="4.6338 -5.0 0.0951"
+      type="box"
+      size="0.5759 2.0 0.005"
+      quat="0.9961947 0.0 -0.0871557 0.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+<!-- 新建10度斜坡：宽3m，高端与前一个10度斜坡高端衔接，向+x方向下坡 -->
+<geom pos="5.7681 -5.5 0.0951"
+      type="box"
+      size="0.5759 1.5 0.005"
+      quat="0.9961947 0.0 0.0871557 0.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+
+
+
+
+
+<!--绕杆-->
+<!-- 直径1m圆形颜色块，仅显示，不碰撞 -->
+<geom pos="1.8 -10.1 0.0"
+      type="cylinder"
+      size="0.1 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+<geom pos="3.2 -12.5 0.0"
+      type="cylinder"
+      size="0.1 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+<geom pos="1.55 -12.75 0.0"
+      type="cylinder"
+      size="0.1 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+<!-- 原杆 -->
+<geom pos="1.8 -10.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="1.8 -10.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!-- y轴负方向第1根：间隔1m -->
+<geom pos="1.8 -11.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="1.8 -11.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!-- y轴负方向第2根：继续间隔1m -->
+<geom pos="1.8 -12.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="1.8 -12.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!-- x轴正方向第3根：继续间隔1m -->
+<geom pos="2.8 -12.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="2.8 -12.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!--===================================================================其他障碍=====================================================================================-->
+
+<!-- 5cm台阶 -->
+<geom pos="1.0 2.0 0.025" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="1.3 2.0 0.075" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="1.6 2.0 0.125" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="1.9 2.0 0.175" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="2.2 2.0 0.225" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="2.5 2.0 0.275" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="2.8 2.0 0.325" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="3.45 2.0 0.375" type="box" size="0.5 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="4.1 2.0 0.325" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="4.4 2.0 0.275" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="4.7 2.0 0.225" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.0 2.0 0.175" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.3 2.0 0.125" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.6 2.0 0.075" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.9 2.0 0.025" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+
+<!-- 斜坡 -->
+<geom pos="2.0 4.0 0.1" type="box" size="1.5 0.75 0.005" quat="0.9950041652780258 0.0 -0.09983341664682815 0.0" />
+
+
+
+
+<geom pos="1.4 6.0 0.165" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="1.6 6.0 0.275" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="1.8 6.0 0.385" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.0 6.0 0.495" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.2 6.0 0.605" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.4 6.0 0.715" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.5999999999999996 6.0 0.825" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.8 6.0 0.9349999999999999" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="3.0 6.0 1.045" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="-2.3179973398407565 5.173660321080885 -0.25" type="box" size="0.2568216778785459 0.2608020098770089 0.2619541072037832" quat="0.9930658271270357 -0.05910360995856133 -0.06731065544310916 0.0761334482746007"/>
+<geom pos="-2.3179973398407565 5.3620612607436735 -0.25" type="box" size="0.23778028947050467 0.26051225137569556 0.27137457425982286" quat="0.9959802827127009 0.07435995796871331 0.0004862731479656538 -0.04993632582444846"/>
+<geom pos="-2.3179973398407565 5.545897059602109 -0.25" type="box" size="0.23841994611378936 0.2717839884518381 0.22585827399286504" quat="0.9983504429181294 -0.004811901627841528 0.03471877670768104 -0.045473566737405234"/>
+<geom pos="-2.3179973398407565 5.7436240471772795 -0.25" type="box" size="0.2552179019048769 0.2548992578792955 0.22547735976326444" quat="0.9968270877924189 -0.029673908987198697 0.06777847718526858 -0.029347814214192768"/>
+<geom pos="-2.3179973398407565 5.940214647011584 -0.25" type="box" size="0.24313116620329878 0.2372064979204117 0.26079933745117434" quat="0.9952106364954509 0.05088987714749159 -0.07605843245051555 -0.03436748846516202"/>
+<geom pos="-2.3179973398407565 6.165430585901471 -0.25" type="box" size="0.24786042386990592 0.2322559052231109 0.2644037606269708" quat="0.9936075351397807 -0.05017393314139304 0.06986162224674641 -0.07311632022760194"/>
+<geom pos="-2.3179973398407565 6.315657865031069 -0.25" type="box" size="0.23704265198840277 0.24982080672772003 0.2530694373586838" quat="0.9981716459301547 0.036179123385437884 0.04523497974247521 0.017269420947002005"/>
+<geom pos="-2.3179973398407565 6.489372835072359 -0.25" type="box" size="0.2647428927965494 0.2716292502682415 0.23725049444938928" quat="0.9954041516289313 0.019099981466976248 0.07663366510923347 0.05415761257454444"/>
+<geom pos="-2.094119617957536 5.194943631570213 -0.25" type="box" size="0.23176840693038148 0.23782936054799508 0.2282032657053922" quat="0.9973366591969123 0.030692664684553082 -0.030015392527106853 0.058962910104125923"/>
+<geom pos="-2.094119617957536 5.441090326561234 -0.25" type="box" size="0.2602142310926322 0.27213502289176367 0.2574009440402366" quat="0.9948915480473169 0.023650461407535205 0.08508706956405496 -0.04890453856469255"/>
+<geom pos="-2.094119617957536 5.642958951230403 -0.25" type="box" size="0.24800055056479955 0.24050676557282252 0.23522489807277194" quat="0.9912055235117067 0.06188914817453858 -0.09720856678264424 -0.0650525790586179"/>
+<geom pos="-2.094119617957536 5.884810142659838 -0.25" type="box" size="0.24637516954806898 0.24364583504893206 0.2682443460752295" quat="0.9964495083946477 -0.07467561549070643 -0.038754439691442995 0.003165924091257204"/>
+<geom pos="-2.094119617957536 6.129893093768145 -0.25" type="box" size="0.2378958269703999 0.25045408022075055 0.24760364656411665" quat="0.9946240462872651 -0.03963038800955788 -0.08917648605658504 0.03464091840526007"/>
+<geom pos="-2.094119617957536 6.31135792935651 -0.25" type="box" size="0.2612154064649683 0.2252849660353503 0.26933214617336004" quat="0.997742809172301 0.019590451180868298 0.06391648942914147 -0.006339033565912353"/>
+<geom pos="-2.094119617957536 6.4905285510291915 -0.25" type="box" size="0.22768983323309258 0.23025468157122184 0.2748062344121069" quat="0.9940843963100783 0.05105793020576152 0.009164090402224958 -0.0954218016127878"/>
+<geom pos="-2.094119617957536 6.650993287072798 -0.25" type="box" size="0.25443787917913946 0.24575387105878618 0.22934424641420587" quat="0.9953832160779188 -0.0075142647566912866 0.08305501342257929 0.04751477371219154"/>
+<geom pos="-1.8951777534978893 5.178742263102535 -0.25" type="box" size="0.256127371544264 0.24353861032614957 0.2714578153598507" quat="0.996547165679354 -0.008558846106514282 0.009384988866828401 0.08205129318749771"/>
+<geom pos="-1.8951777534978893 5.353646654030374 -0.25" type="box" size="0.23858897994497189 0.2426691342857623 0.26961659613969635" quat="0.997689480058865 0.014145078555906384 0.009115651940537827 -0.06582190381793727"/>
+<geom pos="-1.8951777534978893 5.575640096199311 -0.25" type="box" size="0.24430646334884096 0.2676093509240798 0.23693271670520474" quat="0.9976019172675967 -0.06794745094616256 0.006187887696065785 -0.011630503849579829"/>
+<geom pos="-1.8951777534978893 5.728196634759373 -0.25" type="box" size="0.25668697148716296 0.2743986770827004 0.23696403861156407" quat="0.9946311966964959 -0.0774649076913635 0.05866511796477466 0.03558615698054331"/>
+<geom pos="-1.8951777534978893 5.954288148947323 -0.25" type="box" size="0.2748179458028994 0.25407956175122554 0.25142548243710355" quat="0.9927720663552536 0.03885622734718657 0.06180632563910765 0.09525647469886914"/>
+<geom pos="-1.8951777534978893 6.198076908897641 -0.25" type="box" size="0.23522918501552104 0.2714895927340788 0.23659922178360912" quat="0.9944408196408429 0.027722532803612636 -0.06883505448003607 -0.07470376618171105"/>
+<geom pos="-1.8951777534978893 6.356335079330304 -0.25" type="box" size="0.253243646653926 0.26648639763264487 0.22751627090926196" quat="0.9924345646356405 0.06351743627133029 0.09661714031088077 -0.041283149154730255"/>
+<geom pos="-1.8951777534978893 6.6037178705715744 -0.25" type="box" size="0.2664884535468762 0.26472237049442093 0.2545826559482188" quat="0.996208812433608 0.04792667434016892 0.03088022810200046 -0.06570728596343135"/>
+<geom pos="-1.7450143557797366 5.181798049538029 -0.25" type="box" size="0.22764974830145798 0.2314500042225232 0.26635647118774647" quat="0.996420562587967 -0.02530598413067966 -0.01712092616039989 -0.07881968983996092"/>
+<geom pos="-1.7450143557797366 5.396539066742657 -0.25" type="box" size="0.24515338305934362 0.25502436912192245 0.23509532716059323" quat="0.9974817825235419 0.033698559354477776 -0.06057332552735373 -0.01501242370995589"/>
+<geom pos="-1.7450143557797366 5.5477605493550115 -0.25" type="box" size="0.2368415768982088 0.2653984778068547 0.25193186806340717" quat="0.9933816957092091 -0.04426535864216467 0.0892264769079807 0.057201577537394625"/>
+<geom pos="-1.7450143557797366 5.764238738853998 -0.25" type="box" size="0.257475541143157 0.25587521442146555 0.2684267956125346" quat="0.9955849335110497 0.004076118120639651 -0.09299366924131539 0.012091439447074191"/>
+<geom pos="-1.7450143557797366 5.942956213887727 -0.25" type="box" size="0.23647345784635188 0.22779489919605103 0.2690566454457882" quat="0.9997372307105926 0.020286738832439623 -0.010113822112874779 0.003410038259163467"/>
+<geom pos="-1.7450143557797366 6.162981335139796 -0.25" type="box" size="0.2336152227884161 0.23785626414299832 0.26272786991330355" quat="0.9954569549676405 0.08638675499005252 -0.03150236191317218 -0.024705881136540285"/>
+<geom pos="-1.7450143557797366 6.344025407207907 -0.25" type="box" size="0.2364006101773331 0.23674709170116234 0.2660167000427503" quat="0.9941020005048972 -0.09350614404282417 -0.05493059192638792 0.0006660998579442658"/>
+<geom pos="-1.7450143557797366 6.5791872438688825 -0.25" type="box" size="0.259653062502181 0.26359758888480966 0.27170867851854713" quat="0.9955751902075182 0.06572529509454274 0.04059253213564211 -0.05350207998588654"/>
+<geom pos="-1.4950537649717406 5.196975242498044 -0.25" type="box" size="0.24902209995429173 0.24604186796843594 0.26555264385759036" quat="0.9981753741369879 0.027059282981497578 0.02078151255996191 0.049818133312398136"/>
+<geom pos="-1.4950537649717406 5.415093649392617 -0.25" type="box" size="0.22561317519118573 0.23246623591498758 0.2516053992906602" quat="0.9961431033663982 -0.019679813255757937 0.07777937635153075 0.035524515199325105"/>
+<geom pos="-1.4950537649717406 5.596072881787104 -0.25" type="box" size="0.2545347271371952 0.2527292932516396 0.272364707011277" quat="0.9987990019474599 -0.0468132050914785 0.002764373695484487 -0.01419280718860587"/>
+<geom pos="-1.4950537649717406 5.788444207457658 -0.25" type="box" size="0.257823578756761 0.22815201323013437 0.2506904868770564" quat="0.9907800223935689 0.08207700037219429 -0.06679273748567588 -0.08459931119619979"/>
+<geom pos="-1.4950537649717406 5.962479724512656 -0.25" type="box" size="0.23447002037921025 0.260091883647859 0.2613547781123637" quat="0.994185747243896 -0.045779429335849345 -0.09537527521663794 -0.020062420196245392"/>
+<geom pos="-1.4950537649717406 6.137620949716146 -0.25" type="box" size="0.25502378303486756 0.24137626830945555 0.26521755821448284" quat="0.9954511067004865 0.04827241542514884 0.023378972724717166 -0.07874193109224191"/>
+<geom pos="-1.4950537649717406 6.345742058941403 -0.25" type="box" size="0.23763376291279575 0.27259418014745557 0.24184880568666417" quat="0.9979730938550316 -0.0521706328108408 -0.03256129013590577 0.01636127740178204"/>
+<geom pos="-1.4950537649717406 6.548966807280673 -0.25" type="box" size="0.23004532593424165 0.24736965888987583 0.22917624237245732" quat="0.993938322382385 -0.041286377558295506 -0.09233464220936707 0.04308549844056845"/>
+<geom pos="-1.2872521554407157 5.194072124237239 -0.25" type="box" size="0.2725857334366114 0.23611730648841156 0.25109418723334265" quat="0.9870880287314421 -0.09538067591152324 -0.0888119213223197 -0.09312460914696315"/>
+<geom pos="-1.2872521554407157 5.418639818976418 -0.25" type="box" size="0.2326397456179607 0.2609646699674687 0.2717115772948157" quat="0.9928206111485406 -0.07383059897166898 -0.08473752136579614 0.040936892980589765"/>
+<geom pos="-1.2872521554407157 5.655974569843163 -0.25" type="box" size="0.22633109461398734 0.25911291311168594 0.23532484499883452" quat="0.9947769304653071 -0.023834507399340135 0.09176246279588879 -0.037820963666801724"/>
+<geom pos="-1.2872521554407157 5.8981628648303595 -0.25" type="box" size="0.27278934520816167 0.2559269445001904 0.26076472835929454" quat="0.9936318869346414 0.0561019165840355 0.09234219275279033 0.0319557140415977"/>
+<geom pos="-1.2872521554407157 6.064939027275534 -0.25" type="box" size="0.2481756437845515 0.2613413397088905 0.24788858471207542" quat="0.9992310163557966 0.006919915638721734 0.03652449975321466 -0.012468024618680441"/>
+<geom pos="-1.2872521554407157 6.288670079370122 -0.25" type="box" size="0.2574127585385611 0.27445220033632356 0.22507618952620437" quat="0.9984337612791753 0.04947883993440395 -0.0010074967830211658 -0.026093173185657285"/>
+<geom pos="-1.2872521554407157 6.4987760234638605 -0.25" type="box" size="0.2442462188069663 0.2639082925274208 0.24918893213917415" quat="0.9910480688126146 0.08639971284060838 -0.0903343762232188 -0.04688832899783648"/>
+<geom pos="-1.2872521554407157 6.675611985491267 -0.25" type="box" size="0.2507168814441121 0.26699708557208374 0.26588306060638556" quat="0.9968882348111167 0.009466759168649483 -0.06920917552433178 0.03652831489763918"/>
+<geom pos="-1.0678149575697586 5.238251070535694 -0.25" type="box" size="0.2331754586513582 0.22873009754409884 0.2593258638743009" quat="0.9962563091989393 0.014506022500180732 0.069646748226122 0.049114887295595266"/>
+<geom pos="-1.0678149575697586 5.471732496077581 -0.25" type="box" size="0.2653495693182789 0.26581370557074685 0.2509273010188512" quat="0.9948019811445705 -0.05898118120008214 0.04034485064897968 -0.07254330845220838"/>
+<geom pos="-1.0678149575697586 5.691153717662407 -0.25" type="box" size="0.26309077142424103 0.26536949948987093 0.26566703066149744" quat="0.998331608401392 -0.025703385975953945 -0.05139460706256987 0.005650661991725779"/>
+<geom pos="-1.0678149575697586 5.938910564562482 -0.25" type="box" size="0.23082606038737274 0.23890770539441533 0.25941695887199245" quat="0.99040745217393 -0.09494180013051996 0.05600240658814475 -0.08332384846283197"/>
+<geom pos="-1.0678149575697586 6.13746533389903 -0.25" type="box" size="0.2676431215498913 0.2569308659994288 0.24597694927356487" quat="0.9955894165044442 0.04511524606234168 -0.003741035154326279 -0.08217258042101841"/>
+<geom pos="-1.0678149575697586 6.328213254628996 -0.25" type="box" size="0.24188540919954635 0.25556145122229207 0.2605619987765001" quat="0.9929731934387934 -0.06598945456828725 -0.06809928840246085 0.07079629875087447"/>
+<geom pos="-1.0678149575697586 6.554891603524267 -0.25" type="box" size="0.23121916641107804 0.25266417867731916 0.25489063309084503" quat="0.9984565221200774 -0.02714563973423854 0.029423903667849492 0.03849573446816443"/>
+<geom pos="-1.0678149575697586 6.718482966003368 -0.25" type="box" size="0.27418971733780156 0.2623437838864593 0.23694037285314332" quat="0.9968347573500278 0.015128289848683302 0.06990516881209438 0.03471121949050888"/>
+<geom pos="-0.8851136474992356 5.233272057743225 -0.25" type="box" size="0.2587161160845902 0.2542459313914242 0.25268742624288776" quat="0.9956323623605605 0.07455213252171443 -0.05585558926184348 0.006191260373025539"/>
+<geom pos="-0.8851136474992356 5.410759300563648 -0.25" type="box" size="0.24019625793101548 0.2509280955260936 0.26698317271101046" quat="0.9960056830073374 0.025487333399660517 0.08516393482003458 -0.008377318140719903"/>
+<geom pos="-0.8851136474992356 5.622546965631826 -0.25" type="box" size="0.22527085609750916 0.22924847380626232 0.23073331588883172" quat="0.9982201065088221 0.016292210993739946 0.013620740418279952 0.05572843307423328"/>
+<geom pos="-0.8851136474992356 5.856118124892678 -0.25" type="box" size="0.26833255247166926 0.2512767990265972 0.2502231376336179" quat="0.9914910532543448 0.0657880493114159 0.05073139628229043 -0.10021850784978792"/>
+<geom pos="-0.8851136474992356 6.010914595891683 -0.25" type="box" size="0.24926441451407527 0.22868800964152894 0.26501630221174116" quat="0.990736605756697 -0.0765632176593298 -0.09876375004658954 0.05314859727296924"/>
+<geom pos="-0.8851136474992356 6.1657945570092965 -0.25" type="box" size="0.26823427325895977 0.263134568634285 0.23692064318485426" quat="0.9903439387899988 0.07219524819688558 0.09310840228148876 0.07305856872598579"/>
+<geom pos="-0.8851136474992356 6.383066094410735 -0.25" type="box" size="0.23772929143507357 0.2619329708548053 0.23258884134606547" quat="0.9928463525385446 0.07348810370848206 0.09252710448645944 -0.017156742103069993"/>
+<geom pos="-0.8851136474992356 6.551482176174038 -0.25" type="box" size="0.25127741834221506 0.25307864976328337 0.234931895271364" quat="0.994062479132312 -0.084918495271965 -0.05565932062022146 -0.03912386445844099"/>
+<geom pos="-0.6441452943469552 5.193190826541208 -0.25" type="box" size="0.27016649011158844 0.23778885128164629 0.25859862297032477" quat="0.9957774962333289 0.08893738517887846 -0.021654911548168992 0.006955883744530942"/>
+<geom pos="-0.6441452943469552 5.407266358883468 -0.25" type="box" size="0.24982888074520473 0.26972381569065607 0.2275629646713632" quat="0.9942571418988088 0.02444442288698024 0.050336521935401994 -0.09122193010664256"/>
+<geom pos="-0.6441452943469552 5.56363948710079 -0.25" type="box" size="0.24725848381417467 0.2326432801330426 0.2476341019968084" quat="0.9935769557551848 -0.05688836927692267 -0.06858315894908604 0.0697488117593134"/>
+<geom pos="-0.6441452943469552 5.716461923308758 -0.25" type="box" size="0.226746163574222 0.25188961955216527 0.24650452053954758" quat="0.9981029078160676 0.03471313574303335 -0.0493522060742233 -0.012245136651074443"/>
+<geom pos="-0.6441452943469552 5.896430073001983 -0.25" type="box" size="0.25284949298017617 0.23421620066432108 0.2621382648463894" quat="0.9996413412447853 0.009523771195346772 -0.02374945051322109 -0.007902547492127014"/>
+<geom pos="-0.6441452943469552 6.110845613879558 -0.25" type="box" size="0.23421403581041014 0.2504556848552827 0.24096555776925194" quat="0.9982371379292613 0.05906199571247059 0.004184466367765714 0.004097238396071534"/>
+<geom pos="-0.6441452943469552 6.301410327752269 -0.25" type="box" size="0.2296817145699627 0.2701766916372966 0.22803599234835198" quat="0.9993879202059675 0.00903361239651759 0.0071600522792900426 -0.03302896372607247"/>
+<geom pos="-0.6441452943469552 6.470340592771366 -0.25" type="box" size="0.2742166750011587 0.23990439594655139 0.2609404931878803" quat="0.9954980192986956 -0.06477558432537366 -0.03478863652617152 0.059812774691783824"/>
+<geom pos="-0.43033316974190594 5.176946043237525 -0.25" type="box" size="0.2511344405622137 0.26383180387822514 0.2729516287367074" quat="0.9939152287364423 -0.054017256979398534 -0.08418383902399898 -0.0461273810376057"/>
+<geom pos="-0.43033316974190594 5.409693792302493 -0.25" type="box" size="0.23878438774624536 0.22858057493504688 0.24808981791323623" quat="0.9942721168135948 -0.06322741578652484 -0.032958930763236916 -0.0796175891553822"/>
+<geom pos="-0.43033316974190594 5.573593446642387 -0.25" type="box" size="0.2647545698926724 0.25215736713350106 0.25522885339490087" quat="0.9971918839448468 -0.004732654353889047 0.07222000940536273 0.019241071144380294"/>
+<geom pos="-0.43033316974190594 5.819531161619798 -0.25" type="box" size="0.2538318010250847 0.23661725487197 0.26323696729639623" quat="0.9929867679849618 0.07588268800385974 -0.046355180482866284 -0.07791208834634974"/>
+<geom pos="-0.43033316974190594 6.047888701789067 -0.25" type="box" size="0.25684720236482583 0.25568474221031684 0.24397094296107352" quat="0.9925256546500537 -0.08214715587137877 0.06615741576234473 0.06138294537877935"/>
+<geom pos="-0.43033316974190594 6.283676056613722 -0.25" type="box" size="0.2432692787269645 0.2742601402399693 0.2689467974776609" quat="0.9989897410981501 0.0047037010179340685 0.0014590493232283363 -0.04466814919444852"/>
+<geom pos="-0.43033316974190594 6.458239009480634 -0.25" type="box" size="0.2474330649412728 0.2551435998627988 0.23002773988805483" quat="0.9982311862818315 0.0506459597703481 0.025989046975463247 -0.01714802993392417"/>
+<geom pos="-0.43033316974190594 6.703309377175586 -0.25" type="box" size="0.2301029552870502 0.2556223065028505 0.2349557527965884" quat="0.9948672058648923 -0.034182171957127715 -0.0030330068097560517 -0.09519255582537542"/>
+<geom type="hfield" hfield="perlin_hfield" pos="-1.5 4.0 0.0" quat="1.0 0.0 0.0 0.0"/>
+<geom type="hfield" hfield="image_hfield" pos="-1.5 2.0 0.0" quat="0.7073882691671998 0.0 0.0 -0.706825181105366"/>
+</worldbody>
+</mujoco>

05_software/real/sim2real/mjcf/wheelleg.xml

@@ -0,0 +1,157 @@
+<mujoco model="wheelleg">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <default>
+    <geom margin="0"/>
+  </default>
+  <asset>
+    <mesh name="base_link" content_type="model/stl" file="base_link.STL"/>
+    <mesh name="fl_hip_abduction_Link" content_type="model/stl" file="fl_hip_abduction_Link.STL"/>
+    <mesh name="fl_hip_pitch_Link" content_type="model/stl" file="fl_hip_pitch_Link.STL"/>
+    <mesh name="fl_knee_Link" content_type="model/stl" file="fl_knee_Link.STL"/>
+    <mesh name="fl_wheel_Link" content_type="model/stl" file="fl_wheel_Link.STL"/>
+    <mesh name="fr_hip_abduction_Link" content_type="model/stl" file="fr_hip_abduction_Link.STL"/>
+    <mesh name="fr_hip_pitch_Link" content_type="model/stl" file="fr_hip_pitch_Link.STL"/>
+    <mesh name="fr_knee_Link" content_type="model/stl" file="fr_knee_Link.STL"/>
+    <mesh name="fr_wheel_Link" content_type="model/stl" file="fr_wheel_Link.STL"/>
+    <mesh name="rl_hip_abduction_Link" content_type="model/stl" file="rl_hip_abduction_Link.STL"/>
+    <mesh name="rl_hip_pitch_Link" content_type="model/stl" file="rl_hip_pitch_Link.STL"/>
+    <mesh name="rl_knee_Link" content_type="model/stl" file="rl_knee_Link.STL"/>
+    <mesh name="rl_wheel_Link" content_type="model/stl" file="rl_wheel_Link.STL"/>
+    <mesh name="rr_hip_abduction_Link" content_type="model/stl" file="rr_hip_abduction_Link.STL"/>
+    <mesh name="rr_hip_pitch_Link" content_type="model/stl" file="rr_hip_pitch_Link.STL"/>
+    <mesh name="rr_knee_Link" content_type="model/stl" file="rr_knee_Link.STL"/>
+    <mesh name="rr_wheel_Link" content_type="model/stl" file="rr_wheel_Link.STL"/>
+  </asset>
+
+  <worldbody>
+    <body name="base_link">
+      <inertial pos="0.1517 0.0002 0.0542" mass="3.5" diaginertia="0.0215 0.0904 0.0985"/>
+      <joint type="free"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="base_link"/>
+      <geom size="0.178 0.1175 0.073" pos="0.1518 0 0.054" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+      <body name="fl_hip_abduction_Link" pos="0.32826 0.066172 0.053981">
+        <inertial pos="0.0488 -0.0026 0.0007" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="fl_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.436 0.611" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_hip_abduction_Link"/>
+        <body name="fl_hip_pitch_Link" pos="0.06389 -0.027344 0.00010727" quat="0.999997 -0.0025023 0 0">
+          <inertial pos="0.0019 0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="fl_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="fl_knee_Link" pos="0 0.1035 -0.25" quat="0.999997 0.0025023 0 0">
+            <inertial pos="0.0002 0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 0.0002"/>
+            <joint name="fl_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 0.025 -0.20011" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="fl_wheel_Link" pos="0 0.014699 -0.20011">
+              <inertial pos="-0.0002 0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="fl_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 0.04074 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="fr_hip_abduction_Link" pos="0.32826 -0.065853 0.054034">
+        <inertial pos="0.0488 0.0026 0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="fr_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.611 0.436" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_hip_abduction_Link"/>
+        <body name="fr_hip_pitch_Link" pos="0.06389 0.027311 -0.00036027" quat="0.999976 -0.00686995 0 0">
+          <inertial pos="-0.0019 -0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="fr_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 -0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 -0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="fr_knee_Link" pos="-0.00075079 -0.1035 -0.25" quat="0.999976 0.00686995 0 0">
+            <inertial pos="-0.0002 -0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 0.0001"/>
+            <joint name="fr_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 -0.025 -0.1998" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 -0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="fr_wheel_Link" pos="0 -0.018447 -0.1998">
+              <inertial pos="0.0002 -0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="fr_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 -0.040735 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="rl_hip_abduction_Link" pos="-0.024743 0.066141 0.054034">
+        <inertial pos="-0.0488 -0.0026 -0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="rl_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.436 0.611" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_hip_abduction_Link"/>
+        <body name="rl_hip_pitch_Link" pos="-0.06389 -0.027309 0.00045509">
+          <inertial pos="0.0019 0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="rl_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="rl_knee_Link" pos="0 0.099459 -0.25163">
+            <inertial pos="0.0002 0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 -0.0003"/>
+            <joint name="rl_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 0.025 -0.20027" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="rl_wheel_Link" pos="0 0.012475 -0.20027">
+              <inertial pos="-0.0002 0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="rl_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 0.040737 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="rr_hip_abduction_Link" pos="-0.024743 -0.065884 0.053981">
+        <inertial pos="-0.0488 0.0026 0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="rr_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.611 0.436" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_hip_abduction_Link"/>
+        <body name="rr_hip_pitch_Link" pos="-0.06389 0.027341 0.00041625">
+          <inertial pos="-0.002 -0.1111 -0.0498" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="rr_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 -0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 -0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="rr_knee_Link" pos="-0.00075079 -0.099408 -0.25165">
+            <inertial pos="-0.0002 -0.0225 -0.1541" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 -0.0001"/>
+            <joint name="rr_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 -0.025 -0.20027" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 -0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="rr_wheel_Link" pos="0 -0.012435 -0.20027">
+              <inertial pos="0.0002 -0.0407 -0.0005" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="rr_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 -0.040737 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="imu_link" pos="0.1518 0 0.127">
+        <inertial pos="0 0 0" mass="0" diaginertia="0 0 0"/>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <general name="fl_hip_abduction_joint" joint="fl_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fl_hip_pitch_joint" joint="fl_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fl_knee_joint" joint="fl_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fl_wheel_joint" joint="fl_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+    <general name="fr_hip_abduction_joint" joint="fr_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fr_hip_pitch_joint" joint="fr_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fr_knee_joint" joint="fr_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fr_wheel_joint" joint="fr_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+    <general name="rl_hip_abduction_joint" joint="rl_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rl_hip_pitch_joint" joint="rl_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rl_knee_joint" joint="rl_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rl_wheel_joint" joint="rl_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+    <general name="rr_hip_abduction_joint" joint="rr_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rr_hip_pitch_joint" joint="rr_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rr_knee_joint" joint="rr_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rr_wheel_joint" joint="rr_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+  </actuator>
+</mujoco>

05_software/real/sim2real/policy/policy_runner.py

@@ -0,0 +1,176 @@
+from pathlib import Path
+
+import numpy as np
+import torch
+import torch.nn as nn
+
+
+class PolicyMLP(nn.Module):
+    def __init__(self, obs_dim: int, action_dim: int):
+        super().__init__()
+        self.register_buffer("obs_mean", torch.zeros(obs_dim))
+        self.register_buffer("obs_std", torch.ones(obs_dim))
+        self.net = nn.Sequential(
+            nn.Linear(obs_dim, 512),
+            nn.ELU(),
+            nn.Linear(512, 256),
+            nn.ELU(),
+            nn.Linear(256, 128),
+            nn.ELU(),
+            nn.Linear(128, action_dim),
+        )
+
+    def forward(self, x: torch.Tensor) -> torch.Tensor:
+        x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6)
+        return self.net(x)
+
+
+def load_policy(model_path: Path, device: torch.device) -> PolicyMLP:
+    checkpoint = torch.load(model_path, map_location=device, weights_only=False)
+    state_dict = checkpoint["actor_state_dict"]
+
+    input_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight"
+    output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight"
+    obs_dim = int(state_dict[input_key].shape[1])
+    action_dim = int(state_dict[output_key].shape[0])
+
+    model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim)
+    remapped_state_dict: dict[str, torch.Tensor] = {}
+    for key, value in state_dict.items():
+        if key.startswith("mlp."):
+            remapped_state_dict[key.replace("mlp.", "net.")] = value
+        elif key.startswith("net."):
+            remapped_state_dict[key] = value
+        elif key == "obs_normalizer._mean":
+            remapped_state_dict["obs_mean"] = value.squeeze()
+        elif key == "obs_normalizer._var":
+            remapped_state_dict["obs_std"] = torch.sqrt(value.squeeze() + 1e-5)
+
+    model.load_state_dict(remapped_state_dict, strict=False)
+    model.eval()
+    model.to(device)
+    model.expected_obs_dim = obs_dim
+    model.expected_action_dim = action_dim
+    return model
+
+
+class PolicyRunner:
+    BASE_OBS_DIM = 53
+    DEFAULT_STAND_POSE = np.array(
+        [
+            0.0, 0.9, -1.8,
+            0.0, 0.9, -1.8,
+            0.0, 0.9, -1.8,
+            0.0, 0.9, -1.8,
+            0.0, 0.0, 0.0, 0.0,
+        ],
+        dtype=np.float32,
+    )
+
+    def __init__(
+        self,
+        policy_path: Path,
+        device: torch.device | None = None,
+        enable_zero_cmd_suppression: bool = True,
+        hold_zero_command_pose: bool = True,
+        command_release_s: float = 0.35,
+        action_scale: np.ndarray | None = None,
+        zero_cmd_use_yaw_rate: bool = True,
+    ):
+        self.device = device or torch.device("cuda" if torch.cuda.is_available() else "cpu")
+        self.policy_path = Path(policy_path)
+        self.enable_zero_cmd_suppression = bool(enable_zero_cmd_suppression)
+        self.hold_zero_command_pose = bool(hold_zero_command_pose)
+        self.command_release_s = max(float(command_release_s), 1e-3)
+        print(f"[PolicyRunner] device={self.device}, policy={self.policy_path}")
+        self.policy = load_policy(self.policy_path, self.device)
+        if self.policy.expected_obs_dim != self.BASE_OBS_DIM:
+            raise ValueError(
+                f"Unsupported policy obs dim {self.policy.expected_obs_dim}. "
+                f"Current sim2real only supports {self.BASE_OBS_DIM}-D actor observations."
+            )
+
+        self.default_dof_pos = self.DEFAULT_STAND_POSE.copy()
+        self.last_actions = np.zeros(16, dtype=np.float32)
+
+        self.action_scale = np.asarray(
+            action_scale
+            if action_scale is not None
+            else [
+                0.125, 0.25, 0.25,
+                0.125, 0.25, 0.25,
+                0.125, 0.25, 0.25,
+                0.125, 0.25, 0.25,
+                5.0, 5.0, 5.0, 5.0,
+            ],
+            dtype=np.float32,
+        )
+        if self.action_scale.shape != (16,):
+            raise ValueError(f"action_scale must be shape (16,), got {self.action_scale.shape}")
+
+        self.zero_cmd_lin_thresh = 0.05
+        self.zero_cmd_yaw_thresh = 0.05
+        self.zero_yaw_rate_thresh = 0.10
+        self.zero_cmd_use_yaw_rate = bool(zero_cmd_use_yaw_rate)
+        self._command_release_alpha = 0.0
+        print(
+            f"[PolicyRunner] obs_dim={self.policy.expected_obs_dim}, "
+            f"base_obs_dim={self.BASE_OBS_DIM}, history=1, "
+            f"action_dim={self.policy.expected_action_dim}, "
+            f"zero_cmd_suppression={self.enable_zero_cmd_suppression}, "
+            f"hold_zero_command_pose={self.hold_zero_command_pose}"
+        )
+
+    def reset(self, prime_obs: np.ndarray | None = None) -> None:
+        self.last_actions = np.zeros(16, dtype=np.float32)
+        self._command_release_alpha = 0.0
+
+    def _is_zero_command(self, command: np.ndarray, base_ang_vel: np.ndarray) -> bool:
+        cmd_is_zero = (
+            np.linalg.norm(command[:2]) < self.zero_cmd_lin_thresh
+            and abs(command[2]) < self.zero_cmd_yaw_thresh
+        )
+        if not self.zero_cmd_use_yaw_rate:
+            return cmd_is_zero
+        return cmd_is_zero and abs(base_ang_vel[2]) < self.zero_yaw_rate_thresh
+
+    def command_activation_metrics(self, command: np.ndarray) -> tuple[float, float]:
+        command = np.asarray(command, dtype=np.float32)
+        planar = float(np.linalg.norm(command[:2]))
+        yaw = float(abs(command[2]))
+        return planar, yaw
+
+    def is_command_active(self, command: np.ndarray) -> bool:
+        planar, yaw = self.command_activation_metrics(command)
+        return planar >= self.zero_cmd_lin_thresh or yaw >= self.zero_cmd_yaw_thresh
+
+    def step(self, obs: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+        obs = np.asarray(obs, dtype=np.float32)
+        expected_obs_dim = int(self.policy.expected_obs_dim)
+        if obs.shape[0] != expected_obs_dim:
+            raise ValueError(
+                f"Observation dim mismatch: got {obs.shape[0]}, expected {expected_obs_dim}."
+            )
+
+        obs_tensor = torch.tensor(obs, dtype=torch.float32, device=self.device).unsqueeze(0)
+        with torch.no_grad():
+            raw_actions = self.policy(obs_tensor).squeeze(0).cpu().numpy()
+
+        raw_actions = np.clip(raw_actions, -10.0, 10.0).astype(np.float32)
+        command = obs[6:9]
+        base_ang_vel = obs[0:3] / 0.25
+        zero_command = self._is_zero_command(command, base_ang_vel)
+        if zero_command:
+            self._command_release_alpha = 0.0
+            if self.hold_zero_command_pose:
+                raw_actions[:] = 0.0
+            elif self.enable_zero_cmd_suppression:
+                raw_actions[12:16] = 0.0
+                raw_actions[:12] *= 0.5
+        else:
+            self._command_release_alpha = min(1.0, self._command_release_alpha + 0.02 / self.command_release_s)
+            raw_actions *= self._command_release_alpha
+
+        self.last_actions = raw_actions.copy()
+        scaled_actions = raw_actions * self.action_scale
+        return scaled_actions, raw_actions

05_software/real/sim2real/requirements-orin.txt

@@ -0,0 +1,7 @@
+numpy
+PyYAML
+torch
+pyserial
+
+# Optional:
+# pynput        # only needed for CLI keyboard control

05_software/real/sim2real/safety/runtime_guard.py

@@ -0,0 +1,78 @@
+"""通用运行期守护：每个控制周期调用一次，无副作用，只做检查。
+
+设计原则：
+- 守护函数本身不下发动作、不打印（除非 verbose），只返回判定
+- 调用方决定收到 GuardStop 时怎么办（damping_brake 或 raise）
+- 起立期 / 等待期 / 主循环都共用同一组检查
+"""
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Optional
+
+import numpy as np
+
+
+class GuardLevel(IntEnum):
+    OK = 0
+    WARN = 1       # 仅记录，不停
+    STOP = 2       # 主调方应立刻 damping_brake + 退出当前阶段
+
+
+@dataclass
+class GuardDecision:
+    level: GuardLevel
+    reason: str  # 触发时人类可读说明，OK 时为空
+
+
+class RuntimeGuard:
+    """启动/起立/主循环共用的安全守护。
+
+    不监控目标位置范围（那是 SafetyMonitor 的职责）。这里只关心
+    机身整体状态：是否倾倒、是否翻滚、是否检测到 NaN、用户是否按急停。
+    """
+
+    def __init__(self,
+                 max_ang_vel: float = 12.0,
+                 max_tilt_z: float = -0.30,
+                 imu_age_warn_ms: float = 60.0,
+                 imu_age_stop_ms: float = 200.0):
+        self.max_ang_vel = max_ang_vel
+        self.max_tilt_z = max_tilt_z
+        self.imu_age_warn_ms = imu_age_warn_ms
+        self.imu_age_stop_ms = imu_age_stop_ms
+
+    def check(self,
+              imu_gyro: np.ndarray,
+              projected_gravity: np.ndarray,
+              imu_age_ms: float,
+              estop_triggered: bool,
+              extra_nan_arrays: tuple = ()) -> GuardDecision:
+        # 1) 用户急停
+        if estop_triggered:
+            return GuardDecision(GuardLevel.STOP, "user E-stop")
+
+        # 2) NaN 检查（任意输入数组中出现 NaN）
+        for arr in (imu_gyro, projected_gravity, *extra_nan_arrays):
+            if arr is None:
+                continue
+            if np.any(np.isnan(arr)) or np.any(np.isinf(arr)):
+                return GuardDecision(GuardLevel.STOP, "NaN/Inf detected in observation/action")
+
+        # 3) IMU 数据陈旧
+        if imu_age_ms > self.imu_age_stop_ms:
+            return GuardDecision(GuardLevel.STOP, f"IMU stale {imu_age_ms:.0f}ms")
+        warned_imu = imu_age_ms > self.imu_age_warn_ms
+
+        # 4) 倾倒
+        if projected_gravity[2] > self.max_tilt_z:
+            return GuardDecision(GuardLevel.STOP,
+                                 f"tilt: g_z={projected_gravity[2]:.3f}")
+
+        # 5) 角速度爆表
+        ang_norm = float(np.linalg.norm(imu_gyro))
+        if ang_norm > self.max_ang_vel:
+            return GuardDecision(GuardLevel.STOP, f"ang_vel overflow: |w|={ang_norm:.2f}")
+
+        if warned_imu:
+            return GuardDecision(GuardLevel.WARN, f"IMU age {imu_age_ms:.0f}ms")
+        return GuardDecision(GuardLevel.OK, "")

05_software/real/sim2real/safety/safety_monitor.py

@@ -0,0 +1,107 @@
+"""三级安全监控（对应方法论 97.11）。
+
+Level 0: 正常
+Level 1: 限幅（位置/速度异常）— 截断目标位置幅值，记录连续触发次数
+Level 2: 刹车（连续限幅 N 次 / IMU 角速度过大 / 倾倒）— 卸载刚度只留阻尼
+Level 3: 急停（用户触发）— 让上层断电
+
+设计原则：监控只判定，不直接关电机；返回 SafetyDecision 由上层决策。
+"""
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any, Optional
+
+import numpy as np
+
+
+class SafetyLevel(IntEnum):
+    NORMAL = 0
+    CLIP = 1
+    BRAKE = 2
+    ESTOP = 3
+
+
+@dataclass
+class SafetyDecision:
+    level: SafetyLevel
+    message: str
+    clipped_target: Optional[np.ndarray]
+    details: Optional[dict[str, Any]] = None
+
+
+class SafetyMonitor:
+    """安全监控（按 50Hz 控制频率调用）。
+
+    Args:
+        max_target_offset: 单关节相对默认位姿的最大偏离 (rad)
+        max_ang_vel: IMU 角速度模 (rad/s)
+        max_tilt_rad: 机身重力 z 轴投影低于该值认为已严重倾倒
+        clip_to_brake: 连续 clip 多少帧升级为刹车
+    """
+
+    def __init__(self,
+                 max_target_offset: float = 0.6,
+                 max_ang_vel: float = 10.0,
+                 max_tilt_z: float = -0.3,
+                 clip_to_brake: int = 3):
+        self.max_target_offset = max_target_offset
+        self.max_ang_vel = max_ang_vel
+        self.max_tilt_z = max_tilt_z  # projected_gravity z 应当 ~ -1，明显小于 -0.3 视作倾倒
+        self.clip_to_brake = clip_to_brake
+        self.consecutive_clips = 0
+
+    def check(self,
+              target_pose: np.ndarray,
+              default_pose: np.ndarray,
+              imu_gyro: np.ndarray,
+              projected_gravity: np.ndarray,
+              estop_triggered: bool) -> SafetyDecision:
+        if estop_triggered:
+            return SafetyDecision(SafetyLevel.ESTOP, "user E-stop", None, None)
+
+        # 倾倒（projected_gravity[2] 应在 -1 附近，越接近 0 越倾斜）
+        if projected_gravity[2] > self.max_tilt_z:
+            return SafetyDecision(
+                SafetyLevel.BRAKE,
+                f"tilt detected: g_z={projected_gravity[2]:.3f}",
+                None,
+                {"g_z": float(projected_gravity[2])},
+            )
+
+        # 角速度爆表（猛烈翻滚）
+        if np.linalg.norm(imu_gyro) > self.max_ang_vel:
+            return SafetyDecision(
+                SafetyLevel.BRAKE,
+                f"angular velocity overflow: |w|={np.linalg.norm(imu_gyro):.2f}",
+                None,
+                {"ang_vel_norm": float(np.linalg.norm(imu_gyro))},
+            )
+
+        # 目标位置偏离过大 → 截断到允许范围
+        offset_leg = target_pose[:12] - default_pose[:12]
+        clipped_offset = np.clip(offset_leg, -self.max_target_offset, self.max_target_offset)
+        if not np.allclose(offset_leg, clipped_offset):
+            self.consecutive_clips += 1
+            clipped = target_pose.copy()
+            clipped[:12] = default_pose[:12] + clipped_offset
+            exceeded = np.where(np.abs(offset_leg) > self.max_target_offset)[0].tolist()
+            max_offset = float(np.max(np.abs(offset_leg)))
+            details = {
+                "joint_indices": exceeded,
+                "max_leg_offset": max_offset,
+                "consecutive_clips": int(self.consecutive_clips),
+            }
+            if self.consecutive_clips >= self.clip_to_brake:
+                return SafetyDecision(
+                    SafetyLevel.BRAKE,
+                    f"clipped {self.consecutive_clips} frames in a row",
+                    clipped,
+                    details,
+                )
+            return SafetyDecision(SafetyLevel.CLIP, "target leg offset out of range", clipped, details)
+
+        self.consecutive_clips = 0
+        return SafetyDecision(SafetyLevel.NORMAL, "", None, None)
+
+    def reset(self):
+        self.consecutive_clips = 0

05_software/real/sim2real/startup/pose_initializer.py

@@ -0,0 +1,329 @@
+"""起立姿态初始化器（实测起点版本）。
+
+设计：
+  - 不再假设机器人的物理起始姿态（不再有 CRAWL_POSE / GROUND_POSE 起点）
+  - enable 后从 io.read_measured_pose() 读 16 关节实测，直接作为插值起点
+  - 余弦插值到 STAND_POSE，transition_time 根据最大偏差自适应
+  - 全程 RuntimeGuard 守护（空格急停/倾倒/翻滚/NaN/IMU 陈旧）
+  - 50Hz 写 LogBundle CSV（phase 字段标识阶段）
+
+Phase 流程：
+  STARTUP_SOFT_HOLD  — 软起步保持实测姿态，kp 从 0.125 渐升到 1.0
+  STARTUP_TRANSITION — 实测起点 → STAND 余弦插值
+  STARTUP_HOLD_AFTER — 站稳后保持 1 秒
+"""
+import time
+from typing import Optional
+
+import numpy as np
+
+from safety.runtime_guard import GuardLevel, RuntimeGuard
+from tools.logger import LogBundle
+from tools.math_utils import get_gravity_orientation
+
+
+# 仅作为目标姿态使用（训练侧 default_dof_pos）
+STAND_POSE = np.array([
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.0, 0.0, 0.0,
+], dtype=np.float32)
+
+
+class PoseInitFailed(RuntimeError):
+    """起立流程触发安全停止。main.py 捕获后立即 damping_brake。"""
+
+
+class PoseInitializer:
+    def __init__(self, real_io, control_dt: float = 0.02,
+                 transition_time_min: float = 2.0,
+                 transition_time_max: float = 6.0,
+                 transition_seconds_per_rad: float = 1.5,
+                 hold_time: float = 1.0,
+                 settle_pos_threshold: float = 0.12,
+                 settle_vel_threshold: float = 0.6,
+                 timeout_extra: float = 3.0,
+                 progress_log_interval: float = 0.5,
+                 ramp_kp_time: float = 1.0,
+                 soft_hold_duration: float = 1.0,
+                 max_dev_warn: float = 1.5,
+                 max_dev_abort: float = 3.0):
+        """
+        Args:
+            transition_time_min/max/_per_rad: 自适应公式
+                t = clip(min, max, max_dev * seconds_per_rad)
+            timeout_extra: 起立超时 = transition_time + timeout_extra
+            soft_hold_duration: 起立前先在实测姿态保持几秒，期间 kp ramp-up
+            max_dev_warn: 最大偏差超过此值打警告（仅日志）
+            max_dev_abort: 最大偏差超过此值直接 PoseInitFailed（拒绝起立）
+        """
+        self.io = real_io
+        self.control_dt = control_dt
+        self.transition_time_min = transition_time_min
+        self.transition_time_max = transition_time_max
+        self.transition_seconds_per_rad = transition_seconds_per_rad
+        self.hold_time = hold_time
+        self.settle_pos_threshold = settle_pos_threshold
+        self.settle_vel_threshold = settle_vel_threshold
+        self.timeout_extra = timeout_extra
+        self.progress_log_interval = progress_log_interval
+        self.ramp_kp_time = ramp_kp_time
+        self.soft_hold_duration = soft_hold_duration
+        self.max_dev_warn = max_dev_warn
+        self.max_dev_abort = max_dev_abort
+
+        self.logger: Optional[LogBundle] = None
+        self.guard: Optional[RuntimeGuard] = None
+        self.keyboard = None
+
+    def attach(self, logger: LogBundle, guard: RuntimeGuard, keyboard):
+        self.logger = logger
+        self.guard = guard
+        self.keyboard = keyboard
+
+    # ---- 通用每周期工作 ----
+    def _tick(self, phase: str, sim_target: np.ndarray, kp_scale: float, next_exec: float):
+        """读状态 → guard 检查 → 写日志 → 锁帧。返回 (state_dict, next_exec)。
+        若 guard.STOP，立即抛 PoseInitFailed。"""
+        loop_t0 = time.perf_counter()
+
+        state = self.io.read_state()
+        proj_g = get_gravity_orientation(state["quat_wxyz"])
+
+        guard_dec = None
+        if self.guard is not None:
+            estop = bool(self.keyboard and self.keyboard.is_estop_triggered())
+            guard_dec = self.guard.check(
+                imu_gyro=state["imu_gyro"],
+                projected_gravity=proj_g,
+                imu_age_ms=float(state["imu_age_ms"]),
+                estop_triggered=estop,
+                extra_nan_arrays=(sim_target, state["joint_pos"], state["joint_vel"]),
+            )
+
+        if self.logger is not None:
+            motor_diag = state.get("motor_stale", {})
+            self.logger.state(
+                phase=phase,
+                joint_pos=state["joint_pos"],
+                joint_vel=state["joint_vel"],
+                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                target_pose=sim_target,
+                raw_action=None,
+                gyro=state["imu_gyro"],
+                accel=state["imu_accel"],
+                quat=state["quat_wxyz"],
+                proj_gravity=proj_g,
+                command=np.zeros(3, dtype=np.float32),
+                imu_age_ms=float(state["imu_age_ms"]),
+                loop_dt_ms=(time.perf_counter() - loop_t0) * 1000.0,
+                safety_level=0,
+                guard_level=int(guard_dec.level) if guard_dec else 0,
+                holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                stale_max=int(motor_diag.get("stale_max", 0)),
+                fresh_count=int(motor_diag.get("fresh_count", 16)),
+                kp_scale=kp_scale,
+                nan_flag=int(np.any(np.isnan(state["joint_pos"]))),
+                kp_leg_cmd=float(self.io.kp_leg * kp_scale),
+                kd_leg_cmd=float(self.io.kd_leg),
+                kd_wheel_cmd=float(self.io.kd_wheel),
+                target_source="startup_hold",
+                guard_reason=guard_dec.reason if guard_dec else "",
+            )
+
+        if guard_dec is not None and guard_dec.level == GuardLevel.STOP:
+            if self.logger:
+                self.logger.event("GUARD_STOP", phase=phase, reason=guard_dec.reason)
+            raise PoseInitFailed(f"[{phase}] {guard_dec.reason}")
+
+        next_exec += self.control_dt
+        slack = next_exec - time.perf_counter()
+        if slack > 0:
+            coarse = slack - 0.002
+            if coarse > 0:
+                time.sleep(coarse)
+            while time.perf_counter() < next_exec:
+                pass
+        else:
+            next_exec = time.perf_counter()
+        return state, next_exec
+
+    # ---- 主入口：从实测姿态起立到 STAND ----
+    def transition_to_stand_from_current(self,
+                                          target_pose: Optional[np.ndarray] = None
+                                          ) -> np.ndarray:
+        """完整起立流程：
+          1. 读实测起点
+          2. 偏差检查（warn / abort）
+          3. SOFT_HOLD：保持实测姿态 + kp ramp-up
+          4. TRANSITION：余弦插值到 target，transition_time 自适应
+          5. HOLD_AFTER：保持 1 秒
+        返回最终 target_pose（供主循环使用）。
+        """
+        if target_pose is None:
+            target_pose = STAND_POSE.copy()
+        target_pose = target_pose.astype(np.float32).copy()
+        target_pose[12:] = 0.0
+
+        # === 1. 读实测起点（要求电机反馈完整）===
+        ok, missing = self.io.wait_feedback_ready(max_attempts=20, poll_interval=0.05)
+        if not ok:
+            msg = f"feedback incomplete: {len(missing)} motors no response: {missing[:4]}"
+            if self.logger:
+                self.logger.event("STARTUP_NO_FEEDBACK",
+                                  missing=[m[2] for m in missing])
+            raise PoseInitFailed(msg)
+
+        start_pose = self.io.read_measured_pose().astype(np.float32).copy()
+        start_pose[12:] = 0.0   # 轮子起点固定为 0 速度
+
+        # === 2. 偏差检查 ===
+        diff = np.abs(start_pose[:12] - target_pose[:12])
+        max_dev = float(np.max(diff))
+        max_dev_joint = int(np.argmax(diff))
+        transition_time = float(np.clip(
+            max_dev * self.transition_seconds_per_rad,
+            self.transition_time_min, self.transition_time_max
+        ))
+        timeout = transition_time + self.timeout_extra
+
+        if self.logger:
+            self.logger.event(
+                "STARTUP_PLAN",
+                start_pose_leg=start_pose[:12].tolist(),
+                target_pose_leg=target_pose[:12].tolist(),
+                max_dev=max_dev,
+                max_dev_joint_idx=max_dev_joint,
+                transition_time=transition_time,
+                timeout=timeout,
+            )
+        print(f"[PoseInit] 实测起点最大偏差 {max_dev:.3f} rad (关节 idx={max_dev_joint}); "
+              f"transition_time={transition_time:.2f}s")
+
+        if max_dev > self.max_dev_abort:
+            raise PoseInitFailed(
+                f"实测起点偏差过大 ({max_dev:.2f} rad > abort 阈值 "
+                f"{self.max_dev_abort})；请检查电机是否在合理姿势"
+            )
+        if max_dev > self.max_dev_warn:
+            print(f"[PoseInit] WARNING 偏差 {max_dev:.2f} rad > {self.max_dev_warn}; "
+                  f"起立可能比较剧烈")
+            if self.logger:
+                self.logger.event("STARTUP_LARGE_DEV", max_dev=max_dev)
+
+        # === 3. SOFT_HOLD：实测姿态 + kp ramp-up ===
+        if self.logger:
+            self.logger.event("STARTUP_SOFT_HOLD_BEGIN",
+                              duration=self.soft_hold_duration,
+                              ramp_kp_time=self.ramp_kp_time,
+                              ramp_kp_min=0.125)
+        n = max(1, int(self.soft_hold_duration / max(self.control_dt, 1e-3)))
+        next_exec = time.perf_counter()
+        t0 = next_exec
+        ramp_min = 0.125
+        for i in range(n):
+            elapsed = time.perf_counter() - t0
+            if elapsed < self.ramp_kp_time:
+                kp_scale = ramp_min + (1.0 - ramp_min) * (elapsed / self.ramp_kp_time)
+            else:
+                kp_scale = 1.0
+            self.io.hold_pose(start_pose, kp_scale=kp_scale)
+            _s, next_exec = self._tick("STARTUP_SOFT_HOLD", start_pose, kp_scale, next_exec)
+        if self.logger:
+            self.logger.event("STARTUP_SOFT_HOLD_END")
+
+        # === 4. TRANSITION：余弦插值 ===
+        if self.logger:
+            self.logger.event("STARTUP_TRANSITION_BEGIN",
+                              transition_time=transition_time, timeout=timeout)
+        print(f"[PoseInit] 起立: transition={transition_time:.2f}s, "
+              f"hold={self.hold_time}s, timeout={timeout:.2f}s")
+
+        t0 = time.perf_counter()
+        last_log = t0
+        reached = False
+        hold_start: Optional[float] = None
+        next_exec = t0
+
+        while True:
+            now = time.perf_counter()
+            elapsed = now - t0
+            phase = min(1.0, elapsed / max(transition_time, 1e-3))
+
+            if elapsed > timeout:
+                if self.logger:
+                    self.logger.event("STARTUP_TIMEOUT", elapsed=elapsed)
+                raise PoseInitFailed(
+                    f"transition timeout after {elapsed:.2f}s, target not reached"
+                )
+
+            blend = 0.5 - 0.5 * np.cos(np.pi * phase)
+            blended = start_pose.astype(np.float32).copy()
+            blended[:12] = start_pose[:12] + blend * (target_pose[:12] - start_pose[:12])
+            blended[12:] = 0.0
+            self.io.hold_pose(blended, kp_scale=1.0)
+            state, next_exec = self._tick("STARTUP_TRANSITION", blended, 1.0, next_exec)
+
+            joint_pos = state["joint_pos"]
+            joint_vel = state["joint_vel"]
+            pos_err = float(np.max(np.abs(joint_pos[:12] - target_pose[:12])))
+            vel_err = float(np.max(np.abs(joint_vel[:12])))
+
+            if now - last_log >= self.progress_log_interval:
+                msg = (f"[PoseInit] phase={phase*100:5.1f}% | "
+                       f"max_pos_err={pos_err:.3f} | max_vel={vel_err:.3f}")
+                print(msg)
+                if self.logger:
+                    self.logger.event("STARTUP_PROGRESS",
+                                      phase=phase, pos_err=pos_err, vel_err=vel_err)
+                last_log = now
+
+            if (phase >= 1.0
+                    and pos_err <= self.settle_pos_threshold
+                    and vel_err <= self.settle_vel_threshold):
+                if not reached:
+                    reached = True
+                    hold_start = now
+                    if self.logger:
+                        self.logger.event("STARTUP_REACHED",
+                                          pos_err=pos_err, vel_err=vel_err)
+                    print(f"[PoseInit] 已到位，保持 {self.hold_time:.2f}s")
+                elif hold_start is not None and now - hold_start >= self.hold_time:
+                    break
+            elif phase >= 1.0:
+                reached = False
+                hold_start = None
+
+        # === 5. HOLD_AFTER ===
+        if self.logger:
+            self.logger.event("STARTUP_HOLD_AFTER_BEGIN", duration=self.hold_time)
+        n_hold = max(1, int(self.hold_time / max(self.control_dt, 1e-3)))
+        next_exec = time.perf_counter()
+        for _ in range(n_hold):
+            self.io.hold_pose(target_pose, kp_scale=1.0)
+            _s, next_exec = self._tick("STARTUP_HOLD_AFTER", target_pose, 1.0, next_exec)
+
+        if self.logger:
+            self.logger.event("STARTUP_TRANSITION_END")
+        print("[PoseInit] 默认站姿初始化完成")
+        return target_pose
+
+    # ---- 等用户回车（外部调用，期间持续保持） ----
+    def hold_until_user_confirm(self, target_pose: np.ndarray, evt) -> bool:
+        """阻塞循环到 evt.is_set()，期间持续 PD 保持站姿、跑 guard、写日志。
+        返回 True 正常确认，False 因 guard.STOP 中止。"""
+        if self.logger:
+            self.logger.event("WAIT_USER_BEGIN")
+        next_exec = time.perf_counter()
+        while not evt.is_set():
+            self.io.hold_pose(target_pose, kp_scale=1.0)
+            try:
+                _s, next_exec = self._tick("WAIT_USER", target_pose, 1.0, next_exec)
+            except PoseInitFailed as e:
+                print(f"[PoseInit] WAIT_USER 期间触发停止: {e}")
+                return False
+        if self.logger:
+            self.logger.event("WAIT_USER_END")
+        return True

05_software/real/sim2real/startup/stand_balance.py

@@ -0,0 +1,120 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any, Dict
+
+import numpy as np
+
+
+@dataclass
+class StandBalanceDebug:
+    roll: float
+    pitch: float
+    roll_rate: float
+    pitch_rate: float
+    hip_base: float
+    knee_base: float
+    roll_corr: float
+    pitch_corr: float
+    stable: bool
+
+
+class StandBalanceController:
+    def __init__(self, cfg: Dict[str, Any], control_dt: float):
+        self.enabled = bool(cfg.get("enabled", True))
+        self.control_dt = float(control_dt)
+        self.height = float(cfg.get("height", 0.33))
+        self.kp_roll = float(cfg.get("kp_roll", 0.85))
+        self.kp_pitch = float(cfg.get("kp_pitch", 0.70))
+        self.kd_roll_rate = float(cfg.get("kd_roll_rate", 0.03))
+        self.kd_pitch_rate = float(cfg.get("kd_pitch_rate", 0.025))
+        self.lateral_lean_gain = float(cfg.get("lateral_lean_gain", 0.0))
+        self.hip_abduction_clip = float(cfg.get("hip_abduction_clip", 0.45))
+        self.hip_pitch_clip = tuple(cfg.get("hip_pitch_clip", [-1.0, 2.5]))
+        self.knee_clip = tuple(cfg.get("knee_clip", [-2.6, -0.3]))
+        self.stable_roll_deg = float(cfg.get("stable_roll_deg", 6.0))
+        self.stable_pitch_deg = float(cfg.get("stable_pitch_deg", 8.0))
+        self.stable_gyro_deg_s = float(cfg.get("stable_gyro_deg_s", 45.0))
+        self.enter_hold_s = float(cfg.get("enter_hold_s", 1.0))
+
+        self.profile_h = np.asarray(
+            cfg.get("profile_h", [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]),
+            dtype=np.float32,
+        )
+        self.profile_hip = np.asarray(
+            cfg.get("profile_hip", [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]),
+            dtype=np.float32,
+        )
+        self.profile_knee = np.asarray(
+            cfg.get("profile_knee", [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]),
+            dtype=np.float32,
+        )
+        self._stable_time = 0.0
+        self._last_debug = StandBalanceDebug(0.0, 0.0, 0.0, 0.0, 0.9, -1.8, 0.0, 0.0, False)
+
+    @property
+    def last_debug(self) -> StandBalanceDebug:
+        return self._last_debug
+
+    def reset(self) -> None:
+        self._stable_time = 0.0
+
+    def _estimate_roll_pitch(self, projected_gravity: np.ndarray) -> tuple[float, float]:
+        gx, gy, gz = [float(v) for v in projected_gravity]
+        roll = float(np.arctan2(-gy, max(1e-6, -gz)))
+        pitch = float(np.arctan2(gx, np.sqrt(max(1e-6, gy * gy + gz * gz))))
+        return roll, pitch
+
+    def _base_leg_pose(self) -> tuple[float, float]:
+        h_clamp = float(np.clip(self.height, float(self.profile_h[0]), float(self.profile_h[-1])))
+        hip = float(np.interp(h_clamp, self.profile_h, self.profile_hip))
+        knee = float(np.interp(h_clamp, self.profile_h, self.profile_knee))
+        return hip, knee
+
+    def compute_target(self, state: Dict[str, Any], command: np.ndarray | None = None) -> np.ndarray:
+        projected_gravity = np.asarray(state["projected_gravity"], dtype=np.float32)
+        imu_gyro = np.asarray(state["imu_gyro"], dtype=np.float32)
+        cmd = np.zeros(3, dtype=np.float32) if command is None else np.asarray(command, dtype=np.float32)
+
+        hip_base, knee_base = self._base_leg_pose()
+        roll, pitch = self._estimate_roll_pitch(projected_gravity)
+        roll_rate = float(imu_gyro[0])
+        pitch_rate = float(imu_gyro[1])
+
+        roll_corr = -self.kp_roll * roll - self.kd_roll_rate * roll_rate
+        pitch_corr = -self.kp_pitch * pitch - self.kd_pitch_rate * pitch_rate
+        lateral_lean = self.lateral_lean_gain * float(cmd[1])
+
+        target = np.zeros(16, dtype=np.float32)
+        for leg_idx in range(4):
+            side = 1.0 if leg_idx in (0, 2) else -1.0
+            target[leg_idx * 3 + 0] = float(
+                np.clip(side * roll_corr + lateral_lean, -self.hip_abduction_clip, self.hip_abduction_clip)
+            )
+            target[leg_idx * 3 + 1] = float(
+                np.clip(hip_base + pitch_corr, self.hip_pitch_clip[0], self.hip_pitch_clip[1])
+            )
+            target[leg_idx * 3 + 2] = float(np.clip(knee_base, self.knee_clip[0], self.knee_clip[1]))
+        target[12:] = 0.0
+
+        stable = (
+            abs(np.degrees(roll)) <= self.stable_roll_deg
+            and abs(np.degrees(pitch)) <= self.stable_pitch_deg
+            and max(abs(np.degrees(roll_rate)), abs(np.degrees(pitch_rate))) <= self.stable_gyro_deg_s
+        )
+        self._stable_time = self._stable_time + self.control_dt if stable else 0.0
+        self._last_debug = StandBalanceDebug(
+            roll=roll,
+            pitch=pitch,
+            roll_rate=roll_rate,
+            pitch_rate=pitch_rate,
+            hip_base=hip_base,
+            knee_base=knee_base,
+            roll_corr=roll_corr,
+            pitch_corr=pitch_corr,
+            stable=stable,
+        )
+        return target
+
+    def is_stable(self) -> bool:
+        return self._stable_time >= self.enter_hold_s