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:
1 parent
fd19efc6fc
commit
7195e323bf
15 files changed
+326
-79
No files matched your search
+131
-46
@@ -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> {
|
||||
|
||||
@@ -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}");
|
||||
}
|
||||
@@ -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"],
|
||||
|
||||
Reference in new issue
Block a user