Close kinematic loops: driven joints move four-bars and sliders (#1141)
Driven joints now solve closed kinematic loops, so a four-bar or slider-crank moves as a whole when one joint is driven.
This commit is contained in:
Mateusz Sych authored and GitHub committed
1 parent
2e6951135a
commit
94faf9ce1b
7 files changed
+1025
-64
No files matched your search
@@ -3,6 +3,8 @@
|
||||
//! joint graph and moves the occurrences it places; the session re-solves after any edit too,
|
||||
//! so joints follow the geometry their origins snap to.
|
||||
|
||||
use std::collections::BTreeMap;
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_doc::expr::Kind;
|
||||
use solidcraft_doc::joints::{self, Joint, JointKind, JointOrigin, MotionLink, NamedOrigin, Snap};
|
||||
@@ -25,7 +27,8 @@ pub static COMMANDS: &[CommandSpec] = &[
|
||||
),
|
||||
CommandSpec::new("joint.rigid_group", "Rigid Group", rigid_group).at("SOLID", "ASSEMBLE").icon("rigid_group").params("occurrences: [ids or names] (move together, as they are)"),
|
||||
CommandSpec::new("joint.drive", "Drive Joints", drive).at("SOLID", "ASSEMBLE").icon("drive").params(
|
||||
"joint: id|name; value | values: [angles in deg or expressions, distances] (clamped to the limits)",
|
||||
"joint: id|name; value | values: [angles in deg or expressions, distances] (clamped to the limits; the other joints of a \
|
||||
closed loop follow, and the motion stops where the loop can no longer close → stopped_by_loop)",
|
||||
),
|
||||
CommandSpec::new("joint.motion_link", "Motion Link", motion_link)
|
||||
.at("SOLID", "ASSEMBLE")
|
||||
@@ -43,8 +46,14 @@ pub static COMMANDS: &[CommandSpec] = &[
|
||||
|
||||
/// Re-solve the joints and move the occurrences they place (after any edit).
|
||||
pub fn resolve(s: &mut Session) -> joints::Solution {
|
||||
resolve_holding(s, &BTreeMap::new())
|
||||
}
|
||||
|
||||
/// Re-solve the joints, keeping the `held` values (the ones being driven, mapped to the values
|
||||
/// they move from) where loops are closed.
|
||||
pub fn resolve_holding(s: &mut Session, held: &joints::Held) -> joints::Solution {
|
||||
let st = s.model.state();
|
||||
let sol = joints::solve(&s.doc, &st);
|
||||
let sol = joints::solve_holding(&s.doc, &st, held);
|
||||
if !sol.world.is_empty() {
|
||||
let mut next = (*s.doc).clone();
|
||||
if next.apply_joint_solution(&sol) {
|
||||
@@ -353,13 +362,19 @@ fn drive(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
}
|
||||
}
|
||||
joints::clamp(j, &mut q);
|
||||
// Moved as far toward `q` as contact sets allow.
|
||||
let r = super::motion::drive_joint(s, id, q)?;
|
||||
// Moved as far toward `q` as contact sets and closed loops allow. Values left out of
|
||||
// `values` are not driven: in a closed loop they follow.
|
||||
let given = match p.get("values") {
|
||||
Some(Value::Array(a)) => a.len(),
|
||||
_ => 1,
|
||||
};
|
||||
let r = super::motion::drive_joint(s, id, q, given)?;
|
||||
let sol = joints::solve(&s.doc, &s.model.state());
|
||||
let mut out = sol_json(&sol);
|
||||
out["joint"] = json!(id);
|
||||
out["values"] = json!(s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.values.clone()));
|
||||
out["stopped_by_contact"] = r["stopped_by_contact"].clone();
|
||||
out["stopped_by_loop"] = r["stopped_by_loop"].clone();
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
|
||||
@@ -295,3 +295,428 @@ fn interference_box_sweep_matches_every_pair_check() {
|
||||
let (pairs, skipped) = super::overlapping_pairs(&boxes, 10);
|
||||
assert_eq!((pairs.len(), skipped), (10, every.len() - 10));
|
||||
}
|
||||
|
||||
/// A small block in a new component: a part of a linkage, where only its joints matter.
|
||||
fn part(s: &mut Session, name: &str, corner: [f64; 3]) {
|
||||
run(s, "component.activate", json!({"component": "root"}));
|
||||
run(s, "component.create", json!({"name": name}));
|
||||
run(s, "solid.box", json!({"length": 2, "width": 2, "height": 2, "corner": corner}));
|
||||
}
|
||||
|
||||
/// A joint between two parts as they are, at a world point with its z along `z`.
|
||||
fn pin(s: &mut Session, kind: &str, name: &str, a: &str, b: &str, p: Vec3, z: Vec3) {
|
||||
let at = |o: &str| json!({"occurrence": o, "point": [p.x, p.y, p.z], "z": [z.x, z.y, z.z]});
|
||||
let r = run(s, "joint.create", json!({"type": kind, "a": at(a), "b": at(b), "flip": true, "name": name}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
|
||||
/// Where circles about `p` (radius `r`) and `q` (radius `s`) in the XY plane meet.
|
||||
fn meet(p: Vec3, r: f64, q: Vec3, s: f64) -> [Vec3; 2] {
|
||||
let d = q - p;
|
||||
let l = d.len();
|
||||
let a = (r * r - s * s + l * l) / (2.0 * l);
|
||||
let m = p + d * (a / l);
|
||||
let n = Vec3::new(-d.y, d.x, 0.0) * ((r * r - a * a).max(0.0).sqrt() / l);
|
||||
[m + n, m - n]
|
||||
}
|
||||
|
||||
/// A crank-rocker four-bar about +Z: ground pivots O2 = (0, 0) and O4 = (45, 0), crank 20,
|
||||
/// coupler 50, rocker 40, assembled with the crank at 90°. Returns the crank pin P0 and the
|
||||
/// rocker pin Q0 (the upper, open assembly).
|
||||
fn four_bar() -> (Session, Vec3, Vec3) {
|
||||
let mut s = Session::default();
|
||||
let (o2, o4, p0) = (Vec3::ZERO, Vec3::new(45.0, 0.0, 0.0), Vec3::new(0.0, 20.0, 0.0));
|
||||
let [q0, _] = meet(p0, 50.0, o4, 40.0);
|
||||
for (name, c) in [("Ground", o2), ("Crank", p0), ("Coupler", (p0 + q0) * 0.5), ("Rocker", q0)] {
|
||||
part(&mut s, name, [c.x, c.y, -5.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
pin(&mut s, "revolute", "Crank", "Ground", "Crank", o2, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Crank pin", "Crank", "Coupler", p0, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Rocker pin", "Coupler", "Rocker", q0, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Rocker", "Ground", "Rocker", o4, Vec3::Z);
|
||||
(s, p0, q0)
|
||||
}
|
||||
|
||||
/// The four-bar's rocker pin with the crank driven to `to`°, followed a degree at a time from
|
||||
/// `from`° (where it is at `q`), so it stays on its branch.
|
||||
fn rocker_pin(from: f64, to: f64, mut q: Vec3) -> Vec3 {
|
||||
let n = ((to - from).abs().ceil() as usize).max(1);
|
||||
for k in 1..=n {
|
||||
let th = (90.0 + from + (to - from) * k as f64 / n as f64).to_radians();
|
||||
let [m1, m2] = meet(Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0), 50.0, Vec3::new(45.0, 0.0, 0.0), 40.0);
|
||||
q = if m1.dist(q) <= m2.dist(q) { m1 } else { m2 };
|
||||
}
|
||||
q
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn four_bar_follows_its_crank() {
|
||||
let (mut s, p0, q0) = four_bar();
|
||||
let mut q = q0;
|
||||
let mut at_330 = q0;
|
||||
for k in 1..=12 {
|
||||
let v = 30.0 * f64::from(k);
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": v}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{v}°: {r}");
|
||||
assert!(r["stopped_by_loop"].is_null(), "{v}°: {r}");
|
||||
// The driven joint keeps exactly the value asked for.
|
||||
assert!((r["values"][0].as_f64().unwrap_or(f64::NAN) - v.to_radians()).abs() < 1e-12, "{r}");
|
||||
q = rocker_pin(v - 30.0, v, q);
|
||||
let th = (90.0 + v).to_radians();
|
||||
let p = Vec3::new(20.0 * th.cos(), 20.0 * th.sin(), 0.0);
|
||||
for (part, local, want) in [("Crank", p0, p), ("Coupler", p0, p), ("Coupler", q0, q), ("Rocker", q0, q)] {
|
||||
assert!(near(place(&s, part, local), want), "{v}°: {part} {:?} wants {want:?}", place(&s, part, local));
|
||||
}
|
||||
if k == 11 {
|
||||
at_330 = q;
|
||||
}
|
||||
}
|
||||
// Back where it started after a full turn, and undo steps back.
|
||||
assert!(near(q, q0));
|
||||
run(&mut s, "edit.undo", json!({}));
|
||||
assert!(near(place(&s, "Rocker", q0), at_330), "{:?}", place(&s, "Rocker", q0));
|
||||
// One 180° drive lands on the same (open) branch as the stepped path.
|
||||
let (mut s, _, q0) = four_bar();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 180}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, 180.0, q0)), "{:?}", place(&s, "Rocker", q0));
|
||||
// A motion study playing the crank places the rocker the same way.
|
||||
run(&mut s, "motion.study", json!({"name": "Turn", "steps": 10, "keys": [{"joint": "Crank", "points": [[0, 0], [10, 120]]}]}));
|
||||
let r = run(&mut s, "motion.play", json!({"study": "Turn", "step": 5}));
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(180.0, 60.0, rocker_pin(0.0, 180.0, q0))), "{:?}", place(&s, "Rocker", q0));
|
||||
}
|
||||
|
||||
/// The rocker swings between its toggle positions, where crank and coupler line up (|Q| = 70
|
||||
/// and |Q| = 30, about 69.3° and 139.2° from the ground line); driving it further stops where
|
||||
/// the loop still closes.
|
||||
#[test]
|
||||
fn four_bar_rocker_stops_where_the_loop_cannot_close() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
let start = q0.y.atan2(q0.x - 45.0).to_degrees();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": 100}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let reached = r["values"][0].as_f64().unwrap_or(f64::NAN).to_degrees();
|
||||
// |Q|² = 45² + 40² + 2·45·40·cos φ = 30².
|
||||
let limit = ((900.0f64 - 3625.0) / 3600.0).acos().to_degrees() - start;
|
||||
assert!(reached > limit - 5.0 && reached <= limit, "stopped at {reached}°, the limit is {limit}°");
|
||||
// Inside its swing it moves freely, and the crank follows.
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": -10}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
let q = place(&s, "Rocker", q0);
|
||||
assert!((q.y.atan2(q.x - 45.0).to_degrees() - (start - 10.0)).abs() < 1e-6, "{q:?}");
|
||||
assert!(place(&s, "Coupler", q0).dist(q) < 1e-6);
|
||||
// A full turn would end where it started, but the rocker can't get there.
|
||||
let (mut s, _, _) = four_bar();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert!(r["values"][0].as_f64().unwrap_or(f64::NAN).to_degrees() <= limit, "{r}");
|
||||
}
|
||||
|
||||
/// A loop value a motion link sets follows through the start of its link chain: a dial outside
|
||||
/// the loop turns a gear, which sets the rocker's angle, so turning the crank turns the dial.
|
||||
#[test]
|
||||
fn four_bar_turns_the_source_of_a_linked_loop_value() {
|
||||
let linked = || {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
for (name, x) in [("Dial", 80.0), ("Gear", 100.0)] {
|
||||
part(&mut s, name, [x, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Gear", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Gear", "b": "Rocker", "ratio": 0.5}));
|
||||
(s, q0)
|
||||
};
|
||||
let (mut s, q0) = linked();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let q = rocker_pin(0.0, 60.0, q0);
|
||||
assert!(near(place(&s, "Rocker", q0), q), "{:?} wants {q:?}", place(&s, "Rocker", q0));
|
||||
let turn = q.y.atan2(q.x - 45.0) - q0.y.atan2(q0.x - 45.0);
|
||||
let dial = s.doc.assembly.joints.iter().find(|j| j.name == "Dial").and_then(|j| j.values.first().copied()).unwrap_or(f64::NAN);
|
||||
assert!((dial - 2.0 * turn).abs() < 1e-9, "dial {dial}, rocker turned {turn}");
|
||||
// Driving the dial drives the rocker through the chain: two turns of the dial would turn
|
||||
// the rocker a full turn, which it can't make.
|
||||
let (mut s, _) = linked();
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 720}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
|
||||
/// Values left out of a drive are free to follow in a loop: a plate on a planar joint whose
|
||||
/// travel a diagonal slider limits. Driving only the planar joint's x moves its y along.
|
||||
#[test]
|
||||
fn values_left_out_of_a_drive_follow_the_loop() {
|
||||
let mut s = Session::default();
|
||||
part(&mut s, "Ground", [-1.0, -1.0, -5.0]);
|
||||
part(&mut s, "Plate", [-1.0, -1.0, 0.0]);
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
let frame = |o: &str, z: [f64; 3], x: [f64; 3]| json!({"occurrence": o, "frame": {"origin": [0, 0, 0], "z": z, "x": x}});
|
||||
let flat = |o: &str| frame(o, [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]);
|
||||
let r = run(&mut s, "joint.create", json!({"type": "planar", "a": flat("Ground"), "b": flat("Plate"), "flip": true, "name": "Table"}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let diagonal = |o: &str| frame(o, [1.0, 1.0, 0.0], [0.0, 0.0, 1.0]);
|
||||
let r = run(&mut s, "joint.create", json!({"type": "slider", "a": diagonal("Ground"), "b": diagonal("Plate"), "flip": true, "name": "Rail"}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Table", "values": [10]}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
assert!(near(place(&s, "Plate", Vec3::ZERO), Vec3::new(10.0, 10.0, 0.0)), "{:?}", place(&s, "Plate", Vec3::ZERO));
|
||||
}
|
||||
|
||||
/// A slider-crank: crank 15 about the origin, rod 40, a piston sliding along X. The piston
|
||||
/// sits at a·cos θ + √(b² − (a·sin θ)²).
|
||||
#[test]
|
||||
fn slider_crank_moves_the_piston() {
|
||||
let mut s = Session::default();
|
||||
let (o, p0) = (Vec3::ZERO, Vec3::new(0.0, 15.0, 0.0));
|
||||
let s0 = Vec3::new((40.0f64 * 40.0 - 15.0 * 15.0).sqrt(), 0.0, 0.0);
|
||||
for (name, c) in [("Ground", o), ("Crank", p0), ("Rod", s0 * 0.5), ("Piston", s0)] {
|
||||
part(&mut s, name, [c.x, c.y, -5.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
pin(&mut s, "revolute", "Crank", "Ground", "Crank", o, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Crank pin", "Crank", "Rod", p0, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Wrist pin", "Rod", "Piston", s0, Vec3::Z);
|
||||
pin(&mut s, "slider", "Bore", "Ground", "Piston", s0, Vec3::X);
|
||||
for v in [40.0, 135.0, 270.0] {
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": v}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{v}°: {r}");
|
||||
let th = (90.0f64 + v).to_radians();
|
||||
let x = 15.0 * th.cos() + (40.0f64.powi(2) - (15.0 * th.sin()).powi(2)).sqrt();
|
||||
assert!(near(place(&s, "Piston", s0), Vec3::new(x, 0.0, 0.0)), "{v}°: {:?} wants x = {x}", place(&s, "Piston", s0));
|
||||
assert!(near(place(&s, "Rod", p0), Vec3::new(15.0 * th.cos(), 15.0 * th.sin(), 0.0)));
|
||||
}
|
||||
}
|
||||
|
||||
/// A joint frame on an occurrence, by origin and axes.
|
||||
fn framed(o: &str, origin: [f64; 3], z: [f64; 3], x: [f64; 3]) -> Value {
|
||||
json!({"occurrence": o, "frame": {"origin": origin, "z": z, "x": x}})
|
||||
}
|
||||
|
||||
/// A loop open from the start closes from where it is: an arm on a revolute joint, and a rigid
|
||||
/// joint that wants it turned 90°.
|
||||
#[test]
|
||||
fn an_open_loop_closes_from_where_it_is() {
|
||||
let mut s = Session::default();
|
||||
part(&mut s, "Ground", [-1.0, -1.0, -5.0]);
|
||||
part(&mut s, "Arm", [0.0, -1.0, 0.0]);
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
pin(&mut s, "revolute", "Pivot", "Ground", "Arm", Vec3::ZERO, Vec3::Z);
|
||||
let r = run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": framed("Ground", [0.0; 3], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]),
|
||||
"b": framed("Arm", [0.0; 3], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), "flip": true, "name": "Lock"}),
|
||||
);
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
assert!(near(place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)), Vec3::new(0.0, 10.0, 0.0)), "{:?}", place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
/// A loop that can't close does not hold up another one: the four-bar still drives beside a
|
||||
/// block whose rigid joint wants it where its pivot can't take it.
|
||||
#[test]
|
||||
fn a_broken_loop_does_not_stop_another() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
part(&mut s, "Block", [200.0, -1.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Block pivot", "Ground", "Block", Vec3::new(200.0, 0.0, 0.0), Vec3::Z);
|
||||
run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": {"occurrence": "Ground", "point": [200, 0, 0]}, "b": {"occurrence": "Block", "point": [210, 0, 0]}, "flip": true, "name": "Wrong"}),
|
||||
);
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
let names: Vec<&str> = r["conflicts"].as_array().into_iter().flatten().filter_map(|c| c["name"].as_str()).collect();
|
||||
assert_eq!(names, ["Wrong"], "{r}");
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, 60.0, q0)), "{:?}", place(&s, "Rocker", q0));
|
||||
}
|
||||
|
||||
/// A loop locked rigid can't move at all: driving its crank stops at once.
|
||||
#[test]
|
||||
fn a_locked_loop_does_not_move() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "Ground", "b": "Rocker", "name": "Lock"}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert!(r["values"][0].as_f64().unwrap_or(f64::NAN).abs() < 1e-3, "{r}");
|
||||
}
|
||||
|
||||
/// A link that turns the rocker 100 times as far as its dial: a small turn of the dial is a full
|
||||
/// turn of the rocker, which it can't make.
|
||||
#[test]
|
||||
fn a_geared_down_loop_value_is_followed_all_the_way() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Rocker", "ratio": 100}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 3.6}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
|
||||
/// A suppressed dial still turns the rocker through its motion link, and a rocker whose value
|
||||
/// the file leaves out starts from 0: both are followed all the way, so a full turn stops.
|
||||
#[test]
|
||||
fn a_full_turn_stops_from_a_suppressed_dial_or_a_missing_value() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Rocker", "ratio": 0.5}));
|
||||
run(&mut s, "joint.edit", json!({"joint": "Dial", "suppressed": true}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 720}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
// Undriven, the suppressed dial still follows the rocker when the crank turns.
|
||||
run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 0}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
let (mut s, _, _) = four_bar();
|
||||
if let Some(j) = s.doc_mut().assembly.joints.iter_mut().find(|j| j.name == "Rocker") {
|
||||
j.values.clear();
|
||||
}
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Rocker", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
}
|
||||
|
||||
/// Loops that only share a rigid joint are independent: an arm whose lock can't close does not
|
||||
/// stop another arm, beside it on the same rigidly mounted base, from closing its own loop.
|
||||
#[test]
|
||||
fn loops_sharing_only_a_rigid_joint_are_solved_apart() {
|
||||
let mut s = Session::default();
|
||||
for (name, x) in [("Ground", -1.0), ("Base", -1.0), ("Arm", 0.0), ("Other", 100.0)] {
|
||||
part(&mut s, name, [x, -1.0, 0.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "Ground", "b": "Base", "name": "Mount"}));
|
||||
pin(&mut s, "revolute", "Pivot", "Base", "Arm", Vec3::ZERO, Vec3::Z);
|
||||
pin(&mut s, "revolute", "Other pivot", "Base", "Other", Vec3::new(100.0, 0.0, 0.0), Vec3::Z);
|
||||
run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": {"occurrence": "Ground", "point": [100, 0, 0]}, "b": {"occurrence": "Other", "point": [110, 0, 0]}, "flip": true, "name": "Wrong"}),
|
||||
);
|
||||
let r = run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": framed("Ground", [0.0; 3], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]),
|
||||
"b": framed("Arm", [0.0; 3], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]), "flip": true, "name": "Lock"}),
|
||||
);
|
||||
// The broken loop is reported (on whichever of its joints closes it), the arm's is not.
|
||||
let names: Vec<&str> = r["conflicts"].as_array().into_iter().flatten().filter_map(|c| c["name"].as_str()).collect();
|
||||
assert!(names.len() == 1 && ["Wrong", "Other pivot"].contains(&names[0]), "{r}");
|
||||
assert!(near(place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)), Vec3::new(0.0, 10.0, 0.0)), "{:?}", place(&s, "Arm", Vec3::new(10.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
/// Two links writing one value: whichever carries a driven dial's turn into the loop, the turn
|
||||
/// is followed (here dial A turns gear B, which turns the rocker, before dial D rewrites B).
|
||||
#[test]
|
||||
fn a_turn_carried_in_by_any_link_is_followed() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
for (name, x) in [("A", 80.0), ("B", 100.0), ("D", 120.0)] {
|
||||
part(&mut s, name, [x, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "A", "b": "B", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "B", "b": "Rocker", "ratio": 0.5}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "D", "b": "B", "ratio": 1}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "A", "value": 720}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
}
|
||||
|
||||
/// Several links writing one value: every value a link chain starts from can move to close the
|
||||
/// loop, whichever of them the link order lets through (here dial A's, before dial B rewrites
|
||||
/// the gear).
|
||||
#[test]
|
||||
fn a_loop_value_moves_through_whichever_link_carries_it() {
|
||||
let (mut s, _, q0) = four_bar();
|
||||
for (name, x) in [("A", 80.0), ("G", 100.0), ("B", 120.0)] {
|
||||
part(&mut s, name, [x, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", name, "Ground", name, Vec3::new(x, 0.0, 0.0), Vec3::Z);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "A", "b": "G", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "G", "b": "Rocker", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "B", "b": "G", "ratio": 1}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Crank", "value": 60}));
|
||||
assert!(r["stopped_by_loop"].is_null() && r["conflicts"].as_array().map(Vec::len) == Some(0), "{r}");
|
||||
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, 60.0, q0)), "{:?}", place(&s, "Rocker", q0));
|
||||
}
|
||||
|
||||
/// Two values of one joint can feed two independent loops: a puck's x drives a pair of parallel
|
||||
/// slides, its y a slide whose rigid lock can't close. Driving x moves the healthy pair.
|
||||
#[test]
|
||||
fn values_of_one_joint_feed_independent_loops() {
|
||||
let mut s = Session::default();
|
||||
for (name, y) in [("Ground", -10.0), ("Puck", -20.0), ("Car", 50.0), ("Other", 100.0)] {
|
||||
part(&mut s, name, [-1.0, y, 0.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
let flat = |o: &str| framed(o, [0.0, -20.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]);
|
||||
run(&mut s, "joint.create", json!({"type": "planar", "a": flat("Ground"), "b": flat("Puck"), "flip": true, "name": "Table"}));
|
||||
pin(&mut s, "slider", "Rail", "Ground", "Car", Vec3::new(0.0, 50.0, 0.0), Vec3::X);
|
||||
pin(&mut s, "slider", "Guide", "Ground", "Car", Vec3::new(10.0, 50.0, 0.0), Vec3::X);
|
||||
pin(&mut s, "slider", "Lift", "Ground", "Other", Vec3::new(0.0, 100.0, 0.0), Vec3::Y);
|
||||
run(
|
||||
&mut s,
|
||||
"joint.create",
|
||||
json!({"type": "rigid", "a": {"occurrence": "Ground", "point": [0, 100, 0]}, "b": {"occurrence": "Other", "point": [10, 100, 0]}, "flip": true, "name": "Wrong"}),
|
||||
);
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 0, "b": "Rail", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 1, "b": "Lift", "ratio": 1}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Table", "values": [10]}));
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
let names: Vec<&str> = r["conflicts"].as_array().into_iter().flatten().filter_map(|c| c["name"].as_str()).collect();
|
||||
assert_eq!(names, ["Wrong"], "{r}");
|
||||
assert!(near(place(&s, "Car", Vec3::new(0.0, 50.0, 0.0)), Vec3::new(10.0, 50.0, 0.0)), "{:?}", place(&s, "Car", Vec3::new(0.0, 50.0, 0.0)));
|
||||
}
|
||||
|
||||
/// Two loops can move different values of one joint: each keeps what it moved. A table's x
|
||||
/// feeds one pair of parallel slides and its y another; a study drives both pairs.
|
||||
#[test]
|
||||
fn loops_moving_different_values_of_one_joint_keep_both() {
|
||||
let mut s = Session::default();
|
||||
for (name, y) in [("Ground", -10.0), ("Puck", -20.0), ("CarA", 50.0), ("CarB", 100.0)] {
|
||||
part(&mut s, name, [-1.0, y, 0.0]);
|
||||
}
|
||||
let g = occ(&s, "Ground");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
|
||||
let flat = |o: &str| framed(o, [0.0, -20.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0]);
|
||||
run(&mut s, "joint.create", json!({"type": "planar", "a": flat("Ground"), "b": flat("Puck"), "flip": true, "name": "Table"}));
|
||||
for (car, y) in [("CarA", 50.0), ("CarB", 100.0)] {
|
||||
pin(&mut s, "slider", &format!("{car} rail"), "Ground", car, Vec3::new(0.0, y, 0.0), Vec3::X);
|
||||
pin(&mut s, "slider", &format!("{car} guide"), "Ground", car, Vec3::new(10.0, y, 0.0), Vec3::X);
|
||||
}
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 0, "b": "CarA rail", "ratio": 1}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Table", "ia": 1, "b": "CarB rail", "ratio": 1}));
|
||||
let keys = json!([{"joint": "CarA guide", "points": [[0, 0], [10, 10]]}, {"joint": "CarB guide", "points": [[0, 0], [10, 20]]}]);
|
||||
run(&mut s, "motion.study", json!({"name": "Both", "steps": 10, "keys": keys}));
|
||||
let r = run(&mut s, "motion.play", json!({"study": "Both", "step": 10}));
|
||||
assert!(r["stopped_by_loop"].is_null(), "{r}");
|
||||
let table = s.doc.assembly.joints.iter().find(|j| j.name == "Table").map(|j| j.values.clone()).unwrap_or_default();
|
||||
assert!(table.len() == 3 && (table[0] - 10.0).abs() < 1e-6 && (table[1] - 20.0).abs() < 1e-6, "{table:?}");
|
||||
// A solve with nothing driven leaves them where they are.
|
||||
run(&mut s, "joint.solve", json!({}));
|
||||
assert!(near(place(&s, "CarA", Vec3::new(0.0, 50.0, 0.0)), Vec3::new(10.0, 50.0, 0.0)), "{:?}", place(&s, "CarA", Vec3::new(0.0, 50.0, 0.0)));
|
||||
assert!(near(place(&s, "CarB", Vec3::new(0.0, 100.0, 0.0)), Vec3::new(20.0, 100.0, 0.0)), "{:?}", place(&s, "CarB", Vec3::new(0.0, 100.0, 0.0)));
|
||||
}
|
||||
|
||||
/// A clamped link can jump: a 100:1 link to a rocker limited to a full turn reaches its limit
|
||||
/// after the dial's first few degrees, back where it started. Each step is checked, so the drive
|
||||
/// still stops where the rocker can't go.
|
||||
#[test]
|
||||
fn a_clamped_link_is_followed_step_by_step() {
|
||||
let (mut s, _, _) = four_bar();
|
||||
part(&mut s, "Dial", [80.0, 0.0, -5.0]);
|
||||
pin(&mut s, "revolute", "Dial", "Ground", "Dial", Vec3::new(80.0, 0.0, 0.0), Vec3::Z);
|
||||
run(&mut s, "joint.limits", json!({"joint": "Rocker", "min": 0, "max": 360}));
|
||||
run(&mut s, "joint.motion_link", json!({"a": "Dial", "b": "Rocker", "ratio": 100}));
|
||||
let r = run(&mut s, "joint.drive", json!({"joint": "Dial", "value": 360}));
|
||||
assert!(r["stopped_by_loop"].is_object(), "{r}");
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{r}");
|
||||
}
|
||||
@@ -1,5 +1,7 @@
|
||||
//! Contact Sets, Motion Study and Exploded views.
|
||||
|
||||
use std::collections::BTreeSet;
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_doc::Mat;
|
||||
use solidcraft_doc::joints::{self, JointKind};
|
||||
@@ -8,7 +10,7 @@ use solidcraft_geom::Vec3;
|
||||
use solidcraft_kernel::Body;
|
||||
|
||||
use super::CommandSpec;
|
||||
use super::joints::{joint_id, occurrence_id, resolve, value};
|
||||
use super::joints::{joint_id, occurrence_id, resolve_holding, value};
|
||||
use crate::params::{bad, bool_, str_, vec3};
|
||||
use crate::{EngineError, Result, Session};
|
||||
|
||||
@@ -254,27 +256,41 @@ fn closest(p: Vec3, a: Vec3, b: Vec3, c: Vec3) -> Vec3 {
|
||||
a + ab * (vb / den) + ac * (vc / den)
|
||||
}
|
||||
|
||||
/// Set joint values, stopping short where contact sets would collide: the values are moved
|
||||
/// from where they are toward `want` as far as they go without a collision.
|
||||
fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)]) -> Result<Value> {
|
||||
/// Set joint values, stopping short where contact sets would collide or a closed loop could no
|
||||
/// longer close: the values are moved from where they are toward `want` as far as they go. The
|
||||
/// `given` values (joint, index) are driven; in a closed loop the others follow.
|
||||
fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)], given: &BTreeSet<(u64, usize)>) -> Result<Value> {
|
||||
let baseline = s.doc.clone();
|
||||
let start: Vec<(u64, Vec<f64>)> =
|
||||
want.iter().map(|(id, _)| (*id, s.doc.assembly.joints.iter().find(|j| j.id == *id).map(|j| j.values.clone()).unwrap_or_default())).collect();
|
||||
// Closed loops follow the driven values from where they start.
|
||||
let held: joints::Held = given
|
||||
.iter()
|
||||
.filter(|&&(id, i)| want.iter().any(|(w, q)| *w == id && i < q.len()))
|
||||
.map(|&(id, i)| {
|
||||
// A value the file leaves out is 0, as the solver reads it.
|
||||
let from = start.iter().find(|(s, _)| *s == id).and_then(|(_, q)| q.get(i).copied());
|
||||
((id, i), from.unwrap_or(0.0))
|
||||
})
|
||||
.collect();
|
||||
let set = |s: &mut Session, t: f64| {
|
||||
// Trial order must not affect free anchors or linked joint placements.
|
||||
s.doc = baseline.clone();
|
||||
let doc = s.doc_mut();
|
||||
for ((id, a), (_, b)) in start.iter().zip(want) {
|
||||
if let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == *id) {
|
||||
let mut q: Vec<f64> = b.iter().enumerate().map(|(i, bv)| a.get(i).copied().unwrap_or(*bv) * (1.0 - t) + bv * t).collect();
|
||||
let mut q: Vec<f64> = b.iter().enumerate().map(|(i, bv)| a.get(i).copied().unwrap_or(0.0) * (1.0 - t) + bv * t).collect();
|
||||
joints::clamp(j, &mut q);
|
||||
j.values = q;
|
||||
}
|
||||
}
|
||||
resolve(s)
|
||||
resolve_holding(s, &held)
|
||||
};
|
||||
// Loops already open at the start stay as they are; a loop the motion opens stops it.
|
||||
let open_at_start: BTreeSet<u64> = set(s, 0.0).conflicts.iter().map(|c| c.0).collect();
|
||||
let opened = |sol: &joints::Solution| sol.conflicts.iter().find(|c| !open_at_start.contains(&c.0)).map(|c| c.1.clone());
|
||||
let cache = if s.doc.assembly.contact_enabled { Some(contact_sweep::ContactGeometry::new(s)?) } else { None };
|
||||
let sol;
|
||||
let mut sol;
|
||||
let mut stopped = None;
|
||||
if let Some(cache) = cache.filter(|cache| cache.has_pairs()) {
|
||||
let mut end = s.scratch();
|
||||
@@ -313,16 +329,33 @@ fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)]) -> Result<Value
|
||||
} else {
|
||||
sol = set(s, 1.0);
|
||||
}
|
||||
let mut stopped_by_loop = None;
|
||||
if let Some(mut name) = opened(&sol) {
|
||||
// The furthest the loops still close, short of where contact stopped; the joint named is
|
||||
// the one whose loop opens first.
|
||||
let (mut lo, mut hi) = (0.0, stopped.as_ref().and_then(|v| v["fraction"].as_f64()).unwrap_or(1.0));
|
||||
for _ in 0..14 {
|
||||
let mid = (lo + hi) / 2.0;
|
||||
match opened(&set(s, mid)) {
|
||||
None => lo = mid,
|
||||
Some(n) => (hi, name) = (mid, n),
|
||||
}
|
||||
}
|
||||
sol = set(s, lo);
|
||||
stopped = None;
|
||||
stopped_by_loop = Some(json!({"joint": name, "fraction": lo}));
|
||||
}
|
||||
let values: Vec<Value> = want
|
||||
.iter()
|
||||
.map(|(id, _)| json!({"joint": id, "values": s.doc.assembly.joints.iter().find(|j| j.id == *id).map(|j| j.values.clone())}))
|
||||
.collect();
|
||||
Ok(json!({"values": values, "stopped_by_contact": stopped, "conflicts": sol.conflicts.len()}))
|
||||
Ok(json!({"values": values, "stopped_by_contact": stopped, "stopped_by_loop": stopped_by_loop, "conflicts": sol.conflicts.len()}))
|
||||
}
|
||||
|
||||
/// Drive one joint (joint.drive), honouring contact sets.
|
||||
pub fn drive_joint(s: &mut Session, id: u64, q: Vec<f64>) -> Result<Value> {
|
||||
drive_with_contact(s, &[(id, q)])
|
||||
pub fn drive_joint(s: &mut Session, id: u64, q: Vec<f64>, given: usize) -> Result<Value> {
|
||||
let given = (0..given.min(q.len())).map(|i| (id, i)).collect();
|
||||
drive_with_contact(s, &[(id, q)], &given)
|
||||
}
|
||||
|
||||
fn enable_sets(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
@@ -517,6 +550,11 @@ fn values_at(s: &Session, m: &MotionStudy, step: f64) -> Vec<(u64, Vec<f64>)> {
|
||||
out
|
||||
}
|
||||
|
||||
/// The values a study keys: (joint, index).
|
||||
fn keyed(m: &MotionStudy) -> BTreeSet<(u64, usize)> {
|
||||
m.keys.iter().map(|k| (k.joint, k.index)).collect()
|
||||
}
|
||||
|
||||
/// Where each occurrence is: (name, world transform).
|
||||
fn positions(s: &Session) -> Vec<(String, Mat)> {
|
||||
s.doc.occurrences.iter().map(|o| (o.name.clone(), joints::occurrence_world(&s.doc, o.id))).collect()
|
||||
@@ -532,7 +570,7 @@ fn play(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
let m = s.doc.assembly.studies.get(i).cloned().ok_or_else(|| bad(cmd, "study"))?;
|
||||
let step = p.get("step").and_then(Value::as_f64).filter(|x| x.is_finite()).ok_or_else(|| bad(cmd, "`step` must be a number"))?;
|
||||
let want = values_at(s, &m, step.clamp(0.0, f64::from(m.steps)));
|
||||
let mut out = drive_with_contact(s, &want)?;
|
||||
let mut out = drive_with_contact(s, &want, &keyed(&m))?;
|
||||
out["positions"] = positions(s).iter().map(|(n, t)| json!({"occurrence": n, "pose": pose_json(t)})).collect();
|
||||
Ok(out)
|
||||
}
|
||||
@@ -551,10 +589,11 @@ fn export(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
for k in 0..samples {
|
||||
let step = f64::from(m.steps) * k as f64 / (samples - 1) as f64;
|
||||
let want = values_at(&t, &m, step);
|
||||
let r = drive_with_contact(&mut t, &want)?;
|
||||
let r = drive_with_contact(&mut t, &want, &keyed(&m))?;
|
||||
rows.push(json!({
|
||||
"step": step,
|
||||
"stopped_by_contact": r["stopped_by_contact"],
|
||||
"stopped_by_loop": r["stopped_by_loop"],
|
||||
"positions": positions(&t).iter().map(|(n, x)| json!({"occurrence": n, "pose": pose_json(x)})).collect::<Vec<_>>(),
|
||||
}));
|
||||
}
|
||||
|
||||
Reference in new issue
Block a user