-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathyumi_exp_data_extraction.py
92 lines (74 loc) · 2.32 KB
/
yumi_exp_data_extraction.py
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
import imp
import numpy as np
import pickle
from YumiKinematics import YumiKinematics
from mjc_exp_policy import kin_params
from gps.utility.data_logger import DataLogger
from gps.proto.gps_pb2 import JOINT_ANGLES
from gps.proto.gps_pb2 import JOINT_VELOCITIES
from gps.proto.gps_pb2 import ACTION
# logfile = './Results/yumi_peg_exp_raw_data_34.p'
logfile = './Results/yumi_peg_exp_new_raw_data_train.p'
data_logger = DataLogger()
# gps_dir = '/home/shahbaz/Research/Software/Spyder_ws/gps_model_learning/'
# exp_name = 'yumi_model_learning_3'
gps_dir = '/home/shahbaz/Research/Software/Spyder_ws/gps/'
exp_name = 'yumi_model_learning_new_1'
itr = 0
exp_dir = gps_dir + 'experiments/' + exp_name + '/'
hyperparams_file = exp_dir + 'hyperparams.py'
hyperparams = imp.load_source('hyperparams', hyperparams_file)
config = hyperparams.config
_data_files_dir = config['common']['data_files_dir']
yumiKin = YumiKinematics(kin_params)
traj_sample_lists = data_logger.unpickle(_data_files_dir +
('traj_sample_itr_%02d.pkl' % itr))
# alg_sample_lists = data_logger.unpickle(_data_files_dir +
# ('algorithm_itr_%02d.pkl' % itr))
traj_sample_list = traj_sample_lists[0] # cond 0
num_samples = len(traj_sample_list)
Ts = traj_sample_list[0].T
Xs = []
Us = []
for sample in traj_sample_list:
jp_pt = sample.get(JOINT_ANGLES)
jv_pt = sample.get(JOINT_VELOCITIES)
Xs.append(np.concatenate((jp_pt,jv_pt),axis=1))
jt_pt = sample.get(ACTION)
Us.append(jt_pt)
Xs = np.array(Xs)
Us = np.array(Us)
dP = 7
dV = 7
dU = 7
N, T, dX = Xs.shape
assert(T==Ts)
assert(Xs.shape[2]==(dP+dV))
assert(dP==dV)
Qts = Xs[:, :, :dP]
Qts_d = Xs[:, :, dP:dP+dV]
Uts = Us
Ets = np.zeros((N, T, 6))
Ets_d = np.zeros((N, T, 6))
Fts = np.zeros((N, T, 6))
for n in range(N):
for i in range(T):
Ets[n, i] = yumiKin.fwd_pose(Qts[n,i])
J_A = yumiKin.get_analytical_jacobian(Qts[n,i])
Ets_d[n,i] = J_A.dot(Qts_d[n,i])
Fts[n,i] = np.linalg.pinv(J_A.T).dot(Uts[n,i])
EXts = np.concatenate((Ets,Ets_d),axis=2)
exp_params = {}
exp_params['dP'] = 7
exp_params['dV'] = 7
exp_params['dU'] = 7
exp_params['dt'] = 0.05
exp_params['T'] = T
exp_params['num_samples'] = N
exp_data={}
exp_data['exp_params'] = exp_params
exp_data['X'] = Xs
exp_data['U'] = Us
exp_data['EX'] = EXts
exp_data['F'] = Fts
# pickle.dump( exp_data, open( logfile, "wb" ) )