diff --git a/CMakeLists.txt b/CMakeLists.txt index 99a7d15..c1e859e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -97,7 +97,14 @@ if(BUILD_TESTING) target_include_directories(runtime_test PRIVATE src ${ONNXRUNTIME_ROOT_DIR}/include) target_link_libraries(runtime_test rmcs_rl_bridge ${ONNXRUNTIME_ROOT_DIR}/lib/libonnxruntime.so) target_compile_definitions(runtime_test PRIVATE - RMCS_RL_IDENTITY_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/normalized_identity.onnx") + RMCS_RL_IDENTITY_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/normalized_identity.onnx" + RMCS_RL_SEQ_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/seq_identity.onnx" + RMCS_RL_SEQ_MISMATCH_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/seq_history_mismatch.onnx" + RMCS_RL_SEQ_LAYOUT_MISMATCH_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/seq_layout_mismatch.onnx" + RMCS_RL_SEQ_LAYOUT_ONLY_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/seq_layout_only.onnx" + RMCS_RL_SEQ_LAYOUT_DYNAMIC_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/seq_layout_dynamic.onnx" + RMCS_RL_EXTRA_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/extra_input_identity.onnx" + RMCS_RL_TYPED_EXTRA_FIXTURE="${PROJECT_SOURCE_DIR}/test/data/typed_extra_input_identity.onnx") endif() ament_export_dependencies(rosidl_default_runtime) diff --git a/README.md b/README.md index 0eb8657..694c161 100644 --- a/README.md +++ b/README.md @@ -52,28 +52,111 @@ ros2 run rmcs_rl policy_server --ros-args --params-file <车辆配置.yaml> ## 模型与契约 -模型输入为 float32 `obs[batch,N]`，输出为 float32 `actions[batch,M]`，batch 为 1 或动态。 +模型结构由 `policy_server.model_type` 或 ONNX metadata `rmcs_model_type` 切换，支持： + +- `mlp`：输入可以是 float32 `obs[1,N]`，也可以是带历史帧的 `obs[1,history,feature]`； +- `transformer`：输入为 float32 `obs[1,history,feature]`； +- `generic`：保留 rank-1/2/3 的形状适配，供后续单输入结构接入。 + +输出支持 rank-1/2/3，只要除 batch 外的元素总数等于动作数 `M`。batch 为 1 或动态。 +`auto` 优先读取 `rmcs_model_type`，没有 metadata 时才按 rank-2 推断 MLP、rank-3 推断 +Transformer。 + +多输入模型（例如独立 attention mask 或 position id）除 `obs` 外的输入必须是常量张量，在 +`policy_server` 段用 `extra_inputs.<输入名>: [元素列表]` 声明；启动时根据 ONNX 输入类型 +创建 `float32`、`int64` 或 `bool` 张量，并校验名字、元素数量和形状；缺声明、类型不匹配 +或声明了模型没有的输入都会拒绝； +输出仍必须是唯一的 `actions`。 + +```yaml +policy_server: + ros__parameters: + extra_inputs: + attn_mask: [true, true, true, true] # bool mask；也支持 int64 position id +``` + 部署模型须含 `rmcs_obs_layout`、`rmcs_actions_layout`；建议带 `policy_layout_hash` 和版本。 `history_length × 单帧维数 = rl_obs_size`。归一化均值和标准差必须成对提供，标准差为正数。 -新模型先核对训练的观测顺序、坐标、基准角、缩放和频率，再盖章： +rank-3 模型的序列切分受强制约束：模型的 `T == v3-history`、`F == rl_obs_size / history_length`， +其中 `v3-history` 来自必需的 `rmcs_obs_layout`，并且最终还会由动作消息的 layout hash 与 +`rl_bridge` 再次核对。`rmcs_history_length`、`rmcs_obs_frame_size` 和 `sequence_length` 是 +启动时的辅助维度声明，缺失或不一致都会拒绝；它们不能替代布局签名。 + +带历史帧的模型示例（桥和策略进程都要使用同一历史长度）： + +```yaml +rl_bridge: + ros__parameters: + history_length: 8 + rl_obs_size: 208 # 8 × 26 +policy_server: + ros__parameters: + model_type: "transformer" # 带历史帧 MLP 这里写 "mlp" + sequence_length: 8 # 动态 rank-3 输入时需要 + feature_size: 26 + rl_obs_size: 208 +``` + +### 模型盖章 + +部署模型必须含 `rmcs_obs_layout`、`rmcs_actions_layout`，建议同时带 `policy_layout_hash` +和版本。盖章按**当前 YAML** 生成签名，所以顺序是先核对训练布局、再盖章、再同步 YAML、 +最后复验。 + +**1. 装依赖（一次性）**：需要一个带 `onnx` 与 `PyYAML` 的 Python。没有就运行： + +```sh +bash src/rmcs_rl/tool/install_model_tools.sh +``` + +它创建 `/opt/rmcs-tools/venv`；已有环境则 `export RMCS_ONNX_PYTHON=/bin/python` 直接跳过安装。 +`stamp_model.sh` 自己会依次找 `RMCS_ONNX_PYTHON`、该 venv、`python3`；下面直接调用 +`python3` 的脚本若系统解释器没装 `onnx`，改用 `/bin/python`。 + +**2. 核对训练布局**：逐条确认观测顺序、坐标、基准角、缩放和频率与当前 YAML 一致。 +YAML 写错会盖出「自洽但错误」的签名，后续校验照样通过，所以这一步不能省。 + +**3. 盖章**：推荐用一键脚本。它先提示模型布局与 YAML 是否一致（不一致只警告不阻断）， +盖完立即复验，最后打印要手工改的 YAML 两行： ```sh bash src/rmcs_rl/tool/stamp_model.sh <模型.onnx> --config <车辆配置.yaml> --version <版本> ``` -按工具输出同步 `policy_server.rl_model_path`、`rl_bridge.expected_model_id`，然后验证： +模型放在 `src/rmcs_rl/models/` 下时只写文件名；`--config` 缺省为 +`src/rmcs_bringup/config/wheel-leg-infantry-rl.yaml`，`--node` 缺省 `rl_bridge`。 +带历史帧或特殊结构时补 `--model-type mlp|transformer|generic`、`--sequence-length N`、 +`--feature-size N`。脚本只盖 metadata，不改计算图/权重，也不改 YAML——YAML 由你手工改最安全。 + +只需写 metadata、不自动复验时用底层脚本。盖章默认写入序列键（`rmcs_history_length`/ +`rmcs_obs_frame_size`，取自 YAML）；部署端还会从必需的 `rmcs_obs_layout` 中读取 +`v3-history`，防止只依赖可选 metadata。`rmcs_model_type` 可选，缺省由部署端按输入 rank 推断： + +```sh +python3 src/rmcs_rl/tool/stamp_layout_metadata.py \ + --model policy.onnx --from-config <车辆配置.yaml> --node rl_bridge \ + --model-type transformer +``` + +**4. 同步 YAML 两处**：按工具输出手工改 `policy_server.rl_model_path` 与 +`rl_bridge.expected_model_id`，不要让脚本改配置文件。 + +**5. 复验**： ```sh python3 src/rmcs_rl/tool/check_policy_contract.py <模型.onnx> --config <车辆配置.yaml> --node rl_bridge --expect-model-id ``` +**6. 重启 RMCS**：更新模型或配置后必须重启。模型或布局不匹配会锁存拒绝动作，需要重启恢复。 + 盖章只写元数据；布局匹配不能证明训练侧的坐标或动作含义正确。 -更新模型或配置后重启 RMCS。模型或布局不匹配会锁存拒绝动作，需要重启恢复。 -当前 `models/deformable_sps_V2.onnx` 缺少 `rmcs_obs_layout` 和 `rmcs_actions_layout`， -完整契约检查与策略服务会拒绝它。这是已有模型的部署缺口，需核对训练契约后补元数据； -不能只按张量维数推断其含义。 +当前部署使用 `models/deformable_v2_transformer.onnx`（8 帧历史 × 32 维单帧，输入 rank-3 +`obs[1,8,32]`，`rmcs_model_type=transformer`）；其 `rmcs_obs_layout`、 +`rmcs_actions_layout`、`policy_layout_hash` 和 `policy_version` 已按 +`deformable-infantry-omni-rl.yaml` 盖章。盖章只确认运行时布局契约，不能只按张量维数 +推断训练侧的坐标或动作含义；更新模型或 YAML 后必须重新盖章并同步 `expected_model_id`。 ## 运行接口 @@ -94,5 +177,8 @@ colcon test --merge-install --packages-select rmcs_rl rmcs_core PYTHON=<带 onnx、onnxruntime、PyYAML 的 Python> bash src/rmcs_rl/test/test_layout_contract.sh ``` -C++ 回归覆盖推理归一化、历史帧与跨线程动作快照；Python 回归覆盖布局解析、盖章及错误契约拒绝。 +C++ 回归覆盖推理归一化、历史帧、rank-3 序列输入、float32/int64/bool extra_inputs 常量喂入与错配拒绝、 +跨线程动作快照；Python 回归覆盖布局解析、盖章、T==history 强制、model_type/rank 匹配、 +多输入声明对账及错误契约拒绝。夹具由 `test/gen_seq_fixtures.py`、 +`test/gen_extra_input_fixture.py` 和 `test/gen_typed_extra_input_fixture.py` 确定性生成后提交。 车辆包保留 DM 使能、帧解码、闭链几何、Body IMU 以及 MuJoCo 查看器回归。 diff --git a/include/rmcs_rl/rl_bridge/action_feedback_history.hpp b/include/rmcs_rl/rl_bridge/action_feedback_history.hpp new file mode 100644 index 0000000..9ca937a --- /dev/null +++ b/include/rmcs_rl/rl_bridge/action_feedback_history.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_rl { + +// Prior-action features retain their training delay when inference is faster. +class ActionFeedbackHistory { +public: + using Clock = std::chrono::steady_clock; + explicit ActionFeedbackHistory(std::size_t size) : values_(size, 0.0) {} + + void store(Clock::time_point observation_time, std::span action) { + if (action.size() != values_.size()) + throw std::invalid_argument("action feedback size mismatch"); + if (!samples_.empty() && observation_time <= samples_.back().time) + return; + samples_.push_back({observation_time, std::vector{action.begin(), action.end()}}); + } + + const std::vector& sample(Clock::time_point now, std::chrono::nanoseconds delay) { + const auto target = now - delay; + while (samples_.size() > 1 && samples_[1].time <= target) + samples_.pop_front(); + if (!samples_.empty() && samples_.front().time <= target) + values_ = samples_.front().action; + return values_; + } + + void reset() { + samples_.clear(); + std::fill(values_.begin(), values_.end(), 0.0); + } + +private: + struct Sample { + Clock::time_point time; + std::vector action; + }; + std::deque samples_; + std::vector values_; +}; + +} // namespace rmcs_rl diff --git a/include/rmcs_rl/rl_bridge/config.hpp b/include/rmcs_rl/rl_bridge/config.hpp index 7b5ab24..5d17a36 100644 --- a/include/rmcs_rl/rl_bridge/config.hpp +++ b/include/rmcs_rl/rl_bridge/config.hpp @@ -24,11 +24,12 @@ struct BridgeConfig { double policy_rate = 50.0; double max_action_age = 0.04; std::chrono::nanoseconds pub_period{}; - std::uint64_t expected_model_id = 0; + std::chrono::nanoseconds history_period{}; InvalidMode invalid_mode = InvalidMode::kNaN; std::string enable_path; bool enable_default = false; std::string reset_path; + std::string flat_hold_path; }; BridgeConfig load_bridge_config(rclcpp::Node& node); diff --git a/include/rmcs_rl/rl_bridge/observation_history.hpp b/include/rmcs_rl/rl_bridge/observation_history.hpp index b08f31d..a6456ba 100644 --- a/include/rmcs_rl/rl_bridge/observation_history.hpp +++ b/include/rmcs_rl/rl_bridge/observation_history.hpp @@ -1,6 +1,8 @@ #pragma once #include +#include +#include #include #include @@ -9,13 +11,22 @@ namespace rmcs_rl { // Frames are oldest first; startup and reset fill the history with the first valid frame. class ObservationHistory { public: - ObservationHistory(std::size_t frame_size, std::size_t history_length); + using Clock = std::chrono::steady_clock; + ObservationHistory(std::size_t frame_size, std::size_t history_length, + std::chrono::nanoseconds sample_period = {}); const std::vector& append(std::span frame); + const std::vector& append(std::span frame, Clock::time_point now); void reset(); private: std::size_t frame_size_; std::vector values_; + struct Sample { + Clock::time_point time; + std::vector frame; + }; + std::chrono::nanoseconds sample_period_{}; + std::deque samples_; bool initialized_ = false; }; diff --git a/include/rmcs_rl/rl_bridge/observation_timeline.hpp b/include/rmcs_rl/rl_bridge/observation_timeline.hpp index bd322d2..43b0261 100644 --- a/include/rmcs_rl/rl_bridge/observation_timeline.hpp +++ b/include/rmcs_rl/rl_bridge/observation_timeline.hpp @@ -11,10 +11,14 @@ class ObservationTimeline { using Clock = std::chrono::steady_clock; bool due(Clock::time_point now, std::chrono::nanoseconds period) const { - return !started_ || now - current_time_ >= period; + return !started_ || now - scheduled_time_ >= period; } - std::uint64_t publish(Clock::time_point now) { + std::uint64_t publish(Clock::time_point now, std::chrono::nanoseconds period = {}) { + if (!started_ || period.count() <= 0) + scheduled_time_ = now; + else + scheduled_time_ += period * ((now - scheduled_time_) / period); previous_valid_ = started_; previous_time_ = current_time_; current_time_ = now; @@ -34,6 +38,7 @@ class ObservationTimeline { // Keep IDs monotonic so a delayed action cannot match a new frame after reset. started_ = previous_valid_ = false; current_time_ = previous_time_ = {}; + scheduled_time_ = {}; } [[nodiscard]] std::uint64_t sequence() const { return sequence_; } @@ -41,6 +46,7 @@ class ObservationTimeline { private: Clock::time_point current_time_{}; Clock::time_point previous_time_{}; + Clock::time_point scheduled_time_{}; std::uint64_t sequence_ = 0; bool started_ = false; bool previous_valid_ = false; diff --git a/models/deformable_sps_V1.onnx b/models/deformable_sps_V1.onnx index d0308f3..5eaaae9 100644 Binary files a/models/deformable_sps_V1.onnx and b/models/deformable_sps_V1.onnx differ diff --git a/models/deformable_sps_V2.onnx b/models/deformable_sps_V2.onnx deleted file mode 100644 index 2d092a8..0000000 Binary files a/models/deformable_sps_V2.onnx and /dev/null differ diff --git a/models/deformable_v2_transformer.onnx b/models/deformable_v2_transformer.onnx new file mode 100644 index 0000000..33ad4b5 Binary files /dev/null and b/models/deformable_v2_transformer.onnx differ diff --git a/models/deformable_v3_transformer.onnx b/models/deformable_v3_transformer.onnx new file mode 100644 index 0000000..b2cf8d6 Binary files /dev/null and b/models/deformable_v3_transformer.onnx differ diff --git a/models/wheel_leg_v1.onnx b/models/wheel_leg_v1_MLP.onnx similarity index 100% rename from models/wheel_leg_v1.onnx rename to models/wheel_leg_v1_MLP.onnx diff --git a/models/wheel_leg_v3.onnx b/models/wheel_leg_v3.onnx new file mode 100644 index 0000000..e7d7e28 Binary files /dev/null and b/models/wheel_leg_v3.onnx differ diff --git a/models/wheel_leg_v3_test.onnx b/models/wheel_leg_v3_test.onnx new file mode 100644 index 0000000..9b897fc Binary files /dev/null and b/models/wheel_leg_v3_test.onnx differ diff --git a/src/policy/onnx_runtime.cpp b/src/policy/onnx_runtime.cpp index 2ea9857..e7d63a3 100644 --- a/src/policy/onnx_runtime.cpp +++ b/src/policy/onnx_runtime.cpp @@ -1,51 +1,427 @@ #include "onnx_runtime.hpp" #include +#include +#include +#include +#include +#include #include +#include namespace rmcs_rl { +namespace { + +std::string lower(std::string value) { + std::transform(value.begin(), value.end(), value.begin(), [](unsigned char character) { + return static_cast(std::tolower(character)); + }); + return value; +} + +std::size_t parse_positive(const std::optional& value, const char* key) { + if (!value || value->empty()) + return 0; + std::size_t consumed = 0; + std::size_t result = 0; + try { + result = std::stoull(*value, &consumed); + } catch (const std::exception&) { + throw std::invalid_argument(std::string{key} + " metadata must be a positive integer"); + } + if (consumed != value->size() || result == 0) + throw std::invalid_argument(std::string{key} + " metadata must be a positive integer"); + return result; +} + +bool batch_dimension_ok(std::int64_t dimension) { + // ONNX uses -1 for a dynamic dimension. Symbolic dimensions are returned + // as -1 by the C++ API, so this also covers named batch dimensions. + return dimension == 1 || dimension == -1; +} + +struct LayoutDimensions { + std::size_t history = 0; + std::size_t feature = 0; + std::size_t observation = 0; +}; + +std::optional layout_dimensions(const std::optional& value) { + if (!value || value->empty()) + return std::nullopt; + constexpr std::string_view prefix = "v3-history="; + const std::string_view signature = *value; + if (!signature.starts_with(prefix)) + return std::nullopt; + + const auto header_end = signature.find('|', prefix.size()); + if (header_end == std::string_view::npos) + throw std::invalid_argument("rmcs_obs_layout is missing observation entries"); + const auto history_text = signature.substr(prefix.size(), header_end - prefix.size()); + LayoutDimensions result; + const auto [history_end, history_error] = std::from_chars( + history_text.data(), history_text.data() + history_text.size(), result.history); + if (history_error != std::errc{} + || history_end != history_text.data() + history_text.size() || result.history == 0) + throw std::invalid_argument("rmcs_obs_layout history must be a positive integer"); + + std::size_t begin = header_end + 1; + while (begin < signature.size()) { + const auto end = signature.find('|', begin); + const auto entry = signature.substr( + begin, end == std::string_view::npos ? std::string_view::npos : end - begin); + const auto at = entry.rfind('@'); + if (at == std::string_view::npos || at + 1 == entry.size()) + throw std::invalid_argument("rmcs_obs_layout entry is missing @dim"); + std::size_t dimension = 0; + const auto dim_text = entry.substr(at + 1); + const auto [dim_end, dim_error] = std::from_chars( + dim_text.data(), dim_text.data() + dim_text.size(), dimension); + if (dim_error != std::errc{} || dim_end != dim_text.data() + dim_text.size() + || dimension == 0) + throw std::invalid_argument("rmcs_obs_layout entry dimension must be positive"); + if (dimension > std::numeric_limits::max() - result.feature) + throw std::invalid_argument("rmcs_obs_layout dimensions overflow"); + result.feature += dimension; + if (end == std::string_view::npos) + break; + begin = end + 1; + } + if (result.feature == 0 || result.history > std::numeric_limits::max() / result.feature) + throw std::invalid_argument("rmcs_obs_layout dimensions overflow"); + result.observation = result.history * result.feature; + return result; +} + +} // namespace + +OnnxRuntime::OnnxRuntime(const Config& config) + : input_name_(config.input_name) + , output_name_(config.output_name) { + if (config.model_path.empty()) + throw std::invalid_argument("model path must not be empty"); + const auto requested_type = lower(config.model_type); + if (requested_type != "auto" && requested_type != "mlp" && requested_type != "transformer" + && requested_type != "generic") + throw std::invalid_argument( + "model_type must be auto, mlp, transformer, or generic (got '" + config.model_type + + "')"); -OnnxRuntime::OnnxRuntime( - const std::string& model_path, const std::string& input_name, const std::string& output_name) - : input_name_(input_name) - , output_name_(output_name) { Ort::SessionOptions options; options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); options.SetIntraOpNumThreads(1); options.SetInterOpNumThreads(1); - session_ = Ort::Session{env_, model_path.c_str(), options}; + session_ = Ort::Session{env_, config.model_path.c_str(), options}; - if (session_.GetInputCount() != 1 || session_.GetOutputCount() != 1) - throw std::invalid_argument("model must have exactly one input and one output"); - const auto actual_input = session_.GetInputNameAllocated(0, allocator_); + if (session_.GetOutputCount() != 1) + throw std::invalid_argument("model must have exactly one output"); + const std::size_t input_count = session_.GetInputCount(); + if (input_count == 0) + throw std::invalid_argument("model must have at least one input"); + + input_names_.reserve(input_count); + std::size_t primary_index = input_count; // sentinel: not found + for (std::size_t index = 0; index < input_count; ++index) { + const auto actual_input = session_.GetInputNameAllocated(index, allocator_); + input_names_.emplace_back(actual_input.get()); + if (input_names_.back() == input_name_) + primary_index = index; + } + if (primary_index == input_count) { + std::string listing; + for (std::size_t index = 0; index < input_count; ++index) + listing += (index == 0 ? "" : ", ") + input_names_[index]; + throw std::invalid_argument( + "primary input '" + input_name_ + "' not found; model inputs: [" + listing + "]"); + } const auto actual_output = session_.GetOutputNameAllocated(0, allocator_); - if (actual_input.get() != input_name_ || actual_output.get() != output_name_) + if (actual_output.get() != output_name_) throw std::invalid_argument( - "tensor names must be '" + input_name_ + "' / '" + output_name_ + "', got '" - + actual_input.get() + "' / '" + actual_output.get() + "'"); - - const auto input_type = session_.GetInputTypeInfo(0); + "output tensor must be named '" + output_name_ + "', got '" + actual_output.get() + + "'"); + // Keep TypeInfo alive while reading the TensorTypeAndShapeInfo. The + // latter is a view into TypeInfo in ONNX Runtime; chaining a temporary + // here can read freed type metadata on some runtime builds. + const auto input_type = session_.GetInputTypeInfo(primary_index); const auto output_type = session_.GetOutputTypeInfo(0); + if (input_type.GetONNXType() != ONNX_TYPE_TENSOR || output_type.GetONNXType() != ONNX_TYPE_TENSOR) + throw std::invalid_argument("obs and actions must be tensors"); const auto input_info = input_type.GetTensorTypeAndShapeInfo(); const auto output_info = output_type.GetTensorTypeAndShapeInfo(); if (input_info.GetElementType() != ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT || output_info.GetElementType() != ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT) - throw std::invalid_argument("tensor element type must be float32"); - - const auto input_shape = input_info.GetShape(); - const auto output_shape = output_info.GetShape(); - if (input_shape.size() != 2 || output_shape.size() != 2) - throw std::invalid_argument("tensor rank must be 2 ([batch, N])"); - if ((input_shape[0] != 1 && input_shape[0] != -1) - || (output_shape[0] != 1 && output_shape[0] != -1)) - throw std::invalid_argument("batch dimension must be 1 or dynamic"); - if (input_shape[1] <= 0 || output_shape[1] <= 0) - throw std::invalid_argument("feature dimensions must be concrete positive values"); - - // The server evaluates one robot observation, even when the exported batch is dynamic. - input_shape_ = {1, input_shape[1]}; - input_buffer_.resize(static_cast(input_shape[1])); - output_size_ = static_cast(output_shape[1]); + throw std::invalid_argument("obs and actions element types must be float32"); + + const auto raw_input_shape = input_info.GetShape(); + const auto raw_output_shape = output_info.GetShape(); + if (raw_input_shape.empty() || raw_input_shape.size() > 3) + throw std::invalid_argument( + "supported policy input ranks are 1 ([N]), 2 ([batch,N]), and 3 ([batch,sequence,feature])"); + if (raw_input_shape.size() >= 2 && !batch_dimension_ok(raw_input_shape[0])) + throw std::invalid_argument("input batch dimension must be 1 or dynamic"); + if (raw_output_shape.empty() || raw_output_shape.size() > 3) + throw std::invalid_argument("supported policy output ranks are 1, 2, or 3"); + if (raw_output_shape.size() >= 2 && !batch_dimension_ok(raw_output_shape[0])) + throw std::invalid_argument("output batch dimension must be 1 or dynamic"); + + const auto metadata_type = lower(metadata("rmcs_model_type").value_or("")); + if (!metadata_type.empty() && metadata_type != "mlp" && metadata_type != "transformer" + && metadata_type != "generic") + throw std::invalid_argument("rmcs_model_type metadata must be mlp, transformer, or generic"); + if (requested_type != "auto" && requested_type != "generic" && !metadata_type.empty() + && requested_type != metadata_type) + throw std::invalid_argument( + "model_type parameter '" + requested_type + "' disagrees with rmcs_model_type='" + + metadata_type + "'"); + + std::string resolved_type = requested_type; + if (resolved_type == "auto") + resolved_type = metadata_type; + if (resolved_type.empty()) { + if (raw_input_shape.size() == 2) + resolved_type = "mlp"; + else if (raw_input_shape.size() == 3) + resolved_type = "transformer"; + else + resolved_type = "generic"; + } + if (resolved_type == "mlp" && raw_input_shape.size() != 2 && raw_input_shape.size() != 3) + throw std::invalid_argument( + "model_type=mlp requires input shape [batch,obs] or [batch,history,feature]"); + if (resolved_type == "transformer" && raw_input_shape.size() != 3) + throw std::invalid_argument( + "model_type=transformer requires input shape [batch,sequence,feature]"); + info_.model_type = resolved_type; + info_.input_rank = raw_input_shape.size(); + info_.output_rank = raw_output_shape.size(); + + const auto metadata_sequence = parse_positive(metadata("rmcs_history_length"), "rmcs_history_length"); + const auto metadata_feature = parse_positive(metadata("rmcs_obs_frame_size"), "rmcs_obs_frame_size"); + const auto layout = layout_dimensions(metadata("rmcs_obs_layout")); + if (raw_input_shape.size() == 3 && !layout && (config.sequence_length == 0 || config.feature_size == 0)) + throw std::invalid_argument( + "rank-3 model without metadata requires sequence_length and feature_size"); + if (layout && metadata_sequence != 0 && metadata_sequence != layout->history) + throw std::invalid_argument("rmcs_history_length does not match rmcs_obs_layout"); + if (layout && metadata_feature != 0 && metadata_feature != layout->feature) + throw std::invalid_argument("rmcs_obs_frame_size does not match rmcs_obs_layout"); + if (layout && config.sequence_length != 0 && config.sequence_length != layout->history) + throw std::invalid_argument("sequence_length does not match rmcs_obs_layout"); + if (layout && config.feature_size != 0 && config.feature_size != layout->feature) + throw std::invalid_argument("feature_size does not match rmcs_obs_layout"); + std::size_t sequence = config.sequence_length != 0 + ? config.sequence_length + : metadata_sequence != 0 ? metadata_sequence + : layout ? layout->history : 0; + std::size_t feature = config.feature_size != 0 + ? config.feature_size + : metadata_feature != 0 ? metadata_feature + : layout ? layout->feature : 0; + std::size_t expected_observation = config.observation_size; + if (expected_observation == 0 && layout) + expected_observation = layout->observation; + if (layout && expected_observation != layout->observation) + throw std::invalid_argument("observation_size does not match rmcs_obs_layout dimensions"); + + input_shape_.resize(raw_input_shape.size()); + if (raw_input_shape.size() == 1) { + const auto declared = raw_input_shape[0] > 0 ? static_cast(raw_input_shape[0]) : 0; + const auto resolved = declared != 0 ? declared : expected_observation; + if (resolved == 0) + throw std::invalid_argument("dynamic rank-1 input needs observation_size"); + if (declared != 0 && expected_observation != 0 && declared != expected_observation) + throw std::invalid_argument("model input size does not match observation_size"); + input_shape_[0] = static_cast(resolved); + info_.feature_size = resolved; + info_.sequence_length = 1; + } else if (raw_input_shape.size() == 2) { + const auto declared = raw_input_shape[1] > 0 ? static_cast(raw_input_shape[1]) : 0; + const auto resolved = declared != 0 ? declared : expected_observation; + if (resolved == 0) + throw std::invalid_argument("dynamic rank-2 input needs observation_size"); + if (declared != 0 && expected_observation != 0 && declared != expected_observation) + throw std::invalid_argument("model input feature size does not match observation_size"); + input_shape_ = {1, static_cast(resolved)}; + info_.feature_size = resolved; + info_.sequence_length = 1; + } else { + const auto declared_sequence = raw_input_shape[1] > 0 + ? static_cast(raw_input_shape[1]) + : 0; + const auto declared_feature = raw_input_shape[2] > 0 + ? static_cast(raw_input_shape[2]) + : 0; + if (sequence == 0) + sequence = declared_sequence; + if (feature == 0) + feature = declared_feature; + if (sequence == 0 && feature != 0 && expected_observation != 0 + && expected_observation % feature == 0) + sequence = expected_observation / feature; + if (feature == 0 && sequence != 0 && expected_observation != 0 + && expected_observation % sequence == 0) + feature = expected_observation / sequence; + if (sequence == 0 || feature == 0) + throw std::invalid_argument( + "rank-3 input needs concrete sequence/feature dimensions or sequence_length/feature_size"); + if ((declared_sequence != 0 && declared_sequence != sequence) + || (declared_feature != 0 && declared_feature != feature)) + throw std::invalid_argument( + "model input dims (T=" + std::to_string(declared_sequence) + + ", F=" + std::to_string(declared_feature) + ") do not match expected (T=" + + std::to_string(sequence) + ", F=" + std::to_string(feature) + + ") from sequence_length/feature_size or rmcs_history_length/rmcs_obs_frame_size " + "(model sequence must equal bridge history_length)"); + if (expected_observation != 0 && sequence * feature != expected_observation) + throw std::invalid_argument("sequence_length * feature_size does not match observation_size"); + input_shape_ = {1, static_cast(sequence), static_cast(feature)}; + info_.sequence_length = sequence; + info_.feature_size = feature; + } + input_buffer_.resize( + std::accumulate(input_shape_.begin(), input_shape_.end(), std::size_t{1}, [](auto value, auto dim) { + return value * static_cast(dim); + })); + + // Extra (non-obs) inputs: every one needs a constant from extra_inputs.*, + // and every declared constant must match a model input. + auto remaining = config.extra_input_values; + constant_slot_.assign(input_count, -1); + for (std::size_t index = 0; index < input_count; ++index) { + if (index == primary_index) + continue; + const auto found = remaining.find(input_names_[index]); + if (found == remaining.end()) + throw std::invalid_argument( + "model input '" + input_names_[index] + + "' has no constant value; declare it under policy_server extra_inputs." + " in the vehicle YAML"); + const auto source = std::move(found->second); + remaining.erase(found); + + const auto extra_type = session_.GetInputTypeInfo(index); + if (extra_type.GetONNXType() != ONNX_TYPE_TENSOR) + throw std::invalid_argument( + "extra input '" + input_names_[index] + "' must be a tensor"); + const auto extra_info = extra_type.GetTensorTypeAndShapeInfo(); + const auto extra_element_type = extra_info.GetElementType(); + if (extra_element_type != ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT + && extra_element_type != ONNX_TENSOR_ELEMENT_DATA_TYPE_INT64 + && extra_element_type != ONNX_TENSOR_ELEMENT_DATA_TYPE_BOOL) + throw std::invalid_argument( + "extra input '" + input_names_[index] + "' element type must be float32, int64, or bool"); + const auto raw_shape = extra_info.GetShape(); + if (raw_shape.empty() || raw_shape.size() > 3) + throw std::invalid_argument( + "extra input '" + input_names_[index] + "' rank must be 1, 2, or 3"); + + ConstantInput constant; + constant.name = input_names_[index]; + constant.type = extra_element_type; + std::size_t elements = 1; + std::size_t first_dim = 0; + if (raw_shape.size() >= 2) { + if (!batch_dimension_ok(raw_shape[0])) + throw std::invalid_argument( + "extra input '" + constant.name + "' batch dimension must be 1 or dynamic"); + constant.shape.push_back(1); + first_dim = 1; + } + for (std::size_t dim = first_dim; dim < raw_shape.size(); ++dim) { + if (raw_shape[dim] <= 0) + throw std::invalid_argument( + "extra input '" + constant.name + + "' non-batch dimensions must be concrete (dynamic dim " + + std::to_string(dim) + ")"); + constant.shape.push_back(raw_shape[dim]); + elements *= static_cast(raw_shape[dim]); + } + if (std::visit([](const auto& values) { return values.size(); }, source) != elements) + throw std::invalid_argument( + "extra input '" + constant.name + "' needs " + std::to_string(elements) + + " values, got " + + std::to_string(std::visit([](const auto& values) { return values.size(); }, source))); + constant.element_count = elements; + if (extra_element_type == ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT) { + constant.float_values.reserve(elements); + if (const auto* floats = std::get_if>(&source)) { + for (const double value : *floats) { + const float converted = static_cast(value); + if (!std::isfinite(converted)) + throw std::invalid_argument( + "extra input '" + constant.name + "' contains a non-finite or out-of-range float32 value"); + constant.float_values.push_back(converted); + } + } else if (const auto* integers = std::get_if>(&source)) { + for (const auto value : *integers) { + const float converted = static_cast(value); + if (!std::isfinite(converted)) + throw std::invalid_argument( + "extra input '" + constant.name + "' contains an out-of-range float32 value"); + constant.float_values.push_back(converted); + } + } else { + throw std::invalid_argument( + "extra input '" + constant.name + "' needs a float or integer list"); + } + } else if (extra_element_type == ONNX_TENSOR_ELEMENT_DATA_TYPE_INT64) { + if (const auto* integers = std::get_if>(&source)) { + constant.int64_values = *integers; + } else if (const auto* floats = std::get_if>(&source)) { + constant.int64_values.reserve(elements); + for (const double value : *floats) { + const auto promoted = static_cast(value); + if (!std::isfinite(value) || std::trunc(value) != value + || promoted < static_cast(std::numeric_limits::min()) + || promoted > static_cast(std::numeric_limits::max())) + throw std::invalid_argument( + "extra input '" + constant.name + "' contains a non-integral or out-of-range int64 value"); + constant.int64_values.push_back(static_cast(value)); + } + } else { + throw std::invalid_argument( + "extra input '" + constant.name + "' needs an integer or integral float list for int64 tensor"); + } + } else { + const auto* booleans = std::get_if>(&source); + if (!booleans) + throw std::invalid_argument( + "extra input '" + constant.name + "' needs a bool list for bool tensor"); + constant.bool_values = std::make_unique(elements); + for (std::size_t value_index = 0; value_index < elements; ++value_index) + constant.bool_values[value_index] = (*booleans)[value_index]; + } + constant_slot_[index] = static_cast(constants_.size()); + constants_.push_back(std::move(constant)); + } + if (!remaining.empty()) { + std::string listing; + for (const auto& [name, values] : remaining) { + static_cast(values); + listing += (listing.empty() ? "" : ", ") + name; + } + throw std::invalid_argument( + "extra_inputs entry(s) [" + listing + "] have no matching model input"); + } + + std::size_t output_size = 1; + bool output_dynamic = false; + for (std::size_t index = raw_output_shape.size() >= 2 ? 1 : 0; index < raw_output_shape.size(); ++index) { + if (raw_output_shape[index] <= 0) + output_dynamic = true; + else + output_size *= static_cast(raw_output_shape[index]); + } + if (output_dynamic) { + if (config.action_size == 0) + throw std::invalid_argument("dynamic output needs action_size"); + output_size = config.action_size; + } else if (config.action_size != 0 && output_size != config.action_size) { + throw std::invalid_argument("model output size does not match action_size"); + } + if (output_size == 0) + throw std::invalid_argument("model output must contain at least one action"); + output_size_ = output_size; } std::optional OnnxRuntime::metadata(const char* key) const { @@ -60,13 +436,39 @@ void OnnxRuntime::run(std::span input, std::span output) { if (input.size() != input_size() || output.size() != output_size()) throw std::invalid_argument("inference buffers do not match model dimensions"); std::copy(input.begin(), input.end(), input_buffer_.begin()); - auto tensor = Ort::Value::CreateTensor( - memory_info_, input_buffer_.data(), input_buffer_.size(), input_shape_.data(), - input_shape_.size()); - const char* input_names[] = {input_name_.c_str()}; + + const std::size_t input_count = input_names_.size(); + std::vector input_names; + input_names.reserve(input_count); + std::vector input_tensors; + input_tensors.reserve(input_count); + for (std::size_t index = 0; index < input_count; ++index) { + input_names.push_back(input_names_[index].c_str()); + if (constant_slot_[index] < 0) { + input_tensors.push_back(Ort::Value::CreateTensor( + memory_info_, input_buffer_.data(), input_buffer_.size(), input_shape_.data(), + input_shape_.size())); + } else { + auto& constant = constants_[static_cast(constant_slot_[index])]; + if (constant.type == ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT) { + input_tensors.push_back(Ort::Value::CreateTensor( + memory_info_, constant.float_values.data(), constant.element_count, + constant.shape.data(), constant.shape.size())); + } else if (constant.type == ONNX_TENSOR_ELEMENT_DATA_TYPE_INT64) { + input_tensors.push_back(Ort::Value::CreateTensor( + memory_info_, constant.int64_values.data(), constant.element_count, + constant.shape.data(), constant.shape.size())); + } else { + input_tensors.push_back(Ort::Value::CreateTensor( + memory_info_, constant.bool_values.get(), constant.element_count, + constant.shape.data(), constant.shape.size())); + } + } + } const char* output_names[] = {output_name_.c_str()}; - const auto outputs = - session_.Run(Ort::RunOptions{nullptr}, input_names, &tensor, 1, output_names, 1); + const auto outputs = session_.Run( + Ort::RunOptions{nullptr}, input_names.data(), input_tensors.data(), input_count, + output_names, 1); if (outputs.size() != 1 || !outputs[0].IsTensor() || outputs[0].GetTensorTypeAndShapeInfo().GetElementCount() != output_size()) throw std::runtime_error("inference output does not match model dimensions"); diff --git a/src/policy/onnx_runtime.hpp b/src/policy/onnx_runtime.hpp index 791275e..0182c0d 100644 --- a/src/policy/onnx_runtime.hpp +++ b/src/policy/onnx_runtime.hpp @@ -1,11 +1,13 @@ #pragma once -#include #include #include +#include +#include #include #include #include +#include #include #include @@ -14,26 +16,80 @@ namespace rmcs_rl { class OnnxRuntime { public: + using ExtraInputValues = std::variant< + std::vector, std::vector, std::vector>; + + struct Config { + std::string model_path; + std::string input_name = "obs"; + std::string output_name = "actions"; + std::string model_type = "auto"; + std::size_t sequence_length = 0; + std::size_t feature_size = 0; + std::size_t observation_size = 0; + std::size_t action_size = 0; + // 额外输入张量的常量值（键 = 模型输入名，值 = 展平的元素）， + // 由 policy_server 的 extra_inputs. YAML 参数提供。 + std::map extra_input_values; + }; + + struct Info { + std::string model_type; + std::size_t input_rank = 0; + std::size_t output_rank = 0; + std::size_t sequence_length = 1; + std::size_t feature_size = 0; + }; + + explicit OnnxRuntime(const Config& config); + + // Kept for small external users of the old deployment API. OnnxRuntime( const std::string& model_path, const std::string& input_name, - const std::string& output_name); + const std::string& output_name) + : OnnxRuntime(Config{ + .model_path = model_path, + .input_name = input_name, + .output_name = output_name, + .model_type = "auto", + .sequence_length = 0, + .feature_size = 0, + .observation_size = 0, + .action_size = 0, + .extra_input_values = {}}) {} [[nodiscard]] std::size_t input_size() const { return input_buffer_.size(); } [[nodiscard]] std::size_t output_size() const { return output_size_; } + [[nodiscard]] const Info& info() const { return info_; } [[nodiscard]] std::optional metadata(const char* key) const; void run(std::span input, std::span output); private: + struct ConstantInput { + std::string name; + std::vector shape; + ONNXTensorElementDataType type = ONNX_TENSOR_ELEMENT_DATA_TYPE_UNDEFINED; + std::size_t element_count = 0; + std::vector float_values; + std::vector int64_values; + std::unique_ptr bool_values; + }; + Ort::Env env_{ORT_LOGGING_LEVEL_WARNING, "rmcs_rl"}; Ort::AllocatorWithDefaultOptions allocator_; Ort::MemoryInfo memory_info_{Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault)}; Ort::Session session_{nullptr}; std::string input_name_; std::string output_name_; - std::array input_shape_{}; + std::vector input_names_; // 会话输入顺序 + std::vector constants_; + // 与 input_names_ 并行：主输入为 -1，其余为 constants_ 下标 + std::vector constant_slot_; + std::vector input_shape_; std::vector input_buffer_; std::size_t output_size_ = 0; + Info info_; }; } // namespace rmcs_rl diff --git a/src/policy/policy_model.cpp b/src/policy/policy_model.cpp index f950d14..dda66ff 100644 --- a/src/policy/policy_model.cpp +++ b/src/policy/policy_model.cpp @@ -45,32 +45,35 @@ std::optional clip_metadata(const OnnxRuntime& runtime, const char* key) } // namespace PolicyModel::PolicyModel(const Config& config) - : runtime_(config.path, config.input_name, config.output_name) { + : runtime_(OnnxRuntime::Config{ + config.path, + config.input_name, + config.output_name, + config.model_type, + config.sequence_length, + config.feature_size, + config.observation_size, + config.action_size, + config.extra_input_values}) { info_.path = config.path; info_.obs_size = runtime_.input_size(); info_.action_size = runtime_.output_size(); + info_.model_type = runtime_.info().model_type; + info_.input_rank = runtime_.info().input_rank; + info_.output_rank = runtime_.info().output_rank; + info_.sequence_length = runtime_.info().sequence_length; + info_.feature_size = runtime_.info().feature_size; std::string error; - if (!model_id_of_file(config.path, info_.model_id, error)) - throw std::runtime_error(error); + // Model identity and layout metadata are intentionally optional. The bridge owns the + // observation/action contract from YAML; the policy server only validates tensor sizes. + model_id_of_file(config.path, info_.model_id, error); info_.obs_signature = runtime_.metadata("rmcs_obs_layout").value_or(""); info_.actions_signature = runtime_.metadata("rmcs_actions_layout").value_or(""); info_.version = runtime_.metadata("policy_version").value_or(""); - if (info_.obs_signature.empty() || info_.actions_signature.empty()) - throw std::invalid_argument( - "model is missing rmcs_obs_layout / rmcs_actions_layout; " - "stamp it with tool/stamp_layout_metadata.py before deploying"); - info_.layout_hash = rmcs_rl::layout_hash( - info_.obs_signature, info_.actions_signature, info_.obs_size, info_.action_size); - if (const auto declared = runtime_.metadata("policy_layout_hash"); - declared && !declared->empty()) { - std::size_t consumed = 0; - const auto hash = std::stoull(*declared, &consumed, 16); - if (consumed != declared->size() || hash != info_.layout_hash) - throw std::invalid_argument( - "policy_layout_hash does not match layout metadata: expected " - + hex16(info_.layout_hash)); - } + if (!info_.obs_signature.empty() && !info_.actions_signature.empty()) + info_.layout_hash = rmcs_rl::layout_hash( + info_.obs_signature, info_.actions_signature, info_.obs_size, info_.action_size); load_normalization(config); obs_buffer_.resize(info_.obs_size); diff --git a/src/policy/policy_model.hpp b/src/policy/policy_model.hpp index e12ccf7..4add7a9 100644 --- a/src/policy/policy_model.hpp +++ b/src/policy/policy_model.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -17,9 +18,16 @@ class PolicyModel { std::string path; std::string input_name = "obs"; std::string output_name = "actions"; + std::string model_type = "auto"; + std::size_t sequence_length = 0; + std::size_t feature_size = 0; + std::size_t observation_size = 0; + std::size_t action_size = 0; bool normalization_from_metadata = true; std::optional obs_clip; std::optional action_clip; + // 键 = 额外输入张量名，值 = 展平常量（来自 policy_server extra_inputs.） + std::map extra_input_values; }; struct Info { @@ -29,6 +37,11 @@ class PolicyModel { std::string version; std::size_t obs_size = 0; std::size_t action_size = 0; + std::string model_type; + std::size_t input_rank = 0; + std::size_t output_rank = 0; + std::size_t sequence_length = 1; + std::size_t feature_size = 0; std::uint64_t layout_hash = 0; std::uint64_t model_id = 0; }; diff --git a/src/policy_server.cpp b/src/policy_server.cpp index 6039c40..d04a5e3 100644 --- a/src/policy_server.cpp +++ b/src/policy_server.cpp @@ -1,14 +1,18 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include +#include #include #include @@ -39,6 +43,60 @@ class PolicyServer final : public rclcpp::Node { ament_index_cpp::get_package_share_directory("rmcs_rl") + "/" + config.path; config.input_name = string_or(*this, "input_name", "obs"); config.output_name = string_or(*this, "output_name", "actions"); + config.model_type = string_or(*this, "model_type", "auto"); + if (const auto value = integer_parameter(*this, "sequence_length"); value.has_value()) { + if (*value < 1) + throw std::invalid_argument("policy_server: sequence_length must be positive"); + config.sequence_length = static_cast(*value); + } + if (const auto value = integer_parameter(*this, "feature_size"); value.has_value()) { + if (*value < 1) + throw std::invalid_argument("policy_server: feature_size must be positive"); + config.feature_size = static_cast(*value); + } + if (const auto value = integer_parameter(*this, "rl_obs_size"); value.has_value()) { + if (*value < 1) + throw std::invalid_argument("policy_server: rl_obs_size must be positive"); + config.observation_size = static_cast(*value); + } + if (const auto value = integer_parameter(*this, "rl_action_size"); value.has_value()) { + if (*value < 1) + throw std::invalid_argument("policy_server: rl_action_size must be positive"); + config.action_size = static_cast(*value); + } + for (const auto& parameter_name : list_parameters({"extra_inputs"}, 1).names) { + static constexpr std::string_view kPrefix = "extra_inputs."; + if (!parameter_name.starts_with(kPrefix)) + continue; + const auto extra_name = parameter_name.substr(kPrefix.size()); + if (extra_name.empty()) + throw std::invalid_argument( + "policy_server: extra_inputs entry must be extra_inputs."); + const auto parameter = get_parameter(parameter_name); + OnnxRuntime::ExtraInputValues values; + switch (parameter.get_type()) { + case rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY: + values = parameter.as_double_array(); + break; + case rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY: + values = parameter.as_integer_array(); + break; + case rclcpp::ParameterType::PARAMETER_BOOL_ARRAY: + values = parameter.as_bool_array(); + break; + default: + throw std::invalid_argument( + "policy_server: " + parameter_name + " must be a float, integer, or bool list"); + } + if (std::visit([](const auto& items) { return items.empty(); }, values)) + throw std::invalid_argument("policy_server: " + parameter_name + " is empty"); + if (const auto* floats = std::get_if>(&values)) + for (const double value : *floats) + if (!std::isfinite(value)) + throw std::invalid_argument( + "policy_server: " + parameter_name + " contains a non-finite value"); + config.extra_input_values.emplace(extra_name, std::move(values)); + } config.normalization_from_metadata = bool_or(*this, "normalization_from_metadata", true); if (const double clip = number_or(*this, "obs_clip", -1.0); clip >= 0.0) config.obs_clip = clip; @@ -63,9 +121,10 @@ class PolicyServer final : public rclcpp::Node { const auto& info = model_->info(); RCLCPP_INFO(get_logger(), "policy loaded: %s", info.path.c_str()); RCLCPP_INFO( - get_logger(), "model_id=%s layout_hash=%s version=%s obs=%zu action=%zu", + get_logger(), "model_id=%s layout_hash=%s version=%s type=%s input_rank=%zu output_rank=%zu obs=%zu action=%zu", hex16(info.model_id).c_str(), hex16(info.layout_hash).c_str(), info.version.c_str(), - info.obs_size, info.action_size); + info.model_type.c_str(), info.input_rank, info.output_rank, info.obs_size, + info.action_size); RCLCPP_INFO(get_logger(), "obs signature: %s", info.obs_signature.c_str()); RCLCPP_INFO(get_logger(), "action signature: %s", info.actions_signature.c_str()); RCLCPP_INFO(get_logger(), "waiting for obs on %s/obs", rl_base.c_str()); @@ -74,7 +133,7 @@ class PolicyServer final : public rclcpp::Node { private: void on_observation(msg::Observation::UniquePtr message) { const auto& info = model_->info(); - if (message->layout_hash != info.layout_hash) { + if (info.layout_hash != 0 && message->layout_hash != info.layout_hash) { if (!layout_mismatch_logged_) { layout_mismatch_logged_ = true; RCLCPP_ERROR( @@ -95,7 +154,9 @@ class PolicyServer final : public rclcpp::Node { } action.header.stamp = get_clock()->now(); action.obs_seq = message->obs_seq; - action.layout_hash = info.layout_hash; + // The bridge is the source of truth for the YAML-defined layout when a model has no + // stamped metadata. Preserve the observation contract on the action message. + action.layout_hash = message->layout_hash; action.model_id = info.model_id; action_publisher_->publish(action); const auto elapsed = diff --git a/src/rl_bridge.cpp b/src/rl_bridge.cpp index e175f76..b94fa7f 100644 --- a/src/rl_bridge.cpp +++ b/src/rl_bridge.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -43,7 +44,8 @@ class RlBridge final get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , config_(load_bridge_config(*this)) - , history_(config_.obs_frame_size, config_.history_length) { + , history_(config_.obs_frame_size, config_.history_length, config_.history_period) + , action_feedback_(config_.action_size) { for (const auto& term : config_.action_terms) { action_outputs_.push_back(std::make_unique>()); @@ -100,7 +102,22 @@ class RlBridge final const auto now = std::chrono::steady_clock::now(); maybe_handle_reset_(); - maybe_publish_observation_(now); + const bool enabled = read_enable_(); + double flat_hold_value = 0.0; + const bool flat_hold = flat_hold_slot_ != kNoSlot + && read_double(slots_[flat_hold_slot_], flat_hold_value) + && std::isfinite(flat_hold_value) && flat_hold_value != 0.0; + if (enabled != last_enabled_ || flat_hold != last_flat_hold_) { + reset_runtime_(); + last_enabled_ = enabled; + last_flat_hold_ = flat_hold; + } + if (!enabled) { + reset_action_state(last_actions_, written_); + history_.reset(); + } + if (enabled) + maybe_publish_observation_(now); ActionSnapshot& snapshot = read_snapshot_; const bool has_snapshot = action_channel_.try_read(snapshot); @@ -117,21 +134,33 @@ class RlBridge final check_contract_(snapshot); } - const bool enabled = read_enable_(); const bool fresh = has_snapshot && is_finite(age) && age <= config_.max_action_age; const bool valid = enabled && contract_ok_ && has_snapshot && fresh && seq_ok && finite; + // The action fed into history must match the flat-ground command that is executed. + if (valid && flat_hold) + std::fill(snapshot.action.begin(), snapshot.action.end(), 0.0); write_actions( valid, snapshot, config_.action_terms, config_.invalid_mode, written_, action_outputs_); - if (valid) - last_actions_ = snapshot.action; + if (valid) { + if (config_.history_period.count() == 0) { + last_actions_ = snapshot.action; + } else if (snapshot.obs_seq != last_feedback_obs_seq_) { + const auto observation_time = now - std::chrono::duration_cast( + std::chrono::duration(age)); + action_feedback_.store(observation_time, snapshot.action); + last_feedback_obs_seq_ = snapshot.obs_seq; + } + } *valid_output_ = valid ? 1.0 : 0.0; *healthy_output_ = (contract_ok_ && fresh) ? 1.0 : 0.0; *action_age_output_ = age; *obs_seq_output_ = timeline_.sequence(); + if (last_valid_ && !valid) + reset_runtime_(); if (valid != last_valid_) { if (valid) { RCLCPP_INFO( @@ -212,6 +241,10 @@ class RlBridge final reset_slot_ = acquire_slot( *this, config_.reset_path, {Binding::kSize, Binding::kInt, Binding::kDouble}, false, output_map, slots_, "reset interface"); + if (!config_.flat_hold_path.empty()) + flat_hold_slot_ = acquire_slot( + *this, config_.flat_hold_path, {Binding::kBool, Binding::kDouble}, true, + output_map, slots_, "flat hold interface"); } void maybe_handle_reset_() { @@ -229,10 +262,13 @@ class RlBridge final return; std::vector obs; + if (config_.history_period.count() > 0) + last_actions_ = action_feedback_.sample(now, config_.history_period); if (build_observation( config_.obs_terms, slots_, last_actions_, config_.obs_frame_size, obs)) { - publish_observation_(history_.append(obs), now); + publish_observation_(history_.append(obs, now), now); } else { + reset_runtime_(); ++obs_invalid_count_; RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 1000, @@ -261,14 +297,6 @@ class RlBridge final "(policy process and bridge disagree; refusing to output actions)", hex16(snapshot.layout_hash).c_str(), hex16(layout_hash_).c_str()); } - } else if ( - config_.expected_model_id != 0 && snapshot.model_id != config_.expected_model_id) { - if (contract_ok_) { - contract_ok_ = false; - RCLCPP_FATAL( - get_logger(), "model mismatch: action model_id=%s != expected_model_id=%s", - hex16(snapshot.model_id).c_str(), hex16(config_.expected_model_id).c_str()); - } } } @@ -306,7 +334,7 @@ class RlBridge final const std::vector& obs, std::chrono::steady_clock::time_point now) { rmcs_rl::msg::Observation message; message.header.stamp = get_clock()->now(); - message.obs_seq = timeline_.publish(now); + message.obs_seq = timeline_.publish(now, config_.pub_period); message.layout_hash = layout_hash_; message.obs = obs; obs_publisher_->publish(message); @@ -325,6 +353,8 @@ class RlBridge final void reset_runtime_() { reset_action_state(last_actions_, written_); history_.reset(); + action_feedback_.reset(); + last_feedback_obs_seq_ = 0; timeline_.reset(); } @@ -341,6 +371,7 @@ class RlBridge final std::size_t enable_slot_ = kNoSlot; std::size_t reset_slot_ = kNoSlot; + std::size_t flat_hold_slot_ = kNoSlot; std::string obs_signature_; std::string actions_signature_; @@ -349,12 +380,16 @@ class RlBridge final std::vector last_actions_; std::vector written_; ObservationHistory history_; + ActionFeedbackHistory action_feedback_; + std::uint64_t last_feedback_obs_seq_ = 0; ObservationTimeline timeline_; std::uint64_t last_reset_count_ = 0; std::uint64_t obs_invalid_count_ = 0; bool contract_ok_ = true; bool last_valid_ = false; + bool last_enabled_ = false; + bool last_flat_hold_ = false; ActionChannel action_channel_; ActionSnapshot read_snapshot_; diff --git a/src/rl_bridge/config.cpp b/src/rl_bridge/config.cpp index ab9d6d4..c42c426 100644 --- a/src/rl_bridge/config.cpp +++ b/src/rl_bridge/config.cpp @@ -79,11 +79,15 @@ BridgeConfig load_bridge_config(rclcpp::Node& node) { throw std::invalid_argument("RlBridge: policy_rate must be finite and > 0"); config.pub_period = std::chrono::duration_cast( std::chrono::duration(1.0 / config.policy_rate)); + const double history_rate = number_or(node, "history_rate", config.policy_rate); + if (!is_finite(history_rate) || history_rate <= 0.0 || history_rate > config.policy_rate) + throw std::invalid_argument("RlBridge: history_rate must be finite, positive and <= policy_rate"); + if (history_rate != config.policy_rate) + config.history_period = std::chrono::duration_cast( + std::chrono::duration(1.0 / history_rate)); config.max_action_age = number_or(node, "max_action_age", 2.0 / config.policy_rate); if (!(config.max_action_age > 0.0) || !is_finite(config.max_action_age)) throw std::invalid_argument("RlBridge: max_action_age must be finite and > 0"); - config.expected_model_id = - parse_u64(string_or(node, "expected_model_id", "0"), "expected_model_id"); const std::string invalid = string_or(node, "invalid_value", "nan"); if (invalid == "nan") @@ -102,6 +106,7 @@ BridgeConfig load_bridge_config(rclcpp::Node& node) { config.enable_path = string_or(node, "enable_interface", config.rl_base + "/enable"); config.enable_default = bool_or(node, "enable_default", false); config.reset_path = string_or(node, "reset_interface", ""); + config.flat_hold_path = string_or(node, "flat_hold_interface", ""); return config; } diff --git a/src/rl_bridge/observation_history.cpp b/src/rl_bridge/observation_history.cpp index cb88e9f..fcb64b0 100644 --- a/src/rl_bridge/observation_history.cpp +++ b/src/rl_bridge/observation_history.cpp @@ -1,15 +1,20 @@ #include #include +#include #include namespace rmcs_rl { -ObservationHistory::ObservationHistory(std::size_t frame_size, std::size_t history_length) +ObservationHistory::ObservationHistory(std::size_t frame_size, std::size_t history_length, + std::chrono::nanoseconds sample_period) : frame_size_(frame_size) - , values_(frame_size * history_length) { + , values_(frame_size * history_length) + , sample_period_(sample_period) { if (frame_size == 0 || history_length == 0) throw std::invalid_argument("observation frame size and history length must be positive"); + if (sample_period.count() < 0) + throw std::invalid_argument("observation history period cannot be negative"); } const std::vector& ObservationHistory::append(std::span frame) { @@ -26,8 +31,34 @@ const std::vector& ObservationHistory::append(std::span fr return values_; } +const std::vector& ObservationHistory::append(std::span frame, Clock::time_point now) { + if (sample_period_.count() == 0) + return append(frame); + if (frame.size() != frame_size_) + throw std::invalid_argument("observation frame does not match configured frame size"); + if (!samples_.empty() && now <= samples_.back().time) + throw std::invalid_argument("observation history timestamps must increase"); + if (!samples_.empty() && now - samples_.back().time > 2 * sample_period_) + reset(); + samples_.push_back({now, std::vector{frame.begin(), frame.end()}}); + const std::size_t length = values_.size() / frame_size_; + const auto oldest_time = now - sample_period_ * static_cast(length - 1); + while (samples_.size() > 1 && samples_[1].time <= oldest_time) + samples_.pop_front(); + std::size_t sample = 0; + for (std::size_t i = 0; i < length; ++i) { + const auto target = now - sample_period_ * static_cast(length - i - 1); + while (sample + 1 < samples_.size() && samples_[sample + 1].time <= target) + ++sample; + std::copy(samples_[sample].frame.begin(), samples_[sample].frame.end(), + values_.begin() + i * frame_size_); + } + return values_; +} + void ObservationHistory::reset() { initialized_ = false; + samples_.clear(); std::fill(values_.begin(), values_.end(), 0.0); } diff --git a/test/data/extra_input_identity.onnx b/test/data/extra_input_identity.onnx new file mode 100644 index 0000000..9289bb7 Binary files /dev/null and b/test/data/extra_input_identity.onnx differ diff --git a/test/data/seq_history_mismatch.onnx b/test/data/seq_history_mismatch.onnx new file mode 100644 index 0000000..423b0af Binary files /dev/null and b/test/data/seq_history_mismatch.onnx differ diff --git a/test/data/seq_identity.onnx b/test/data/seq_identity.onnx new file mode 100644 index 0000000..31c723d Binary files /dev/null and b/test/data/seq_identity.onnx differ diff --git a/test/data/seq_layout_dynamic.onnx b/test/data/seq_layout_dynamic.onnx new file mode 100644 index 0000000..c770858 Binary files /dev/null and b/test/data/seq_layout_dynamic.onnx differ diff --git a/test/data/seq_layout_mismatch.onnx b/test/data/seq_layout_mismatch.onnx new file mode 100644 index 0000000..fda5d0f Binary files /dev/null and b/test/data/seq_layout_mismatch.onnx differ diff --git a/test/data/seq_layout_only.onnx b/test/data/seq_layout_only.onnx new file mode 100644 index 0000000..71f6eaf Binary files /dev/null and b/test/data/seq_layout_only.onnx differ diff --git a/test/data/typed_extra_input_identity.onnx b/test/data/typed_extra_input_identity.onnx new file mode 100644 index 0000000..a1987f3 Binary files /dev/null and b/test/data/typed_extra_input_identity.onnx differ diff --git a/test/gen_extra_input_fixture.py b/test/gen_extra_input_fixture.py new file mode 100644 index 0000000..7602a33 --- /dev/null +++ b/test/gen_extra_input_fixture.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +"""生成多输入策略回归夹具（确定性，无训练依赖）。 + +产出（写入 test/data/）： + extra_input_identity.onnx + inputs : obs float32[batch,2], offset float32[batch,1] + output : actions float32[batch,2] = obs + offset（广播） + 用于校验 policy_server extra_inputs. 常量喂入与缺失/多余/数量错配的拒绝。 + +用法：python3 gen_extra_input_fixture.py +""" +import sys +from pathlib import Path + +DATA_DIR = Path(__file__).resolve().parent / "data" + + +def main() -> None: + import onnx + from onnx import TensorProto, helper + + DATA_DIR.mkdir(exist_ok=True) + add = helper.make_node("Add", ["obs", "offset"], ["actions"], name="add_offset") + graph = helper.make_graph( + [add], + "extra_input_policy", + [ + helper.make_tensor_value_info("obs", TensorProto.FLOAT, ["batch", 2]), + helper.make_tensor_value_info("offset", TensorProto.FLOAT, ["batch", 1]), + ], + [helper.make_tensor_value_info("actions", TensorProto.FLOAT, ["batch", 2])], + ) + model = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)]) + model.ir_version = 8 + model.producer_name = "rmcs_rl/gen_extra_input_fixture" + for key, value in { + "rmcs_obs_layout": "v3-history=1|constant:0+0@2", + "rmcs_actions_layout": "v2|#0:a|#1:b", + }.items(): + entry = model.metadata_props.add() + entry.key = key + entry.value = value + onnx.checker.check_model(model) + path = DATA_DIR / "extra_input_identity.onnx" + onnx.save(model, path) + print(f"wrote {path}") + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/test/gen_seq_fixtures.py b/test/gen_seq_fixtures.py new file mode 100644 index 0000000..50f5b07 --- /dev/null +++ b/test/gen_seq_fixtures.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +"""生成 rank-3 序列策略回归夹具（确定性，无训练依赖）。 + +产出（写入 test/data/）： + seq_identity.onnx obs float32[batch,2,3] -> actions float32[batch,3] + Flatten 后 actions[j] = x[j] + x[j+3]； + metadata: rmcs_model_type=transformer, + rmcs_history_length=2, rmcs_obs_frame_size=3。 + seq_history_mismatch.onnx 同图，但 rmcs_history_length=3（与张量 T=2 不符）， + 用于校验 policy_server 启动期拒绝。 + seq_layout_mismatch.onnx 同图，但 rmcs_obs_layout 的 v3-history=3（与张量 T=2 不符）， + 用于校验不依赖可选 metadata 的布局签名拒绝。 + seq_layout_only.onnx 同图，仅保留必需的布局签名，不带序列 metadata。 + seq_layout_dynamic.onnx 输入 T/F 为动态维度，仅用布局签名解析形状。 + +用法：python3 gen_seq_fixtures.py +""" +import sys +from pathlib import Path + +DATA_DIR = Path(__file__).resolve().parent / "data" + + +def _build(history: int, frame: int, metadata: dict, dynamic: bool = False) -> "object": + import numpy as np + import onnx + from onnx import TensorProto, helper, numpy_helper + + obs = history * frame + act = frame + # W (obs, act) = [I; I; ...] 每个历史帧块都加到同一输出槽： + # actions[j] = sum_t x[t, j] + weight = np.zeros((obs, act), dtype=np.float32) + for t in range(history): + for j in range(act): + weight[t * frame + j, j] = 1.0 + weight_init = numpy_helper.from_array(weight, name="W") + flatten = helper.make_node("Flatten", ["obs"], ["flat"], name="flatten_seq", axis=1) + matmul = helper.make_node("MatMul", ["flat", "W"], ["actions"], name="sum_history") + graph = helper.make_graph( + [flatten, matmul], + "seq_policy", + [helper.make_tensor_value_info( + "obs", TensorProto.FLOAT, + ["batch", "sequence", "feature"] if dynamic else ["batch", history, frame])], + [helper.make_tensor_value_info("actions", TensorProto.FLOAT, ["batch", act])], + [weight_init], + ) + model = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)]) + model.ir_version = 8 + model.producer_name = "rmcs_rl/gen_seq_fixtures" + for key, value in metadata.items(): + entry = model.metadata_props.add() + entry.key = key + entry.value = value + onnx.checker.check_model(model) + return model + + +def main() -> None: + import onnx + + DATA_DIR.mkdir(exist_ok=True) + shared = { + "rmcs_obs_layout": "v3-history=2|constant:0+0+0@3", + "rmcs_actions_layout": "v2|#0:a|#1:b|#2:c", + "rmcs_model_type": "transformer", + "rmcs_obs_frame_size": "3", + } + ok_meta = dict(shared, rmcs_history_length="2") + mismatch_meta = dict(shared, rmcs_history_length="3") + + ok_path = DATA_DIR / "seq_identity.onnx" + mismatch_path = DATA_DIR / "seq_history_mismatch.onnx" + layout_mismatch_path = DATA_DIR / "seq_layout_mismatch.onnx" + layout_only_path = DATA_DIR / "seq_layout_only.onnx" + layout_dynamic_path = DATA_DIR / "seq_layout_dynamic.onnx" + onnx.save(_build(2, 3, ok_meta), ok_path) + onnx.save(_build(2, 3, mismatch_meta), mismatch_path) + layout_mismatch_meta = dict(ok_meta, + rmcs_obs_layout="v3-history=3|constant:0+0+0@3") + onnx.save(_build(2, 3, layout_mismatch_meta), layout_mismatch_path) + layout_only_meta = { + "rmcs_obs_layout": shared["rmcs_obs_layout"], + "rmcs_actions_layout": shared["rmcs_actions_layout"], + "rmcs_model_type": shared["rmcs_model_type"], + } + onnx.save(_build(2, 3, layout_only_meta), layout_only_path) + onnx.save(_build(2, 3, layout_only_meta, dynamic=True), layout_dynamic_path) + print(f"wrote {ok_path}") + print(f"wrote {mismatch_path}") + print(f"wrote {layout_mismatch_path}") + print(f"wrote {layout_only_path}") + print(f"wrote {layout_dynamic_path}") + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/test/gen_synthetic_policy.py b/test/gen_synthetic_policy.py index 0994648..e8b15e8 100644 --- a/test/gen_synthetic_policy.py +++ b/test/gen_synthetic_policy.py @@ -8,18 +8,55 @@ import rl_layout as layout -def _export_with_onnx(obs: int, act: int, output: str) -> None: - """actions = obs @ W with zero weights; no training dependency.""" +def _export_with_onnx(obs: int, act: int, output: str, model_type: str, + sequence_length: int, feature_size: int, + extra_specs: list) -> None: + """Export a zero-action MLP/sequence fixture without a training dependency. + + extra_specs: [(name, dims)] 额外输入；每个 ReduceSum 成标量后加到主输出， + 保证额外输入被图消费（onnx.checker 要求）且输出仍为 [1, act]。 + """ import numpy as np import onnx from onnx import TensorProto, helper, numpy_helper - weight = numpy_helper.from_array(np.zeros((obs, act), dtype=np.float32), name="W") - node = helper.make_node("MatMul", ["obs", "W"], ["actions"], name="zero_action") + if sequence_length > 1: + input_shape = [1, sequence_length, feature_size] + if model_type == "transformer": + node = helper.make_node( + "ReduceMean", ["obs"], ["pooled"], name="sequence_pool", axes=[1], keepdims=0) + weight_shape = (feature_size, act) + matmul_input = "pooled" + else: + node = helper.make_node("Flatten", ["obs"], ["flat"], name="flatten_history", axis=1) + weight_shape = (obs, act) + matmul_input = "flat" + else: + input_shape = [1, obs] + node = None + weight_shape = (obs, act) + matmul_input = "obs" + weight = numpy_helper.from_array(np.zeros(weight_shape, dtype=np.float32), name="W") + matmul_output = "actions" if not extra_specs else "logits" + matmul = helper.make_node("MatMul", [matmul_input, "W"], [matmul_output], name="zero_action") + nodes = [node, matmul] if node is not None else [matmul] + + input_infos = [helper.make_tensor_value_info("obs", TensorProto.FLOAT, input_shape)] + current = matmul_output + for index, (name, dims) in enumerate(extra_specs): + input_infos.append( + helper.make_tensor_value_info(name, TensorProto.FLOAT, [1, *dims])) + nodes.append(helper.make_node( + "ReduceSum", [name], [f"{name}_sum"], name=f"sum_{name}", keepdims=0)) + final = "actions" if index == len(extra_specs) - 1 else f"add_{index}" + nodes.append(helper.make_node( + "Add", [current, f"{name}_sum"], [final], name=f"add_{name}")) + current = final + graph = helper.make_graph( - [node], + nodes, "zero_policy", - [helper.make_tensor_value_info("obs", TensorProto.FLOAT, [1, obs])], + input_infos, [helper.make_tensor_value_info("actions", TensorProto.FLOAT, [1, act])], [weight], ) @@ -32,10 +69,18 @@ def _export_with_onnx(obs: int, act: int, output: str) -> None: def main() -> None: parser = argparse.ArgumentParser( - description="生成零动作合成策略 ONNX（obs float32[1,N] -> actions float32[1,M]）" + description="生成零动作合成策略 ONNX（支持 MLP/Transformer 序列输入）" ) parser.add_argument("--obs", type=int, default=None, help="observation size（缺省 22）") parser.add_argument("--act", type=int, default=None, help="action size（缺省 4）") + parser.add_argument("--model-type", choices=("mlp", "transformer", "generic"), default="mlp", + help="fixture 结构；MLP 可生成 rank-2 或 rank-3 输入") + parser.add_argument("--sequence-length", type=int, default=None, + help="序列长度；>1 时生成 rank-3 输入") + parser.add_argument("--feature-size", type=int, default=None, + help="单帧维度；缺省由 obs/sequence 推导") + parser.add_argument("--extra-input", action="append", default=[], metavar="NAME:DIMS", + help="额外输入张量，如 mask:4 或 pos:2x3（可重复）") parser.add_argument("--from-config", "--config", dest="config", default=None, help="部署 YAML：由 observation_terms/action_terms 推导 obs/act 尺寸") parser.add_argument("--node", default=layout.DEFAULT_NODE, @@ -66,12 +111,41 @@ def main() -> None: print(f"ERROR: obs/act 必须为正整数（obs={obs} act={act}）", file=sys.stderr) sys.exit(1) - _export_with_onnx(obs, act, args.output) + sequence_length = args.sequence_length or 1 + if sequence_length < 1: + print("ERROR: sequence-length 必须 >= 1", file=sys.stderr) + sys.exit(1) + feature_size = args.feature_size or (obs // sequence_length if obs % sequence_length == 0 else 0) + if feature_size <= 0 or sequence_length * feature_size != obs: + print("ERROR: obs 必须能按 sequence-length 分解为正整数 feature-size", file=sys.stderr) + sys.exit(1) - print( - f"wrote {args.output}: obs float32[1,{obs}] -> actions float32[1,{act}] " - "(zero policy)" - ) + extra_specs = [] + for spec in args.extra_input: + if ":" not in spec: + print(f"ERROR: --extra-input 需要 NAME:DIMS 形式（got {spec!r}）", file=sys.stderr) + sys.exit(1) + name, dims_text = spec.split(":", 1) + try: + dims = [int(part) for part in dims_text.split("x")] + except ValueError: + print(f"ERROR: --extra-input 维度非法（got {spec!r}）", file=sys.stderr) + sys.exit(1) + if not name or not dims or any(dim <= 0 for dim in dims): + print(f"ERROR: --extra-input 需要非空名字与正整数维（got {spec!r}）", file=sys.stderr) + sys.exit(1) + if name == "obs" or any(existing == name for existing, _ in extra_specs): + print(f"ERROR: --extra-input 名字重复或与 obs 冲突（got {name!r}）", file=sys.stderr) + sys.exit(1) + extra_specs.append((name, dims)) + + _export_with_onnx(obs, act, args.output, args.model_type, sequence_length, feature_size, + extra_specs) + + input_desc = f"[1,{obs}]" if sequence_length == 1 else f"[1,{sequence_length},{feature_size}]" + for name, dims in extra_specs: + input_desc += f" + {name}[1,{'x'.join(str(dim) for dim in dims)}]" + print(f"wrote {args.output}: obs float32{input_desc} -> actions float32[1,{act}] (zero policy)") if __name__ == "__main__": diff --git a/test/gen_typed_extra_input_fixture.py b/test/gen_typed_extra_input_fixture.py new file mode 100644 index 0000000..d033b31 --- /dev/null +++ b/test/gen_typed_extra_input_fixture.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +"""生成带 int64/bool 常量输入的确定性策略夹具。""" +import sys +from pathlib import Path + +DATA_DIR = Path(__file__).resolve().parent / "data" + + +def main() -> None: + import onnx + from onnx import TensorProto, helper + + cast_ids = helper.make_node("Cast", ["position_ids"], ["position_float"], + name="cast_position", to=TensorProto.FLOAT) + cast_mask = helper.make_node("Cast", ["attention_mask"], ["mask_float"], + name="cast_mask", to=TensorProto.FLOAT) + add_position = helper.make_node("Add", ["obs", "position_float"], ["with_position"], + name="add_position") + add_mask = helper.make_node("Add", ["with_position", "mask_float"], ["actions"], + name="add_mask") + graph = helper.make_graph( + [cast_ids, cast_mask, add_position, add_mask], + "typed_extra_input_policy", + [ + helper.make_tensor_value_info("obs", TensorProto.FLOAT, ["batch", 2]), + helper.make_tensor_value_info("position_ids", TensorProto.INT64, ["batch", 2]), + helper.make_tensor_value_info("attention_mask", TensorProto.BOOL, ["batch", 2]), + ], + [helper.make_tensor_value_info("actions", TensorProto.FLOAT, ["batch", 2])], + ) + model = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)]) + model.ir_version = 8 + model.producer_name = "rmcs_rl/gen_typed_extra_input_fixture" + for key, value in { + "rmcs_obs_layout": "v3-history=1|constant:0+0@2", + "rmcs_actions_layout": "v2|#0:a|#1:b", + }.items(): + entry = model.metadata_props.add() + entry.key = key + entry.value = value + onnx.checker.check_model(model) + path = DATA_DIR / "typed_extra_input_identity.onnx" + onnx.save(model, path) + print(f"wrote {path}") + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/test/runtime_test.cpp b/test/runtime_test.cpp index bcf26f8..930c8ea 100644 --- a/test/runtime_test.cpp +++ b/test/runtime_test.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -28,6 +29,58 @@ TEST(ObservationHistory, PreservesFrameSizeOrderAndReset) { EXPECT_THROW(history.append(std::array{1.0}), std::invalid_argument); } +TEST(ActionFeedbackHistory, KeepsTenMillisecondPriorActionAndClearsOnReset) { + using namespace std::chrono_literals; + const ActionFeedbackHistory::Clock::time_point start{}; + ActionFeedbackHistory feedback{1}; + feedback.store(start, std::array{1.0}); + EXPECT_EQ(feedback.sample(start + 5ms, 10ms), (std::vector{0.0})); + feedback.store(start + 5ms, std::array{2.0}); + EXPECT_EQ(feedback.sample(start + 10ms, 10ms), (std::vector{1.0})); + feedback.store(start + 10ms, std::array{3.0}); + EXPECT_EQ(feedback.sample(start + 15ms, 10ms), (std::vector{2.0})); + feedback.store(start + 5ms, std::array{999.0}); // delayed response cannot rewind state + EXPECT_EQ(feedback.sample(start + 20ms, 10ms), (std::vector{3.0})); + feedback.reset(); + EXPECT_EQ(feedback.sample(start + 30ms, 10ms), (std::vector{0.0})); + EXPECT_THROW(feedback.store(start, std::array{1.0, 2.0}), std::invalid_argument); +} + +TEST(ObservationHistory, FasterInferencePreservesTrainingTimeSpan) { + using namespace std::chrono_literals; + const ObservationHistory::Clock::time_point start{}; + for (const std::size_t length : {5u, 8u}) { + ObservationHistory history{1, length, 10ms}; + for (int ms = 0; ms <= 100; ms += 5) { + const auto& values = history.append(std::array{double(ms)}, start + std::chrono::milliseconds(ms)); + for (std::size_t i = 0; i < length; ++i) + EXPECT_DOUBLE_EQ(values[i], std::max(0.0, double(ms) - 10.0 * (length - 1 - i))); + } + history.reset(); + const auto& values = history.append(std::array{42.0}, start + 1s); + for (double value : values) + EXPECT_DOUBLE_EQ(value, 42.0); + EXPECT_THROW(history.append(std::array{1.0}, start + 1s), std::invalid_argument); + const auto& after_gap = history.append(std::array{99.0}, start + 2s); + for (double value : after_gap) + EXPECT_DOUBLE_EQ(value, 99.0); + } +} + +TEST(ObservationTimeline, SchedulingJitterDoesNotAccumulate) { + using namespace std::chrono_literals; + ObservationTimeline timeline; + const ObservationTimeline::Clock::time_point start{}; + timeline.publish(start, 5ms); + EXPECT_TRUE(timeline.due(start + 6ms, 5ms)); + timeline.publish(start + 6ms, 5ms); + EXPECT_FALSE(timeline.due(start + 9ms, 5ms)); + EXPECT_TRUE(timeline.due(start + 10ms, 5ms)); + timeline.publish(start + 21ms, 5ms); + EXPECT_FALSE(timeline.due(start + 24ms, 5ms)); + EXPECT_TRUE(timeline.due(start + 25ms, 5ms)); +} + TEST(ObservationTimeline, RejectsPreResetAndOlderActions) { using namespace std::chrono_literals; const ObservationTimeline::Clock::time_point start{}; @@ -147,6 +200,113 @@ TEST(PolicyModel, ParameterClipsOverrideMetadata) { EXPECT_EQ(action, (std::array{3.0, -3.0})); } +TEST(PolicyModel, LoadsRank3SequenceInput) { + PolicyModel::Config config; + config.path = RMCS_RL_SEQ_FIXTURE; + PolicyModel model{config}; + const auto& info = model.info(); + EXPECT_EQ(info.model_type, "transformer"); + EXPECT_EQ(info.input_rank, 3u); + EXPECT_EQ(info.sequence_length, 2u); + EXPECT_EQ(info.feature_size, 3u); + ASSERT_EQ(info.obs_size, 6u); + ASSERT_EQ(info.action_size, 3u); + std::array action{}; + std::string error; + ASSERT_TRUE(model.run(std::array{1., 2., 3., 4., 5., 6.}, action, error)) << error; + EXPECT_EQ(action, (std::array{5.0, 7.0, 9.0})); + EXPECT_FALSE(model.run(std::array{1., 2., 3.}, action, error)); +} + +TEST(PolicyModel, RejectsSequenceHistoryMetadataMismatch) { + PolicyModel::Config config; + config.path = RMCS_RL_SEQ_MISMATCH_FIXTURE; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + +TEST(PolicyModel, RejectsSequenceLayoutHistoryMismatch) { + PolicyModel::Config config; + config.path = RMCS_RL_SEQ_LAYOUT_MISMATCH_FIXTURE; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + +TEST(PolicyModel, LoadsSequenceUsingRequiredLayoutOnly) { + PolicyModel::Config config; + config.path = RMCS_RL_SEQ_LAYOUT_ONLY_FIXTURE; + PolicyModel model{config}; + EXPECT_EQ(model.info().sequence_length, 2u); + EXPECT_EQ(model.info().feature_size, 3u); +} + +TEST(PolicyModel, LoadsDynamicSequenceUsingRequiredLayoutOnly) { + PolicyModel::Config config; + config.path = RMCS_RL_SEQ_LAYOUT_DYNAMIC_FIXTURE; + PolicyModel model{config}; + EXPECT_EQ(model.info().sequence_length, 2u); + EXPECT_EQ(model.info().feature_size, 3u); +} + +TEST(PolicyModel, RejectsTransformerTypeOnRank2Input) { + PolicyModel::Config config; + config.path = RMCS_RL_IDENTITY_FIXTURE; + config.model_type = "transformer"; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + +TEST(PolicyModel, FeedsExtraInputConstants) { + PolicyModel::Config config; + config.path = RMCS_RL_EXTRA_FIXTURE; + config.extra_input_values["offset"] = std::vector{10.0}; + PolicyModel model{config}; + ASSERT_EQ(model.info().obs_size, 2u); + ASSERT_EQ(model.info().action_size, 2u); + std::array action{}; + std::string error; + ASSERT_TRUE(model.run(std::array{3.0, 6.0}, action, error)) << error; + EXPECT_EQ(action, (std::array{13.0, 16.0})); +} + +TEST(PolicyModel, RejectsMissingExtraInputDeclaration) { + PolicyModel::Config config; + config.path = RMCS_RL_EXTRA_FIXTURE; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + +TEST(PolicyModel, RejectsExtraInputWithoutMatchingModelInput) { + PolicyModel::Config config; + config.path = RMCS_RL_EXTRA_FIXTURE; + config.extra_input_values["offset"] = std::vector{10.0}; + config.extra_input_values["bogus"] = std::vector{1.0}; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + +TEST(PolicyModel, RejectsWrongExtraInputValueCount) { + PolicyModel::Config config; + config.path = RMCS_RL_EXTRA_FIXTURE; + config.extra_input_values["offset"] = std::vector{1.0, 2.0}; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + +TEST(PolicyModel, FeedsTypedExtraInputConstants) { + PolicyModel::Config config; + config.path = RMCS_RL_TYPED_EXTRA_FIXTURE; + config.extra_input_values["position_ids"] = std::vector{10, 20}; + config.extra_input_values["attention_mask"] = std::vector{true, false}; + PolicyModel model{config}; + std::array action{}; + std::string error; + ASSERT_TRUE(model.run(std::array{1., 2.}, action, error)) << error; + EXPECT_EQ(action, (std::array{12.0, 22.0})); +} + +TEST(PolicyModel, RejectsWrongTypedExtraInputConstants) { + PolicyModel::Config config; + config.path = RMCS_RL_TYPED_EXTRA_FIXTURE; + config.extra_input_values["position_ids"] = std::vector{true, false}; + config.extra_input_values["attention_mask"] = std::vector{true, false}; + EXPECT_THROW(PolicyModel{config}, std::invalid_argument); +} + } // namespace } // namespace rmcs_rl #include diff --git a/test/test_layout_contract.sh b/test/test_layout_contract.sh index 7786a6c..38b9ceb 100755 --- a/test/test_layout_contract.sh +++ b/test/test_layout_contract.sh @@ -191,6 +191,118 @@ run_expect 1 "(e) rl_action_size != 动作词条数" \ "$PY" "$TOOLS/check_policy_contract.py" "$MODEL" --config "$WORK/x_actsize.yaml" expect_msg "rl_action_size" "尺寸不一致提示" +echo "== 5. Transformer rank-3 与 T==history 强制 ==" +FIXTURE_H4="$WORK/x_h4.yaml" +FIXTURE_H5="$WORK/x_h5.yaml" +MODEL_SEQ="$WORK/policy_seq.onnx" + +cat >"$FIXTURE_H4" <<'YAML' +rl_bridge: + ros__parameters: + history_length: 4 + rl_obs_size: 20 + rl_action_size: 6 + observation_terms: + - index=0 path=/chassis/control_height + - index=1 path=/chassis/control_velocity take=x + - index=2 path=/wheel_leg/imu/angular_velocity take=vec3 + action_terms: + - index=0 output=/wheel_leg/rl/action/lf0 + - index=1 output=/wheel_leg/rl/action/lf1 + - index=2 output=/wheel_leg/rl/action/l_wheel + - index=3 output=/wheel_leg/rl/action/rf0 + - index=4 output=/wheel_leg/rl/action/rf1 + - index=5 output=/wheel_leg/rl/action/r_wheel +YAML +sed -e 's/history_length: 4/history_length: 5/' \ + -e 's|index=2 path=/wheel_leg/imu/angular_velocity take=vec3|index=2 type=joint_pos joints=lf0,lf1 relative=true|' \ + "$FIXTURE_H4" >"$FIXTURE_H5" + +run_expect 0 "gen rank-3 transformer 夹具 (T=4, F=5)" \ + "$PY" "$HERE/gen_synthetic_policy.py" --obs 20 --act 6 \ + --model-type transformer --sequence-length 4 -o "$MODEL_SEQ" +run_expect 0 "盖章 transformer（默认写 rmcs_history_length）" \ + "$PY" "$TOOLS/stamp_layout_metadata.py" --model "$MODEL_SEQ" \ + --from-config "$FIXTURE_H4" --model-type transformer +expect_msg "rmcs_history_length" "盖章输出含序列键" +run_expect 0 "check rank-3 transformer（T==history 正例）" \ + "$PY" "$TOOLS/check_policy_contract.py" "$MODEL_SEQ" --config "$FIXTURE_H4" +expect_msg "model sequence dim == expected history" "T 等值检查已执行" + +cp "$MODEL_SEQ" "$WORK/policy_seq_h5.onnx" +run_expect 0 "同一模型按 history=5 配置盖章（模拟窗口错配）" \ + "$PY" "$TOOLS/stamp_layout_metadata.py" --model "$WORK/policy_seq_h5.onnx" \ + --from-config "$FIXTURE_H5" --model-type transformer +run_expect 1 "(f) 模型 T=4 != history=5 → 必须 FAIL" \ + "$PY" "$TOOLS/check_policy_contract.py" "$WORK/policy_seq_h5.onnx" --config "$FIXTURE_H5" +expect_msg "model sequence dim == expected history" "T 不匹配提示" + +cp "$MODEL" "$WORK/policy_mlptype.onnx" +run_expect 0 "给 rank-2 模型盖 rmcs_model_type=transformer" \ + "$PY" "$TOOLS/stamp_layout_metadata.py" --model "$WORK/policy_mlptype.onnx" \ + --from-config "$FIXTURE" --model-type transformer +run_expect 1 "(g) model_type=transformer 但输入 rank=2 → 必须 FAIL" \ + "$PY" "$TOOLS/check_policy_contract.py" "$WORK/policy_mlptype.onnx" --config "$FIXTURE" +expect_msg "input rank/model type" "rank/type 不匹配提示" + +echo "== 6. 多输入（policy_server extra_inputs 常量）==" +FIXTURE_EXTRA="$WORK/x_extra.yaml" +MODEL_EXTRA="$WORK/policy_extra.onnx" +{ + cat "$FIXTURE" + cat <<'YAML' + +policy_server: + ros__parameters: + rl_obs_size: 20 + rl_action_size: 6 + extra_inputs: + mask: [1, 1, 1, 1] +YAML +} >"$FIXTURE_EXTRA" + +run_expect 0 "gen 多输入夹具（obs + mask:4）" \ + "$PY" "$HERE/gen_synthetic_policy.py" --obs 20 --act 6 --extra-input mask:4 -o "$MODEL_EXTRA" +run_expect 0 "盖章多输入夹具" \ + "$PY" "$TOOLS/stamp_layout_metadata.py" --model "$MODEL_EXTRA" --from-config "$FIXTURE_EXTRA" +run_expect 0 "(h) 多输入 + extra_inputs 声明匹配 → PASS" \ + "$PY" "$TOOLS/check_policy_contract.py" "$MODEL_EXTRA" --config "$FIXTURE_EXTRA" +expect_msg "extra input mask" "额外输入校验已执行" + +run_expect 1 "(i) 模型有额外输入但 YAML 无 extra_inputs → FAIL" \ + "$PY" "$TOOLS/check_policy_contract.py" "$MODEL_EXTRA" --config "$FIXTURE" +expect_msg "extra_inputs match model" "缺声明提示" + +FIXTURE_EXTRA_UNKNOWN="$WORK/x_extra_unknown.yaml" +{ + cat "$FIXTURE" + cat <<'YAML' + +policy_server: + ros__parameters: + extra_inputs: + bogus: [1, 2, 3] +YAML +} >"$FIXTURE_EXTRA_UNKNOWN" +run_expect 1 "(j) YAML 声明了模型没有的额外输入 → FAIL" \ + "$PY" "$TOOLS/check_policy_contract.py" "$MODEL" --config "$FIXTURE_EXTRA_UNKNOWN" +expect_msg "extra_inputs match model" "多余声明提示" + +run_expect 0 "typed extra_inputs（int64 + bool）自检 → PASS" \ + "$PY" "$TOOLS/check_policy_contract.py" \ + "$PKG/test/data/typed_extra_input_identity.onnx" --obs 2 --act 2 +expect_msg "dtype" "typed extra_inputs 类型校验" + +run_expect 0 "仅布局签名解析动态 rank-3 → PASS" \ + "$PY" "$TOOLS/check_policy_contract.py" \ + "$PKG/test/data/seq_layout_dynamic.onnx" +expect_msg "rank-3 layout history" "动态序列由布局解析" + +run_expect 1 "模型 T 与布局 history 不符 → FAIL" \ + "$PY" "$TOOLS/check_policy_contract.py" \ + "$PKG/test/data/seq_layout_mismatch.onnx" +expect_msg "metadata history == layout history" "布局与序列 metadata 不一致" + echo echo "==== 结果：PASS=$PASS FAIL=$FAIL ====" [ "$FAIL" = 0 ] || exit 1 diff --git a/tool/check_policy_contract.py b/tool/check_policy_contract.py index c9dc793..2c664d9 100644 --- a/tool/check_policy_contract.py +++ b/tool/check_policy_contract.py @@ -4,8 +4,11 @@ Two modes: * self-check (no --config; used by CI, deployment YAML lives in the RMCS repo): - model loads; one input "obs" / one output "actions"; float32; rank 2; - batch 1 or dynamic; concrete feature dimensions. Layout metadata is OPTIONAL: + model loads; primary input "obs" / one output "actions" are float32; rank 1/2/3; + MLP uses rank 2 or rank 3 and Transformer uses rank 3. Extra (non-obs) + inputs may be float32, int64, or bool; with --config they must match + policy_server extra_inputs. + Layout metadata is OPTIONAL: - missing / v1 -> SKIP (stamped layout metadata is optional for model-only self-check) - present v2 -> signatures must be internally consistent with the tensor sizes, and policy_layout_hash must match those signatures @@ -132,8 +135,9 @@ def _check_normalization(report, meta, obs_size): report.check(key, math.isfinite(value) and value > 0.0, f"{key}={text}") -def _dummy_inference(report, model_path, obs_size, act_size): - """跑一次零输入推理，确认输出有限、形状正确（无 onnxruntime 则跳过）。""" +def _dummy_inference(report, model_path, obs_size, act_size, model_type, + input_rank, sequence_length, feature_size, extra_specs=None): + """跑一次零输入推理，确认输出有限、元素数正确（无 onnxruntime 则跳过）。""" try: import numpy as np import onnxruntime as ort @@ -142,16 +146,30 @@ def _dummy_inference(report, model_path, obs_size, act_size): return try: session = ort.InferenceSession(str(model_path), providers=["CPUExecutionProvider"]) - obs = np.zeros((1, obs_size), dtype=np.float32) - actions = session.run(["actions"], {"obs": obs})[0] + if input_rank == 3: + obs = np.zeros((1, sequence_length, feature_size), dtype=np.float32) + elif input_rank == 1: + obs = np.zeros((obs_size,), dtype=np.float32) + else: + obs = np.zeros((1, obs_size), dtype=np.float32) + feed = {"obs": obs} + for name, (shape, values, dtype) in (extra_specs or {}).items(): + count = 1 + for dim in shape: + count *= dim + if values is None: + values = [True] * count if dtype == "bool" else [1] * count + np_dtype = {"float32": np.float32, "int64": np.int64, "bool": np.bool_}[dtype] + feed[name] = np.asarray(values, dtype=np_dtype).reshape(shape) + actions = session.run(["actions"], feed)[0] except Exception as exc: report.check("dummy inference", False, f"{type(exc).__name__}: {exc}") return finite = bool(np.isfinite(actions).all()) - shape_ok = list(actions.shape) == [1, act_size] + shape_ok = int(np.prod(actions.shape, dtype=np.int64)) == act_size report.check( "dummy inference", finite and shape_ok, - f"actions{list(actions.shape)} finite={finite} " + f"actions{list(actions.shape)} elements={actions.size} finite={finite} " f"range=[{actions.min():.4f},{actions.max():.4f}]", ) @@ -171,6 +189,12 @@ def main() -> None: help="期望的观测尺寸（缺省取模型形状）") parser.add_argument("--act", type=int, default=None, help="期望的动作尺寸（缺省取模型形状）") + parser.add_argument("--model-type", choices=("auto", "mlp", "transformer", "generic"), + default="auto", help="模型结构（auto 按 metadata/输入 rank 推断）") + parser.add_argument("--sequence-length", type=int, default=None, + help="Transformer 序列长度（动态输入或无 YAML 时使用）") + parser.add_argument("--feature-size", type=int, default=None, + help="Transformer 单帧维度（动态输入或无 YAML 时使用）") parser.add_argument("--expect-model-id", default=None, help="可选：期望的 model_id（16 位 hex），用于部署对账") args = parser.parse_args() @@ -232,42 +256,256 @@ def main() -> None: for value in outputs: print(f"output : {value.name} {_shape_of(value)}") - report.check("tensor count/names", len(inputs) == 1 and len(outputs) == 1 - and inputs[0].name == "obs" and outputs[0].name == "actions", - f"inputs={[v.name for v in inputs]} outputs={[v.name for v in outputs]}" - if not (len(inputs) == 1 and len(outputs) == 1) else "obs -> actions") - - if not (len(inputs) == 1 and len(outputs) == 1): - print("== SUMMARY: FAIL (张量不合法，后续检查跳过) ==") + input_names = [value.name for value in inputs] + names_ok = (len(outputs) == 1 and outputs[0].name == "actions" and "obs" in input_names) + report.check("tensor names", names_ok, + f"inputs={input_names} outputs={[v.name for v in outputs]}") + if not names_ok: + print("== SUMMARY: FAIL (张量不合法：需要主输入 obs + 唯一输出 actions，后续检查跳过) ==") sys.exit(1) - inp, out = inputs[0], outputs[0] + inp = next(value for value in inputs if value.name == "obs") + out = outputs[0] def _dtype_name(value_info): elem = value_info.type.tensor_type.elem_type return "float32" if elem == TensorProto.FLOAT else TensorProto.DataType.Name(elem).lower() - report.check("dtype float32", inp.type.tensor_type.elem_type == TensorProto.FLOAT - and out.type.tensor_type.elem_type == TensorProto.FLOAT, - f"obs={_dtype_name(inp)} actions={_dtype_name(out)}") + primary_dtype_ok = inp.type.tensor_type.elem_type == TensorProto.FLOAT + output_dtype_ok = out.type.tensor_type.elem_type == TensorProto.FLOAT + extra_dtype_ok = all(value.type.tensor_type.elem_type in ( + TensorProto.FLOAT, TensorProto.INT64, TensorProto.BOOL) + for value in inputs if value.name != "obs") + dtype_ok = primary_dtype_ok and output_dtype_ok and extra_dtype_ok + report.check("dtype", dtype_ok, + f"inputs={[_dtype_name(value) for value in inputs]} actions={_dtype_name(out)}") in_shape, out_shape = _shape_of(inp), _shape_of(out) - report.check("rank 2", len(in_shape) == 2 and len(out_shape) == 2, - f"obs={in_shape} actions={out_shape}") - if len(in_shape) != 2 or len(out_shape) != 2: - print("== SUMMARY: FAIL (秩不为 2，后续检查跳过) ==") + meta = {prop.key: prop.value for prop in model.metadata_props} + declared_type = meta.get("rmcs_model_type", "").strip().lower() + report.check("model_type metadata", declared_type in ("", "mlp", "transformer", "generic"), + f"rmcs_model_type={declared_type or ''}") + if declared_type and args.model_type not in ("auto", "generic"): + report.check("model_type parameter/metadata", args.model_type == declared_type, + f"parameter={args.model_type} metadata={declared_type}") + model_type = declared_type if args.model_type == "auto" and declared_type else args.model_type + if model_type == "auto": + model_type = {1: "generic", 2: "mlp", 3: "transformer"}.get(len(in_shape), "generic") + rank_ok = ((model_type == "mlp" and len(in_shape) in (2, 3)) + or (model_type == "transformer" and len(in_shape) == 3) + or (model_type == "generic" and 1 <= len(in_shape) <= 3)) + report.check("input rank/model type", rank_ok, + f"model_type={model_type} obs_rank={len(in_shape)} actions_rank={len(out_shape)}") + if not rank_ok: + print("== SUMMARY: FAIL (输入 rank 与模型结构不匹配，后续检查跳过) ==") sys.exit(1) - batch_ok = all( - dim == 1 or dim is None or isinstance(dim, str) - for dim in (in_shape[0], out_shape[0]) - ) + batch_ok = all(dim == 1 or dim is None or isinstance(dim, str) + for dim in ((in_shape[0],) if len(in_shape) >= 2 else ())) + if len(out_shape) >= 2: + batch_ok = batch_ok and (out_shape[0] == 1 or out_shape[0] is None + or isinstance(out_shape[0], str)) report.check("batch dimension", batch_ok, - f"obs={in_shape[0]!r} actions={out_shape[0]!r}（允许 1 或动态维）") - model_obs, model_act = in_shape[1], out_shape[1] - if not isinstance(model_obs, int) or not isinstance(model_act, int) \ - or model_obs <= 0 or model_act <= 0: - report.check("concrete obs/action feature size", False, - f"obs={in_shape} actions={out_shape}（特征维必须是正整数）") + f"obs={in_shape[0] if len(in_shape) >= 2 else ''!r} " + f"actions={out_shape[0] if len(out_shape) >= 2 else ''!r}（允许 1 或动态维）") + + # 额外输入（如 attention mask）：形状校验 + 与 policy_server extra_inputs 对账 + extra_shapes = {value.name: _shape_of(value) for value in inputs if value.name != "obs"} + declared_extra: dict = {} + if args.config: + import yaml + document = yaml.safe_load(open(args.config, encoding="utf-8")) or {} + section = ((document.get("policy_server") or {}).get("ros__parameters") or {}) + raw_extra = section.get("extra_inputs") or {} + if not isinstance(raw_extra, dict): + report.check("extra_inputs section", False, f"extra_inputs={raw_extra!r} 必须是映射") + else: + declared_extra = raw_extra + extra_specs: dict = {} + if extra_shapes or declared_extra: + if args.config: + missing = sorted(set(extra_shapes) - set(declared_extra)) + unknown = sorted(set(declared_extra) - set(extra_shapes)) + report.check("extra_inputs match model", + not missing and not unknown, + f"模型缺声明={missing} 声明无对应输入={unknown}" + if (missing or unknown) else f"{sorted(extra_shapes)}") + else: + report.info("extra_inputs", + f"模型额外输入 {sorted(extra_shapes)}（无 --config，仅校验形状，dummy 用 1 填充）") + for name, shape in sorted(extra_shapes.items()): + ok = True + detail = f"{name}{shape}" + runtime_shape = [] + first_dim = 0 + if len(shape) >= 2: + if not (shape[0] == 1 or shape[0] is None or isinstance(shape[0], str)): + ok = False + detail += " batch 维必须为 1 或动态" + runtime_shape.append(1) + first_dim = 1 + elements = 1 + for dim in shape[first_dim:]: + if not isinstance(dim, int) or dim <= 0: + ok = False + detail += f" 非 batch 维必须是具体正整数（got {dim!r}）" + break + runtime_shape.append(dim) + elements *= dim + values = None + dtype = _dtype_name(next(value for value in inputs if value.name == name)) + if dtype not in ("float32", "int64", "bool"): + ok = False + detail += f" dtype={dtype}（运行时仅支持 float32/int64/bool）" + if ok and args.config and name in declared_extra: + raw_values = declared_extra[name] + if not isinstance(raw_values, list) or not raw_values: + ok = False + detail += " extra_inputs 值必须是非空列表" + else: + try: + if dtype == "float32": + if any(isinstance(item, bool) for item in raw_values): + raise ValueError + values = [float(item) for item in raw_values] + elif dtype == "int64": + if any(isinstance(item, bool) for item in raw_values): + raise ValueError + values = [int(item) for item in raw_values] + if any(isinstance(item, float) and item != int(item) + for item in raw_values): + raise ValueError + else: + if any(not isinstance(item, bool) for item in raw_values): + raise ValueError + values = list(raw_values) + except (TypeError, ValueError, OverflowError): + ok = False + values = None + detail += f" 不符合 {dtype} 元素类型" + else: + if dtype == "float32" and any(not math.isfinite(item) for item in values): + ok = False + detail += " 含非有限值" + values = None + elif len(values) != elements: + ok = False + detail += f" 值数量 {len(values)} != 元素数 {elements}" + values = None + report.check(f"extra input {name}", ok, detail) + if ok: + extra_specs[name] = (tuple(runtime_shape), values, dtype) + + layout_sequence = layout_feature = 0 + obs_layout_text = meta.get("rmcs_obs_layout", "") + if obs_layout_text.startswith("v3-history="): + try: + header, separator, entries = obs_layout_text.partition("|") + if not separator or not entries: + raise ValueError("缺少观测词条") + layout_sequence = int(header.removeprefix("v3-history=")) + if layout_sequence < 1: + raise ValueError("history 必须为正整数") + layout_observation = layout.obs_signature_dim(obs_layout_text) + layout_feature = layout_observation // layout_sequence + except (ValueError, layout.LayoutError) as exc: + report.check("rmcs_obs_layout dimensions", False, str(exc)) + layout_sequence = layout_feature = 0 + if len(in_shape) == 3: + report.check("rank-3 layout history", layout_sequence > 0 and layout_feature > 0, + f"layout T={layout_sequence} F={layout_feature}") + + history_length = layout.history_length(args.config, args.node) if args.config else 1 + metadata_sequence = metadata_feature = 0 + for key in ("rmcs_history_length", "rmcs_obs_frame_size"): + text = meta.get(key) + if text is None: + continue + try: + value = int(text) + except ValueError: + report.check(key, False, f"{key}={text!r} 不是正整数") + continue + report.check(key, value > 0, f"{key}={value}") + if key == "rmcs_history_length": + metadata_sequence = value + else: + metadata_feature = value + config_feature_size = obs_size // history_length if obs_size is not None else None + if args.config: + if layout_sequence: + report.check("layout history == config history_length", + layout_sequence == history_length, + f"layout={layout_sequence} history_length={history_length}") + if layout_feature and config_feature_size: + report.check("layout frame == config frame size", + layout_feature == config_feature_size, + f"layout={layout_feature} 单帧={config_feature_size}") + if metadata_sequence: + report.check("metadata history == config history_length", + metadata_sequence == history_length, + f"rmcs_history_length={metadata_sequence} history_length={history_length}") + if metadata_feature and config_feature_size: + report.check("metadata frame == config frame size", + metadata_feature == config_feature_size, + f"rmcs_obs_frame_size={metadata_feature} 单帧={config_feature_size}") + if args.sequence_length and args.sequence_length != history_length: + report.check("--sequence-length == history_length", False, + f"--sequence-length={args.sequence_length} history_length={history_length}") + if args.feature_size and config_feature_size and args.feature_size != config_feature_size: + report.check("--feature-size == config frame size", False, + f"--feature-size={args.feature_size} 单帧={config_feature_size}") + if layout_sequence and metadata_sequence: + report.check("metadata history == layout history", metadata_sequence == layout_sequence, + f"rmcs_history_length={metadata_sequence} layout={layout_sequence}") + if layout_feature and metadata_feature: + report.check("metadata frame == layout frame", metadata_feature == layout_feature, + f"rmcs_obs_frame_size={metadata_feature} layout={layout_feature}") + if layout_sequence and args.sequence_length: + report.check("--sequence-length == layout history", args.sequence_length == layout_sequence, + f"--sequence-length={args.sequence_length} layout={layout_sequence}") + if layout_feature and args.feature_size: + report.check("--feature-size == layout frame", args.feature_size == layout_feature, + f"--feature-size={args.feature_size} layout={layout_feature}") + + input_tail = in_shape[0:] if len(in_shape) == 1 else in_shape[1:] + sequence_length = args.sequence_length or metadata_sequence or layout_sequence or ( + history_length if len(in_shape) == 3 and args.config else None) + feature_size = args.feature_size or metadata_feature or layout_feature or ( + config_feature_size if len(in_shape) == 3 and args.config else None) + if len(in_shape) == 3: + declared_sequence, declared_feature = input_tail + if isinstance(declared_sequence, int) and declared_sequence > 0: + if sequence_length is not None: + report.check("model sequence dim == expected history", + declared_sequence == sequence_length, + f"model T={declared_sequence} expected={sequence_length}" + "（= --sequence-length/rmcs_history_length/history_length，" + "与运行时 onnx_runtime 同优先级）") + sequence_length = declared_sequence + if isinstance(declared_feature, int) and declared_feature > 0: + if feature_size is not None: + report.check("model feature dim == expected frame", + declared_feature == feature_size, + f"model F={declared_feature} expected={feature_size}") + feature_size = declared_feature + model_obs = sequence_length * feature_size if sequence_length and feature_size else None + else: + model_obs = input_tail[0] if len(input_tail) == 1 else None + output_tail = out_shape[0:] if len(out_shape) == 1 else out_shape[1:] + model_act = 1 + dynamic_output = False + for dim in output_tail: + if isinstance(dim, int) and dim > 0: + model_act *= dim + else: + dynamic_output = True + if dynamic_output: + model_act = act_size + concrete_ok = (isinstance(model_obs, int) and model_obs > 0 + and isinstance(model_act, int) and model_act > 0) + report.check("concrete obs/action size", concrete_ok, + f"obs={model_obs} actions={model_act} shape obs={in_shape} actions={out_shape}") + if not concrete_ok: print(f"== SUMMARY: {'PASS' if report.ok else 'FAIL'} ==") sys.exit(0 if report.ok else 1) @@ -279,11 +517,10 @@ def _dtype_name(value_info): report.info("model sizes", f"obs={model_obs} actions={model_act}（取自模型）") report.check("obs feature shape", model_obs == obs_size, - f"{in_shape} feature={model_obs} expected={obs_size}") + f"{in_shape} flattened={model_obs} expected={obs_size}") report.check("actions feature shape", model_act == act_size, - f"{out_shape} feature={model_act} expected={act_size}") + f"{out_shape} flattened={model_act} expected={act_size}") - meta = {prop.key: prop.value for prop in model.metadata_props} obs_meta, act_meta = meta.get("rmcs_obs_layout"), meta.get("rmcs_actions_layout") version_line = meta.get("policy_version") if version_line: @@ -362,7 +599,8 @@ def _dtype_name(value_info): else f"模型={declared_hash!r} 期望={layout.hex16(digest)}") _check_normalization(report, meta, obs_size) - _dummy_inference(report, args.model, obs_size, act_size) + _dummy_inference(report, args.model, obs_size, act_size, model_type, len(in_shape), + sequence_length, feature_size or obs_size, extra_specs) try: file_id = layout.model_id(args.model) diff --git a/tool/rl_layout.py b/tool/rl_layout.py index a67a222..f0f38d5 100644 --- a/tool/rl_layout.py +++ b/tool/rl_layout.py @@ -757,7 +757,10 @@ def write_metadata(model_path, updates: Dict[str, object]) -> None: def stamp_metadata(model_path, obs_sig: str, act_sig: str, obs_size: int, act_size: int, policy_version: Optional[str] = None, obs_mean=None, obs_std=None, - obs_clip=None, action_clip=None) -> Dict[str, str]: + obs_clip=None, action_clip=None, + model_type: Optional[str] = None, + sequence_length: Optional[int] = None, + feature_size: Optional[int] = None) -> Dict[str, str]: """按规范串盖章：返回实际写入的 metadata（含 policy_layout_hash）。""" updates = { "rmcs_obs_layout": obs_sig, @@ -774,6 +777,12 @@ def stamp_metadata(model_path, obs_sig: str, act_sig: str, obs_size: int, act_si updates["rmcs_obs_clip"] = format_scale(obs_clip) if action_clip is not None: updates["rmcs_action_clip"] = format_scale(action_clip) + if model_type: + updates["rmcs_model_type"] = str(model_type).lower() + if sequence_length is not None: + updates["rmcs_history_length"] = str(int(sequence_length)) + if feature_size is not None: + updates["rmcs_obs_frame_size"] = str(int(feature_size)) write_metadata(model_path, updates) return updates diff --git a/tool/stamp_layout_metadata.py b/tool/stamp_layout_metadata.py index b67346e..47aea94 100644 --- a/tool/stamp_layout_metadata.py +++ b/tool/stamp_layout_metadata.py @@ -14,6 +14,9 @@ rmcs_obs_mean/std 可选（--obs-mean/--obs-std，长度必须 == rl_obs_size） rmcs_obs_clip 可选单浮点（--obs-clip） rmcs_action_clip 可选单浮点（--action-clip） + rmcs_model_type 可选结构标识 mlp/transformer/generic（--model-type；缺省由部署端按 rank 推断） + rmcs_history_length 序列长度（缺省取 YAML history_length，部署端据此强制 模型T==history） + rmcs_obs_frame_size 单帧观测维度（缺省由 rl_obs_size/history_length 推导） 用法： python3 stamp_layout_metadata.py --model policy.onnx --from-config deploy.yaml @@ -63,6 +66,12 @@ def main() -> None: parser.add_argument("--obs-std", default=None, help="经验标准差，逗号分隔，长度同 obs_size") parser.add_argument("--obs-clip", type=float, default=None, help="观测 clip（单浮点，可选）") parser.add_argument("--action-clip", type=float, default=None, help="动作 clip（单浮点，可选）") + parser.add_argument("--model-type", choices=("mlp", "transformer", "generic"), default=None, + help="模型结构标识；不填则由部署端按输入 rank 推断") + parser.add_argument("--sequence-length", type=int, default=None, + help="序列长度（缺省从 YAML history_length 取值）") + parser.add_argument("--feature-size", type=int, default=None, + help="单帧观测维度（缺省由 rl_obs_size/history_length 推导）") args = parser.parse_args() try: @@ -81,12 +90,26 @@ def main() -> None: if value is not None and (not math.isfinite(value) or value <= 0.0): raise layout.LayoutError(f"{option}={value!r} 必须是有限正数") + history_length = layout.history_length(args.config, args.node) + sequence_length = args.sequence_length if args.sequence_length is not None else history_length + feature_size = args.feature_size if args.feature_size is not None else ( + obs_size // history_length) + if sequence_length < 1: + raise layout.LayoutError(f"--sequence-length 必须 >= 1") + if feature_size < 1: + raise layout.LayoutError(f"--feature-size 必须 >= 1") + if sequence_length * feature_size != obs_size: + raise layout.LayoutError( + f"sequence_length * feature_size={sequence_length * feature_size} != rl_obs_size={obs_size}") + before = layout.read_metadata(args.model) updates = layout.stamp_metadata( args.model, obs_sig, act_sig, obs_size, act_size, policy_version=args.policy_version, obs_mean=obs_mean, obs_std=obs_std, obs_clip=args.obs_clip, action_clip=args.action_clip, + model_type=args.model_type, sequence_length=sequence_length, + feature_size=feature_size, ) except layout.LayoutError as exc: print(f"ERROR: {exc}", file=sys.stderr) diff --git a/tool/stamp_model.sh b/tool/stamp_model.sh index 84348fa..fc9b392 100755 --- a/tool/stamp_model.sh +++ b/tool/stamp_model.sh @@ -5,6 +5,7 @@ # 用法: # stamp_model.sh [--version X] [--config ] [--node rl_bridge] # [--obs-mean a,b,... --obs-std a,b,...] [--obs-clip V] [--action-clip V] +# [--model-type mlp|transformer|generic] [--sequence-length N --feature-size N] # # 例: # stamp_model.sh wheel_leg_v3.onnx --version 3.0 @@ -59,7 +60,8 @@ while [[ $# -gt 0 ]]; do NODE="${2:?--node 需要值}" shift 2 ;; - --obs-mean | --obs-std | --obs-clip | --action-clip) + --obs-mean | --obs-std | --obs-clip | --action-clip \ + | --model-type | --sequence-length | --feature-size) EXTRA+=("$1" "${2:?$1 需要值}") shift 2 ;;