モータ校正 primitives の分離設計 (stats / fit / urdf_guard)

モータ校正 node を書くとき共通で要る 3 部品 (統計、摩擦モデル LS fit、URDF ベース安全ガード) を header-only で切り出す設計。将来ライブラリ化しやすい

要点: モータ校正 (friction fit / accel_ff_gain / homing 等) を C++ で書くと、どの校正でも共通で要る 3 部品 (mean_std、CoulombViscousFit、UrdfGuard) がある。これらを header-only の primitives として node から分離しておくと、将来複数 node で共有 or 汎用ライブラリに移送しやすい。

共通で要る 3 部品 #

Header	中身
`stats.hpp`	`mean_std(vec)` — 平均と population standard deviation
`coulomb_viscous_fit.hpp`	`duty(v) = c·sign(v) + b·v + g` の 3 変数 LS fit
`urdf_guard.hpp`	URDF joint limit を subscribe + 位置逸脱検出

`mean_std`#

サンプル区間の平均と σ を計算。σ 標準閾値 (offset + 3σ 等) を作る全ての stage で使う。population std ではなく sample std (n-1 分母) にすべきかは校正の目的で判断: 少数サンプルの推定なら n-1、baseline のノイズ幅なら n で十分。

`CoulombViscousFit`#

摩擦補償係数の推定。定常速度 v を印加した時の定常 duty d から:

d = c·sign(v) + b·v + g

の 3 変数を LS fit する。3x3 正規方程式を Cramer で解けば Eigen 依存が要らない。

c (coulomb): sign(v) の係数、静止摩擦相当 b (viscous): v の係数、粘性摩擦相当 g (bias): 定数項、重力バイアス相当

対称な速度セット ([-2,-1,-0.5,0.5,1,2] 等) を印加すれば c と b が clean に分離される。片側だけだと g と紛らわしい。

R² の解釈: 0.9 以下は「静止摩擦がランダム」「stick-slip」「加速中を含めた」等を疑う。dwell を長く取って定常だけを平均に採る。

`UrdfGuard`#

URDF joint limit を subscribe して、position が bounds を出た時に検出する。2 mode:

Inside mode ([lower + margin, upper - margin]):

校正中のような「安全域内で動かす」用途
逸脱で stage abort、mechanism を保護

Outside mode ([lower - margin, upper + margin]):

Homing のような「意図的に limit まで到達する」用途
逸脱 = 過走行 sanity check、runaway detection

内部で URDF listener (transient_local /robot_description を subscribe) を持ち、wait_for(timeout) で起動時 blocking 待機。

Header-only にする理由 #

依存が少ない (stats: <vector> + <cmath> のみ)
template 化しやすい (msg 型に対して generic な collector も同じ形で)
将来ライブラリに移送する時、ヘッダを別 dir に move + include path 変更で完了
動的リンクの複雑さが無い (小さい util なので inline で十分)

Namespace の付け方 (将来移送の準備)#

<pkg>::<domain> の 2 段:

namespace motor_test::tuning {   // 将来 fibril::tuning に sed 一発
inline void mean_std(...) { ... }
}

「calibration」は避ける (ADC 校正の慣用と混同する。既存 startCalibration() や CALIBRATION_CONFIG_* と衝突)。「tuning」「characterization」「identification」あたりが安全。

Directory 構成 #

<pkg>/include/<pkg>/tuning/
  stats.hpp
  coulomb_viscous_fit.hpp
  urdf_guard.hpp

CMake:

include_directories(include)   # public header 公開

これで:

同 package 内の node から <pkg>/tuning/stats.hpp> として include
別 package から使う時は <pkg> を depend + 同じパスで include

将来の library 移送手順 #

fibril_tuning 等の共通 package を作る時:

include/<pkg>/tuning/*.hpp → fibril_tuning/include/fibril/tuning/
sed -i 's|<pkg>::tuning|fibril::tuning|g' **/*.hpp **/*.cpp
sed -i 's|<pkg>/tuning|fibril/tuning|g' **/*.cpp
各 node の package.xml に <depend>fibril_tuning</depend> 追加
移送元の include/<pkg>/tuning/ は git rm

Header only なので symbol 名は変わらず、動作は変わらない。

校正 orchestrator との切り分け #

primitives は「1 stage 内の測定」を担当。stage 間の順序や yaml 出力は node 側の orchestrator の責任。この分離で:

orchestrator を Python から C++ に書き換えても primitives はそのまま
別モータ (linear / rotary) 用の別 orchestrator でも primitives は共有
primitives の unit test が orchestrator の integration test と分離できる

最終更新: 2026年8月16日