SolidCraft: an open-source parametric 3D CAD application built in pure Rust. Desktop, web, and automation through MCP.
Dynamics: joint springs, motors and motion links (refs #9719) (#9768)
PaddleStroke136b62fa47a4
diff --git a/crates/dynamics/src/integrate.rs b/crates/dynamics/src/integrate.rsindex f32ed3ebfb..805b627f39 100644--- a/crates/dynamics/src/integrate.rs+++ b/crates/dynamics/src/integrate.rs@@ -6,9 +6,9 @@ use crate::model::{Integrator, MAX_MAGNITUDE, MAX_STEPS, Model, Settings};use crate::system::System;use crate::{DynamicsError, Result, Sample, Stats, Trajectory};-/// Dormand–Prince 5(4) tableau (the system is autonomous, so the nodes are not needed): stage-/// weights, fifth-order weights and the difference between the fifth- and fourth-order weights-/// (the error estimate).+/// Dormand–Prince 5(4) tableau: nodes, stage weights, fifth-order weights and the difference+/// between the fifth- and fourth-order weights (the error estimate).+const DP_C: [f64; 7] = [0.0, 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0];const DP_A: [[f64; 6]; 7] = [[0.0; 6],[1.0 / 5.0, 0.0, 0.0, 0.0, 0.0, 0.0],@@ -58,17 +58,18 @@ struct Run<'a> {}impl Run<'_> {- fn deriv(&self, y: &[f64]) -> Result<Vec<f64>> {- self.sys.accelerations(y).map(|a| a.ydot)+ fn deriv(&self, t: f64, y: &[f64]) -> Result<Vec<f64>> {+ self.sys.accelerations(t, y).map(|a| a.ydot)}/// Normalise, project and check a new state.fn finish_step(&mut self, y: &mut [f64], t: f64) -> Result<()> {self.sys.normalize(y).map_err(|e| at_time(e, t))?;if self.settings.projection {- self.sys.project_positions(y, self.settings.projection_tol).map_err(|e| at_time(e, t))?;- self.sys.project_velocities(y).map_err(|e| at_time(e, t))?;+ self.sys.project_positions(t, y, self.settings.projection_tol).map_err(|e| at_time(e, t))?;+ self.sys.project_velocities(t, y).map_err(|e| at_time(e, t))?;}+ self.sys.resync(y);if !y.iter().all(|x| x.is_finite() && x.abs() < MAX_MAGNITUDE) {return Err(DynamicsError::Diverged { time: t });}@@ -82,23 +83,23 @@ impl Run<'_> {Ok(())}- fn rk4(&mut self, y: &[f64], h: f64) -> Result<Vec<f64>> {- let k1 = self.deriv(y)?;- let k2 = self.deriv(&combine(y, std::slice::from_ref(&k1), &[0.5], h))?;- let k3 = self.deriv(&combine(y, std::slice::from_ref(&k2), &[0.5], h))?;- let k4 = self.deriv(&combine(y, std::slice::from_ref(&k3), &[1.0], h))?;+ fn rk4(&mut self, t: f64, y: &[f64], h: f64) -> Result<Vec<f64>> {+ let k1 = self.deriv(t, y)?;+ let k2 = self.deriv(t + 0.5 * h, &combine(y, std::slice::from_ref(&k1), &[0.5], h))?;+ let k3 = self.deriv(t + 0.5 * h, &combine(y, std::slice::from_ref(&k2), &[0.5], h))?;+ let k4 = self.deriv(t + h, &combine(y, std::slice::from_ref(&k3), &[1.0], h))?;Ok(combine(y, &[k1, k2, k3, k4], &[1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0], h))}/// One Dormand–Prince step: the fifth-order state and the scaled error norm.- fn dopri(&mut self, y: &[f64], h: f64, rel_tol: f64, abs_tol: f64) -> Result<(Vec<f64>, f64)> {+ fn dopri(&mut self, t: f64, y: &[f64], h: f64, rel_tol: f64, abs_tol: f64) -> Result<(Vec<f64>, f64)> {let mut ks: Vec<Vec<f64>> = Vec::with_capacity(7);- for a in DP_A.iter().take(6) {+ for (a, c) in DP_A.iter().zip(DP_C).take(6) {let stage = combine(y, &ks, a, h);- ks.push(self.deriv(&stage)?);+ ks.push(self.deriv(t + c * h, &stage)?);}let y5 = combine(y, &ks, &DP_B, h);- ks.push(self.deriv(&y5)?);+ ks.push(self.deriv(t + h, &y5)?);let err = combine(&vec![0.0; y.len()], &ks, &DP_E, h);let n = y.len().max(1) as f64;let sum: f64 = err@@ -123,7 +124,7 @@ impl Run<'_> {let h = span / n;for _ in 0..n as usize {self.count_step(*t)?;- let mut next = self.rk4(y, h).map_err(|e| at_time(e, *t))?;+ let mut next = self.rk4(*t, y, h).map_err(|e| at_time(e, *t))?;*t += h;self.finish_step(&mut next, *t)?;*y = next;@@ -136,7 +137,7 @@ impl Run<'_> {self.count_step(*t)?;let remaining = t_end - *t;let h = h_next.clamp(min_step, max_step).min(remaining);- let (mut next, err) = self.dopri(y, h, rel_tol, abs_tol).map_err(|e| at_time(e, *t))?;+ let (mut next, err) = self.dopri(*t, y, h, rel_tol, abs_tol).map_err(|e| at_time(e, *t))?;// The usual controller: 0.9·err^(−1/5), within [0.2, 5].let factor = if err > 0.0 { (0.9 * err.powf(-0.2)).clamp(0.2, 5.0) } else { 5.0 };if err <= 1.0 {@@ -164,11 +165,18 @@ impl Run<'_> {}fn sample(&mut self, y: &[f64], t: f64) -> Result<Sample> {- let acc = self.sys.accelerations(y).map_err(|e| at_time(e, t))?;- let constraint_error = self.sys.constraint_error(y);+ let acc = self.sys.accelerations(t, y).map_err(|e| at_time(e, t))?;+ let constraint_error = self.sys.constraint_error(t, y);self.stats.max_constraint_error = self.stats.max_constraint_error.max(constraint_error);self.stats.redundant_equations = self.stats.redundant_equations.max(acc.row_count().saturating_sub(acc.rank));- Ok(Sample { time: t, bodies: self.sys.body_states(y), joints: self.sys.joint_results(y, &acc), energy: self.sys.energy(y), constraint_error })+ Ok(Sample {+ time: t,+ bodies: self.sys.body_states(y),+ joints: self.sys.joint_results(y, &acc),+ drives: self.sys.drive_efforts(&acc),+ energy: self.sys.energy(y),+ constraint_error,+ })}}@@ -177,11 +185,12 @@ impl Run<'_> {pub fn simulate(model: &Model, settings: &Settings) -> Result<Trajectory> {settings.validate()?;let sys = System::new(model, settings.baumgarte)?;- let mut y = System::initial_state(model)?;- // An inconsistent start (frames a little apart, velocities the joints don't allow) is- // brought onto the joints first, whether or not later steps project.- sys.project_positions(&mut y, settings.projection_tol).map_err(|e| at_time(e, 0.0))?;- sys.project_velocities(&mut y).map_err(|e| at_time(e, 0.0))?;+ let mut y = sys.initial_state(model)?;+ // An inconsistent start (frames a little apart, velocities the joints don't allow, drives+ // starting elsewhere) is brought onto the joints first, whether or not later steps project.+ sys.project_positions(0.0, &mut y, settings.projection_tol).map_err(|e| at_time(e, 0.0))?;+ sys.project_velocities(0.0, &mut y).map_err(|e| at_time(e, 0.0))?;+ sys.resync(&mut y);let mut run = Run { sys: &sys, settings, stats: Stats::default() };let count = (settings.duration / settings.output_interval).ceil() as usize;let mut samples = Vec::with_capacity(count + 1);diff --git a/crates/dynamics/src/lib.rs b/crates/dynamics/src/lib.rsindex 24f21c4d65..243becd955 100644--- a/crates/dynamics/src/lib.rs+++ b/crates/dynamics/src/lib.rs@@ -3,7 +3,9 @@//!//! The crate knows nothing of documents or occurrences: a [`Model`] lists bodies (mass,//! inertia about the centre of mass, starting pose and velocities), joints between frames on-//! them ([`JointKind`]: rigid, revolute, slider, cylindrical, planar, ball) and force elements;+//! them ([`JointKind`]: rigid, revolute, slider, cylindrical, planar, ball), force elements+//! (point and joint spring-dampers, motors along joint values, loads), drives that make a joint+//! value follow a [`Profile`] in time, and motion links (gears, racks, screws) between values;//! [`simulate`] returns a [`Trajectory`] of poses, joint values and rates, joint reactions and//! energies at a fixed output interval. The assembly layer maps occurrences and joints to it.//!@@ -31,8 +33,8 @@ use solidcore_record::Vec3;pub use integrate::simulate;pub use math::{Mat3, Quat};pub use model::{- Anchor, Body, Force, Integrator, Joint, JointKind, MAX_BODIES, MAX_CONSTRAINT_ROWS, MAX_FORCES, MAX_JOINTS, MAX_MAGNITUDE,- MAX_PROJECTION_ITERATIONS, MAX_SAMPLES, MAX_STEPS, Marker, Model, STANDARD_GRAVITY, Settings,+ Anchor, Body, Coord, Drive, Force, Integrator, Joint, JointKind, Link, MAX_BODIES, MAX_CONSTRAINT_ROWS, MAX_FORCES, MAX_JOINTS, MAX_MAGNITUDE,+ MAX_PROFILE_POINTS, MAX_PROJECTION_ITERATIONS, MAX_SAMPLES, MAX_STEPS, Marker, Model, Profile, STANDARD_GRAVITY, Settings,};/// Why a simulation could not run or stopped.@@ -90,14 +92,25 @@ pub struct Energy {pub kinetic: f64,/// Potential energy of gravity, zero at the world origin.pub gravity: f64,- /// Energy stored in springs (preload included).+ /// Energy stored in springs, point and joint ones (preload included).pub spring: f64,+ /// Work put in since the start by motors, drives and loads.+ #[serde(default)]+ pub input: f64,+ /// Energy taken out since the start by dampers.+ #[serde(default)]+ pub dissipated: f64,}impl Energy {+ /// Kinetic plus potential energy.pub fn total(&self) -> f64 {self.kinetic + self.gravity + self.spring}+ /// `total + dissipated − input`: constant along a run up to the integration error.+ pub fn balance(&self) -> f64 {+ self.total() + self.dissipated - self.input+ }}/// The system at one output time.@@ -107,8 +120,12 @@ pub struct Sample {pub time: f64,pub bodies: Vec<BodyState>,pub joints: Vec<JointResult>,+ /// What each drive applies along its joint value to make it follow (mN·mm for an angle, mN+ /// for a distance), in [`Model::drives`] order.+ #[serde(default, skip_serializing_if = "Vec::is_empty")]+ pub drives: Vec<f64>,pub energy: Energy,- /// The largest joint equation error (mm for points; directions are unitless).+ /// The largest constraint equation error (mm for points; directions are unitless).pub constraint_error: f64,}diff --git a/crates/dynamics/src/model.rs b/crates/dynamics/src/model.rsindex e4af1e1b6e..5956bf359d 100644--- a/crates/dynamics/src/model.rs+++ b/crates/dynamics/src/model.rs@@ -141,10 +141,172 @@ pub struct Joint {pub b: Marker,}-/// A force element.+/// One value of a joint: `dof` indexes [`JointKind::values`]. Revolute, slider, cylindrical+/// and planar joints have values that springs, motors, drives and links can act on; ball and+/// rigid joints have none.+#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]+pub struct Coord {+ pub joint: usize,+ #[serde(default)]+ pub dof: usize,+}++impl Coord {+ pub fn new(joint: usize, dof: usize) -> Coord {+ Coord { joint, dof }+ }+}++/// Points a table may hold at most.+pub const MAX_PROFILE_POINTS: usize = 10_000;++/// A value along time (s): a motor's torque or force, or the value a drive imposes (radians,+/// mm).+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]+#[serde(tag = "type", rename_all = "snake_case")]+pub enum Profile {+ Constant {+ value: f64,+ },+ /// `value + rate·t`.+ Linear {+ value: f64,+ rate: f64,+ },+ /// From `from` to `to` between `start` and `start + duration`, along a cycloidal step+ /// (`τ − sin(2πτ)/2π`) whose speed and acceleration both start and end at zero.+ Ramp {+ from: f64,+ to: f64,+ #[serde(default)]+ start: f64,+ duration: f64,+ },+ /// `offset + amplitude·sin(2π·frequency·t + phase)` (frequency in Hz).+ Harmonic {+ #[serde(default)]+ offset: f64,+ amplitude: f64,+ frequency: f64,+ #[serde(default)]+ phase: f64,+ },+ /// (time, value) points, times increasing: linear in between, held before the first and+ /// after the last. As a drive its speed jumps at the points (its acceleration is taken as 0).+ Table {+ points: Vec<(f64, f64)>,+ },+}++impl Profile {+ /// The value and its first and second time derivatives at `t`.+ pub fn eval(&self, t: f64) -> (f64, f64, f64) {+ match self {+ Profile::Constant { value } => (*value, 0.0, 0.0),+ Profile::Linear { value, rate } => (value + rate * t, *rate, 0.0),+ Profile::Ramp { from, to, start, duration } => {+ let tau = (t - start) / duration;+ if !(tau > 0.0) {+ return (*from, 0.0, 0.0);+ }+ if tau >= 1.0 {+ return (*to, 0.0, 0.0);+ }+ let span = to - from;+ let (s, c) = (std::f64::consts::TAU * tau).sin_cos();+ (+ from + span * (tau - s / std::f64::consts::TAU),+ span / duration * (1.0 - c),+ span / (duration * duration) * std::f64::consts::TAU * s,+ )+ }+ Profile::Harmonic { offset, amplitude, frequency, phase } => {+ let w = std::f64::consts::TAU * frequency;+ let (s, c) = (w * t + phase).sin_cos();+ (offset + amplitude * s, amplitude * w * c, -amplitude * w * w * s)+ }+ Profile::Table { points } => {+ let (Some(first), Some(last)) = (points.first(), points.last()) else { return (0.0, 0.0, 0.0) };+ if t <= first.0 {+ return (first.1, 0.0, 0.0);+ }+ if t >= last.0 {+ return (last.1, 0.0, 0.0);+ }+ points+ .windows(2)+ .find_map(|w| {+ let (a, b) = (w.first()?, w.get(1)?);+ (t >= a.0 && t < b.0).then(|| {+ let slope = (b.1 - a.1) / (b.0 - a.0);+ (a.1 + slope * (t - a.0), slope, 0.0)+ })+ })+ .unwrap_or((last.1, 0.0, 0.0))+ }+ }+ }++ fn validate(&self) -> std::result::Result<(), String> {+ let fin = |x: f64| x.is_finite() && x.abs() < MAX_MAGNITUDE;+ let ok = match self {+ Profile::Constant { value } => fin(*value),+ Profile::Linear { value, rate } => fin(*value) && fin(*rate),+ Profile::Ramp { from, to, start, duration } => fin(*from) && fin(*to) && fin(*start) && fin(*duration) && *duration > 1e-9,+ Profile::Harmonic { offset, amplitude, frequency, phase } => {+ fin(*offset) && fin(*amplitude) && fin(*frequency) && fin(*phase) && frequency.abs() < 1e6+ }+ Profile::Table { points } => {+ if points.is_empty() || points.len() > MAX_PROFILE_POINTS {+ return Err(format!("a table needs 1 to {MAX_PROFILE_POINTS} points"));+ }+ points.iter().all(|(t, v)| fin(*t) && fin(*v))+ && points.windows(2).all(|w| matches!((w.first(), w.get(1)), (Some(a), Some(b)) if b.0 > a.0))+ }+ };+ if ok { Ok(()) } else { Err("a profile's numbers must be finite (ramps need a duration, tables increasing times)".into()) }+ }+}++/// A joint value made to follow a profile along time (radians or mm): the joint turns or+/// slides as told, whatever it takes; [`crate::Sample::drives`] reports what it takes.+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]+pub struct Drive {+ pub at: Coord,+ pub motion: Profile,+}++/// A motion link: `follower = ratio·driver + offset`, between values of two joints (a gear+/// pair: angle to angle; a rack and pinion: angle to distance, the ratio being the pinion's+/// radius; a screw: angle to distance along the same cylindrical joint, the ratio being+/// pitch / 2π). Without an offset, the one that holds at the start is kept.#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]+pub struct Link {+ pub driver: Coord,+ pub follower: Coord,+ pub ratio: f64,+ #[serde(default, skip_serializing_if = "Option::is_none")]+ pub offset: Option<f64>,+}++/// A force element.+#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]#[serde(tag = "type", rename_all = "snake_case")]pub enum Force {+ /// A spring and damper on a joint value: the joint is pushed back by+ /// `stiffness·(value − rest) + damping·rate` (torsional for an angle: mN·mm per radian and+ /// mN·mm·s; linear for a distance: kg/s² and kg/s).+ JointSpring {+ at: Coord,+ stiffness: f64,+ #[serde(default)]+ damping: f64,+ #[serde(default)]+ rest: f64,+ },+ /// A motor's torque (mN·mm) or force (mN) along a joint value, following a profile: it+ /// pushes B's side forward and A's side back.+ Motor { at: Coord, effort: Profile },/// A linear spring and damper between two points: the tension is/// `stiffness·(length − free_length) + damping·(rate of length) + preload`.Spring {@@ -243,6 +405,10 @@ pub struct Model {pub joints: Vec<Joint>,#[serde(default)]pub forces: Vec<Force>,+ #[serde(default, skip_serializing_if = "Vec::is_empty")]+ pub drives: Vec<Drive>,+ #[serde(default, skip_serializing_if = "Vec::is_empty")]+ pub links: Vec<Link>,/// Gravity (mm/s²), e.g. `(0, 0, −9806.65)`.#[serde(default)]pub gravity: Vec3,@@ -282,7 +448,10 @@ impl Model {if self.forces.len() > MAX_FORCES {return Err(DynamicsError::TooLarge(format!("{} force elements (at most {MAX_FORCES})", self.forces.len())));}- let rows: usize = self.joints.iter().map(|j| j.kind.equations()).sum();+ if self.drives.len() > MAX_JOINTS || self.links.len() > MAX_JOINTS {+ return Err(DynamicsError::TooLarge(format!("drives and links: at most {MAX_JOINTS} each")));+ }+ let rows: usize = self.joints.iter().map(|j| j.kind.equations()).sum::<usize>() + self.drives.len() + self.links.len();if rows > MAX_CONSTRAINT_ROWS {return Err(DynamicsError::TooLarge(format!("{rows} constraint equations (at most {MAX_CONSTRAINT_ROWS})")));}@@ -339,7 +508,45 @@ impl Model {return Err(bad(format!("force {i}: point, force and torque must be finite")));}}+ Force::JointSpring { at, stiffness, damping, rest } => {+ self.check_coord(*at).map_err(|e| bad(format!("force {i}: {e}")))?;+ let ok = |x: f64| (0.0..MAX_MAGNITUDE).contains(&x);+ if !ok(*stiffness) || !ok(*damping) || !rest.is_finite() || rest.abs() >= MAX_MAGNITUDE {+ return Err(bad(format!("force {i}: stiffness and damping must be ≥ 0 and finite, rest finite")));+ }+ }+ Force::Motor { at, effort } => {+ self.check_coord(*at).map_err(|e| bad(format!("force {i}: {e}")))?;+ effort.validate().map_err(|e| bad(format!("force {i}: {e}")))?;+ }+ }+ }+ for (i, d) in self.drives.iter().enumerate() {+ self.check_coord(d.at).map_err(|e| bad(format!("drive {i}: {e}")))?;+ d.motion.validate().map_err(|e| bad(format!("drive {i}: {e}")))?;+ }+ for (i, l) in self.links.iter().enumerate() {+ self.check_coord(l.driver).map_err(|e| bad(format!("link {i}: {e}")))?;+ self.check_coord(l.follower).map_err(|e| bad(format!("link {i}: {e}")))?;+ if l.driver == l.follower {+ return Err(bad(format!("link {i}: a value can't drive itself")));}+ let fin = |x: f64| x.is_finite() && x.abs() < MAX_MAGNITUDE;+ if !fin(l.ratio) || !l.offset.is_none_or(fin) {+ return Err(bad(format!("link {i}: ratio and offset must be finite")));+ }+ }+ Ok(())+ }++ /// The joint and value a [`Coord`] names, if they exist and can be acted on.+ fn check_coord(&self, c: Coord) -> std::result::Result<(), String> {+ let j = self.joints.get(c.joint).ok_or_else(|| format!("no joint {}", c.joint))?;+ if matches!(j.kind, JointKind::Ball | JointKind::Rigid) {+ return Err(format!("joint {}: not supported yet: springs, motors, drives and links on {:?} joints", c.joint, j.kind));+ }+ if c.dof >= j.kind.values().len() {+ return Err(format!("joint {} has no value {}", c.joint, c.dof));}Ok(())}diff --git a/crates/dynamics/src/system.rs b/crates/dynamics/src/system.rsindex b118242659..ea0af22902 100644--- a/crates/dynamics/src/system.rs+++ b/crates/dynamics/src/system.rs@@ -12,13 +12,20 @@//!//! through the Schur complement `(J M⁻¹ Jᵀ) λ = J M⁻¹ F − γ` (`linalg::solve_psd`, which takes//! repeated joint equations), and `λ` gives the joints' reactions.+//!+//! Joint springs and motors act along joint values (generalized forces `Jᵀ Q` along a value's+//! row); drives add a rheonomic equation `value − f(t) = 0` (so `J u = ḟ`, `J u̇ = f̈ − J̇ u`),+//! and motion links a linear one between two values. The values they use are also kept in the+//! state, so an angle counts whole turns.++use std::f64::consts::TAU;use solidcore_record::Vec3;use solidcraft_geom::prelude::*;use crate::linalg::{Sym, solve_psd};use crate::math::{Mat3, Quat, cross2};-use crate::model::{Force, JointKind, MAX_MAGNITUDE, MAX_PROJECTION_ITERATIONS, Model, marker_axes};+use crate::model::{Coord, Force, JointKind, MAX_MAGNITUDE, MAX_PROJECTION_ITERATIONS, Model, Profile, marker_axes};use crate::{BodyState, DynamicsError, Energy, JointResult, Result};/// Numbers per body in the flat state: x (3), q (4), v (3), ω (3).@@ -97,18 +104,32 @@ struct MarkerWorld {/// A body's part of one Jacobian row: (linear, angular).type Block = (usize, Vec3, Vec3);-/// One constraint equation at a state.+/// One constraint equation at a state: `Φ = 0`, `J u = rate_target`.pub(crate) struct Row {+ /// A's and B's parts (for a joint's own equations, its two sides).a: Option<Block>,b: Option<Block>,+ /// A motion link's driver's parts.+ c: Option<Block>,+ d: Option<Block>,pub(crate) phi: f64,- /// `J̇ u`: the equation's second derivative at zero accelerations.+ /// `J̇ u` less the imposed acceleration: the equation's second derivative at zero+ /// accelerations.jdot_u: f64,+ /// The rate a drive imposes (`ḟ`); 0 for the others.+ rate_target: f64,+}++fn scaled(b: Option<Block>, k: f64) -> Option<Block> {+ b.map(|(i, l, a)| (i, l * k, a * k))}impl Row {+ fn new(a: Option<Block>, b: Option<Block>, phi: f64, jdot_u: f64) -> Row {+ Row { a, b, c: None, d: None, phi, jdot_u, rate_target: 0.0 }+ }fn blocks(&self) -> impl Iterator<Item = &Block> {- self.a.iter().chain(self.b.iter())+ self.a.iter().chain(self.b.iter()).chain(self.c.iter()).chain(self.d.iter())}/// `J u`.fn rate(&self, kin: &[Kin]) -> f64 {@@ -116,13 +137,58 @@ impl Row {}}+/// How a joint value is measured.+#[derive(Clone, Copy, PartialEq)]+enum CoordKind {+ /// B's turn about A's z (from A's x towards A's y).+ Angle,+ /// B's origin along one of A's axes (0, 1, 2 = x, y, z).+ Along(usize),+}++/// A joint value springs, motors, drives or links act on. Its value is kept in the state too,+/// so an angle counts whole turns.+#[derive(Clone, Copy)]+struct CoordConst {+ joint: usize,+ kind: CoordKind,+}++struct LinkConst {+ driver: usize,+ follower: usize,+ ratio: f64,+ offset: f64,+}+/// A model ready to integrate.+///+/// The flat state holds every body (`STRIDE` numbers each), then the tracked joint values,+/// then the work put in (motors, drives, loads) and the energy dissipated (dampers).pub(crate) struct System {bodies: Vec<BodyConst>,joints: Vec<JointConst>,+ /// Point springs and loads.forces: Vec<Force>,gravity: Vec3,baumgarte: Option<(f64, f64)>,+ coords: Vec<CoordConst>,+ /// (value, stiffness, damping, rest).+ springs: Vec<(usize, f64, f64, f64)>,+ /// (value, effort).+ motors: Vec<(usize, Profile)>,+ /// (value, motion).+ drives: Vec<(usize, Profile)>,+ links: Vec<LinkConst>,+}++/// `θ` turned by whole turns to lie nearest `near`.+fn unwrap_angle(theta: f64, near: f64) -> f64 {+ if !near.is_finite() {+ return theta;+ }+ let turns = ((near - theta) / TAU).round();+ if turns.is_finite() { theta + TAU * turns } else { theta }}/// Accelerations and multipliers at one state.@@ -164,12 +230,81 @@ impl System {.iter().map(|j| Ok(JointConst { kind: j.kind, a: marker_const(&j.a)?, b: marker_const(&j.b)? })).collect::<Result<Vec<_>>>()?;- Ok(System { bodies, joints, forces: model.forces.clone(), gravity: model.gravity, baumgarte })+ let mut sys = System {+ bodies,+ joints,+ forces: model.forces.iter().filter(|f| matches!(f, Force::Spring { .. } | Force::Load { .. })).cloned().collect(),+ gravity: model.gravity,+ baumgarte,+ coords: Vec::new(),+ springs: Vec::new(),+ motors: Vec::new(),+ drives: Vec::new(),+ links: Vec::new(),+ };+ let mut found: Vec<Coord> = Vec::new();+ let mut index = |c: Coord, sys: &mut System| -> Result<usize> {+ if let Some(i) = found.iter().position(|f| *f == c) {+ return Ok(i);+ }+ let kind = model.joints.get(c.joint).map(|j| j.kind);+ let kind = match (kind, c.dof) {+ (Some(JointKind::Revolute | JointKind::Cylindrical), 0) | (Some(JointKind::Planar), 2) => CoordKind::Angle,+ (Some(JointKind::Slider), 0) | (Some(JointKind::Cylindrical), 1) => CoordKind::Along(2),+ (Some(JointKind::Planar), d @ (0 | 1)) => CoordKind::Along(d),+ _ => return Err(DynamicsError::InvalidModel(format!("joint {} has no value {} to act on", c.joint, c.dof))),+ };+ found.push(c);+ sys.coords.push(CoordConst { joint: c.joint, kind });+ Ok(found.len() - 1)+ };+ for f in &model.forces {+ match f {+ Force::JointSpring { at, stiffness, damping, rest } => {+ let i = index(*at, &mut sys)?;+ sys.springs.push((i, *stiffness, *damping, *rest));+ }+ Force::Motor { at, effort } => {+ let i = index(*at, &mut sys)?;+ sys.motors.push((i, effort.clone()));+ }+ Force::Spring { .. } | Force::Load { .. } => {}+ }+ }+ for d in &model.drives {+ let i = index(d.at, &mut sys)?;+ sys.drives.push((i, d.motion.clone()));+ }+ let mut pending = Vec::new();+ for l in &model.links {+ pending.push((index(l.driver, &mut sys)?, index(l.follower, &mut sys)?, l.ratio, l.offset));+ }+ // A link without an offset keeps the one that holds at the start.+ let y = sys.initial_state(model)?;+ let kin = sys.kinematics(&y);+ let value = |i: usize| sys.coord_row(&kin, &y, i).phi;+ let links = pending+ .into_iter()+ .map(|(d, f, ratio, offset)| LinkConst { driver: d, follower: f, ratio, offset: offset.unwrap_or_else(|| value(f) - ratio * value(d)) })+ .collect();+ sys.links = links;+ Ok(sys)+ }++ fn coord_base(&self) -> usize {+ self.bodies.len() * STRIDE+ }+ fn work_base(&self) -> usize {+ self.coord_base() + self.coords.len()+ }+ /// Numbers in the flat state.+ pub(crate) fn state_len(&self) -> usize {+ self.work_base() + 2}/// The flat state of the model's starting conditions.- pub(crate) fn initial_state(model: &Model) -> Result<Vec<f64>> {- let mut y = vec![0.0; model.bodies.len() * STRIDE];+ pub(crate) fn initial_state(&self, model: &Model) -> Result<Vec<f64>> {+ let mut y = vec![0.0; self.state_len()];for (i, b) in model.bodies.iter().enumerate() {let at = i * STRIDE;let q = b.orientation.normalized().ok_or_else(|| DynamicsError::InvalidModel(format!("body {i}: orientation is not a rotation")))?;@@ -178,9 +313,48 @@ impl System {put3(&mut y, at + 7, b.velocity);put3(&mut y, at + 10, b.angular_velocity);}+ // A driven angle counts its turns from where its motion starts.+ for (i, motion) in &self.drives {+ if let Some(s) = y.get_mut(self.coord_base() + i) {+ *s = motion.eval(0.0).0;+ }+ }+ self.resync(&mut y);Ok(y)}+ /// Sets each tracked joint value to the one the bodies show (an angle: the turn nearest+ /// the tracked one).+ pub(crate) fn resync(&self, y: &mut [f64]) {+ let kin = self.kinematics(y);+ let values: Vec<f64> = (0..self.coords.len()).map(|i| self.coord_row(&kin, y, i).phi).collect();+ for (i, v) in values.into_iter().enumerate() {+ if let Some(s) = y.get_mut(self.coord_base() + i) {+ *s = v;+ }+ }+ }++ /// A tracked joint value's equation row: `Φ` is the value itself, `J u` its rate.+ fn coord_row(&self, kin: &[Kin], y: &[f64], i: usize) -> Row {+ let (Some(c), tracked) = (self.coords.get(i), y.get(self.coord_base() + i).copied().unwrap_or(f64::NAN)) else {+ return Row::new(None, None, 0.0, 0.0);+ };+ let Some(j) = self.joints.get(c.joint) else { return Row::new(None, None, 0.0, 0.0) };+ let a = Self::marker_world(kin, &j.a);+ let b = Self::marker_world(kin, &j.b);+ let [ax, ay, az] = a.axes;+ match c.kind {+ CoordKind::Angle => {+ let bx = b.axes[0];+ let theta = unwrap_angle(bx.dot(ay).atan2(bx.dot(ax)), tracked);+ // θ̇ = (ω_B − ω_A)·z_A; at zero accelerations its rate is (ω_B − ω_A)·(ω_A × z_A).+ Row::new(a.body.map(|k| (k, Vec3::ZERO, -az)), b.body.map(|k| (k, Vec3::ZERO, az)), theta, (b.w - a.w).dot(a.w.cross(az)))+ }+ CoordKind::Along(k) => dot2_row(&a, a.axes.get(k).copied().unwrap_or(az), &b),+ }+ }+/// Unit quaternions again after a step.pub(crate) fn normalize(&self, y: &mut [f64]) -> Result<()> {for i in 0..self.bodies.len() {@@ -210,17 +384,42 @@ impl System {MarkerWorld { body: m.body, r, p: k.x + r, pdot: k.v + k.w.cross(r), axes, w: k.w }}- /// The constraint equations of every joint at `kin`, in joint order.- fn rows(&self, kin: &[Kin]) -> Vec<Row> {+ /// The constraint equations at time `t`: every joint's in joint order, then one per drive+ /// (`value − f(t) = 0`), then one per link (`follower − ratio·driver − offset = 0`).+ fn rows(&self, kin: &[Kin], t: f64, y: &[f64]) -> Vec<Row> {let mut rows = Vec::new();for j in &self.joints {let a = Self::marker_world(kin, &j.a);let b = Self::marker_world(kin, &j.b);joint_rows(j.kind, &a, &b, &mut rows);}+ for (i, motion) in &self.drives {+ let (f, fd, fdd) = motion.eval(t);+ let mut r = self.coord_row(kin, y, *i);+ r.phi -= f;+ r.jdot_u -= fdd;+ r.rate_target = fd;+ rows.push(r);+ }+ for l in &self.links {+ let (f, d) = (self.coord_row(kin, y, l.follower), self.coord_row(kin, y, l.driver));+ rows.push(Row {+ a: f.a,+ b: f.b,+ c: scaled(d.a, -l.ratio),+ d: scaled(d.b, -l.ratio),+ phi: f.phi - l.ratio * d.phi - l.offset,+ jdot_u: f.jdot_u - l.ratio * d.jdot_u,+ rate_target: 0.0,+ });+ }rows}+ fn joint_row_count(&self) -> usize {+ self.joints.iter().map(|j| j.kind.equations()).sum()+ }+/// `M⁻¹` applied to one body's (force, torque).fn minv(&self, kin: &[Kin], body: usize, f: Vec3, t: Vec3) -> (Vec3, Vec3) {match (self.bodies.get(body), kin.get(body)) {@@ -264,8 +463,38 @@ impl System {(s, w)}- /// Applied forces and torques (about each centre of mass, world), gyroscopic terms included.- fn applied(&self, kin: &[Kin]) -> Vec<(Vec3, Vec3)> {+ /// Applied forces and torques (about each centre of mass, world), gyroscopic terms included,+ /// and the power they put in and the power dampers take out.+ fn applied(&self, kin: &[Kin], t: f64, y: &[f64]) -> (Vec<(Vec3, Vec3)>, f64, f64) {+ let mut input = 0.0;+ let mut dissipated = 0.0;+ let mut out = self.point_forces(kin, &mut input, &mut dissipated);+ // A generalized force Q along a joint value is Jᵀ Q on the bodies.+ let mut along = |row: &Row, q: f64| {+ for (i, l, a) in row.blocks() {+ if let Some(slot) = out.get_mut(*i) {+ slot.0 += *l * q;+ slot.1 += *a * q;+ }+ }+ };+ for (i, k, c, rest) in &self.springs {+ let row = self.coord_row(kin, y, *i);+ let rate = row.rate(kin);+ along(&row, -k * (row.phi - rest) - c * rate);+ dissipated += c * rate * rate;+ }+ for (i, effort) in &self.motors {+ let row = self.coord_row(kin, y, *i);+ let q = effort.eval(t).0;+ along(&row, q);+ input += q * row.rate(kin);+ }+ (out, input, dissipated)+ }++ /// Gravity, gyroscopic terms, point springs and loads.+ fn point_forces(&self, kin: &[Kin], input: &mut f64, dissipated: &mut f64) -> Vec<(Vec3, Vec3)> {let mut out: Vec<(Vec3, Vec3)> = self.bodies.iter()@@ -293,21 +522,25 @@ impl System {let tension = stiffness * (len - free_length) + damping * rate + preload;add(a.body, ra, n * tension, Vec3::ZERO);add(b.body, rb, n * -tension, Vec3::ZERO);+ *dissipated += damping * rate * rate;}Force::Load { body, point, force, torque } => {- let r = kin.get(body).map_or(Vec3::ZERO, |k| k.rot.mul_vec(point));+ let k = kin.get(body).copied().unwrap_or(GROUND);+ let r = k.rot.mul_vec(point);add(Some(body), r, force, torque);+ *input += force.dot(k.v + k.w.cross(r)) + torque.dot(k.w);}+ Force::JointSpring { .. } | Force::Motor { .. } => {}}}out}/// The state's derivative and the joints' multipliers.- pub(crate) fn accelerations(&self, y: &[f64]) -> Result<Accel> {+ pub(crate) fn accelerations(&self, t: f64, y: &[f64]) -> Result<Accel> {let kin = self.kinematics(y);- let applied = self.applied(&kin);- let rows = self.rows(&kin);+ let (applied, mut input, dissipated) = self.applied(&kin, t, y);+ let rows = self.rows(&kin, t, y);let (s, w) = self.schur(&kin, &rows);// Unconstrained accelerations M⁻¹F.let free: Vec<(Vec3, Vec3)> = applied.iter().enumerate().map(|(i, (f, t))| self.minv(&kin, i, *f, *t)).collect();@@ -315,7 +548,7 @@ impl System {.iter().map(|r| {let jmf: f64 = r.blocks().map(|(i, l, a)| free.get(*i).map_or(0.0, |(fl, fa)| l.dot(*fl) + a.dot(*fa))).sum();- let stab = self.baumgarte.map_or(0.0, |(alpha, beta)| 2.0 * alpha * r.rate(&kin) + beta * beta * r.phi);+ let stab = self.baumgarte.map_or(0.0, |(alpha, beta)| 2.0 * alpha * (r.rate(&kin) - r.rate_target) + beta * beta * r.phi);jmf + r.jdot_u + stab}).collect();@@ -338,25 +571,50 @@ impl System {put3(&mut ydot, at + 7, a.0);put3(&mut ydot, at + 10, a.1);}+ for i in 0..self.coords.len() {+ let rate = self.coord_row(&kin, y, i).rate(&kin);+ if let Some(s) = ydot.get_mut(self.coord_base() + i) {+ *s = rate;+ }+ }+ // A drive's generalized force is −λ along its value: its power is −λ·(value rate).+ let first_drive = self.joint_row_count();+ for k in 0..self.drives.len() {+ if let (Some(r), Some(l)) = (rows.get(first_drive + k), lambda.get(first_drive + k)) {+ input -= l * r.rate(&kin);+ }+ }+ let wb = self.work_base();+ for (k, p) in [input, dissipated].into_iter().enumerate() {+ if let Some(s) = ydot.get_mut(wb + k) {+ *s = p;+ }+ }if !ydot.iter().all(|x| x.is_finite()) {return Err(DynamicsError::Diverged { time: f64::NAN });}Ok(Accel { ydot, lambda, rows, rank })}- /// The largest joint equation error at `y`.- pub(crate) fn constraint_error(&self, y: &[f64]) -> f64 {- self.rows(&self.kinematics(y)).iter().map(|r| r.phi.abs()).fold(0.0, f64::max)+ /// What each drive applies along its value (mN·mm or mN), from the multipliers.+ pub(crate) fn drive_efforts(&self, acc: &Accel) -> Vec<f64> {+ let first = self.joint_row_count();+ (0..self.drives.len()).map(|k| -acc.lambda.get(first + k).copied().unwrap_or(0.0)).collect()+ }++ /// The largest constraint equation error at `y` and time `t`.+ pub(crate) fn constraint_error(&self, t: f64, y: &[f64]) -> f64 {+ self.rows(&self.kinematics(y), t, y).iter().map(|r| r.phi.abs()).fold(0.0, f64::max)}/// Moves the bodies the least (mass-weighted) onto the joints: Gauss–Newton on `Φ = 0`./// Returns the error left.- pub(crate) fn project_positions(&self, y: &mut [f64], tol: f64) -> Result<f64> {+ pub(crate) fn project_positions(&self, t: f64, y: &mut [f64], tol: f64) -> Result<f64> {let mut err;let mut iteration = 0;loop {let kin = self.kinematics(y);- let rows = self.rows(&kin);+ let rows = self.rows(&kin, t, y);err = rows.iter().map(|r| r.phi.abs()).fold(0.0, f64::max);if !(err > tol) || iteration >= MAX_PROJECTION_ITERATIONS {break;@@ -387,15 +645,16 @@ impl System {Ok(err)}- /// Removes the velocities the joints don't allow (mass-weighted): `J u = 0`.- pub(crate) fn project_velocities(&self, y: &mut [f64]) -> Result<()> {+ /// Removes the velocities the joints don't allow (mass-weighted): `J u = ḟ` (0 but for+ /// drives).+ pub(crate) fn project_velocities(&self, t: f64, y: &mut [f64]) -> Result<()> {let kin = self.kinematics(y);- let rows = self.rows(&kin);+ let rows = self.rows(&kin, t, y);if rows.is_empty() {return Ok(());}let (s, w) = self.schur(&kin, &rows);- let ju: Vec<f64> = rows.iter().map(|r| r.rate(&kin)).collect();+ let ju: Vec<f64> = rows.iter().map(|r| r.rate(&kin) - r.rate_target).collect();let (mu, _) = solve_psd(s, &ju, RANK_TOL).ok_or(DynamicsError::Singular { time: f64::NAN })?;for (wr, m) in w.iter().zip(&mu) {for (i, ml, ma) in wr {@@ -417,7 +676,7 @@ impl System {.collect()}- /// Kinetic, gravity and spring energy.+ /// Kinetic, gravity and spring energy, and the work put in and dissipated so far.pub(crate) fn energy(&self, y: &[f64]) -> Energy {let kin = self.kinematics(y);let mut e = Energy::default();@@ -433,6 +692,13 @@ impl System {e.spring += 0.5 * stiffness * stretch * stretch + preload * stretch;}}+ for (i, k, _, rest) in &self.springs {+ let stretch = self.coord_row(&kin, y, *i).phi - rest;+ e.spring += 0.5 * k * stretch * stretch;+ }+ let wb = self.work_base();+ e.input = y.get(wb).copied().unwrap_or(0.0);+ e.dissipated = y.get(wb + 1).copied().unwrap_or(0.0);e}@@ -477,19 +743,19 @@ impl System {/// `(Φ, J, J̇u)` for one body point's world component along `e`: `e·(p_B − p_A)`.fn point_row(a: &MarkerWorld, b: &MarkerWorld, e: Vec3) -> Row {let acc = |m: &MarkerWorld| cross2(m.w, m.r);- Row { a: a.body.map(|i| (i, -e, -a.r.cross(e))), b: b.body.map(|i| (i, e, b.r.cross(e))), phi: e.dot(b.p - a.p), jdot_u: e.dot(acc(b) - acc(a)) }+ Row::new(a.body.map(|i| (i, -e, -a.r.cross(e))), b.body.map(|i| (i, e, b.r.cross(e))), e.dot(b.p - a.p), e.dot(acc(b) - acc(a)))}/// `u·v = 0` for a direction `u` of A and `v` of B.fn dot1_row(a: &MarkerWorld, u: Vec3, b: &MarkerWorld, v: Vec3) -> Row {let (ud, vd) = (a.w.cross(u), b.w.cross(v));let (udd, vdd) = (cross2(a.w, u), cross2(b.w, v));- Row {- a: a.body.map(|i| (i, Vec3::ZERO, u.cross(v))),- b: b.body.map(|i| (i, Vec3::ZERO, v.cross(u))),- phi: u.dot(v),- jdot_u: udd.dot(v) + 2.0 * ud.dot(vd) + u.dot(vdd),- }+ Row::new(+ a.body.map(|i| (i, Vec3::ZERO, u.cross(v))),+ b.body.map(|i| (i, Vec3::ZERO, v.cross(u))),+ u.dot(v),+ udd.dot(v) + 2.0 * ud.dot(vd) + u.dot(vdd),+ )}/// `d·u = 0` for `d = p_B − p_A` and a direction `u` of A.@@ -498,12 +764,12 @@ fn dot2_row(a: &MarkerWorld, u: Vec3, b: &MarkerWorld) -> Row {let dd = b.pdot - a.pdot;let ddd = cross2(b.w, b.r) - cross2(a.w, a.r);let (ud, udd) = (a.w.cross(u), cross2(a.w, u));- Row {- a: a.body.map(|i| (i, -u, u.cross(d) - a.r.cross(u))),- b: b.body.map(|i| (i, u, b.r.cross(u))),- phi: d.dot(u),- jdot_u: ddd.dot(u) + 2.0 * dd.dot(ud) + d.dot(udd),- }+ Row::new(+ a.body.map(|i| (i, -u, u.cross(d) - a.r.cross(u))),+ b.body.map(|i| (i, u, b.r.cross(u))),+ d.dot(u),+ ddd.dot(u) + 2.0 * dd.dot(ud) + d.dot(udd),+ )}fn joint_rows(kind: JointKind, a: &MarkerWorld, b: &MarkerWorld, rows: &mut Vec<Row>) {@@ -576,7 +842,7 @@ pub(crate) mod testing {/// `(Φ, J u, J̇ u)` of every row at `y`.pub(crate) fn rows_at(sys: &System, y: &[f64]) -> Vec<(f64, f64, f64)> {let kin = sys.kinematics(y);- sys.rows(&kin).iter().map(|r| (r.phi, r.rate(&kin), r.jdot_u)).collect()+ sys.rows(&kin, 0.0, y).iter().map(|r| (r.phi, r.rate(&kin), r.jdot_u)).collect()}/// The state moved along its velocities for `h` (positions and orientations only).diff --git a/crates/dynamics/src/tests.rs b/crates/dynamics/src/tests.rsindex 877568fd8e..8a78825d70 100644--- a/crates/dynamics/src/tests.rs+++ b/crates/dynamics/src/tests.rs@@ -38,7 +38,7 @@ fn pendulum(len: f64, theta: f64, omega: f64, inertia: f64) -> Model {a: Marker::ground(Vec3::ZERO, Vec3::Y, Vec3::X),b: Marker::new(Some(0), Vec3::new(0.0, 0.0, len), Vec3::Y, Vec3::X),};- Model { bodies: vec![body], joints: vec![joint], forces: vec![], gravity: down() }+ Model { bodies: vec![body], joints: vec![joint], forces: vec![], drives: vec![], links: vec![], gravity: down() }}/// Times where `f` crosses zero upwards, linearly interpolated between samples.@@ -68,7 +68,7 @@ fn free_fall_follows_the_parabola() {let mut body = Body::at_rest(2.5, Vec3::new(10.0, 20.0, 30.0), Vec3::new(1.0, 2.0, 3.0));body.velocity = Vec3::new(10.0, -5.0, 50.0);body.angular_velocity = Vec3::new(0.0, 0.0, 1.0);- let model = Model { bodies: vec![body.clone()], joints: vec![], forces: vec![], gravity: down() };+ let model = Model { bodies: vec![body.clone()], joints: vec![], forces: vec![], drives: vec![], links: vec![], gravity: down() };let traj = ok(simulate(&model, &Settings::rk4(1.0, 0.01, 0.1)));assert_eq!(traj.samples.len(), 11);for s in &traj.samples {@@ -158,6 +158,7 @@ fn double_pendulum() -> Model {],forces: vec![],gravity: down(),+ ..Model::default()}}@@ -215,6 +216,7 @@ fn slider_crank_follows_its_closed_form() {],forces: vec![],gravity: Vec3::ZERO,+ ..Model::default()};let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));// A planar linkage built from 3D joints repeats three equations; they are solved, not refused.@@ -256,6 +258,7 @@ fn spring_mass(damping: f64) -> Model {preload: 0.0,}],gravity: down(),+ ..Model::default()}}@@ -294,7 +297,7 @@ fn torque_free_body_keeps_energy_and_momentum() {let mut body = Body::at_rest(1.0, Vec3::new(1000.0, 2000.0, 3000.0), Vec3::ZERO);body.orientation = Quat::from_axis_angle(Vec3::new(1.0, 2.0, 3.0), 0.7);body.angular_velocity = Vec3::new(1.0, 0.3, 5.0);- let model = Model { bodies: vec![body], joints: vec![], forces: vec![], gravity: Vec3::ZERO };+ let model = Model { bodies: vec![body], joints: vec![], forces: vec![], drives: vec![], links: vec![], gravity: Vec3::ZERO };let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-4, 1e-2)));let momentum = |s: &Sample| {let b = s.bodies.first().copied();@@ -329,6 +332,7 @@ fn spherical_pendulum_keeps_energy_and_vertical_momentum() {}],forces: vec![],gravity: down(),+ ..Model::default()};let traj = ok(simulate(&model, &Settings::rk4(3.0, 5e-4, 1e-2)));let lz = |s: &Sample| {@@ -369,9 +373,10 @@ fn jacobians_match_finite_differences() {a: Marker::new(a_body, Vec3::new(4.0, -3.0, 7.0), Vec3::new(0.2, 0.5, 1.0), Vec3::new(1.0, 0.0, 0.3)),b: Marker::new(Some(1), Vec3::new(-6.0, 2.0, 1.0), Vec3::new(-0.4, 1.0, 0.1), Vec3::new(0.0, 0.3, 1.0)),};- let model = Model { bodies: vec![a.clone(), b.clone()], joints: vec![joint], forces: vec![], gravity: Vec3::ZERO };+ let model =+ Model { bodies: vec![a.clone(), b.clone()], joints: vec![joint], forces: vec![], drives: vec![], links: vec![], gravity: Vec3::ZERO };let sys = ok(System::new(&model, None));- let y = ok(System::initial_state(&model));+ let y = ok(sys.initial_state(&model));let h = 1e-4;let (now, plus, minus) = (testing::rows_at(&sys, &y),@@ -400,6 +405,7 @@ fn joint_values_read_the_relative_motion() {joints: vec![Joint { kind, a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X), b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X) }],forces: vec![],gravity: Vec3::ZERO,+ ..Model::default()};let traj = ok(simulate(&model, &Settings::rk4(0.01, 0.01, 0.01)));traj.samples.first().and_then(|s| s.joints.first()).map(|j| j.values.clone()).unwrap_or_default()@@ -523,6 +529,10 @@ fn hostile_input_is_an_error_not_a_panic() {fn models_round_trip_through_json() {let mut model = spring_mass(1.5);model.forces.push(Force::Load { body: 0, point: Vec3::new(1.0, 0.0, 0.0), force: Vec3::new(0.0, 5.0, 0.0), torque: Vec3::ZERO });+ model.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: 3.0, damping: 0.5, rest: 1.0 });+ model.forces.push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Table { points: vec![(0.0, 1.0), (2.0, 3.0)] } });+ model.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Ramp { from: 0.0, to: 1.0, start: 0.5, duration: 2.0 } });+ model.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 2.0, offset: Some(0.5) });let text = match serde_json::to_string(&model) {Ok(t) => t,Err(e) => panic!("{e}"),@@ -538,3 +548,253 @@ fn models_round_trip_through_json() {};assert!(settings.projection && settings.validate().is_ok());}++// Joint springs, motors, drives and motion links.++/// `a − b` turned into (−π, π].+fn wrap(x: f64) -> f64 {+ (x + PI).rem_euclid(TAU) - PI+}++/// A wheel turning about Z on a revolute to the ground at `at`, its centre of mass on the axis.+/// Wheels turning about Z on revolutes to the ground, centres of mass on their axes (inertia,+/// centre); joint `i` holds body `i`.+fn rotors(parts: &[(f64, Vec3)]) -> Model {+ let mut model = Model::default();+ for (i, (inertia, at)) in parts.iter().enumerate() {+ model.bodies.push(Body::at_rest(1.0, Vec3::new(*inertia, *inertia, *inertia), *at));+ model.joints.push(Joint {+ kind: JointKind::Revolute,+ a: Marker::ground(*at, Vec3::Z, Vec3::X),+ b: Marker::new(Some(i), Vec3::ZERO, Vec3::Z, Vec3::X),+ });+ }+ model+}++fn value(s: &Sample, joint: usize, dof: usize) -> f64 {+ s.joints.get(joint).and_then(|j| j.values.get(dof)).copied().unwrap_or(f64::NAN)+}++#[test]+fn profiles_give_consistent_derivatives() {+ let profiles = [+ Profile::Constant { value: 2.0 },+ Profile::Linear { value: 1.0, rate: -3.0 },+ Profile::Ramp { from: 0.5, to: 2.0, start: 0.2, duration: 0.6 },+ Profile::Harmonic { offset: 1.0, amplitude: 0.3, frequency: 2.0, phase: 0.4 },+ ];+ let h = 1e-5;+ for p in &profiles {+ for k in 0..100 {+ let t = k as f64 * 0.0123;+ let (f, fd, fdd) = p.eval(t);+ let (fp, fdp, _) = p.eval(t + h);+ let (fm, fdm, _) = p.eval(t - h);+ assert!((fd - (fp - fm) / (2.0 * h)).abs() < 1e-5 * (1.0 + fd.abs()), "{p:?} at {t}: ḟ");+ assert!((fdd - (fdp - fdm) / (2.0 * h)).abs() < 1e-3 * (1.0 + fdd.abs()), "{p:?} at {t}: f̈");+ assert!(f.is_finite());+ }+ }+ let ramp = Profile::Ramp { from: 0.5, to: 2.0, start: 0.2, duration: 0.6 };+ assert_eq!(ramp.eval(0.0).0, 0.5);+ assert_eq!(ramp.eval(0.8).0, 2.0);+ assert!((ramp.eval(0.5).0 - 1.25).abs() < 1e-12);+ let table = Profile::Table { points: vec![(0.0, 1.0), (1.0, 3.0), (2.0, 0.0)] };+ assert_eq!(table.eval(-1.0), (1.0, 0.0, 0.0));+ assert_eq!(table.eval(0.5), (2.0, 2.0, 0.0));+ assert_eq!(table.eval(1.5), (1.5, -3.0, 0.0));+ assert_eq!(table.eval(9.0), (0.0, 0.0, 0.0));+}++#[test]+fn driven_pendulum_reaches_its_commanded_angle_and_holds_it() {+ // A drive lifts the pendulum from hanging to horizontal along a cycloidal ramp in 1 s,+ // then holds it: the angle follows the ramp, and the holding torque is m·g·L.+ let (len, mass) = (100.0, 1.0);+ let mut model = pendulum(len, 0.0, 0.0, 40.0);+ let ramp = Profile::Ramp { from: 0.0, to: PI / 2.0, start: 0.0, duration: 1.0 };+ model.drives.push(Drive { at: Coord::new(0, 0), motion: ramp.clone() });+ let traj = ok(simulate(&model, &Settings::rk4(1.5, 1e-3, 1e-2)));+ let b0 = traj.samples.first().map(|s| s.energy.balance()).unwrap_or(f64::NAN);+ for s in &traj.samples {+ let angle = value(s, 0, 0);+ assert!((angle - ramp.eval(s.time).0).abs() < 1e-9, "t={}: angle {angle} vs {}", s.time, ramp.eval(s.time).0);+ // Work put in = kinetic + gravity energy gained.+ assert!((s.energy.balance() - b0).abs() < 1e-6 * G * len, "t={}: balance {}", s.time, s.energy.balance() - b0);+ }+ let held: Vec<&Sample> = traj.samples.iter().filter(|s| s.time >= 1.0).collect();+ assert!(held.len() > 40);+ for s in held {+ let effort = s.drives.first().copied().unwrap_or(f64::NAN);+ assert!((effort.abs() - mass * G * len).abs() < 1e-6 * G * len, "t={}: holding torque {effort}", s.time);+ assert!((s.energy.input - mass * G * len).abs() < 1e-6 * G * len, "work put in {}", s.energy.input);+ assert!(s.energy.kinetic < 1e-9 * G * len);+ }+}++#[test]+fn driven_speed_counts_whole_turns() {+ // 10 rad/s for 2 s: more than three turns; the driven angle never jumps a turn.+ let mut model = rotors(&[(500.0, Vec3::ZERO)]);+ model.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Linear { value: 0.0, rate: 10.0 } });+ let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));+ for s in &traj.samples {+ assert!(wrap(value(s, 0, 0) - 10.0 * s.time).abs() < 1e-9, "t={}", s.time);+ let w = s.bodies.first().map(|b| b.angular_velocity.z).unwrap_or(f64::NAN);+ assert!((w - 10.0).abs() < 1e-9);+ assert!(s.drives.first().is_some_and(|e| e.abs() < 1e-6), "{:?}", s.drives);+ }+ assert!(traj.stats.max_constraint_error < 1e-9);+}++#[test]+fn torsion_spring_period() {+ // ω = √(k/I) = √(5e5 / 500) rad/s; released from 0.3 rad.+ let (inertia, k) = (500.0_f64, 5e5_f64);+ let w = (k / inertia).sqrt();+ let mut model = rotors(&[(inertia, Vec3::ZERO)]);+ if let Some(b) = model.bodies.first_mut() {+ b.orientation = Quat::from_axis_angle(Vec3::Z, 0.3);+ }+ model.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: k, damping: 0.0, rest: 0.0 });+ let traj = ok(simulate(&model, &Settings::rk4(1.0, 1e-4, 1e-3)));+ for s in &traj.samples {+ let expect = 0.3 * (w * s.time).cos();+ assert!((value(s, 0, 0) - expect).abs() < 1e-7, "t={}: {} vs {expect}", s.time, value(s, 0, 0));+ }+ let measured = mean_period(&upward_crossings(&traj, |s| value(s, 0, 0)));+ assert!((measured - TAU / w).abs() < 1e-5 * TAU / w);+ let e0 = traj.samples.first().map(|s| s.energy.total()).unwrap_or(0.0);+ assert!((e0 - 0.5 * k * 0.09).abs() < 1e-9 * e0);+ // With damping the energy left plus the energy dissipated stays what it was.+ if let Some(Force::JointSpring { damping, .. }) = model.forces.first_mut() {+ *damping = 2.0 * 0.05 * (k * inertia).sqrt();+ }+ let traj = ok(simulate(&model, &Settings::rk4(1.0, 1e-4, 1e-2)));+ let last = traj.samples.last().map(|s| s.energy).unwrap_or_default();+ assert!(last.total() < 0.2 * e0 && last.dissipated > 0.8 * e0);+ assert!(traj.samples.iter().all(|s| (s.energy.balance() - e0).abs() < 1e-7 * e0));+}++#[test]+fn gear_pair_keeps_its_ratio_and_motor_work_is_kinetic_energy() {+ // A motor's constant torque τ on gear 1; gear 2 turns at −r₁/r₂ = −0.5 of it. The pair+ // accelerates as one inertia I₁ + 0.25·I₂: θ₁ = ½(τ/I_eff)t², and the motor's work τ·θ₁ is+ // all kinetic energy. Over 3 s gear 1 makes more than half a turn past ±π.+ let (i1, i2, ratio, tau) = (1000.0, 2000.0, -0.5, 1500.0);+ let alpha = tau / (i1 + ratio * ratio * i2);+ let mut model = rotors(&[(i1, Vec3::ZERO), (i2, Vec3::new(30.0, 0.0, 0.0))]);+ model.forces.push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Constant { value: tau } });+ model.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio, offset: None });+ let traj = ok(simulate(&model, &Settings::rk4(3.0, 1e-3, 1e-2)));+ for s in &traj.samples {+ let theta = 0.5 * alpha * s.time * s.time;+ assert!(wrap(value(s, 0, 0) - theta).abs() < 1e-8, "t={}: gear 1 {} vs {theta}", s.time, value(s, 0, 0));+ assert!(wrap(value(s, 1, 0) - ratio * theta).abs() < 1e-8, "t={}: gear 2", s.time);+ assert!((s.energy.input - tau * theta).abs() < 1e-6 * tau * 4.5, "t={}: work {}", s.time, s.energy.input);+ assert!((s.energy.kinetic - s.energy.input).abs() < 1e-6 * tau * 4.5, "t={}: kinetic {}", s.time, s.energy.kinetic);+ }+ assert!(0.5 * alpha * 9.0 > PI);+}++#[test]+fn rack_and_pinion_follow_the_pinion_radius() {+ // A pinion of radius 20 mm drives a rack on a slider along X; a harmonic motor torque. The+ // rack's travel is 20·θ, and the motor's work is the kinetic energy of both.+ let mut model = rotors(&[(800.0, Vec3::ZERO)]);+ model.bodies.push(Body::at_rest(0.5, Vec3::new(100.0, 100.0, 100.0), Vec3::new(0.0, 20.0, 0.0)));+ model.joints.push(Joint {+ kind: JointKind::Slider,+ a: Marker::ground(Vec3::new(0.0, 20.0, 0.0), Vec3::X, Vec3::Y),+ b: Marker::new(Some(1), Vec3::ZERO, Vec3::X, Vec3::Y),+ });+ model+ .forces+ .push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Harmonic { offset: 0.0, amplitude: 2000.0, frequency: 1.0, phase: 0.0 } });+ model.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 20.0, offset: None });+ let traj = ok(simulate(&model, &Settings::rk4(2.0, 1e-3, 1e-2)));+ // I_eff = 800 + 0.5·20²; θ̈ = τ(t)/I_eff with τ = A sin(2πt): θ = A/I_eff·(t/2π − sin(2πt)/4π²).+ let i_eff = 800.0 + 0.5 * 400.0;+ for s in &traj.samples {+ let t = s.time;+ let theta = 2000.0 / i_eff * (t / TAU - (TAU * t).sin() / (TAU * TAU));+ assert!((value(s, 0, 0) - theta).abs() < 1e-8, "t={t}: pinion {} vs {theta}", value(s, 0, 0));+ assert!((value(s, 1, 0) - 20.0 * theta).abs() < 1e-7, "t={t}: rack {}", value(s, 1, 0));+ assert!((s.energy.kinetic - s.energy.input).abs() < 1e-8 * 2000.0, "t={t}: {:?}", s.energy);+ }+}++#[test]+fn screw_turns_as_it_falls() {+ // A nut on a vertical cylindrical joint, its travel tied to its turn by a 5 mm pitch, falls+ // under gravity: z̈ = −m·g·k² / (I + m·k²) with k = pitch / 2π.+ let (mass, inertia, k) = (0.2, 50.0, 5.0 / TAU);+ let model = Model {+ bodies: vec![Body::at_rest(mass, Vec3::new(inertia, inertia, inertia), Vec3::ZERO)],+ joints: vec![Joint {+ kind: JointKind::Cylindrical,+ a: Marker::ground(Vec3::ZERO, Vec3::Z, Vec3::X),+ b: Marker::new(Some(0), Vec3::ZERO, Vec3::Z, Vec3::X),+ }],+ links: vec![Link { driver: Coord::new(0, 0), follower: Coord::new(0, 1), ratio: k, offset: None }],+ gravity: down(),+ ..Model::default()+ };+ let traj = ok(simulate(&model, &Settings::rk4(1.0, 1e-3, 1e-2)));+ let a = -mass * G * k * k / (inertia + mass * k * k);+ for s in &traj.samples {+ let z = s.bodies.first().map(|b| b.position.z).unwrap_or(f64::NAN);+ assert!((z - 0.5 * a * s.time * s.time).abs() < 1e-7, "t={}: z {z}", s.time);+ assert!(wrap(value(s, 0, 0) - z / k).abs() < 1e-8, "t={}: turn", s.time);+ assert!(s.energy.balance().abs() < 1e-6 * mass * G, "t={}: {:?}", s.time, s.energy);+ }+ // It went down more than one turn.+ assert!(0.5 * a < -5.0);+}++#[test]+fn hostile_springs_motors_drives_and_links_are_errors() {+ let good = rotors(&[(500.0, Vec3::ZERO), (500.0, Vec3::new(30.0, 0.0, 0.0))]);+ let mut bad: Vec<Model> = Vec::new();+ let mut with = |f: &dyn Fn(&mut Model)| {+ let mut m = good.clone();+ f(&mut m);+ bad.push(m);+ };+ with(&|m| m.drives.push(Drive { at: Coord::new(5, 0), motion: Profile::Constant { value: 0.0 } }));+ with(&|m| m.drives.push(Drive { at: Coord::new(0, 1), motion: Profile::Constant { value: 0.0 } }));+ with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Constant { value: f64::NAN } }));+ with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Ramp { from: 0.0, to: 1.0, start: 0.0, duration: 0.0 } }));+ with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Table { points: vec![] } }));+ with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Table { points: vec![(1.0, 0.0), (0.5, 1.0)] } }));+ with(&|m| m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Table { points: vec![(0.0, 0.0); MAX_PROFILE_POINTS + 1] } }));+ with(&|m| m.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: -1.0, damping: 0.0, rest: 0.0 }));+ with(&|m| m.forces.push(Force::JointSpring { at: Coord::new(0, 0), stiffness: 1.0, damping: f64::INFINITY, rest: 0.0 }));+ with(&|m| m.forces.push(Force::Motor { at: Coord::new(9, 0), effort: Profile::Constant { value: 1.0 } }));+ with(&|m| {+ m.forces+ .push(Force::Motor { at: Coord::new(0, 0), effort: Profile::Harmonic { offset: 0.0, amplitude: 1.0, frequency: f64::NAN, phase: 0.0 } })+ });+ with(&|m| m.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(0, 0), ratio: 1.0, offset: None }));+ with(&|m| m.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: f64::NAN, offset: None }));+ with(&|m| m.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 1.0, offset: Some(f64::INFINITY) }));+ with(&|m| m.links = vec![Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 1.0, offset: None }; MAX_JOINTS + 1]);+ with(&|m| {+ // Ball and rigid joints have no value to act on yet.+ if let Some(j) = m.joints.get_mut(0) {+ j.kind = JointKind::Ball;+ }+ m.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Constant { value: 0.0 } });+ });+ for (i, m) in bad.iter().enumerate() {+ assert!(simulate(m, &Settings::rk4(0.1, 1e-3, 1e-2)).is_err(), "case {i} should be refused");+ }+ // Links that fight each other, and drives that can't be met, stop or finish but never panic.+ let mut locked = good.clone();+ locked.links.push(Link { driver: Coord::new(0, 0), follower: Coord::new(1, 0), ratio: 1.0, offset: None });+ locked.links.push(Link { driver: Coord::new(1, 0), follower: Coord::new(0, 0), ratio: 2.0, offset: None });+ locked.drives.push(Drive { at: Coord::new(0, 0), motion: Profile::Harmonic { offset: 0.0, amplitude: 1e6, frequency: 1e3, phase: 0.0 } });+ let _ = simulate(&locked, &Settings::rk4(0.1, 1e-3, 1e-2));+ let _ = simulate(&locked, &Settings::adaptive(0.1, 1e-2, 1e-9, 1e-9));+}