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
202
203
204
205
206
207
208
209
210
211
212
213
214
215
|
// Copyright 2019 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
using System;
using System.Collections;
using System.Collections.Generic;
using System.Xml;
using NUnit.Framework;
using UnityEngine;
using UnityEngine.TestTools.Utils;
namespace Mujoco {
[TestFixture]
public class MjSceneGenerationTests {
[Test]
public void AssigningComponentsUniqueName() {
_fakeBodyA.enabled = true;
_fakeBodyB.enabled = true;
_scene.CreateScene(skipCompile:true);
Assert.That(_fakeBodyA.MujocoName, Is.Not.EqualTo(_fakeBodyB.MujocoName));
}
[Test]
public void AttachingComponentsToMjNodesThroughId() {
_fakeBodyA.enabled = true;
_fakeBodyB.enabled = true;
_scene.CreateScene();
Assert.That(_fakeBodyA.MujocoId, Is.GreaterThanOrEqualTo(0));
Assert.That(_fakeBodyB.MujocoId, Is.GreaterThanOrEqualTo(0));
Assert.That(_fakeBodyA.MujocoId, Is.Not.EqualTo(_fakeBodyB.MujocoId));
}
[Test]
public unsafe void ModelStructureIsInitialized() {
_fakeBodyA.enabled = true;
_fakeBodyB.enabled = true;
_scene.CreateScene();
Assert.That(_scene.Model->nbody, Is.GreaterThanOrEqualTo(2));
}
[Test]
public void SyncingComponentStateOnFixedUpdate() {
_fakeBodyA.enabled = true;
_fakeBodyB.enabled = true;
_scene.CreateScene();
Assert.That(_fakeBodyA.StateSynced, Is.False);
Assert.That(_fakeBodyB.StateSynced, Is.False);
_scene.StepScene();
Assert.That(_fakeBodyA.StateSynced, Is.True);
Assert.That(_fakeBodyB.StateSynced, Is.True);
}
[Test]
public unsafe void PhysicsRuntimeError() {
_scene.CreateScene();
_scene.Data->qpos[0] = float.PositiveInfinity;
Assert.That(
() => { _scene.StepScene(); },
Throws.TypeOf<PhysicsRuntimeException>()
.With.Message.EqualTo("BADQPOS: NaN/inf in qpos."));
}
[Test]
public void MjcfHierarchyMirrorsTheHierarchyOfUnityObjects() {
_fakeBodyA.transform.parent = _fakeBodyB.transform;
_fakeBodyA.enabled = true;
_fakeBodyB.enabled = true;
var mjcf = _scene.CreateScene(skipCompile:true);
// Three components are already in this scene: body, joint and inertia (see SetUp function below)
var mjcfFakeBodyB = mjcf.SelectNodes("/mujoco/worldbody/body")[1] as XmlElement;
var mjcfFakeBodyA = mjcf.SelectNodes("/mujoco/worldbody/body/body")[0] as XmlElement;
Assert.That(mjcfFakeBodyA, Is.Not.Null);
Assert.That(mjcfFakeBodyB, Is.Not.Null);
// B is the parent, so it appears first in the XML
Assert.That(mjcfFakeBodyB.GetAttribute("name"), Is.EqualTo("component_3"));
Assert.That(mjcfFakeBodyA.GetAttribute("name"), Is.EqualTo("component_4"));
}
[Test]
public void ActuatorsAreAddedToDedicatedTag() {
_actuator.enabled = true;
var mjcf = _scene.CreateScene(skipCompile:true);
var mjcfForActuator = mjcf.SelectNodes("/mujoco/actuator/general")[0] as XmlElement;
Assert.That(mjcfForActuator, Is.Not.Null);
}
[Test]
public void SensorsAreAddedToDedicatedTag() {
_sensor.enabled = true;
var mjcf = _scene.CreateScene(skipCompile:true);
var mjcfForSensor = mjcf.SelectNodes("/mujoco/sensor/jointpos")[0] as XmlElement;
Assert.That(mjcfForSensor, Is.Not.Null);
}
[Test]
public unsafe void SceneRecreatedWithAddition() {
_scene.CreateScene();
_scene.Data->qvel[0] = 1;
var nq = _scene.Model->nq;
var tickedRotation = _body.transform.rotation;
var body = new GameObject("body").AddComponent<MjBody>();
var inertia = new GameObject("inertia").AddComponent<MjInertial>();
inertia.transform.parent = body.transform;
var joint = new GameObject("joint").AddComponent<MjHingeJoint>();
joint.transform.parent = body.transform;
_scene.RecreateScene();
Assert.That(_scene.Model->nq, Is.EqualTo(nq+1));
Assert.That(_scene.Data->qvel[joint.DofAddress], Is.EqualTo(0));
Assert.That(_scene.Data->qvel[_joint.DofAddress], Is.EqualTo(1));
UnityEngine.Object.DestroyImmediate(inertia.gameObject);
UnityEngine.Object.DestroyImmediate(joint.gameObject);
UnityEngine.Object.DestroyImmediate(body.gameObject);
}
[Test]
public unsafe void SceneRecreatedAfterDeletion() {
var body = new GameObject("body").AddComponent<MjBody>();
var inertia = new GameObject("inertia").AddComponent<MjInertial>();
inertia.transform.parent = body.transform;
var joint = new GameObject("joint").AddComponent<MjHingeJoint>();
joint.transform.parent = body.transform;
_scene.CreateScene();
_scene.Data->qvel[0] = 1;
_scene.Data->qvel[1] = 2;
var nq = _scene.Model->nq;
UnityEngine.Object.DestroyImmediate(inertia.gameObject);
UnityEngine.Object.DestroyImmediate(joint.gameObject);
UnityEngine.Object.DestroyImmediate(body.gameObject);
_scene.RecreateScene();
Assert.That(_scene.Model->nq, Is.EqualTo(nq-1));
Assert.That(_scene.Data->qvel[0], Is.EqualTo(2));
}
#region Test setup.
public class FakeMjBody : MjBaseBody {
public bool StateSynced = false;
public override MujocoLib.mjtObj ObjectType => MujocoLib.mjtObj.mjOBJ_BODY;
protected override void OnParseMjcf(XmlElement mjcf) {}
protected override XmlElement OnGenerateMjcf(XmlDocument doc) {
// We'll create an actual element because the code will then pass it on to Mujoco scene
// compiler, and we care that the compiler doesn't fail.
var mjcf = doc.CreateElement("body");
return mjcf;
}
public override unsafe void OnSyncState(MujocoLib.mjData_* data) {
StateSynced = true;
}
}
private MjScene _scene;
private FakeMjBody _fakeBodyA;
private FakeMjBody _fakeBodyB;
private MjJointScalarSensor _sensor;
private MjBody _body;
private MjInertial _inertia;
private MjHingeJoint _joint;
private MjActuator _actuator;
private Vector4EqualityComparer _quaternionComparer;
[SetUp]
public void SetUp() {
_quaternionComparer = new Vector4EqualityComparer(1e-5f);
_scene = MjScene.Instance;
_fakeBodyA = new GameObject("component").AddComponent<FakeMjBody>();
_fakeBodyB = new GameObject("component").AddComponent<FakeMjBody>();
_sensor = new GameObject("sensor").AddComponent<MjJointScalarSensor>();
_actuator = new GameObject("actuator").AddComponent<MjActuator>();
// body, joint and inertia are always present so that the actuator and sensor are valid
_body = new GameObject("body").AddComponent<MjBody>();
_inertia = new GameObject("inertia").AddComponent<MjInertial>();
_inertia.transform.parent = _body.transform;
_joint = new GameObject("joint").AddComponent<MjHingeJoint>();
_joint.transform.parent = _body.transform;
_sensor.Joint = _joint;
_sensor.SensorType = MjJointScalarSensor.AvailableSensors.JointPos;
_actuator.Joint = _joint;
_fakeBodyA.enabled = false;
_fakeBodyB.enabled = false;
_sensor.enabled = false;
_actuator.enabled = false;
}
[TearDown]
public void TearDown() {
UnityEngine.Object.DestroyImmediate(_fakeBodyA.gameObject);
UnityEngine.Object.DestroyImmediate(_fakeBodyB.gameObject);
UnityEngine.Object.DestroyImmediate(_sensor.gameObject);
UnityEngine.Object.DestroyImmediate(_actuator.gameObject);
UnityEngine.Object.DestroyImmediate(_joint.gameObject);
UnityEngine.Object.DestroyImmediate(_inertia.gameObject);
UnityEngine.Object.DestroyImmediate(_body.gameObject);
UnityEngine.Object.DestroyImmediate(_scene.gameObject);
}
#endregion
}
}
|
