///|
/// Placed-shape and interference analysis for a posed component tree.
///
/// The heavy lifting lives in `gmlewis/step/components/collision`: an AABB
/// broad phase followed by an oriented-box separating-axis test. This file
/// materialises each CAD body in its link's world frame so assemblies built
/// from `Component` can be checked for unintended positive-volume overlaps.
///|
/// Every body in the posed component tree as a placed oriented bounding box.
///
/// Bodies inside one link are treated as one rigid instance; merged fasteners
/// and bearings are therefore not reported against their host. The OBB uses
/// the body's exact local bounding box, which is conservative for curved
/// profiles.
pub fn Component::placed_shapes(
self : Component,
joint_positions~ : Map[String, Double] = Map([]),
) -> Array[@step_collision.PlacedShape] {
let (links, _, _) = self.compile_links()
let robot = self.to_robot()
let fk = robot.forward_kinematics(joint_positions~)
let shapes : Array[@step_collision.PlacedShape] = []
for link in links {
let world = fk.get(link.name).unwrap_or(@urdf.Transform::identity())
let placement = @step_common.Placement::{
rot: urdf_to_step_matrix(world.rot),
trans: (
world.trans.0 * 1000.0,
world.trans.1 * 1000.0,
world.trans.2 * 1000.0,
),
}
for body in link.bodies {
let bounds = body.node.bounding_box()
shapes.push({
instance_name: link.name,
layer_name: body.name,
obb: @step_collision.Obb::from_placement(placement, bounds),
})
}
}
shapes
}
///|
/// Classifies every pair of distinct link instances in the posed tree.
pub fn Component::collision_reports(
self : Component,
joint_positions~ : Map[String, Double] = Map([]),
tol~ : @step_collision.TolerancePolicy = @step_collision.TolerancePolicy::default(),
policy~ : @step_collision.ContactPolicy = @step_collision.ContactPolicy::none(),
) -> Array[@step_collision.CollisionReport] {
@step_collision.pair_reports(
self.placed_shapes(joint_positions~),
tol~,
policy~,
)
}
///|
/// One-line summaries of unintended positive-volume overlaps only.
pub fn Component::collision_checks(
self : Component,
joint_positions~ : Map[String, Double] = Map([]),
tol~ : @step_collision.TolerancePolicy = @step_collision.TolerancePolicy::default(),
policy~ : @step_collision.ContactPolicy = @step_collision.ContactPolicy::none(),
) -> Array[String] {
let warnings : Array[String] = []
for report in self.collision_reports(joint_positions~, tol~, policy~) {
if report.is_collision() {
warnings.push(report.summary())
}
}
warnings
}
///|
/// A text report of every contact, interference, and collision pair.
pub fn Component::collision_report_text(
self : Component,
joint_positions~ : Map[String, Double] = Map([]),
tol~ : @step_collision.TolerancePolicy = @step_collision.TolerancePolicy::default(),
policy~ : @step_collision.ContactPolicy = @step_collision.ContactPolicy::none(),
) -> String {
let reports = self.collision_reports(joint_positions~, tol~, policy~)
let mut collisions = 0
let mut contacts = 0
let mut fits = 0
let mut indeterminate = 0
let mut out = "Interference analysis\n---------------------\n"
for report in reports {
match report.kind {
@step_collision.CollisionKind::Collision => {
collisions = collisions + 1
out = out + "COLLISION | " + report.summary() + "\n"
}
@step_collision.CollisionKind::Contact => {
contacts = contacts + 1
out = out + "contact | " + report.summary() + "\n"
}
@step_collision.CollisionKind::InterferenceFit => {
fits = fits + 1
out = out + "fit | " + report.summary() + "\n"
}
@step_collision.CollisionKind::Indeterminate => {
indeterminate = indeterminate + 1
out = out + "indeterminate | " + report.summary() + "\n"
}
@step_collision.CollisionKind::Clearance => ()
}
}
out +
"summary: " +
collisions.to_string() +
" collision(s), " +
contacts.to_string() +
" contact(s), " +
fits.to_string() +
" declared fit(s), " +
indeterminate.to_string() +
" indeterminate\n"
}