def test_mjfunctions(self):
# Test calling mujoco function from within callback
fn = '''
void fun(const mjModel *m, mjData *d) {
// set userdata to rotation matrix for body_xquat[2]
mju_quat2Mat(d->userdata, &(d->xquat[4 * 2]));
}
'''
sim = MjSim(load_model_from_xml(XML.format(nuserdata=9)),
substep_callback=fn)
sim.data.ctrl[:] = [9, 13]
for _ in range(30):
sim.step()
sim.substep_callback()
mat = np.zeros(9, dtype=np.float64)
functions.mju_quat2Mat(mat, sim.data.body_xquat[2])
np.testing.assert_array_equal(sim.data.userdata, mat)
def test_quat2mat(self):
s = (N, N, 4)
quats = uniform(-1, 1, size=s) * randint(2, size=s)
mats = quat2mat(quats)
self.assertEqual(mats.shape, (N, N, 3, 3))
for i in range(N):
for j in range(N):
# Compare to transforms3d
res = quaternions.quat2mat(quats[i, j])
np.testing.assert_almost_equal(mats[i, j], res)
# Compare to MuJoCo
quat = normalize_quat(quats[i, j])
mat = np.zeros(9, dtype=np.float64)
functions.mju_quat2Mat(mat, quat)
if np.isnan(mat).any():
continue # MuJoCo returned NaNs
np.testing.assert_almost_equal(quat2mat(quat),
mat.reshape((3, 3)))
"""
model = load_model_from_xml(MODEL_XML)
sim = MjSim(model)
print("Nicely exposed function:\n")
print(sim.model.get_xml())
print("\nversus MuJoCo internals:\n\n")
functions.mj_saveLastXML("/tmp/saved.xml", model, "", 0)
with open("/tmp/saved.xml", "r") as f:
print(f.read())
sim.render(100, 100)
modelpos = np.zeros(3)
modelquat = np.zeros(4)
roompos = np.ones(3)
roomquat = np.array([1., 0., 1., 0.])
functions.mjv_room2model(modelpos, modelquat, roompos, roomquat,
sim.render_contexts[0].scn)
print("\n\nAnother internal function, mjv_room2model:")
print("modelpos = %s, modelquat = %s" % (str(modelpos), str(modelquat)))
res = np.zeros(9)
functions.mju_quat2Mat(res, roomquat)
print("\n\nAnother internal function, mju_quat2Mat:\n%s" % res)
def quat2mat(self, quat):
result = np.empty(9, dtype=np.double)
functions.mju_quat2Mat(result, np.asarray(quat))
return result
model = load_model_from_xml(MODEL_XML)
sim = MjSim(model)
print("Nicely exposed function:\n")
print(sim.model.get_xml())
print("\nversus MuJoCo internals:\n\n")
functions.mj_saveLastXML("/tmp/saved.xml", model, "", 0)
with open("/tmp/saved.xml", "r") as f:
print(f.read())
sim.render(100, 100)
modelpos = np.zeros(3)
modelquat = np.zeros(4)
roompos = np.ones(3)
roomquat = np.array([1., 0., 1., 0.])
functions.mjv_room2model(modelpos, modelquat, roompos,
roomquat, sim.render_contexts[0].scn)
print("\n\nAnother internal function, mjv_room2model:")
print("modelpos = %s, modelquat = %s" % (str(modelpos), str(modelquat)))
res = np.zeros(9)
functions.mju_quat2Mat(res, roomquat)
print("\n\nAnother internal function, mju_quat2Mat:\n%s" % res)