///|
/// Static gravity torque and actuator margin at one joint.
#derive(Debug, Eq)
pub(all) struct JointTorque {
joint : String
tau_static_nm : Double
tau_rated_nm : Double
safety_factor : Double
/// True when the joint has no actuator effort limit (a passive linkage).
passive : Bool
ok : Bool
}
///|
/// Minimum world Z of one wheel or foot contact link.
#derive(Debug, Eq)
pub(all) struct ContactAlignment {
name : String
min_z_m : Double
}
///|
/// Whole-robot statics at one pose.
#derive(Debug, Eq)
pub(all) struct PoseAnalysis {
name : String
total_mass_kg : Double
com_m : (Double, Double, Double)
inertia : @urdf.Inertia
joints : Array[JointTorque]
contacts : Array[ContactAlignment]
/// Largest left/right contact-link Z difference in metres.
contact_misalignment_m : Double
}
///|
/// Statics across every requested pose.
#derive(Debug, Eq)
pub(all) struct RobotAnalysis {
total_mass_kg : Double
poses : Array[PoseAnalysis]
warnings : Array[String]
}
///|
/// 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"
}
///|
/// Transforms a link-local inertial into the world frame by composing its
/// origin with the link's world transform. The tensor itself is expressed in
/// the inertial frame, whose world orientation is now carried by
/// `origin.rpy`; `Inertial::combine` rotates it by exactly that frame.
fn world_inertial(
inertial : @urdf.Inertial,
world : @urdf.Transform,
) -> @urdf.Inertial {
inertial.transform_by(world)
}
///|
/// Appends every link in the subtree rooted at `link_name` (including it).
fn collect_subtree(
children : Map[String, Array[String]],
link_name : String,
out : Array[String],
) -> Unit {
out.push(link_name)
match children.get(link_name) {
Some(names) =>
for child in names {
collect_subtree(children, child, out)
}
None => ()
}
}
///|
fn vec3_dot(
a : (Double, Double, Double),
b : (Double, Double, Double),
) -> Double {
a.0 * b.0 + a.1 * b.1 + a.2 * b.2
}
///|
/// Strips a left/right prefix so `l_wheel_link` and `r_wheel_link` group.
fn contact_group_key(name : String) -> String {
let n = name.to_lower()
if n.has_prefix("left_") {
String::unsafe_substring(n, start=5, end=n.length())
} else if n.has_prefix("right_") {
String::unsafe_substring(n, start=6, end=n.length())
} else if n.has_prefix("l_") || n.has_prefix("r_") {
String::unsafe_substring(n, start=2, end=n.length())
} else {
n
}
}
///|
/// Minimum world Z of a contact link's bodies, in metres.
fn contact_link_min_z(
links : Array[CompiledLink],
fk : Map[String, @urdf.Transform],
link_name : String,
) -> Double? {
let world = fk.get(link_name).unwrap_or(@urdf.Transform::identity())
let mut found = false
let mut min_z = 1.0e18
for link in links {
if link.name != link_name {
continue
}
for body in link.bodies {
let placed = transform_scene_node_urdf(body.node, world)
let (_x0, _x1, _y0, _y1, z0, _z1) = placed.bounding_box()
let z = z0 / 1000.0
if z < min_z {
min_z = z
}
found = true
}
}
if found {
Some(min_z)
} else {
None
}
}
///|
fn analyze_pose(
robot : @urdf.Robot,
links : Array[CompiledLink],
joint_positions : Map[String, Double],
pose_name : String,
) -> PoseAnalysis {
let fk = robot.forward_kinematics(joint_positions~)
let world_inertials : Array[@urdf.Inertial] = []
for link in robot.links {
match link.inertial {
Some(inertial) =>
world_inertials.push(
world_inertial(
inertial,
fk.get(link.name).unwrap_or(@urdf.Transform::identity()),
),
)
None => ()
}
}
let combined = @urdf.Inertial::combine(world_inertials)
let total_mass = combined.mass
let com = combined.origin.xyz
// Parent -> child link names for subtree torque sums.
let children : Map[String, Array[String]] = Map([])
for joint in robot.joints {
match children.get(joint.parent) {
Some(names) => names.push(joint.child)
None => children.set(joint.parent, [joint.child])
}
}
// Link inertials in world, keyed by link, for subtree sums.
let link_world : Map[String, @urdf.Inertial] = Map([])
for link in robot.links {
match link.inertial {
Some(inertial) =>
link_world.set(
link.name,
world_inertial(
inertial,
fk.get(link.name).unwrap_or(@urdf.Transform::identity()),
),
)
None => ()
}
}
let gravity = 9.80665
let joints : Array[JointTorque] = []
for joint in robot.joints {
let revolute = match joint.joint_type {
Revolute | Continuous => true
_ => false
}
if !revolute {
continue
}
let child_world = fk.get(joint.child).unwrap_or(@urdf.Transform::identity())
let r_joint = child_world.trans
let axis_world = child_world.apply_dir(joint.axis)
let subtree : Array[String] = []
collect_subtree(children, joint.child, subtree)
// Sum the moments of every subtree link before projecting onto the axis so
// opposing link moments cancel, as they physically do.
let mut moment = (0.0, 0.0, 0.0)
for name in subtree {
match link_world.get(name) {
Some(inertial) =>
if inertial.mass > 0.0 {
let r = (
inertial.origin.xyz.0 - r_joint.0,
inertial.origin.xyz.1 - r_joint.1,
inertial.origin.xyz.2 - r_joint.2,
)
let force = (0.0, 0.0, -inertial.mass * gravity)
let m = vec3_cross(r, force)
moment = (moment.0 + m.0, moment.1 + m.1, moment.2 + m.2)
}
None => ()
}
}
let tau = vec3_dot(moment, axis_world).abs()
let rated = match joint.limit {
Some(limit) => limit.effort
None => 0.0
}
// A joint with no actuator effort is a passive linkage (4-bar knee, guide,
// ...). It still carries gravity, but there is no motor to size.
let passive = rated <= 0.0
let safety = if passive {
0.0
} else {
rated / (if tau > 1.0e-6 { tau } else { 1.0e-6 })
}
joints.push({
joint: joint.name,
tau_static_nm: tau,
tau_rated_nm: rated,
safety_factor: safety,
passive,
ok: if passive {
true
} else {
safety >= 1.5
},
})
}
let contacts : Array[ContactAlignment] = []
let group_min : Map[String, Double] = Map([])
for link in links {
let lower = link.name.to_lower()
// Cosmetic motor bells ride on the wheel axle but are not ground contact.
if lower.contains("_rotor") {
continue
}
if lower.contains("wheel") ||
lower.contains("foot") ||
lower.contains("tire") {
match contact_link_min_z(links, fk, link.name) {
Some(min_z) => {
contacts.push({ name: link.name, min_z_m: min_z, })
let key = contact_group_key(link.name)
match group_min.get(key) {
Some(existing) => if min_z < existing { group_min.set(key, min_z) }
None => group_min.set(key, min_z)
}
}
None => ()
}
}
}
// Compare left/right links sharing a group key by their min Z spread.
let mut misalignment = 0.0
let group_max : Map[String, Double] = Map([])
for contact in contacts {
let key = contact_group_key(contact.name)
match group_max.get(key) {
Some(existing) =>
if contact.min_z_m > existing {
group_max.set(key, contact.min_z_m)
}
None => group_max.set(key, contact.min_z_m)
}
}
for key, minimum in group_min {
match group_max.get(key) {
Some(maximum) => {
let delta = maximum - minimum
if delta > misalignment {
misalignment = delta
}
}
None => ()
}
}
{
name: pose_name,
total_mass_kg: total_mass,
com_m: com,
inertia: combined.inertia,
joints,
contacts,
contact_misalignment_m: misalignment,
}
}
///|
/// Analyzes whole-robot statics at every requested pose: composite mass and
/// CoM, static gravity torque at each revolute/continuous joint versus the
/// URDF effort limit, safety margins, and wheel/foot ground-contact alignment.
///
/// An empty `poses` array analyzes the zero pose under the name `"default"`.
pub fn analyze_robot(
component : Component,
poses : Array[(String, Map[String, Double])],
) -> RobotAnalysis {
let robot = component.to_robot()
let (links, _, _) = component.compile_links()
let requested = if poses.length() == 0 {
[("default", Map([]))]
} else {
poses
}
let pose_analyses : Array[PoseAnalysis] = []
for entry in requested {
pose_analyses.push(analyze_pose(robot, links, entry.1, entry.0))
}
let total_mass = if pose_analyses.length() > 0 {
pose_analyses[0].total_mass_kg
} else {
0.0
}
let warnings : Array[String] = []
for pose in pose_analyses {
for joint in pose.joints {
if !joint.ok {
warnings.push(
pose.name +
": joint '" +
joint.joint +
"' safety factor " +
joint.safety_factor.to_string() +
" < 1.5 (static " +
joint.tau_static_nm.to_string() +
" Nm, rated " +
joint.tau_rated_nm.to_string() +
" Nm)",
)
}
}
if pose.contact_misalignment_m > 1.0e-3 {
warnings.push(
pose.name +
": wheel/foot ground contacts differ by " +
(pose.contact_misalignment_m * 1000.0).to_string() +
" mm",
)
}
}
{ total_mass_kg: total_mass, poses: pose_analyses, warnings, }
}