PCA and Plotting Code

gym/scripts/pca.py

@@ -0,0 +1,82 @@
+import pandas as pd
+import numpy as np
+import matplotlib.pyplot as plt
+from ast import literal_eval
+from sklearn.preprocessing import StandardScaler
+from sklearn.decomposition import PCA
+
+
+import os
+
+from gym.envs import __init__
+from gym import LEGGED_GYM_ROOT_DIR
+from gym.utils import get_args, task_registry
+
+# torch needs to be imported after isaacgym imports in local source
+import torch
+import pandas as pd
+import numpy as np
+import pandas as pd
+
+
+def setup(args):
+    env_cfg, train_cfg = task_registry.create_cfgs(args)
+    env_cfg.env.num_envs = min(env_cfg.env.num_envs, 16)
+    if hasattr(env_cfg, "push_robots"):
+        env_cfg.push_robots.toggle = False
+    env_cfg.commands.resampling_time = 1
+    env_cfg.env.episode_length_s = 9999
+    env_cfg.env.num_projectiles = 20
+    task_registry.make_gym_and_sim()
+    env = task_registry.make_env(args.task, env_cfg)
+    env.cfg.init_state.reset_mode = "reset_to_range"
+
+    return env
+args = get_args()
+env = setup(args)
+
+xls = pd.ExcelFile("/home/aileen/ORCAgym/gym/scripts/mini_cheetah_logs.xlsx")
+df1 = pd.read_excel(xls, 'q').to_numpy()
+df2 = pd.read_excel(xls, 'qd').to_numpy()
+df_feet_contacts = pd.read_excel(xls,'grf').to_numpy()
+
+print(env.dof_names)
+#PCA
+dataset = pd.DataFrame(df1)
+features = env.dof_names
+dataset.columns = features
+
+features = np.char.mod('%s', features).tolist()
+x = dataset.loc[:, features].values
+print(dataset.loc[:, features].values.shape)
+x = StandardScaler().fit_transform(x) # normalizing the features
+print(x.shape)
+print(np.mean(x),np.std(x))
+feat_cols = ['feature'+str(i) for i in range(x.shape[1])]
+normalised = pd.DataFrame(x,columns=feat_cols)
+print(normalised.tail())
+
+pca = PCA(n_components=3)
+principalComponents = pca.fit_transform(x)
+principal_Df = pd.DataFrame(data = principalComponents
+            , columns = ['principal component 1', 'principal component 2', 'principal component 3'])
+
+print(principal_Df.tail())
+
+print('Explained variation per principal component: {}'.format(pca.explained_variance_ratio_))
+
+#print(swing_dataset['label'] )
+
+plt.figure()
+plt.figure(figsize=(10,10))
+ax = plt.axes(projection ="3d")
+
+plt.title(f"Principal Component Analysis of Dataset",fontsize=20)
+ax.scatter3D(principal_Df.loc[:, 'principal component 1']
+            , principal_Df.loc[:, 'principal component 2']
+            ,principal_Df.loc[:, 'principal component 3'], s = 5)
+ax.set_xlabel('Principal Component - 1',fontsize=20)
+ax.set_ylabel('Principal Component - 2',fontsize=20)
+ax.set_zlabel('Principal Component - 3',fontsize=20)
+plt.show()
+#plt.savefig(f'pca plot')

gym/scripts/pcasteve.py

@@ -0,0 +1,121 @@
+import pandas as pd
+import numpy as np
+import matplotlib.pyplot as plt
+from ast import literal_eval
+from sklearn.preprocessing import StandardScaler
+from sklearn.decomposition import PCA
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib.patches import FancyArrowPatch
+from mpl_toolkits.mplot3d import proj3d
+from gym.envs import __init__
+from gym import LEGGED_GYM_ROOT_DIR
+from gym.utils import get_args, task_registry
+
+
+def setup(args):
+    env_cfg, train_cfg = task_registry.create_cfgs(args)
+    env_cfg.env.num_envs = min(env_cfg.env.num_envs, 16)
+    if hasattr(env_cfg, "push_robots"):
+        env_cfg.push_robots.toggle = False
+    env_cfg.commands.resampling_time = 1
+    env_cfg.env.episode_length_s = 9999
+    env_cfg.env.num_projectiles = 20
+    task_registry.make_gym_and_sim()
+    env = task_registry.make_env(args.task, env_cfg)
+    env.cfg.init_state.reset_mode = "reset_to_range"
+    return env
+
+def shuffle_along_axis(a, axis):
+    idx = np.random.rand(*a.shape).argsort(axis=axis)
+    return np.take_along_axis(a,idx,axis=axis)
+class Arrow3D(FancyArrowPatch):
+    def __init__(self, xs, ys, zs, *args, **kwargs):
+        super().__init__((0,0), (0,0), *args, **kwargs)
+        self._verts3d = xs, ys, zs
+
+    def do_3d_projection(self, renderer=None):
+        xs3d, ys3d, zs3d = self._verts3d
+        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.M)
+        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
+
+        return np.min(zs)
+
+args = get_args()
+env = setup(args)
+
+#import data
+set_num = 4 #from 1-4 for 4 legs 
+xls = pd.ExcelFile("/home/aileen/ORCAgym/gym/scripts/mini_cheetah_logs.xlsx")
+df1 = pd.read_excel(xls, 'q').to_numpy()[:,(set_num-1)*3:set_num*3]
+#df2 = pd.read_excel(xls, 'qd').to_numpy()
+#tau = pd.read_excel(xls, 'tau').to_numpy()
+#df_feet_contacts = pd.read_excel(xls,'grf').to_numpy()
+
+#randomize
+df1 = shuffle_along_axis(df1,0)
+# df1 = np.random.permutation(df1[:,0])
+#df1 = df1[np.random.permutation(np.arange(df1.shape[0])), :]
+print(df1.shape)
+
+#PCA
+dataset = pd.DataFrame(df1)
+features = env.dof_names[(set_num-1)*3:set_num*3]
+dataset.columns = features
+
+features = np.char.mod('%s', features).tolist()
+x = dataset.loc[:, features].values
+print(dataset.loc[:, features].values.shape)
+x = StandardScaler().fit_transform(x) # normalizing the features
+print(x.shape)
+print(np.mean(x),np.std(x))
+feat_cols = ['feature'+str(i) for i in range(x.shape[1])]
+normalised = pd.DataFrame(x,columns=feat_cols)
+print(normalised.tail())
+
+pca = PCA(n_components=3)
+principalComponents = pca.fit_transform(x)
+
+print(pca.components_)
+print(sum(pca.components_[:,0]*pca.components_[:,1]))
+print(sum(pca.components_[:,1]*pca.components_[:,2]))
+print(sum(pca.components_[:,2]*pca.components_[:,0]))
+principal_Df = pd.DataFrame(data = principalComponents
+            , columns = ['principal component 1', 'principal component 2', 'principal component 3'])
+print('Explained variation per principal component: {}'.format(pca.explained_variance_ratio_))
+
+
+#checks on orthogonality
+import math  
+a = pca.components_[:,0]
+b = pca.components_[:,1]
+c= pca.components_[:,2]
+print(np.dot(a,b))
+print(math.acos(np.dot(a,b)/(np.linalg.norm(a)*np.linalg.norm(b))))
+print(math.pi/2)
+print(math.acos(np.dot(c,b)/(np.linalg.norm(c)*np.linalg.norm(b))))
+print(math.acos(np.dot(c,a)/(np.linalg.norm(c)*np.linalg.norm(a))))
+
+#plotting
+fig = plt.figure(figsize=(10,10))
+ax = fig.add_subplot(111, projection='3d')
+ax.scatter3D(normalised.loc[:,"feature0"],normalised.loc[:,"feature1"],normalised.loc[:,"feature2"],s=5)
+    # #principal_Df.loc[:, 'principal component 1']
+    #         , principal_Df.loc[:, 'principal component 2']
+    #         ,principal_Df.loc[:, 'principal component 3'], s = 5)
+coeff = pca.components_.T
+print(coeff)
+for i in range(3):
+        arrow_prop_dict = dict(mutation_scale=20, arrowstyle='-|>', color='r', shrinkA=0, shrinkB=0)
+
+        a = Arrow3D([0,coeff[i,0]], [0,coeff[i,1]], [0,coeff[i,2]], **arrow_prop_dict)
+        ax.add_artist(a)
+        #plt.text(coeff[i,0]* 1.15, coeff[i,1] * 1.15, coeff[i,2]*1.15, "Var"+str(i+1), color = 'g', ha = 'center', va = 'center')
+
+plt.title(f"Principal Component Analysis of Dataset",fontsize=20)
+ax.set_xlabel(features[0],fontsize=20)
+ax.set_ylabel(features[1],fontsize=20)
+ax.set_zlabel(features[2],fontsize=20)
+plt.show()
+
+
+#plt.savefig(f'pca plot')

gym/scripts/play_ORC.py

@@ -7,27 +7,29 @@
 from ORC import adjust_settings
 # torch needs to be imported after isaacgym imports in local source
 import torch
+import numpy as np
+import pandas as pd 
 
 
 def setup(args):
     env_cfg, train_cfg = task_registry.create_cfgs(args)
     env_cfg.env.num_envs = 50
     if hasattr(env_cfg, "push_robots"):
         env_cfg.push_robots.toggle = False
-    env_cfg.commands.resampling_time = 9999
+    # env_cfg.commands.resampling_time = 9999
     env_cfg.env.episode_length_s = 9999
     env_cfg.init_state.timeout_reset_ratio = 1.
     env_cfg.domain_rand.randomize_base_mass = False
     env_cfg.domain_rand.randomize_friction = False
     env_cfg.terrain.mesh_type = "plane"
     env_cfg.osc.init_to = 'random'
-    env_cfg.osc.process_noise = 0.
-    env_cfg.osc.omega_var = 0.
-    env_cfg.osc.coupling_var = 0.
-    env_cfg.commands.ranges.lin_vel_x = [0., 0.]
-    env_cfg.commands.ranges.lin_vel_y = 0.
-    env_cfg.commands.ranges.yaw_vel = 0.
-    env_cfg.commands.var = 0.
+    # env_cfg.osc.process_noise = 0.
+    # env_cfg.osc.omega_var = 0.
+    # env_cfg.osc.coupling_var = 0.
+    # env_cfg.commands.ranges.lin_vel_x = [0., 0.]
+    # env_cfg.commands.ranges.lin_vel_y = 0.
+    # env_cfg.commands.ranges.yaw_vel = 0.
+    # env_cfg.commands.var = 0.
 
     train_cfg.policy.noise.scale = 1.0
 
@@ -52,27 +54,63 @@ def setup(args):
 
 
 def play(env, runner, train_cfg):
+    saveLogs = True
+    log = {'dof_pos_obs': [], 
+           'dof_vel': [], 
+           'torques': [],
+           'grf': [], 
+           'oscillators': [],
+           'base_lin_vel': [],
+           'base_ang_vel': [],
+           'commands': [],
+           'dof_pos_error': [],
+           'reward': [],
+            'dof_names': [],
+           }
     RECORD_FRAMES = False
+    print(env.dof_names)
+
     # * set up interface: GamepadInterface(env) or KeyboardInterface(env)
     COMMANDS_INTERFACE = hasattr(env, "commands")
-    if COMMANDS_INTERFACE:
-        # interface = GamepadInterface(env)
-        interface = KeyboardInterface(env)
-    img_idx = 0
+    # if COMMANDS_INTERFACE:
+    #     # interface = GamepadInterface(env)
+    #     interface = KeyboardInterface(env)
+    # img_idx = 0
 
     for i in range(10*int(env.max_episode_length)):
 
         if RECORD_FRAMES:
-            if i % 5:
+            if i % 5:               
                 filename = os.path.join(LEGGED_GYM_ROOT_DIR,
                                         'gym', 'scripts', 'gifs',
                                         train_cfg.runner.experiment_name,
                                         f"{img_idx}.png")
+                #print(filename)
                 env.gym.write_viewer_image_to_file(env.viewer, filename)
                 img_idx += 1
 
-        if COMMANDS_INTERFACE:
-            interface.update(env)
+        #print(env.num_envs)
+        #print(env.torques.size())
+        log['dof_pos_obs'] += (env.dof_pos_obs.tolist())
+        log['dof_vel'] += (env.dof_vel.tolist())
+        log['torques'] += (env.torques.tolist())
+        log['grf'] += env.grf.tolist()
+        log['oscillators'] += env.oscillators.tolist()
+        log['base_lin_vel'] += env.base_lin_vel.tolist()
+        log['base_ang_vel'] += env.base_ang_vel.tolist()
+        log['commands'] += env.commands.tolist()
+        log['dof_pos_error']+=(env.default_dof_pos - env.dof_pos).tolist()
+
+        reward_weights = runner.policy_cfg['reward']['weights']
+        log['reward'] += runner.get_rewards(reward_weights).tolist()
+
+        print(i)
+        if i ==1000 and saveLogs:
+            log['dof_names'] = env.dof_names
+            np.savez('new_logs', **log)
+
+        # if COMMANDS_INTERFACE:
+        #     interface.update(env)
         runner.set_actions(runner.get_inference_actions())
         env.step()