
import sys
import numpy as np, math
import copy
from threading import RLock

import itertools
import matplotlib.pyplot as pp

import roslib; roslib.load_manifest('sandbox_tapo_darpa_m3')

import rospy
import tf

import hrl_lib.util as ut
import hrl_lib.transforms as tr
import hrl_lib.matplotlib_util as mpu

import nlp_opt_traj as nlp


if __name__ == '__main__':
    
    jep = []
    time = []
    data_dict = ut.load_pickle('JEPs.pkl')

    # Trajectory JEPs
    for key in data_dict.keys():
        time.append(key)
        
    time = sorted(time)

    for key in time:
        jep.append(data_dict[key])
    
    rows, columns = np.shape(jep)
    jep_restruct = []
    for column in range(columns):
        for row in range(rows):
            jep_restruct.append(jep[row][column])
    
    jep_restruct = np.reshape(np.array(jep_restruct), (columns,rows))
    for row in range(columns):
        pp.plot(time,jep_restruct[row],'-s', linewidth=3)
    pp.title('Joint Trajectories', fontsize='14')
    pp.legend(('J1', 'J2', 'J3', 'J4', 'J5', 'J6', 'J7'), loc='4')
    pp.ylabel('Joint Angles (degrees)', fontsize='14')
    pp.xlabel('Time (s)', fontsize='14')
    pp.grid('on')    
    pp.show()






