From a6fde6bc7a255fdba852925daa2d4f58b111ff25 Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 12:53:03 +0900 Subject: [PATCH 1/7] feat(actuator): runtime actuator swap, direct-control session, backlash and user-defined stage Co-Authored-By: Claude Opus 5.5 --- core/include/vdsim/actuator.hpp | 44 +++++++++++ core/include/vdsim/sim_session.hpp | 43 ++++++++++ core/include/vdsim/vec_session.hpp | 11 +++ core/src/actuator_model.cpp | 79 ++++++++++++++++++- core/src/params.cpp | 2 + core/src/sim_session.cpp | 114 ++++++++++++++++++++++++++- core/src/vec_session.cpp | 23 ++++++ tests/unit/test_actuator.cpp | 62 +++++++++++++++ tests/unit/test_sim_session.cpp | 122 +++++++++++++++++++++++++++++ 9 files changed, 494 insertions(+), 6 deletions(-) diff --git a/core/include/vdsim/actuator.hpp b/core/include/vdsim/actuator.hpp index b0b2ee1b..ba6075ba 100644 --- a/core/include/vdsim/actuator.hpp +++ b/core/include/vdsim/actuator.hpp @@ -15,6 +15,7 @@ #pragma once #include "vdsim/snapshot.hpp" +#include #include #include "vdsim/control.hpp" @@ -22,6 +23,22 @@ namespace vdsim { +// User-defined channel stage. Runs after the built-in chain of its channel +// (dead zone, dead time, backlash, lag, rate limit, saturation) on that chain's +// output; leave the built-in fields at their defaults to replace the chain. +// The prototype held in ChannelActuator is never stepped: every ActuatorModel +// clone()s its own running instance, so per-env sessions never share state. +class IChannelModel { +public: + virtual ~IChannelModel() = default; + virtual double step(double u, double dt) = 0; + virtual void reset() = 0; + virtual std::unique_ptr clone() const = 0; + // Snapshot support (optional): flat state vector, any length. + virtual std::vector get_state() const { return {}; } + virtual void set_state(const std::vector&) {} +}; + // Per-channel first-order-plus-dead-time + rate limit + saturation. struct ChannelActuator { double dead_time_s {0.0}; // pure transport delay L [s] (0 = none) @@ -32,6 +49,11 @@ struct ChannelActuator { // clearance). Applies to one-sided 0..1 channels. double out_min {-1e12}; // saturation double out_max { 1e12}; + double backlash {0.0}; // play / rate-independent hysteresis width [-] + // (full band: output follows the input only + // after it has moved width/2 past the output); + // sits between dead time and the lag. 0 = off. + std::shared_ptr user; // optional user stage (see above) }; // LuGre dynamic friction (Stribeck + presliding hysteresis + stick-slip). @@ -86,6 +108,15 @@ class ActuatorModel { void save_aux(std::vector& v) const; void restore_aux(const std::vector& v, std::size_t& p); + // Swap parameters on a live model WITHOUT clearing its memory (transport + // rings, lag / rate state, servo + LuGre bristle, brake temperature). + // initialize() resets all of that, so it cannot express an actuator whose + // characteristics change *during* a rollout -- degradation, fault + // injection, a latency step. Transport rings grow in place, history kept, + // when the dead time increases. + void set_params(const ActuatorParams& p); + const ActuatorParams& params() const { return p_; } + double brake_temperature() const { return brake_T_; } double steer_angle() const { return steer_pos_; } @@ -109,6 +140,17 @@ class ActuatorModel { } }; FracDelay steer_buf_, throttle_buf_, brake_buf_; + // Backlash (play operator) state per channel. + double steer_bl_ {0.0}, throttle_bl_ {0.0}, brake_bl_ {0.0}; + // Running instance of each channel's user stage (cloned from the prototype + // in ChannelActuator::user; rebuilt only when the prototype pointer changes). + struct UserSlot { + std::shared_ptr proto; + std::unique_ptr inst; + }; + UserSlot steer_user_, throttle_user_, brake_user_; + static void sync_user(UserSlot& s, const std::shared_ptr& want); + void sync_users(); // First-order lag states. double steer_lag_ {0.0}, throttle_lag_ {0.0}, brake_lag_ {0.0}; // Previous outputs (for rate limit) and steering servo states. @@ -118,6 +160,8 @@ class ActuatorModel { bool initialized_ {false}; double push_delay(FracDelay& buf, double v, double L, double dt) const; + // Grow a transport ring to hold delay L without discarding its history. + void grow_ring(FracDelay& buf, double L, double dt); }; // Feedback-path transport delay: returns the state as seen by the controller diff --git a/core/include/vdsim/sim_session.hpp b/core/include/vdsim/sim_session.hpp index e8fbb861..2eb8e3f4 100644 --- a/core/include/vdsim/sim_session.hpp +++ b/core/include/vdsim/sim_session.hpp @@ -46,6 +46,11 @@ struct SimConfig { double sensor_delay_s {0.0}; // feedback transport delay (0 = none) double nominal_dt {0.005}; // for delay-buffer sizing SessionKind session_kind {SessionKind::Standard}; + // DirectControl only: wheel-torque totals [Nm] that map to throttle = 1 / + // brake = 1 when the actuator filters the CmdL1 torque channels. <= 0 = + // derive from VehicleParams (max_motor_torque*final_drive_ratio, max_brake_torque). + double drive_torque_ref {0.0}; + double brake_torque_ref {0.0}; }; // Thread-safe snapshot of one tick's result (true + measured state plus the @@ -85,6 +90,12 @@ struct DomainRandomization { double tire_stiffness_scale {1.0}; // B_long, B_lat, cornering_stiffness double tire_mu_scale {1.0}; // TireParams::mu_nominal double sensor_delay_s {-1.0}; // [s], <0 = keep the configured value + // Actuator channels. Rebuilt at reset() from the as-built ActuatorParams, + // so the scales never compound across episodes. + double steer_delay_scale {1.0}; // x steer ChannelActuator::dead_time_s + double steer_tau_scale {1.0}; // x steer ChannelActuator::tau_s + double rate_limit_scale {1.0}; // x rate_limit on all three channels + double brake_deadzone_add {0.0}; // [-] added to brake dead_zone (pad wear) }; class SimSession { @@ -118,6 +129,22 @@ class SimSession { void set_randomization(const DomainRandomization& d); DomainRandomization randomization() const; + // Swap the actuator parameters on a live session. Unlike construction this + // keeps every actuator memory, so dead time / lag / rate limit / dead-zone + // can change *during* an episode (degradation, fault injection). The next + // reset() returns to the as-built parameters with DomainRandomization + // applied -- this setter is an in-episode override, not a new baseline. + void set_actuator(const ActuatorParams& p); + ActuatorParams actuator() const; + + // Retune the L5-L8 cascade (speed/accel PI, pure-pursuit lookahead, yaw-rate + // PI) on a live session. Integrators and pursuit progress are kept; a + // reset() keeps the new gains. Throws std::invalid_argument on a negative + // or non-finite gain. No effect on a DirectControl session, which latches + // CmdL1 and never runs the cascade. + void set_cascade_gains(const CascadeGains& g); + CascadeGains cascade_gains() const; + // R6: this session's stochastic stream (sensor noise). Re-armed at every // reset(), so episode k with the same seed is bitwise identical no matter // what ran before it. Unset = the SensorParams seed from construction. @@ -150,6 +177,7 @@ class SimSession { std::unique_ptr ground_; std::unique_ptr network_; // ECU/CAN network (deadtime + drop) ActuatorModel actuator_; + ActuatorParams base_actuator_ {}; // as-built; randomization rebuilds from this SensorDelay sensor_; SensorModel sensors_; CascadeController cascade_; // Lc5-L8 → CmdL4 (with measured-state feedback) @@ -186,6 +214,12 @@ class SimSession { std::array wheel_alpha_peak_ {{0,0,0,0}}; std::array wheel_kappa_peak_ {{0,0,0,0}}; SensorMeas sensors_meas_ {}; + // DirectControl longitudinal channel: torque normalisation and the last + // non-zero per-wheel share, so delayed / lagged torque keeps its distribution. + double drive_ref_ {0.0}, brake_ref_ {0.0}; + std::array w_drive_ {{0.25, 0.25, 0.25, 0.25}}; + std::array w_brake_ {{0.25, 0.25, 0.25, 0.25}}; + std::array w0_ {{0.25, 0.25, 0.25, 0.25}}; double sim_time_ {0.0}; SessionKind session_kind_ {SessionKind::Standard}; std::chrono::steady_clock::time_point last_input_tp_ {std::chrono::steady_clock::now()}; @@ -209,6 +243,15 @@ struct DirectControlSessionOptions { // behaviour), "L3" 14-DOF ride, "L4" 14-DOF + KC tables. // The control contract does not change with the level. std::string level {"L2"}; + // Actuator chain between the latched command and the plant. Default = + // identity (bit-identical to a session without it). Steering is filtered + // always; the CmdL1 torque channels are filtered as normalised throttle / + // brake once a throttle or brake channel is non-trivial. + ActuatorParams actuator {}; + // Wheel-torque totals [Nm] equal to throttle = 1 / brake = 1; <= 0 = derive + // from the vehicle (max_motor_torque*final_drive_ratio, max_brake_torque). + double drive_torque_ref {0.0}; + double brake_torque_ref {0.0}; }; std::unique_ptr make_direct_control_session( diff --git a/core/include/vdsim/vec_session.hpp b/core/include/vdsim/vec_session.hpp index d75fe012..bc4d4b60 100644 --- a/core/include/vdsim/vec_session.hpp +++ b/core/include/vdsim/vec_session.hpp @@ -112,6 +112,17 @@ class VecSession { // R6: one independent stochastic stream per env (size 1 = seed, seed+1, ...). void set_seeds(const std::vector& seeds); + // Per-env actuator swap (size 1 broadcasts). Takes effect immediately + // and keeps every actuator memory (transport rings, first-order lag, + // rate-limit state, steering servo + LuGre bristle, brake temperature), + // so this is the in-episode degradation / fault channel for a rollout. + // set_randomizations() is the other half: it waits for the next reset(). + void set_actuators(const std::vector& p); + std::vector actuators() const; + + // Per-env cascade retune (size 1 broadcasts); keeps the controller memory. + void set_cascade_gains(const std::vector& g); + // Advance every session by `repeat` ticks of `dt`. repeat > 1 is the RL // action-hold (frame skip): it stays inside the worker, so one barrier // covers the whole control interval instead of one per physics tick. diff --git a/core/src/actuator_model.cpp b/core/src/actuator_model.cpp index 6c99172e..7517225d 100644 --- a/core/src/actuator_model.cpp +++ b/core/src/actuator_model.cpp @@ -29,6 +29,16 @@ inline double first_order(double y, double u, double tau, double dt) { return a * y + (1.0 - a) * u; } +// Play (backlash) operator: the output holds until the input has moved width/2 +// past it, then follows at that offset. width <= 0 -> passthrough, state untouched. +inline double play(double& y, double u, double width) { + if (width <= 0.0) return u; + const double h = 0.5 * width; + if (u - y > h) y = u - h; + else if (y - u > h) y = u + h; + return y; +} + // Piecewise-linear lookup (ascending x). Empty table -> default. double lerp_table(const std::vector& xs, const std::vector& ys, double x, double def) { @@ -69,16 +79,58 @@ double ActuatorModel::push_delay(FracDelay& buf, double v, void ActuatorModel::initialize(const ActuatorParams& p, double nominal_dt) { p_ = p; nominal_dt_ = (nominal_dt > 1e-6) ? nominal_dt : 0.005; + sync_users(); reset(); // sets brake_T_ = T_ambient initialized_ = true; } +void ActuatorModel::sync_user(UserSlot& s, + const std::shared_ptr& want) { + if (s.proto == want) return; + s.proto = want; + s.inst = want ? want->clone() : nullptr; +} + +void ActuatorModel::sync_users() { + sync_user(steer_user_, p_.steer.ch.user); + sync_user(throttle_user_, p_.throttle.user); + sync_user(brake_user_, p_.brake.ch.user); +} + +void ActuatorModel::grow_ring(FracDelay& b, double L, double dt) { + if (L <= 0.0 || dt <= 1e-9) return; + const int need = static_cast(std::floor(L / dt)) + 3; + const int sz = static_cast(b.buf.size()); + if (sz >= need) return; + // FracDelay::resize() zero-fills, which would wipe the delay line at the + // exact moment the delay grows. Re-lay the samples oldest-first instead. + std::vector out(static_cast(need), 0.0); + for (int i = 0; i < sz; ++i) + out[static_cast(i)] = b.ago(sz - 1 - i); + b.buf.swap(out); + b.w = sz; // next write slot; sz < need by construction +} + +void ActuatorModel::set_params(const ActuatorParams& p) { + p_ = p; + sync_users(); + // Pre-grow here so push_delay()'s own resize() (which zero-fills) never + // fires as a side effect of a mid-rollout dead-time increase. + grow_ring(steer_buf_, p_.steer.ch.dead_time_s, nominal_dt_); + grow_ring(throttle_buf_, p_.throttle.dead_time_s, nominal_dt_); + grow_ring(brake_buf_, p_.brake.ch.dead_time_s, nominal_dt_); + initialized_ = true; +} + void ActuatorModel::reset() { steer_buf_.clear(); throttle_buf_.clear(); brake_buf_.clear(); steer_lag_ = throttle_lag_ = brake_lag_ = 0.0; steer_out_ = throttle_out_ = brake_out_ = 0.0; steer_pos_ = steer_vel_ = lugre_z_ = 0.0; brake_T_ = p_.brake.T_ambient; + steer_bl_ = throttle_bl_ = brake_bl_ = 0.0; + for (UserSlot* u : {&steer_user_, &throttle_user_, &brake_user_}) + if (u->inst) u->inst->reset(); } CmdL4 ActuatorModel::apply(const CmdL4& desired, double speed_mps, double dt) { @@ -88,6 +140,7 @@ CmdL4 ActuatorModel::apply(const CmdL4& desired, double speed_mps, double dt) { // ---------------- Steering ---------------- double s_cmd = push_delay(steer_buf_, desired.steer_angle_wheel, p_.steer.ch.dead_time_s, dt); + s_cmd = play(steer_bl_, s_cmd, p_.steer.ch.backlash); if (p_.steer.friction.enabled) { // Torque-servo steering with LuGre friction (substepped). const auto& L = p_.steer.friction; @@ -131,7 +184,8 @@ CmdL4 ActuatorModel::apply(const CmdL4& desired, double speed_mps, double dt) { steer_out_ = saturate(y, p_.steer.ch.out_min, p_.steer.ch.out_max); steer_pos_ = steer_out_; } - out.steer_angle_wheel = steer_out_; + out.steer_angle_wheel = steer_user_.inst ? steer_user_.inst->step(steer_out_, dt) + : steer_out_; // ---------------- Throttle ---------------- { @@ -143,13 +197,16 @@ CmdL4 ActuatorModel::apply(const CmdL4& desired, double speed_mps, double dt) { } double t_cmd = push_delay(throttle_buf_, t_in, p_.throttle.dead_time_s, dt); + t_cmd = play(throttle_bl_, t_cmd, p_.throttle.backlash); double y = first_order(throttle_lag_, t_cmd, p_.throttle.tau_s, dt); throttle_lag_ = y; y = rate_limit(throttle_out_, y, p_.throttle.rate_limit, dt); const double lo = std::max(0.0, p_.throttle.out_min); const double hi = std::min(1.0, p_.throttle.out_max); throttle_out_ = saturate(y, lo, hi); - out.throttle = throttle_out_; + out.throttle = throttle_user_.inst + ? saturate(throttle_user_.inst->step(throttle_out_, dt), 0.0, 1.0) + : throttle_out_; } // ---------------- Brake ---------------- @@ -161,6 +218,7 @@ CmdL4 ActuatorModel::apply(const CmdL4& desired, double speed_mps, double dt) { b_in = (b_in > dz) ? (b_in - dz) / std::max(1e-6, 1.0 - dz) : 0.0; } double b_cmd = push_delay(brake_buf_, b_in, p_.brake.ch.dead_time_s, dt); + b_cmd = play(brake_bl_, b_cmd, p_.brake.ch.backlash); double y = first_order(brake_lag_, b_cmd, p_.brake.ch.tau_s, dt); brake_lag_ = y; y = rate_limit(brake_out_, y, p_.brake.ch.rate_limit, dt); @@ -177,6 +235,8 @@ CmdL4 ActuatorModel::apply(const CmdL4& desired, double speed_mps, double dt) { const double hi = std::min(1.0, p_.brake.ch.out_max); brake_out_ = saturate(y, lo, hi); out.brake = saturate(brake_out_ * mu_scale, 0.0, 1.0); + if (brake_user_.inst) + out.brake = saturate(brake_user_.inst->step(out.brake, dt), 0.0, 1.0); } return out; @@ -226,6 +286,13 @@ void ActuatorModel::save_aux(std::vector& v) const { v.push_back(steer_out_); v.push_back(throttle_out_); v.push_back(brake_out_); v.push_back(steer_pos_); v.push_back(steer_vel_); v.push_back(lugre_z_); v.push_back(brake_T_); + v.push_back(steer_bl_); v.push_back(throttle_bl_); v.push_back(brake_bl_); + for (const UserSlot* u : {&steer_user_, &throttle_user_, &brake_user_}) { + const std::vector st = u->inst ? u->inst->get_state() + : std::vector{}; + v.push_back(static_cast(st.size())); + v.insert(v.end(), st.begin(), st.end()); + } } void ActuatorModel::restore_aux(const std::vector& v, std::size_t& p) { @@ -242,6 +309,14 @@ void ActuatorModel::restore_aux(const std::vector& v, std::size_t& p) { brake_out_ = snap::get(v, p); steer_pos_ = snap::get(v, p); steer_vel_ = snap::get(v, p); lugre_z_ = snap::get(v, p); brake_T_ = snap::get(v, p); + steer_bl_ = snap::get(v, p); throttle_bl_ = snap::get(v, p); + brake_bl_ = snap::get(v, p); + for (UserSlot* u : {&steer_user_, &throttle_user_, &brake_user_}) { + const std::size_t n = static_cast(snap::get(v, p)); + std::vector st(n); + for (std::size_t i = 0; i < n; ++i) st[i] = snap::get(v, p); + if (u->inst) u->inst->set_state(st); + } } void SensorDelay::save_aux(std::vector& v) const { diff --git a/core/src/params.cpp b/core/src/params.cpp index 8951037b..160ef198 100644 --- a/core/src/params.cpp +++ b/core/src/params.cpp @@ -484,12 +484,14 @@ void pull_noise(const YAML::Node& root, const char* key, SensorNoise& n) { pull(g, "noise_std", n.noise_std); pull(g, "bias", n.bias); pull(g, "bias_rw", n.bias_rw); + pull(g, "lag_tau_s", n.lag_tau_s); } void emit_noise(YAML::Emitter& out, const char* key, const SensorNoise& n) { out << YAML::Key << key << YAML::Value << YAML::BeginMap << YAML::Key << "noise_std" << YAML::Value << n.noise_std << YAML::Key << "bias" << YAML::Value << n.bias << YAML::Key << "bias_rw" << YAML::Value << n.bias_rw + << YAML::Key << "lag_tau_s" << YAML::Value << n.lag_tau_s << YAML::EndMap; } } // namespace diff --git a/core/src/sim_session.cpp b/core/src/sim_session.cpp index 944ac370..0427a4e3 100644 --- a/core/src/sim_session.cpp +++ b/core/src/sim_session.cpp @@ -2,6 +2,7 @@ #include "vdsim/sim_session.hpp" #include +#include namespace vdsim { @@ -12,6 +13,19 @@ void apply_vehicle_steer_deadtime(SimConfig& cfg, const VehicleParams& vp) { cfg.actuator.steer.ch.tau_s = vp.steer_deadtime_s; } +// True when a throttle / brake channel does anything; otherwise the DirectControl +// torque path is left untouched so the default stays bit-identical. +bool channel_engaged(const ChannelActuator& c) { + return c.dead_time_s > 0.0 || c.tau_s > 0.0 || c.rate_limit > 0.0 || + c.dead_zone > 0.0 || c.out_min > 0.0 || c.out_max < 1.0 || + c.backlash > 0.0 || c.user != nullptr; +} + +bool longitudinal_engaged(const ActuatorParams& p) { + return channel_engaged(p.throttle) || channel_engaged(p.brake.ch) || + p.brake.thermal_enabled; +} + void snapshot_dynamics(IVehicleDynamics& dyn, std::array& tire_forces, std::array& tire_forces_wheel, @@ -50,10 +64,19 @@ SimSession::SimSession(std::unique_ptr dyn, network_ = make_default_veh_network(cfg.veh_network); SimConfig cfg_act = cfg; apply_vehicle_steer_deadtime(cfg_act, vp); + base_actuator_ = cfg_act.actuator; actuator_.initialize(cfg_act.actuator, cfg_act.nominal_dt); sensor_.initialize(cfg.sensor_delay_s, cfg.nominal_dt); sensors_.initialize(cfg.sensors); cascade_.initialize(vp); + + drive_ref_ = (cfg.drive_torque_ref > 0.0) ? cfg.drive_torque_ref + : vp.max_motor_torque * vp.final_drive_ratio; + brake_ref_ = (cfg.brake_torque_ref > 0.0) ? cfg.brake_torque_ref + : vp.max_brake_torque; + const double sf = vp.drive_split_front; + w0_ = {{0.5 * sf, 0.5 * sf, 0.5 * (1.0 - sf), 0.5 * (1.0 - sf)}}; + w_drive_ = w_brake_ = w0_; } @@ -120,7 +143,9 @@ SessionSnapshot SimSession::snapshot() const { sensor_.save_aux(v); sensors_.save_aux(v, s.rng); dyn_->save_aux(v); - cascade_.save_state(v); + for (int i = 0; i < NUM_WHEELS; ++i) v.push_back(w_drive_[i]); + for (int i = 0; i < NUM_WHEELS; ++i) v.push_back(w_brake_[i]); + cascade_.save_state(v); // keep last: tests/rl/test_rl_env.py addresses it as the tail return s; } @@ -183,6 +208,8 @@ void SimSession::restore(const SessionSnapshot& s) { // transients, so the aux block is replayed after it, not before. dyn_->reset(true_state_); dyn_->restore_aux(v, p); + for (int i = 0; i < NUM_WHEELS; ++i) w_drive_[i] = snap::get(v, p); + for (int i = 0; i < NUM_WHEELS; ++i) w_brake_[i] = snap::get(v, p); cascade_.restore_state(v, p); } @@ -216,6 +243,33 @@ DomainRandomization SimSession::randomization() const { return rand_; } +void SimSession::set_actuator(const ActuatorParams& p) { + std::lock_guard lk(mtx_); + actuator_.set_params(p); +} + +ActuatorParams SimSession::actuator() const { + std::lock_guard lk(mtx_); + return actuator_.params(); +} + +void SimSession::set_cascade_gains(const CascadeGains& g) { + const double v[] = {g.vx.kp, g.vx.ki, g.vx.i_max, g.vx.ax_clamp, + g.ax.kp, g.ax.ki, g.ax.kd, g.ax.kff, g.ax.i_max, + g.pp_lookahead_min, g.pp_lookahead_k, g.yaw_kp, g.yaw_ki}; + for (double x : v) + if (!std::isfinite(x) || x < 0.0) + throw std::invalid_argument( + "SimSession.set_cascade_gains: every gain must be finite and >= 0"); + std::lock_guard lk(mtx_); + cascade_.set_gains(g); +} + +CascadeGains SimSession::cascade_gains() const { + std::lock_guard lk(mtx_); + return cascade_.gains(); +} + // Rebuild the plant parameters from the as-built baseline and re-initialize in // place. Called under mtx_ from reset(). void SimSession::apply_randomization_locked() { @@ -241,6 +295,20 @@ void SimSession::apply_randomization_locked() { const double delay = (rand_.sensor_delay_s >= 0.0) ? rand_.sensor_delay_s : base_sensor_delay_s_; sensor_.initialize(delay, nominal_dt_); + + // Actuator channels: rebuild from the as-built params so repeated resets do + // not compound the scales. set_params() rather than initialize() -- the + // memory is cleared by actuator_.reset() in reset() right after this. + ActuatorParams ap = base_actuator_; + if (rand_.steer_delay_scale > 0.0) ap.steer.ch.dead_time_s *= rand_.steer_delay_scale; + if (rand_.steer_tau_scale > 0.0) ap.steer.ch.tau_s *= rand_.steer_tau_scale; + if (rand_.rate_limit_scale > 0.0) { + ap.steer.ch.rate_limit *= rand_.rate_limit_scale; + ap.throttle.rate_limit *= rand_.rate_limit_scale; + ap.brake.ch.rate_limit *= rand_.rate_limit_scale; + } + ap.brake.ch.dead_zone += rand_.brake_deadzone_add; + actuator_.set_params(ap); } void SimSession::reset(const State& s0) { @@ -260,6 +328,7 @@ void SimSession::reset(const State& s0) { sensor_.reset(s0); sensors_.reset(); cascade_.reset(); + w_drive_ = w_brake_ = w0_; true_state_ = s0; meas_state_ = s0; latched_ = ControlInput{CmdL4{}}; @@ -360,8 +429,42 @@ void SimSession::tick(double dt) { const double speed = s.speed_xy(); CmdL4 steer_desired{}; steer_desired.steer_angle_wheel = steer_from_input(cmd); + + // Longitudinal channel: total drive / brake wheel torque -> normalised + // throttle / brake -> actuator -> redistributed with the commanded + // per-wheel shares. Off (and bit-identical) unless a channel is set. + const CmdL1* l1 = std::get_if(&cmd); + const bool long_on = l1 != nullptr && drive_ref_ > 0.0 && brake_ref_ > 0.0 && + longitudinal_engaged(actuator_.params()); + if (long_on) { + double drive_sum = 0.0, brake_sum = 0.0; + for (int i = 0; i < NUM_WHEELS; ++i) { + drive_sum += std::max(l1->motor_torque[i], 0.0); + brake_sum += l1->brake_torque[i]; + } + if (drive_sum > 1e-9) + for (int i = 0; i < NUM_WHEELS; ++i) + w_drive_[i] = std::max(l1->motor_torque[i], 0.0) / drive_sum; + if (brake_sum > 1e-9) + for (int i = 0; i < NUM_WHEELS; ++i) + w_brake_[i] = l1->brake_torque[i] / brake_sum; + steer_desired.throttle = drive_sum / drive_ref_; + steer_desired.brake = brake_sum / brake_ref_; + } + const CmdL4 steer_realized = actuator_.apply(steer_desired, speed, dt); - const ControlInput dyn_cmd = with_steer_angle(cmd, steer_realized.steer_angle_wheel); + ControlInput dyn_cmd = with_steer_angle(cmd, steer_realized.steer_angle_wheel); + if (long_on) { + CmdL1 c = *l1; + c.steer_angle_wheel = steer_realized.steer_angle_wheel; + for (int i = 0; i < NUM_WHEELS; ++i) { + // Negative motor torque (regen) is not a pedal: it passes through. + const double regen = std::min(l1->motor_torque[i], 0.0); + c.motor_torque[i] = regen + steer_realized.throttle * drive_ref_ * w_drive_[i]; + c.brake_torque[i] = steer_realized.brake * brake_ref_ * w_brake_[i]; + } + dyn_cmd = ControlInput{c}; + } dyn_->step(dyn_cmd, contacts, dt); const State next = dyn_->state(); @@ -385,8 +488,8 @@ void SimSession::tick(double dt) { ax_ = ax; ay_ = ay; roll_ = roll; pitch_ = pitch; rack_ = rack; az_ = az; heave_z_ = heave; contacts_ = contacts; steer_applied_ = steer_realized.steer_angle_wheel; - throttle_applied_ = 0.0; - brake_applied_ = 0.0; + throttle_applied_ = long_on ? steer_realized.throttle : 0.0; + brake_applied_ = long_on ? steer_realized.brake : 0.0; Fz_ = Fz; snapshot_dynamics(*dyn_, tire_forces_, tire_forces_wheel_, slip_ratio_, slip_angle_, wheel_mu_, wheel_mu_peak_, @@ -507,6 +610,9 @@ std::unique_ptr make_direct_control_session( SimConfig cfg; cfg.session_kind = SessionKind::DirectControl; cfg.nominal_dt = opts.nominal_dt; + cfg.actuator = opts.actuator; + cfg.drive_torque_ref = opts.drive_torque_ref; + cfg.brake_torque_ref = opts.brake_torque_ref; apply_vehicle_steer_deadtime(cfg, vp); VehicleParams vp_dc = vp; vp_dc.plant_path = true; diff --git a/core/src/vec_session.cpp b/core/src/vec_session.cpp index bb5a881b..72a61dee 100644 --- a/core/src/vec_session.cpp +++ b/core/src/vec_session.cpp @@ -245,6 +245,29 @@ void VecSession::set_seeds(const std::vector& seeds) { sessions_[i]->set_seed(bcast ? seeds[0] + static_cast(i) : seeds[i]); } +void VecSession::set_actuators(const std::vector& p) { + if (p.size() != sessions_.size() && p.size() != 1) + throw std::invalid_argument("VecSession.set_actuators: expected 1 or size() specs"); + const bool bcast = (p.size() == 1); + for (std::size_t i = 0; i < sessions_.size(); ++i) + sessions_[i]->set_actuator(bcast ? p[0] : p[i]); +} + +void VecSession::set_cascade_gains(const std::vector& g) { + if (g.size() != sessions_.size() && g.size() != 1) + throw std::invalid_argument("VecSession.set_cascade_gains: expected 1 or size() specs"); + const bool bcast = (g.size() == 1); + for (std::size_t i = 0; i < sessions_.size(); ++i) + sessions_[i]->set_cascade_gains(bcast ? g[0] : g[i]); +} + +std::vector VecSession::actuators() const { + std::vector out; + out.reserve(sessions_.size()); + for (const auto& s : sessions_) out.push_back(s->actuator()); + return out; +} + // ---------------- observation / termination ---------------- void VecSession::set_obs_fields(const std::vector& names) { diff --git a/tests/unit/test_actuator.cpp b/tests/unit/test_actuator.cpp index 4583347b..81aaf0f1 100644 --- a/tests/unit/test_actuator.cpp +++ b/tests/unit/test_actuator.cpp @@ -3,6 +3,8 @@ #include #include +#include +#include #include "vdsim/actuator.hpp" @@ -138,3 +140,63 @@ TEST(Actuator, SensorDelayReturnsPast) { } EXPECT_NEAR(seen_vx, 10.0, 1.0 + 1e-9); // ~10 steps behind k=20 } + +// Backlash: play operator -- holds inside the band, follows at u -/+ w/2 on engage/reversal. +TEST(Actuator, BacklashPlayOperator) { + ActuatorParams p; + p.steer.ch.backlash = 0.02; + ActuatorModel a; a.initialize(p, 0.005); + auto y = [&](double u) { return a.apply(cmd(0, 0, u), 0.0, 0.005).steer_angle_wheel; }; + EXPECT_NEAR(y(0.10), 0.09, 1e-12); + EXPECT_NEAR(y(0.095), 0.09, 1e-12); + EXPECT_NEAR(y(0.07), 0.08, 1e-12); +} + +// Backlash 0 leaves the chain bit-identical. +TEST(Actuator, ZeroBacklashIsPassthrough) { + ActuatorModel a; a.initialize(ActuatorParams{}, 0.005); + EXPECT_DOUBLE_EQ(a.apply(cmd(0, 0, 0.123), 0.0, 0.005).steer_angle_wheel, 0.123); +} + +namespace { +struct Gain : IChannelModel { + double k; int n {0}; + explicit Gain(double k_) : k(k_) {} + double step(double u, double) override { ++n; return k * u + 1e-3 * n; } + void reset() override { n = 0; } + std::unique_ptr clone() const override { return std::make_unique(*this); } + std::vector get_state() const override { return {static_cast(n)}; } + void set_state(const std::vector& v) override { n = static_cast(v.at(0)); } +}; +} // namespace + +// User stage runs after the built-in chain; the prototype is never stepped and +// two models built from the same prototype do not share state. +TEST(Actuator, UserStageRunsAndIsNotShared) { + auto proto = std::make_shared(0.5); + ActuatorParams p; + p.throttle.user = proto; + ActuatorModel a, b; a.initialize(p, 0.005); b.initialize(p, 0.005); + double ya = 0; + for (int k = 0; k < 3; ++k) ya = a.apply(cmd(0.4, 0, 0), 0.0, 0.005).throttle; + EXPECT_NEAR(ya, 0.5 * 0.4 + 3e-3, 1e-12); + EXPECT_NEAR(b.apply(cmd(0.4, 0, 0), 0.0, 0.005).throttle, 0.5 * 0.4 + 1e-3, 1e-12); + EXPECT_EQ(proto->n, 0); +} + +// Backlash state and user-stage state round-trip through save_aux/restore_aux. +TEST(Actuator, BacklashAndUserStateSnapshotRoundTrip) { + ActuatorParams p; + p.steer.ch.backlash = 0.02; + p.throttle.user = std::make_shared(1.0); + ActuatorModel a; a.initialize(p, 0.005); + for (int k = 0; k < 7; ++k) a.apply(cmd(0.3, 0, 0.05 * k), 0.0, 0.005); + std::vector snap; a.save_aux(snap); + CmdL4 r1 = a.apply(cmd(0.3, 0, 0.2), 0.0, 0.005); + for (int k = 0; k < 5; ++k) a.apply(cmd(0.9, 0, -0.3), 0.0, 0.005); + std::size_t pos = 0; a.restore_aux(snap, pos); + EXPECT_EQ(pos, snap.size()); + CmdL4 r2 = a.apply(cmd(0.3, 0, 0.2), 0.0, 0.005); + EXPECT_DOUBLE_EQ(r1.throttle, r2.throttle); + EXPECT_DOUBLE_EQ(r1.steer_angle_wheel, r2.steer_angle_wheel); +} diff --git a/tests/unit/test_sim_session.cpp b/tests/unit/test_sim_session.cpp index a9a5e0e5..b926427d 100644 --- a/tests/unit/test_sim_session.cpp +++ b/tests/unit/test_sim_session.cpp @@ -4,7 +4,9 @@ #include #include #include +#include #include +#include #include #include "vdsim/realtime_runner.hpp" @@ -131,6 +133,91 @@ TEST(SimSession, DynamicSteeringTorqueYaws) { vp.max_steer_angle_wheel * vp.steering_ratio + 1e-6) << "rack clamps at lock"; } +// ---- Sensor lag (SensorNoise::lag_tau_s) ----------------------------------- + +static SimConfig lag_cfg(double tau) { + SimConfig cfg; + cfg.sensors.enabled = true; // all noise 0 -> measured == (lagged) truth + cfg.sensors.gnss_vel.lag_tau_s = tau; + return cfg; +} + +// tau = 0 is a pure pass-through; tau > 0 starts primed on the truth (no +// transient from zero) and then trails an accelerating signal. +TEST(SimSession, SensorLagTrailsTruthAndStartsPrimed) { + auto plain = make_session(lag_cfg(0.0)); + auto lag = make_session(lag_cfg(0.5)); + CmdL4 cmd; cmd.throttle = 1.0; + plain->set_input(cmd); lag->set_input(cmd); + plain->tick(0.005); lag->tick(0.005); + EXPECT_NEAR(plain->output().sensors.gnss_vx, plain->state().vx(), 1e-9); + EXPECT_NEAR(lag->output().sensors.gnss_vx, 20.0, 0.1) << "primed on the first sample"; + for (int i = 0; i < 400; ++i) { plain->tick(0.005); lag->tick(0.005); } + EXPECT_NEAR(plain->output().sensors.gnss_vx, plain->state().vx(), 1e-9); + EXPECT_LT(lag->output().sensors.gnss_vx, lag->state().vx() - 0.2); +} + +// The lag state is part of the sensor snapshot: restore replays bit-identically. +TEST(SimSession, SensorLagSurvivesSnapshotRestore) { + auto s = make_session(lag_cfg(0.3)); + CmdL4 cmd; cmd.throttle = 0.8; + s->set_input(cmd); + for (int i = 0; i < 100; ++i) s->tick(0.005); + const SessionSnapshot snap = s->snapshot(); + for (int i = 0; i < 100; ++i) s->tick(0.005); + const double first = s->output().sensors.gnss_vx; + s->restore(snap); + for (int i = 0; i < 100; ++i) s->tick(0.005); + EXPECT_EQ(first, s->output().sensors.gnss_vx); +} + +// ---- CascadeGains ----------------------------------------------------------- + +static double l6_speed_after(SimSession& s, double v_target, double seconds) { + CmdL6 cmd; cmd.v_target = v_target; + for (int i = 0; i < int(seconds / 0.005); ++i) { s.set_input(ControlInput{cmd}); s.tick(0.005); } + return s.state().vx(); +} + +TEST(SimSession, CascadeGainsDefaultsAreBitIdentical) { + auto a = make_session(); + auto b = make_session(); + b->set_cascade_gains(CascadeGains{}); + EXPECT_EQ(l6_speed_after(*a, 10.0, 6.0), l6_speed_after(*b, 10.0, 6.0)); +} + +TEST(SimSession, CascadeGainsRetuneChangesTracking) { + auto def = make_session(); + auto off = make_session(); + CascadeGains g; g.vx.kp = 0.0; g.vx.ki = 0.0; // speed loop disabled + off->set_cascade_gains(g); + EXPECT_NEAR(l6_speed_after(*def, 10.0, 15.0), 10.0, 1.5); + EXPECT_GT(l6_speed_after(*off, 10.0, 15.0), 15.0) << "no speed loop -> no braking to target"; +} + +TEST(SimSession, CascadeGainsPersistAcrossResetAndRandomization) { + auto s = make_session(); + CascadeGains g; g.vx.kp = 1.7; g.yaw_ki = 0.2; g.pp_lookahead_min = 3.0; + s->set_cascade_gains(g); + DomainRandomization d; d.mass_scale = 1.1; + s->set_randomization(d); + State s0; s0.velocity = {20.0, 0.0, 0.0}; + s->reset(s0); + const CascadeGains r = s->cascade_gains(); + EXPECT_EQ(r.vx.kp, 1.7); + EXPECT_EQ(r.yaw_ki, 0.2); + EXPECT_EQ(r.pp_lookahead_min, 3.0); +} + +TEST(SimSession, CascadeGainsRejectBadValues) { + auto s = make_session(); + CascadeGains neg; neg.ax.kp = -0.1; + EXPECT_THROW(s->set_cascade_gains(neg), std::invalid_argument); + CascadeGains nan_; nan_.yaw_kp = std::nan(""); + EXPECT_THROW(s->set_cascade_gains(nan_), std::invalid_argument); + EXPECT_EQ(s->cascade_gains().ax.kp, CascadeGains{}.ax.kp) << "rejected set leaves gains unchanged"; +} + // The fail-safe is a ControlInput now. Its default must still be the historical // L4 brake, and latching it through the variant overload must leave the session // bit-identical to the old CmdL4 overload the runner used to call. @@ -198,3 +285,38 @@ TEST(SimSession, RealTimeRunnerL1FailsafeLatchesAndBrakes) { EXPECT_TRUE(std::isfinite(s->state().vx())); EXPECT_LT(s->state().vx(), 20.0); } + +// ---- Direct-control level selection (work order 32, B2) -------------------- +// Ported from the retired feat/direct-control-dynamics-level branch: its +// dynamics_level option is DirectControlSessionOptions::level here. +static std::unique_ptr make_dc_level(const char* level) { + VehicleParams vp; TireParams tp; SolverParams sp; + DirectControlSessionOptions o; + o.level = level; + return make_direct_control_session(vp, TireSetup(tp), sp, o); +} + +TEST(DirectControlSession, DefaultLevelIsSevenDof) { + EXPECT_EQ(DirectControlSessionOptions{}.level, "L2"); + VehicleParams vp; TireParams tp; SolverParams sp; + auto s = make_direct_control_session(vp, TireSetup(tp), sp); + EXPECT_EQ(s->dynamics().level(), IVehicleDynamics::Level::L2_SevenDOF); +} + +TEST(DirectControlSession, L3AndL4CreateAndTickFinite) { + const std::pair cases[] = { + {"L3", IVehicleDynamics::Level::L3_FourteenDOF}, + {"L4", IVehicleDynamics::Level::L4_Kinematic}}; + const double dt = DirectControlSessionOptions{}.nominal_dt; + for (const auto& [name, lvl] : cases) { + auto s = make_dc_level(name); + EXPECT_EQ(s->dynamics().level(), lvl) << name; + EXPECT_TRUE(s->vehicle_params().plant_path) << name; + State s0; s0.velocity = {20.0, 0.0, 0.0}; + s->reset(s0); + CmdL1 cmd; cmd.steer_angle_wheel = 0.02; + for (int i = 0; i < 2000; ++i) { s->set_input(cmd); s->tick(dt); } + EXPECT_TRUE(std::isfinite(s->state().vx())) << name; + EXPECT_GT(s->state().yaw_rate(), 1e-4) << name; + } +} From 29479571c6fa63a7b8c6229f2f35794eea6921cc Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 12:53:03 +0900 Subject: [PATCH 2/7] feat(control,sensors): cascade gains and sensor lag in the core Co-Authored-By: Claude Opus 5.5 --- core/include/vdsim/control_converter.hpp | 22 ++++++++++++++ core/include/vdsim/sensors.hpp | 5 ++++ core/src/control_converter.cpp | 21 +++++++++---- core/src/sensor_model.cpp | 38 ++++++++++++++++-------- 4 files changed, 68 insertions(+), 18 deletions(-) diff --git a/core/include/vdsim/control_converter.hpp b/core/include/vdsim/control_converter.hpp index 6130b390..105552ee 100644 --- a/core/include/vdsim/control_converter.hpp +++ b/core/include/vdsim/control_converter.hpp @@ -36,6 +36,9 @@ class LongAxController { void initialize(const Gains& g) noexcept; void reset() noexcept; + // Swap the gains without touching the integrator (live retune). + void set_gains(const Gains& g) noexcept { g_ = g; } + const Gains& gains() const noexcept { return g_; } // Returns (throttle, brake), both in [0, 1]. std::pair update(double ax_target, @@ -77,6 +80,9 @@ class LongVxController { void initialize(const Gains& g) noexcept; void reset() noexcept; + // Swap the gains without touching the integrator (live retune). + void set_gains(const Gains& g) noexcept { g_ = g; } + const Gains& gains() const noexcept { return g_; } // Returns desired ax_target [m/s^2] given (v_target, v_meas, dt). double update(double v_target, double v_meas, double dt) noexcept; @@ -181,11 +187,26 @@ class DriverModel { // fed the measured state each tick. This is the "controller" entity in the // architecture: User Lc-command → CascadeController → CmdL4 → VehNetwork → actuator. // ============================================================================= +// Tunable gains of the CascadeController. Defaults are the values the cascade +// used before they became settable, so an untouched session behaves as before. +struct CascadeGains { + LongVxController::Gains vx {}; // L6 speed loop + LongAxController::Gains ax {}; // L5 accel loop (+ pedal feed-forward) + double pp_lookahead_min {1.5}; // [m] pure-pursuit lookahead floor + double pp_lookahead_k {0.40}; // [s] Ld = max(min, k * vx) + double yaw_kp {0.15}; // [rad steer / (rad/s)] yaw-rate PI + double yaw_ki {0.05}; // [rad steer / rad] +}; + class CascadeController { public: void initialize(const VehicleParams& vp); void reset() noexcept; + // Live retune: keeps every integrator and the pursuit progress. + void set_gains(const CascadeGains& g) noexcept; + const CascadeGains& gains() const noexcept { return gains_; } + // Convert a ladder command to CmdL4 using current feedback. // meas : measured plant state (pose, vx for cascade) // ax_meas : measured longitudinal accel [m/s²] (for L5/L6 inner loop) @@ -212,6 +233,7 @@ class CascadeController { // out.steer_mode/steer_actuator (L1-L3 sub-L4, for a Dynamic steering subsystem). void lat_to_cmd(const LcLatCmd& lat, const State& meas, double dt, CmdL4& out); + CascadeGains gains_ {}; LongVxController vxc_; LongAxController axc_; PurePursuitController pp_; diff --git a/core/include/vdsim/sensors.hpp b/core/include/vdsim/sensors.hpp index d028c6b0..19baa5e6 100644 --- a/core/include/vdsim/sensors.hpp +++ b/core/include/vdsim/sensors.hpp @@ -22,6 +22,7 @@ struct SensorNoise { double noise_std {0.0}; // measurement white noise 1-sigma double bias {0.0}; // constant offset double bias_rw {0.0}; // random-walk std rate: bias += N(0,1)*bias_rw*sqrt(dt) + double lag_tau_s {0.0}; // first-order sensor lag [s] on the true signal, before bias/noise; 0 = none }; struct SensorParams { @@ -70,6 +71,10 @@ class SensorModel { double b_ax_{0}, b_ay_{0}, b_az_{0}, b_wx_{0}, b_wy_{0}, b_wz_{0}; std::array b_w_{{0, 0, 0, 0}}; double b_st_{0}, b_gx_{0}, b_gy_{0}, b_gvx_{0}, b_gvy_{0}; + // first-order lag state per channel (ax ay az wx wy wz w0..w3 steer gx gy gvx gvy) + static constexpr int kLagCh = 15; + std::array lag_{}; + bool lag_primed_{false}; }; } // namespace vdsim diff --git a/core/src/control_converter.cpp b/core/src/control_converter.cpp index c1d74cb4..7331e58a 100644 --- a/core/src/control_converter.cpp +++ b/core/src/control_converter.cpp @@ -201,13 +201,24 @@ DriverModel::Output DriverModel::update(double x, double y, double psi, double v void CascadeController::initialize(const VehicleParams& vp) { wheelbase_ = vp.wheelbase > 1e-3 ? vp.wheelbase : 2.7; max_steer_ = vp.max_steer_angle_wheel > 1e-3 ? vp.max_steer_angle_wheel : 0.5; - vxc_.initialize({}); - axc_.initialize({}); + vxc_.initialize(gains_.vx); + axc_.initialize(gains_.ax); + set_gains(gains_); + reset(); +} + +void CascadeController::set_gains(const CascadeGains& g) noexcept { + gains_ = g; + vxc_.set_gains(g.vx); + axc_.set_gains(g.ax); PurePursuitController::Gains pg; - pg.wheelbase = wheelbase_; - pg.max_steer = max_steer_; + pg.wheelbase = wheelbase_; + pg.max_steer = max_steer_; + pg.lookahead_min = g.pp_lookahead_min; + pg.lookahead_k = g.pp_lookahead_k; pp_.initialize(pg); - reset(); + r_kp_ = g.yaw_kp; + r_ki_ = g.yaw_ki; } void CascadeController::reset() noexcept { diff --git a/core/src/sensor_model.cpp b/core/src/sensor_model.cpp index 85627263..b5e4133b 100644 --- a/core/src/sensor_model.cpp +++ b/core/src/sensor_model.cpp @@ -26,6 +26,8 @@ void SensorModel::reset() { b_ax_ = b_ay_ = b_az_ = b_wx_ = b_wy_ = b_wz_ = 0.0; b_w_ = {{0.0, 0.0, 0.0, 0.0}}; b_st_ = b_gx_ = b_gy_ = b_gvx_ = b_gvy_ = 0.0; + lag_.fill(0.0); + lag_primed_ = false; } SensorMeas SensorModel::apply(const State& s, double ax, double ay, @@ -49,25 +51,31 @@ SensorMeas SensorModel::apply(const State& s, double ax, double ay, } const double sdt = std::sqrt(std::max(0.0, dt)); - auto meas = [&](double truth, const SensorNoise& n, double& rwbias) { + auto meas = [&](double truth, const SensorNoise& n, double& rwbias, int li) { + if (n.lag_tau_s > 0.0) { + if (!lag_primed_) lag_[li] = truth; + else if (dt > 0.0) lag_[li] += (1.0 - std::exp(-dt / n.lag_tau_s)) * (truth - lag_[li]); + truth = lag_[li]; + } if (n.bias_rw > 0.0) rwbias += nd_(rng_) * n.bias_rw * sdt; const double w = (n.noise_std > 0.0) ? nd_(rng_) * n.noise_std : 0.0; return truth + n.bias + rwbias + w; }; - m.ax = meas(ax, p_.imu_accel, b_ax_); - m.ay = meas(ay, p_.imu_accel, b_ay_); - m.az = meas(kGravity, p_.imu_accel, b_az_); - m.wx = meas(s.angular_velocity.x(), p_.imu_gyro, b_wx_); - m.wy = meas(s.angular_velocity.y(), p_.imu_gyro, b_wy_); - m.wz = meas(s.angular_velocity.z(), p_.imu_gyro, b_wz_); + m.ax = meas(ax, p_.imu_accel, b_ax_, 0); + m.ay = meas(ay, p_.imu_accel, b_ay_, 1); + m.az = meas(kGravity, p_.imu_accel, b_az_, 2); + m.wx = meas(s.angular_velocity.x(), p_.imu_gyro, b_wx_, 3); + m.wy = meas(s.angular_velocity.y(), p_.imu_gyro, b_wy_, 4); + m.wz = meas(s.angular_velocity.z(), p_.imu_gyro, b_wz_, 5); for (int i = 0; i < NUM_WHEELS; ++i) - m.wheel_speed[i] = meas(s.wheel_spin[i], p_.wheel_speed, b_w_[i]); - m.steer = meas(steer_true, p_.steer, b_st_); - m.gnss_x = meas(s.position.x(), p_.gnss_pos, b_gx_); - m.gnss_y = meas(s.position.y(), p_.gnss_pos, b_gy_); - m.gnss_vx = meas(vE, p_.gnss_vel, b_gvx_); - m.gnss_vy = meas(vN, p_.gnss_vel, b_gvy_); + m.wheel_speed[i] = meas(s.wheel_spin[i], p_.wheel_speed, b_w_[i], 6 + i); + m.steer = meas(steer_true, p_.steer, b_st_, 10); + m.gnss_x = meas(s.position.x(), p_.gnss_pos, b_gx_, 11); + m.gnss_y = meas(s.position.y(), p_.gnss_pos, b_gy_, 12); + m.gnss_vx = meas(vE, p_.gnss_vel, b_gvx_, 13); + m.gnss_vy = meas(vN, p_.gnss_vel, b_gvy_, 14); + lag_primed_ = true; nd_.reset(); // R8: leave no un-snapshottable cache return m; } @@ -80,6 +88,8 @@ void SensorModel::save_aux(std::vector& v, std::string& rng) const { for (int i = 0; i < NUM_WHEELS; ++i) v.push_back(b_w_[i]); v.push_back(b_st_); v.push_back(b_gx_); v.push_back(b_gy_); v.push_back(b_gvx_); v.push_back(b_gvy_); + for (double x : lag_) v.push_back(x); + v.push_back(lag_primed_ ? 1.0 : 0.0); std::ostringstream os; os << rng_; rng = os.str(); @@ -92,6 +102,8 @@ void SensorModel::restore_aux(const std::vector& v, std::size_t& p, for (int i = 0; i < NUM_WHEELS; ++i) b_w_[i] = snap::get(v, p); b_st_ = snap::get(v, p); b_gx_ = snap::get(v, p); b_gy_ = snap::get(v, p); b_gvx_ = snap::get(v, p); b_gvy_ = snap::get(v, p); + for (double& x : lag_) x = snap::get(v, p); + lag_primed_ = snap::get(v, p) != 0.0; if (!rng.empty()) { std::istringstream is(rng); is >> rng_; From ac72039a62d147ae19d9c3bd67d916f7790a917c Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 12:53:03 +0900 Subject: [PATCH 3/7] feat(python): actuator option on VDSimPlant, cascade and actuator bindings, generic presets Co-Authored-By: Claude Opus 5.5 --- configs/catalog/manifest.yaml | 4 + configs/parts/actuator/generic_sedan.yaml | 20 ++++ configs/parts/actuator/ideal.yaml | 12 ++ python/bindings.cpp | 120 ++++++++++++++++++- python/vdsim_plant.py | 138 ++++++++++++++++++++++ 5 files changed, 288 insertions(+), 6 deletions(-) create mode 100644 configs/parts/actuator/generic_sedan.yaml create mode 100644 configs/parts/actuator/ideal.yaml diff --git a/configs/catalog/manifest.yaml b/configs/catalog/manifest.yaml index 9fc25a77..e053df05 100644 --- a/configs/catalog/manifest.yaml +++ b/configs/catalog/manifest.yaml @@ -1,6 +1,10 @@ catalog_id: vdsim.builtin catalog_version: 1 parts: +- id: actuator.generic_sedan + path: parts/actuator/generic_sedan.yaml +- id: actuator.ideal + path: parts/actuator/ideal.yaml - id: aero.fsk_formula path: parts/aero/fsk_formula.yaml - id: aero.race_car diff --git a/configs/parts/actuator/generic_sedan.yaml b/configs/parts/actuator/generic_sedan.yaml new file mode 100644 index 00000000..37193e07 --- /dev/null +++ b/configs/parts/actuator/generic_sedan.yaml @@ -0,0 +1,20 @@ +id: actuator.generic_sedan +type: actuator +version: 1 +schema: actuator_v1 +label: generic sedan actuator set (nominal placeholders, not measured) +tags: +- actuator +- generic_sedan +body: + steer: + dead_time_s: 0.05 + tau_s: 0.08 + backlash: 0.002 + throttle: + dead_time_s: 0.10 + tau_s: 0.25 + brake: + dead_time_s: 0.08 + tau_s: 0.15 + dead_zone: 0.03 diff --git a/configs/parts/actuator/ideal.yaml b/configs/parts/actuator/ideal.yaml new file mode 100644 index 00000000..1478319d --- /dev/null +++ b/configs/parts/actuator/ideal.yaml @@ -0,0 +1,12 @@ +id: actuator.ideal +type: actuator +version: 1 +schema: actuator_v1 +label: ideal actuator (no delay, lag or play) +tags: +- actuator +- reference +body: + steer: {} + throttle: {} + brake: {} diff --git a/python/bindings.cpp b/python/bindings.cpp index da6ff6d2..26c28f79 100644 --- a/python/bindings.cpp +++ b/python/bindings.cpp @@ -178,6 +178,62 @@ static std::unique_ptr build_ground_session( std::move(dyn), std::move(ground), vp, tp, solver, cfg); } +namespace { + +// Duck-typed Python object as a user channel stage: step(u, dt) -> float, +// optional reset(), get_state() -> list[float], set_state(list). The object +// passed in is only a prototype; each ActuatorModel deep-copies its own. +class PyChannelModel final : public vdsim::IChannelModel { +public: + explicit PyChannelModel(py::object o) : obj_(std::move(o)) {} + ~PyChannelModel() override { + if (Py_IsInitialized()) { + py::gil_scoped_acquire g; + obj_ = py::object(); + } + } + double step(double u, double dt) override { + py::gil_scoped_acquire g; + return obj_.attr("step")(u, dt).cast(); + } + void reset() override { + py::gil_scoped_acquire g; + if (py::hasattr(obj_, "reset")) obj_.attr("reset")(); + } + std::unique_ptr clone() const override { + py::gil_scoped_acquire g; + py::object c = py::module_::import("copy").attr("deepcopy")(obj_); + return std::make_unique(std::move(c)); + } + std::vector get_state() const override { + py::gil_scoped_acquire g; + if (!py::hasattr(obj_, "get_state")) return {}; + return obj_.attr("get_state")().cast>(); + } + void set_state(const std::vector& v) override { + py::gil_scoped_acquire g; + if (py::hasattr(obj_, "set_state")) obj_.attr("set_state")(v); + } + py::object object() const { return obj_; } + +private: + py::object obj_; +}; + +py::object channel_user_get(const vdsim::ChannelActuator& c) { + const auto* a = dynamic_cast(c.user.get()); + return a ? a->object() : py::none(); +} + +void channel_user_set(vdsim::ChannelActuator& c, py::object o) { + if (o.is_none()) { c.user.reset(); return; } + if (!py::hasattr(o, "step")) + throw py::type_error("user channel model needs a step(u, dt) -> float method"); + c.user = std::make_shared(std::move(o)); +} + +} // namespace + PYBIND11_MODULE(vdsim, m) { m.doc() = "VDSim core Python bindings"; @@ -944,7 +1000,10 @@ PYBIND11_MODULE(vdsim, m) { py::class_(m, "LongAxGains") .def(py::init<>()) .def_readwrite("kp", &vdsim::LongAxController::Gains::kp) - .def_readwrite("ki", &vdsim::LongAxController::Gains::ki); + .def_readwrite("ki", &vdsim::LongAxController::Gains::ki) + .def_readwrite("kd", &vdsim::LongAxController::Gains::kd) + .def_readwrite("kff", &vdsim::LongAxController::Gains::kff) + .def_readwrite("i_max", &vdsim::LongAxController::Gains::i_max); py::class_(m, "LongAxController") .def(py::init<>()) @@ -955,7 +1014,21 @@ PYBIND11_MODULE(vdsim, m) { py::class_(m, "LongVxGains") .def(py::init<>()) .def_readwrite("kp", &vdsim::LongVxController::Gains::kp) - .def_readwrite("ki", &vdsim::LongVxController::Gains::ki); + .def_readwrite("ki", &vdsim::LongVxController::Gains::ki) + .def_readwrite("i_max", &vdsim::LongVxController::Gains::i_max) + .def_readwrite("ax_clamp", &vdsim::LongVxController::Gains::ax_clamp); + + py::class_(m, "CascadeGains", + "Gains of the L5-L8 cascade: vx (LongVxGains), ax (LongAxGains), " + "pp_lookahead_min [m], pp_lookahead_k [s], yaw_kp, yaw_ki. Defaults " + "equal the previously hard-coded values.") + .def(py::init<>()) + .def_readwrite("vx", &vdsim::CascadeGains::vx) + .def_readwrite("ax", &vdsim::CascadeGains::ax) + .def_readwrite("pp_lookahead_min", &vdsim::CascadeGains::pp_lookahead_min) + .def_readwrite("pp_lookahead_k", &vdsim::CascadeGains::pp_lookahead_k) + .def_readwrite("yaw_kp", &vdsim::CascadeGains::yaw_kp) + .def_readwrite("yaw_ki", &vdsim::CascadeGains::yaw_ki); py::class_(m, "LongVxController") .def(py::init<>()) @@ -971,7 +1044,12 @@ PYBIND11_MODULE(vdsim, m) { .def_readwrite("rate_limit", &vdsim::ChannelActuator::rate_limit) .def_readwrite("dead_zone", &vdsim::ChannelActuator::dead_zone) .def_readwrite("out_min", &vdsim::ChannelActuator::out_min) - .def_readwrite("out_max", &vdsim::ChannelActuator::out_max); + .def_readwrite("out_max", &vdsim::ChannelActuator::out_max) + .def_readwrite("backlash", &vdsim::ChannelActuator::backlash) + .def_property("user", &channel_user_get, &channel_user_set, + "Optional user stage: any object with step(u, dt) -> float (and " + "optionally reset(), get_state(), set_state()). Applied to the " + "output of the built-in chain; deep-copied per session."); py::class_(m, "LuGreParams") .def(py::init<>()) .def_readwrite("enabled", &vdsim::LuGreParams::enabled) @@ -1008,7 +1086,8 @@ PYBIND11_MODULE(vdsim, m) { .def(py::init<>()) .def_readwrite("noise_std", &vdsim::SensorNoise::noise_std) .def_readwrite("bias", &vdsim::SensorNoise::bias) - .def_readwrite("bias_rw", &vdsim::SensorNoise::bias_rw); + .def_readwrite("bias_rw", &vdsim::SensorNoise::bias_rw) + .def_readwrite("lag_tau_s", &vdsim::SensorNoise::lag_tau_s); py::class_(m, "SensorParams") .def(py::init<>()) .def_readwrite("enabled", &vdsim::SensorParams::enabled) @@ -1104,7 +1183,11 @@ PYBIND11_MODULE(vdsim, m) { .def_readwrite("mu_scale", &vdsim::DomainRandomization::mu_scale) .def_readwrite("tire_stiffness_scale", &vdsim::DomainRandomization::tire_stiffness_scale) .def_readwrite("tire_mu_scale", &vdsim::DomainRandomization::tire_mu_scale) - .def_readwrite("sensor_delay_s", &vdsim::DomainRandomization::sensor_delay_s); + .def_readwrite("sensor_delay_s", &vdsim::DomainRandomization::sensor_delay_s) + .def_readwrite("steer_delay_scale", &vdsim::DomainRandomization::steer_delay_scale) + .def_readwrite("steer_tau_scale", &vdsim::DomainRandomization::steer_tau_scale) + .def_readwrite("rate_limit_scale", &vdsim::DomainRandomization::rate_limit_scale) + .def_readwrite("brake_deadzone_add", &vdsim::DomainRandomization::brake_deadzone_add); // -------- R8: session snapshot / restore -------- py::class_(m, "SessionSnapshot") @@ -1157,6 +1240,18 @@ PYBIND11_MODULE(vdsim, m) { py::arg("spec"), "Latch a DomainRandomization; applied at the next reset().") .def("randomization", &vdsim::SimSession::randomization) + .def("set_actuator", &vdsim::SimSession::set_actuator, py::arg("actuator"), + "Swap ActuatorParams on a live session, keeping actuator memory " + "(delay rings, lag, LuGre, brake temperature). Takes effect on the " + "next tick(); the next reset() returns to the as-built params.") + .def("actuator", &vdsim::SimSession::actuator, + "The ActuatorParams currently in force on this session.") + .def("set_cascade_gains", &vdsim::SimSession::set_cascade_gains, py::arg("gains"), + "Retune the L5-L8 cascade on a live session, keeping integrators. " + "Persists across reset(). ValueError on negative/non-finite gains. " + "No effect on a direct-control session (it never runs the cascade).") + .def("cascade_gains", &vdsim::SimSession::cascade_gains, + "The CascadeGains currently in force on this session.") .def("state", &vdsim::SimSession::state) .def("measured_state", &vdsim::SimSession::measured_state) .def("output", &vdsim::SimSession::output, @@ -1290,6 +1385,16 @@ PYBIND11_MODULE(vdsim, m) { .def("set_randomizations", &vdsim::VecSession::set_randomizations, py::arg("specs"), "Per-env DomainRandomization (a list of length 1 broadcasts).") + .def("set_actuators", &vdsim::VecSession::set_actuators, + py::arg("actuators"), + "Per-env ActuatorParams swap, effective immediately and keeping " + "actuator memory (a list of length 1 broadcasts).") + .def("set_cascade_gains", &vdsim::VecSession::set_cascade_gains, + py::arg("gains"), + "Per-env CascadeGains swap, keeping controller memory (a list of " + "length 1 broadcasts).") + .def("actuators", &vdsim::VecSession::actuators, + "The ActuatorParams currently in force on each env.") .def("set_obs_fields", &vdsim::VecSession::set_obs_fields, py::arg("fields"), "Declare the flat observation layout; a bare per-wheel name " "expands to 4 columns (FL FR RL RR), 'slip_ratio.2' picks one.") @@ -1462,7 +1567,10 @@ PYBIND11_MODULE(vdsim, m) { .def(py::init<>()) .def_readwrite("friction", &vdsim::DirectControlSessionOptions::friction) .def_readwrite("nominal_dt", &vdsim::DirectControlSessionOptions::nominal_dt) - .def_readwrite("level", &vdsim::DirectControlSessionOptions::level); + .def_readwrite("level", &vdsim::DirectControlSessionOptions::level) + .def_readwrite("actuator", &vdsim::DirectControlSessionOptions::actuator) + .def_readwrite("drive_torque_ref", &vdsim::DirectControlSessionOptions::drive_torque_ref) + .def_readwrite("brake_torque_ref", &vdsim::DirectControlSessionOptions::brake_torque_ref); m.def("make_direct_control_session", [](const vdsim::VehicleParams& vp, const vdsim::TireSetup& ts, diff --git a/python/vdsim_plant.py b/python/vdsim_plant.py index ca6ca7b9..9715ff34 100644 --- a/python/vdsim_plant.py +++ b/python/vdsim_plant.py @@ -308,6 +308,124 @@ def _poly_patches_from_map_2d(friction_map_2d): return out +_CHANNEL_KEYS = ("dead_time_s", "tau_s", "rate_limit", "dead_zone", "out_min", "out_max", + "backlash") +_STEER_EXTRA = ("friction", "inertia", "servo_kp", "servo_kd") +_BRAKE_EXTRA = ("thermal",) +_LUGRE_KEYS = ("sigma0", "sigma1", "sigma2", "Tc", "Ts", "ws") +_THERMAL_KEYS = ("heat_coeff", "cool_coeff", "T_ambient", "mu_T_temp", "mu_T_scale") + + +def _finite(v, where): + v = float(v) + if not math.isfinite(v): + raise ValueError(f"actuator[{where}] must be finite") + return v + + +def _fill_channel(dst, spec, where, extra=()): + if not isinstance(spec, dict): + raise ValueError(f"actuator[{where}] must be a dict of {list(_CHANNEL_KEYS)}") + for k, v in spec.items(): + if k == "user": + dst.user = v + elif k in _CHANNEL_KEYS: + setattr(dst, k, _finite(v, f"{where}][{k}")) + elif k not in extra: + raise ValueError(f"actuator[{where}]: unknown key {k!r}; " + f"allowed {list(_CHANNEL_KEYS) + ['user'] + list(extra)}") + + +def _conf_catalog_root(): + pkg = Path(__file__).resolve().parent + if (pkg / "vdsim_configs" / "catalog" / "manifest.yaml").is_file(): + return pkg + for c in (REPO, Path.cwd()): + if (c / "configs" / "catalog" / "manifest.yaml").is_file(): + return c + return REPO + + +def load_actuator_preset(preset_id: str) -> dict: + """Body of a catalog part ``type: actuator`` (schema ``actuator_v1``) as a spec dict.""" + if str(pkg_dir := Path(__file__).resolve().parent) not in sys.path: + sys.path.insert(0, pkg_dir.as_posix()) + from catalog import CatalogResolver + doc = CatalogResolver(_conf_catalog_root()).load_part(preset_id) + if doc["type"] != "actuator" or doc["schema"] != "actuator_v1": + raise ValueError(f"{preset_id!r} is a {doc['type']}/{doc['schema']} part, not actuator/actuator_v1") + return dict(doc["body"]) + + +def _merge_spec(base, over): + out = {k: (dict(v) if isinstance(v, dict) else v) for k, v in base.items()} + for ch, spec in over.items(): + if isinstance(spec, dict) and isinstance(out.get(ch), dict): + out[ch].update(spec) + else: + out[ch] = spec + return out + + +def build_actuator_params(spec): + """Turn ``spec`` into a ``vdsim.ActuatorParams``. + + ``spec`` is one of + * ``None`` -- identity (no actuator effect); + * a ready ``vdsim.ActuatorParams``; + * a catalog id string, e.g. ``"actuator.passenger_ev_nominal"``; + * a dict with any of ``steer`` / ``throttle`` / ``brake`` (each a dict of + ``dead_time_s``, ``tau_s``, ``rate_limit``, ``dead_zone``, ``out_min``, + ``out_max``, ``backlash``, ``user``; steer also ``friction`` / ``inertia`` / + ``servo_kp`` / ``servo_kd``; brake also ``thermal``). A ``"preset"`` key + names a catalog id the other keys are laid over. + Unlisted fields keep the identity default. + """ + if spec is None: + return vdsim.ActuatorParams() + if isinstance(spec, vdsim.ActuatorParams): + return spec + if isinstance(spec, str): + spec = {"preset": spec} + if not isinstance(spec, dict): + raise ValueError("actuator must be None, a preset id, vdsim.ActuatorParams or a dict") + spec = dict(spec) + if "preset" in spec: + spec = _merge_spec(load_actuator_preset(spec.pop("preset")), spec) + unknown = set(spec) - {"steer", "throttle", "brake"} + if unknown: + raise ValueError(f"actuator: unknown channel(s) {sorted(unknown)}; " + "allowed ['steer', 'throttle', 'brake']") + ap = vdsim.ActuatorParams() + if "steer" in spec: + st = spec["steer"] + _fill_channel(ap.steer.ch, st, "steer", _STEER_EXTRA) + if isinstance(st.get("friction"), dict): + ap.steer.friction.enabled = True + for k, v in st["friction"].items(): + if k not in _LUGRE_KEYS: + raise ValueError(f"actuator[steer][friction]: unknown key {k!r}; allowed {list(_LUGRE_KEYS)}") + setattr(ap.steer.friction, k, _finite(v, f"steer][friction][{k}")) + for k in ("inertia", "servo_kp", "servo_kd"): + if k in st: + setattr(ap.steer, k, _finite(st[k], f"steer][{k}")) + if "throttle" in spec: + _fill_channel(ap.throttle, spec["throttle"], "throttle") + if "brake" in spec: + br = spec["brake"] + _fill_channel(ap.brake.ch, br, "brake", _BRAKE_EXTRA) + if isinstance(br.get("thermal"), dict): + ap.brake.thermal_enabled = True + for k, v in br["thermal"].items(): + if k not in _THERMAL_KEYS: + raise ValueError(f"actuator[brake][thermal]: unknown key {k!r}; allowed {list(_THERMAL_KEYS)}") + if k in ("mu_T_temp", "mu_T_scale"): + setattr(ap.brake, k, [_finite(x, f"brake][thermal][{k}") for x in v]) + else: + setattr(ap.brake, k, _finite(v, f"brake][thermal][{k}")) + return ap + + def _validate_dt(control_dt: float, substep_dt: float): if not (control_dt > 0.0): raise ValueError("control_dt must be > 0") @@ -509,6 +627,9 @@ def __init__( control_dt: float = 0.05, substep_dt: float = 5e-4, level: str = "L2", + actuator=None, + drive_torque_ref: float = 0.0, + brake_torque_ref: float = 0.0, ): if level not in PLANT_LEVELS: raise ValueError( @@ -551,6 +672,10 @@ def __init__( opts.friction.polygons = poly_patches opts.friction.blend_distance = 1.0 opts.level = level + if actuator is not None: + opts.actuator = build_actuator_params(actuator) + opts.drive_torque_ref = float(drive_torque_ref) + opts.brake_torque_ref = float(brake_torque_ref) self.level = level self.control_dt = float(control_dt) @@ -595,6 +720,19 @@ def __init__( }, } + @property + def actuator(self): + """The ``vdsim.ActuatorParams`` currently in force (a copy).""" + return self._sess.actuator() + + def set_actuator(self, actuator): + """Swap the actuator on a live plant, keeping its memory (delay line, lag). + + Accepts what ``actuator=`` accepts. The next :meth:`reset` returns to the + as-built actuator. + """ + self._sess.set_actuator(build_actuator_params(actuator)) + def reset(self, state0=None): """Seed pose [X, Y, psi, vx, vy, r]; wheel spin = vx/R (make_init_state).""" if state0 is None: From 2d3f178135175cf6723c1e60989d21d67bc0bcbb Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 12:53:04 +0900 Subject: [PATCH 4/7] test(actuator): plant actuator, cascade sensor, retired direct-control options, closed-loop touch point Co-Authored-By: Claude Opus 5.5 --- cosim/CMakeLists.txt | 12 ++ tests/scripts/test_cascade_sensor.py | 107 +++++++++++ tests/scripts/test_closedloop_eval.py | 26 +++ tests/scripts/test_plant_actuator.py | 265 ++++++++++++++++++++++++++ tests/scripts/test_vla_plant.py | 49 +++++ 5 files changed, 459 insertions(+) create mode 100755 tests/scripts/test_cascade_sensor.py create mode 100755 tests/scripts/test_plant_actuator.py diff --git a/cosim/CMakeLists.txt b/cosim/CMakeLists.txt index d638868e..a3c77306 100644 --- a/cosim/CMakeLists.txt +++ b/cosim/CMakeLists.txt @@ -122,6 +122,18 @@ if(Python3_FOUND) set_tests_properties(vla_plant PROPERTIES WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ENVIRONMENT "VDSIM_BUILD_DIR=${CMAKE_BINARY_DIR};PYTHONPATH=${CMAKE_BINARY_DIR}/python:${CMAKE_SOURCE_DIR}/python") + add_test(NAME plant_actuator + COMMAND ${Python3_EXECUTABLE} + ${CMAKE_SOURCE_DIR}/tests/scripts/test_plant_actuator.py) + set_tests_properties(plant_actuator PROPERTIES + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ENVIRONMENT "VDSIM_BUILD_DIR=${CMAKE_BINARY_DIR};PYTHONPATH=${CMAKE_BINARY_DIR}/python:${CMAKE_SOURCE_DIR}/python") + add_test(NAME cascade_sensor + COMMAND ${Python3_EXECUTABLE} + ${CMAKE_SOURCE_DIR}/tests/scripts/test_cascade_sensor.py) + set_tests_properties(cascade_sensor PROPERTIES + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ENVIRONMENT "VDSIM_BUILD_DIR=${CMAKE_BINARY_DIR};PYTHONPATH=${CMAKE_BINARY_DIR}/python:${CMAKE_SOURCE_DIR}/python") # Closed-loop evaluation modes (work order 31): sync and virtual_async # bit-identity, seeded latency and overrun schedules. add_test(NAME closedloop_eval diff --git a/tests/scripts/test_cascade_sensor.py b/tests/scripts/test_cascade_sensor.py new file mode 100755 index 00000000..4fc3eb43 --- /dev/null +++ b/tests/scripts/test_cascade_sensor.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +"""Python surface of CascadeGains (set_cascade_gains) and SensorNoise.lag_tau_s.""" +import sys +import tempfile +from pathlib import Path + +REPO = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(REPO / "build" / "python")) +sys.path.insert(0, str(REPO / "python")) + +import vdsim # noqa: E402 + + +def _session(**kw): + vp, tp = vdsim.VehicleParams(), vdsim.TireParams() + s = vdsim.make_sim_session(vp, tp, "L2", nominal_dt=0.005, **kw) + s.reset(vdsim.make_init_state(v=20.0, wheel_radius=vp.wheel_radius_nominal)) + return s + + +def _speed_after(s, v_target, seconds): + cmd = vdsim.CmdL6() + cmd.v_target = v_target + for _ in range(int(seconds / 0.005)): + s.set_input(cmd) + s.tick(0.005) + return s.output().state.vx() + + +def test_cascade_gains_defaults_and_roundtrip(): + g = vdsim.CascadeGains() + assert (g.vx.kp, g.vx.ki, g.ax.kp, g.ax.ki, g.ax.kff) == (0.8, 0.20, 0.4, 0.6, 0.10) + assert (g.pp_lookahead_min, g.pp_lookahead_k, g.yaw_kp, g.yaw_ki) == (1.5, 0.40, 0.15, 0.05) + s = _session() + assert s.cascade_gains().vx.kp == 0.8 + g.vx.kp, g.ax.kd = 1.2, 0.01 + s.set_cascade_gains(g) + got = s.cascade_gains() + assert got.vx.kp == 1.2 and got.ax.kd == 0.01 + + +def test_cascade_gains_default_set_is_bit_identical(): + a, b = _session(), _session() + b.set_cascade_gains(vdsim.CascadeGains()) + assert _speed_after(a, 10.0, 6.0) == _speed_after(b, 10.0, 6.0) + + +def test_cascade_gains_change_tracking_and_reject_bad_values(): + off = _session() + g = vdsim.CascadeGains() + g.vx.kp = g.vx.ki = 0.0 + off.set_cascade_gains(g) + assert _speed_after(_session(), 10.0, 15.0) < 11.5 + assert _speed_after(off, 10.0, 15.0) > 15.0 + bad = vdsim.CascadeGains() + bad.yaw_kp = -1.0 + try: + off.set_cascade_gains(bad) + except ValueError: + pass + else: + raise AssertionError("negative gain must be rejected") + assert off.cascade_gains().yaw_kp == 0.15 + + +def test_vec_session_cascade_gains_broadcast_and_length_check(): + vp, tp = vdsim.VehicleParams(), vdsim.TireParams() + v = vdsim.make_vec_session(2, vp, tp, "L2", nominal_dt=0.005) + v.set_cascade_gains([vdsim.CascadeGains()]) + v.set_cascade_gains([vdsim.CascadeGains(), vdsim.CascadeGains()]) + try: + v.set_cascade_gains([vdsim.CascadeGains()] * 3) + except ValueError: + pass + else: + raise AssertionError("wrong length must be rejected") + + +def test_sensor_lag_field_trails_truth_and_yaml_roundtrip(): + sp = vdsim.SensorParams() + sp.enabled = True + sp.gnss_vel.lag_tau_s = 0.5 + with tempfile.TemporaryDirectory() as d: + p = str(Path(d) / "sensors.yaml") + sp.to_yaml(p) + assert vdsim.SensorParams.from_yaml(p).gnss_vel.lag_tau_s == 0.5 + lag, plain = _session(sensors=sp), _session() + cmd = vdsim.CmdL4() + cmd.throttle = 1.0 + for s in (lag, plain): + s.set_input(cmd) + s.tick(0.005) + assert abs(lag.output().sensors.gnss_vx - 20.0) < 0.1, "first sample must be primed on truth" + for _ in range(400): + lag.tick(0.005) + o = lag.output() + assert o.sensors.gnss_vx < o.state.vx() - 0.2 + + +if __name__ == "__main__": + n = 0 + for name, fn in list(globals().items()): + if name.startswith("test_") and callable(fn): + fn() + n += 1 + print("PASS", name) + print(f"{n} passed") diff --git a/tests/scripts/test_closedloop_eval.py b/tests/scripts/test_closedloop_eval.py index 7eff0d2b..b0bc8ce3 100755 --- a/tests/scripts/test_closedloop_eval.py +++ b/tests/scripts/test_closedloop_eval.py @@ -383,6 +383,31 @@ def test_lateral_error_and_tracking_stats(): f"{st['e_y_rmse_m']:.4f} m over {st['e_y_n']} samples") +ACTUATOR = {"steer": {"dead_time_s": 0.005, "tau_s": 0.02}, + "throttle": {"dead_time_s": 0.01, "tau_s": 0.05}} + + +def test_actuator_plant_sync_equals_async_zero(): + """Work order 32: with actuator= on the plant, mode A and mode B with zero + latency stay bit-identical (skip and queue), and the chain acts on the run. + """ + plant = dict(PLANT, actuator=ACTUATOR) + a = ClosedLoopEval(plant, controller, TC, mode="sync", duration=DURATION, + state0=STATE0).run() + keys = TRACE_STATE_KEYS + ("u_steer", "u_fx") + for policy in ("skip", "queue"): + b = ClosedLoopEval(plant, controller, TC, mode="virtual_async", + duration=DURATION, latency=LatencyModel.fixed(0.0), + overrun=policy, state0=STATE0).run() + d = _max_abs_diff(a.trace, b.trace, keys) + assert d == 0.0, f"actuator plant, {policy}: fixed(0) differs from sync by {d}" + bare = _run("sync") + d_act = _max_abs_diff(bare.trace, a.trace) + assert d_act > 1e-6, f"the actuator chain had no effect (max|diff| = {d_act})" + print(f"actuator plant ok: sync == virtual_async fixed(0); chain moves the run " + f"by {d_act:.3e}") + + def main(): test_s1_sync_matches_plant_loop() test_s2_async_zero_latency_matches_sync() @@ -396,6 +421,7 @@ def main(): test_trace_virtual_async_timing_channels() test_trace_timing_needs_eval_mode() test_lateral_error_and_tracking_stats() + test_actuator_plant_sync_equals_async_zero() print("test_closedloop_eval: all ok") diff --git a/tests/scripts/test_plant_actuator.py b/tests/scripts/test_plant_actuator.py new file mode 100755 index 00000000..425338b2 --- /dev/null +++ b/tests/scripts/test_plant_actuator.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +"""VDSimPlant actuator= : longitudinal + steer channels in the direct-control path.""" +import math +import sys +from pathlib import Path + +REPO = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(REPO / "build" / "python")) +sys.path.insert(0, str(REPO / "python")) + +import vdsim # noqa: E402 +from vdsim_plant import VDSimPlant, build_actuator_params # noqa: E402 + +DT, SUB = 0.01, 0.001 +V0 = 15.0 + + +def _plant(**kw): + p = VDSimPlant(config="generic_sedan.yaml", level="L3", control_dt=DT, substep_dt=SUB, **kw) + p.reset([0.0, 0.0, 0.0, V0, 0.0, 0.0]) + return p + + +def _fx_for_throttle(p, thr): + return thr * p._vp.max_motor_torque * p._vp.final_drive_ratio / p._vp.wheel_radius_nominal + + +def test_default_is_identity(): + a, b = _plant(), _plant(actuator={}) + for k in range(200): + u = [0.04 * math.sin(0.05 * k), 900.0 * math.sin(0.03 * k)] + assert repr(a.step(u)) == repr(b.step(u)) + + +def test_throttle_dead_time_and_lag_match_analytic(): + L, tau, thr = 0.10, 0.20, 0.4 + p = _plant(actuator={"throttle": {"dead_time_s": L, "tau_s": tau}}) + fx = _fx_for_throttle(p, thr) + worst = 0.0 + for k in range(120): + p.step([0.0, fx]) + t = (k + 1) * DT + want = 0.0 if t <= L else thr * (1.0 - math.exp(-(t - L) / tau)) + worst = max(worst, abs(p._sess.output().throttle_applied - want)) + assert worst < 1e-9, worst + + +def test_delay_changes_the_trajectory_and_keeps_wheel_shares(): + plain, slow = _plant(), _plant(actuator={"throttle": {"dead_time_s": 0.3}}) + fx = _fx_for_throttle(plain, 0.3) + for _ in range(100): + a, b = plain.step([0.0, fx]), slow.step([0.0, fx]) + assert a["vx"] > b["vx"] + 0.05 + sf = plain._vp.drive_split_front + wf = sum(slow._dyn.tire_forces_body()[i][0] for i in (0, 1)) + wr = sum(slow._dyn.tire_forces_body()[i][0] for i in (2, 3)) + assert wf > 0 and wr > 0 + assert abs(wf / (wf + wr) - sf) < 0.15 + + +def test_brake_channel_is_applied_and_released_after_delay(): + p = _plant(actuator={"brake": {"dead_time_s": 0.05, "tau_s": 0.0}}) + fx = -0.3 * p._vp.max_brake_torque / p._vp.wheel_radius_nominal + seen = [] + for k in range(20): + p.step([0.0, fx]) + seen.append(p._sess.output().brake_applied) + assert seen[3] == 0.0 and seen[-1] > 0.25 + + +def test_steer_channel_lags(): + p = _plant(actuator={"steer": {"dead_time_s": 0.05}}) + seen = [] + for _ in range(10): + p.step([0.1, 0.0]) + seen.append(p._sess.output().steer_applied) + assert seen[3] == 0.0 and abs(seen[-1] - 0.1) < 1e-9 + + +def test_set_actuator_midflight_and_reset_returns_to_built(): + p = _plant() + for _ in range(20): + p.step([0.0, 500.0]) + p.set_actuator({"throttle": {"tau_s": 0.5}}) + assert abs(p.actuator.throttle.tau_s - 0.5) < 1e-12 + p.reset([0.0, 0.0, 0.0, V0, 0.0, 0.0]) + p.step([0.0, 500.0]) + + +def test_snapshot_replay_matches_within_ulp(): + act = {"throttle": {"dead_time_s": 0.1, "tau_s": 0.2}, "brake": {"dead_time_s": 0.05}} + p = _plant(actuator=act) + fx = _fx_for_throttle(p, 0.4) + out, snap = [], None + for k in range(100): + if k == 40: + snap = p._sess.snapshot() + out.append(p.step([0.02, fx if k < 60 else -fx])) + p._sess.restore(snap) + for k in range(40, 100): + o = p.step([0.02, fx if k < 60 else -fx]) + for key in ("X", "Y", "psi", "vx", "vy", "r"): + assert abs(o[key] - out[k][key]) < 1e-9, (k, key) + + +def test_bad_specs_raise(): + for bad in ({"thrott": {}}, {"throttle": {"tau": 1.0}}, {"throttle": {"tau_s": float("nan")}}, 3): + try: + build_actuator_params(bad) + except ValueError: + continue + raise AssertionError(f"accepted {bad!r}") + assert isinstance(build_actuator_params(None), vdsim.ActuatorParams) + + +def test_backlash_play_operator(): + p = _plant(actuator={"steer": {"backlash": 0.02}}) + def applied(delta): + for _ in range(3): + p.step([delta, 0.0]) + return p._sess.output().steer_applied + assert abs(applied(0.10) - 0.09) < 1e-12 # engaged: follows at u - w/2 + assert abs(applied(0.095) - 0.09) < 1e-12 # inside the band: holds + assert abs(applied(0.07) - 0.08) < 1e-12 # reversal: follows at u + w/2 + + +class _Gain: + def __init__(self, k): + self.k, self.n = k, 0 + + def step(self, u, dt): + self.n += 1 + return self.k * u + + def reset(self): + self.n = 0 + + def get_state(self): + return [float(self.n)] + + def set_state(self, v): + self.n = int(v[0]) + + +def test_user_channel_stage_runs_and_is_not_shared(): + proto = _Gain(0.5) + a = _plant(actuator={"steer": {"user": proto}}) + b = _plant(actuator={"steer": {"user": proto}}) + for _ in range(5): + a.step([0.1, 0.0]) + assert abs(a._sess.output().steer_applied - 0.05) < 1e-12 + b.step([0.1, 0.0]) + assert proto.n == 0 # prototype is never stepped + assert a._sess.actuator().steer.ch.user is proto + + +class _Drift(_Gain): + def step(self, u, dt): + self.n += 1 + return self.k * u + 1e-6 * self.n + + +def test_user_throttle_stage_scales_force(): + plain = _plant() + half = _plant(actuator={"throttle": {"user": _Gain(0.5)}}) + fx = _fx_for_throttle(plain, 0.4) + for _ in range(50): + plain.step([0.0, fx]) + half.step([0.0, fx]) + assert abs(half._sess.output().throttle_applied - 0.2) < 1e-9 + assert plain.step([0.0, fx])["vx"] > half.step([0.0, fx])["vx"] + + +def test_user_stage_state_survives_snapshot(): + p = _plant(actuator={"throttle": {"user": _Drift(1.0)}}) + fx = _fx_for_throttle(p, 0.4) + for _ in range(30): + p.step([0.0, fx]) + snap = p._sess.snapshot() + p.step([0.0, fx]) + first = p._sess.output().throttle_applied + p.step([0.0, fx]) + p._sess.restore(snap) + p.step([0.0, fx]) + assert p._sess.output().throttle_applied == first + + +def test_preset_id_and_override(): + p = _plant(actuator="actuator.generic_sedan") + a = p.actuator + assert abs(a.steer.ch.dead_time_s - 0.05) < 1e-12 and abs(a.steer.ch.tau_s - 0.08) < 1e-12 + q = _plant(actuator={"preset": "actuator.generic_sedan", "steer": {"dead_time_s": 0.2}}) + assert abs(q.actuator.steer.ch.dead_time_s - 0.2) < 1e-12 + assert abs(q.actuator.steer.ch.tau_s - 0.08) < 1e-12 # untouched field kept + ideal = build_actuator_params("actuator.ideal") + assert ideal.steer.ch.dead_time_s == 0.0 and ideal.throttle.tau_s == 0.0 + try: + build_actuator_params("brake.sedan") + except ValueError: + pass + else: + raise AssertionError("non-actuator part accepted") + + +def test_steer_friction_and_brake_thermal_specs(): + ap = build_actuator_params({ + "steer": {"friction": {"Tc": 0.8, "Ts": 1.2}, "servo_kp": 80.0}, + "brake": {"thermal": {"heat_coeff": 1e-3, "cool_coeff": 1e-2, + "mu_T_temp": [20.0, 400.0], "mu_T_scale": [1.0, 0.6]}}}) + assert ap.steer.friction.enabled and abs(ap.steer.friction.Tc - 0.8) < 1e-12 + assert ap.brake.thermal_enabled and list(ap.brake.mu_T_scale) == [1.0, 0.6] + + +def _steer_applied_series(actuator, n=80): + """Applied road-wheel steer after each step of a one-substep plant. + + The command varies every step, so a pure shift is distinguishable from a + constant that merely starts late. + """ + p = VDSimPlant(config="generic_sedan.yaml", level="L3", control_dt=SUB, + substep_dt=SUB, actuator=actuator) + p.reset([0.0, 0.0, 0.0, V0, 0.0, 0.0]) + out = [] + for k in range(n): + p.step([0.05 + 0.04 * math.sin(0.3 * k), 0.0]) + out.append(p._sess.output().steer_applied) + return out + + +def test_steer_dead_time_is_an_exact_substep_shift(): + """B1 (work order 32): steer dead time D delays the applied steer by D/substep. + + Replaces the retired direct-control steer_dead_time_s option: the same + field is filled through actuator={"steer": {"dead_time_s": D}}. Checked for + D = 5 ms and 20 ms with and without the first-order lag; a shift one + substep short must not match. + """ + for tau in (0.0, 0.02): + ref = _steer_applied_series({"steer": {"dead_time_s": 0.0, "tau_s": tau}}) + assert any(v != 0.0 for v in ref[:2]), "zero dead time must act at once" + for D in (0.005, 0.020): + n = int(round(D / SUB)) + got = _steer_applied_series({"steer": {"dead_time_s": D, "tau_s": tau}}) + assert all(v == 0.0 for v in got[:n]), (tau, D, got[:n + 1]) + assert got[n:] == ref[:len(ref) - n], f"tau={tau} D={D}: not a {n}-substep shift" + assert got[n - 1:] != ref[:len(ref) - n + 1], f"tau={tau} D={D}: shift test cannot tell n-1" + + +def test_steer_zero_dead_time_equals_no_actuator(): + """B1: dead_time_s = 0 on the steer channel is bit-identical to actuator=None.""" + a = _plant() + b = _plant(actuator={"steer": {"dead_time_s": 0.0}}) + for k in range(200): + u = [0.04 * math.sin(0.05 * k), 900.0 * math.sin(0.03 * k)] + assert repr(a.step(u)) == repr(b.step(u)) + + +if __name__ == "__main__": + n = 0 + for name, fn in list(globals().items()): + if name.startswith("test_") and callable(fn): + fn() + n += 1 + print("PASS", name) + print(f"{n} passed") diff --git a/tests/scripts/test_vla_plant.py b/tests/scripts/test_vla_plant.py index e073de36..65c1f059 100644 --- a/tests/scripts/test_vla_plant.py +++ b/tests/scripts/test_vla_plant.py @@ -226,7 +226,56 @@ def test_speed_budget(): elapsed, BUDGET_S)) +def test_level_default_and_rejects(): + """B2 (work order 32): the retired dynamics_level option is ``level`` here. + + Ported from feat/direct-control-dynamics-level: an unknown level is refused + by name, and the default plant equals an explicit level='L2' bit for bit. + """ + for bad in ("L5", "l3", "7dof", ""): + try: + VDSimPlant(level=bad) + except ValueError as e: + assert "level" in str(e), str(e) + else: + raise AssertionError(f"level={bad!r} should raise ValueError") + traj_u = [(0.03, 800.0)] * 20 + runs = [] + for kw in ({}, {"level": "L2"}): + plant = VDSimPlant(base_mu=0.9, control_dt=DT, substep_dt=SUB, **kw) + plant.reset([0.0, 0.0, 0.0, V0, 0.0, 0.0]) + runs.append([tuple(plant.step(list(u))[k] for k in ("X", "Y", "psi", "vx", "vy", "r")) + for u in traj_u]) + assert runs[0] == runs[1], "default must equal explicit level='L2'" + print("level reject + default: ok") + + +def test_level_l3_builds_fourteen_dof_and_steps(): + """B2: level='L3' builds the 14-DOF model and keeps the L2 observation layout. + + L4 is not here: VDSimPlant refuses a bare L4 (test_experiment_api, + test_plant_bare_l4_is_refused). The core factory's L4 is covered by + DirectControlSession.L3AndL4CreateAndTickFinite in test_sim_session.cpp. + """ + expect = {"L2": vdsim.Level.L2_SevenDOF, "L3": vdsim.Level.L3_FourteenDOF} + ref_keys = None + for lvl, enum in expect.items(): + plant = VDSimPlant(base_mu=0.9, control_dt=DT, substep_dt=SUB, level=lvl) + assert plant._dyn.level() == enum, f"{lvl}: wrong dynamics object {plant._dyn.level()}" + obs = plant.reset([0.0, 0.0, 0.0, V0, 0.0, 0.0]) + for _ in range(40): + obs = plant.step([0.03, 500.0]) + keys = (sorted(obs.keys()), sorted(obs["wheel"][0].keys()), len(obs["wheel"])) + ref_keys = ref_keys or keys + assert keys == ref_keys, f"{lvl}: obs layout differs from L2" + assert all(math.isfinite(v) for v in (obs["X"], obs["vx"], obs["r"])), lvl + assert obs["r"] > 0.01, f"{lvl}: expected positive yaw rate, r={obs['r']}" + print("level L3 builds 14-DOF + obs layout: ok") + + def main(): + test_level_default_and_rejects() + test_level_l3_builds_fourteen_dof_and_steps() test_cmdl1_element_torque_assign() test_longitudinal_fx_accel() test_dry_qualitative_lane_change() From a12e49f30eb48e27823c7e6689bd7d594245f7d1 Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 12:53:04 +0900 Subject: [PATCH 5/7] docs(actuator): usage guide, theory and catalog notes, docs navigation Co-Authored-By: Claude Opus 5.5 --- docs/ACTUATOR_GUIDE.md | 207 +++++++++++++++++++++++ docs/design/PARTS_CATALOG.md | 2 +- docs/references/actuator_nonlinearity.md | 2 +- docs/theory/17_actuator_and_sensing.md | 4 + mkdocs.yml | 1 + 5 files changed, 214 insertions(+), 2 deletions(-) create mode 100644 docs/ACTUATOR_GUIDE.md diff --git a/docs/ACTUATOR_GUIDE.md b/docs/ACTUATOR_GUIDE.md new file mode 100644 index 00000000..259f80c9 --- /dev/null +++ b/docs/ACTUATOR_GUIDE.md @@ -0,0 +1,207 @@ +# Actuator Model Guide + +VDSim 이 기본 제공하는 액추에이터 모델(데드타임, 1차 랙, 백래시, 레이트 제한, 포화, 데드존, +조향 LuGre 마찰, 브레이크 열 페이드)의 사용법과, 사용자 정의 단계(user stage) 확장법을 정리한다. +연구 코드에서 지연·랙을 파이썬 래퍼로 따로 구현할 필요가 없도록 하는 것이 목적이다. +모델 식과 근거 문헌은 `references/actuator_nonlinearity.md`, 이론은 `theory/17_actuator_and_sensing.md`. + +## 1. 구조 + +```mermaid +flowchart LR + U[명령 u] --> DZ[데드존] + DZ --> DT[전송 지연
dead_time_s] + DT --> BL[백래시
backlash] + BL --> LAG[1차 랙
tau_s] + LAG --> RL[레이트 제한
rate_limit] + RL --> SAT[포화
out_min/out_max] + SAT --> US[사용자 단계
user] + US --> Y[실현 명령] +``` + +- 채널은 `steer`, `throttle`, `brake` 세 개이며 채널마다 같은 체인을 가진다. +- 모든 필드의 기본값은 꺼짐(0)이며, 전부 꺼져 있으면 기존 결과와 비트 단위로 동일하다. +- `steer` 는 선택적으로 서보+LuGre 마찰(`friction`) 모드를, `brake` 는 열 페이드(`thermal`)를 가진다. +- 조향 채널 단위는 노면 바퀴 각 [rad], `throttle`/`brake` 는 0..1 페달 비율이다. + +## 2. 필드 + +| 필드 | 단위 | 의미 | 0 일 때 | +|---|---|---|---| +| `dead_time_s` | s | 순수 전송 지연 (분수 샘플 보간) | 없음 | +| `tau_s` | s | 1차 랙 시정수 | 없음 | +| `rate_limit` | 단위/s | 출력 변화율 상한 | 비활성 | +| `dead_zone` | - | 입력 데드밴드 후 재스케일 (페달 유격, 패드 간극) | 없음 | +| `out_min`, `out_max` | - | 포화 | 사실상 무제한 | +| `backlash` | - | 유격폭(전체 밴드). 입력이 출력에서 폭/2 이상 벗어나야 따라감 | 없음 | +| `user` | - | 사용자 정의 단계 (5절) | 없음 | +| `steer.friction` | - | LuGre `sigma0 sigma1 sigma2 Tc Ts ws` (켜면 서보 모드) | 꺼짐 | +| `steer.inertia/servo_kp/servo_kd` | - | 서보 모드 파라미터 | 기본값 | +| `brake.thermal` | - | `heat_coeff cool_coeff T_ambient mu_T_temp mu_T_scale` | 꺼짐 | + +백래시는 play 연산자(속도 무관 히스테리시스)이다. 폭 0.02 에서 입력이 0.10 으로 올라가면 출력은 0.09, +입력이 0.095 로 내려가도 유지, 0.07 로 내려가면 0.08 이 된다. + +## 3. 사용법 (파이썬) + +```python +from vdsim_plant import VDSimPlant + +plant = VDSimPlant(config="generic_sedan.yaml", level="L3", + control_dt=0.01, substep_dt=0.001, + actuator={ + "steer": {"dead_time_s": 0.05, "tau_s": 0.08, "backlash": 0.002}, + "throttle": {"dead_time_s": 0.10, "tau_s": 0.25}, + "brake": {"dead_time_s": 0.08, "tau_s": 0.15, "dead_zone": 0.03}, + }) +plant.reset([0, 0, 0, 15.0, 0, 0]) +plant.step([delta, fx_total]) # 계약은 그대로 u = [delta, Fx_total] +plant.actuator # 현재 ActuatorParams +plant.set_actuator({"throttle": {"tau_s": 0.5}}) # 롤아웃 도중 교체 (메모리 유지) +``` + +- `actuator` 인자는 `None`, `ActuatorParams`, 카탈로그 ID 문자열, dict 중 하나이다. +- dict 는 `"preset": "actuator.generic_sedan"` 키로 프리셋을 깔고 그 위에 필드를 덮어쓸 수 있다. +- 알 수 없는 키나 비유한 값은 `ValueError` 로 거부된다 (오타가 조용히 무시되지 않음). +- `set_actuator()` 는 전송 지연 링과 랙·서보·열 상태를 지우지 않고 파라미터만 바꾼다. 열화·고장 주입·지연 스텝에 쓴다. +- `reset()` 은 생성 시 지정한 액추에이터로 돌아간다. +- 직접 제어 세션(`make_direct_control_session`)은 `DirectControlSessionOptions.actuator` 로 같은 구조체를 받는다. +- 벡터 환경은 `VecSession` 에서 env 별로 교체한다. + +## 4. 프리셋 (차량별 YAML) + +위치는 `configs/parts/actuator/` 이고 `configs/catalog/manifest.yaml` 에 등록되어 있다 (type `actuator`, schema `actuator_v1`). + +| ID | 내용 | +|---|---| +| `actuator.ideal` | 모든 채널 꺼짐 (기본 동작과 동일) | +| `actuator.generic_sedan` | 공칭 임시값 + 조향 백래시 0.002 | + +새 프리셋은 YAML 하나를 추가하고 매니페스트에 등록하면 된다. +실차에서 측정한 차량 고유 값은 이 공개 카탈로그에 두지 않는다. 그런 값은 dict 나 `ActuatorParams` 로 실행 시에 넘긴다. + +```yaml +id: actuator.my_vehicle +type: actuator +version: 1 +schema: actuator_v1 +body: + steer: {dead_time_s: 0.10, tau_s: 0.05} + throttle: {dead_time_s: 0.08, tau_s: 0.20} + brake: {dead_time_s: 0.06, tau_s: 0.10, dead_zone: 0.02} +``` + +실측 로그가 있으면 임시값을 반드시 실측으로 교체한다. 프리셋의 임시값으로 얻은 결과는 논문 수치로 쓰지 않는다. + +## 5. 사용자 정의 단계 + +내장 체인으로 표현되지 않는 동작(비선형 맵, 학습 모델, 온도 의존 지연 등)은 `user` 단계로 끼운다. +`user` 는 해당 채널의 내장 체인 출력 뒤에 실행된다. 내장 필드를 모두 0 으로 두면 체인 전체를 대체한다. + +### 파이썬 (덕 타이핑) + +```python +class Quantizer: + def __init__(self, q): self.q = q + def step(self, u, dt): # 필수: 출력을 돌려준다 + return round(u / self.q) * self.q + def reset(self): pass # 선택: 에피소드 시작 시 호출 + def get_state(self): return [] # 선택: 스냅샷에 저장할 float 리스트 + def set_state(self, v): pass # 선택 + +plant = VDSimPlant(..., actuator={"steer": {"user": Quantizer(0.001)}}) +``` + +- 객체는 `step(u, dt)` 만 필수이며, 없으면 `TypeError` 가 난다. `reset` 이 없으면 호출이 생략되고, `get_state/set_state` 가 없으면 스냅샷이 이 단계의 내부 상태를 복원하지 못한다. +- 넘긴 객체는 원형(prototype)이며 직접 호출되지 않는다. env 마다 `copy.deepcopy` 한 복제본이 돌기 때문에 상태가 env 간에 공유되지 않는다. +- 호출은 물리 서브스텝마다 일어나며 GIL 을 잡는다. 순수 파이썬이라 무거운 연산을 넣으면 시뮬 속도가 떨어진다. +- `throttle`/`brake` 의 사용자 출력은 [0, 1] 로 다시 포화된다. + +### C++ (`IChannelModel`) + +```cpp +struct Quantizer : vdsim::IChannelModel { + double q; + explicit Quantizer(double q_) : q(q_) {} + double step(double u, double) override { return std::round(u / q) * q; } + void reset() override {} + std::unique_ptr clone() const override { return std::make_unique(*this); } + // 선택: std::vector get_state() const / void set_state(const std::vector&) +}; +params.steer.ch.user = std::make_shared(0.001); +``` + +## 6. 종방향 채널의 동작 (직접 제어 경로) + +- 직접 제어 입력은 휠별 토크이다. 구동 합과 제동 합을 `drive_ref = max_motor_torque * final_drive_ratio`, `brake_ref = max_brake_torque` 로 정규화해 `throttle`/`brake` 로 액추에이터에 통과시킨다. +- 이후 직전의 0 이 아닌 휠별 분배 비율로 되돌려 나눈다. 초기 분배는 `drive_split_front` 를 따른다. +- 기준값은 `DirectControlSessionOptions.drive_torque_ref/brake_torque_ref` (VDSimPlant 의 같은 이름 인자) 로 덮어쓸 수 있다. +- 종방향 경로는 `dead_time/tau/rate_limit/dead_zone/포화/backlash/user/브레이크 열` 중 하나라도 켜졌을 때만 활성화된다. 꺼져 있으면 기존 경로와 비트 동일하다. +- 주의: 음의 모터 토크(회생)는 액추에이터를 거치지 않고 그대로 통과한다. +- 주의: `throttle`/`brake` 는 단방향 0..1 채널이므로 기준값을 넘는 요청은 포화된다. +- 주의: 같은 틱에 구동과 제동을 동시에 요청하면 두 채널이 각자 처리된다. + +## 7. 검증 + +- `tests/unit/test_actuator.cpp` (gtest): 체인 각 단계, 백래시, 사용자 단계, 스냅샷 왕복. +- `tests/scripts/test_plant_actuator.py` (ctest `plant_actuator`): 기본값 동일성, 데드타임+랙 해석해 일치(1e-9), 궤적과 휠 분배, 사용자 단계 비공유, 프리셋, 스냅샷 재생. +- 알려진 사항: L3 스냅샷 복원 시 psi, Y 에 1 ulp(부동소수점 최소 단위) 수준 불일치가 있다. 액추에이터가 없어도 재현되는 기존 현상이며 이번 변경과 무관하다. 테스트는 1e-9 허용오차를 쓴다. + +## 8. 다른 서브시스템 현황 (조사 결과) + +액추에이터와 같은 패턴(사용자가 래퍼에서 직접 구현)이 다른 영역에 있는지 저장소 기준으로 조사했다. +저장소 안에서 손으로 짠 대체 구현은 발견되지 않았고, 저장소 밖 래퍼는 이 조사 범위 밖이다. + +| 영역 | 내장 모델 | 확장점 | 판정 | +|---|---|---|---| +| 제어기/센서 | 제어 사다리, 캐스케이드(PI 안티윈드업), 운전자 반응시간, 네트워크 지연, 센서 잡음/바이어스/1차 랙 | 캐스케이드 게인 `set_cascade_gains`, 센서 `lag_tau_s` (9절) | 반영됨. 사용자 정의 제어기 훅은 없음 (루프 외부에서 `step()`) | +| 서스펜션 | 스프링·댐퍼·ARB, 토폴로지, 운동학 | 파이썬/`.so` 모듈 (`SuspensionModule`) | 현 상태 유지 (`force()` 가 무상태여야 하는 제약 있음) | +| 타이어 | mf96/linear/MF2002, 이완 길이, LuGre, 카탈로그 | C++ `ITireModel` 만 (파이썬 트램폴린 없음) | 보강 권장: 파이썬 타이어 훅, 열·마모 상태 | +| 구동계 | 엔진 맵, 변속기, 디프, 변속 정책 콜백 | `DrivetrainModule` | EV 모터·배터리·회생 모델 부재 (EV 연구 시 필요) | +| 공력, 노면 | 상수 계수, mu 프로파일, ISO 8608 | 없음 | 현 상태 유지 | + +후속 제안은 이 표의 "보강 권장" 항목 기준으로 별도 진행한다. + +## 9. 제어기 캐스케이드 게인과 센서 랙 + +### 캐스케이드 게인 (`CascadeGains`) + +`make_sim_session`/`make_vec_session` 세션은 L5~L8 명령을 내부 캐스케이드(속도 PI -> 가속도 PI -> 페달, 순추종, 요레이트 PI)로 낮춘다. +게인은 예전에 코드에 박혀 있었고 이제 세션마다 바꿀 수 있다. 기본값은 이전 하드코딩 값과 같아서 건드리지 않으면 결과가 비트 단위로 같다. + +| 필드 | 기본값 | 의미 | +|---|---|---| +| `vx.kp`, `vx.ki`, `vx.i_max`, `vx.ax_clamp` | 0.8, 0.20, 3.0, 3.5 | L6 속도 루프. 출력은 목표 가속도 [m/s^2] | +| `ax.kp`, `ax.ki`, `ax.kd`, `ax.kff`, `ax.i_max` | 0.4, 0.6, 0.0, 0.10, 2.5 | L5 가속도 루프와 페달 전향 보상 | +| `pp_lookahead_min` [m], `pp_lookahead_k` [s] | 1.5, 0.40 | 순추종 전방주시 거리 Ld = max(min, k * vx) | +| `yaw_kp`, `yaw_ki` | 0.15, 0.05 | 요레이트 PI (L6 r_target -> 조향각) | + +```python +g = vdsim.CascadeGains() +g.vx.kp = 1.2 # 속도 루프를 더 공격적으로 +sess.set_cascade_gains(g) # 에피소드 중에도 가능, 적분기는 유지됨 +sess.cascade_gains().vx.kp # 현재 값 확인 +vec.set_cascade_gains([g]) # VecSession: 길이 1 = 전 env 동일, 길이 N = env별 +``` + +- 음수·NaN 게인은 `ValueError` 이고 기존 값은 그대로 남는다. +- 값은 `reset()` 과 도메인 랜덤화 뒤에도 유지된다 (차량 파라미터에서 오는 휠베이스·최대 조향각만 다시 읽는다). +- 직접 제어 세션(`VDSimPlant`)은 CmdL1 을 그대로 받아 캐스케이드를 쓰지 않으므로 이 게인은 영향이 없다. 그 경로의 제어기는 호출하는 쪽의 몫이다. + +### 센서 랙 (`SensorNoise.lag_tau_s`) + +각 센서 그룹(`imu_accel`, `imu_gyro`, `wheel_speed`, `steer`, `gnss_pos`, `gnss_vel`)에 1차 지연 시정수 `lag_tau_s` [s] 가 생겼다. +참값에 1차 저역통과(`y += (1 - exp(-dt/tau)) * (truth - y)`)를 먼저 적용하고 그 위에 바이어스·잡음을 더한다. 0 이면 필터가 없다. + +```python +sp = vdsim.SensorParams() +sp.enabled = True +sp.gnss_vel.lag_tau_s = 0.15 # GNSS 속도 대역폭 제한 +sess = vdsim.make_sim_session(vp, tp, "L2", sensors=sp) +``` + +- 첫 샘플은 참값으로 초기화되어 0 에서 올라오는 과도가 없다. `reset()` 에서 다시 초기화된다. +- 필터 상태는 스냅샷에 들어가 복원 후 비트 단위로 재생된다. 센서 스냅샷 레이아웃이 15+1 값만큼 늘었으므로 이전 버전이 만든 스냅샷은 복원할 수 없다. +- 센서가 꺼져 있으면(`enabled=False`) 항등이라 랙도 적용되지 않는다. 전송 지연은 별개의 `sensor_delay_s` 로 계속 쓴다. +- YAML 에서는 `gnss_vel: { noise_std: 0.05, lag_tau_s: 0.15 }` 로 쓴다. +- 검증: `tests/unit/test_sim_session.cpp` (SensorLag*, CascadeGains*), `tests/scripts/test_cascade_sensor.py` (ctest `cascade_sensor`). diff --git a/docs/design/PARTS_CATALOG.md b/docs/design/PARTS_CATALOG.md index bbc0d086..ccf473c2 100644 --- a/docs/design/PARTS_CATALOG.md +++ b/docs/design/PARTS_CATALOG.md @@ -92,7 +92,7 @@ Rules: | `brake` | `brake_subsystem_v1` | WS1 brake module config (v0.3 wires L3) | | `steering` | `steering_subsystem_v1` | ratio, deadtime | | `drivetrain` | `drivetrain_v1` | `engine_rotational_inertia`, diff, ratios (`V0.2_DRIVETRAIN.md`) | -| `actuator` | `actuator_v1` | `ActuatorParams` | +| `actuator` | `actuator_v1` | `ActuatorParams` (steer/throttle/brake channels incl. backlash; presets `actuator.ideal`, `actuator.generic_sedan`; see `ACTUATOR_GUIDE.md`) | | `sensor_suite` | `sensor_suite_v1` | `SensorParams` + mount list | New types add a row + resolver branch; they do not change the envelope. diff --git a/docs/references/actuator_nonlinearity.md b/docs/references/actuator_nonlinearity.md index 75ee9026..edb18025 100644 --- a/docs/references/actuator_nonlinearity.md +++ b/docs/references/actuator_nonlinearity.md @@ -2,7 +2,7 @@ actuator/sensor 의 물리 비선형성·지연을 VDSim 에 반영하기 위한 조사 레퍼런스. 모델 형태(식), typical parameter, 학술/산업 근거(DOI), 그리고 control 사다리 -Lc1-Lc8 별 적용 방식을 정리한다. 구현 전 spec 근거 문서이며, thesis 인용 시 +Lc1-Lc8 별 적용 방식을 정리한다. 구현 전 spec 근거 문서이며(사용법은 [Actuator Model Guide](../ACTUATOR_GUIDE.md)),, thesis 인용 시 flag 표시된 항목은 publisher 페이지로 재확인할 것. 신뢰도 표기: [S] 출처 확인 수치 · [E] engineering 추정 · [F] citation 미확정(재확인 필요). diff --git a/docs/theory/17_actuator_and_sensing.md b/docs/theory/17_actuator_and_sensing.md index c3a43d2f..319a6a58 100644 --- a/docs/theory/17_actuator_and_sensing.md +++ b/docs/theory/17_actuator_and_sensing.md @@ -294,3 +294,7 @@ implicit 으로 풀면 무조건 안정. 상세 bibliography 와 실측 ID 문헌은 `docs/references/actuator_nonlinearity.md`. 핵심: LuGre(#2), steering friction physics(#1), steering/driveline backlash (#4·#15·#16), EMB/EHB clamp+dead-zone(#9·#11·#12), FOPDT for MPC(#19·#22). + +--- + +사용법(기본 모델, 백래시, 프리셋, 사용자 정의 단계)은 [Actuator Model Guide](../ACTUATOR_GUIDE.md) 참고. diff --git a/mkdocs.yml b/mkdocs.yml index e9591d2e..2aa03fba 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -136,6 +136,7 @@ nav: - "Catalog, scenes & physics": CATALOG_AND_PHYSICS.md - Running VDSim: RUNNING.md - "RL environment (vdsim_rl)": RL_GUIDE.md + - "Actuator model guide": ACTUATOR_GUIDE.md - "Closed-loop evaluation modes": guides/CLOSED_LOOP_EVAL.md - Validation: VALIDATION.md - "Assumptions & limitations": ASSUMPTIONS.md From a2358a80159fb0bc2669ec6fc90d6631e02b5092 Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 12:53:04 +0900 Subject: [PATCH 6/7] docs(validation): mark the currency block pending for the actuator library tree Co-Authored-By: Claude Opus 5.5 --- docs/VALIDATION.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/VALIDATION.md b/docs/VALIDATION.md index 21dbf349..0addfc81 100644 --- a/docs/VALIDATION.md +++ b/docs/VALIDATION.md @@ -83,11 +83,13 @@ version and the new numbers have to land in the same commit. ```text -tests: 518/518 +tests: PENDING config: cmake --preset validation && cmake --build --preset validation && ctest --preset validation presets: CMakePresets.json@c4d279dcd60c661b61f480db95f5fe86fa82a7b8 toolchain: cmake 3.31.10 -commit: e919fd3 +commit: PENDING +pending: registration surface changed (actuator library tests added on top of main); + this tree has no canonical 3-run measurement yet date: 2026-10-13 excluded: gui_v3_e2e (GUI v3 test group formally deferred -- VDSIM_BUILD_GUI_V3_TESTS=OFF) excluded: gui_v3_api_smoke (GUI v3 test group formally deferred -- VDSIM_BUILD_GUI_V3_TESTS=OFF) From 8f5a8dd7f02e6968b5b14111d8beb832fbcbd70b Mon Sep 17 00:00:00 2001 From: JiwonSeok Date: Tue, 13 Oct 2026 14:04:05 +0900 Subject: [PATCH 7/7] Record the canonical actuator library validation measurement Docs-only: fills the VALIDATION currency block measured on the previous head of this branch. Co-Authored-By: Claude Opus 5.5 --- docs/VALIDATION.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/docs/VALIDATION.md b/docs/VALIDATION.md index 0addfc81..8eb0e45f 100644 --- a/docs/VALIDATION.md +++ b/docs/VALIDATION.md @@ -83,13 +83,11 @@ version and the new numbers have to land in the same commit. ```text -tests: PENDING +tests: 532/532 config: cmake --preset validation && cmake --build --preset validation && ctest --preset validation presets: CMakePresets.json@c4d279dcd60c661b61f480db95f5fe86fa82a7b8 toolchain: cmake 3.31.10 -commit: PENDING -pending: registration surface changed (actuator library tests added on top of main); - this tree has no canonical 3-run measurement yet +commit: a2358a8 date: 2026-10-13 excluded: gui_v3_e2e (GUI v3 test group formally deferred -- VDSIM_BUILD_GUI_V3_TESTS=OFF) excluded: gui_v3_api_smoke (GUI v3 test group formally deferred -- VDSIM_BUILD_GUI_V3_TESTS=OFF)