Dev/obs

.github/workflows/ci.yml

@@ -90,7 +90,7 @@ jobs:
               echo "FAIL: contract mismatch should have been rejected"
               exit 1
             fi
-            if ! grep -q "FAIL obs shape" <<<"${output}"; then
+            if ! grep -q "FAIL obs feature shape" <<<"${output}"; then
               echo "FAIL: expected a shape mismatch, got:"
               echo "${output}"
               exit 1

config/executor.yaml

@@ -5,6 +5,7 @@ rmcs_executor:
     components:
       - rmcs_core::hardware::WheelLegInfantryRL -> wheel_leg_infantry_rl
       - rmcs_core::controller::chassis::WheelLegChassisController -> wheel_leg_chassis_controller
+      - rmcs_core::controller::chassis::WheelLegRlImu -> wheel_leg_rl_imu
       - rmcs_rl::RlBridge -> rl_bridge
       - rmcs_rl::PolicyServerLauncher -> policy_server_launcher
 
@@ -19,65 +20,66 @@ wheel_leg_chassis_controller:
     yaw_rate_max: 3.0
     heading_kp: 3.0
     deadzone: 0.08
-    command_height_min: 0.20
-    command_height_max: 0.42
-    default_command_height: 0.22
+    command_height_min: 0.26
+    command_height_max: 0.28
+    default_command_height: 0.27
 
 rl_bridge:
   ros__parameters:
     history_length: 1
     rl_base: "/wheel_leg/rl"
-    policy_rate: 100.0
-    max_action_age: 0.02            # 缺省 = 2 / policy_rate；超时 → valid=0（绝不保持旧动作）
+    policy_rate: 50.0
+    max_action_age: 0.04            # 缺省 = 2 / policy_rate；超时 → valid=0（绝不保持旧动作）
 
-    rl_obs_size: 28
+    rl_obs_size: 35
     rl_action_size: 6
 
     enable_interface: "/wheel_leg/rl/enable"
     enable_default: false
     invalid_value: "nan"            # 必须加引号！不加会被 YAML 解析成 float，桥启动即失败
     reset_interface: "/chassis/reset_count"
-    expected_model_id: "0"
+    expected_model_id: "0xd4612f6cd48a6e9c"
 
-    joint_names: [left_hip_joint, left_knee_joint, left_wheel, right_hip_joint, right_knee_joint, right_wheel]
+    joint_names: [left_hip_joint, left_knee_joint, right_hip_joint, right_knee_joint, left_wheel, right_wheel]
     joint_base_path: "/wheel_leg"
     joint_angle_suffix: "/angle"
     joint_velocity_suffix: "/velocity"
     joint_torque_suffix: "/torque"
     default_joint_pos:
-      left_hip_joint: -0.5
-      left_knee_joint: -0.35
+      left_hip_joint: 0.0
+      left_knee_joint: 0.0
+      right_hip_joint: 0.0
+      right_knee_joint: 0.0
       left_wheel: 0.0
-      right_hip_joint: 0.5
-      right_knee_joint: 0.35
       right_wheel: 0.0
 
     observation_terms:
       - index=0  path=/chassis/control_velocity take=x
       - index=1  path=/chassis/control_velocity take=y
       - index=2  path=/chassis/control_velocity take=z
       - index=3  path=/chassis/control_height scale=5.0
-      - index=4  path=/wheel_leg/imu/angular_velocity take=vec3 scale=0.5
-      - index=7  path=/wheel_leg/imu/quaternion transform=projected_gravity
-      - index=10 type=joint_pos joints=left_hip_joint,left_knee_joint relative=true
-      - index=12 type=joint_pos joints=left_wheel zero=true
-      - index=13 type=joint_pos joints=right_hip_joint,right_knee_joint relative=true
-      - index=15 type=joint_pos joints=right_wheel zero=true
-      - index=16 type=joint_vel joints=left_hip_joint,left_knee_joint,left_wheel,right_hip_joint,right_knee_joint,right_wheel scale=0.1
+      # 训练 IMU 观测直接使用车身 Body xyz；URDF 轴向差异不对 IMU 做 90° 换轴。
+      - index=4  path=/wheel_leg/rl/imu/angular_velocity take=vec3 scale=0.5
+      - index=7  path=/wheel_leg/rl/imu/projected_gravity take=vec3
+      - index=10 type=joint_pos joints=left_hip_joint,left_knee_joint,right_hip_joint,right_knee_joint relative=true
+      - index=14 type=joint_pos joints=left_wheel,right_wheel zero=true
+      - index=16 type=joint_vel joints=left_hip_joint,left_knee_joint,right_hip_joint,right_knee_joint scale=0.1
+      - index=20 type=joint_vel joints=left_wheel,right_wheel scale=0.1
       - index=22 type=last_action
+      - index=28 type=constant value=1,0,0,0,0,0,0
 
     action_terms:
-      - index=0 output=/wheel_leg/rl/action/left_hip_joint
-      - index=1 output=/wheel_leg/rl/action/left_knee_joint
-      - index=2 output=/wheel_leg/rl/action/left_wheel
-      - index=3 output=/wheel_leg/rl/action/right_hip_joint
-      - index=4 output=/wheel_leg/rl/action/right_knee_joint
-      - index=5 output=/wheel_leg/rl/action/right_wheel
+      - index=0 output=/wheel_leg/rl/action/left_hip name=left_hip
+      - index=1 output=/wheel_leg/rl/action/left_knee name=left_knee
+      - index=2 output=/wheel_leg/rl/action/right_hip name=right_hip
+      - index=3 output=/wheel_leg/rl/action/right_knee name=right_knee
+      - index=4 output=/wheel_leg/rl/action/left_wheel name=left_wheel
+      - index=5 output=/wheel_leg/rl/action/right_wheel name=right_wheel
 
 policy_server:
   ros__parameters:
     rl_base: "/wheel_leg/rl"                    # 必须与桥一致
-    rl_model_path: "models/wheel_leg_V1.onnx"
+    rl_model_path: "models/wheel_leg_v1.onnx"
     input_name: "obs"
     output_name: "actions"
     normalization_from_metadata: true

include/rmcs_rl/onnxruntime_inference.hpp

@@ -79,12 +79,18 @@ class OnnxRuntimeInference {
             const auto input_shape = input_info.GetShape();
             const auto output_shape = output_info.GetShape();
             if (input_shape.size() != 2 || output_shape.size() != 2) {
-                error = "tensor rank must be 2 ([1, N])";
+                error = "tensor rank must be 2 ([batch, N])";
                 session_.reset();
                 return false;
             }
-            if (input_shape[0] != 1 || output_shape[0] != 1) {
-                error = "batch dimension must be 1";
+            if ((input_shape[0] != 1 && input_shape[0] != -1)
+                || (output_shape[0] != 1 && output_shape[0] != -1)) {
+                error = "batch dimension must be 1 or dynamic";
+                session_.reset();
+                return false;
+            }
+            if (input_shape[1] <= 0 || output_shape[1] <= 0) {
+                error = "feature dimensions must be concrete positive values";
                 session_.reset();
                 return false;
             }
@@ -106,8 +112,10 @@ class OnnxRuntimeInference {
 
             config_.input_size = model_input_size;
             config_.output_size = model_output_size;
-            input_shape_.assign(input_shape.begin(), input_shape.end());
-            output_shape_.assign(output_shape.begin(), output_shape.end());
+            // This deployment path runs one observation at a time. Normalize a
+            // symbolic batch dimension to 1 for Ort::CreateTensor below.
+            input_shape_ = {1, static_cast<std::int64_t>(model_input_size)};
+            output_shape_ = {1, static_cast<std::int64_t>(model_output_size)};
             input_buffer_.assign(config_.input_size, 0.0F);
             output_buffer_.assign(config_.output_size, 0.0F);
             memory_info_ = Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault);

src/rl_bridge/observation.cpp

@@ -90,8 +90,10 @@ bool build_observation(
                 if (!term.zero) {
                     if (!read_double(slots[term.joint_slots[j]], value))
                         return false;
-                    if (term.relative)
-                        value -= term.joint_defaults[j];
+                    if (term.relative) {
+                        const double delta = value - term.joint_defaults[j];
+                        value = std::atan2(std::sin(delta), std::cos(delta));
+                    }
                 }
                 push(
                     term.index + j, value, term.scale, term.has_clip, term.clip_min, term.clip_max,

tool/check_policy_contract.py

@@ -4,8 +4,8 @@
 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;
-      concrete (non-dynamic) shapes. Layout metadata is OPTIONAL:
+      model loads; one input "obs" / one output "actions"; float32; rank 2;
+      batch 1 or dynamic; concrete feature dimensions. 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
@@ -257,11 +257,17 @@ def _dtype_name(value_info):
         print("== SUMMARY: FAIL (秩不为 2，后续检查跳过) ==")
         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])
+    )
+    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 size", False,
-                     f"obs={in_shape} actions={out_shape}（动态或符号维，无法自检）")
+        report.check("concrete obs/action feature size", False,
+                     f"obs={in_shape} actions={out_shape}（特征维必须是正整数）")
         print(f"== SUMMARY: {'PASS' if report.ok else 'FAIL'} ==")
         sys.exit(0 if report.ok else 1)
 
@@ -272,12 +278,10 @@ def _dtype_name(value_info):
     if args.obs is None and args.act is None:
         report.info("model sizes", f"obs={model_obs} actions={model_act}（取自模型）")
 
-    report.check("obs shape", in_shape == [1, obs_size],
-                 f"{in_shape} == [1,{obs_size}]" if in_shape == [1, obs_size]
-                 else f"{in_shape} != [1,{obs_size}]")
-    report.check("actions shape", out_shape == [1, act_size],
-                 f"{out_shape} == [1,{act_size}]" if out_shape == [1, act_size]
-                 else f"{out_shape} != [1,{act_size}]")
+    report.check("obs feature shape", model_obs == obs_size,
+                 f"{in_shape} feature={model_obs} expected={obs_size}")
+    report.check("actions feature shape", model_act == act_size,
+                 f"{out_shape} feature={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")

tool/install_model_tools.sh

@@ -0,0 +1,63 @@
+#!/usr/bin/env bash
+# 安装 ONNX 迭代工具链（仅开发/部署用）：
+#   在容器里建一个固定 venv，并装 onnx + pyyaml，供 stamp_model.sh / check_policy_contract 使用。
+#   运行期 policy_server 只需要 libonnxruntime.so（由 install_rl_deps.sh 负责），与本工具隔离。
+#
+# 用法:
+#   bash install_model_tools.sh
+# 可选环境变量:
+#   RMCS_TOOLS_VENV   工具 venv 目录（默认 /opt/rmcs-tools/venv）
+#   RMCS_ONNX_PYTHON  若已指向一个带 onnx+yaml 的 python，则直接跳过安装
+set -euo pipefail
+
+VENV_DIR="${RMCS_TOOLS_VENV:-/opt/rmcs-tools/venv}"
+
+log() { printf '[install_model_tools] %s\n' "$*"; }
+die() { printf '[install_model_tools] ERROR: %s\n' "$*" >&2; exit 1; }
+
+run_privileged() {
+    if command -v sudo >/dev/null 2>&1; then
+        sudo "$@"
+    else
+        "$@"
+    fi
+}
+
+# 已有可用的 onnx python → 跳过
+if [[ -n "${RMCS_ONNX_PYTHON:-}" ]] \
+    && "$RMCS_ONNX_PYTHON" -c "import onnx, yaml" >/dev/null 2>&1; then
+    log "RMCS_ONNX_PYTHON 已可用，跳过安装: $RMCS_ONNX_PYTHON"
+    exit 0
+fi
+
+command -v python3 >/dev/null 2>&1 || die "找不到 python3"
+
+# 建 venv；若 python3-venv 缺失则先安装它
+if ! { [[ -x "$VENV_DIR/bin/python" ]] \
+        && "$VENV_DIR/bin/python" -c "import onnx, yaml" >/dev/null 2>&1; }; then
+    if ! run_privileged python3 -m venv "$VENV_DIR" >/dev/null 2>&1 \
+        || ! "$VENV_DIR/bin/python" -m pip --version >/dev/null 2>&1; then
+        log "python3 venv 不可用，安装 python3-venv ..."
+        run_privileged apt-get update
+        run_privileged apt-get install -y python3-venv
+        run_privileged rm -rf "$VENV_DIR"
+        run_privileged python3 -m venv "$VENV_DIR" || die "创建 venv 失败: $VENV_DIR"
+    fi
+
+    log "安装 onnx + pyyaml 到 $VENV_DIR ..."
+    run_privileged "$VENV_DIR/bin/python" -m pip install --upgrade pip
+    run_privileged "$VENV_DIR/bin/python" -m pip install --upgrade onnx pyyaml
+fi
+
+"$VENV_DIR/bin/python" -c "import onnx, yaml; print('[install_model_tools] onnx', onnx.__version__)" \
+    || die "onnx 安装校验失败"
+
+cat <<EOF
+
+[install_model_tools] 完成。
+  工具 venv : $VENV_DIR
+  python    : $VENV_DIR/bin/python
+
+stamp_model.sh 会自动探测该 venv；如需手动指定：
+  export RMCS_ONNX_PYTHON=$VENV_DIR/bin/python
+EOF