File size: 1,851 Bytes
987ed1b | 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 | import struct
import numpy as np
def parse_mjl_logs(read_filename, skipamount):
with open(read_filename, mode='rb') as file:
fileContent = file.read()
headers = struct.unpack('iiiiiii', fileContent[:28])
nq = headers[0]
nv = headers[1]
nu = headers[2]
nmocap = headers[3]
nsensordata = headers[4]
nuserdata = headers[5]
name_len = headers[6]
name = struct.unpack(str(name_len) + 's', fileContent[28:28+name_len])[0]
rem_size = len(fileContent[28 + name_len:])
num_floats = int(rem_size/4)
dat = np.asarray(struct.unpack(str(num_floats) + 'f', fileContent[28+name_len:]))
recsz = 1 + nq + nv + nu + 7*nmocap + nsensordata + nuserdata
if rem_size % recsz != 0:
print("ERROR")
else:
dat = np.reshape(dat, (int(len(dat)/recsz), recsz))
dat = dat.T
time = dat[0,:][::skipamount] - 0*dat[0, 0]
qpos = dat[1:nq + 1, :].T[::skipamount, :]
qvel = dat[nq+1:nq+nv+1,:].T[::skipamount, :]
ctrl = dat[nq+nv+1:nq+nv+nu+1,:].T[::skipamount,:]
mocap_pos = dat[nq+nv+nu+1:nq+nv+nu+3*nmocap+1,:].T[::skipamount, :]
mocap_quat = dat[nq+nv+nu+3*nmocap+1:nq+nv+nu+7*nmocap+1,:].T[::skipamount, :]
sensordata = dat[nq+nv+nu+7*nmocap+1:nq+nv+nu+7*nmocap+nsensordata+1,:].T[::skipamount,:]
userdata = dat[nq+nv+nu+7*nmocap+nsensordata+1:,:].T[::skipamount,:]
data = dict(nq=nq,
nv=nv,
nu=nu,
nmocap=nmocap,
nsensordata=nsensordata,
name=name,
time=time,
qpos=qpos,
qvel=qvel,
ctrl=ctrl,
mocap_pos=mocap_pos,
mocap_quat=mocap_quat,
sensordata=sensordata,
userdata=userdata,
logName = read_filename
)
return data
|