# IN_FILE_NAME = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_31.npz'
# IN_FILE_NAME = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_54.npz'
# IN_FILE_NAME = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_56.npz'
# IN_FILE_NAME = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_57.npz'
# IN_FILE_NAME = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_59.npz'
IN_FILE_NAME = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_16_03.npz'
OUT_FILE_NAME = IN_FILE_NAME.split('.')[0] + '_format.npz'
THRESH_VIZ = 8
SAVE = True
# dt = 1e-3 # discretization timespan
dt = 2e-3 # discretization timespan
arr_dic = read_data_file_laas(IN_FILE_NAME, dt)
N = len(arr_dic['t'])
# arr_dic = shortened_arr_dic(arr_dic, 0, N=N-200)
# arr_dic = shortened_arr_dic(arr_dic, 0, 2000)
# arr_dic = shortened_arr_dic(arr_dic, 0, 2000)
# arr_dic = shortened_arr_dic(arr_dic, 58*50, 25*500)
t_arr = arr_dic['t']
N = len(t_arr)
# mean the forces
# arr_dic['o_a_oi'][:,:] = np.mean(arr_dic['o_a_oi'], axis=0)
# arr_dic['imu_acc'][:,:] = np.mean(arr_dic['imu_acc'], axis=0)
# arr_dic['i_omg_oi'][:,:] = np.mean(arr_dic['i_omg_oi'], axis=0)
# arr_dic['qa'][:,:] = np.mean(arr_dic['qa'], axis=0)
# filter the kinematics with centered window to compute 1rst and 2nd derivatives
# 19:05 Replay stamping
# 19:06 Marche 0.32 (30s)
data_file_lst = [
'Logs_09_10_20_soir/data_2020_10_09_18_58.npz',
'Logs_09_10_20_soir/data_2020_10_09_19_00.npz',
'Logs_09_10_20_soir/data_2020_10_09_19_02.npz',
'Logs_09_10_20_soir/data_2020_10_09_19_03.npz',
'Logs_09_10_20_soir/data_2020_10_09_19_05.npz',
'Logs_09_10_20_soir/data_2020_10_09_19_06.npz',
]
# data_file_lst = [
# 'Logs_09_10_20_soir/data_2020_10_09_19_02.npz',
# ]
for data_file in data_file_lst:
arr_dic = read_data_file_laas(DATA_FOLDER + data_file, dt)
N = len(arr_dic['t'])
err_max_m_R_i = np.zeros(3)
err_max_w_R_o = np.zeros(3)
o_rpy_i_arr = np.zeros((N, 3))
w_rpy_m_arr = np.zeros((N, 3))
for i in range(N):
o_R_i = pin.Quaternion(arr_dic['o_q_i'][i, :].reshape(
(4, 1))).toRotationMatrix()
w_R_m = pin.Quaternion(arr_dic['w_q_m'][i, :].reshape(
(4, 1))).toRotationMatrix()
# o_R_i = arr_dic['o_R_i'][i,:,:]
# w_R_m = arr_dic['w_R_m'][i,:,:]
o_rpy_i = pin.rpy.matrixToRpy(o_R_i)
w_rpy_m = pin.rpy.matrixToRpy(w_R_m)
import numpy as np
import matplotlib.pyplot as plt
from example_robot_data import load
from data_readers import read_data_file_laas
dt = 1e-3
# file_path = '/home/mfourmy/Documents/Phd_LAAS/solo-estimation/data/Experiments_Replay_30_11_2020_bis/data_2020_11_30_17_18.npz' # sin
# file_path = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_Replay_03_12_2020_palette/data_2020_12_03_17_36.npz' # sin smaller
# file_path = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_Replay_03_12_2020_palette/data_2020_12_03_17_38.npz' # sin traj
# file_path = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_Replay_03_12_2020_palette/data_2020_12_03_17_41.npz' # manual short
# file_path = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_Replay_03_12_2020_palette/data_2020_12_03_17_44.npz' # manual longer
# file_path = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Calibration_Manuelle_09_12_2020/data_2020_12_09_14_46.npz' # manual NO CONTROL
file_path = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Calibration_Manuelle_09_12_2020/data_2020_12_09_14_48.npz' # manual NO CONTROL
arr_dic = read_data_file_laas(file_path, dt)
VIEW = False
robot = load('solo12')
LEGS = ['FL', 'FR', 'HR', 'HL']
# contact_frame_names = [leg+'_ANKLE' for leg in LEGS]
contact_frame_names = [leg + '_FOOT' for leg in LEGS]
contact_ids = [
robot.model.getFrameId(leg_name) for leg_name in contact_frame_names
]
t_arr = arr_dic['t']
N = len(t_arr)
S = 500
E = N - 30
t_arr = t_arr[S:E]
# data_file = 'data.npz'
# data_file = 'data_2020_10_08_09_50_Walking_Novicon.npz'
# data_file = 'data_2020_10_08_10_04_StandingStill.npz'
# data_file = 'data_2020_10_09_16_10_Stamping.npz'
# data_file = 'data_2020_10_09_16_12_SinTraj.npz'
# last data with turned imu
# data_file = 'Logs_09_10_20_soir/data_2020_10_09_18_58.npz'
data_file = "Logs_15_10_2020/data_2020_10_15_14_34.npz" # standing
# data_file = "Logs_15_10_2020/data_2020_10_15_14_36.npz" # sinXYZ
# data_file = "data_2020_10_31_20_12.npz"
# data_file = "Logs_15_10_2020/data_2020_10_15_14_38.npz"
arr_dic = read_data_file_laas(BASE_FOLDER + data_file, dt)
# arr_dic = shortened_arr_dic(arr_dic, 2000, len(arr_dic['t'])-200)
# arr_dic = shortened_arr_dic(arr_dic, 5000, 7150)
t_arr = arr_dic['t']
i_a_oi_arr = arr_dic['i_a_oi']
imu_acc_arr = arr_dic['imu_acc']
o_R_i_arr = arr_dic['o_R_i']
g = 9.806 * np.array([0, 0, -1])
# try to compute imu biases
bacc_arr = np.array([
imu_acc + o_R_i.T @ g - i_a_oi
for i_a_oi, imu_acc, o_R_i in zip(i_a_oi_arr, imu_acc_arr, o_R_i_arr)
])
# file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_17.npz' # // # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_25.npz' # Standing still (5s), mocap 500Hz # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_26.npz' # // # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_29.npz' # Moving up (5s), mocap 500Hz # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_30.npz' # // # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_31.npz' # Moving up->front->down (10s), mocap 500Hz # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_32.npz' # // # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_54.npz' # Moving up then random movements (15s), mocap 500Hz # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_56.npz' # // # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_57.npz' # // # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_15_59.npz' # Already in air, random movements (15s), mocap 500Hz # file_name = '/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/Experiments_MocapIMU_2021_04_23/data_2021_04_23_16_03.npz' # // dt = 2e-3 dt_mocap = 2e-3 arr_dic = read_data_file_laas(file_name, dt) # if default format t_arr = arr_dic['t'] i_omg_oi_arr = arr_dic['i_omg_oi'] # angvel IMU w_p_wm_arr = arr_dic['w_p_wm'] # position mocap w_R_m_arr = arr_dic['w_R_m'] # orientation mocap w_q_m_arr = arr_dic['w_q_m'] # orientation mocap o_a_oi_arr = arr_dic['o_a_oi'] # IMU absolute acceleration N = 100 w_R_m_prev_arr = np.roll(w_R_m_arr, N, axis=0) w_R_m_post_arr = np.roll(w_R_m_arr, -N, axis=0) # m_omg_om_arr = np.array([ pin.log(w_R_m1.T@w_R_m2)/(2*N*dt_mocap) for w_R_m1, w_R_m2 in zip(w_R_m_prev_arr, w_R_m_arr)]) m_omg_om_arr = np.array([ pin.log(w_R_m1.T@w_R_m2)/(2*N*dt_mocap) for w_R_m1, w_R_m2 in zip(w_R_m_prev_arr, w_R_m_post_arr)])