Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Scunfu
|
59b0583444 |
-48
@@ -1,48 +0,0 @@
|
||||
# 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
|
||||
~$*
|
||||
@@ -1,14 +0,0 @@
|
||||
# 项目文档
|
||||
|
||||
本目录用于保存 16DOF 轮足项目自身的技术文档和使用说明。
|
||||
|
||||
后续建议按主题组织：
|
||||
|
||||
```text
|
||||
01_doc/
|
||||
├─ architecture/ # 系统架构和数据流
|
||||
├─ control/ # 控制与强化学习原理
|
||||
├─ deployment/ # Sim2Sim 和 Sim2Real 部署
|
||||
├─ hardware/ # 接线、标定和硬件兼容性
|
||||
└─ user_guide/ # 安装、运行和调试说明
|
||||
```
|
||||
@@ -1,22 +0,0 @@
|
||||
# 第一代软件闭环
|
||||
|
||||
第一代 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++ 部署架构。
|
||||
@@ -1,5 +0,0 @@
|
||||
# SolidWorks 源文件
|
||||
|
||||
`solidworks/` 保留原始装配层级和文件名。
|
||||
|
||||
建议使用 `solidworks/WEEKDOG.SLDASM` 作为整机入口。`26版sw单件/` 包含组成整机的单件和子装配体，不应随意改名或拆散。
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,40 +0,0 @@
|
||||
# 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 是否完全一致。
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,11 +0,0 @@
|
||||
# STEP 交换文件
|
||||
|
||||
本目录提供不依赖 SolidWorks 的通用机械交换文件。
|
||||
|
||||
- `WEEKDOG.STEP`：整机
|
||||
- `DOG身体.STEP`：机器人身体
|
||||
- `髋关节END.STEP`：髋关节
|
||||
- `大腿.STEP`：大腿组件
|
||||
- `轮小腿.STEP`：轮足小腿组件
|
||||
- `轮子完整.STEP`：完整轮组
|
||||
- 其余文件：轮子、轮毂垫片和连接零件
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,660 +0,0 @@
|
||||
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;
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1 +0,0 @@
|
||||
机械设计采用26版本sw设计，装配，以及urdf导出，为减少sw装配出现问题，采用单件装配小件各个关节装配体，转step关节文件，step各关节装配总装配体方案
|
||||
@@ -1,5 +0,0 @@
|
||||
# 硬件
|
||||
|
||||
本目录用于保存 16DOF 轮足机器人的自研电路、接线图、BOM、传感器与计算平台说明。
|
||||
|
||||
第三方硬件资料不作为自研成果提交。
|
||||
@@ -1,5 +0,0 @@
|
||||
# 嵌入式固件
|
||||
|
||||
本目录用于保存运行在 MCU 或其他嵌入式控制器上的固件源码和构建工程。
|
||||
|
||||
编译生成的 `.hex`、`.bin`、`.elf`、`.axf` 等文件不进入源码目录，可在需要时作为 Release 附件发布。
|
||||
@@ -1,37 +0,0 @@
|
||||
# 软件
|
||||
|
||||
本目录当前保存 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)
|
||||
@@ -1,21 +0,0 @@
|
||||
# 第一代真机控制
|
||||
|
||||
本目录保存 16DOF 轮足机器人的早期真机控制实现。
|
||||
|
||||
## `ik_real`
|
||||
|
||||
基于几何逆运动学和轨迹插值的真机控制探索，不依赖强化学习策略。主要用于验证电机接口、关节映射和姿态轨迹。
|
||||
|
||||
## `sim2real`
|
||||
|
||||
第一代 Python 策略部署栈，包含：
|
||||
|
||||
- 53D 观测到 16D 动作的策略运行时
|
||||
- 电机映射和真机 IO
|
||||
- IMU 接入
|
||||
- 站立初始化与平衡
|
||||
- 运行时安全检查和阻尼刹车
|
||||
- Web 调试界面
|
||||
- 对齐、标定和独立检查工具
|
||||
|
||||
部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
|
||||
@@ -1,9 +0,0 @@
|
||||
# IK 真机控制探索
|
||||
|
||||
该目录保存强化学习部署前的逆运动学真机控制代码。
|
||||
|
||||
- `sim2real_control_api.py`：真机控制接口
|
||||
- `trajectory_interpolator.py`：关节/姿态轨迹插值
|
||||
- `sim_to_real_deploy_beifen.py`：早期部署脚本备份
|
||||
|
||||
文件名中的 `beifen` 来自原始资料。为保持早期版本可追溯性，本次归档不修改源码和文件名。
|
||||
@@ -1,274 +0,0 @@
|
||||
"""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}")
|
||||
@@ -1,234 +0,0 @@
|
||||
#!/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()
|
||||
@@ -1,237 +0,0 @@
|
||||
#!/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: 最简单但速度会突变")
|
||||
@@ -1,50 +0,0 @@
|
||||
# `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
|
||||
```
|
||||
@@ -1,48 +0,0 @@
|
||||
# `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 模型运行。
|
||||
如果后续模型结构再改，必须重新核对观测、动作缩放、控制频率和部署文档。
|
||||
@@ -1,45 +0,0 @@
|
||||
# `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`
|
||||
@@ -1,77 +0,0 @@
|
||||
# `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
|
||||
@@ -1,74 +0,0 @@
|
||||
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
|
||||
@@ -1,71 +0,0 @@
|
||||
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
|
||||
@@ -1,89 +0,0 @@
|
||||
"""键盘控制器 — 兼容 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()
|
||||
@@ -1,136 +0,0 @@
|
||||
"""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
|
||||
@@ -1,310 +0,0 @@
|
||||
"""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)
|
||||
@@ -1,99 +0,0 @@
|
||||
"""仿真→实机电机映射。
|
||||
|
||||
数据来源：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"
|
||||
@@ -1,135 +0,0 @@
|
||||
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
|
||||
@@ -1,725 +0,0 @@
|
||||
"""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()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,22 +0,0 @@
|
||||
<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>
|
||||
@@ -1,327 +0,0 @@
|
||||
<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>
|
||||
@@ -1,157 +0,0 @@
|
||||
<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>
|
||||
Binary file not shown.
@@ -1,176 +0,0 @@
|
||||
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
|
||||
@@ -1,7 +0,0 @@
|
||||
numpy
|
||||
PyYAML
|
||||
torch
|
||||
pyserial
|
||||
|
||||
# Optional:
|
||||
# pynput # only needed for CLI keyboard control
|
||||
@@ -1,78 +0,0 @@
|
||||
"""通用运行期守护：每个控制周期调用一次，无副作用，只做检查。
|
||||
|
||||
设计原则：
|
||||
- 守护函数本身不下发动作、不打印（除非 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, "")
|
||||
@@ -1,107 +0,0 @@
|
||||
"""三级安全监控（对应方法论 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
|
||||
@@ -1,329 +0,0 @@
|
||||
"""起立姿态初始化器（实测起点版本）。
|
||||
|
||||
设计：
|
||||
- 不再假设机器人的物理起始姿态（不再有 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
|
||||
@@ -1,120 +0,0 @@
|
||||
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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user