rapier3d-mjcf 0.34.0

MuJoCo MJCF loader for the 3D rapier physics engine.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201

//! Phase-2 tests: defaults, includes, frame folding, inertiafromgeom.

use rapier3d::dynamics::{ImpulseJointSet, RigidBodySet};
use rapier3d::geometry::ColliderSet;
use rapier3d_mjcf::{MjcfLoaderOptions, MjcfRobot};
use std::fs;
use tempfile::tempdir;

#[test]
fn defaults_baked_into_joints() {
    let xml = r#"
    <mujoco>
      <default>
        <joint damping="0.5" type="hinge"/>
        <default class="leg">
          <joint damping="0.3" range="-30 30"/>
        </default>
      </default>
      <worldbody>
        <body name="b">
          <joint name="j1"/>
          <joint name="j2" class="leg"/>
          <inertial mass="1" diaginertia="0.1 0.1 0.1"/>
          <geom type="box" size="0.1 0.1 0.1"/>
        </body>
      </worldbody>
    </mujoco>
    "#;
    let (robot, model) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap();

    // The parser baked the defaults into the AST.
    let body = &model.bodies[1].body;
    assert_eq!(body.joints[0].damping, 0.5);
    assert_eq!(body.joints[1].damping, 0.3);
    let r = body.joints[1].range.unwrap();
    assert!((r[0] + 30.0_f64.to_radians()).abs() < 1e-12);

    // The robot has 2 joints (this is a multi-joint body so 1 intermediate).
    assert_eq!(robot.joints.len(), 2);
}

#[test]
fn childclass_propagates() {
    let xml = r#"
    <mujoco>
      <default>
        <default class="leg">
          <joint damping="0.7"/>
        </default>
      </default>
      <worldbody>
        <body childclass="leg">
          <body>
            <joint name="distal"/>
            <inertial mass="1" diaginertia="0.1 0.1 0.1"/>
          </body>
        </body>
      </worldbody>
    </mujoco>
    "#;
    let (_robot, model) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap();
    // The grand-child body's joint inherits from the legacy `leg` class.
    let inner = &model.bodies[2].body;
    assert_eq!(inner.joints[0].damping, 0.7);
}

#[test]
fn include_inlines_children() {
    let dir = tempdir().unwrap();
    let inc = dir.path().join("inc.xml");
    fs::write(
        &inc,
        r#"<mujoco>
          <default>
            <joint damping="0.42"/>
          </default>
        </mujoco>"#,
    )
    .unwrap();
    let main = dir.path().join("main.xml");
    fs::write(
        &main,
        r#"<mujoco>
          <include file="inc.xml"/>
          <worldbody>
            <body><joint name="j"/><inertial mass="1" diaginertia="0.1 0.1 0.1"/></body>
          </worldbody>
        </mujoco>"#,
    )
    .unwrap();
    let (_, model) = MjcfRobot::from_file(&main, MjcfLoaderOptions::default()).unwrap();
    let body = &model.bodies[1].body;
    assert_eq!(body.joints[0].damping, 0.42);
}

#[test]
fn frame_folds_pose_into_children() {
    let xml = r#"
    <mujoco>
      <worldbody>
        <body name="parent" pos="0 0 0">
          <freejoint/>
          <inertial mass="1" diaginertia="0.1 0.1 0.1"/>
          <frame pos="0 0 0.5">
            <body name="child" pos="0.1 0 0">
              <inertial mass="0.1" diaginertia="0.01 0.01 0.01"/>
            </body>
          </frame>
        </body>
      </worldbody>
    </mujoco>
    "#;
    let (_robot, model) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap();
    // The child's body pose should be (0.1, 0, 0) translated by (0, 0, 0.5).
    let child = &model.bodies[2].body;
    assert!((child.pose.translation.x - 0.1).abs() < 1e-12);
    assert!((child.pose.translation.z - 0.5).abs() < 1e-12);
}

#[test]
fn inertia_from_geom_when_no_inertial() {
    let xml = r#"
    <mujoco>
      <compiler inertiafromgeom="auto"/>
      <worldbody>
        <body name="ball">
          <freejoint/>
          <geom type="sphere" size="0.1" density="2000"/>
        </body>
      </worldbody>
    </mujoco>
    "#;
    let (robot, _) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap();
    let mut bodies = RigidBodySet::new();
    let mut colliders = ColliderSet::new();
    let mut impulse_joints = ImpulseJointSet::new();
    let handles =
        robot.insert_using_impulse_joints(&mut bodies, &mut colliders, &mut impulse_joints);
    // Force a mass-properties update by simulating one step.
    use rapier3d::prelude::*;
    let mut multibody_joints = MultibodyJointSet::new();
    let mut ccd = CCDSolver::new();
    let mut pipeline = PhysicsPipeline::new();
    let integration_parameters = IntegrationParameters::default();
    let mut islands = IslandManager::new();
    let mut broad_phase = DefaultBroadPhase::new();
    let mut narrow_phase = NarrowPhase::new();
    let gravity = Vector::new(0.0, 0.0, -9.81);
    let physics_hooks = ();
    let event_handler = ();
    pipeline.step(
        gravity,
        &integration_parameters,
        &mut islands,
        &mut broad_phase,
        &mut narrow_phase,
        &mut bodies,
        &mut colliders,
        &mut impulse_joints,
        &mut multibody_joints,
        &mut ccd,
        &physics_hooks,
        &event_handler,
    );
    let h = handles.bodies[1].as_ref().unwrap().body;
    let mass = bodies.get(h).unwrap().mass();
    // Sphere of r=0.1 with density 2000 → m = 2000 * (4/3)π r³ ≈ 8.38
    assert!(mass > 5.0 && mass < 12.0, "mass = {mass}");
}

#[test]
fn discardvisual_drops_visual_geoms() {
    let xml = r#"
    <mujoco>
      <compiler discardvisual="true"/>
      <worldbody>
        <body><freejoint/>
          <inertial mass="1" diaginertia="0.1 0.1 0.1"/>
          <!-- visual-only geom (contype=conaffinity=0) -->
          <geom type="sphere" size="0.1" contype="0" conaffinity="0"/>
          <geom type="sphere" size="0.05"/>
        </body>
      </worldbody>
    </mujoco>
    "#;
    // create_colliders_from_visual_shapes would normally accept the visual geom.
    let opts = MjcfLoaderOptions {
        create_colliders_from_visual_shapes: true,
        ..Default::default()
    };
    let (robot, _) = MjcfRobot::from_str(xml, opts, ".").unwrap();
    // discardvisual=true should still drop the visual geom even though
    // create_colliders_from_visual_shapes is on.
    let mut bodies = RigidBodySet::new();
    let mut colliders = ColliderSet::new();
    let mut impulse_joints = ImpulseJointSet::new();
    let handles =
        robot.insert_using_impulse_joints(&mut bodies, &mut colliders, &mut impulse_joints);
    let dyn_body = handles.bodies[1].as_ref().unwrap();
    assert_eq!(dyn_body.colliders.len(), 1);
}