Joint drives say when they move nothing; drive values in degrees; quiet replies (#1304)

joint.drive answers moved, and a reason (with unconnected) when the joint can move nothing; motion.play answers moved and unmoved; joint.list answers floating chains. joint.drive values come back in joint.list's shape (deg/mm). quiet: true trims joint answers and motion.play poses. A single value keeps a joint's other values. Drive Joints shows the floating case (drive.unconnected, all locales).
This commit is contained in:
Mateusz Sych authored and GitHub committed 2026-10-15 02:12:58 -04:00
1 parent fd19efc6fc
commit 7195e323bf
15 files changed
+326 -79

No files matched your search

+131 -46
View File
@@ -3,7 +3,7 @@
//! 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 std::collections::{BTreeMap, BTreeSet};
use serde_json::{Value, json};
use solidcraft_doc::expr::Kind;
@@ -20,15 +20,21 @@ pub static COMMANDS: &[CommandSpec] = &[
),
CommandSpec::new("joint.create", "Joint", joint).at("SOLID", "ASSEMBLE").icon("joint").key("J").params(
"type: rigid|revolute|slider|cylindrical|pin_slot|planar|ball; a, b: joint origins ({occurrence, face|circle|point: [x,y,z] in world} | origin name); \
values?: [angles in deg or expressions, distances]; limits?: [[min, max] | null…]; flip?: bool; offset?; angle?; name?",
values?: [angles in deg or expressions, distances]; limits?: [[min, max] | null…]; flip?: bool (default false: the origins are mated, \
b's z against a's as two faces meet; true: b's z along a's, so two origins given at one point with the same z stay as they are); \
offset?; angle?; name?; quiet?: bool (leave out dof and fixed)",
),
CommandSpec::new("joint.as_built", "As-Built Joint", as_built).at("SOLID", "ASSEMBLE").icon("as_built").params(
"type, a: occurrence, b: occurrence (stay where they are); at?: joint origin (default: b's origin) — the joint's frame",
"type, a: occurrence, b: occurrence (stay where they are); at?: joint origin (default: b's origin) — the joint's frame; \
name?; limits?: [[min, max] | null…]; quiet?: bool",
),
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; the other joints of a \
closed loop follow, and the motion stops where the loop can no longer close → stopped_by_loop)",
"joint: id|name; value | values: [angles in deg or expressions, distances] (values not given keep theirs; 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); quiet?: bool (leave out dof \
and fixed) → values: [{name, value, unit: deg|mm, limits}] as joint.list gives them; moved: whether any occurrence moved; \
reason: why the joint can move nothing (suppressed, its origins lost, or its chain joined to nothing grounded → unconnected: \
the occurrence named)",
),
CommandSpec::new("joint.motion_link", "Motion Link", motion_link)
.at("SOLID", "ASSEMBLE")
@@ -37,7 +43,10 @@ pub static COMMANDS: &[CommandSpec] = &[
CommandSpec::new("joint.limits", "Joint Limits", limits).params("joint; index?: value index (default 0); min?, max? (omit both to clear)"),
CommandSpec::new("joint.edit", "Edit Joint", edit).params("joint; type?, flip?, offset?, angle?, suppressed?, name?"),
CommandSpec::new("joint.delete", "Delete Joint", delete).params("joint: id|name (or origin: name, link: index)"),
CommandSpec::new("joint.list", "List Joints", list).noundo().params("→ joints (type, occurrences, values, limits), origins, links, degrees of freedom, conflicts"),
CommandSpec::new("joint.list", "List Joints", list).noundo().params(
"→ joints (type, occurrences, values, limits), origins, links, degrees of freedom, conflicts, floating: [{occurrences, joints}] \
(joint chains joined to nothing grounded: the solver places none of them, so driving them moves nothing)",
),
CommandSpec::new("joint.solve", "Solve Joints", solve_cmd).params("re-place the occurrences from the joints → conflicts, errors, dof"),
CommandSpec::new("inspect.interference", "Interference", interference).at("SOLID", "INSPECT").icon("interference").noundo().params(
"bodies?: [names] (default: all, in world placement) → interferences: pairs that overlap with the overlap volume, area, bounding box and centre; unchecked: pairs that could not be checked, with the reason. Bodies are pruned by bounding box first, so any number of bodies is fine; at most 20000 pairs whose boxes overlap are intersected per call (truncated: true and skipped_pairs: n say when more were left out)",
@@ -64,13 +73,93 @@ pub fn resolve_holding(s: &mut Session, held: &joints::Held) -> joints::Solution
sol
}
fn sol_json(sol: &joints::Solution) -> Value {
json!({
/// A solve's answer; `quiet: true` in the command's params leaves out the per-occurrence lists.
fn sol_json(sol: &joints::Solution, p: &Value) -> Value {
let mut out = json!({
"conflicts": sol.conflicts.iter().map(|(id, n, w)| json!({"joint": id, "name": n, "why": w})).collect::<Vec<_>>(),
"errors": sol.errors.iter().map(|(id, n, w)| json!({"joint": id, "name": n, "why": w})).collect::<Vec<_>>(),
"dof": sol.dof.iter().map(|(o, d)| json!({"occurrence": o, "dof": d})).collect::<Vec<_>>(),
"fixed": sol.fixed,
})
});
if !bool_(p, "quiet").unwrap_or(false) {
out["dof"] = sol.dof.iter().map(|(o, d)| json!({"occurrence": o, "dof": d})).collect();
out["fixed"] = json!(sol.fixed);
}
out
}
/// A joint's values as `joint.list` gives them: degrees for angles, mm for distances, each with
/// its name and limits.
pub(crate) fn named_values(j: &Joint, q: &[f64]) -> Vec<Value> {
j.kind
.dofs()
.iter()
.enumerate()
.map(|(i, n)| {
let (v, angle) = (q.get(i).copied().unwrap_or(0.0), j.kind.is_angle(i));
json!({"name": n, "value": if angle { v.to_degrees() } else { v }, "unit": if angle { "deg" } else { "mm" },
"limits": j.limits.get(i).copied().flatten().map(|(a, b)| if angle { [a.to_degrees(), b.to_degrees()] } else { [a, b] })})
})
.collect()
}
fn occurrence_name(s: &Session, id: u64) -> String {
s.doc.occurrences.iter().find(|o| o.id == id).map(|o| o.name.clone()).unwrap_or_else(|| id.to_string())
}
/// Why driving joint `id` moves nothing whatever its values (`None`: it places what it joins),
/// with the occurrence that is joined to nothing grounded when that is the reason.
pub(crate) fn cannot_move(s: &Session, sol: &joints::Solution, id: u64) -> Option<(String, Option<String>)> {
let j = s.doc.assembly.joints.iter().find(|j| j.id == id)?;
// Its value carries through motion links to joints that may move (a suppressed dial too).
if s.doc.assembly.links.iter().any(|l| l.a == id || l.b == id) {
return None;
}
if j.suppressed {
return Some((format!("{} is suppressed", j.name), None));
}
if let Some((_, _, why)) = sol.errors.iter().find(|e| e.0 == id) {
return Some((why.clone(), None));
}
// The solver places what it reaches from the root and grounded occurrences.
let placed = |o: u64| o == 0 || sol.world.contains_key(&o);
if placed(j.a.occurrence) || placed(j.b.occurrence) {
return None;
}
let name = occurrence_name(s, j.a.occurrence);
Some((format!("{name} is not connected to anything grounded (ground it, or join it to a grounded part)"), Some(name)))
}
/// Joint chains joined to nothing grounded, which the solver places none of: each chain's
/// occurrences and joints, by name.
fn floating_chains(s: &Session, sol: &joints::Solution) -> Vec<Value> {
let placed = |o: u64| o == 0 || sol.world.contains_key(&o);
let errors: BTreeSet<u64> = sol.errors.iter().map(|e| e.0).collect();
let mut next: BTreeMap<u64, Vec<(u64, &Joint)>> = BTreeMap::new();
for j in s.doc.assembly.joints.iter().filter(|j| !j.suppressed && !errors.contains(&j.id)) {
if !placed(j.a.occurrence) && !placed(j.b.occurrence) {
next.entry(j.a.occurrence).or_default().push((j.b.occurrence, j));
next.entry(j.b.occurrence).or_default().push((j.a.occurrence, j));
}
}
let mut seen = BTreeSet::new();
let mut out = Vec::new();
for &start in next.keys() {
if !seen.insert(start) {
continue;
}
let (mut occs, mut js, mut todo) = (vec![start], BTreeSet::new(), vec![start]);
while let Some(o) = todo.pop() {
for (other, j) in next.get(&o).into_iter().flatten() {
js.insert((j.id, j.name.as_str()));
if seen.insert(*other) {
occs.push(*other);
todo.push(*other);
}
}
}
out.push(json!({"occurrences": occs.iter().map(|o| occurrence_name(s, *o)).collect::<Vec<_>>(),
"joints": js.iter().map(|(_, n)| *n).collect::<Vec<_>>()}));
}
out
}
pub(crate) fn occurrence_id(s: &Session, v: Option<&Value>, cmd: &str) -> Result<u64> {
@@ -225,7 +314,7 @@ fn next_joint_id(s: &Session) -> u64 {
s.doc.assembly.joints.iter().map(|j| j.id).max().unwrap_or(0) + 1
}
fn add_joint(s: &mut Session, j: Joint) -> Result<Value> {
fn add_joint(s: &mut Session, j: Joint, p: &Value) -> Result<Value> {
if s.doc.assembly.joints.len() >= 10_000 {
return Err(EngineError::Other("too many joints".into()));
}
@@ -238,7 +327,7 @@ fn add_joint(s: &mut Session, j: Joint) -> Result<Value> {
s.visibility.hidden_joints.retain(|h| *h != id);
s.doc_mut().assembly.joints.push(j);
let sol = resolve(s);
let mut out = sol_json(&sol);
let mut out = sol_json(&sol, p);
out["joint"] = json!(id);
out["name"] = json!(name);
Ok(out)
@@ -292,7 +381,7 @@ fn joint(s: &mut Session, p: &Value) -> Result<Value> {
angle,
suppressed: false,
};
add_joint(s, j)
add_joint(s, j, p)
}
fn as_built(s: &mut Session, p: &Value) -> Result<Value> {
@@ -325,7 +414,7 @@ fn as_built(s: &mut Session, p: &Value) -> Result<Value> {
angle: 0.0,
suppressed: false,
};
add_joint(s, j)
add_joint(s, j, p)
}
fn rigid_group(s: &mut Session, p: &Value) -> Result<Value> {
@@ -357,26 +446,32 @@ fn drive(s: &mut Session, p: &Value) -> Result<Value> {
}
let doc = s.doc_mut();
let Some(j) = doc.assembly.joints.iter_mut().find(|j| j.id == id) else { return Err(bad(cmd, "joint")) };
// Values not given keep theirs.
if let Some(Value::Array(a)) = p.get("values") {
for (i, v) in q.iter_mut().enumerate().skip(a.len()) {
*v = j.values.get(i).copied().unwrap_or(*v);
}
}
joints::clamp(j, &mut 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.
// Values not given (after `value`'s first, or past the end of `values`) keep theirs.
let given = match p.get("values") {
Some(Value::Array(a)) => a.len(),
_ => 1,
};
for (i, v) in q.iter_mut().enumerate().skip(given) {
*v = j.values.get(i).copied().unwrap_or(*v);
}
joints::clamp(j, &mut 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 before = super::motion::positions(s);
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);
let mut out = sol_json(&sol, p);
out["joint"] = json!(id);
out["values"] = json!(s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| j.values.clone()));
out["values"] = json!(s.doc.assembly.joints.iter().find(|j| j.id == id).map(|j| named_values(j, &j.values)));
out["stopped_by_contact"] = r["stopped_by_contact"].clone();
out["stopped_by_loop"] = r["stopped_by_loop"].clone();
out["moved"] = json!(super::motion::moved(&before, &super::motion::positions(s)).next().is_some());
if let Some((why, unconnected)) = cannot_move(s, &sol, id) {
out["reason"] = json!(why);
if let Some(o) = unconnected {
out["unconnected"] = json!(o);
}
}
Ok(out)
}
@@ -395,7 +490,7 @@ fn motion_link(s: &mut Session, p: &Value) -> Result<Value> {
let offset = num(p, "offset").unwrap_or(0.0);
s.doc_mut().assembly.links.push(MotionLink { a, ia, b, ib, ratio, offset });
let sol = resolve(s);
Ok(sol_json(&sol))
Ok(sol_json(&sol, p))
}
fn limits(s: &mut Session, p: &Value) -> Result<Value> {
@@ -422,7 +517,7 @@ fn limits(s: &mut Session, p: &Value) -> Result<Value> {
joints::clamp(j, &mut q);
j.values = q;
let sol = resolve(s);
Ok(sol_json(&sol))
Ok(sol_json(&sol, p))
}
fn edit(s: &mut Session, p: &Value) -> Result<Value> {
@@ -463,7 +558,7 @@ fn edit(s: &mut Session, p: &Value) -> Result<Value> {
j.name = n.to_string();
}
let sol = resolve(s);
Ok(sol_json(&sol))
Ok(sol_json(&sol, p))
}
fn delete(s: &mut Session, p: &Value) -> Result<Value> {
@@ -492,12 +587,12 @@ fn delete(s: &mut Session, p: &Value) -> Result<Value> {
doc.assembly.joints.retain(|j| j.id != id);
doc.assembly.links.retain(|l| l.a != id && l.b != id);
let sol = resolve(s);
let mut out = sol_json(&sol);
let mut out = sol_json(&sol, p);
out["deleted"] = json!(id);
Ok(out)
}
fn list(s: &mut Session, _p: &Value) -> Result<Value> {
fn list(s: &mut Session, p: &Value) -> Result<Value> {
let st = s.model.state();
let sol = joints::solve(&s.doc, &st);
let vals = joints::joint_values(&s.doc.assembly);
@@ -507,18 +602,7 @@ fn list(s: &mut Session, _p: &Value) -> Result<Value> {
.joints
.iter()
.map(|j| {
let q = vals.get(&j.id).cloned().unwrap_or_default();
let named: Vec<Value> = j
.kind
.dofs()
.iter()
.enumerate()
.map(|(i, n)| {
let v = q.get(i).copied().unwrap_or(0.0);
json!({"name": n, "value": if j.kind.is_angle(i) { v.to_degrees() } else { v }, "unit": if j.kind.is_angle(i) { "deg" } else { "mm" },
"limits": j.limits.get(i).copied().flatten().map(|(a, b)| if j.kind.is_angle(i) { [a.to_degrees(), b.to_degrees()] } else { [a, b] })})
})
.collect();
let named = named_values(j, vals.get(&j.id).map(Vec::as_slice).unwrap_or_default());
let world = |o: &JointOrigin| s.doc.origin_world(&st, o).ok().map(|m| json!({"origin": [m[3][0], m[3][1], m[3][2]], "z": [m[2][0], m[2][1], m[2][2]]}));
json!({"id": j.id, "name": j.name, "type": j.kind, "a": j.a.occurrence, "b": j.b.occurrence, "values": named,
"flip": j.flip, "offset": j.offset, "suppressed": j.suppressed, "visible": s.joint_visible(j.id), "frame_a": world(&j.a), "frame_b": world(&j.b)})
@@ -527,16 +611,17 @@ fn list(s: &mut Session, _p: &Value) -> Result<Value> {
let links: Vec<Value> =
s.doc.assembly.links.iter().map(|l| json!({"a": l.a, "ia": l.ia, "b": l.b, "ib": l.ib, "ratio": l.ratio, "offset": l.offset})).collect();
let origins: Vec<Value> = s.doc.assembly.origins.iter().map(|o| json!({"name": o.name, "occurrence": o.origin.occurrence})).collect();
let mut out = sol_json(&sol);
let mut out = sol_json(&sol, p);
out["joints"] = json!(js);
out["links"] = json!(links);
out["origins"] = json!(origins);
out["floating"] = json!(floating_chains(s, &sol));
Ok(out)
}
fn solve_cmd(s: &mut Session, _p: &Value) -> Result<Value> {
fn solve_cmd(s: &mut Session, p: &Value) -> Result<Value> {
let sol = resolve(s);
Ok(sol_json(&sol))
Ok(sol_json(&sol, p))
}
fn interference(s: &mut Session, p: &Value) -> Result<Value> {
+90 -7
View File
@@ -95,7 +95,7 @@ fn slider_limits_clamp() {
// B now sits on A.
assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 10.0)), "{:?}", place(&s, "B", Vec3::new(25.0, 5.0, 0.0)));
let r = run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 25}));
assert_eq!(r["values"], json!([10.0]), "{r}");
assert_eq!((&r["values"][0]["value"], &r["values"][0]["unit"]), (&json!(10.0), &json!("mm")), "{r}");
assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 20.0)));
run(&mut s, "joint.drive", json!({"joint": "Rail", "value": -5}));
assert!(near(place(&s, "B", Vec3::new(25.0, 5.0, 0.0)), Vec3::new(5.0, 5.0, 10.0)));
@@ -361,8 +361,8 @@ fn four_bar_follows_its_crank() {
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}");
// The driven joint keeps exactly the value asked for, answered in degrees as asked.
assert!((r["values"][0]["value"].as_f64().unwrap_or(f64::NAN) - v).abs() < 1e-9 && r["values"][0]["unit"] == "deg", "{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);
@@ -399,7 +399,7 @@ fn four_bar_rocker_stops_where_the_loop_cannot_close() {
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();
let reached = r["values"][0]["value"].as_f64().unwrap_or(f64::NAN);
// |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}°");
@@ -413,7 +413,7 @@ fn four_bar_rocker_stops_where_the_loop_cannot_close() {
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}");
assert!(r["values"][0]["value"].as_f64().unwrap_or(f64::NAN) <= limit, "{r}");
}
/// A loop value a motion link sets follows through the start of its link chain: a dial outside
@@ -550,7 +550,7 @@ fn a_locked_loop_does_not_move() {
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}");
assert!(r["values"][0]["value"].as_f64().unwrap_or(f64::NAN).to_radians().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
@@ -576,7 +576,7 @@ fn a_full_turn_stops_from_a_suppressed_dial_or_a_missing_value() {
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}");
assert!(r["stopped_by_loop"].is_object() && r["moved"] == true && r.get("reason").is_none(), "{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}));
@@ -955,3 +955,86 @@ fn a_link_chain_stored_backwards_drives_a_loop() {
assert!(near(place(&s, "Rocker", q0), rocker_pin(0.0, v, q0)), "{v}: {:?}", place(&s, "Rocker", q0));
}
}
/// A turret on a pedestal joined to nothing grounded (Rover v7's turret): the solver places
/// neither, so driving the turret moves nothing. The drive, a motion study and joint.list say
/// so, naming the pedestal, instead of answering as if it worked.
#[test]
fn a_turret_on_a_floating_pedestal_says_it_moves_nothing() {
let mut s = Session::default();
for (name, corner) in [("Chassis", [0.0, 0.0, 0.0]), ("Arm", [10.0, 0.0, 0.0]), ("Pedestal", [50.0, 0.0, 0.0]), ("Turret", [50.0, 0.0, 2.0])] {
part(&mut s, name, corner);
}
let g = occ(&s, "Chassis");
run(&mut s, "occurrence.ground", json!({"occurrence": g, "grounded": true}));
pin(&mut s, "revolute", "Shoulder", "Chassis", "Arm", Vec3::new(10.0, 1.0, 1.0), Vec3::Y);
pin(&mut s, "revolute", "Azimuth", "Pedestal", "Turret", Vec3::new(51.0, 1.0, 2.0), Vec3::Z);
let corner = Vec3::new(52.0, 0.0, 2.0);
let start = place(&s, "Turret", corner);
let r = run(&mut s, "joint.drive", json!({"joint": "Azimuth", "value": 30, "quiet": true}));
assert_eq!((&r["moved"], &r["unconnected"]), (&json!(false), &json!("Pedestal:1")), "{r}");
assert!(r["reason"].as_str().is_some_and(|w| w.starts_with("Pedestal:1 is not connected to anything grounded")), "{r}");
assert!(r.get("dof").is_none() && r.get("fixed").is_none(), "quiet: {r}");
// The value is set and answered in degrees, as asked; the turret stays where it was.
assert!((r["values"][0]["value"].as_f64().unwrap_or(f64::NAN) - 30.0).abs() < 1e-9 && r["values"][0]["unit"] == "deg", "{r}");
assert!(near(place(&s, "Turret", corner), start));
let l = run(&mut s, "joint.list", json!({}));
assert_eq!(l["floating"], json!([{"occurrences": ["Pedestal:1", "Turret:1"], "joints": ["Azimuth"]}]), "{l}");
// A study keying both joints moves the arm and says the turret can't move.
let keys = json!([{"joint": "Azimuth", "points": [[0, 0], [10, 90]]}, {"joint": "Shoulder", "points": [[0, 0], [10, 45]]}]);
run(&mut s, "motion.study", json!({"name": "Scan", "steps": 10, "keys": keys}));
let r = run(&mut s, "motion.play", json!({"study": "Scan", "step": 10, "quiet": true}));
assert_eq!(r["moved"], true, "{r}");
let unmoved = r["unmoved"].as_array().cloned().unwrap_or_default();
assert!(unmoved.len() == 1 && unmoved[0]["name"] == "Azimuth" && unmoved[0]["reason"].as_str().is_some_and(|w| w.contains("Pedestal:1")), "{r}");
// Quiet: only the arm, which moved, has its pose in the answer.
assert_eq!(r["positions"].as_array().map(|p| p.iter().map(|x| x["occurrence"].clone()).collect::<Vec<_>>()), Some(vec![json!("Arm:1")]), "{r}");
// Fixed to the chassis, the pedestal is placed and the turret turns.
run(&mut s, "joint.as_built", json!({"type": "rigid", "a": "Chassis", "b": "Pedestal", "name": "Mount"}));
let r = run(&mut s, "joint.drive", json!({"joint": "Azimuth", "value": 60}));
assert!(r["moved"] == true && r.get("reason").is_none() && r.get("unconnected").is_none() && r["dof"].is_array(), "{r}");
assert!(!near(place(&s, "Turret", corner), start));
assert_eq!(run(&mut s, "joint.list", json!({}))["floating"], json!([]));
}
/// `quiet: true` leaves the per-occurrence lists out of joint answers, and `moved` says whether
/// a drive moved anything: driving a joint to where it is moves nothing, with no reason given.
#[test]
fn quiet_joint_answers_and_a_drive_to_where_it_is() {
let mut s = two_cubes();
let r =
run(&mut s, "joint.create", json!({"type": "slider", "a": {"face": [5, 5, 10]}, "b": {"face": [25, 5, 0]}, "name": "Rail", "quiet": true}));
assert!(r.get("dof").is_none() && r.get("fixed").is_none() && r["conflicts"].is_array(), "{r}");
let r = run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 5, "quiet": true}));
assert!(r.get("dof").is_none() && r["moved"] == true, "{r}");
let r = run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 5}));
assert!(r["dof"].is_array() && r["moved"] == false && r.get("reason").is_none(), "{r}");
// A suppressed joint with no motion link moves nothing, and says why.
run(&mut s, "joint.edit", json!({"joint": "Rail", "suppressed": true}));
let r = run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 8}));
assert!(r["moved"] == false && r["reason"] == "Rail is suppressed" && r.get("unconnected").is_none(), "{r}");
}
/// An as-built joint takes a name and limits like any other joint.
#[test]
fn as_built_takes_a_name_and_limits() {
let mut s = two_cubes();
let r = run(&mut s, "joint.as_built", json!({"type": "slider", "a": "A", "b": "B", "name": "Rail", "limits": [[0, 5]]}));
assert_eq!(r["name"], "Rail", "{r}");
let r = run(&mut s, "joint.drive", json!({"joint": "Rail", "value": 20}));
assert_eq!((&r["values"][0]["value"], &r["values"][0]["limits"]), (&json!(5.0), &json!([0.0, 5.0])), "{r}");
assert!(near(place(&s, "B", Vec3::new(20.0, 0.0, 0.0)), Vec3::new(20.0, 0.0, 5.0)), "{:?}", place(&s, "B", Vec3::new(20.0, 0.0, 0.0)));
}
/// `value` drives a joint's first value only: the others keep theirs, as with a shorter `values`.
#[test]
fn a_single_value_keeps_the_joints_other_values() {
let mut s = two_cubes();
run(&mut s, "joint.as_built", json!({"type": "cylindrical", "a": "A", "b": "B", "name": "Spindle"}));
run(&mut s, "joint.drive", json!({"joint": "Spindle", "values": [30, 5]}));
let at = |r: &Value| [0, 1].map(|i| r["values"][i]["value"].as_f64().unwrap_or(f64::NAN));
let r = run(&mut s, "joint.drive", json!({"joint": "Spindle", "value": 45}));
assert!((at(&r)[0] - 45.0).abs() < 1e-9 && (at(&r)[1] - 5.0).abs() < 1e-9, "{r}");
let r = run(&mut s, "joint.drive", json!({"joint": "Spindle", "values": [60]}));
assert!((at(&r)[0] - 60.0).abs() < 1e-9 && (at(&r)[1] - 5.0).abs() < 1e-9, "{r}");
}
+38 -12
View File
@@ -10,7 +10,7 @@ use solidcraft_geom::Vec3;
use solidcraft_kernel::Body;
use super::CommandSpec;
use super::joints::{joint_id, occurrence_id, resolve_holding, value};
use super::joints::{cannot_move, joint_id, named_values, occurrence_id, resolve_holding, value};
use crate::params::{bad, bool_, str_, vec3};
use crate::{EngineError, Result, Session};
@@ -37,8 +37,12 @@ pub static COMMANDS: &[CommandSpec] = &[
"name?; study? (edit an existing one); steps? (timeline length, default 100); keys: [{joint, index?, points: [[step, value], …]}] \
(values: degrees for angles, mm for distances, or expressions)",
),
CommandSpec::new("motion.play", "Play Motion Study", play)
.params("study: id|name; step (0…steps) — sets the joints to the study's values there (contact sets stop them) → positions"),
CommandSpec::new("motion.play", "Play Motion Study", play).params(
"study: id|name; step (0…steps) — sets the joints to the study's values there (contact sets stop them); quiet?: bool \
(positions of only the occurrences that moved) → values: [{joint, values: [{name, value, unit}]}], positions, moved: whether \
any occurrence moved, unmoved: [{joint, name, reason}] (keyed joints that can move nothing, such as a chain joined to \
nothing grounded)",
),
CommandSpec::new("motion.export", "Export Motion Study", export)
.noundo()
.params("study; samples? (default steps + 1); path? (.json or .csv) → occurrence positions per step (the design is not changed)"),
@@ -258,8 +262,9 @@ fn closest(p: Vec3, a: Vec3, b: Vec3, c: Vec3) -> Vec3 {
/// 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> {
/// `given` values (joint, index) are driven; in a closed loop the others follow. Returns the
/// answer and the last solve.
fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)], given: &BTreeSet<(u64, usize)>) -> Result<(Value, joints::Solution)> {
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();
@@ -347,15 +352,16 @@ fn drive_with_contact(s: &mut Session, want: &[(u64, Vec<f64>)], given: &BTreeSe
}
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())}))
.map(|(id, _)| json!({"joint": id, "values": s.doc.assembly.joints.iter().find(|j| j.id == *id).map(|j| named_values(j, &j.values))}))
.collect();
Ok(json!({"values": values, "stopped_by_contact": stopped, "stopped_by_loop": stopped_by_loop, "conflicts": sol.conflicts.len()}))
let out = json!({"values": values, "stopped_by_contact": stopped, "stopped_by_loop": stopped_by_loop, "conflicts": sol.conflicts.len()});
Ok((out, sol))
}
/// Drive one joint (joint.drive), honouring contact sets.
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)
Ok(drive_with_contact(s, &[(id, q)], &given)?.0)
}
fn enable_sets(s: &mut Session, p: &Value) -> Result<Value> {
@@ -556,10 +562,16 @@ fn keyed(m: &MotionStudy) -> BTreeSet<(u64, usize)> {
}
/// Where each occurrence is: (name, world transform).
fn positions(s: &Session) -> Vec<(String, Mat)> {
pub(super) fn positions(s: &Session) -> Vec<(String, Mat)> {
s.doc.occurrences.iter().map(|o| (o.name.clone(), joints::occurrence_world(&s.doc, o.id))).collect()
}
/// The occurrences placed differently `after` than `before` ([`positions`] of one design, so in
/// the same order even where two share a name).
pub(super) fn moved<'a>(before: &'a [(String, Mat)], after: &'a [(String, Mat)]) -> impl Iterator<Item = &'a (String, Mat)> {
after.iter().zip(before).filter(|((_, a), (_, b))| a.iter().flatten().zip(b.iter().flatten()).any(|(x, y)| (x - y).abs() > 1e-9)).map(|(a, _)| a)
}
fn pose_json(m: &Mat) -> Value {
json!({"origin": [m[3][0], m[3][1], m[3][2]], "x": [m[0][0], m[0][1], m[0][2]], "y": [m[1][0], m[1][1], m[1][2]], "z": [m[2][0], m[2][1], m[2][2]]})
}
@@ -570,8 +582,22 @@ 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, &keyed(&m))?;
out["positions"] = positions(s).iter().map(|(n, t)| json!({"occurrence": n, "pose": pose_json(t)})).collect();
let before = positions(s);
let (mut out, sol) = drive_with_contact(s, &want, &keyed(&m))?;
let after = positions(s);
let moved: Vec<&(String, Mat)> = moved(&before, &after).collect();
out["moved"] = json!(!moved.is_empty());
let shown: Vec<&(String, Mat)> = if bool_(p, "quiet").unwrap_or(false) { moved } else { after.iter().collect() };
out["positions"] = shown.iter().map(|(n, t)| json!({"occurrence": n, "pose": pose_json(t)})).collect();
let joints: BTreeSet<u64> = m.keys.iter().map(|k| k.joint).collect();
out["unmoved"] = joints
.iter()
.filter_map(|id| {
let (why, _) = cannot_move(s, &sol, *id)?;
let name = s.doc.assembly.joints.iter().find(|j| j.id == *id).map(|j| j.name.clone());
Some(json!({"joint": id, "name": name, "reason": why}))
})
.collect();
Ok(out)
}
@@ -589,7 +615,7 @@ 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, &keyed(&m))?;
let (r, _) = drive_with_contact(&mut t, &want, &keyed(&m))?;
rows.push(json!({
"step": step,
"stopped_by_contact": r["stopped_by_contact"],