1use serde::{Deserialize, Serialize};
2
3use crate::config::{AllocationStrategy, LIMB_COUNT};
4use crate::state::{ControlInput, LIMB_NAMES, LimbFlow, StepDiagnostics, SystemState};
5
6#[derive(Debug, Clone, Serialize, Deserialize)]
7pub struct ModelParameters {
8 pub ambient_temperature_k: f64,
9 pub thermal_initial_k: f64,
10 pub thermal_capacity_j_per_k: f64,
11 pub thermal_soft_limit_k: f64,
12 pub thermal_limit_k: f64,
13 pub thermal_rejection_w_per_k: f64,
14 pub thermal_rejection_quadratic_w_per_k2: f64,
15 pub recharge_efficiency: f64,
16 pub continuous_power_w: f64,
17 pub pulse_energy_max_j: f64,
18 pub pulse_energy_initial_j: f64,
19 pub pulse_energy_min_j: f64,
20 pub low_energy_threshold_j: f64,
21 pub actuator_peak_power_w: f64,
22 pub actuator_idle_power_w: f64,
23 pub actuator_velocity_power_coeff_w_per_mps: f64,
24 pub actuator_position_power_coeff_w_per_m: f64,
25 pub actuator_heat_fraction: f64,
26 pub transfer_heat_fraction: f64,
27 pub loss_idle_w: f64,
28 pub loss_storage_coeff_w: f64,
29 pub loss_thermal_coeff_w_per_k: f64,
30 pub mechanical_mass_kg: f64,
31 pub damping_n_s_per_m: f64,
32 pub stiffness_n_per_m: f64,
33 pub reference_actuator_force_n: f64,
34 pub damping_temp_coeff: f64,
35 pub stiffness_temp_softening: f64,
36 pub stiffness_position_coeff: f64,
37 pub min_gain_fraction: f64,
38 pub energy_gain_soft_zone_j: f64,
39 pub thermal_gain_soft_zone_k: f64,
40 pub max_displacement_m: f64,
41 pub max_velocity_m_per_s: f64,
42 pub local_buffer_count: usize,
43 pub local_buffer_energy_max_j: f64,
44 pub local_buffer_initial_j: f64,
45 pub local_buffer_transfer_limit_w: f64,
46 pub local_buffer_low_threshold_j: f64,
47 pub local_buffer_recovery_tau_s: f64,
48 pub local_buffer_target_fraction: f64,
49 pub local_buffer_loss_w: f64,
50 pub actuator_demand_scale: f64,
51 pub damping_scale: f64,
52 pub stiffness_scale: f64,
53}
54
55impl Default for ModelParameters {
56 fn default() -> Self {
57 Self {
58 ambient_temperature_k: 293.15,
59 thermal_initial_k: 293.15,
60 thermal_capacity_j_per_k: 4.0e8,
61 thermal_soft_limit_k: 326.15,
62 thermal_limit_k: 338.15,
63 thermal_rejection_w_per_k: 4.5e6,
64 thermal_rejection_quadratic_w_per_k2: 4.0e4,
65 recharge_efficiency: 0.97,
66 continuous_power_w: 50.0e6,
67 pulse_energy_max_j: 4.0e9,
68 pulse_energy_initial_j: 4.0e9,
69 pulse_energy_min_j: 0.25e9,
70 low_energy_threshold_j: 0.60e9,
71 actuator_peak_power_w: 1.0e9,
72 actuator_idle_power_w: 5.0e6,
73 actuator_velocity_power_coeff_w_per_mps: 45.0e6,
74 actuator_position_power_coeff_w_per_m: 20.0e6,
75 actuator_heat_fraction: 0.33,
76 transfer_heat_fraction: 0.02,
77 loss_idle_w: 1.5e6,
78 loss_storage_coeff_w: 2.4e6,
79 loss_thermal_coeff_w_per_k: 0.12e6,
80 mechanical_mass_kg: 4.0e5,
81 damping_n_s_per_m: 2.4e6,
82 stiffness_n_per_m: 7.0e6,
83 reference_actuator_force_n: 5.0e6,
84 damping_temp_coeff: 1.1,
85 stiffness_temp_softening: 0.32,
86 stiffness_position_coeff: 0.08,
87 min_gain_fraction: 0.22,
88 energy_gain_soft_zone_j: 1.2e9,
89 thermal_gain_soft_zone_k: 14.0,
90 max_displacement_m: 8.0,
91 max_velocity_m_per_s: 6.5,
92 local_buffer_count: LIMB_COUNT,
93 local_buffer_energy_max_j: 180.0e6,
94 local_buffer_initial_j: 180.0e6,
95 local_buffer_transfer_limit_w: 225.0e6,
96 local_buffer_low_threshold_j: 36.0e6,
97 local_buffer_recovery_tau_s: 12.0,
98 local_buffer_target_fraction: 0.92,
99 local_buffer_loss_w: 0.25e6,
100 actuator_demand_scale: 1.0,
101 damping_scale: 1.0,
102 stiffness_scale: 1.0,
103 }
104 }
105}
106
107pub struct StepOutcome {
108 pub next_state: SystemState,
109 pub diagnostics: StepDiagnostics,
110}
111
112pub fn step_state(
113 params: &ModelParameters,
114 state: &SystemState,
115 input: &ControlInput,
116 dt_s: f64,
117) -> StepOutcome {
118 let command_fraction = (input.command_fraction * params.actuator_demand_scale).max(0.0);
119 let weights = allocation_weights(input.allocation_strategy);
120 let requested_actuator_power_w = actuator_power_draw(params, state, command_fraction);
121 let parasitic_loss_w = parasitic_loss(params, state.ep_j, state.temperature_k);
122
123 let target_buffer_j = params.local_buffer_energy_max_j * params.local_buffer_target_fraction;
124 let mut preliminary_transfer = [0.0; LIMB_COUNT];
125 let mut requested_by_limb = [0.0; LIMB_COUNT];
126 let mut recharge_by_limb = [0.0; LIMB_COUNT];
127 let mut total_transfer_request_w = 0.0;
128 let mut commanded_recharge_power_w = 0.0;
129
130 for index in 0..LIMB_COUNT {
131 requested_by_limb[index] = requested_actuator_power_w * weights[index];
132 recharge_by_limb[index] = ((target_buffer_j - state.local_buffers_j[index]).max(0.0)
133 / params.local_buffer_recovery_tau_s.max(1.0e-6))
134 .max(0.0);
135 preliminary_transfer[index] = (requested_by_limb[index] + recharge_by_limb[index])
136 .min(params.local_buffer_transfer_limit_w);
137 total_transfer_request_w += preliminary_transfer[index];
138 commanded_recharge_power_w += recharge_by_limb[index];
139 }
140
141 let available_central_energy_j = (state.ep_j
142 + params.recharge_efficiency * params.continuous_power_w * dt_s)
143 - parasitic_loss_w * dt_s;
144 let central_scale = if total_transfer_request_w > 0.0 {
145 (available_central_energy_j / (total_transfer_request_w * dt_s)).clamp(0.0, 1.0)
146 } else {
147 1.0
148 };
149
150 let mut next_local_buffers_j = [0.0; LIMB_COUNT];
151 let mut limb_flows: [LimbFlow; LIMB_COUNT] = std::array::from_fn(|index| LimbFlow {
152 limb: LIMB_NAMES[index].to_string(),
153 requested_power_w: 0.0,
154 transfer_power_w: 0.0,
155 delivered_power_w: 0.0,
156 buffer_energy_before_j: state.local_buffers_j[index],
157 buffer_energy_after_j: state.local_buffers_j[index],
158 saturation: false,
159 });
160
161 let mut central_transfer_power_w = 0.0;
162 let mut delivered_actuator_power_w = 0.0;
163 let mut saturation_count = 0.0;
164
165 for index in 0..LIMB_COUNT {
166 let transfer_power_w = preliminary_transfer[index] * central_scale;
167 let available_from_buffer_w = state.local_buffers_j[index] / dt_s.max(1.0e-9);
168 let delivered_power_w =
169 requested_by_limb[index].min(transfer_power_w + available_from_buffer_w);
170 let saturation = delivered_power_w + 1.0 < requested_by_limb[index];
171 let next_buffer = (state.local_buffers_j[index]
172 + (transfer_power_w - delivered_power_w - params.local_buffer_loss_w) * dt_s)
173 .clamp(0.0, params.local_buffer_energy_max_j);
174
175 limb_flows[index] = LimbFlow {
176 limb: LIMB_NAMES[index].to_string(),
177 requested_power_w: requested_by_limb[index],
178 transfer_power_w,
179 delivered_power_w,
180 buffer_energy_before_j: state.local_buffers_j[index],
181 buffer_energy_after_j: next_buffer,
182 saturation,
183 };
184 next_local_buffers_j[index] = next_buffer;
185 central_transfer_power_w += transfer_power_w;
186 delivered_actuator_power_w += delivered_power_w;
187 if saturation {
188 saturation_count += 1.0;
189 }
190 }
191
192 let energy_factor = energy_factor(params, state.ep_j);
193 let thermal_factor = thermal_factor(params, state.temperature_k);
194 let gain = params.reference_actuator_force_n * energy_factor * thermal_factor;
195 let damping = damping(params, state.temperature_k);
196 let stiffness = stiffness(params, state.y_m, state.temperature_k);
197 let delivered_ratio = if requested_actuator_power_w > 1.0 {
198 (delivered_actuator_power_w / requested_actuator_power_w).clamp(0.0, 1.0)
199 } else {
200 1.0
201 };
202 let mechanical_force_n = gain * command_fraction * delivered_ratio;
203 let acceleration_mps2 =
204 (mechanical_force_n + input.disturbance_n - damping * state.v_mps - stiffness * state.y_m)
205 / params.mechanical_mass_kg.max(1.0);
206 let raw_next_v_mps = state.v_mps + acceleration_mps2 * dt_s;
207 let next_v_mps =
208 raw_next_v_mps.clamp(-params.max_velocity_m_per_s, params.max_velocity_m_per_s);
209 let raw_next_y_m = state.y_m + next_v_mps * dt_s;
210 let next_y_m = raw_next_y_m.clamp(-params.max_displacement_m, params.max_displacement_m);
211 let mechanical_power_w = mechanical_force_n * next_v_mps;
212
213 let heat_generation_w = heat_generation(
214 params,
215 delivered_actuator_power_w,
216 central_transfer_power_w,
217 parasitic_loss_w,
218 );
219 let heat_rejection_w = heat_rejection(params, state.temperature_k);
220 let ep_dot_j_per_s = params.recharge_efficiency * params.continuous_power_w
221 - central_transfer_power_w
222 - parasitic_loss_w;
223 let temperature_dot_k_per_s =
224 (heat_generation_w - heat_rejection_w) / params.thermal_capacity_j_per_k.max(1.0);
225
226 let raw_next_ep_j = state.ep_j + ep_dot_j_per_s * dt_s;
227 let next_ep_j = raw_next_ep_j.clamp(0.0, params.pulse_energy_max_j);
228 let raw_next_temperature_k = state.temperature_k + temperature_dot_k_per_s * dt_s;
229 let next_temperature_k = raw_next_temperature_k.max(params.ambient_temperature_k);
230
231 let next_state = SystemState {
232 time_s: state.time_s + dt_s,
233 ep_j: next_ep_j,
234 temperature_k: next_temperature_k,
235 y_m: next_y_m,
236 v_mps: next_v_mps,
237 local_buffers_j: next_local_buffers_j,
238 };
239
240 let diagnostics = StepDiagnostics {
241 command_fraction,
242 disturbance_n: input.disturbance_n,
243 active_segment: input.active_segment.clone(),
244 allocation_strategy: input.allocation_strategy,
245 requested_actuator_power_w,
246 delivered_actuator_power_w,
247 central_transfer_power_w,
248 commanded_recharge_power_w,
249 parasitic_loss_w,
250 heat_generation_w,
251 heat_rejection_w,
252 gain,
253 damping,
254 stiffness,
255 mechanical_force_n,
256 acceleration_mps2,
257 delivered_ratio,
258 energy_factor,
259 thermal_factor,
260 saturation_fraction: saturation_count / LIMB_COUNT as f64,
261 ep_dot_j_per_s,
262 temperature_dot_k_per_s,
263 mechanical_power_w,
264 raw_next_ep_j,
265 raw_next_temperature_k,
266 raw_next_y_m,
267 raw_next_v_mps,
268 ep_clamped: (next_ep_j - raw_next_ep_j).abs() > 1.0e-9,
269 temperature_clamped: (next_temperature_k - raw_next_temperature_k).abs() > 1.0e-9,
270 y_clamped: (next_y_m - raw_next_y_m).abs() > 1.0e-9,
271 v_clamped: (next_v_mps - raw_next_v_mps).abs() > 1.0e-9,
272 limb_flows,
273 };
274
275 StepOutcome {
276 next_state,
277 diagnostics,
278 }
279}
280
281pub fn actuator_power_draw(
282 params: &ModelParameters,
283 state: &SystemState,
284 command_fraction: f64,
285) -> f64 {
286 let command_term = params.actuator_peak_power_w * command_fraction.powf(1.15);
287 let kinematic_term = params.actuator_velocity_power_coeff_w_per_mps * state.v_mps.abs()
288 + params.actuator_position_power_coeff_w_per_m * state.y_m.abs();
289 params.actuator_idle_power_w + command_term + kinematic_term
290}
291
292pub fn parasitic_loss(params: &ModelParameters, ep_j: f64, temperature_k: f64) -> f64 {
293 let soc = (ep_j / params.pulse_energy_max_j.max(1.0)).clamp(0.0, 1.5);
294 let thermal_excess = (temperature_k - params.ambient_temperature_k).max(0.0);
295 params.loss_idle_w
296 + params.loss_storage_coeff_w * soc * soc
297 + params.loss_thermal_coeff_w_per_k * thermal_excess
298}
299
300pub fn heat_generation(
301 params: &ModelParameters,
302 delivered_actuator_power_w: f64,
303 central_transfer_power_w: f64,
304 parasitic_loss_w: f64,
305) -> f64 {
306 params.actuator_heat_fraction * delivered_actuator_power_w
307 + params.transfer_heat_fraction * central_transfer_power_w
308 + parasitic_loss_w
309}
310
311pub fn heat_rejection(params: &ModelParameters, temperature_k: f64) -> f64 {
312 let delta_k = (temperature_k - params.ambient_temperature_k).max(0.0);
313 params.thermal_rejection_w_per_k * delta_k
314 + params.thermal_rejection_quadratic_w_per_k2 * delta_k * delta_k
315}
316
317pub fn damping(params: &ModelParameters, temperature_k: f64) -> f64 {
318 let temp_fraction = normalized_temperature_fraction(params, temperature_k);
319 params.damping_n_s_per_m
320 * params.damping_scale
321 * (1.0 + params.damping_temp_coeff * temp_fraction)
322}
323
324pub fn stiffness(params: &ModelParameters, y_m: f64, temperature_k: f64) -> f64 {
325 let temp_fraction = normalized_temperature_fraction(params, temperature_k);
326 let thermal_softening = (1.0 - params.stiffness_temp_softening * temp_fraction).max(0.35);
327 let geometric_hardening = 1.0 + params.stiffness_position_coeff * y_m * y_m;
328 params.stiffness_n_per_m * params.stiffness_scale * thermal_softening * geometric_hardening
329}
330
331pub fn energy_factor(params: &ModelParameters, ep_j: f64) -> f64 {
332 let normalized = ((ep_j - params.low_energy_threshold_j)
333 / params.energy_gain_soft_zone_j.max(1.0))
334 .clamp(0.0, 1.0);
335 params.min_gain_fraction + (1.0 - params.min_gain_fraction) * smoothstep01(normalized)
336}
337
338pub fn thermal_factor(params: &ModelParameters, temperature_k: f64) -> f64 {
339 let normalized = ((params.thermal_limit_k - temperature_k)
340 / params.thermal_gain_soft_zone_k.max(1.0))
341 .clamp(0.0, 1.0);
342 params.min_gain_fraction + (1.0 - params.min_gain_fraction) * smoothstep01(normalized)
343}
344
345fn normalized_temperature_fraction(params: &ModelParameters, temperature_k: f64) -> f64 {
346 let numerator = (temperature_k - params.ambient_temperature_k).max(0.0);
347 let denominator = (params.thermal_limit_k - params.ambient_temperature_k).max(1.0);
348 (numerator / denominator).clamp(0.0, 1.5)
349}
350
351fn smoothstep01(x: f64) -> f64 {
352 let x = x.clamp(0.0, 1.0);
353 x * x * (3.0 - 2.0 * x)
354}
355
356fn allocation_weights(strategy: AllocationStrategy) -> [f64; LIMB_COUNT] {
357 strategy.normalized_weights()
358}