# INTTYPE = 'pinocchio'
PATH = 'figs_'+INTTYPE+'/'
cs = ContactSequence()
print('Loadding cs file: ', examples_dir + file_name)
cs.loadFromBinary(examples_dir + file_name)
q_traj = cs.concatenateQtrajectories()
dq_traj = cs.concatenateDQtrajectories()
ddq_traj = cs.concatenateDDQtrajectories()
c_traj = cs.concatenateCtrajectories()
dc_traj = cs.concatenateDCtrajectories()
Lc_traj = cs.concatenateLtrajectories()
contact_frames = cs.getAllEffectorsInContact()
f_traj_lst = [cs.concatenateContactForceTrajectories(l) for l in contact_frames]
print(contact_frames)
min_ts = q_traj.min()
max_ts = q_traj.max()
print('traj dur (s): ', max_ts - min_ts)
dt = 1e-3 # discretization timespan
# dt = 1e-2 # discretization timespan
t_arr = np.arange(min_ts, max_ts, dt)
q_arr = np.array([q_traj(t) for t in t_arr])
dq_arr = np.array([dq_traj(t) for t in t_arr])
ddq_arr = np.array([ddq_traj(t) for t in t_arr])
c_arr = np.array([c_traj(t) for t in t_arr])
dc_arr = np.array([dc_traj(t) for t in t_arr])
cs_file_path = cs_file_dir + cs_file_name
cs = ContactSequence()
cs.loadFromBinary(cs_file_path)
##############################################
# extract raw trajectory from contact sequence
##############################################
q_traj = cs.concatenateQtrajectories()
dq_traj = cs.concatenateDQtrajectories()
ddq_traj = cs.concatenateDDQtrajectories()
tau_traj = cs.concatenateTauTrajectories()
leg_name_lst = cs.getAllEffectorsInContact()
print('EE in contact')
print(leg_name_lst)
leg_cforces_traj_lst = [cs.concatenateContactForceTrajectories(leg_name) for leg_name in leg_name_lst]
min_ts = q_traj.min()
max_ts = q_traj.max()
dt = 1e-3 # discretization timespan
t_arr = np.arange(min_ts, max_ts, dt)
N = t_arr.shape[0] # nb of discretized timestamps
# #####
# # for testing
# N = 1000
# t_arr = t_arr[:N]
# ####
q_arr = np.array([q_traj(t) for t in t_arr])
dq_arr = np.array([dq_traj(t) for t in t_arr])
ddq_arr = np.array([ddq_traj(t) for t in t_arr])
