refresh all scripts

chenlx92 · chenlx92 · commit 3dd86d4734fb · 2018-05-10T16:41:27.000+08:00
diff --git a/scripts/feature_selection.py b/scripts/feature_selection.py
@@ -12,6 +12,7 @@
 from sklearn.feature_selection import VarianceThreshold
 import matplotlib.pyplot as plt
 
+
 # the current file path
 file_path = os.path.dirname(__file__)
 
diff --git a/scripts/hmm_based_state_estimation.py b/scripts/hmm_based_state_estimation.py
@@ -0,0 +1,62 @@
+#!/usr/bin/python
+import numpy as np
+from hmmlearn import hmm
+import os
+import ConfigParser
+from sklearn.externals import joblib
+from sklearn.mixture import GaussianMixture
+
+# read the current file path
+file_path = os.path.dirname(__file__)
+# read model cfg file
+cp_models = ConfigParser.SafeConfigParser()
+cp_models.read(os.path.join(file_path, '../cfg/models.cfg'))
+datasets_path = os.path.join(file_path, cp_models.get('datasets', 'path'))
+
+# load dataset
+datasets_raw = joblib.load(os.path.join(datasets_path, 'pkl/datasets_raw.pkl'))
+
+# task name
+task_name = joblib.load(os.path.join(datasets_path, 'pkl/task_name_list.pkl'))
+
+# joint number
+num_joints = cp_models.getint('datasets', 'num_joints')
+
+
+def main():
+    states = task_name
+    n_task = len(states)
+
+    # don't know the task prior here
+    start_probability = np.array(([1.0/n_task]*n_task))
+
+    # don't know the task transition probability here
+    transition_probability = np.array([1.0/n_task]*n_task*n_task).reshape([n_task, n_task])
+
+    gmm_model_full = [GaussianMixture(n_components = 3) for i in range(n_task)]
+
+    task_data_full = []
+    for task_idx, task_data in enumerate(datasets_raw):
+        demo_data_full = np.array([]).reshape(0, num_joints)
+        for demo_data in task_data:
+            h = np.hstack([demo_data['left_hand'], demo_data['left_joints']])
+            demo_data_full = np.vstack([demo_data_full, h])
+        task_data_full.append(demo_data_full)
+
+    for task_data, gmm_model in zip(task_data_full, gmm_model_full):
+        gmm_model.fit(task_data)
+
+    # hmm_model = hmm.GMMHMM()
+    # hmm_model.n_components = n_task
+    # hmm_model.startprob_ = start_probability
+    # hmm_model.transmat_ = transition_probability
+    # hmm_model.means_ = gmm_model_full[0].means_
+
+    # testing
+    hmm_model1 = hmm.GMMHMM()
+    hmm_model1.n_components = 10
+    hmm_model1.fit(task_data_full[0])
+    print 1
+
+if __name__ == '__main__':
+    main()
diff --git a/scripts/ipromps_lib.py b/scripts/ipromps_lib.py
@@ -452,11 +452,7 @@ def __init__(self, num_joints=28, num_obs_joints=None, num_basis=11, sigma_basis
         :param num_alpha_candidate:
         """
         # compute the obs noise after preprocessing
-        # noise_cov_full = min_max_scaler.scale_.T * sigmay * min_max_scaler.scale_
-        # noise_cov_full = np.diag(min_max_scaler.scale_, 0) * np.diag(min_max_scaler.scale_, 0) * sigmay
-        # noise_cov_full = np.diag(min_max_scaler.scale_, 0) * sigmay
         noise_cov_full = sigmay
-
         NDProMP.__init__(self, num_joints=num_joints, num_basis=num_basis,
                          sigma_basis=sigma_basis, num_samples=num_samples, sigmay=noise_cov_full)
 
diff --git a/scripts/load_data.py b/scripts/load_data.py
@@ -40,19 +40,18 @@ def main():
     datasets_raw = []
     for task_path in task_path_list:
         task_csv_path = os.path.join(task_path, 'csv')
-        print('Loading data from ' + task_csv_path)
-        demo_path_list = glob.glob(os.path.join(task_csv_path, '201*'))   # the prefix of data, guarantee the right file
+        print('Loading data from: ' + task_csv_path)
+        demo_path_list = glob.glob(os.path.join(task_csv_path, '201*'))   # the prefix of dataset file
         demo_temp = []
         for demo_path in demo_path_list:
             data_csv = pd.read_csv(os.path.join(demo_path, 'multiModal_states.csv'))    # the file name of csv
-            # the info of interest: convert the object to int / float
             demo_temp.append({
-                              'stamp': (data_csv.values[:, 2].astype(int)-data_csv.values[0, 2])*1e-9,  # the time stamp
-                              'left_hand':np.hstack([
+                              'stamp': (data_csv.values[:, 2].astype(int)-data_csv.values[0, 2])*1e-9,
+                              'left_hand': np.hstack([
                                   data_csv.values[:, 207:210].astype(float),   # human left hand position
                                   data_csv.values[:, 7:15].astype(float),  # emg
                                   ]),
-                              'left_joints': data_csv.values[:, 317:320].astype(float)  # robot ee actually
+                              'left_joints': data_csv.values[:, 317:324].astype(float)  # robot ee actually
                               })
         datasets_raw.append(demo_temp)
 
@@ -69,7 +68,6 @@ def main():
             # append them to list
             demo_norm_temp.append({
                 'alpha': time_stamp[-1],
-                # 'emg': emg_filtered,
                 'left_hand': left_hand_filtered,
                 'left_joints': left_joints_filtered
             })
@@ -87,13 +85,11 @@ def main():
             left_hand_filtered = gaussian_filter1d(demo_data['left_hand'].T, sigma=sigma).T
             left_joints_filtered = gaussian_filter1d(demo_data['left_joints'].T, sigma=sigma).T
             # normalize the datasets
-            # emg_norm = griddata(time_stamp, emg_filtered, grid, method='linear')
             left_hand_norm = griddata(time_stamp, left_hand_filtered, grid, method='linear')
             left_joints_norm = griddata(time_stamp, left_joints_filtered, grid, method='linear')
             # append them to list
             demo_norm_temp.append({
                                     'alpha': time_stamp[-1],
-                                    # 'emg': emg_norm,
                                     'left_hand': left_hand_norm,
                                     'left_joints': left_joints_norm
                                     })
@@ -106,7 +102,7 @@ def main():
         datasets4train.append(data)
     y_full = np.array([]).reshape(0, num_joints)
     for task_idx, task_data in enumerate(datasets4train):
-        print('Preprocessing data of task: ' + task_name_list[task_idx])
+        print('Preprocessing data for task: ' + task_name_list[task_idx])
         for demo_data in task_data:
             h = np.hstack([demo_data['left_hand'], demo_data['left_joints']])
             y_full = np.vstack([y_full, h])
@@ -120,11 +116,9 @@ def main():
             temp = datasets_norm_full[(task_idx * num_demo + demo_idx) * len_norm:
             (task_idx * num_demo + demo_idx) * len_norm + len_norm, :]
             datasets_temp.append({
-                                  # 'left_hand': temp[:, 0:3],
-                                  # 'left_joints': temp[:, 3:6],
                                     'left_hand': temp[:, 0:11],
-                                    'left_joints': temp[:, 11:14],
-                                  'alpha': datasets4train[task_idx][demo_idx]['alpha']})
+                                    'left_joints': temp[:, 11:18],
+                                    'alpha': datasets4train[task_idx][demo_idx]['alpha']})
         datasets_norm_preproc.append(datasets_temp)
 
     # save all the datasets
diff --git a/scripts/noise_cov_cal.py b/scripts/noise_cov_cal.py
@@ -20,16 +20,20 @@ def main():
     csv_path = os.path.join(datasets_path, 'info/noise/multiModal_states.csv')
     data = pd.read_csv(csv_path)
 
-    # extract the all signals data
+    # # extract the all signals data
+    # emg = data.values[:, 7:15].astype(float)
+    # left_hand = data.values[:, 207:210].astype(float)
+    # left_joints = data.values[:, 317:320].astype(float)     # robot ee actually
+    # # left_joints = data.values[:, 317:324].astype(float)  # robot ee actually
+    # # left_joints = data.values[:, 99:106].astype(float)
+
+    # extract the all signals data ORIENTATION
     emg = data.values[:, 7:15].astype(float)
     left_hand = data.values[:, 207:210].astype(float)
-    left_joints = data.values[:, 317:320].astype(float)     # robot ee actually
-    # left_joints = data.values[:, 317:324].astype(float)  # robot ee actually
-    # left_joints = data.values[:, 99:106].astype(float)
+    left_joints = data.values[:, 317:324].astype(float)  # robot ee actually
 
     # stack them as a big matrix
     full_data = np.hstack([left_hand, emg, left_joints])[1200:, :]
-    # full_data = np.hstack([left_hand, left_joints])
     full_data = min_max_scaler.transform(full_data)
 
     # compute the noise observation covariance matrix
diff --git a/scripts/test_data_gen.py b/scripts/test_data_gen.py
@@ -33,8 +33,8 @@
 
 def main():
     task_id = 0
-    test_index = 37
-    obs_ratio = 0.5
+    test_index = 25
+    obs_ratio = 0.1
 
     # read test data
     obs_data_dict = datasets_raw[task_id][test_index]
@@ -53,7 +53,7 @@ def main():
 
     # choose the data
     num_obs = int(len(timestamp)*obs_ratio)
-    num_obs = num_obs - num_obs%15
+    num_obs -= num_obs % 15
     obs_data_post_arr = obs_data_post_arr[0:num_obs:15, :]
     timestamp = timestamp[0:num_obs:15]
     obs_data_post_arr = obs_data_post_arr
@@ -112,5 +112,4 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
-    # visualization.main()
+    main()
diff --git a/scripts/test_data_gen_orientation.py b/scripts/test_data_gen_orientation.py
@@ -20,58 +20,55 @@
 sigma = cp_models.get('filter', 'sigma')
 ipromps_set = joblib.load(os.path.join(datasets_path, 'pkl/ipromps_set.pkl'))
 datasets_raw = joblib.load(os.path.join(datasets_path, 'pkl/datasets_raw.pkl'))
-test_index = cp_models.getint('visualization', 'test_index')
-
 
 # read datasets cfg file
 cp_datasets = ConfigParser.SafeConfigParser()
 cp_datasets.read(os.path.join(datasets_path, './info/cfg/datasets.cfg'))
+
 # read datasets params
 data_index_sec = cp_datasets.items('index_17')
-# data_index_sec = cp_datasets.items('index_25')
 data_index = [map(int, task[1].split(',')) for task in data_index_sec]
 
 
 def main():
-    task_id = 0
-    test_index = 37
-    obs_ratio = 0.35
+    # super param
+    task_id = 1
+    test_index = 1
+    obs_ratio = 0.5
 
-    # read test data
+    # load test data
     obs_data_dict = datasets_raw[task_id][test_index]
-
     left_hand = obs_data_dict['left_hand']
     left_joints = obs_data_dict['left_joints']
     obs_data = np.hstack([left_hand, left_joints])
     timestamp = obs_data_dict['stamp']
 
-    # filter the data
+    # filter data and preprocessing data
     obs_data = gaussian_filter1d(obs_data.T, sigma=sigma).T
-    # preprocessing for the data
-    obs_data_post_arr = ipromps_set[0].min_max_scaler.transform(obs_data)
-    # consider the unobserved info
-    obs_data_post_arr[:, -7:] = 0.0
+    obs_data_preproc = ipromps_set[0].min_max_scaler.transform(obs_data)
+    # set unobserved info
+    obs_data_preproc[:, -7:] = 0.0
 
     # choose the data
     num_obs = int(len(timestamp)*obs_ratio)
-    num_obs = num_obs - num_obs%15
-    obs_data_post_arr = obs_data_post_arr[0:num_obs:15, :]
+    num_obs -= num_obs % 15
+    obs_data_preproc = obs_data_preproc[0:num_obs:15, :]
     timestamp = timestamp[0:num_obs:15]
 
     # phase estimation
     print('Phase estimating...')
     alpha_max_list = []
     for ipromp in ipromps_set:
         alpha_temp = ipromp.alpha_candidate(num_alpha_candidate)
-        idx_max = ipromp.estimate_alpha(alpha_temp, obs_data_post_arr, timestamp)
+        idx_max = ipromp.estimate_alpha(alpha_temp, obs_data_preproc, timestamp)
         alpha_max_list.append(alpha_temp[idx_max]['candidate'])
         ipromp.set_alpha(alpha_temp[idx_max]['candidate'])
 
     # task recognition
     print('Adding via points in each trained model...')
     for task_idx, ipromp in enumerate(ipromps_set):
         for idx in range(len(timestamp)):
-            ipromp.add_viapoint(timestamp[idx] / alpha_max_list[task_idx], obs_data_post_arr[idx, :])
+            ipromp.add_viapoint(timestamp[idx] / alpha_max_list[task_idx], obs_data_preproc[idx, :])
         ipromp.param_update(unit_update=True)
     print('Computing the likelihood for each model under observations...')
 
@@ -83,25 +80,15 @@ def main():
     # idx_max_prob = 0 # a trick for testing
     print('The max fit model index is task %s' % task_name[idx_max_prob])
 
-    # # robot motion generation
-    # [traj_time, traj] = ipromps_set[idx_max_prob].gen_real_traj(alpha_max_list[idx_max_prob])
-    # traj = ipromps_set[idx_max_prob].min_max_scaler.inverse_transform(traj)
-    # robot_traj = traj[:, -3:]
-
     # robot motion generation
     traj_full = []
     for ipromp_id, ipromp in enumerate(ipromps_set):
         [traj_time, traj] = ipromp.gen_real_traj(alpha_max_list[ipromp_id])
         traj = ipromp.min_max_scaler.inverse_transform(traj)
         robot_traj = traj[:, -7:]
-        human_traj= traj[:, 0:-7]
+        human_traj = traj[:, 0:3]
         traj_full.append([human_traj, robot_traj])
 
-    # # test: robot motion generation for task2
-    # [traj_time2, traj2] = ipromps_set[2].gen_real_traj(alpha_max_list[2])
-    # traj2 = ipromps_set[2].min_max_scaler.inverse_transform(traj2)
-    # robot_traj2 = traj2[:, -3:]
-
     # save the conditional result
     print('Saving the post IProMPs...')
     joblib.dump(ipromps_set, os.path.join(datasets_path, 'pkl/ipromps_set_post_offline.pkl'))
@@ -112,4 +99,3 @@ def main():
 
 if __name__ == '__main__':
     main()
-    # visualization.main()
diff --git a/scripts/visualization.py b/scripts/visualization.py
@@ -27,8 +27,8 @@
 datasets_norm_preproc = joblib.load(os.path.join(datasets_path, 'pkl/datasets_norm_preproc.pkl'))
 task_name = joblib.load(os.path.join(datasets_path, 'pkl/task_name_list.pkl'))
 [robot_traj_offline, ground_truth, num_obs] = joblib.load(os.path.join(datasets_path, 'pkl/robot_traj_offline.pkl'))
-robot_traj_online = joblib.load(os.path.join(datasets_path, 'pkl/robot_traj_online.pkl'))
-obs_data_online = joblib.load(os.path.join(datasets_path, 'pkl/obs_data_online.pkl'))
+# robot_traj_online = joblib.load(os.path.join(datasets_path, 'pkl/robot_traj_online.pkl'))
+# obs_data_online = joblib.load(os.path.join(datasets_path, 'pkl/obs_data_online.pkl'))
 
 
 # read datasets cfg file
@@ -44,8 +44,8 @@
 #             'left_joints': [11, 14]
 #             }
 info_n_idx = {
-            'left_hand': [0, 3],
-            'left_joints': [3, 6]
+            'left_hand': [0, 11],
+            'left_joints': [11, 18]
             }
 # the info to be plotted
 info = cp_models.get('visualization', 'info')
@@ -265,7 +265,7 @@ def plot_offline_3d_obs(num=0):
         ax.plot(data[:, 0], data[:, 1], data[:, 2],
                 '-', linewidth=5, color='blue', label='Human trajectory ground truth', alpha = 1.0)
         data = ground_truth['left_joints']
-        ax.plot(data[:, -3], data[:, -2], data[:, -1],
+        ax.plot(data[:, -7], data[:, -6], data[:, -5],
                 linewidth=5, linestyle='-', color='r', label='Robot trajectory ground Truth', alpha=1.0)
         ax.set_xlabel('X (m)')
         ax.set_ylabel('Y (m)')
@@ -298,7 +298,7 @@ def plot_gen_3d_offline_r_traj(num=0):
         fig = plt.figure(task_idx+num)
         ax = fig.gca(projection='3d')
         data = robot_traj_offline[task_idx][1]
-        ax.plot(data[:, -3], data[:, -2], data[:, -1], 'r',
+        ax.plot(data[:, -7], data[:, -6], data[:, -5], 'r',
                 linewidth=5, linestyle='--', label='Predicted robot trajectory')
         data = robot_traj_offline[task_idx][0]
         ax.plot(data[:, 0], data[:, 1], data[:, 2], 'b',
