Actuator library: runtime swap, direct-control session, actuator option on VDSimPlant

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

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

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: {}

core/include/vdsim/actuator.hpp

@@ -15,13 +15,30 @@
 #pragma once
 
 #include "vdsim/snapshot.hpp"
+#include <memory>
 #include <vector>
 
 #include "vdsim/control.hpp"
 #include "vdsim/state.hpp"
 
 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<IChannelModel> clone() const = 0;
+    // Snapshot support (optional): flat state vector, any length.
+    virtual std::vector<double> get_state() const { return {}; }
+    virtual void set_state(const std::vector<double>&) {}
+};
+
 // 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<const IChannelModel> 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<double>& v) const;
     void restore_aux(const std::vector<double>& 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<const IChannelModel> proto;
+        std::unique_ptr<IChannelModel>       inst;
+    };
+    UserSlot steer_user_, throttle_user_, brake_user_;
+    static void sync_user(UserSlot& s, const std::shared_ptr<const IChannelModel>& 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

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<double, double> 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_;

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<double, NUM_WHEELS> 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<double, kLagCh> lag_{};
+    bool lag_primed_{false};
 };
 
 }  // namespace vdsim

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<IContactProvider> ground_;
     std::unique_ptr<IVehNetwork>      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<double, NUM_WHEELS> wheel_alpha_peak_ {{0,0,0,0}};
     std::array<double, NUM_WHEELS> 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<double, NUM_WHEELS> w_drive_ {{0.25, 0.25, 0.25, 0.25}};
+    std::array<double, NUM_WHEELS> w_brake_ {{0.25, 0.25, 0.25, 0.25}};
+    std::array<double, NUM_WHEELS> 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<SimSession> make_direct_control_session(

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<unsigned>& 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<ActuatorParams>& p);
+    std::vector<ActuatorParams> actuators() const;
+
+    // Per-env cascade retune (size 1 broadcasts); keeps the controller memory.
+    void set_cascade_gains(const std::vector<CascadeGains>& 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.