use std::ffi::{c_char, c_int, CStr, CString};
use std::marker::PhantomData;
use std::ptr::{self, NonNull};
use std::path::Path;
use crate::error::MjEditError;
#[macro_use]
mod utility;
use utility::*;
mod traits;
pub use traits::*;
mod default;
pub use default::*;
use super::mj_model::{
MjModel, MjtObj, MjtGeom, MjtJoint, MjtCamLight,
MjtLightType, MjtSensor, MjtDataType, MjtGain,
MjtBias, MjtDyn, MjtEq, MjtTexture, MjtColorSpace,
MjtTrn, MjtStage, MjtFlexSelf, MjtProjection,
MjtSleepPolicy, MjtWrap, MjtTextureRole, MjtCubeFace
};
use super::mj_auxiliary::{MjVfs, MjVisual, MjStatistic, MjLROpt};
use super::mj_option::MjOption;
use super::mj_primitive::*;
use crate::mujoco_c::*;
use crate::getter_setter;
use crate::mujoco_c::{mjs_addHField as mjs_addHfield, mjsHField as mjsHfield, mjs_asHField as mjs_asHfield};
use crate::util::{assert_mujoco_version, ERROR_BUF_LEN};
fn check_objtype(t: MjtObj) -> Result<(), MjEditError> {
if (t as i32) < (MjtObj::mjNOBJECT as i32) {
Ok(())
} else {
Err(MjEditError::InvalidParameter(format!(
"object type must be a real MjtObj below MjtObj::mjNOBJECT, got {t:?}"
)))
}
}
fn check_numeric_size(size: i32) -> Result<(), MjEditError> {
if size < 0 {
Err(MjEditError::InvalidParameter(format!(
"numeric size must be non-negative, got {size}"
)))
} else {
Ok(())
}
}
pub type MjtGeomInertia = mjtGeomInertia;
pub type MjtMeshInertia = mjtMeshInertia;
pub type MjtBuiltin = mjtBuiltin;
pub type MjtMeshBuiltin = mjtMeshBuiltin;
pub type MjtMark = mjtMark;
pub type MjtLimited = mjtLimited;
pub type MjtAlignFree = mjtAlignFree;
pub type MjtInertiaFromGeom = mjtInertiaFromGeom;
pub type MjtOrientation = mjtOrientation;
pub type MjtCTimer = mjtCTimer;
pub type MjsOrientation = mjsOrientation;
impl MjsOrientation {
pub fn set_euler(&mut self, angle: &[f64; 3]) {
self.type_ = MjtOrientation::mjORIENTATION_EULER;
self.euler = *angle;
}
pub fn set_axis_angle(&mut self, angle: &[f64; 4]) {
self.type_ = MjtOrientation::mjORIENTATION_AXISANGLE;
self.axisangle = *angle;
}
pub fn set_xy_axis(&mut self, angle: &[f64; 6]) {
self.type_ = MjtOrientation::mjORIENTATION_XYAXES;
self.xyaxes = *angle;
}
pub fn set_z_axis(&mut self, angle: &[f64; 3]) {
self.type_ = MjtOrientation::mjORIENTATION_ZAXIS;
self.zaxis = *angle;
}
pub fn switch_quat(&mut self) {
self.type_ = MjtOrientation::mjORIENTATION_QUAT;
}
}
pub type MjsCompiler = mjsCompiler;
impl MjsCompiler {
getter_setter! {[&] with, get, set, [
autolimits: bool; "infer \"limited\" attribute based on range.";
balanceinertia: bool; "automatically impose A + B >= C rule.";
fitaabb: bool; "meshfit to aabb instead of inertia box.";
degree: bool; "angles in radians or degrees.";
discardvisual: bool; "discard visual geoms in parser.";
usethread: bool; "use multiple threads to speed up compiler.";
fusestatic: bool; "fuse static bodies with parent.";
saveinertial: bool; "save explicit inertial clause for all bodies to XML.";
alignfree: bool; "align free joints with inertial frame.";
]}
getter_setter! {[&] with, get, set, [
boundmass: f64; "enforce minimum body mass.";
boundinertia: f64; "enforce minimum body diagonal inertia.";
settotalmass: f64; "rescale masses and inertias; <=0: ignore.";
]}
getter_setter! {[&] with, get, set, [
inertiafromgeom: MjtInertiaFromGeom [force]; "use geom inertias.";
]}
getter_setter! {[&] with, get, [
inertiagrouprange: &[i32; 2]; "range of geom groups used to compute inertia.";
eulerseq: &[c_char; 3]; "sequence for euler rotations.";
LRopt: &MjLROpt; "options for lengthrange computation.";
]}
string_set_get_with! {[&]
meshdir; "mesh and hfield directory.";
texturedir; "texture directory.";
}
}
#[derive(Debug)]
pub struct MjSpec(NonNull<mjSpec>);
unsafe impl Send for MjSpec {}
impl MjSpec {
pub fn new() -> Self {
Self::try_new().expect("MuJoCo failed to allocate MjSpec")
}
pub fn try_new() -> Result<Self, MjEditError> {
assert_mujoco_version();
let ptr = unsafe { mj_makeSpec() };
Ok(MjSpec(NonNull::new(ptr).ok_or(MjEditError::AllocationFailed)?))
}
pub fn try_clone(&self) -> Result<Self, MjEditError> {
let ptr = unsafe { mj_copySpec(self.0.as_ptr()) };
NonNull::new(ptr).map(MjSpec).ok_or(MjEditError::AllocationFailed)
}
pub fn from_xml<T: AsRef<Path>>(path: T) -> Result<Self, MjEditError> {
Self::from_xml_file(path, None)
}
pub fn from_xml_vfs<T: AsRef<Path>>(path: T, vfs: &MjVfs) -> Result<Self, MjEditError> {
Self::from_xml_file(path, Some(vfs))
}
fn from_xml_file<T: AsRef<Path>>(path: T, vfs: Option<&MjVfs>) -> Result<Self, MjEditError> {
assert_mujoco_version();
let mut error_buffer = [0; ERROR_BUF_LEN];
unsafe {
let path_str = path.as_ref().to_str()
.ok_or(MjEditError::InvalidUtf8Path)?;
let path = CString::new(path_str).unwrap();
let raw_ptr = mj_parseXML(
path.as_ptr(), vfs.map_or(ptr::null(), |v| v.ffi()),
error_buffer.as_mut_ptr(), error_buffer.len() as c_int
);
Self::check_spec(raw_ptr, &error_buffer)
}
}
pub fn from_xml_string(xml: &str) -> Result<Self, MjEditError> {
assert_mujoco_version();
let c_xml = CString::new(xml).unwrap();
let mut error_buffer = [0; ERROR_BUF_LEN];
unsafe {
let spec_ptr = mj_parseXMLString(
c_xml.as_ptr(), ptr::null(),
error_buffer.as_mut_ptr(), error_buffer.len() as c_int
);
Self::check_spec(spec_ptr, &error_buffer)
}
}
pub fn from_parse<T: AsRef<Path>>(filename: T, content_type: &str) -> Result<Self, MjEditError> {
Self::from_parse_file(filename, content_type, None)
}
pub fn from_parse_vfs<T: AsRef<Path>>(filename: T, content_type: &str, vfs: &MjVfs) -> Result<Self, MjEditError> {
Self::from_parse_file(filename, content_type, Some(vfs))
}
fn from_parse_file<T: AsRef<Path>>(filename: T, content_type: &str, vfs: Option<&MjVfs>) -> Result<Self, MjEditError> {
assert_mujoco_version();
let mut error_buffer = [0; ERROR_BUF_LEN];
unsafe {
let c_filename = CString::new(
filename.as_ref().to_str()
.ok_or(MjEditError::InvalidUtf8Path)?
).unwrap();
let c_content_type = CString::new(content_type).unwrap();
let ptr = mj_parse(
c_filename.as_ptr(), c_content_type.as_ptr(),
vfs.map_or(ptr::null(), |v| v.ffi()),
error_buffer.as_mut_ptr(), error_buffer.len() as i32
);
Self::check_spec(ptr, &error_buffer)
}
}
fn check_spec(spec_ptr: *mut mjSpec, error_buffer: &[c_char]) -> Result<Self, MjEditError> {
if spec_ptr.is_null() {
let message = unsafe { CStr::from_ptr(error_buffer.as_ptr()) }
.to_string_lossy()
.into_owned();
Err(MjEditError::ParseFailed(message))
}
else {
Ok(MjSpec(unsafe { NonNull::new_unchecked(spec_ptr) }))
}
}
pub fn ffi(&self) -> &mjSpec {
unsafe { self.0.as_ref() }
}
pub unsafe fn ffi_mut(&mut self) -> &mut mjSpec {
unsafe { self.0.as_mut() }
}
pub unsafe fn delete_element(&mut self, element: *mut mjsElement) -> Result<(), MjEditError> {
if element.is_null() {
return Err(MjEditError::DeleteFailed("null element pointer".to_owned()));
}
let spec = unsafe { self.ffi_mut() };
let owner = unsafe { mjs_getSpec(element) };
if owner != spec {
return Err(MjEditError::DeleteFailed("element does not belong to this spec".to_owned()));
}
if unsafe { (*element).elemtype } == MjtObj::mjOBJ_DEFAULT {
return Err(MjEditError::UnsupportedOperation);
}
if unsafe { (*element).elemtype } == MjtObj::mjOBJ_BODY {
let name = unsafe { read_mjs_string(mjs_getName(element)) };
if name == "world" {
return Err(MjEditError::UnsupportedOperation);
}
}
let result = unsafe { mjs_delete(spec, element) };
match result {
0 => Ok(()),
_ => {
let error_msg = unsafe {
let ptr = mjs_getError(spec);
if ptr.is_null() {
"Unknown error".to_owned()
} else {
CStr::from_ptr(ptr).to_string_lossy().into_owned()
}
};
Err(MjEditError::DeleteFailed(error_msg))
}
}
}
pub fn compile(&mut self) -> Result<MjModel, MjEditError> {
for texture in self.texture_iter() {
if texture.builtin() != MjtBuiltin::mjBUILTIN_NONE && texture.nchannel() < 3 {
return Err(MjEditError::CompileFailed(
"texture with a builtin pattern requires nchannel >= 3".to_owned(),
));
}
}
let result = unsafe { MjModel::from_raw( mj_compile(self.0.as_ptr(), ptr::null()) ) };
result.map_err(|_| {
let error_msg: String = unsafe {
let ptr = mjs_getError(self.ffi_mut());
if ptr.is_null() {
"Compilation failed (unknown error)".to_owned()
} else {
CStr::from_ptr(ptr).to_string_lossy().into_owned()
}
};
MjEditError::CompileFailed(error_msg)
})
}
pub fn timer(&self) -> &[f64; MjtCTimer::mjNCTIMER as usize] {
unsafe { &*mjs_getTimer(self.0.as_ptr()).cast() }
}
pub fn save_xml<T: AsRef<Path>>(&self, filename: T) -> Result<(), MjEditError> {
let mut error_buff = [0; ERROR_BUF_LEN];
let cname = CString::new(
filename.as_ref().to_str()
.ok_or(MjEditError::InvalidUtf8Path)?
).unwrap(); let result = unsafe { mj_saveXML(
self.ffi(), cname.as_ptr(),
error_buff.as_mut_ptr(), error_buff.len() as i32
) };
match result {
0 => Ok(()),
_ => {
let message = unsafe { CStr::from_ptr(error_buff.as_ptr()) }
.to_string_lossy()
.into_owned();
Err(MjEditError::SaveFailed(message))
}
}
}
pub fn save_xml_string(&self, buffer_size: usize) -> Result<String, MjEditError> {
let mut error_buff = [0; ERROR_BUF_LEN];
let mut result_buff = vec![0u8; buffer_size];
let result = unsafe { mj_saveXMLString(
self.ffi(), result_buff.as_mut_ptr().cast(), result_buff.len() as i32,
error_buff.as_mut_ptr(), error_buff.len() as i32
) };
match result {
0 => Ok(CStr::from_bytes_until_nul(&result_buff).unwrap().to_string_lossy().into_owned()),
r if r > 0 => Err(MjEditError::XmlBufferTooSmall { required_size: r as usize }),
_ => {
let message = unsafe { CStr::from_ptr(error_buff.as_ptr()) }
.to_string_lossy()
.into_owned();
Err(MjEditError::SaveFailed(message))
}
}
}
}
impl MjSpec {
find_x_method! {
body, geom, joint, site, camera, light, frame, actuator, sensor, flex, pair, equality, exclude, tendon,
numeric, text, tuple, key, mesh, hfield, skin, texture, material, plugin
}
find_x_method_direct! { default }
pub fn world_body(&self) -> &MjsBody {
self.body("world").unwrap()
}
pub fn world_body_mut(&mut self) -> &mut MjsBody {
self.body_mut("world").unwrap()
}
}
impl MjSpec {
string_set_get_with! {
[ffi, ffi_mut] modelname; "model name.";
[ffi, ffi_mut] comment; "comment at top of XML.";
[ffi, ffi_mut] modelfiledir; "path to model file.";
}
getter_setter! {
with, get, [
[ffi, ffi_mut] compiler: &MjsCompiler; "compiler options.";
[ffi, ffi_mut] stat: &MjStatistic; "statistic overrides.";
[ffi, ffi_mut] visual: &MjVisual; "visualization options.";
[ffi, ffi_mut] option: &MjOption; "simulation options.";
]
}
getter_setter! {
with, get, set, [
[ffi, ffi_mut] strippath: bool; "whether to strip paths from mesh files.";
[ffi, ffi_mut] hasImplicitPluginElem: bool; "already encountered an implicit plugin sensor/actuator.";
]
}
getter_setter! {
get, [
[ffi] memory: MjtSize; "number of bytes in arena+stack memory.";
[ffi] nemax: i32; "max number of equality constraints.";
[ffi] nuserdata: i32; "number of mjtNums in userdata.";
[ffi] nuser_body: i32; "number of mjtNums in body_user.";
[ffi] nuser_jnt: i32; "number of mjtNums in jnt_user.";
[ffi] nuser_geom: i32; "number of mjtNums in geom_user.";
[ffi] nuser_site: i32; "number of mjtNums in site_user.";
[ffi] nuser_cam: i32; "number of mjtNums in cam_user.";
[ffi] nuser_tendon: i32; "number of mjtNums in tendon_user.";
[ffi] nuser_actuator: i32; "number of mjtNums in actuator_user.";
[ffi] nuser_sensor: i32; "number of mjtNums in sensor_user.";
[ffi] nkey: i32; "number of keyframes.";
]
}
}
impl MjSpec {
add_x_method! { actuator, pair, equality, tendon, mesh, material }
add_x_method_no_default! {
sensor, flex, exclude, numeric, text, tuple, key, plugin,
hfield, skin, texture
}
pub fn add_default(&mut self, class_name: &str, parent_class_name: Option<&str>) -> &mut MjsDefault {
self.try_add_default(class_name, parent_class_name).unwrap()
}
pub fn try_add_default(&mut self, class_name: &str, parent_class_name: Option<&str>) -> Result<&mut MjsDefault, MjEditError> {
let c_class_name = CString::new(class_name).unwrap();
let parent_ptr = if let Some(name) = parent_class_name {
self.default(name).ok_or(MjEditError::NotFound)?
} else {
ptr::null()
};
unsafe {
let ptr_default = mjs_addDefault(
self.ffi_mut(),
c_class_name.as_ptr(),
parent_ptr
);
if ptr_default.is_null() {
Err(MjEditError::AlreadyExists)
}
else {
Ok(&mut *ptr_default)
}
}
}
}
#[derive(Debug)]
pub struct MjsSpecItemIterMut<'a, T> {
ffi_ptr: *mut mjSpec,
last: *mut mjsElement,
item_type: PhantomData<&'a mut T>
}
#[derive(Debug, Clone)]
pub struct MjsSpecItemIter<'a, T> {
ffi_ptr: *const mjSpec,
last: *mut mjsElement,
item_type: PhantomData<&'a T>
}
impl<'a, T: SpecObject> MjsSpecItemIterMut<'a, T> {
fn new(root: &'a mut MjSpec) -> Self {
let last = unsafe { mjs_firstElement(root.0.as_ptr(), T::OBJ_TYPE) };
Self { ffi_ptr: root.0.as_ptr(), last, item_type: PhantomData }
}
}
impl<'a, T: SpecObject> MjsSpecItemIter<'a, T> {
fn new(root: &'a MjSpec) -> Self {
let last = unsafe { mjs_firstElement(root.0.as_ptr(), T::OBJ_TYPE) };
Self { ffi_ptr: root.0.as_ptr(), last, item_type: PhantomData }
}
}
impl<'a, T: SpecObject + 'a> Iterator for MjsSpecItemIterMut<'a, T> {
type Item = &'a mut T;
fn next(&mut self) -> Option<Self::Item> {
if self.last.is_null() {
return None;
}
unsafe {
let out = T::from_element_as_ptr_mut(self.last).as_mut();
self.last = mjs_nextElement(self.ffi_ptr, self.last);
out
}
}
}
impl<'a, T: SpecObject + 'a> Iterator for MjsSpecItemIter<'a, T> {
type Item = &'a T;
fn next(&mut self) -> Option<Self::Item> {
if self.last.is_null() {
return None;
}
unsafe {
let out = T::from_element_as_ptr_mut(self.last).as_ref();
self.last = mjs_nextElement(self.ffi_ptr, self.last);
out
}
}
}
impl<'a, T: SpecObject + 'a> std::iter::FusedIterator for MjsSpecItemIterMut<'a, T> {}
impl<'a, T: SpecObject + 'a> std::iter::FusedIterator for MjsSpecItemIter<'a, T> {}
impl MjSpec {
spec_get_iter! {
body, geom, joint, site, camera, light, frame, actuator, sensor, flex, pair, equality,
exclude, tendon, numeric, text, tuple, key, mesh, hfield, skin, texture, material, plugin
}
}
impl Default for MjSpec {
fn default() -> Self {
Self::new()
}
}
impl Drop for MjSpec {
fn drop(&mut self) {
unsafe { mj_deleteSpec(self.0.as_ptr()); }
}
}
impl Clone for MjSpec {
fn clone(&self) -> Self {
self.try_clone().expect("MuJoCo failed to clone MjSpec")
}
}
mjs_struct!(Site [SpecObject]);
impl MjsSite {
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "position.";
quat: &[f64; 4]; "orientation.";
alt: &MjsOrientation; "alternative orientation.";
fromto: &[f64; 6]; "alternative for capsule, cylinder, box, ellipsoid.";
size: &[f64; 3]; "geom size.";
rgba: &[f32; 4]; "rgba when material is omitted.";
]}
getter_setter!([&] with, get, set, [
type_ + _: MjtGeom; "geom type.";
group: i32; "group.";
]);
userdata_method!(f64);
string_set_get_with! {[&]
material; "name of material.";
}
}
mjs_struct!(Joint [SpecObject]);
impl MjsJoint {
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "anchor position.";
axis: &[f64; 3]; "joint axis.";
ref_ + _: &f64; "value at reference configuration: qpos0.";
springdamper: &[f64; 2]; "timeconst, dampratio.";
stiffness: &[f64; mjNPOLY as usize + 1]; "stiffness coefficient.";
range: &[f64; 2]; "joint limits.";
solref_limit: &[MjtNum; mjNREF as usize]; "solver reference: joint limits.";
solimp_limit: &[MjtNum; mjNIMP as usize]; "solver impedance: joint limits.";
actfrcrange: &[f64; 2]; "actuator force limits.";
damping: &[f64; mjNPOLY as usize + 1]; "damping coefficient.";
solref_friction: &[MjtNum; mjNREF as usize]; "solver reference: dof friction.";
solimp_friction: &[MjtNum; mjNIMP as usize]; "solver impedance: dof friction.";
]
}
getter_setter!([&] with, get, set, [
type_ + _: MjtJoint; "joint type.";
group: i32; "joint group.";
springref: f64; "spring reference value: qpos_spring.";
margin: f64; "margin value for joint limit detection.";
armature: f64; "armature inertia (mass for slider).";
frictionloss: f64; "friction loss.";
]);
getter_setter! {
[&] with, get, set, [
align: MjtAlignFree [force]; "align free joint with body com (mjtAlignFree).";
limited: MjtLimited [force]; "does joint have limits (mjtLimited).";
actfrclimited: MjtLimited [force]; "are actuator forces on joint limited (mjtLimited).";
]
}
getter_setter! {
[&] with, get, set, [
actgravcomp: bool; "is gravcomp force applied via actuators.";
]
}
userdata_method!(f64);
}
mjs_struct!(Geom [SpecObject]);
impl MjsGeom {
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "geom position.";
quat: &[f64; 4]; "geom orientation.";
alt: &MjsOrientation; "alternative orientation.";
fromto: &[f64; 6]; "alternative for capsule, cylinder, box, ellipsoid.";
size: &[f64; 3]; "geom size.";
rgba: &[f32; 4]; "rgba when material is omitted.";
friction: &[f64; 3]; "one-sided friction coefficients: slide, roll, spin.";
solref: &[MjtNum; mjNREF as usize]; "solver reference.";
solimp: &[MjtNum; mjNIMP as usize]; "solver impedance.";
fluid_coefs: &[MjtNum; 5]; "ellipsoid-fluid interaction coefs."
]
}
getter_setter! {
get, [
plugin: &MjsPlugin; "sdf plugin.";
]
}
getter_setter!([&] with, get, set, [
type_ + _: MjtGeom; "geom type.";
group: i32; "group.";
contype: i32; "contact type.";
conaffinity: i32; "contact affinity.";
condim: i32; "contact dimensionality.";
priority: i32; "contact priority.";
solmix: f64; "solver mixing for contact pairs.";
margin: f64; "margin for contact detection.";
gap: f64; "additional contact detection buffer.";
mass: f64; "used to compute density.";
density: f64; "used to compute mass and inertia from volume or surface.";
typeinertia: MjtGeomInertia; "selects between surface and volume inertia.";
fluid_ellipsoid: MjtNum; "whether ellipsoid-fluid model is active.";
fitscale: f64; "scale mesh uniformly.";
]);
userdata_method!(f64);
string_set_get_with! {[&]
meshname; "mesh attached to geom.";
material; "name of material.";
hfieldname; "heightfield attached to geom.";
}
}
mjs_struct!(Camera [SpecObject]);
impl MjsCamera {
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "camera position.";
quat: &[f64; 4]; "camera orientation.";
alt: &MjsOrientation; "alternative orientation.";
intrinsic: &[f32; 4]; "intrinsic parameters.";
sensor_size: &[f32; 2]; "sensor size.";
resolution: &[i32; 2]; "resolution.";
focal_length: &[f32; 2]; "focal length (length).";
focal_pixel: &[f32; 2]; "focal length (pixel).";
principal_length: &[f32; 2]; "principal point (length).";
principal_pixel: &[f32; 2]; "principal point (pixel).";
]
}
getter_setter!([&] with, get, set, [
mode: MjtCamLight; "camera mode.";
fovy: f64; "field of view in y direction.";
ipd: f64; "inter-pupillary distance for stereo.";
proj: MjtProjection; "camera projection type.";
output: i32; "bit flags for output type.";
]);
userdata_method!(f64);
string_set_get_with! {[&]
targetbody; "target body for tracking/targeting.";
}
}
mjs_struct!(Light [SpecObject]);
impl MjsLight {
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "light position.";
dir: &[f64; 3]; "light direction.";
ambient: &[f32; 3]; "ambient color.";
diffuse: &[f32; 3]; "diffuse color.";
specular: &[f32; 3]; "specular color.";
attenuation: &[f32; 3]; "OpenGL attenuation (quadratic model).";
]
}
getter_setter!([&] with, get, set, [
mode: MjtCamLight; "light mode.";
type_ + _: MjtLightType; "light type.";
bulbradius: f32; "bulb radius, for soft shadows.";
intensity: f32; "intensity, in candelas.";
range: f32; "range of effectiveness.";
cutoff: f32; "OpenGL cutoff.";
exponent: f32; "OpenGL exponent.";
]);
getter_setter! {
[&] with, get, set, [
active: bool; "active flag.";
castshadow: bool; "whether light cast shadows."
]
}
string_set_get_with! {[&]
texture; "texture name for image lights.";
targetbody; "target body for targeting.";
}
}
mjs_struct!(Frame [SpecObject]);
impl MjsFrame {
add_x_method_by_frame! { body, site, joint, geom, camera, light }
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "frame position.";
quat: &[f64; 4]; "frame orientation.";
alt: &MjsOrientation; "alternative orientation.";
]
}
string_set_get_with! {[&]
childclass; "childclass name.";
}
pub fn add_frame(&mut self) -> &mut MjsFrame {
self.try_add_frame().expect("mjs_addFrame returned null; allocation failed")
}
pub fn try_add_frame(&mut self) -> Result<&mut MjsFrame, MjEditError> {
let parent_body = unsafe { mjs_getParent(self.element_mut_pointer()) };
debug_assert!(!parent_body.is_null(), "mjs_getParent returned null; frame has no parent body");
let ptr = unsafe { mjs_addFrame(parent_body, self) };
unsafe { ptr.as_mut() }.ok_or(MjEditError::AllocationFailed)
}
}
mjs_struct!(Actuator [SpecObject]);
impl MjsActuator {
getter_setter! {
[&] with, get, [
gear: &[f64; 6]; "gear parameters.";
gainprm: &[f64; mjNGAIN as usize]; "gain parameters.";
biasprm: &[f64; mjNBIAS as usize]; "bias parameters.";
dynprm: &[f64; mjNDYN as usize]; "dynamic parameters.";
lengthrange: &[f64; 2]; "transmission length range.";
damping: &[f64; mjNPOLY as usize + 1]; "damping coefficient.";
ctrlrange: &[f64; 2]; "control range.";
forcerange: &[f64; 2]; "force range.";
actrange: &[f64; 2]; "activation range.";
]
}
getter_setter! {
get, [
plugin: &MjsPlugin; "actuator plugin.";
]
}
getter_setter!([&] with, get, set, [
gaintype: MjtGain; "gain type.";
biastype: MjtBias; "bias type.";
dyntype: MjtDyn; "dyn type.";
group: i32; "group.";
actdim: i32; "number of activation variables.";
trntype: MjtTrn; "transmission type.";
cranklength: f64; "crank length, for slider-crank.";
inheritrange: f64; "automatic range setting for position and intvelocity.";
armature: f64; "armature inertia.";
nsample: i32; "number of samples in history buffer.";
interp: i32; "interpolation order (0=ZOH, 1=linear, 2=cubic).";
delay: f64; "delay time in seconds; 0: no delay.";
]);
getter_setter! {
[&] with, get, set, [
ctrllimited: MjtLimited [force]; "are control limits defined.";
forcelimited: MjtLimited [force]; "are force limits defined.";
]
}
getter_setter! {
[&] with, get, set, [
actlimited: MjtLimited [force]; "are activation limits defined.";
]
}
getter_setter! {
[&] with, get, set, [
actearly: bool; "apply next activations to qfrc.";
]
}
userdata_method!(f64);
string_set_get_with! {[&]
target; "name of transmission target.";
refsite; "reference site, for site transmission.";
slidersite; "site defining cylinder, for slider-crank.";
}
}
fn actuator_set_result(c_err_msg: *const c_char) -> Result<(), MjEditError> {
let err_msg = unsafe { CStr::from_ptr(c_err_msg) }.to_string_lossy();
if err_msg.is_empty() {
Ok(())
}
else {
Err(MjEditError::InvalidParameter(err_msg.into_owned()))
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct PositionConfig {
pub kp: f64,
pub inheritrange: f64,
pub kv: Option<f64>,
pub dampratio: Option<f64>,
pub timeconst: Option<f64>,
}
impl PositionConfig {
getter_setter! {
with, [
kp: f64; "the proportional (position) gain.";
inheritrange: f64; "the automatic range-inheritance factor.";
kv: f64; "the velocity feedback gain (mutually exclusive with dampratio).";
dampratio: f64; "the damping ratio (mutually exclusive with kv).";
timeconst: f64; "the first-order activation-filter time constant.";
]
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct IntVelocityConfig {
pub kp: f64,
pub inheritrange: f64,
pub kv: Option<f64>,
pub dampratio: Option<f64>,
pub timeconst: Option<f64>,
}
impl IntVelocityConfig {
getter_setter! {
with, [
kp: f64; "the proportional gain.";
inheritrange: f64; "the automatic range-inheritance factor.";
kv: f64; "the velocity feedback gain (mutually exclusive with dampratio).";
dampratio: f64; "the damping ratio (mutually exclusive with kv).";
timeconst: f64; "the first-order activation-filter time constant.";
]
}
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct DcMotorConfig {
pub resistance: f64,
pub input_mode: i32,
pub motorconst: Option<[f64; 2]>,
pub nominal: Option<[f64; 3]>,
pub saturation: Option<[f64; 3]>,
pub inductance: Option<[f64; 2]>,
pub cogging: Option<[f64; 3]>,
pub controller: Option<[f64; 6]>,
pub thermal: Option<[f64; 6]>,
pub lugre: Option<[f64; 5]>,
}
impl DcMotorConfig {
getter_setter! {
with, [
resistance: f64; "the electrical resistance.";
input_mode: i32; "the input mode selector.";
motorconst: [f64; 2]; "the torque and back-EMF constants [Kt, Ke].";
nominal: [f64; 3]; "the nominal ratings [voltage, stall_torque, no_load_speed].";
saturation: [f64; 3]; "the saturation [tau_max, i_max, di_dt_max].";
inductance: [f64; 2]; "the inductance [L, te].";
cogging: [f64; 3]; "the cogging [amplitude, periodicity, phase].";
controller: [f64; 6]; "the controller [kp, ki, kd, slewmax, Imax, v_max].";
thermal: [f64; 6]; "the thermal [R_th, C, tau_th, alpha, T0, T_ambient].";
lugre: [f64; 5]; "the LuGre friction [stiffness, damping, coulomb, static, stribeck].";
]
}
}
impl MjsActuator {
pub fn set_to_motor(&mut self) {
unsafe { mjs_setToMotor(self) };
}
pub fn set_to_position(&mut self, config: PositionConfig) -> Result<(), MjEditError> {
let PositionConfig { kp, inheritrange, mut kv, mut dampratio, mut timeconst } = config;
let c_err_msg = unsafe { mjs_setToPosition(
self, kp,
kv.as_mut().map_or(ptr::null_mut(), |x| x),
dampratio.as_mut().map_or(ptr::null_mut(), |x| x),
timeconst.as_mut().map_or(ptr::null_mut(), |x| x),
inheritrange
) };
actuator_set_result(c_err_msg)
}
pub fn set_to_int_velocity(&mut self, config: IntVelocityConfig) -> Result<(), MjEditError> {
let IntVelocityConfig { kp, inheritrange, mut kv, mut dampratio, mut timeconst } = config;
let c_err_msg = unsafe { mjs_setToIntVelocity(
self, kp,
kv.as_mut().map_or(ptr::null_mut(), |x| x),
dampratio.as_mut().map_or(ptr::null_mut(), |x| x),
timeconst.as_mut().map_or(ptr::null_mut(), |x| x),
inheritrange
) };
actuator_set_result(c_err_msg)
}
pub fn set_to_velocity(&mut self, kv: f64) {
unsafe { mjs_setToVelocity(self, kv) };
}
pub fn set_to_damper(&mut self, kv: f64) -> Result<(), MjEditError> {
actuator_set_result(unsafe { mjs_setToDamper(self, kv) })
}
pub fn set_to_cylinder(&mut self, timeconst: f64, bias: f64, area: f64, diameter: f64) {
unsafe { mjs_setToCylinder(self, timeconst, bias, area, diameter) };
}
#[allow(clippy::too_many_arguments)]
pub fn set_to_muscle(
&mut self, mut timeconst: [f64; 2], tausmooth: f64, mut range: [f64; 2],
force: f64, scale: f64, lmin: f64, lmax: f64, vmax: f64, fpmax: f64, fvmax: f64
) -> Result<(), MjEditError>
{
let c_err_msg = unsafe { mjs_setToMuscle(
self, &mut timeconst, tausmooth, &mut range,
force, scale, lmin, lmax, vmax, fpmax, fvmax
) };
actuator_set_result(c_err_msg)
}
pub fn set_to_adhesion(&mut self, gain: f64) -> Result<(), MjEditError> {
actuator_set_result(unsafe { mjs_setToAdhesion(self, gain) })
}
pub fn set_to_dc_motor(&mut self, config: DcMotorConfig) -> Result<(), MjEditError> {
let DcMotorConfig {
resistance, input_mode,
mut motorconst, mut nominal, mut saturation, mut inductance,
mut cogging, mut controller, mut thermal, mut lugre
} = config;
let c_err_msg = unsafe { mjs_setToDCMotor(
self,
motorconst.as_mut().map_or(ptr::null_mut(), |x| x),
resistance,
nominal.as_mut().map_or(ptr::null_mut(), |x| x),
saturation.as_mut().map_or(ptr::null_mut(), |x| x),
inductance.as_mut().map_or(ptr::null_mut(), |x| x),
cogging.as_mut().map_or(ptr::null_mut(), |x| x),
controller.as_mut().map_or(ptr::null_mut(), |x| x),
thermal.as_mut().map_or(ptr::null_mut(), |x| x),
lugre.as_mut().map_or(ptr::null_mut(), |x| x),
input_mode
) };
actuator_set_result(c_err_msg)
}
}
mjs_struct!(Sensor [SpecObject]);
impl MjsSensor {
getter_setter! {
[&] with, get, [
intprm: &[i32; mjNSENS as usize]; "integer parameters.";
interval: &[f64; 2]; "[period, time_prev] in seconds.";
]
}
getter_setter! {
get, [
plugin: &MjsPlugin; "sensor plugin.";
]
}
getter_setter!([&] with, get, set, [
type_ + _: MjtSensor; "sensor type.";
objtype: MjtObj { check_objtype, "[`MjEditError::InvalidParameter`] when the object type is not a real object type (i.e. not below [`MjtObj::mjNOBJECT`])" } => MjEditError;
"object type the sensor refers to.";
reftype: MjtObj { check_objtype, "[`MjEditError::InvalidParameter`] when the reference type is not a real object type (i.e. not below [`MjtObj::mjNOBJECT`])" } => MjEditError;
"type of referenced object.";
datatype: MjtDataType; "data type.";
cutoff: f64; "cutoff for real and positive datatypes.";
noise: f64; "noise stdev.";
needstage: MjtStage; "compute stage needed to simulate sensor.";
dim: i32; "number of scalar outputs.";
nsample: i32; "number of samples in history buffer.";
interp: i32; "interpolation order (0=ZOH, 1=linear, 2=cubic).";
delay: f64; "delay time in seconds; 0: no delay.";
]);
userdata_method!(f64);
string_set_get_with! {[&]
refname; "name of referenced object.";
objname; "name of sensorized object.";
}
}
mjs_struct!(Flex [SpecObject]);
impl MjsFlex {
getter_setter! {
[&] with, get, [
rgba: &[f32; 4]; "rgba when material is omitted.";
friction: &[f64; 3]; "contact friction vector.";
solref: &[MjtNum; mjNREF as usize]; "solref for the pair.";
solimp: &[MjtNum; mjNIMP as usize]; "solimp for the pair.";
size: &[f64; 3]; "vertex bounding box half sizes in qpos0.";
cellcount: &[i32; 3]; "grid cell count for finite cell method.";
]
}
getter_setter! {
[&] with, get, set, [
young: f64; "elastic stiffness.";
group: i32; "group.";
contype: i32; "contact type.";
conaffinity: i32; "contact affinity.";
condim: i32; "contact dimensionality.";
priority: i32; "contact priority.";
solmix: f64; "solver mixing for contact pairs.";
margin: f64; "margin for contact detection.";
gap: f64; "additional contact detection buffer.";
dim: i32; "element dimensionality.";
radius: f64; "radius around primitive element.";
activelayers: i32; "number of active element layers in 3D.";
edgestiffness: f64; "edge stiffness.";
edgedamping: f64; "edge damping.";
poisson: f64; "Poisson's ratio.";
damping: f64; "Rayleigh's damping.";
thickness: f64; "thickness (2D only).";
elastic2d: i32; "2D passive forces; 0: none, 1: bending, 2: stretching, 3: both.";
order: i32; "interpolation order (1: trilinear, 2: quadratic).";
]
}
getter_setter! {
[&] with, get, set, [
internal: bool; "enable internal collisions.";
flatskin: bool; "render flex skin with flat shading.";
passive: bool; "mode for passive collisions.";
]
}
getter_setter! {
[&] with, get, set, [
selfcollide: MjtFlexSelf [force]; "mode for flex self collision.";
]
}
string_set_get_with! {[&]
material; "name of material used for rendering.";
}
vec_string_set_append! {
nodebody; "node body names.";
vertbody; "vertex body names.";
}
vec_set_get! {
node: f64; "node positions.";
vert: f64; "vertex positions.";
}
vec_set! {
texcoord: f32; "vertex texture coordinates.";
elem: i32; "element vertex ids.";
}
vec_set! {
[unsafe: "The slice must have exactly `(dim + 1) * nelem` entries and every entry \
must be a valid index into the flex texture coordinates."
] elemtexcoord: i32 => i32; "element texture coordinates.";
}
}
mjs_struct!(Pair [SpecObject]);
impl MjsPair {
getter_setter! {
[&] with, get, [
friction: &[f64; 5]; "contact friction vector.";
solref: &[MjtNum; mjNREF as usize]; "solref for the pair.";
solimp: &[MjtNum; mjNIMP as usize]; "solimp for the pair.";
solreffriction: &[MjtNum; mjNREF as usize]; "solver reference, frictional directions.";
]
}
getter_setter! {
[&] with, get, set, [
margin: f64; "margin for contact detection.";
gap: f64; "additional contact detection buffer.";
condim: i32; "contact dimensionality.";
]
}
string_set_get_with! {[&]
geomname1; "name of geom 1.";
geomname2; "name of geom 2.";
}
}
mjs_struct!(Exclude [SpecObject]);
impl MjsExclude {
string_set_get_with! {[&]
bodyname1; "name of body 1.";
bodyname2; "name of body 2.";
}
}
mjs_struct!(Equality [SpecObject]);
impl MjsEquality {
getter_setter! {
[&] with, get, [
data: &[f64; mjNEQDATA as usize]; "data array for equality parameters.";
solref: &[f64; mjNREF as usize]; "solver reference.";
solimp: &[f64; mjNIMP as usize]; "solver impedance.";
]
}
getter_setter! {[&] with, get, set, [
active: bool; "active flag.";
]}
getter_setter! {[&] with, get, set, [
type_ + _: MjtEq; "equality type.";
objtype: MjtObj; "type of both objects.";
]}
string_set_get_with! {[&]
name1; "name of object 1";
name2; "name of object 2";
}
}
mjs_struct!(Tendon [SpecObject]);
impl MjsTendon {
getter_setter! {
[&] with, get, [
damping: &[f64; mjNPOLY as usize + 1]; "damping coefficient.";
stiffness: &[f64; mjNPOLY as usize + 1]; "stiffness coefficient.";
springlength: &[f64; 2]; "spring length.";
solref_friction: &[f64; mjNREF as usize]; "solver reference: tendon friction.";
solimp_friction: &[f64; mjNIMP as usize]; "solver impedance: tendon friction.";
range: &[f64; 2]; "range.";
actfrcrange: &[f64; 2]; "actuator force limits.";
solref_limit: &[f64; mjNREF as usize]; "solver reference: tendon limits.";
solimp_limit: &[f64; mjNIMP as usize]; "solver impedance: tendon limits.";
rgba: &[f32; 4]; "rgba when material omitted.";
]
}
getter_setter! {[&] with, get, set, [
group: i32; "group.";
frictionloss: f64; "friction loss.";
armature: f64; "inertia associated with tendon velocity.";
margin: f64; "margin value for tendon limit detection.";
width: f64; "width for rendering.";
]}
getter_setter! {
[&] with, get, set, [
limited: MjtLimited [force]; "does tendon have limits (mjtLimited).";
actfrclimited: MjtLimited [force]; "does tendon have actuator force limits."
]
}
userdata_method!(f64);
string_set_get_with! {[&]
material; "name of material for rendering.";
}
#[allow(deprecated)]
pub fn wrap_site(&mut self, name: &str) -> &mut MjsWrap {
self.try_wrap_site(name).expect("failed to wrap site")
}
#[deprecated(
since = "5.0.0",
note = "allocation failure cannot be recovered soundly; use `wrap_site`"
)]
pub fn try_wrap_site(&mut self, name: &str) -> Result<&mut MjsWrap, MjEditError> {
let cname = CString::new(name).unwrap();
let wrap_ptr = unsafe { mjs_wrapSite(self, cname.as_ptr()) };
unsafe { wrap_ptr.as_mut() }.ok_or(MjEditError::AllocationFailed)
}
#[allow(deprecated)]
pub fn wrap_geom(&mut self, name: &str, sidesite: &str) -> &mut MjsWrap {
self.try_wrap_geom(name, sidesite).expect("failed to wrap geom")
}
#[deprecated(
since = "5.0.0",
note = "allocation failure cannot be recovered soundly; use `wrap_geom`"
)]
pub fn try_wrap_geom(&mut self, name: &str, sidesite: &str) -> Result<&mut MjsWrap, MjEditError> {
let cname = CString::new(name).unwrap();
let csidesite = CString::new(sidesite).unwrap();
let wrap_ptr = unsafe { mjs_wrapGeom(
self,
cname.as_ptr(), csidesite.as_ptr()
) };
unsafe { wrap_ptr.as_mut() }.ok_or(MjEditError::AllocationFailed)
}
#[allow(deprecated)]
pub fn wrap_joint(&mut self, name: &str, coef: f64) -> &mut MjsWrap {
self.try_wrap_joint(name, coef).expect("failed to wrap joint")
}
#[deprecated(
since = "5.0.0",
note = "allocation failure cannot be recovered soundly; use `wrap_joint`"
)]
pub fn try_wrap_joint(&mut self, name: &str, coef: f64) -> Result<&mut MjsWrap, MjEditError> {
let cname = CString::new(name).unwrap();
let wrap_ptr = unsafe { mjs_wrapJoint(self, cname.as_ptr(), coef) };
unsafe { wrap_ptr.as_mut() }.ok_or(MjEditError::AllocationFailed)
}
#[allow(deprecated)]
pub fn wrap_pulley(&mut self, divisor: f64) -> &mut MjsWrap {
self.try_wrap_pulley(divisor).expect("failed to wrap pulley")
}
#[deprecated(
since = "5.0.0",
note = "allocation failure cannot be recovered soundly; use `wrap_pulley`"
)]
pub fn try_wrap_pulley(&mut self, divisor: f64) -> Result<&mut MjsWrap, MjEditError> {
let wrap_ptr = unsafe { mjs_wrapPulley(self, divisor) };
unsafe { wrap_ptr.as_mut() }.ok_or(MjEditError::AllocationFailed)
}
pub fn wrap_num(&self) -> usize {
unsafe { mjs_getWrapNum(self) as usize }
}
pub fn wrap(&self, i: usize) -> &MjsWrap {
self.try_wrap(i).unwrap()
}
pub fn try_wrap(&self, i: usize) -> Result<&MjsWrap, MjEditError> {
let len = self.wrap_num();
if i >= len {
return Err(MjEditError::IndexOutOfBounds { id: i, len });
}
let ptr = unsafe { mjs_getWrap(self, i as i32) };
Ok(unsafe { &*ptr })
}
pub fn wrap_mut(&mut self, i: usize) -> &mut MjsWrap {
self.try_wrap_mut(i).unwrap()
}
pub fn try_wrap_mut(&mut self, i: usize) -> Result<&mut MjsWrap, MjEditError> {
let len = self.wrap_num();
if i >= len {
return Err(MjEditError::IndexOutOfBounds { id: i, len });
}
let ptr = unsafe { mjs_getWrap(self, i as i32) };
Ok(unsafe { &mut *ptr })
}
}
mjs_struct!(Wrap);
impl MjsWrap {
getter_setter! {
[&] with, get, set, [
type_ + _: MjtWrap; "wrap type.";
]
}
pub fn side_site(&self) -> Option<&MjsSite> {
let ptr = unsafe { mjs_getWrapSideSite(self) };
if ptr.is_null() { None } else { Some(unsafe { &*ptr }) }
}
pub fn side_site_mut(&mut self) -> Option<&mut MjsSite> {
let ptr = unsafe { mjs_getWrapSideSite(self) };
if ptr.is_null() { None } else { Some(unsafe { &mut *ptr }) }
}
pub fn divisor(&self) -> f64 {
unsafe { mjs_getWrapDivisor(self) }
}
pub fn coef(&self) -> f64 {
unsafe { mjs_getWrapCoef(self) }
}
}
mjs_struct!(Numeric [SpecObject]);
impl MjsNumeric {
getter_setter! {
[&] with, get, set, [
size: i32 { check_numeric_size, "[`MjEditError::InvalidParameter`] when the size is negative" } => MjEditError; "size of the numeric array.";
]
}
vec_set_get! {
data: f64; "initialization data.";
}
}
mjs_struct!(Text [SpecObject]);
impl MjsText {
string_set_get_with! {[&]
data; "text string.";
}
}
mjs_struct!(Tuple [SpecObject]);
impl MjsTuple {
vec_set! {
objtype: MjtObj => i32 { check_objtype, "[`MjEditError::InvalidParameter`] when any value is not a real object type (i.e. not below [`MjtObj::mjNOBJECT`])" } => MjEditError;
"object types. Every value must be a real object type (an `MjtObj` below `mjNOBJECT`).";
}
vec_string_set_append! {
objname; "object names.";
}
vec_set_get! {
objprm: f64; "object parameters.";
}
}
mjs_struct!(Key [SpecObject]);
impl MjsKey {
getter_setter! {
[&] with, get, set, [
time: f64; "time."
]
}
vec_set_get! {
qpos: f64; "qpos.";
qvel: f64; "qvel.";
act: f64; "act.";
mpos: f64; "mocap pos.";
mquat: f64; "mocap quat.";
ctrl: f64; "ctrl.";
}
}
mjs_struct!(Plugin [SpecObject]);
impl MjsPlugin {
string_set_get_with! {[&]
name; "instance name.";
plugin_name; "plugin name.";
}
getter_setter! {
[&] with, get, set, [
active: bool; "is the plugin active.";
]
}
}
mjs_struct!(Mesh [SpecObject]);
impl MjsMesh {
getter_setter! {
[&] with, get, [
refpos: &[f64; 3]; "reference position.";
refquat: &[f64; 4]; "reference orientation.";
scale: &[f64; 3]; "scale vector.";
]
}
getter_setter! {
get, [
plugin: &MjsPlugin; "sdf plugin.";
]
}
getter_setter! {
[&] with, get, set, [
inertia: MjtMeshInertia; "inertia type (convex, legacy, exact, shell).";
maxhullvert: i32; "maximum vertex count for the convex hull.";
octree_maxdepth: i32; "max octree depth.";
]
}
getter_setter! {
[&] with, get,set, [
smoothnormal: bool; "do not exclude large-angle faces from normals.";
needsdf: bool; "compute sdf from mesh.";
]
}
string_set_get_with! {[&]
content_type; "content type of file.";
file; "mesh file.";
material; "name of material.";
}
vec_set! {
uservert: f32; "user vertex data.";
usernormal: f32; "user normal data.";
usertexcoord: f32; "user texcoord data.";
userface: i32; "user vertex indices.";
}
vec_set! {
[unsafe: "Every entry must be in `0..N`, where `N` is the number of user normals: the \
length of the slice passed to `set_usernormal` divided by 3 (each normal is 3 \
`f32`: x, y, z)."]
userfacenormal: i32 => i32; "user face normal indices.";
[unsafe: "Every entry must be in `0..ntexcoord` (the number of user texture coordinates), and \
the slice length must equal the length of the slice passed to `set_userface` (3 per \
face). Unlike face-normal data, MuJoCo does not validate the texcoord-index length, \
so an oversized slice overflows the model's face-texcoord buffer at compile time."]
userfacetexcoord: i32 => i32; "user texcoord indices.";
}
}
mjs_struct!(Hfield [SpecObject]);
impl MjsHfield {
getter_setter! {
[&] with, get, [
size: &[f64; 4]; "size of the hfield.";
]
}
getter_setter! { [&] with, get, set, [
nrow: i32; "number of rows.";
ncol: i32; "number of columns.";
]}
string_set_get_with! {[&]
content_type; "content type of file.";
file; "file: (nrow, ncol, [elevation data]).";
}
pub fn set_userdata<T: AsRef<[f32]>>(&mut self, userdata: T) {
unsafe { write_mjs_vec_f32(userdata.as_ref(), self.userdata) };
}
}
mjs_struct!(Skin [SpecObject]);
impl MjsSkin {
getter_setter! {
[&] with, get, [
rgba: &[f32; 4]; "rgba when material is omitted.";
]
}
getter_setter! {
[&] with, get, set, [
inflate: f32; "inflate in normal direction.";
group: i32; "group for visualization.";
]
}
string_set_get_with! {[&]
material; "name of material used for rendering.";
file; "skin file.";
}
vec_string_set_append! {
bodyname; "body names.";
}
vec_set! {
vert: f32; "vertex positions.";
texcoord: f32; "texture coordinates.";
bindpos: f32; "bind pos.";
bindquat: f32; "bind quat.";
}
vec_set! {
[
unsafe:
"The slice length must be a multiple of 3 and every entry must be in `0..nvert` (the number of skin vertices)."
] face: i32 => i32; "faces.";
}
vec_vec_append! {
vertid: i32; "vertex ids.";
vertweight: f32; "vertex weights.";
}
}
mjs_struct!(Texture [SpecObject]);
impl MjsTexture {
getter_setter! {
[&] with, get, [
rgb1: &[f64; 3]; "first color for builtin.";
rgb2: &[f64; 3]; "second color for builtin.";
markrgb: &[f64; 3]; "mark color.";
gridsize: &[i32; 2]; "size of grid for composite file; (1,1)-repeat.";
gridlayout: &[c_char; 12]; "row-major: L,R,F,B,U,D for faces; . for unused.";
]
}
getter_setter! {
[&] with, get, set, [
random: f64; "probability of random dots.";
width: i32; "image width.";
height: i32; "image height.";
]
}
getter_setter! {
[&] with, get, set, [
nchannel: i32; "number of channels.";
]
}
getter_setter! {
[&] with, get, set, [
type_ + _: MjtTexture [force]; "texture type.";
colorspace: MjtColorSpace [force]; "colorspace.";
builtin: MjtBuiltin [force]; "builtin type.";
mark: MjtMark [force]; "mark type.";
]
}
vec_string_set_append! {
cubefiles[MjtCubeFace] => cubefile; "different file for each side of the cube.";
}
getter_setter! {[&] with, get, set, [
hflip: bool; "horizontal flip.";
vflip: bool; "vertical flip.";
]}
pub fn set_data<T: bytemuck::NoUninit>(&mut self, data: &[T]) {
unsafe { write_mjs_vec_byte(data, self.data) };
}
string_set_get_with! {[&]
file; "png file to load; use for all sides of cube.";
content_type; "content type of file.";
}
}
mjs_struct!(Material [SpecObject]);
impl MjsMaterial {
getter_setter! {
[&] with, get, [
rgba: &[f32; 4]; "rgba color.";
texrepeat: &[f32; 2]; "texture repetition for 2D mapping.";
]
}
getter_setter! {[&] with, get, set, [
texuniform: bool; "make texture cube uniform.";
]}
getter_setter! {
[&] with, get, set, [
emission: f32; "emission.";
specular: f32; "specular.";
shininess: f32; "shininess.";
reflectance: f32; "reflectance.";
metallic: f32; "metallic.";
roughness: f32; "roughness.";
]
}
vec_string_set_append! {
textures[MjtTextureRole] => texture; "names of textures (empty: none).";
}
}
mjs_struct!(Body [SpecObject] {
unsafe fn delete(&mut self) -> Result<(), MjEditError> {
if self.name() == "world" {
return Err(MjEditError::UnsupportedOperation);
}
unsafe { SpecItem::__delete_default__(self) }
}
});
impl MjsBody {
add_x_method! { body, site, joint, geom, camera, light }
pub fn child(&self, name: &str) -> Option<&MjsBody> {
let c_name = CString::new(name).unwrap();
unsafe {
let ptr = mjs_findChild(self, c_name.as_ptr());
if ptr.is_null() { None } else { ptr.as_ref() }
}
}
pub fn child_mut(&mut self, name: &str) -> Option<&mut MjsBody> {
let c_name = CString::new(name).unwrap();
unsafe {
let ptr = mjs_findChild(self, c_name.as_ptr());
if ptr.is_null() { None } else { ptr.as_mut() }
}
}
#[inline]
unsafe fn ffi_mut(&mut self) -> &mut Self {
self
}
pub fn add_frame(&mut self) -> &mut MjsFrame {
self.try_add_frame().expect("mjs_addFrame returned null; allocation failed")
}
pub fn try_add_frame(&mut self) -> Result<&mut MjsFrame, MjEditError> {
let ptr = unsafe { mjs_addFrame(self.ffi_mut(), ptr::null_mut()) };
unsafe { ptr.as_mut() }.ok_or(MjEditError::AllocationFailed)
}
}
impl MjsBody {
getter_setter! {
[&] with, get, [
pos: &[f64; 3]; "frame position.";
quat: &[f64; 4]; "frame orientation.";
alt: &MjsOrientation; "frame alternative orientation.";
ipos: &[f64; 3]; "inertial frame position.";
iquat: &[f64; 4]; "inertial frame orientation.";
inertia: &[f64; 3]; "diagonal inertia (in i-frame).";
ialt: &MjsOrientation; "inertial frame alternative orientation.";
fullinertia: &[f64; 6]; "non-axis-aligned inertia matrix.";
]
}
getter_setter! {
get, [
plugin: &MjsPlugin; "passive force plugin.";
]
}
getter_setter! {
[&] with, get, set, [
mass: f64; "mass.";
gravcomp: f64; "gravity compensation.";
sleep: MjtSleepPolicy; "sleep policy.";
]
}
getter_setter! {
[&] with, get, set, [
mocap: bool; "whether this is a mocap body.";
explicitinertial: bool; "whether to save the body with explicit inertial clause.";
]
}
userdata_method!(f64);
}
#[derive(Debug)]
pub struct MjsBodyItemIterMut<'a, T> {
ffi_ptr: *mut MjsBody,
last: *mut mjsElement,
recurse: bool,
item_type: PhantomData<&'a mut T>
}
impl<'a, T: SpecObject> MjsBodyItemIterMut<'a, T> {
fn new(root: &'a mut MjsBody, recurse: bool) -> Self {
let last = unsafe { mjs_firstChild(root, T::OBJ_TYPE, recurse.into()) };
Self { ffi_ptr: root, last, recurse, item_type: PhantomData }
}
}
impl<'a, T: SpecObject + 'a> Iterator for MjsBodyItemIterMut<'a, T> {
type Item = &'a mut T;
fn next(&mut self) -> Option<Self::Item> {
if self.last.is_null() {
return None;
}
unsafe {
let out = T::from_element_as_ptr_mut(self.last).as_mut();
self.last = mjs_nextChild(self.ffi_ptr, self.last, self.recurse.into());
out
}
}
}
impl<'a, T: SpecObject + 'a> std::iter::FusedIterator for MjsBodyItemIterMut<'a, T> {}
#[derive(Debug, Clone)]
pub struct MjsBodyItemIter<'a, T> {
ffi_ptr: *const MjsBody,
last: *const mjsElement,
recurse: bool,
item_type: PhantomData<&'a T>
}
impl<'a, T: SpecObject> MjsBodyItemIter<'a, T> {
fn new(root: &'a MjsBody, recurse: bool) -> Self {
let last = unsafe {
mjs_firstChild(
root,
T::OBJ_TYPE,
recurse.into()
)
};
Self { ffi_ptr: root, last, recurse, item_type: PhantomData }
}
}
impl<'a, T: SpecObject + 'a> Iterator for MjsBodyItemIter<'a, T> {
type Item = &'a T;
fn next(&mut self) -> Option<Self::Item> {
if self.last.is_null() {
return None;
}
unsafe {
let out = T::from_element_as_ptr_mut(self.last as *mut _).as_ref();
self.last = mjs_nextChild(self.ffi_ptr, self.last, self.recurse.into());
out
}
}
}
impl<'a, T: SpecObject + 'a> std::iter::FusedIterator for MjsBodyItemIter<'a, T> {}
impl MjsBody {
body_get_iter! {[body, joint, geom, site, camera, light, frame] }
}
#[cfg(test)]
mod tests {
use std::io::Write;
use std::path::{Path, PathBuf};
use std::fs;
use super::*;
const MODEL: &str = "\
<mujoco>
<worldbody>
<light ambient=\"0.2 0.2 0.2\"/>
<body name=\"ball\" pos=\".2 .2 .1\">
<geom name=\"green_sphere\" size=\".1\" rgba=\"0 1 0 1\" solref=\"0.004 1.0\"/>
<joint name=\"ball\" type=\"free\"/>
</body>
<geom name=\"floor1\" type=\"plane\" size=\"10 10 1\" solref=\"0.004 1.0\"/>
</worldbody>
</mujoco>";
#[test]
fn test_parse_xml_string() {
assert!(MjSpec::from_xml_string(MODEL).is_ok(), "failed to parse the model");
}
#[test]
fn test_parse_xml_file() {
const PATH: &str = "./mj_spec_test_parse_xml_file.xml";
let mut file = fs::File::create(PATH).expect("file creation failed");
file.write_all(MODEL.as_bytes()).expect("unable to write to file");
file.flush().unwrap();
let spec = MjSpec::from_xml(PATH);
fs::remove_file(PATH).expect("file removal failed");
assert!(spec.is_ok(), "failed to parse the model");
}
#[test]
fn test_parse_xml_vfs() {
const PATH: &str = "./mj_spec_test_parse_xml_vfs.xml";
let mut vfs = MjVfs::new();
vfs.add_from_buffer(PATH, MODEL.as_bytes()).unwrap();
assert!(MjSpec::from_xml_vfs(PATH, &vfs).is_ok(), "failed to parse the model");
}
#[test]
fn test_basic_edit_compile() {
const TIMESTEP: f64 = 0.010;
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
spec.option_mut().timestep = TIMESTEP;
let compiled = spec.compile().expect("could not compile the model");
assert_eq!(compiled.opt().timestep, TIMESTEP);
spec.compile().unwrap();
}
#[test]
fn test_model_name() {
const DEFAULT_MODEL_NAME: &str = "MuJoCo Model";
const NEW_MODEL_NAME: &str = "Test model";
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
assert_eq!(spec.modelname(), DEFAULT_MODEL_NAME);
spec.set_modelname(NEW_MODEL_NAME);
assert_eq!(spec.modelname(), NEW_MODEL_NAME);
spec.compile().unwrap();
}
#[test]
fn test_item_name() {
const NEW_MODEL_NAME: &str = "Test model";
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
let world = spec.world_body_mut();
let body = world.add_body();
assert_eq!(body.name(), "");
body.set_name(NEW_MODEL_NAME).unwrap();
assert_eq!(body.name(), NEW_MODEL_NAME);
spec.compile().unwrap();
}
#[test]
fn test_body_remove() {
const NEW_MODEL_NAME: &str = "Test model";
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
let world = spec.world_body_mut();
let body = world.add_body();
body.set_name(NEW_MODEL_NAME).unwrap();
let body_element = {
let body = spec.body_mut(NEW_MODEL_NAME).expect("failed to obtain the body");
body.element_mut_pointer()
};
assert!(unsafe { spec.delete_element(body_element) }.is_ok(), "failed to delete model");
assert!(spec.body(NEW_MODEL_NAME).is_none(), "body was not removed from spec");
let world_element = {
let world = spec.world_body_mut();
world.element_mut_pointer()
};
assert!(unsafe { spec.delete_element(world_element) }.is_err(), "the world model should not be deletable");
spec.compile().unwrap();
}
#[test]
fn test_joint_remove() {
const NEW_NAME: &str = "Test model";
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
let world = spec.world_body_mut();
let joint = world.add_joint();
joint.set_name(NEW_NAME).unwrap();
let joint_element = {
let joint = spec.joint_mut(NEW_NAME).expect("failed to obtain the body");
joint.element_mut_pointer()
};
assert!(unsafe { spec.delete_element(joint_element) }.is_ok(), "failed to delete model");
assert!(spec.joint(NEW_NAME).is_none(), "body was not removed fom spec");
spec.compile().unwrap();
}
#[test]
fn test_hfield_remove() {
const NEW_NAME: &str = "Test hfield";
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
let hfield = spec.add_hfield();
hfield.set_name(NEW_NAME).unwrap();
let hfield_element = {
let hfield = spec.hfield_mut(NEW_NAME).expect("failed to obtain the hfield");
hfield.element_mut_pointer()
};
assert!(unsafe { spec.delete_element(hfield_element) }.is_ok(), "failed to delete hfield");
assert!(spec.hfield(NEW_NAME).is_none(), "hfield was not removed from spec");
spec.compile().unwrap();
}
#[test]
fn test_body_userdata() {
const NEW_USERDATA: [f64; 3] = [1.0, 2.0, 3.0];
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
let world = spec.world_body_mut();
assert_eq!(world.userdata(), []);
world.set_userdata(NEW_USERDATA);
assert_eq!(world.userdata(), NEW_USERDATA);
spec.compile().unwrap();
}
#[test]
fn test_body_attrs() {
const TEST_VALUE_F64: f64 = 5.25;
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
let world = spec.world_body_mut();
world.set_gravcomp(TEST_VALUE_F64);
assert_eq!(world.gravcomp(), TEST_VALUE_F64);
world.pos_mut()[0] = TEST_VALUE_F64;
assert_eq!(world.pos()[0], TEST_VALUE_F64);
spec.compile().unwrap();
}
#[test]
fn test_default() {
const DEFAULT_NAME: &str = "floor";
const NOT_DEFAULT_NAME: &str = "floor-not";
let mut spec = MjSpec::from_xml_string(MODEL).expect("unable to load the spec");
spec.add_default(DEFAULT_NAME, None);
assert!(spec.default(DEFAULT_NAME).is_some());
assert!(spec.default(NOT_DEFAULT_NAME).is_none());
let world = spec.world_body_mut();
let some_body = world.add_body();
some_body.add_joint().with_name("test");
some_body.add_geom().with_size([0.010, 0.0, 0.0]);
let actuator = spec.add_actuator()
.with_trntype(MjtTrn::mjTRN_JOINT);
actuator.set_target("test");
assert!(actuator.set_default(DEFAULT_NAME).is_ok());
spec.compile().unwrap();
}
#[test]
fn test_actuator_set_to() {
let mut spec = MjSpec::new();
let body = spec.world_body_mut().add_body();
body.add_geom().with_size([0.01, 0.0, 0.0]);
body.add_joint().with_name("hinge").with_type(MjtJoint::mjJNT_HINGE);
let actuator = spec.add_actuator().with_trntype(MjtTrn::mjTRN_JOINT);
actuator.set_target("hinge");
actuator.set_to_motor();
assert_eq!(actuator.gaintype(), MjtGain::mjGAIN_FIXED);
assert_eq!(actuator.biastype(), MjtBias::mjBIAS_NONE);
assert_eq!(actuator.dyntype(), MjtDyn::mjDYN_NONE);
assert_eq!(actuator.gainprm()[0], 1.0);
actuator.set_to_velocity(2.0);
assert_eq!(actuator.gaintype(), MjtGain::mjGAIN_FIXED);
assert_eq!(actuator.biastype(), MjtBias::mjBIAS_AFFINE);
assert_eq!(actuator.gainprm()[0], 2.0);
assert_eq!(actuator.biasprm()[2], -2.0);
assert!(actuator.set_to_damper(-1.0).is_err());
assert!(actuator.set_to_damper(5.0).is_ok());
assert_eq!(actuator.gaintype(), MjtGain::mjGAIN_AFFINE);
assert_eq!(actuator.gainprm()[2], -5.0);
assert!(actuator.set_to_adhesion(-1.0).is_err());
assert!(actuator.set_to_adhesion(1.0).is_ok());
actuator.set_to_cylinder(0.1, 0.0, 1.0, -1.0);
assert_eq!(actuator.dyntype(), MjtDyn::mjDYN_FILTER);
assert!(actuator.set_to_muscle([-1.0, -1.0], -1.0, [-1.0, -1.0], -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0).is_err());
actuator.set_to_muscle([-1.0, -1.0], 0.0, [-1.0, -1.0], -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0).unwrap();
assert_eq!(actuator.gaintype(), MjtGain::mjGAIN_MUSCLE);
assert_eq!(actuator.biastype(), MjtBias::mjBIAS_MUSCLE);
assert_eq!(actuator.dyntype(), MjtDyn::mjDYN_MUSCLE);
assert!(actuator.set_to_position(
PositionConfig::default().with_kp(1.0).with_kv(1.0).with_dampratio(1.0)
).is_err());
actuator.set_to_position(PositionConfig::default().with_kp(1.0).with_kv(1.0)).unwrap();
actuator.set_to_int_velocity(IntVelocityConfig::default().with_kp(1.0)).unwrap();
assert_eq!(actuator.dyntype(), MjtDyn::mjDYN_INTEGRATOR);
assert!(actuator.set_to_dc_motor(DcMotorConfig::default()).is_err());
actuator.set_to_dc_motor(
DcMotorConfig::default().with_motorconst([1.0, 1.0]).with_resistance(1.0)
).unwrap();
assert_eq!(actuator.gaintype(), MjtGain::mjGAIN_DCMOTOR);
assert_eq!(actuator.biastype(), MjtBias::mjBIAS_DCMOTOR);
assert_eq!(actuator.dyntype(), MjtDyn::mjDYN_DCMOTOR);
actuator.set_to_motor();
actuator.set_actearly(false);
actuator.set_ctrllimited(MjtLimited::mjLIMITED_FALSE);
spec.compile().unwrap();
}
#[test]
fn test_save() {
const EXPECTED_XML: &str = "\
<mujoco model=\"MuJoCo Model\">
<compiler angle=\"radian\"/>
<worldbody>
<body>
<geom size=\"0.01\"/>
<site pos=\"0 0 0\"/>
<camera pos=\"0 0 0\"/>
<light pos=\"0 0 0\" dir=\"0 0 -1\"/>
</body>
</worldbody>
</mujoco>
";
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
let body = world.add_body();
body.add_camera();
body.add_geom().with_size([0.010, 0.0, 0.0]);
body.add_light();
body.add_site();
spec.compile().unwrap();
assert_eq!(spec.save_xml_string(1000).unwrap(), EXPECTED_XML);
spec.compile().unwrap();
}
#[test]
fn test_save_xml_string_buffer_too_small() {
let mut spec = MjSpec::new();
spec.world_body_mut().add_body().add_geom().with_size([0.01, 0.0, 0.0]);
spec.compile().unwrap();
let err = spec.save_xml_string(1)
.expect_err("expected XmlBufferTooSmall with a 1-byte buffer");
let required_size = match err {
MjEditError::XmlBufferTooSmall { required_size } => required_size,
other => panic!("expected XmlBufferTooSmall, got {other:?}"),
};
assert!(required_size > 1, "required_size must exceed the original 1-byte buffer");
let xml = spec.save_xml_string(required_size + 1)
.expect("save_xml_string should succeed with required_size + 1 bytes");
assert!(!xml.is_empty(), "saved XML must be non-empty");
}
#[test]
fn test_site() {
const TEST_MATERIAL: &str = "material 1";
const TEST_POSITION: [f64; 3] = [1.0, 2.0, 3.0];
const SITE_NAME: &str = "test_site";
let mut spec = MjSpec::new();
spec.add_material().with_name(TEST_MATERIAL);
let world = spec.world_body_mut();
world.add_site()
.with_name(SITE_NAME);
let site = spec.site_mut(SITE_NAME).unwrap();
assert_eq!(site.material(), "");
site.set_material(TEST_MATERIAL);
assert_eq!(site.material(), TEST_MATERIAL);
let test_userdata: Vec<f64> = vec![0.0; 5];
assert_eq!(site.userdata(), []);
site.set_userdata(&test_userdata);
assert_eq!(site.userdata(), test_userdata);
assert_eq!(site.pos(), &[0.0; 3]);
*site.pos_mut() = TEST_POSITION;
assert_eq!(site.pos(), &TEST_POSITION);
spec.compile().unwrap();
}
#[test]
fn test_frame() {
let mut spec = MjSpec::new();
let world = spec.world_body_mut()
.with_gravcomp(10.0);
world.add_frame()
.with_name("frame_a")
.with_pos([0.5, 0.5, 0.05])
.add_body()
.add_geom()
.with_size([1.0, 0.0, 0.0]);
assert!(spec.frame("frame_a").is_some());
assert!(spec.frame_mut("frame_a").is_some());
spec.compile().unwrap();
}
#[test]
fn test_wrap() {
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
let body1= world.add_body().with_pos([0.0, 0.0, 0.5]);
body1.add_geom().with_size([0.010;3]);
body1.add_site().with_name("ball1");
body1.add_joint().with_type(MjtJoint::mjJNT_FREE);
let body2= world.add_body().with_pos([0.0, 0.0, 0.5]);
body2.add_geom().with_size([0.010;3]);
body2.add_site().with_name("ball2");
body2.add_joint().with_type(MjtJoint::mjJNT_FREE);
let tendon = spec.add_tendon()
.with_range([0.0, 0.25])
.with_rgba([1.0, 0.5, 0.0, 1.0]); tendon.wrap_site("ball1");
tendon.wrap_site("ball2");
spec.world_body_mut().add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
spec.compile().unwrap();
}
#[test]
fn test_body_child_and_id() {
let mut spec = MjSpec::new();
let parent = spec.world_body_mut().add_body().with_name("parent");
parent.add_body().with_name("child");
let parent_ref = spec.body("parent").unwrap();
assert!(parent_ref.child("child").is_some());
assert!(parent_ref.child("missing").is_none());
assert_eq!(parent_ref.id(), None);
let parent_mut = spec.body_mut("parent").unwrap();
assert!(parent_mut.child_mut("child").is_some());
assert!(parent_mut.child_mut("missing").is_none());
spec.compile().unwrap();
assert!(spec.body("parent").unwrap().id().is_some());
}
#[test]
fn test_geom() {
const GEOM_NAME: &str = "test_geom";
const GEOM_INVALID_NAME: &str = "geom_test";
let mut spec = MjSpec::new();
spec.world_body_mut().add_geom()
.with_name(GEOM_NAME);
assert!(spec.geom(GEOM_NAME).is_some());
assert!(spec.geom(GEOM_INVALID_NAME).is_none());
}
#[test]
fn test_camera() {
const CAMERA_NAME: &str = "test_cam";
const CAMERA_INVALID_NAME: &str = "cam_test";
let mut spec = MjSpec::new();
spec.world_body_mut().add_camera()
.with_name(CAMERA_NAME);
assert!(spec.camera(CAMERA_NAME).is_some());
assert!(spec.camera(CAMERA_INVALID_NAME).is_none());
}
#[test]
fn test_light() {
const LIGHT_NAME: &str = "test_light";
const LIGHT_INVALID_NAME: &str = "light_test";
let mut spec = MjSpec::new();
spec.world_body_mut().add_light()
.with_name(LIGHT_NAME);
assert!(spec.light(LIGHT_NAME).is_some());
assert!(spec.light(LIGHT_INVALID_NAME).is_none());
}
#[test]
fn test_exclude() {
const EXCLUDE_NAME: &str = "test_exclude";
const EXCLUDE_INVALID_NAME: &str = "exclude_test";
let mut spec = MjSpec::new();
spec.world_body_mut().add_body().with_name("body1-left");
spec.world_body_mut().add_body().with_name("body2-right");
spec.add_exclude()
.with_name(EXCLUDE_NAME)
.with_bodyname1("body1-left")
.with_bodyname2("body2-right");
assert!(spec.exclude(EXCLUDE_NAME).is_some());
assert!(spec.exclude(EXCLUDE_INVALID_NAME).is_none());
assert!(spec.compile().is_ok());
}
#[test]
fn test_mesh() {
let mut spec = MjSpec::new();
let mesh = spec.add_mesh();
assert!(!mesh.needsdf());
mesh.set_needsdf(true);
assert!(mesh.needsdf());
assert!(!mesh.smoothnormal());
mesh.set_smoothnormal(true);
assert!(mesh.smoothnormal());
}
#[test]
fn test_iteration() {
const LAST_BODY_NAME: &str = "subbody";
const LAST_WORLD_BODY_NAME: &str = "body2";
const N_GEOM: usize = 3;
const N_BODY: usize = 4; const N_SITE: usize = 2;
const N_TENDON: usize = 1;
const N_MESH: usize = 0;
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
let body1= world.add_body().with_pos([0.0, 0.0, 0.5]);
body1.add_geom().with_size([0.010;3]);
body1.add_site().with_name("ball1");
body1.add_joint().with_type(MjtJoint::mjJNT_FREE);
let body2= world.add_body().with_pos([0.0, 0.0, 0.5]).with_name(LAST_WORLD_BODY_NAME);
body2.add_geom().with_size([0.010;3]);
body2.add_site().with_name("ball2");
body2.add_joint().with_type(MjtJoint::mjJNT_FREE);
body2.add_body().with_name(LAST_BODY_NAME);
let tendon = spec.add_tendon()
.with_range([0.0, 0.25])
.with_rgba([1.0, 0.5, 0.0, 1.0]); tendon.wrap_site("ball1");
tendon.wrap_site("ball2");
spec.world_body_mut().add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
assert_eq!(spec.geom_iter_mut().count(), N_GEOM);
assert_eq!(spec.body_iter_mut().count(), N_BODY);
assert_eq!(spec.site_iter_mut().count(), N_SITE);
assert_eq!(spec.tendon_iter_mut().count(), N_TENDON);
assert_eq!(spec.mesh_iter_mut().count(), N_MESH);
assert_eq!(spec.body_iter_mut().last().unwrap().name(), LAST_BODY_NAME);
let world = spec.world_body_mut();
assert_eq!(world.geom_iter_mut(true).count(), N_GEOM);
assert_eq!(world.body_iter_mut(true).count(), N_BODY - 1); assert_eq!(world.site_iter_mut(true).count(), N_SITE);
assert_eq!(world.body_iter_mut(false).last().unwrap().name(), LAST_WORLD_BODY_NAME);
}
#[test]
fn test_parse_vfs() {
let mut vfs = MjVfs::new();
vfs.add_from_buffer("hello.xml", MODEL.as_bytes()).unwrap();
let mut spec = MjSpec::from_parse_vfs("hello.xml", "XML", &vfs).unwrap();
let model = spec.compile().unwrap();
assert!(model.geom("floor1").is_some());
}
#[test]
fn test_parse_file() {
std::fs::write("test_parse_vfs.xml", MODEL).unwrap();
let mut spec = MjSpec::from_parse("test_parse_vfs.xml", "XML").unwrap();
std::fs::remove_file("test_parse_vfs.xml").unwrap();
let model = spec.compile().unwrap();
assert!(model.geom("floor1").is_some());
}
#[test]
fn test_tendon_wrap_methods() {
let mut spec = MjSpec::new();
spec.world_body_mut().add_body().with_name("body1");
spec.world_body_mut().add_body().with_name("body2");
spec.world_body_mut().add_site().with_name("site1");
let tendon = spec.add_tendon();
tendon.wrap_site("site1");
tendon.wrap_joint("joint1", 0.5);
tendon.wrap_pulley(1.5);
assert_eq!(tendon.wrap_num(), 3);
let wrap = tendon.wrap(1);
assert_eq!(wrap.coef(), 0.5);
let wrap_pulley = tendon.wrap(2);
assert_eq!(wrap_pulley.divisor(), 1.5);
}
#[test]
fn test_tendon_wrap_out_of_bounds() {
let mut spec = MjSpec::new();
spec.world_body_mut().add_site().with_name("site1");
let tendon = spec.add_tendon();
tendon.wrap_site("site1");
assert_eq!(tendon.wrap_num(), 1);
match tendon.try_wrap(3) {
Err(MjEditError::IndexOutOfBounds { id, len }) => {
assert_eq!(id, 3);
assert_eq!(len, 1);
}
_ => panic!("expected IndexOutOfBounds"),
}
}
#[test]
#[should_panic]
fn test_tendon_wrap_out_of_bounds_panics() {
let mut spec = MjSpec::new();
spec.world_body_mut().add_site().with_name("site1");
let tendon = spec.add_tendon();
tendon.wrap_site("site1");
let _ = tendon.wrap(3);
}
#[test]
fn test_numeric_vec() {
let mut spec = MjSpec::new();
let numeric = spec.add_numeric();
let name = "test_numeric";
numeric.set_name(name).unwrap();
assert_eq!(numeric.name(), name);
let data = [1.5, 2.5, 3.5, 4.5];
numeric.set_data(&data);
assert_eq!(numeric.data(), &data);
spec.compile().unwrap();
}
#[test]
fn test_text_string() {
let mut spec = MjSpec::new();
let text = spec.add_text();
let name = "test_text";
text.set_name(name).unwrap();
assert_eq!(text.name(), name);
let content = "Hello MuJoCo!";
text.set_data(content);
assert_eq!(text.data(), content);
spec.compile().unwrap();
}
#[test]
fn test_tuple_names_and_params() {
let mut spec = MjSpec::new();
spec.world_body_mut().add_body().with_name("body1");
spec.world_body_mut().add_body().with_name("body2");
let tuple_name = "test_tuple";
let obj_param = [1.0, 2.0];
let tuple = spec.add_tuple();
tuple.set_name(tuple_name).unwrap();
assert_eq!(tuple.name(), tuple_name);
tuple.set_objname("body1 body2");
tuple.set_objprm(&obj_param);
tuple.set_objtype(&[MjtObj::mjOBJ_BODY, MjtObj::mjOBJ_BODY]).unwrap();
assert_eq!(tuple.objprm(), &obj_param);
spec.compile().unwrap();
let xml = spec.save_xml_string(2000).unwrap();
assert!(xml.contains("objname=\"body1\""));
assert!(xml.contains("objname=\"body2\""));
assert!(xml.contains("prm=\"1\""));
assert!(xml.contains("prm=\"2\""));
}
#[test]
fn test_tendon_wrap_site_compiled_model() {
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
let b1 = world.add_body().with_pos([0.0, 0.0, 0.5]);
b1.add_geom().with_size([0.01; 3]);
b1.add_site().with_name("s1");
b1.add_joint().with_type(MjtJoint::mjJNT_FREE);
let b2 = world.add_body().with_pos([1.0, 0.0, 0.5]);
b2.add_geom().with_size([0.01; 3]);
b2.add_site().with_name("s2");
b2.add_joint().with_type(MjtJoint::mjJNT_FREE);
world.add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
let tendon = spec.add_tendon().with_range([0.0, 1.0]);
tendon.wrap_site("s1");
tendon.wrap_site("s2");
let model = spec.compile().unwrap();
assert_eq!(model.ffi().ntendon, 1, "expected one tendon");
assert_eq!(model.ffi().nwrap, 2, "expected two wrap elements");
let wrap_types = model.wrap_type();
assert_eq!(wrap_types[0], MjtWrap::mjWRAP_SITE);
assert_eq!(wrap_types[1], MjtWrap::mjWRAP_SITE);
let wrap_objid = model.wrap_objid();
let s1_id = model.site("s1").unwrap().id as i32;
let s2_id = model.site("s2").unwrap().id as i32;
assert_eq!(wrap_objid[0], s1_id);
assert_eq!(wrap_objid[1], s2_id);
}
#[test]
fn test_tendon_limited_tristate() {
use crate::mujoco_c::mjtLimited_::*;
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
let b1 = world.add_body().with_pos([0.0, 0.0, 0.5]);
b1.add_geom().with_size([0.01; 3]);
b1.add_site().with_name("s1");
b1.add_joint().with_type(MjtJoint::mjJNT_FREE);
let b2 = world.add_body().with_pos([1.0, 0.0, 0.5]);
b2.add_geom().with_size([0.01; 3]);
b2.add_site().with_name("s2");
b2.add_joint().with_type(MjtJoint::mjJNT_FREE);
world.add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
for (name, val) in [("t_false", mjLIMITED_FALSE), ("t_true", mjLIMITED_TRUE), ("t_auto", mjLIMITED_AUTO)] {
let t = spec.add_tendon()
.with_name(name)
.with_range([0.0, 1.0])
.with_limited(val);
t.wrap_site("s1");
t.wrap_site("s2");
}
for (name, expected) in [("t_false", mjLIMITED_FALSE), ("t_true", mjLIMITED_TRUE), ("t_auto", mjLIMITED_AUTO)] {
let t = spec.tendon(name).expect("tendon not found");
assert_eq!(t.limited(), expected,
"Before compile: tendon '{}' limited should be {:?}", name, expected);
}
let model = spec.compile().unwrap();
let tendon_limited = model.tendon_limited();
assert!(!tendon_limited[0], "Compiled tendon 0 limited should be false");
assert!(tendon_limited[1], "Compiled tendon 1 limited should be true");
let _ = tendon_limited[2];
}
#[test]
fn test_tendon_actfrclimited_tristate() {
use crate::mujoco_c::mjtLimited_::*;
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
let b1 = world.add_body().with_pos([0.0, 0.0, 0.5]);
b1.add_geom().with_size([0.01; 3]);
b1.add_site().with_name("s1");
b1.add_joint().with_type(MjtJoint::mjJNT_FREE);
let b2 = world.add_body().with_pos([1.0, 0.0, 0.5]);
b2.add_geom().with_size([0.01; 3]);
b2.add_site().with_name("s2");
b2.add_joint().with_type(MjtJoint::mjJNT_FREE);
world.add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
for (name, val) in [("t_false", mjLIMITED_FALSE), ("t_true", mjLIMITED_TRUE), ("t_auto", mjLIMITED_AUTO)] {
let t = spec.add_tendon()
.with_name(name)
.with_range([0.0, 1.0])
.with_actfrcrange([-1.0, 1.0])
.with_actfrclimited(val);
t.wrap_site("s1");
t.wrap_site("s2");
}
for (name, expected) in [("t_false", mjLIMITED_FALSE), ("t_true", mjLIMITED_TRUE), ("t_auto", mjLIMITED_AUTO)] {
let t = spec.tendon(name).expect("tendon not found");
assert_eq!(t.actfrclimited(), expected,
"Before compile: tendon '{}' actfrclimited should be {:?}", name, expected);
}
spec.compile().unwrap();
}
#[test]
fn test_joint_align_tristate() {
use crate::mujoco_c::mjtAlignFree_::*;
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
world.add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
for (name, val) in [("j_false", mjALIGNFREE_FALSE), ("j_true", mjALIGNFREE_TRUE), ("j_auto", mjALIGNFREE_AUTO)] {
let body = world.add_body().with_pos([0.0, 0.0, 1.0]);
body.add_geom().with_size([0.1; 3]);
body.add_joint()
.with_name(name)
.with_type(MjtJoint::mjJNT_FREE)
.with_align(val);
}
for (name, expected) in [("j_false", mjALIGNFREE_FALSE), ("j_true", mjALIGNFREE_TRUE), ("j_auto", mjALIGNFREE_AUTO)] {
let j = spec.joint(name).expect("joint not found");
assert_eq!(j.align(), expected,
"Before compile: joint '{}' align should be {:?}", name, expected);
}
let model = spec.compile().unwrap();
let jnt_count = model.ffi().njnt as usize;
assert_eq!(jnt_count, 3, "expected 3 joints");
}
#[test]
fn test_joint_limited_tristate() {
use crate::mujoco_c::mjtLimited_::*;
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
world.add_geom().with_type(MjtGeom::mjGEOM_PLANE).with_size([1.0; 3]);
for (name, val) in [("j_false", mjLIMITED_FALSE), ("j_true", mjLIMITED_TRUE), ("j_auto", mjLIMITED_AUTO)] {
let body = world.add_body().with_pos([0.0, 0.0, 1.0]);
body.add_geom().with_size([0.1; 3]);
body.add_joint()
.with_name(name)
.with_type(MjtJoint::mjJNT_SLIDE)
.with_range([0.0, 1.0])
.with_limited(val);
}
for (name, expected) in [("j_false", mjLIMITED_FALSE), ("j_true", mjLIMITED_TRUE), ("j_auto", mjLIMITED_AUTO)] {
let j = spec.joint(name).expect("joint not found");
assert_eq!(j.limited(), expected,
"Before compile: joint '{}' limited should be {:?}", name, expected);
}
let model = spec.compile().unwrap();
let jnt_limited = model.jnt_limited();
assert!(!jnt_limited[0]);
assert!(jnt_limited[1]);
assert!(jnt_limited[2], "Joint with limited=AUTO and range should resolve to true");
}
#[test]
fn test_parse_xml_string_invalid() {
let result = MjSpec::from_xml_string("<not valid mujoco xml>");
assert!(result.is_err(), "parsing invalid XML must return Err");
let msg = result.unwrap_err().to_string();
assert!(!msg.is_empty(), "error message must not be empty for invalid XML");
}
#[test]
fn test_parse_xml_vfs_content() {
const PATH: &str = "./mj_spec_test_parse_xml_vfs_content.xml";
let mut vfs = MjVfs::new();
vfs.add_from_buffer(PATH, MODEL.as_bytes()).unwrap();
let mut spec = MjSpec::from_xml_vfs(PATH, &vfs).expect("VFS parse failed");
let model = spec.compile().expect("compile failed");
assert_eq!(model.ffi().nbody, 2, "expected 2 bodies (world + ball)");
assert_eq!(model.ffi().njnt, 1, "expected 1 joint");
assert_eq!(model.ffi().ngeom, 2, "expected 2 geoms (sphere + floor)");
}
#[test]
fn test_parse_xml_file_content() {
const PATH: &str = "./mj_spec_test_parse_xml_file_content.xml";
let mut file = fs::File::create(PATH).expect("file creation failed");
file.write_all(MODEL.as_bytes()).expect("unable to write");
file.flush().unwrap();
let result = MjSpec::from_xml(PATH);
fs::remove_file(PATH).expect("file removal failed");
let mut spec = result.expect("file parse failed");
let model = spec.compile().expect("compile failed");
assert_eq!(model.ffi().nbody, 2, "expected 2 bodies (world + ball)");
assert_eq!(model.ffi().njnt, 1, "expected 1 joint");
assert_eq!(model.ffi().ngeom, 2, "expected 2 geoms (sphere + floor)");
}
#[test]
fn test_from_parse_path_types() {
const PATH: &str = "./mj_spec_test_from_parse_path_types.xml";
let mut file = fs::File::create(PATH).expect("file creation failed");
file.write_all(MODEL.as_bytes()).expect("write failed");
file.flush().unwrap();
assert!(MjSpec::from_parse(PATH, "").is_ok());
assert!(MjSpec::from_parse(String::from(PATH), "").is_ok());
assert!(MjSpec::from_parse(Path::new(PATH), "").is_ok());
assert!(MjSpec::from_parse(PathBuf::from(PATH), "").is_ok());
fs::remove_file(PATH).expect("file removal failed");
}
#[test]
fn test_from_parse_vfs_path_types() {
const PATH: &str = "./mj_spec_test_from_parse_vfs_path_types.xml";
let mut vfs = MjVfs::new();
vfs.add_from_buffer(PATH, MODEL.as_bytes()).unwrap();
assert!(MjSpec::from_parse_vfs(PATH, "", &vfs).is_ok());
assert!(MjSpec::from_parse_vfs(PathBuf::from(PATH), "", &vfs).is_ok());
assert!(MjSpec::from_parse_vfs(Path::new(PATH), "", &vfs).is_ok());
}
#[test]
fn test_save_xml_path_types() {
let mut spec = MjSpec::new();
spec.world_body_mut().add_body().add_geom().with_size([0.01, 0.0, 0.0]);
spec.compile().unwrap();
let paths: [PathBuf; 3] = [
PathBuf::from("./mj_spec_save_xml_str.xml"),
PathBuf::from("./mj_spec_save_xml_pathbuf.xml"),
PathBuf::from("./mj_spec_save_xml_path.xml"),
];
spec.save_xml(paths[0].to_str().unwrap()).unwrap();
spec.save_xml(paths[1].clone()).unwrap();
spec.save_xml(paths[2].as_path()).unwrap();
for p in &paths {
let content = fs::read_to_string(p).expect("saved file should be readable");
assert!(content.contains("<mujoco"), "saved XML should contain <mujoco tag");
fs::remove_file(p).expect("cleanup failed");
}
}
#[test]
fn test_material_set_texture() {
let mut spec = MjSpec::new();
let world = spec.world_body_mut();
world.add_geom()
.with_type(MjtGeom::mjGEOM_PLANE)
.with_size([1.0, 1.0, 0.01])
.with_material("floor");
spec.add_texture()
.with_name("floor")
.with_type(MjtTexture::mjTEXTURE_2D)
.with_builtin(MjtBuiltin::mjBUILTIN_CHECKER)
.with_rgb1([0.9, 0.9, 0.9])
.with_rgb2([0.1, 0.1, 0.1])
.with_width(512)
.with_height(512);
let mat = spec.add_material().with_name("floor");
mat.set_texture(MjtTextureRole::mjTEXROLE_RGB, "floor");
let model = spec.compile().unwrap();
let xml = spec.save_xml_string(8192).unwrap();
assert!(xml.contains("texture=\"floor\""), "XML should reference the floor texture");
let mat_info = model.material("floor").unwrap();
let mat_view = mat_info.view(&model);
let tex_id = mat_view.texid;
assert_ne!(tex_id[MjtTextureRole::mjTEXROLE_RGB as usize], -1,
"RGB texture slot should be resolved (not -1)");
}
#[test]
fn test_builtin_texture_nchannel_rejected() {
let mut spec = MjSpec::new();
spec.add_texture()
.with_name("badtex")
.with_type(MjtTexture::mjTEXTURE_2D)
.with_builtin(MjtBuiltin::mjBUILTIN_CHECKER)
.with_rgb1([0.9, 0.9, 0.9])
.with_rgb2([0.1, 0.1, 0.1])
.with_width(64)
.with_height(64)
.set_nchannel(1);
let err = spec.compile().unwrap_err();
assert!(matches!(&err, MjEditError::CompileFailed(msg) if msg.contains("nchannel")),
"compile must reject nchannel < 3 builtin texture, got {err:?}");
let mut ok_spec = MjSpec::new();
ok_spec.add_texture()
.with_name("goodtex")
.with_type(MjtTexture::mjTEXTURE_2D)
.with_builtin(MjtBuiltin::mjBUILTIN_CHECKER)
.with_rgb1([0.9, 0.9, 0.9])
.with_rgb2([0.1, 0.1, 0.1])
.with_width(64)
.with_height(64)
.set_nchannel(3);
assert!(ok_spec.compile().is_ok(), "nchannel == 3 builtin texture should compile");
let mut no_builtin = MjSpec::new();
no_builtin.add_texture()
.with_name("plain")
.with_type(MjtTexture::mjTEXTURE_2D)
.with_width(64)
.with_height(64)
.set_nchannel(1);
assert!(matches!(no_builtin.compile().unwrap_err(),
MjEditError::CompileFailed(msg) if !msg.contains("nchannel")),
"nchannel < 3 without a builtin pattern should not trip the nchannel guard");
}
#[test]
fn test_mjs_flex_cellcount_and_order() {
const FLEX_MODEL: &str = "\
<mujoco>\
<worldbody>\
<body name=\"pin\" pos=\"0 0 1\">\
<flexcomp type=\"grid\" count=\"3 3 1\" spacing=\".1 .1 .1\" mass=\"1\"\
name=\"myflex\" radius=\"0.001\" dim=\"2\">\
<elasticity young=\"1e4\" poisson=\"0.0\"/>\
</flexcomp>\
</body>\
</worldbody>\
</mujoco>";
let mut spec = MjSpec::from_xml_string(FLEX_MODEL).expect("failed to parse flex model");
let flex = spec.flex("myflex").expect("flex 'myflex' not found in spec");
assert_eq!(flex.cellcount().len(), 3);
{
let flex_mut = spec.flex_mut("myflex").unwrap();
flex_mut.set_order(2);
}
assert_eq!(spec.flex("myflex").unwrap().order(), 2);
{
let flex_mut = spec.flex_mut("myflex").unwrap();
flex_mut.set_order(1);
}
assert_eq!(spec.flex("myflex").unwrap().order(), 1);
}
#[test]
#[should_panic]
fn test_sensor_objtype_failure() {
let mut spec = MjSpec::new();
spec.add_sensor()
.with_objtype(MjtObj::mjOBJ_FRAME);
}
#[test]
#[should_panic]
fn test_sensor_reftype_failure() {
let mut spec = MjSpec::new();
spec.add_sensor()
.with_reftype(MjtObj::mjOBJ_FRAME);
}
#[test]
fn test_sensor_objtype_reftype_setters() {
let mut spec = MjSpec::new();
let sensor = spec.add_sensor();
assert!(matches!(
sensor.set_objtype(MjtObj::mjOBJ_MODEL),
Err(MjEditError::InvalidParameter(_))
));
assert!(matches!(
sensor.set_reftype(MjtObj::mjOBJ_DEFAULT),
Err(MjEditError::InvalidParameter(_))
));
assert!(sensor.set_objtype(MjtObj::mjOBJ_SITE).is_ok());
assert!(sensor.set_reftype(MjtObj::mjOBJ_BODY).is_ok());
}
#[test]
fn test_tuple_objtype_validation() {
let mut spec = MjSpec::new();
let tuple = spec.add_tuple();
assert!(matches!(
tuple.set_objtype(&[MjtObj::mjOBJ_BODY, MjtObj::mjOBJ_FRAME]),
Err(MjEditError::InvalidParameter(_))
));
assert!(tuple.set_objtype(&[MjtObj::mjOBJ_BODY, MjtObj::mjOBJ_GEOM]).is_ok());
}
#[test]
fn test_numeric_size_validation() {
let mut spec = MjSpec::new();
let numeric = spec.add_numeric();
assert!(matches!(
numeric.set_size(-1),
Err(MjEditError::InvalidParameter(_))
));
assert!(numeric.set_size(4).is_ok());
}
}