Joints: relations that tie part of a placement (#2497)
Joints can tie part of a placement: parallel, angle, point on plane and more (#2496).
This commit is contained in:
@@ -0,0 +1,6 @@
|
||||
//! A joint can be a relation that ties only some directions or points of its two parts:
|
||||
//! `parallel`, `perpendicular`, `angle` (its `angle` is then the tilt between the two z axes),
|
||||
//! `point_on_plane`, `point_on_line` and `line_on_plane`, each with the values its free motion
|
||||
//! needs. Several relations between one pair of parts close loops the solver meets together.
|
||||
//! Older designs need no transformation; the last release asks for an update instead of meeting
|
||||
//! a joint type it can't read.
|
||||
+107
-11
@@ -1,11 +1,13 @@
|
||||
//! Joints between occurrences: joint origins (snap frames on component geometry, re-found on
|
||||
//! every solve so they follow edits), joints (rigid, revolute, slider, cylindrical, pin-slot,
|
||||
//! planar, ball) with their motion values and limits, motion links (gears, racks and screws are
|
||||
//! planar, ball, and relations that tie only part of a placement, `joints_relations.rs`) with
|
||||
//! their motion values and limits, motion links (gears, racks and screws are
|
||||
//! links whose ratio comes from radii or a pitch; a cam's follower follows a table worked out from
|
||||
//! its profile, `joints_cam.rs`), and the solver that places occurrences from the joint graph.
|
||||
//!
|
||||
//! A joint makes B's joint frame coincide with A's frame moved by the joint's motion:
|
||||
//! `T_B · F_b = T_A · F_a · M(q) · Flip`, so `T_B = T_A · F_a · M(q) · Flip · F_b⁻¹`, where `T`
|
||||
//! `T_B · F_b = T_A · F_a · M(q) · Flip · Tail(q)`, so `T_B = T_A · F_a · M(q) · Flip · Tail(q) · F_b⁻¹`
|
||||
//! (`Tail` is a relation's tilt and turns after the mate; the identity for the others), where `T`
|
||||
//! are occurrence world transforms, `F` the joint frames in each component's own frame, and `q`
|
||||
//! the joint's values (angles in radians, distances in mm). The solver walks the graph from
|
||||
//! grounded occurrences (and the root, which never moves). Joints that close a loop (a four-bar
|
||||
@@ -203,6 +205,20 @@ pub enum JointKind {
|
||||
PinSlot,
|
||||
Planar,
|
||||
Ball,
|
||||
/// B's z stays parallel to A's (against it unless flipped); B moves and turns about z freely.
|
||||
Parallel,
|
||||
/// B's z stays square to A's; B moves freely and turns about either z.
|
||||
Perpendicular,
|
||||
/// B's z stays at the joint's `angle` from mated (about their common x); B moves freely and
|
||||
/// turns about either z.
|
||||
Angle,
|
||||
/// B's origin stays on A's xy plane (`offset` off it); B slides on it and turns any way.
|
||||
PointOnPlane,
|
||||
/// B's origin stays on A's z axis; B slides along it and turns any way.
|
||||
PointOnLine,
|
||||
/// B's z axis lies in A's xy plane (`offset` off it: a cylinder's radius makes it tangent);
|
||||
/// B slides on the plane, turns about A's z and about its own axis.
|
||||
LineOnPlane,
|
||||
}
|
||||
|
||||
impl JointKind {
|
||||
@@ -215,6 +231,12 @@ impl JointKind {
|
||||
"pinslot" => JointKind::PinSlot,
|
||||
"planar" => JointKind::Planar,
|
||||
"ball" => JointKind::Ball,
|
||||
"parallel" => JointKind::Parallel,
|
||||
"perpendicular" | "square" => JointKind::Perpendicular,
|
||||
"angle" => JointKind::Angle,
|
||||
"pointonplane" | "pointonface" => JointKind::PointOnPlane,
|
||||
"pointonline" | "pointonaxis" => JointKind::PointOnLine,
|
||||
"lineonplane" | "lineonface" => JointKind::LineOnPlane,
|
||||
_ => return None,
|
||||
})
|
||||
}
|
||||
@@ -228,10 +250,29 @@ impl JointKind {
|
||||
JointKind::PinSlot => &["angle", "slide"],
|
||||
JointKind::Planar => &["x", "y", "angle"],
|
||||
JointKind::Ball => &["yaw", "pitch", "roll"],
|
||||
JointKind::Parallel => &["x", "y", "z", "angle"],
|
||||
JointKind::Perpendicular | JointKind::Angle => &["x", "y", "z", "turn", "spin"],
|
||||
JointKind::PointOnPlane => &["x", "y", "yaw", "pitch", "roll"],
|
||||
JointKind::PointOnLine => &["slide", "yaw", "pitch", "roll"],
|
||||
JointKind::LineOnPlane => &["x", "y", "turn", "spin"],
|
||||
}
|
||||
}
|
||||
pub fn is_angle(self, i: usize) -> bool {
|
||||
matches!(self.dofs().get(i), Some(&("angle" | "yaw" | "pitch" | "roll")))
|
||||
matches!(self.dofs().get(i), Some(&("angle" | "yaw" | "pitch" | "roll" | "turn" | "spin")))
|
||||
}
|
||||
/// A relation that only ties some directions or points (parallel, on a plane…): made where
|
||||
/// the parts are, its values start as the nearest that meet it, so B moves no more than it
|
||||
/// must.
|
||||
pub fn is_relation(self) -> bool {
|
||||
matches!(
|
||||
self,
|
||||
JointKind::Parallel
|
||||
| JointKind::Perpendicular
|
||||
| JointKind::Angle
|
||||
| JointKind::PointOnPlane
|
||||
| JointKind::PointOnLine
|
||||
| JointKind::LineOnPlane
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -599,6 +640,10 @@ fn rot_z(a: f64) -> Mat {
|
||||
let (s, c) = a.sin_cos();
|
||||
[[c, s, 0.0, 0.0], [-s, c, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]
|
||||
}
|
||||
fn rot_x(a: f64) -> Mat {
|
||||
let (s, c) = a.sin_cos();
|
||||
[[1.0, 0.0, 0.0, 0.0], [0.0, c, s, 0.0], [0.0, -s, c, 0.0], [0.0, 0.0, 0.0, 1.0]]
|
||||
}
|
||||
fn rot_y(a: f64) -> Mat {
|
||||
let (s, c) = a.sin_cos();
|
||||
[[c, 0.0, -s, 0.0], [0.0, 1.0, 0.0, 0.0], [s, 0.0, c, 0.0], [0.0, 0.0, 0.0, 1.0]]
|
||||
@@ -620,9 +665,40 @@ pub fn motion(kind: JointKind, q: &[f64]) -> Mat {
|
||||
JointKind::PinSlot => mat_mul(&shift(v(1), 0.0, 0.0), &rot_z(v(0))),
|
||||
JointKind::Planar => mat_mul(&shift(v(0), v(1), 0.0), &rot_z(v(2))),
|
||||
JointKind::Ball => mat_mul(&mat_mul(&rot_z(v(0)), &rot_y(v(1))), &rot_z(v(2))),
|
||||
JointKind::Parallel => mat_mul(&shift(v(0), v(1), v(2)), &rot_z(v(3))),
|
||||
JointKind::Perpendicular | JointKind::Angle => mat_mul(&shift(v(0), v(1), v(2)), &rot_z(v(3))),
|
||||
// Their turn is about B's origin, after the offset (`tail`).
|
||||
JointKind::PointOnPlane => shift(v(0), v(1), 0.0),
|
||||
JointKind::PointOnLine => shift(0.0, 0.0, v(0)),
|
||||
JointKind::LineOnPlane => mat_mul(&shift(v(0), v(1), 0.0), &rot_z(v(2))),
|
||||
}
|
||||
}
|
||||
|
||||
fn turn_zyx(yaw: f64, pitch: f64, roll: f64) -> Mat {
|
||||
mat_mul(&mat_mul(&rot_z(yaw), &rot_y(pitch)), &rot_x(roll))
|
||||
}
|
||||
|
||||
/// What follows the mate (`flip_of`) in a joint's link: for a relation that tilts B's z, the tilt
|
||||
/// and B's turn about its own z (`spin`); for a point relation, B's turn about its origin (yaw,
|
||||
/// pitch and roll about z, y and x: free of gimbal lock at the start, all 0); nothing for the
|
||||
/// other joints.
|
||||
fn tail(j: &Joint, q: &[f64]) -> Mat {
|
||||
let v = |i: usize| q.get(i).copied().unwrap_or(0.0);
|
||||
match j.kind {
|
||||
JointKind::Perpendicular => mat_mul(&rot_x(std::f64::consts::FRAC_PI_2), &rot_z(v(4))),
|
||||
JointKind::Angle => mat_mul(&rot_x(j.angle), &rot_z(v(4))),
|
||||
JointKind::LineOnPlane => mat_mul(&rot_y(std::f64::consts::FRAC_PI_2), &rot_z(v(3))),
|
||||
JointKind::PointOnPlane => turn_zyx(v(2), v(3), v(4)),
|
||||
JointKind::PointOnLine => turn_zyx(v(1), v(2), v(3)),
|
||||
_ => IDENTITY,
|
||||
}
|
||||
}
|
||||
|
||||
/// B's joint frame in A's for the joint's values: `M(q) · Flip · tail`.
|
||||
fn link_of(j: &Joint, q: &[f64]) -> Mat {
|
||||
mat_mul(&mat_mul(&motion(j.kind, q), &flip_of(j)), &tail(j, q))
|
||||
}
|
||||
|
||||
/// Bodies of a component in the model (its own frame).
|
||||
fn component_bodies<'a>(doc: &Document, st: &'a ModelState, component: u64) -> Vec<&'a crate::ModelBody> {
|
||||
st.bodies.iter().filter(|b| doc.model_body_component(b) == component).collect()
|
||||
@@ -854,14 +930,16 @@ pub fn joint_values(asm: &Assembly) -> BTreeMap<u64, Vec<f64>> {
|
||||
/// The turn and offset from A's joint frame to B's: mated (B's z against A's, as faces meet)
|
||||
/// unless flipped.
|
||||
fn flip_of(j: &Joint) -> Mat {
|
||||
let mut m = rot_z(j.angle);
|
||||
// An angle relation's `angle` is its tilt (`tail`); a perpendicular one has none.
|
||||
let turn = if matches!(j.kind, JointKind::Angle | JointKind::Perpendicular) { 0.0 } else { j.angle };
|
||||
let mut m = rot_z(turn);
|
||||
m[3][2] = j.offset;
|
||||
if !j.flip { mat_mul(&m, &[[1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) } else { m }
|
||||
}
|
||||
|
||||
/// B's world transform from A's (or A's from B's, going the other way) for the joint's values.
|
||||
fn through(j: &Joint, (fa, fb): &(Mat, Mat), q: &[f64], from_a: bool, t: &Mat) -> Option<Mat> {
|
||||
let link = mat_mul(&mat_mul(&motion(j.kind, q), &flip_of(j)), &mat_inverse(fb)?);
|
||||
let link = mat_mul(&link_of(j, q), &mat_inverse(fb)?);
|
||||
if from_a {
|
||||
Some(mat_mul(&mat_mul(t, fa), &link))
|
||||
} else {
|
||||
@@ -928,6 +1006,10 @@ impl Tree<'_> {
|
||||
/// on at most: larger ones are only checked.
|
||||
const MAX_LOOP_VALUES: usize = 64;
|
||||
const MAX_LOOPS: usize = 100;
|
||||
/// Loop-closing joints one solve takes on at most, over all its groups: a part placed by several
|
||||
/// relations closes a loop with each one after the first, so a panel of parts has many small
|
||||
/// groups.
|
||||
const MAX_CLOSING: usize = 5_000;
|
||||
/// Residual evaluations one solve may spend on all its loops.
|
||||
const MAX_LOOP_WORK: usize = 200_000;
|
||||
|
||||
@@ -984,13 +1066,13 @@ fn held_moves(asm: &Assembly, held: &Held, touches: &BTreeSet<(u64, usize)>, val
|
||||
/// moves; a group that can't close without moving is left to the conflict check).
|
||||
fn close_loops(tree: &Tree, closing: &[u64], asm: &Assembly, held: &Held, vals: &mut Values) -> (Values, BTreeSet<u64>) {
|
||||
let (mut changed, mut stuck) = (BTreeMap::new(), BTreeSet::new());
|
||||
if closing.len() > MAX_LOOPS {
|
||||
if closing.len() > MAX_CLOSING {
|
||||
return (Values::new(), stuck);
|
||||
}
|
||||
let linked = set_values(asm);
|
||||
let groups = loop_groups(tree, closing, asm, &linked);
|
||||
let spent = std::cell::Cell::new(0usize);
|
||||
for (group, around, touches) in &groups {
|
||||
for (group, around, touches) in groups.iter().filter(|(c, _, _)| c.len() <= MAX_LOOPS) {
|
||||
let moves = held_moves(asm, held, touches, vals);
|
||||
match close_group(tree, group, around, &moves, asm, held, &linked, &spent, vals) {
|
||||
Some(m) => changed.extend(m),
|
||||
@@ -1179,11 +1261,12 @@ fn nearby_pose(tree: &Tree, groups: &[Group], asm: &Assembly, linked: &BTreeSet<
|
||||
/// pose alone counts 0. The tree's count stands where a loop group is over the solver's caps or
|
||||
/// the work runs out.
|
||||
fn loop_dof(tree: &Tree, closing: &[u64], asm: &Assembly, vals: &Values, dof: &mut BTreeMap<u64, usize>) {
|
||||
if closing.len() > MAX_LOOPS {
|
||||
if closing.len() > MAX_CLOSING {
|
||||
return;
|
||||
}
|
||||
let linked = set_values(asm);
|
||||
let groups = loop_groups(tree, closing, asm, &linked);
|
||||
let mut groups = loop_groups(tree, closing, asm, &linked);
|
||||
groups.retain(|(c, _, _)| c.len() <= MAX_LOOPS);
|
||||
// The joints loops depend on: those around them, those whose values they read, and those a
|
||||
// motion link carries any of these values into.
|
||||
let mut relevant: BTreeSet<u64> = groups.iter().flat_map(|(_, a, t)| a.iter().copied().chain(t.iter().map(|(j, _)| *j))).collect();
|
||||
@@ -1592,8 +1675,18 @@ struct Built<'a> {
|
||||
fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Solution) -> Built<'a> {
|
||||
let asm = &doc.assembly;
|
||||
let joints: Vec<&Joint> = asm.joints.iter().filter(|j| !j.suppressed).take(MAX_JOINTS).collect();
|
||||
// Joint frames in component coordinates (errors leave the joint out).
|
||||
// Joint frames in component coordinates (errors leave the joint out). An origin several joints
|
||||
// share (a panel face every part sits on) is found once.
|
||||
let mut frames: BTreeMap<u64, (Mat, Mat)> = BTreeMap::new();
|
||||
let mut found: BTreeMap<(u64, String), std::result::Result<Mat, String>> = BTreeMap::new();
|
||||
let mut resolve = |o: &JointOrigin| -> Result<Mat> {
|
||||
let Ok(comp) = component_of(doc, o.occurrence) else { return resolve_origin(doc, st, o) };
|
||||
found
|
||||
.entry((comp, format!("{:?}", o.snap)))
|
||||
.or_insert_with(|| resolve_origin(doc, st, o).map_err(|e| e.to_string()))
|
||||
.clone()
|
||||
.map_err(DocError::Invalid)
|
||||
};
|
||||
for j in &joints {
|
||||
// A joint whose expressions can't be worked out (a parameter gone) is left out rather
|
||||
// than placed by numbers that no longer say what it was defined by.
|
||||
@@ -1603,7 +1696,7 @@ fn build<'a>(doc: &'a Document, st: &ModelState, vals: &Values, sol: &mut Soluti
|
||||
sol.errors.push((j.id, j.name.clone(), e.to_string()));
|
||||
continue;
|
||||
}
|
||||
match (resolve_origin(doc, st, &j.a), resolve_origin(doc, st, &j.b)) {
|
||||
match (resolve(&j.a), resolve(&j.b)) {
|
||||
(Ok(fa), Ok(fb)) => {
|
||||
frames.insert(j.id, (fa, fb));
|
||||
}
|
||||
@@ -1735,7 +1828,10 @@ mod reach;
|
||||
#[path = "joints_face.rs"]
|
||||
mod face;
|
||||
pub use face::{end_faces, face_name_at, named_face_spot, pin_end, round_end_at};
|
||||
#[path = "joints_relations.rs"]
|
||||
mod relations;
|
||||
pub use reach::{REACH_TOL, Reach, Reached, reach};
|
||||
pub use relations::relation_values;
|
||||
|
||||
/// The rest angle that turns B least as joint `j` is made, as B sits now: none or a half turn
|
||||
/// about the joint's z. Origins whose x is chosen for them (a face, a circle, a point without
|
||||
|
||||
@@ -0,0 +1,130 @@
|
||||
//! Relations: joints that tie only some directions or points of two parts (B parallel to A, B's
|
||||
//! point on A's plane…), so several of them between one pair of parts build up the placement the
|
||||
//! way a designer thinks of it: on that face, square to that edge, 40 mm from that end. The
|
||||
//! solver treats them as joints with more values; a second relation between the same parts closes
|
||||
//! a loop, and the loop solver meets every relation at once (or reports the one that can't be).
|
||||
//!
|
||||
//! A relation is made where the parts are: its values start as the nearest that meet it, so B
|
||||
//! moves no further than the relation needs.
|
||||
|
||||
use super::{Joint, component_bodies, component_of, least_squares, link_of, occurrence_world, resolve_origin};
|
||||
use crate::{Document, IDENTITY, Mat, ModelState, mat_inverse, mat_mul};
|
||||
|
||||
/// Starting turns tried for each angle value: the nearest values may lie a half turn away.
|
||||
const TURNS: [f64; 4] = [0.0, std::f64::consts::FRAC_PI_2, std::f64::consts::PI, -std::f64::consts::FRAC_PI_2];
|
||||
|
||||
/// Starts tried at most (angle values beyond the first three start at 0).
|
||||
const MAX_STARTS: usize = 64;
|
||||
|
||||
/// The values of relation `j` that move B least from where it is now: B's joint frame as it sits
|
||||
/// in A's, met as nearly as the relation allows (its origin as near as it can be to where it is,
|
||||
/// and its axes, each weighed as a point a part's size along it). `None` for a joint that is not a relation, or whose
|
||||
/// origins can't be found.
|
||||
pub fn relation_values(doc: &Document, st: &ModelState, j: &Joint) -> Option<Vec<f64>> {
|
||||
if !j.kind.is_relation() {
|
||||
return None;
|
||||
}
|
||||
let world = |occ: u64| if occ == 0 { IDENTITY } else { occurrence_world(doc, occ) };
|
||||
let fa = mat_mul(&world(j.a.occurrence), &resolve_origin(doc, st, &j.a).ok()?);
|
||||
let fb = mat_mul(&world(j.b.occurrence), &resolve_origin(doc, st, &j.b).ok()?);
|
||||
// B's joint frame in A's as it is.
|
||||
let now = mat_mul(&mat_inverse(&fa)?, &fb);
|
||||
// A turn weighs as far as it moves a point B's size away.
|
||||
let size = component_of(doc, j.b.occurrence).map(|c| component_bodies(doc, st, c).iter().map(|b| b.body.size()).fold(0.0, f64::max));
|
||||
let scale = size.unwrap_or(0.0).clamp(1.0, 1e5);
|
||||
let n = j.kind.dofs().len();
|
||||
let angles: Vec<usize> = (0..n).filter(|i| j.kind.is_angle(*i)).collect();
|
||||
let mut gaps = |q: &[f64]| -> Option<Vec<f64>> {
|
||||
let m: Mat = link_of(j, q);
|
||||
let mut out = Vec::with_capacity(12);
|
||||
for (k, (row, want)) in m.iter().zip(&now).enumerate() {
|
||||
let w = if k == 3 { 1.0 } else { scale };
|
||||
out.extend(row.iter().zip(want).take(3).map(|(a, b)| (a - b) * w));
|
||||
}
|
||||
out.iter().all(|v| v.is_finite()).then_some(out)
|
||||
};
|
||||
let cost = |r: &[f64]| r.iter().map(|v| v * v).sum::<f64>();
|
||||
let bounds = vec![(f64::NEG_INFINITY, f64::INFINITY); n];
|
||||
let mut best: Option<(f64, Vec<f64>)> = None;
|
||||
let tried = angles.len().min(3);
|
||||
for k in 0..TURNS.len().pow(tried as u32).min(MAX_STARTS) {
|
||||
let mut q = vec![0.0; n];
|
||||
let mut code = k;
|
||||
for i in angles.iter().take(tried) {
|
||||
if let (Some(slot), Some(t)) = (q.get_mut(*i), TURNS.get(code % TURNS.len())) {
|
||||
*slot = *t;
|
||||
}
|
||||
code /= TURNS.len();
|
||||
}
|
||||
// Tolerance 0: as near as it gets; `q` keeps the best values reached.
|
||||
least_squares(&mut gaps, &mut q, &bounds, 0.0);
|
||||
let Some(c) = gaps(&q).map(|r| cost(&r)) else { continue };
|
||||
if best.as_ref().is_none_or(|(b, _)| c < *b - 1e-12) {
|
||||
best = Some((c, q));
|
||||
}
|
||||
}
|
||||
// Angles within a half turn of 0.
|
||||
best.map(|(_, mut q)| {
|
||||
for i in &angles {
|
||||
if let Some(v) = q.get_mut(*i) {
|
||||
*v = (*v + std::f64::consts::PI).rem_euclid(std::f64::consts::TAU) - std::f64::consts::PI;
|
||||
}
|
||||
}
|
||||
q
|
||||
})
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
#![allow(clippy::unwrap_used)]
|
||||
use super::*;
|
||||
use crate::joints::{JointKind, JointOrigin, Snap};
|
||||
use solidcraft_geom::Vec3;
|
||||
|
||||
fn joint(kind: JointKind) -> Joint {
|
||||
let at = |occurrence| JointOrigin { occurrence, snap: Snap::Point { point: Vec3::ZERO, z: None } };
|
||||
Joint {
|
||||
id: 1,
|
||||
name: "R".into(),
|
||||
kind,
|
||||
a: at(0),
|
||||
b: at(1),
|
||||
values: Vec::new(),
|
||||
limits: Vec::new(),
|
||||
flip: false,
|
||||
offset: 0.0,
|
||||
angle: 0.6,
|
||||
suppressed: false,
|
||||
exprs: Default::default(),
|
||||
}
|
||||
}
|
||||
|
||||
fn z_of(m: &Mat) -> Vec3 {
|
||||
Vec3::new(m[2][0], m[2][1], m[2][2])
|
||||
}
|
||||
|
||||
/// Whatever its values, each relation keeps what it ties.
|
||||
#[test]
|
||||
fn relations_keep_what_they_tie() {
|
||||
let q = [13.0, -7.0, 21.0, 0.7, -1.9];
|
||||
let at = |kind| {
|
||||
let j = joint(kind);
|
||||
let n = kind.dofs().len();
|
||||
link_of(&j, q.get(..n).unwrap())
|
||||
};
|
||||
// Parallel: z against A's.
|
||||
assert!((z_of(&at(JointKind::Parallel)).z + 1.0).abs() < 1e-12);
|
||||
// Perpendicular: z square to A's.
|
||||
assert!(z_of(&at(JointKind::Perpendicular)).z.abs() < 1e-12);
|
||||
// Angle: 0.6 from mated.
|
||||
assert!((z_of(&at(JointKind::Angle)).z + 0.6f64.cos()).abs() < 1e-12);
|
||||
// Point on plane: the origin on A's plane.
|
||||
assert!(at(JointKind::PointOnPlane)[3][2].abs() < 1e-12);
|
||||
// Point on line: on A's z axis.
|
||||
let m = at(JointKind::PointOnLine);
|
||||
assert!(m[3][0].abs() < 1e-12 && m[3][1].abs() < 1e-12);
|
||||
// Line on plane: the origin on the plane and z along it.
|
||||
let m = at(JointKind::LineOnPlane);
|
||||
assert!(m[3][2].abs() < 1e-12 && z_of(&m).z.abs() < 1e-12);
|
||||
}
|
||||
}
|
||||
@@ -20,11 +20,13 @@ pub static COMMANDS: &[CommandSpec] = &[
|
||||
"name?, occurrence?: id|name (default: from the pick, or the named face's component), face: [x,y,z] | \"<name>\" (centre of a planar face) | circle: [x,y,z] | \"<name>\" (centre of a round end: a point on its wall or rim, the end's centre, or the round face's name as model.faces lists it) | edge: [x,y,z] (a point on an edge of a planar face: the edge's middle, z out of the face, y into it; on?: [x,y,z] a point on the face (default: the larger planar face beside the edge); corner?: [x,y,z] near one end: the corner there, x and y along its two edges into the face; found again by name, so it follows the face as it changes size) | point: [x,y,z], z?: [x,y,z]; body?: the named face's body; end?: \"<name>\" (a circle's end: the face across its rim there; default the end nearer the pick); x?: [x,y,z] (a direction, world) | \"<face name>\" | {face: [x,y,z]} (the frame's x: a direction, a plane's normal, or the way to another hole's axis; keeps a round origin's turn). Faces are kept by name: the origin follows its face, and a circle its end, when the design changes",
|
||||
),
|
||||
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|edge|point: [x,y,z] in world, or face|circle: \"<face name>\"; end?, x? as joint.origin} | origin name); \
|
||||
"type: rigid|revolute|slider|cylindrical|pin_slot|planar|ball, or a relation: parallel|perpendicular|angle|point_on_plane|point_on_line|line_on_plane \
|
||||
(ties only some directions or points, made where the parts are; several between two parts add up); a, b: joint origins ({occurrence, face|circle|edge|point: [x,y,z] in world, or face|circle: \"<face name>\"; end?, x? as joint.origin} | origin 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?: mm or an expression; angle?: deg or an expression (default: none, or a half turn where that turns b less, so faces that \
|
||||
already meet stay as they are); name?; quiet?: bool (leave out dof and fixed). An expression (text) is \
|
||||
offset?: mm or an expression; angle?: deg or an expression (an angle relation's tilt from mated; for other joints the default \
|
||||
is none, or a half turn where that turns b less, so faces that already meet stay as they are); name?; quiet?: bool (leave \
|
||||
out dof and fixed). An expression (text) is \
|
||||
kept with the joint and worked out again whenever the design changes, so the joint follows the parameters it names; a joint whose \
|
||||
expression no longer works out (a parameter deleted) is left out of the solve with the reason in its errors",
|
||||
),
|
||||
@@ -615,7 +617,7 @@ fn joint_origin(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
fn kind_param(p: &Value, cmd: &str) -> Result<JointKind> {
|
||||
str_(p, "type")
|
||||
.and_then(JointKind::parse)
|
||||
.ok_or_else(|| bad(cmd, "`type` must be rigid, revolute, slider, cylindrical, pin_slot, planar or ball"))
|
||||
.ok_or_else(|| bad(cmd, "`type` must be rigid, revolute, slider, cylindrical, pin_slot, planar, ball, parallel, perpendicular, angle, point_on_plane, point_on_line or line_on_plane"))
|
||||
}
|
||||
|
||||
fn joint(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
@@ -647,9 +649,16 @@ fn joint(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
suppressed: false,
|
||||
exprs: JointExprs { offset: typed(p.get("offset")), angle: typed(p.get("angle")), values: value_exprs(p) },
|
||||
};
|
||||
// A relation given no values starts from where its parts are: B moves no further than it must.
|
||||
if p.get("values").is_none()
|
||||
&& p.get("value").is_none()
|
||||
&& let Some(q) = joints::relation_values(&s.doc, &s.model.state(), &j)
|
||||
{
|
||||
j.values = q;
|
||||
}
|
||||
// No angle given: B keeps its turn about the joint's axis where a half turn would otherwise
|
||||
// come only from how its origin's x was chosen.
|
||||
if angle.is_none() {
|
||||
// come only from how its origin's x was chosen. (An angle relation's angle is its tilt.)
|
||||
if angle.is_none() && !j.kind.is_relation() {
|
||||
j.angle = joints::least_turn_angle(&s.doc, &s.model.state(), &j);
|
||||
}
|
||||
add_joint(s, j, p)
|
||||
@@ -1224,6 +1233,16 @@ fn edit(s: &mut Session, p: &Value) -> Result<Value> {
|
||||
if let Some(n) = str_(p, "name").map(str::trim).filter(|n| !n.is_empty() && n.len() <= 128) {
|
||||
j.name = n.to_string();
|
||||
}
|
||||
// A relation whose type, offset, tilt or flip changes starts again from where its parts are.
|
||||
let reseat = kind.is_some() || offset.is_some() || angle.is_some() || p.get("flip").is_some();
|
||||
if reseat {
|
||||
let edited = s.doc.assembly.joints.iter().find(|j| j.id == id).cloned();
|
||||
if let Some(q) = edited.and_then(|j| joints::relation_values(&s.doc, &s.model.state(), &j))
|
||||
&& let Some(j) = s.doc_mut().assembly.joints.iter_mut().find(|j| j.id == id)
|
||||
{
|
||||
j.values = q;
|
||||
}
|
||||
}
|
||||
let sol = resolve(s);
|
||||
Ok(sol_json(&sol, p))
|
||||
}
|
||||
@@ -1367,6 +1386,9 @@ mod frames_tests;
|
||||
#[path = "joints_reach_tests.rs"]
|
||||
mod reach_tests;
|
||||
#[cfg(test)]
|
||||
#[path = "joints_relations_tests.rs"]
|
||||
mod relations_tests;
|
||||
#[cfg(test)]
|
||||
#[path = "joints_tests.rs"]
|
||||
mod tests;
|
||||
#[cfg(test)]
|
||||
|
||||
@@ -0,0 +1,238 @@
|
||||
//! Relations (parallel, perpendicular, angle, point on plane, point on line, line on plane): each
|
||||
//! ties only part of a placement, several between two parts add up, they start from where the
|
||||
//! parts are, and they keep holding as the parts change size.
|
||||
|
||||
use serde_json::{Value, json};
|
||||
use solidcraft_geom::Vec3;
|
||||
|
||||
use super::tests::{occ, place, run};
|
||||
use crate::Session;
|
||||
|
||||
/// A 2 mm panel `W` x 300 (W = 400) in component Panel and a 20 x 10 x 6 bracket in component
|
||||
/// Bracket off to the side, with named origins on both made where they are; then the bracket is
|
||||
/// lifted, turned and tilted, as a part dropped in anywhere.
|
||||
fn panel() -> Session {
|
||||
let mut s = Session::default();
|
||||
run(&mut s, "parameters.add", json!({"name": "W", "expression": "400 mm"}));
|
||||
run(&mut s, "component.create", json!({"name": "Panel"}));
|
||||
run(&mut s, "solid.box", json!({"length": "W", "width": 300, "height": 2, "corner": [0, 0, 0]}));
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
run(&mut s, "component.create", json!({"name": "Bracket"}));
|
||||
run(&mut s, "solid.box", json!({"length": 20, "width": 10, "height": 6, "corner": [0, 500, 0]}));
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
let panel = occ(&s, "Panel");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": panel, "grounded": true}));
|
||||
for (name, o, k, at) in [
|
||||
("Top", "Panel", "face", [100.0, 100.0, 2.0]),
|
||||
("Front", "Panel", "face", [100.0, 0.0, 1.0]),
|
||||
("Right", "Panel", "face", [400.0, 100.0, 1.0]),
|
||||
("Bottom", "Bracket", "face", [10.0, 505.0, 0.0]),
|
||||
("Face", "Bracket", "face", [10.0, 500.0, 3.0]),
|
||||
("Corner", "Bracket", "point", [20.0, 500.0, 0.0]),
|
||||
] {
|
||||
run(&mut s, "joint.origin", json!({"name": name, "occurrence": o, k: at}));
|
||||
}
|
||||
let b = occ(&s, "Bracket");
|
||||
run(&mut s, "occurrence.move", json!({"occurrence": b, "translate": [30, -120, 50]}));
|
||||
run(&mut s, "occurrence.move", json!({"occurrence": b, "axis": [0, 0, 1], "angle": "25 deg", "origin": [40, 385, 50]}));
|
||||
run(&mut s, "occurrence.move", json!({"occurrence": b, "axis": [1, 0, 0], "angle": "10 deg", "origin": [40, 385, 50]}));
|
||||
s
|
||||
}
|
||||
|
||||
fn relation(s: &mut Session, kind: &str, a: &str, b: &str, extra: Value) -> Value {
|
||||
let mut p = json!({"type": kind, "a": a, "b": b, "name": format!("{kind} {a}-{b}")});
|
||||
if let (Some(p), Some(extra)) = (p.as_object_mut(), extra.as_object()) {
|
||||
p.extend(extra.clone());
|
||||
}
|
||||
run(s, "joint.create", p)
|
||||
}
|
||||
|
||||
fn dof(r: &Value, s: &Session) -> Option<u64> {
|
||||
let b = occ(s, "Bracket");
|
||||
r["dof"].as_array()?.iter().find(|d| d["occurrence"] == json!(b))?["dof"].as_u64()
|
||||
}
|
||||
|
||||
fn near(a: Vec3, b: Vec3, tol: f64) -> bool {
|
||||
a.dist(b) < tol
|
||||
}
|
||||
|
||||
fn clean(r: &Value) -> bool {
|
||||
r["conflicts"] == json!([]) && r["errors"] == json!([])
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn stacked_relations_place_a_part_and_follow_the_panel() {
|
||||
let mut s = panel();
|
||||
// Flat on the top (parallel, and a corner on it), its face along the panel's front edge, the
|
||||
// corner 40 mm in from the front and the right end.
|
||||
let r = relation(&mut s, "parallel", "Top", "Bottom", json!({}));
|
||||
assert_eq!(dof(&r, &s), Some(4), "{r}");
|
||||
let r = relation(&mut s, "point_on_plane", "Top", "Corner", json!({}));
|
||||
assert_eq!(dof(&r, &s), Some(3), "{r}");
|
||||
let r = relation(&mut s, "parallel", "Front", "Face", json!({"flip": true}));
|
||||
assert_eq!(dof(&r, &s), Some(2), "{r}");
|
||||
let r = relation(&mut s, "point_on_plane", "Front", "Corner", json!({"offset": -40}));
|
||||
assert_eq!(dof(&r, &s), Some(1), "{r}");
|
||||
let r = relation(&mut s, "point_on_plane", "Right", "Corner", json!({"offset": -40}));
|
||||
assert!(clean(&r), "{r}");
|
||||
assert_eq!(dof(&r, &s), Some(0), "{r}");
|
||||
for w in [400.0, 650.0, 250.0, 1000.0] {
|
||||
run(&mut s, "parameters.change", json!({"name": "W", "expression": format!("{w} mm")}));
|
||||
let r = run(&mut s, "joint.solve", json!({}));
|
||||
assert!(clean(&r), "W {w}: {r}");
|
||||
// The corner, the bracket's other bottom corner along its face, and its top: flat, square
|
||||
// and 40 mm in.
|
||||
assert!(near(place(&s, "Bracket", Vec3::new(20.0, 500.0, 0.0)), Vec3::new(w - 40.0, 40.0, 2.0), 1e-6), "W {w}");
|
||||
assert!(near(place(&s, "Bracket", Vec3::new(0.0, 500.0, 0.0)), Vec3::new(w - 60.0, 40.0, 2.0), 1e-6), "W {w}");
|
||||
assert!(near(place(&s, "Bracket", Vec3::new(20.0, 510.0, 6.0)), Vec3::new(w - 40.0, 50.0, 8.0), 1e-6), "W {w}");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_relation_moves_the_part_no_further_than_it_must() {
|
||||
let mut s = panel();
|
||||
let corner = |s: &Session| place(s, "Bracket", Vec3::new(20.0, 500.0, 0.0));
|
||||
let along = |s: &Session| place(s, "Bracket", Vec3::new(0.0, 510.0, 6.0)) - corner(s);
|
||||
let (before, turn) = (corner(&s), along(&s));
|
||||
let r = relation(&mut s, "point_on_plane", "Top", "Corner", json!({}));
|
||||
assert!(clean(&r), "{r}");
|
||||
assert_eq!(dof(&r, &s), Some(5), "{r}");
|
||||
// Straight down onto the top, turned as it was.
|
||||
assert!(near(corner(&s), Vec3::new(before.x, before.y, 2.0), 1e-6), "{:?} from {before:?}", corner(&s));
|
||||
assert!(near(along(&s), turn, 1e-6), "{:?} from {turn:?}", along(&s));
|
||||
// Parallel to the top: it only turns level, about the corner it keeps.
|
||||
let r = relation(&mut s, "parallel", "Top", "Bottom", json!({}));
|
||||
assert!(clean(&r), "{r}");
|
||||
let up = place(&s, "Bracket", Vec3::new(20.0, 500.0, 6.0)) - corner(&s);
|
||||
assert!(near(up, Vec3::new(0.0, 0.0, 6.0), 1e-6), "{up:?}");
|
||||
assert!((corner(&s).z - 2.0).abs() < 1e-6, "{:?}", corner(&s));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn an_angle_relation_holds_its_tilt_and_follows_an_edit() {
|
||||
let mut s = panel();
|
||||
relation(&mut s, "point_on_plane", "Top", "Corner", json!({}));
|
||||
let tilt = |s: &Session| {
|
||||
let n = place(s, "Bracket", Vec3::new(10.0, 505.0, 0.0)) - place(s, "Bracket", Vec3::new(10.0, 505.0, 1.0));
|
||||
n.normalized().map_or(f64::NAN, |n| n.dot(Vec3::Z).acos().to_degrees())
|
||||
};
|
||||
// 0 deg is mated: the bracket's bottom against the top, its outward normal straight down.
|
||||
let r = relation(&mut s, "angle", "Top", "Bottom", json!({"angle": "30 deg"}));
|
||||
assert!(clean(&r), "{r}");
|
||||
assert!((tilt(&s) - 150.0).abs() < 1e-6, "{}", tilt(&s));
|
||||
run(&mut s, "joint.edit", json!({"joint": "angle Top-Bottom", "angle": "60 deg"}));
|
||||
assert!((tilt(&s) - 120.0).abs() < 1e-6, "{}", tilt(&s));
|
||||
run(&mut s, "joint.edit", json!({"joint": "angle Top-Bottom", "type": "perpendicular"}));
|
||||
assert!((tilt(&s) - 90.0).abs() < 1e-6, "{}", tilt(&s));
|
||||
// The corner stayed on the top through every change.
|
||||
assert!((place(&s, "Bracket", Vec3::new(20.0, 500.0, 0.0)).z - 2.0).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn a_rod_lies_tangent_on_the_panel_and_rolls_along_it() {
|
||||
let mut s = panel();
|
||||
run(&mut s, "component.create", json!({"name": "Rod"}));
|
||||
run(&mut s, "solid.cylinder", json!({"base": [0, 700, 40], "axis": [1, 0, 0], "radius": 5, "height": 80, "operation": "new"}));
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
run(&mut s, "joint.origin", json!({"name": "Axis", "occurrence": "Rod", "circle": [0, 700, 40]}));
|
||||
let r = relation(&mut s, "line_on_plane", "Top", "Axis", json!({"offset": 5}));
|
||||
assert!(clean(&r), "{r}");
|
||||
let rod = occ(&s, "Rod");
|
||||
let d = r["dof"].as_array().and_then(|a| a.iter().find(|d| d["occurrence"] == json!(rod))).and_then(|d| d["dof"].as_u64());
|
||||
assert_eq!(d, Some(4), "{r}");
|
||||
// Its axis level, 5 mm over the top: it touches it along a line.
|
||||
let (a, b) = (place(&s, "Rod", Vec3::new(0.0, 700.0, 40.0)), place(&s, "Rod", Vec3::new(80.0, 700.0, 40.0)));
|
||||
assert!((a.z - 7.0).abs() < 1e-6 && (b.z - 7.0).abs() < 1e-6, "{a:?} {b:?}");
|
||||
// Rolled along: still tangent.
|
||||
run(&mut s, "joint.drive", json!({"joint": "line_on_plane Top-Axis", "values": [100, 50, "30 deg", "90 deg"]}));
|
||||
let (a, b) = (place(&s, "Rod", Vec3::new(0.0, 700.0, 40.0)), place(&s, "Rod", Vec3::new(80.0, 700.0, 40.0)));
|
||||
assert!((a.z - 7.0).abs() < 1e-6 && (b.z - 7.0).abs() < 1e-6, "{a:?} {b:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn relations_that_disagree_are_reported_not_forced() {
|
||||
let mut s = panel();
|
||||
relation(&mut s, "point_on_plane", "Right", "Corner", json!({"offset": -40}));
|
||||
let r = relation(&mut s, "point_on_plane", "Right", "Corner", json!({"offset": -60}));
|
||||
assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(1), "{r}");
|
||||
// A point on the panel's front plane's line: still placed, every number finite.
|
||||
let r = relation(&mut s, "point_on_line", "Front", "Corner", json!({}));
|
||||
assert!(r["dof"].is_array(), "{r}");
|
||||
let p = place(&s, "Bracket", Vec3::new(20.0, 500.0, 0.0));
|
||||
assert!(p.is_finite(), "{p:?}");
|
||||
// Saved and read back, the relations are the same.
|
||||
let saved = serde_json::to_string(&s.doc.assembly.joints).unwrap_or_default();
|
||||
let back: Vec<solidcraft_doc::joints::Joint> = serde_json::from_str(&saved).unwrap_or_default();
|
||||
assert_eq!(back, s.doc.assembly.joints);
|
||||
assert!(saved.contains("\"point_on_plane\"") && saved.contains("\"point_on_line\""), "{saved}");
|
||||
}
|
||||
|
||||
/// More parts placed by relations than one group of loops may hold (each relation after a part's
|
||||
/// first closes a loop): every group is small, so every part is placed.
|
||||
#[test]
|
||||
fn a_panel_full_of_parts_placed_by_relations_stays_solved() {
|
||||
let mut s = Session::default();
|
||||
run(&mut s, "parameters.add", json!({"name": "W", "expression": "1500 mm"}));
|
||||
run(&mut s, "component.create", json!({"name": "Panel"}));
|
||||
run(&mut s, "solid.box", json!({"length": "W", "width": 300, "height": 2, "corner": [0, 0, 0]}));
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
let panel = occ(&s, "Panel");
|
||||
run(&mut s, "occurrence.ground", json!({"occurrence": panel, "grounded": true}));
|
||||
for (name, at) in [("Top", [100.0, 100.0, 2.0]), ("Front", [100.0, 0.0, 1.0]), ("Right", [1500.0, 100.0, 1.0])] {
|
||||
run(&mut s, "joint.origin", json!({"name": name, "occurrence": "Panel", "face": at}));
|
||||
}
|
||||
// 30 brackets, 5 relations each: 120 loops, more than the 100 one group may hold. The joints
|
||||
// go straight into the design and are solved once (made one by one, each would solve all).
|
||||
let n = 30usize;
|
||||
for k in 0..n {
|
||||
let (c, y) = (format!("B{k}"), 500.0 + 30.0 * k as f64);
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
run(&mut s, "component.create", json!({"name": c}));
|
||||
run(&mut s, "solid.box", json!({"length": 20, "width": 10, "height": 6, "corner": [0, y, 0]}));
|
||||
run(&mut s, "component.activate", json!({"component": "root"}));
|
||||
for (o, k2, at) in [("b", "face", [10.0, y + 5.0, 0.0]), ("f", "face", [10.0, y, 3.0]), ("c", "point", [20.0, y, 0.0])] {
|
||||
run(&mut s, "joint.origin", json!({"name": format!("{c}{o}"), "occurrence": c, k2: at}));
|
||||
}
|
||||
let x = 40.0 * (k + 1) as f64;
|
||||
for (kind, a, b, flip, offset) in [
|
||||
("parallel", "Top", "b", false, 0.0),
|
||||
("point_on_plane", "Top", "c", false, 0.0),
|
||||
("parallel", "Front", "f", true, 0.0),
|
||||
("point_on_plane", "Front", "c", false, -40.0),
|
||||
("point_on_plane", "Right", "c", false, -x),
|
||||
] {
|
||||
let origin = |n: &str| s.doc.assembly.origins.iter().find(|o| o.name == n).map(|o| o.origin.clone());
|
||||
let (Some(a), Some(b)) = (origin(a), origin(&format!("{c}{b}"))) else { panic!("origins") };
|
||||
let id = s.doc.assembly.joints.iter().map(|j| j.id).max().unwrap_or(0) + 1;
|
||||
let kind = solidcraft_doc::joints::JointKind::parse(kind).unwrap_or(solidcraft_doc::joints::JointKind::Rigid);
|
||||
let mut j = solidcraft_doc::joints::Joint {
|
||||
id,
|
||||
name: format!("R{id}"),
|
||||
kind,
|
||||
a,
|
||||
b,
|
||||
values: vec![0.0; kind.dofs().len()],
|
||||
limits: vec![None; kind.dofs().len()],
|
||||
flip,
|
||||
offset,
|
||||
angle: 0.0,
|
||||
suppressed: false,
|
||||
exprs: Default::default(),
|
||||
};
|
||||
if let Some(q) = solidcraft_doc::joints::relation_values(&s.doc, &s.model.state(), &j) {
|
||||
j.values = q;
|
||||
}
|
||||
s.doc_mut().assembly.joints.push(j);
|
||||
}
|
||||
}
|
||||
for w in [1500.0, 1800.0] {
|
||||
run(&mut s, "parameters.change", json!({"name": "W", "expression": format!("{w} mm")}));
|
||||
let r = run(&mut s, "joint.solve", json!({}));
|
||||
assert!(clean(&r), "W {w}: {r}");
|
||||
for k in [0, n / 2, n - 1] {
|
||||
let (c, y) = (format!("B{k}"), 500.0 + 30.0 * k as f64);
|
||||
let at = place(&s, &c, Vec3::new(20.0, y, 0.0));
|
||||
assert!(near(at, Vec3::new(w - 40.0 * (k + 1) as f64, 40.0, 2.0), 1e-6), "W {w} {c}: {at:?}");
|
||||
}
|
||||
}
|
||||
}
|
||||
+16
-5
@@ -148,9 +148,11 @@ fn rotation(u: Vec3, a: f64) -> Mat {
|
||||
}
|
||||
|
||||
/// The joint's motion as URDF steps, in order from A's frame: (kind, axis, index of the value).
|
||||
fn steps(kind: JointKind) -> Vec<(Step, Vec3, usize)> {
|
||||
/// `None` for a relation whose turns follow a fixed step (a tilt, or the offset off a plane or
|
||||
/// line): URDF has no fixed step inside a joint's chain.
|
||||
fn steps(kind: JointKind) -> Option<Vec<(Step, Vec3, usize)>> {
|
||||
use Step::{Prismatic as P, Revolute as R};
|
||||
match kind {
|
||||
Some(match kind {
|
||||
JointKind::Rigid => vec![],
|
||||
JointKind::Revolute => vec![(R, Vec3::Z, 0)],
|
||||
JointKind::Slider => vec![(P, Vec3::Z, 0)],
|
||||
@@ -158,7 +160,9 @@ fn steps(kind: JointKind) -> Vec<(Step, Vec3, usize)> {
|
||||
JointKind::PinSlot => vec![(P, Vec3::X, 1), (R, Vec3::Z, 0)],
|
||||
JointKind::Planar => vec![(P, Vec3::X, 0), (P, Vec3::Y, 1), (R, Vec3::Z, 2)],
|
||||
JointKind::Ball => vec![(R, Vec3::Z, 0), (R, Vec3::Y, 1), (R, Vec3::Z, 2)],
|
||||
}
|
||||
JointKind::Parallel => vec![(P, Vec3::X, 0), (P, Vec3::Y, 1), (P, Vec3::Z, 2), (R, Vec3::Z, 3)],
|
||||
JointKind::Perpendicular | JointKind::Angle | JointKind::PointOnPlane | JointKind::PointOnLine | JointKind::LineOnPlane => return None,
|
||||
})
|
||||
}
|
||||
|
||||
/// `raw` with every character but `[A-Za-z0-9_]` (and `-` in file names) replaced by `_`, so it
|
||||
@@ -378,7 +382,14 @@ impl<'a> Tree<'a> {
|
||||
|
||||
/// `child` on joint `j` from `p` (already in the tree): the joint's chain and the child's link.
|
||||
fn attach(&mut self, j: &Joint, fa: &Mat, p: u64, child: u64, q: &[f64]) -> Option<()> {
|
||||
let m = joints::motion(j.kind, q);
|
||||
// A relation URDF can't write is fixed where it is: the child link's frame is the joint's.
|
||||
let (chain, m) = match steps(j.kind) {
|
||||
Some(chain) => (chain, joints::motion(j.kind, q)),
|
||||
None => {
|
||||
self.warnings.push(format!("joint `{}` is a {:?} relation, which URDF has no joint for: written fixed as it is", j.name, j.kind));
|
||||
(Vec::new(), IDENTITY)
|
||||
}
|
||||
};
|
||||
let child_on_b = j.a.occurrence == p;
|
||||
// (parent-side joint frame, child link frame) in the world.
|
||||
let (jf, lf) = if child_on_b {
|
||||
@@ -390,7 +401,7 @@ impl<'a> Tree<'a> {
|
||||
};
|
||||
let (parent_index, parent_frame) = self.placed.get(&p).copied()?;
|
||||
let origin = mat_mul(&mat_inverse(&parent_frame)?, &jf);
|
||||
let mut chain = steps(j.kind);
|
||||
let mut chain = chain;
|
||||
if !child_on_b {
|
||||
chain.reverse();
|
||||
for s in &mut chain {
|
||||
|
||||
@@ -26,8 +26,36 @@ mod trace;
|
||||
pub use trace::{TraceForm, picked_point};
|
||||
|
||||
/// Joint types in the order the Type box lists them (command names).
|
||||
pub const TYPES: [&str; 7] = ["rigid", "revolute", "slider", "cylindrical", "pin_slot", "planar", "ball"];
|
||||
const TYPE_LABELS: [&str; 7] = ["Rigid", "Revolute", "Slider", "Cylindrical", "Pin-slot", "Planar", "Ball"];
|
||||
pub const TYPES: [&str; 13] = [
|
||||
"rigid",
|
||||
"revolute",
|
||||
"slider",
|
||||
"cylindrical",
|
||||
"pin_slot",
|
||||
"planar",
|
||||
"ball",
|
||||
"parallel",
|
||||
"perpendicular",
|
||||
"angle",
|
||||
"point_on_plane",
|
||||
"point_on_line",
|
||||
"line_on_plane",
|
||||
];
|
||||
const TYPE_LABELS: [&str; 13] = [
|
||||
"Rigid",
|
||||
"Revolute",
|
||||
"Slider",
|
||||
"Cylindrical",
|
||||
"Pin-slot",
|
||||
"Planar",
|
||||
"Ball",
|
||||
"Parallel",
|
||||
"Perpendicular",
|
||||
"Angle",
|
||||
"Point on plane",
|
||||
"Point on line",
|
||||
"Line on plane",
|
||||
];
|
||||
/// What a joint origin input accepts (snaps on faces, edges and vertices).
|
||||
pub const SNAPS: u16 = FACES | EDGES | VERTICES;
|
||||
/// Commands whose effect is where occurrences sit (previewed on the placed model).
|
||||
|
||||
@@ -46,6 +46,28 @@ expression stops working out (say a parameter becomes an angle), the joint is le
|
||||
solve and `joint.solve` gives the reason, instead of placing the part from old numbers. Typing a
|
||||
plain number, or driving the value, drops that expression.
|
||||
|
||||
**Relations** are joints that tie only part of a placement, so a part is placed the way a
|
||||
designer describes it, one relation at a time: `parallel` (z along z, free to move and turn
|
||||
about it), `perpendicular` and `angle` (z square to z, or at the joint's `angle` from mated),
|
||||
`point_on_plane` (a point on a face's plane, `offset` off it), `point_on_line` (a point on an
|
||||
axis) and `line_on_plane` (an axis on a face's plane: a cylinder's radius as `offset` lays it
|
||||
tangent). A relation is made where the parts are: B moves no further than the relation needs.
|
||||
Several relations between the same two parts add up; the solver meets them together, each one's
|
||||
degrees of freedom come off what is left, and one that can't be met is reported, not forced:
|
||||
|
||||
```json
|
||||
{"command": "joint.create", "params": {"type": "parallel", "a": "Panel top", "b": "Bracket bottom"}}
|
||||
{"command": "joint.create", "params": {"type": "point_on_plane", "a": "Panel top", "b": "Bracket corner"}}
|
||||
{"command": "joint.create", "params": {"type": "parallel", "a": "Panel front", "b": "Bracket face", "flip": true}}
|
||||
{"command": "joint.create", "params": {"type": "point_on_plane", "a": "Panel front", "b": "Bracket corner", "offset": -40}}
|
||||
{"command": "joint.create", "params": {"type": "point_on_plane", "a": "Panel end", "b": "Bracket corner", "offset": -40}}
|
||||
```
|
||||
|
||||
leaves the bracket flat on the panel, square to its front, its corner 40 mm in from the front and
|
||||
the end, wherever the panel's size takes them. Coincident faces are a `planar` joint, a distance
|
||||
between faces is `planar` with an `offset`, concentric is `cylindrical` and two points that meet
|
||||
are `ball`.
|
||||
|
||||
The check behind it, `inspect.clearance`, does all of that in one call for any number of bodies,
|
||||
which suits scripts and agents checking a whole assembly:
|
||||
|
||||
@@ -169,7 +191,7 @@ it where it is), and it moves.
|
||||
with their units and limits, and `moved` says whether any part moved. A joint's origins are mated
|
||||
by default: B's z axis is turned against A's, as two faces meet. `flip: true` lines them up
|
||||
instead, so two origins given at one point with the same z stay as they are. Without an
|
||||
`angle`, a new joint takes no turn or a half turn about its axis, whichever turns B less: two
|
||||
`angle`, a new joint (other than a relation, which starts where its parts are) takes no turn or a half turn about its axis, whichever turns B less: two
|
||||
faces that already meet, a plate under a panel, stay as they are instead of B spinning about
|
||||
their shared normal. For scripts that drive many steps, `quiet: true` leaves the per-part
|
||||
degrees of freedom out of joint answers, and
|
||||
|
||||
@@ -68,6 +68,8 @@ defaults are written and a warning says so. A value that is not a positive numbe
|
||||
| Pin-slot | `prismatic` x, then `revolute` z |
|
||||
| Planar | `prismatic` x, `prismatic` y, then `revolute` z |
|
||||
| Ball | `revolute` z, `revolute` y, then `revolute` z |
|
||||
| Parallel | `prismatic` x, y and z, then `revolute` z |
|
||||
| Perpendicular, angle, point on plane, point on line, line on plane | `fixed` where it is, with a warning: their turns follow a fixed tilt or offset, which a URDF chain can't hold |
|
||||
|
||||
- **Chains:** a joint with several values becomes one URDF joint per value. Between them sit
|
||||
links named `<joint>_link1`, `<joint>_link2`. The steps before the last are named
|
||||
|
||||
Reference in New Issue
Block a user