diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e0359cc..5d4b39c 100644 --- a/.github/workflows/ci.yml +++ b/.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 diff --git a/config/executor.yaml b/config/executor.yaml index 98e4fee..d5559fd 100644 --- a/config/executor.yaml +++ b/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,37 +20,37 @@ 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: @@ -57,27 +58,28 @@ rl_bridge: - 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 diff --git a/include/rmcs_rl/onnxruntime_inference.hpp b/include/rmcs_rl/onnxruntime_inference.hpp index fd7dd3d..6555ff4 100644 --- a/include/rmcs_rl/onnxruntime_inference.hpp +++ b/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(model_input_size)}; + output_shape_ = {1, static_cast(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); diff --git a/models/deformable_sps_V2.onnx b/models/deformable_sps_V2.onnx new file mode 100644 index 0000000..2d092a8 Binary files /dev/null and b/models/deformable_sps_V2.onnx differ diff --git a/models/wheel_leg_v1.onnx b/models/wheel_leg_v1.onnx new file mode 100644 index 0000000..cc6b83f Binary files /dev/null and b/models/wheel_leg_v1.onnx differ diff --git a/models/wheel_leg_v2.onnx b/models/wheel_leg_v2.onnx new file mode 100644 index 0000000..092cd14 Binary files /dev/null and b/models/wheel_leg_v2.onnx differ diff --git a/src/rl_bridge/observation.cpp b/src/rl_bridge/observation.cpp index 45f12c1..16a2fe9 100644 --- a/src/rl_bridge/observation.cpp +++ b/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, diff --git a/tool/check_policy_contract.py b/tool/check_policy_contract.py index 1c8ef8c..c9dc793 100644 --- a/tool/check_policy_contract.py +++ b/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") diff --git a/tool/install_model_tools.sh b/tool/install_model_tools.sh new file mode 100755 index 0000000..c45eb8c --- /dev/null +++ b/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 < [--version X] [--config ] [--node rl_bridge] +# [--obs-mean a,b,... --obs-std a,b,...] [--obs-clip V] [--action-clip V] +# +# 例: +# stamp_model.sh wheel_leg_v3.onnx --version 3.0 +set -euo pipefail + +TOOL_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PKG_DIR="$(cd "$TOOL_DIR/.." && pwd)" +WS_DIR="$(cd "$PKG_DIR/../.." && pwd)" +DEFAULT_MODELS_DIR="$PKG_DIR/models" +DEFAULT_CONFIG="$WS_DIR/src/rmcs_bringup/config/wheel-leg-infantry-rl.yaml" +VENV_PY="${RMCS_TOOLS_VENV:-/opt/rmcs-tools/venv}/bin/python" + +log() { printf '[stamp_model] %s\n' "$*"; } +die() { printf '[stamp_model] ERROR: %s\n' "$*" >&2; exit 1; } + +usage() { + sed -n '2,11p' "${BASH_SOURCE[0]}" +} + +find_python() { + local cand + for cand in "${RMCS_ONNX_PYTHON:-}" "$VENV_PY" "$(command -v python3 || true)"; do + [[ -n "$cand" && -x "$cand" ]] || continue + if "$cand" -c "import onnx, yaml" >/dev/null 2>&1; then + printf '%s' "$cand" + return 0 + fi + done + return 1 +} + +MODEL="" +VERSION="" +CONFIG="$DEFAULT_CONFIG" +NODE="rl_bridge" +EXTRA=() +while [[ $# -gt 0 ]]; do + case "$1" in + -h | --help) + usage + exit 0 + ;; + --version) + VERSION="${2:?--version 需要值}" + shift 2 + ;; + --config) + CONFIG="${2:?--config 需要值}" + shift 2 + ;; + --node) + NODE="${2:?--node 需要值}" + shift 2 + ;; + --obs-mean | --obs-std | --obs-clip | --action-clip) + EXTRA+=("$1" "${2:?$1 需要值}") + shift 2 + ;; + -*) + die "未知选项 $1" + ;; + *) + if [[ -z "$MODEL" ]]; then MODEL="$1"; else die "多余参数 $1"; fi + shift + ;; + esac +done + +[[ -n "$MODEL" ]] || { usage; die "缺少模型名"; } +[[ -f "$CONFIG" ]] || die "配置不存在: $CONFIG" + +PY="$(find_python)" \ + || die "找不到带 onnx 的 python；先运行 install_model_tools.sh，或 export RMCS_ONNX_PYTHON=/bin/python" + +case "$MODEL" in +*/*) MODEL_PATH="$MODEL" ;; +*) MODEL_PATH="$DEFAULT_MODELS_DIR/$MODEL" ;; +esac +[[ -f "$MODEL_PATH" ]] || die "模型不存在: $MODEL_PATH" + +# 只读预检：模型已盖章时，提示其布局与当前 YAML 是否一致（不阻断，按约定继续盖 + 警告）。 +layout_state="$( + PYTHONPATH="$TOOL_DIR" "$PY" - "$MODEL_PATH" "$CONFIG" "$NODE" <<'PY' +import sys +import rl_layout as L + +model, config, node = sys.argv[1], sys.argv[2], sys.argv[3] +try: + meta = L.read_metadata(model) +except Exception as exc: # noqa: BLE001 + print(f"UNREADABLE {type(exc).__name__}: {exc}") + raise SystemExit(0) + +obs_sig, act_sig = meta.get("rmcs_obs_layout"), meta.get("rmcs_actions_layout") +if not obs_sig or not act_sig: + print("UNSTAMPED") + raise SystemExit(0) + +obs, act, _obs_size, act_size = L.load_config(config, node) +want_obs = L.obs_signature(obs, act_size, L.history_length(config, node)) +want_act = L.action_signature(act) +print("MATCH" if (obs_sig == want_obs and act_sig == want_act) else "MISMATCH") +PY +)" +case "$layout_state" in +MATCH) : ;; +UNSTAMPED) log "警告：模型未盖章，将按当前 YAML 布局盖章（请确认训练布局与当前 YAML 逐条一致）" ;; +MISMATCH) log "警告：模型已有盖章布局与当前 YAML 不一致！将用当前 YAML 覆盖其布局签名，请确认这是你要的" ;; +*) log "警告：无法读取模型 metadata（$layout_state），仍继续盖章" ;; +esac + +STAMP_ARGS=(--model "$MODEL_PATH" --from-config "$CONFIG" --node "$NODE") +[[ -n "$VERSION" ]] && STAMP_ARGS+=(--policy-version "$VERSION") +[[ ${#EXTRA[@]} -gt 0 ]] && STAMP_ARGS+=("${EXTRA[@]}") + +STAMP_OUT="$("$PY" "$TOOL_DIR/stamp_layout_metadata.py" "${STAMP_ARGS[@]}")" +printf '%s\n' "$STAMP_OUT" +NEW_ID="$(printf '%s\n' "$STAMP_OUT" | sed -n 's/^model_id[[:space:]]*:[[:space:]]*//p')" +[[ -n "$NEW_ID" ]] || die "未能从盖章输出解析 model_id" + +log "复验 check_policy_contract ..." +"$PY" "$TOOL_DIR/check_policy_contract.py" "$MODEL_PATH" \ + --config "$CONFIG" --node "$NODE" --expect-model-id "$NEW_ID" \ + || die "复验失败：模型已盖章，但契约校验未通过（见上面的 FAIL 项）" + +cat <