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)