///|
/// Compiles a `Component` tree into URDF links, joints, transmissions, and a
/// complete `@urdf.Robot`, with optional mesh visuals backed by one glTF file
/// per link.
///|
/// How a compiled link's visuals are represented.
pub(all) enum VisualMode {
/// One primitive visual per body, matching its analytical collision.
Primitives
/// One mesh visual per link, referencing `package:///meshes/.gltf`.
MeshPackage(String)
}
///|
/// A URDF link plus the CAD bodies that produced it.
pub(all) struct CompiledLink {
name : String
bodies : Array[BodyPart]
urdf_link : @urdf.Link
}
///|
fn identity_mate(name : String) -> @step_common.MateConnector {
@step_common.MateConnector::new(name)
}
///|
/// transform_of_placement converts a shared millimetre/degree placement to a
/// URDF metre/radian transform.
fn transform_of_placement(p : @step_common.Placement) -> @urdf.Transform {
let (xyz, rpy) = @step_common.MateConnector::new("snap", placement=p).to_m_rad()
@urdf.Transform::from_origin(@urdf.Origin::new(xyz~, rpy~))
}
///|
/// placement_of_transform converts a URDF metre/radian transform to a shared
/// millimetre/degree placement.
fn placement_of_transform(t : @urdf.Transform) -> @step_common.Placement {
let origin = t.to_origin()
@step_common.MateConnector::from_m_rad("snap", origin.xyz, origin.rpy).placement
}
///|
fn joint_type_of(kind : ConnectionKind) -> @urdf.JointType {
match kind {
Fixed => @urdf.JointType::Fixed
Revolute(_) => @urdf.JointType::Revolute
Continuous(_) => @urdf.JointType::Continuous
Prismatic(_) => @urdf.JointType::Prismatic
MergeRigid => @urdf.JointType::Fixed
}
}
///|
fn limit_of(kind : ConnectionKind) -> @urdf.JointLimit? {
match kind {
Revolute(limit) => Some(limit)
Continuous(limit) => limit
Prismatic(limit) => Some(limit)
_ => None
}
}
///|
/// Builds a URDF link from the final bodies in its local frame.
fn build_urdf_link(
link_name : String,
bodies : Array[BodyPart],
mode : VisualMode,
) -> @urdf.Link {
let mut link = @urdf.Link::new(link_name)
if bodies.length() > 0 {
let inertials = bodies.map(fn(b) { b.inertial })
link = link.with_inertial(@urdf.Inertial::combine(inertials))
}
for body in bodies {
for collision in body.collisions {
link = link.add_collision(collision)
}
}
match mode {
Primitives =>
for body in bodies {
let rgba = rgb_to_rgba(body.node.color)
let material = @urdf.Material::color_only(link_name + "_material", rgba)
if body.collisions.length() > 0 {
let c = body.collisions[0]
link = link.add_visual(
@urdf.Visual::new(c.geometry, origin=c.origin).with_material(
material,
),
)
} else {
let (x0, x1, y0, y1, z0, z1) = body.node.bounding_box()
let geometry = @urdf.Geometry::box_mm(x1 - x0, y1 - y0, z1 - z0)
let origin = @urdf.Origin::from_mm_deg(
x=(x0 + x1) / 2.0,
y=(y0 + y1) / 2.0,
z=(z0 + z1) / 2.0,
)
link = link.add_visual(
@urdf.Visual::new(geometry, origin~).with_material(material),
)
}
}
MeshPackage(pkg) =>
if bodies.length() > 0 {
let geometry = @urdf.Geometry::mesh_mm(
"package://\{pkg}/meshes/\{link_name}.gltf",
)
let rgba = rgb_to_rgba(bodies[0].node.color)
let material = @urdf.Material::color_only(link_name + "_material", rgba)
link = link.add_visual(
@urdf.Visual::new(geometry, origin=@urdf.Origin::identity()).with_material(
material,
),
)
}
}
link
}
///|
/// Mutable accumulator for the recursive compile.
priv struct CompileCtx {
links : Array[CompiledLink]
joints : Array[@urdf.Joint]
transmissions : Array[@urdf.Transmission]
mode : VisualMode
}
///|
/// Merges a child component's bodies and mates into an existing link frame,
/// queuing any non-merged sub-attachments onto `kinematic_out`.
fn merge_rigid_into(
bodies : Array[BodyPart],
mates : Map[String, @step_common.MateConnector],
kinematic_out : Array[Attachment],
att : Attachment,
p_mate : @step_common.MateConnector,
c_mate : @step_common.MateConnector,
) -> Unit {
let pair = att.spec.pair
let (snap_placement, _axis) = @step_common.snap_connectors(
p_mate,
c_mate,
offset_mm=pair.offset_mm,
twist_rad=pair.twist_deg * @math.PI / 180.0,
flip_axis=pair.flip_axis,
)
let snap_xform = transform_of_placement(snap_placement)
let snap_shared = placement_of_transform(snap_xform)
for body in att.child.bodies {
bodies.push(body.transform_by(snap_xform))
}
for mate_name, mate in att.child.mates {
let rebased = mate.transformed(snap_shared)
let qualified = "\{att.child.name}.\{mate_name}"
mates.set(qualified, rebased)
if !mates.contains(mate_name) {
mates.set(mate_name, rebased)
}
}
// Re-home the merged child's own attachments onto this link.
for sub in att.child.attachments {
let remapped_pair = @step_common.MatePair::new(
parent="\{att.child.name}.\{sub.spec.pair.parent}",
child=sub.spec.pair.child,
offset_mm=sub.spec.pair.offset_mm,
twist_deg=sub.spec.pair.twist_deg,
flip_axis=sub.spec.pair.flip_axis,
fit=sub.spec.pair.fit,
description=sub.spec.pair.description,
)
let remapped = { ..sub, spec: { ..sub.spec, pair: remapped_pair, }, }
collect_attachment(bodies, mates, kinematic_out, remapped)
}
}
///|
/// First pass over a link's attachments: folds `MergeRigid` children into
/// `bodies` and `mates` immediately and queues kinematic attachments for the
/// second pass.
fn collect_attachment(
bodies : Array[BodyPart],
mates : Map[String, @step_common.MateConnector],
kinematic_out : Array[Attachment],
att : Attachment,
) -> Unit {
match att.spec.kind {
MergeRigid => {
let p_mate = mates
.get(att.spec.pair.parent)
.unwrap_or(identity_mate(att.spec.pair.parent))
let c_mate = att.child
.get_mate(att.spec.pair.child)
.unwrap_or(identity_mate(att.spec.pair.child))
merge_rigid_into(bodies, mates, kinematic_out, att, p_mate, c_mate)
}
_ => kinematic_out.push(att)
}
}
///|
/// Emits a kinematic joint and compiles the child as its own link.
fn attach_kinematic(
ctx : CompileCtx,
link_name : String,
att : Attachment,
p_mate : @step_common.MateConnector,
c_mate : @step_common.MateConnector,
) -> Unit {
let rebase_inv = transform_of_placement(c_mate.placement).inverse()
let rebase_shared = placement_of_transform(rebase_inv)
let child_bodies : Array[BodyPart] = att.child.bodies.map(fn(b) {
b.transform_by(rebase_inv)
})
let child_mates : Map[String, @step_common.MateConnector] = Map([])
for mate_name, mate in att.child.mates {
child_mates.set(mate_name, mate.transformed(rebase_shared))
}
let c_mate_at_origin = @step_common.MateConnector::new(
c_mate.name,
axis=@step_common.normalize_dir(rebase_inv.apply_dir(c_mate.axis)),
)
let pair = att.spec.pair
let mut joint = @urdf.Joint::from_mates(
att.joint_name,
joint_type_of(att.spec.kind),
parent_link=link_name,
parent_mate=p_mate,
child_link=att.child.name,
child_mate=c_mate_at_origin,
offset=pair.offset_mm / 1000.0,
angle_rad=pair.twist_deg * @math.PI / 180.0,
flip_axis=pair.flip_axis,
limit=limit_of(att.spec.kind),
dynamics=att.spec.dynamics,
)
match att.spec.mimic {
Some(mimic) => joint = joint.with_mimic(mimic)
None => ()
}
ctx.joints.push(joint)
match att.spec.transmission_reduction {
Some(reduction) =>
ctx.transmissions.push(
@urdf.Transmission::simple(
"\{att.joint_name}_trans",
att.joint_name,
"\{att.joint_name}_motor",
reduction~,
),
)
None => ()
}
compile_link(
ctx,
att.child.name,
child_bodies,
child_mates,
att.child.attachments,
)
}
///|
/// Collects bodies and mates (following rigid merges), emits this link in
/// topological pre-order, and then recursively compiles its kinematic children.
fn compile_link(
ctx : CompileCtx,
link_name : String,
bodies : Array[BodyPart],
mates : Map[String, @step_common.MateConnector],
attachments : Array[Attachment],
) -> Unit {
let kinematic_atts : Array[Attachment] = []
for att in attachments {
collect_attachment(bodies, mates, kinematic_atts, att)
}
let urdf_link = build_urdf_link(link_name, bodies, ctx.mode)
ctx.links.push({ name: link_name, bodies, urdf_link, })
for att in kinematic_atts {
let p_mate = mates
.get(att.spec.pair.parent)
.unwrap_or(identity_mate(att.spec.pair.parent))
let c_mate = att.child
.get_mate(att.spec.pair.child)
.unwrap_or(identity_mate(att.spec.pair.child))
attach_kinematic(ctx, link_name, att, p_mate, c_mate)
}
}
///|
/// Compiles this component tree into links, joints, and transmissions.
pub fn Component::compile_links(
self : Component,
package_name~ : String? = None,
) -> (Array[CompiledLink], Array[@urdf.Joint], Array[@urdf.Transmission]) {
let mode = match package_name {
Some(pkg) => MeshPackage(pkg)
None => Primitives
}
let ctx : CompileCtx = { links: [], joints: [], transmissions: [], mode, }
let bodies : Array[BodyPart] = []
for body in self.bodies {
bodies.push(body)
}
let mates : Map[String, @step_common.MateConnector] = Map([])
for mate_name, mate in self.mates {
mates.set(mate_name, mate)
}
compile_link(ctx, self.name, bodies, mates, self.attachments)
(ctx.links, ctx.joints, ctx.transmissions)
}
///|
/// Builds a complete URDF robot from the component tree.
pub fn Component::to_robot(
self : Component,
package_name~ : String? = None,
) -> @urdf.Robot {
let (links, joints, transmissions) = self.compile_links(package_name~)
let mut robot = @urdf.Robot::new(
self.display_name(),
documentation=self.description,
)
for link in links {
robot = robot.add_link(link.urdf_link)
}
robot = robot.add_joints(joints)
for transmission in transmissions {
robot = robot.add_transmission(transmission)
}
robot
}
///|
/// Returns a CAD design containing one link's bodies in its local frame.
pub fn Component::link_design(
self : Component,
link_name : String,
) -> @step_cad.Design? {
let (links, _, _) = self.compile_links()
for link in links {
if link.name == link_name {
let mut design = @step_cad.Design::new(name=link.name)
for body in link.bodies {
design = design.add(body.node)
}
return Some(design)
}
}
None
}
///|
/// Builds a ROS 2 URDF bundle with per-link glTF meshes and printable STEP
/// parts.
pub fn Component::to_urdf_bundle(
self : Component,
package_name~ : String = "",
include_step_parts~ : Bool = true,
) -> @urdf.UrdfBundle {
let pkg = if package_name == "" { self.display_name() } else { package_name }
let robot = self.to_robot(package_name=Some(pkg))
let mut bundle = @urdf.UrdfBundle::new(pkg, robot)
let (links, _, _) = self.compile_links(package_name=Some(pkg))
for link in links {
if link.bodies.length() > 0 {
let mut design = @step_cad.Design::new(name=link.name)
for body in link.bodies {
design = design.add(body.node)
}
bundle = bundle.add_mesh_text(
"meshes/\{link.name}.gltf",
gltf_safe_design(design).to_gltf(),
)
if include_step_parts {
bundle = bundle.add_mesh_text(
"parts/\{link.name}.step",
design.to_step_file(),
)
}
}
}
if include_step_parts {
for part in self.printable_parts() {
bundle = bundle.add_mesh_text(
"printable/\{sanitize_part_number(part.part_number)}.step",
part.design.to_step_file(),
)
}
}
bundle
}