///|
/// MuJoCo MJCF serialization.
///|
/// One actuator declaration for the MJCF `` section.
///
/// The tuple fields are `(joint, name, ...)`; use the snake-case constructors
/// (`MjcfActuator::motor`, `::position`, `::velocity`) for readable call sites.
#derive(Debug, Eq)
pub(all) enum MjcfActuator {
/// Torque/force motor: joint, name, ctrl_min, ctrl_max, gear.
Motor(String, String, Double, Double, Double)
/// Position servo: joint, name, kp, kv, ctrl_min, ctrl_max, forcerange.
Position(String, String, Double, Double, Double, Double, Double?)
/// Velocity servo: joint, name, kv, ctrl_min, ctrl_max, forcerange.
Velocity(String, String, Double, Double, Double, Double?)
}
///|
pub fn MjcfActuator::motor(
joint~ : String,
name~ : String = "",
ctrl_min~ : Double,
ctrl_max~ : Double,
gear~ : Double = 1.0,
) -> MjcfActuator {
Motor(joint, name, ctrl_min, ctrl_max, gear)
}
///|
pub fn MjcfActuator::position(
joint~ : String,
name~ : String = "",
kp~ : Double,
kv~ : Double,
ctrl_min~ : Double,
ctrl_max~ : Double,
forcerange? : Double,
) -> MjcfActuator {
Position(joint, name, kp, kv, ctrl_min, ctrl_max, forcerange)
}
///|
pub fn MjcfActuator::velocity(
joint~ : String,
name~ : String = "",
kv~ : Double,
ctrl_min~ : Double,
ctrl_max~ : Double,
forcerange? : Double,
) -> MjcfActuator {
Velocity(joint, name, kv, ctrl_min, ctrl_max, forcerange)
}
///|
/// Simulation-oriented MJCF options.
///
/// The defaults reproduce the original exporter's structure; the CAD compiler
/// fills these in for a standing-pose, contact-clean RL model.
pub(all) struct MjcfOptions {
mesh_dir : String
include_floor : Bool
/// World Z lift for the root free body. Computed from the home pose's
/// lowest collision geometry when `None` and `home_qpos` is non-empty.
root_z_m : Double?
/// Home/standing joint positions (joint name -> radians or metres).
home_qpos : Map[String, Double]
/// Explicit actuators. Empty keeps the legacy "motor for every limited joint"
/// behavior.
actuators : Array[MjcfActuator]
/// Joints to omit from the model (welded bodies), e.g. cosmetic rotors.
freeze_joints : Array[String]
/// Default joint damping (N*m*s/rad) and reflected armature (kg*m^2).
default_damping : Double
default_armature : Double
/// Emit `` pairs for adjacent and loop-constrained bodies.
exclude_adjacent_contacts : Bool
}
///|
pub fn MjcfOptions::new(
mesh_dir~ : String = "meshes",
include_floor~ : Bool = true,
root_z_m~ : Double? = None,
home_qpos~ : Map[String, Double] = Map([]),
actuators~ : Array[MjcfActuator] = [],
freeze_joints~ : Array[String] = [],
default_damping~ : Double = 0.0,
default_armature~ : Double = 0.0,
exclude_adjacent_contacts~ : Bool = false,
) -> MjcfOptions {
{
mesh_dir,
include_floor,
root_z_m,
home_qpos,
actuators,
freeze_joints,
default_damping,
default_armature,
exclude_adjacent_contacts,
}
}
///|
priv struct MjcfAssets {
meshes : Map[String, String]
materials : Map[String, String]
}
///|
fn mjcf_quat(rpy : Vec3) -> String {
let (roll, pitch, yaw) = rpy
let cr = @math.cos(roll * 0.5)
let sr = @math.sin(roll * 0.5)
let cp = @math.cos(pitch * 0.5)
let sp = @math.sin(pitch * 0.5)
let cy = @math.cos(yaw * 0.5)
let sy = @math.sin(yaw * 0.5)
let w = cr * cp * cy + sr * sp * sy
let x = sr * cp * cy - cr * sp * sy
let y = cr * sp * cy + sr * cp * sy
let z = cr * cp * sy - sr * sp * cy
"\{fmt_num(w)} \{fmt_num(x)} \{fmt_num(y)} \{fmt_num(z)}"
}
///|
fn mjcf_basename(path : String) -> String {
match path.rev_find("/") {
Some(index) =>
String::unsafe_substring(path, start=index + 1, end=path.length())
None => path
}
}
///|
fn mjcf_mesh_name(path : String) -> String {
let base = mjcf_basename(path)
base
.replace_all(old="\\", new="_")
.replace_all(old=".", new="_")
.replace_all(old="-", new="_")
.replace_all(old=" ", new="_")
}
///|
fn mjcf_name(name : String) -> String {
name
.replace_all(old="/", new="_")
.replace_all(old="\\", new="_")
.replace_all(old=".", new="_")
.replace_all(old="-", new="_")
.replace_all(old=" ", new="_")
}
///|
fn mjcf_sorted_keys(map : Map[String, String]) -> Array[String] {
let keys : Array[String] = []
for key, _ in map {
keys.push(key)
}
keys.sort_by(fn(a : String, b : String) -> Int32 {
if a < b {
-1
} else if a > b {
1
} else {
0
}
})
keys
}
///|
fn mjcf_collect_assets(robot : Robot) -> MjcfAssets {
let assets : MjcfAssets = { meshes: Map([]), materials: Map([]), }
for material in robot.materials {
match material.color {
Some(color) => assets.materials.set(material.name, fmt_rgba(color))
None => ()
}
}
for link in robot.links {
// Only the geoms that are actually emitted contribute assets: collisions
// take precedence over visuals, matching `mjcf_write_body`.
if link.collisions.length() > 0 {
for collision in link.collisions {
match collision.geometry {
Mesh(path, _) =>
assets.meshes.set(mjcf_mesh_name(path), mjcf_basename(path))
_ => ()
}
}
} else {
for visual in link.visuals {
match visual.geometry {
Mesh(path, _) =>
assets.meshes.set(mjcf_mesh_name(path), mjcf_basename(path))
_ => ()
}
match visual.material {
Some(material) =>
match material.color {
Some(color) =>
if !assets.materials.contains(material.name) {
assets.materials.set(material.name, fmt_rgba(color))
}
None => ()
}
None => ()
}
}
}
}
assets
}
///|
fn mjcf_write_inertial(
sb : StringBuilder,
inertial : Inertial,
level : Int32,
) -> Unit {
let (x, y, z) = inertial.origin.xyz
let (roll, pitch, yaw) = inertial.origin.rpy
pad(sb, level)
sb.write_string(
" 1.0e-12 {
" quat=\"" + mjcf_quat((roll, pitch, yaw)) + "\""
} else {
""
}) +
" mass=\"" +
fmt_num(inertial.mass) +
"\" fullinertia=\"" +
fmt_num(inertial.inertia.ixx) +
" " +
fmt_num(inertial.inertia.iyy) +
" " +
fmt_num(inertial.inertia.izz) +
" " +
fmt_num(inertial.inertia.ixy) +
" " +
fmt_num(inertial.inertia.ixz) +
" " +
fmt_num(inertial.inertia.iyz) +
"\"/>\n",
)
}
///|
fn mjcf_is_frozen(options : MjcfOptions, joint : Joint) -> Bool {
options.freeze_joints.contains(joint.name)
}
///|
fn mjcf_write_joint(
sb : StringBuilder,
joint : Joint,
options : MjcfOptions,
qpos_names : Array[String],
level : Int32,
) -> Unit {
if mjcf_is_frozen(options, joint) {
return
}
match joint.joint_type {
Fixed => ()
Floating => {
pad(sb, level)
sb.write_string("\n")
}
Revolute | Continuous => {
qpos_names.push(joint.name)
pad(sb, level)
sb.write_string(
"
match (limit.lower, limit.upper) {
(Some(lower), Some(upper)) =>
sb.write_string(
" range=\"" +
fmt_num(lower) +
" " +
fmt_num(upper) +
"\" limited=\"true\"",
)
_ => sb.write_string(" limited=\"false\"")
}
None => sb.write_string(" limited=\"false\"")
}
sb.write_string("/>\n")
}
Prismatic => {
qpos_names.push(joint.name)
pad(sb, level)
sb.write_string(
"
match (limit.lower, limit.upper) {
(Some(lower), Some(upper)) =>
sb.write_string(
" range=\"" +
fmt_num(lower) +
" " +
fmt_num(upper) +
"\" limited=\"true\"",
)
_ => sb.write_string(" limited=\"false\"")
}
None => sb.write_string(" limited=\"false\"")
}
sb.write_string("/>\n")
}
Planar => {
qpos_names.push(joint.name)
qpos_names.push(joint.name + "_y")
pad(sb, level)
sb.write_string(
"\n",
)
pad(sb, level)
sb.write_string(
"\n",
)
}
}
}
///|
/// Contact class for one geom: tires/wheels/feet collide with the floor only;
/// structural parts collide with the floor if the robot falls over; internal
/// actuator hardware (stators, gears, bearings, screws) never collides, so a
/// bulky bounding box inside a wheel cannot lift the robot onto its calf.
fn mjcf_contact_attrs(link_name : String, part_name : String) -> String {
let part = part_name.to_lower()
let internal = part.contains("stator") ||
part.contains("_act_") ||
part.contains("act_a12") ||
part.contains("planet") ||
part.contains("herringbone") ||
part.contains("ring") ||
part.contains("sun") ||
part.contains("gear") ||
part.contains("moteus") ||
part.contains("pcb") ||
part.contains("bearing") ||
part.contains("_pin") ||
part.contains("shaft") ||
part.contains("screw") ||
part.contains("bolt") ||
part.contains("iso4762")
if internal {
return " contype=\"0\" conaffinity=\"0\""
}
let lower = link_name.to_lower()
if lower.contains("tire") || lower.contains("wheel") || lower.contains("foot") {
" contype=\"2\" conaffinity=\"1\" friction=\"1.2 0.01 0.001\""
} else {
" contype=\"4\" conaffinity=\"1\" friction=\"1.0 0.01 0.001\""
}
}
///|
fn mjcf_geom_min_z(t : Transform, geometry : Geometry) -> Double? {
match geometry {
Box(size) => {
let (sx, sy, sz) = size
let xs : Array[Double] = [-sx / 2.0, sx / 2.0]
let ys : Array[Double] = [-sy / 2.0, sy / 2.0]
let zs : Array[Double] = [-sz / 2.0, sz / 2.0]
let mut lowest = 1.0e18
for x in xs {
for y in ys {
for z in zs {
let p = t.apply_point((x, y, z))
if p.2 < lowest {
lowest = p.2
}
}
}
}
Some(lowest)
}
Cylinder(radius, length) => {
let axis = t.apply_dir((0.0, 0.0, 1.0))
let vertical = if axis.2.abs() > 1.0 { 1.0 } else { axis.2.abs() }
let horizontal = (1.0 - vertical * vertical).sqrt()
Some(t.trans.2 - (length / 2.0 * vertical + radius * horizontal))
}
Sphere(radius) => Some(t.trans.2 - radius)
Mesh(_, _) => None
}
}
///|
fn mjcf_min_world_z(robot : Robot, qpos : Map[String, Double]) -> Double? {
let fk = robot.forward_kinematics(joint_positions=qpos)
let mut found = false
let mut lowest = 1.0e18
for link in robot.links {
let world = fk.get(link.name).unwrap_or(Transform::identity())
let geoms : Array[(Origin, Geometry)] = if link.collisions.length() > 0 {
link.collisions.map(fn(collision) {
(collision.origin, collision.geometry)
})
} else {
link.visuals.map(fn(visual) { (visual.origin, visual.geometry) })
}
for entry in geoms {
let frame = world.compose(Transform::from_origin(entry.0))
match mjcf_geom_min_z(frame, entry.1) {
Some(z) => {
if z < lowest {
lowest = z
}
found = true
}
None => ()
}
}
}
if found {
Some(lowest)
} else {
None
}
}
///|
/// Sites used by `` for each loop constraint: the first
/// anchor lives on body1, the second on body2 (both in their link frames).
fn mjcf_loop_sites(robot : Robot) -> Map[String, Array[(String, Vec3)]] {
let sites : Map[String, Array[(String, Vec3)]] = Map([])
for constraint in robot.loop_constraints {
let base = mjcf_name(constraint.name)
let first = (base + "_site1", constraint.anchor1)
let second = (base + "_site2", constraint.anchor2)
match sites.get(constraint.body1) {
Some(entries) => entries.push(first)
None => sites.set(constraint.body1, [first])
}
match sites.get(constraint.body2) {
Some(entries) => entries.push(second)
None => sites.set(constraint.body2, [second])
}
}
sites
}
///|
fn mjcf_pair_key(a : String, b : String) -> String {
if a < b {
a + "|" + b
} else {
b + "|" + a
}
}
///|
fn mjcf_add_pair(
pairs : Array[(String, String)],
keys : Array[String],
a : String,
b : String,
) -> Unit {
if a == b {
return
}
let key = mjcf_pair_key(a, b)
if !keys.contains(key) {
keys.push(key)
pairs.push((a, b))
}
}
///|
fn mjcf_contact_pairs(robot : Robot) -> Array[(String, String)] {
let pairs : Array[(String, String)] = []
let keys : Array[String] = []
for joint in robot.joints {
mjcf_add_pair(pairs, keys, joint.parent, joint.child)
}
for constraint in robot.loop_constraints {
mjcf_add_pair(pairs, keys, constraint.body1, constraint.body2)
}
let groups : Map[String, Array[String]] = Map([])
for joint in robot.joints {
match groups.get(joint.parent) {
Some(children) => children.push(joint.child)
None => groups.set(joint.parent, [joint.child])
}
}
for _parent, children in groups {
for i in 0.. Unit {
pad(sb, level)
sb.write_string(" {
let (sx, sy, sz) = size
sb.write_string(
" type=\"box\" size=\"" +
fmt_vec3((sx / 2.0, sy / 2.0, sz / 2.0)) +
"\"",
)
}
Cylinder(radius, length) =>
sb.write_string(
" type=\"cylinder\" size=\"" +
fmt_num(radius) +
" " +
fmt_num(length / 2.0) +
"\"",
)
Sphere(radius) =>
sb.write_string(" type=\"sphere\" size=\"" + fmt_num(radius) + "\"")
Mesh(path, _) =>
sb.write_string(
" type=\"mesh\" mesh=\"" + xml_escape(mjcf_mesh_name(path)) + "\"",
)
}
sb.write_string(" pos=\"" + fmt_vec3(origin.xyz) + "\"")
let (roll, pitch, yaw) = origin.rpy
if roll.abs() + pitch.abs() + yaw.abs() > 1.0e-12 {
sb.write_string(" quat=\"" + mjcf_quat((roll, pitch, yaw)) + "\"")
}
match material {
Some(mat) =>
if assets.materials.contains(mat.name) {
sb.write_string(" material=\"" + xml_escape(mat.name) + "\"")
}
None => ()
}
sb.write_string(mjcf_contact_attrs(link_name, part_name))
sb.write_string("/>\n")
}
///|
fn mjcf_write_body(
sb : StringBuilder,
robot : Robot,
link : Link,
incoming_joint : Joint?,
level : Int32,
links : Map[String, Link],
children : Map[String, Array[Joint]],
assets : MjcfAssets,
imu_sites : Array[String],
options : MjcfOptions,
root_z : Double,
loop_sites : Map[String, Array[(String, Vec3)]],
qpos_names : Array[String],
) -> Unit {
pad(sb, level)
sb.write_string("
sb.write_string(
" pos=\"" +
fmt_vec3(joint.origin.xyz) +
"\" quat=\"" +
mjcf_quat(joint.origin.rpy) +
"\"",
)
None => sb.write_string(" pos=\"0 0 " + fmt_num(root_z) + "\"")
}
sb.write_string(">\n")
match incoming_joint {
None => {
pad(sb, level + 1)
sb.write_string("\n")
}
Some(joint) => mjcf_write_joint(sb, joint, options, qpos_names, level + 1)
}
match link.inertial {
Some(inertial) => mjcf_write_inertial(sb, inertial, level + 1)
None => ()
}
// MuJoCo requires unique geom names; link name + ordinal is unique across
// the model even when a STEP component repeats a layer name.
let mut geom_index = 0
if link.collisions.length() > 0 {
for collision in link.collisions {
mjcf_write_geom(
sb,
link.name + "_geom_" + geom_index.to_string(),
collision.origin,
collision.geometry,
None,
assets,
link.name,
match collision.name {
Some(value) => value
None => link.name
},
level + 1,
)
geom_index = geom_index + 1
}
} else {
for visual in link.visuals {
mjcf_write_geom(
sb,
link.name + "_geom_" + geom_index.to_string(),
visual.origin,
visual.geometry,
visual.material,
assets,
link.name,
link.name,
level + 1,
)
geom_index = geom_index + 1
}
}
if link.name.to_lower().contains("imu") {
let site = link.name + "_imu"
pad(sb, level + 1)
sb.write_string(
"\n",
)
imu_sites.push(site)
}
// Loop-constraint sites: `` ties these exact points.
match loop_sites.get(link.name) {
Some(entries) =>
for entry in entries {
pad(sb, level + 1)
sb.write_string(
"\n",
)
}
None => ()
}
match children.get(link.name) {
Some(outgoing) =>
for joint in outgoing {
match links.get(joint.child) {
Some(child_link) =>
mjcf_write_body(
sb,
robot,
child_link,
Some(joint),
level + 1,
links,
children,
assets,
imu_sites,
options,
root_z,
loop_sites,
qpos_names,
)
None => ()
}
}
None => ()
}
pad(sb, level)
sb.write_string("\n")
}
///|
fn mjcf_forcerange(value : Double?) -> String {
match value {
Some(limit) =>
" forcerange=\"-" + fmt_num(limit) + " " + fmt_num(limit) + "\""
None => ""
}
}
///|
fn mjcf_write_actuator(sb : StringBuilder, actuator : MjcfActuator) -> Unit {
match actuator {
Motor(joint, name, ctrl_min, ctrl_max, gear) => {
pad(sb, 2)
sb.write_string(
" 1.0e-9 {
" gear=\"" + fmt_num(gear) + "\""
} else {
""
}) +
"/>\n",
)
}
Position(joint, name, kp, kv, ctrl_min, ctrl_max, forcerange) => {
pad(sb, 2)
sb.write_string(
"\n",
)
}
Velocity(joint, name, kv, ctrl_min, ctrl_max, forcerange) => {
pad(sb, 2)
sb.write_string(
"\n",
)
}
}
}
///|
fn mjcf_write_actuators(
sb : StringBuilder,
robot : Robot,
options : MjcfOptions,
) -> Unit {
let explicit = options.actuators
let legacy : Array[Joint] = []
if explicit.length() == 0 {
for joint in robot.joints {
let actuated = match joint.joint_type {
Revolute | Continuous | Prismatic => true
_ => false
}
if actuated && !mjcf_is_frozen(options, joint) {
match joint.limit {
Some(limit) => if limit.effort > 0.0 { legacy.push(joint) }
None => ()
}
}
}
}
if explicit.length() == 0 && legacy.length() == 0 {
return
}
pad(sb, 1)
sb.write_string("\n")
if explicit.length() > 0 {
for actuator in explicit {
mjcf_write_actuator(sb, actuator)
}
} else {
for joint in legacy {
let effort = match joint.limit {
Some(limit) => limit.effort
None => 0.0
}
mjcf_write_actuator(
sb,
MjcfActuator::motor(joint=joint.name, ctrl_min=-effort, ctrl_max=effort),
)
}
}
pad(sb, 1)
sb.write_string("\n")
}
///|
/// Converts the robot to MuJoCo MJCF XML with simulation options.
pub fn Robot::to_mjcf_with_options(
self : Robot,
options : MjcfOptions,
) -> String {
let root_z = match options.root_z_m {
Some(z) => z
None =>
if options.home_qpos.length() > 0 {
match mjcf_min_world_z(self, options.home_qpos) {
Some(lowest) => if lowest < 0.0 { -lowest + 0.004 } else { 0.0 }
None => 0.0
}
} else {
0.0
}
}
let sb = StringBuilder()
sb.write_string("\n")
sb.write_string("\n")
pad(sb, 1)
sb.write_string(
"\n",
)
pad(sb, 1)
sb.write_string("\n")
sb.to_string()
}
///|
/// Converts the robot to MuJoCo MJCF XML with the legacy defaults.
///
/// The root link is floating (``), collisions are used as geoms
/// when present (visuals otherwise), and every `LoopConstraint` becomes a
/// native `` between two site frames (`site1`/`site2`)
/// placed at the constraint anchors.
pub fn Robot::to_mjcf_string(
self : Robot,
mesh_dir~ : String = "meshes",
include_floor~ : Bool = true,
) -> String {
self.to_mjcf_with_options(MjcfOptions::new(mesh_dir~, include_floor~))
}