Beispiel #1
0
 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)
Beispiel #2
0
 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)
Beispiel #4
0
 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)