phyz_model/
state.rs

1//! Simulation state — mutable per-timestep data.
2
3use phyz_math::{DVec, SpatialTransform};
4
5/// Mutable simulation state.
6#[derive(Debug, Clone)]
7pub struct State {
8    /// Generalized positions (joint angles/displacements).
9    pub q: DVec,
10    /// Generalized velocities.
11    pub v: DVec,
12    /// Control inputs.
13    ///
14    /// If the model has actuators, entry `i` is the raw control of actuator `i`
15    /// (clamped to its `ctrlrange` and scaled by its `gear` before reaching the
16    /// dynamics — see [`Model::actuator_forces`]). If the model has no
17    /// actuators, entries are raw per-DOF generalized forces.
18    ///
19    /// [`Model::actuator_forces`]: crate::Model::actuator_forces
20    pub ctrl: DVec,
21    /// Simulation time.
22    pub time: f64,
23
24    // Cached quantities (filled by forward kinematics / dynamics)
25    /// Body-to-world transforms for each body.
26    pub body_xform: Vec<SpatialTransform>,
27    /// External generalized forces (e.g., from contacts).
28    pub qfrc_external: DVec,
29}
30
31impl State {
32    /// Create a zero-initialized state for `nq` position DOFs and `nv` velocity DOFs.
33    pub fn new(nq: usize, nv: usize, nbodies: usize) -> Self {
34        Self::new_with_nu(nq, nv, 0, nbodies)
35    }
36
37    /// Create a zero-initialized state, sizing `ctrl` for `nu` actuators.
38    ///
39    /// `ctrl` is sized `max(nv, nu)` so that both conventions (per-DOF forces
40    /// for unactuated models, per-actuator controls otherwise) fit.
41    pub fn new_with_nu(nq: usize, nv: usize, nu: usize, nbodies: usize) -> Self {
42        Self {
43            q: DVec::zeros(nq),
44            v: DVec::zeros(nv),
45            ctrl: DVec::zeros(nv.max(nu)),
46            time: 0.0,
47            body_xform: vec![SpatialTransform::identity(); nbodies],
48            qfrc_external: DVec::zeros(nv),
49        }
50    }
51
52    /// Compute total kinetic energy given mass matrix.
53    /// KE = 0.5 * v^T * M * v
54    pub fn kinetic_energy(&self, mass_matrix: &phyz_math::DMat) -> f64 {
55        0.5 * self.v.dot(&(mass_matrix * &self.v))
56    }
57}
phyz_model/state.rs

phyz_model/
state.rs
