Skip to content

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.rs
index 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.rs
index 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.rs
index 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.rs
index 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.rs
index 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));
+}