Skip to content

GitLab

Commit 67101577 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python 3] Format 

That's almost 10k lines change…
parent 5d658d91
Expand all Hide whitespace changes
Inline Side-by-side
.yapfignore 0 → 100644
View file @ 67101577
python/cmd
python/cmd/cmd_base_vel_est.py
View file @ 67101577
# plug encoder velocities (with different base vel) to balance controller
from dynamic_graph.sot.core import Stack_of_vector, Selec_of_vector
robot.v = Stack_of_vector('v');
plug(robot.base_estimator.v_flex, robot.v.sin1);
robot.v = Stack_of_vector('v')
plug(robot.base_estimator.v_flex, robot.v.sin1)
plug(robot.filters.estimator_kin.dx, robot.v.sin2)
robot.v.selec1(0,6)
robot.v.selec2(0,30)
robot.v.selec1(0, 6)
robot.v.selec2(0, 30)
plug(robot.v.sout, robot.inv_dyn.v)
# Compute finite differences of base position
from dynamic_graph.sot.torque_control.utils.filter_utils import create_chebi2_lp_filter_Wn_03_N_4
#robot.q_fd = create_chebi2_lp_filter_Wn_03_N_4('q_filter', robot.timeStep, 36)
from dynamic_graph.sot.torque_control.filter_differentiator import FilterDifferentiator
robot.q_fd = FilterDifferentiator('q_filter');
robot.q_fd.init(robot.timeStep, 36, (1., 0.), ( 1., 0.));
robot.q_fd = FilterDifferentiator('q_filter')
robot.q_fd.init(robot.timeStep, 36, (1., 0.), (1., 0.))
plug(robot.base_estimator.q, robot.q_fd.x)
create_topic(robot.ros, robot.q_fd.dx, 'q_fd')
create_topic(robot.ros, robot.q_fd.dx, 'q_fd')
# Replace force sensor filters
from dynamic_graph.sot.torque_control.utils.filter_utils import create_butter_lp_filter_Wn_05_N_3, create_chebi2_lp_filter_Wn_03_N_4
......@@ -30,16 +30,16 @@ plug(robot.force_RF_filter.dx, robot.base_estimator.dforceRLEG)
# reconnect sav-gol filters
plug(robot.filters.ft_LF_filter.x_filtered, robot.base_estimator.forceLLEG)
plug(robot.filters.ft_RF_filter.x_filtered, robot.base_estimator.forceRLEG)
plug(robot.filters.ft_LF_filter.dx, robot.base_estimator.dforceLLEG)
plug(robot.filters.ft_RF_filter.dx, robot.base_estimator.dforceRLEG)
plug(robot.filters.ft_LF_filter.dx, robot.base_estimator.dforceLLEG)
plug(robot.filters.ft_RF_filter.dx, robot.base_estimator.dforceRLEG)
# change delay of low-pass filters
dt = robot.timeStep
delay = 0.06
robot.filters.ft_RF_filter.init( dt, 6, delay, 1)
robot.filters.ft_LF_filter.init( dt, 6, delay, 1)
robot.filters.ft_RH_filter.init( dt, 6, delay, 1)
robot.filters.ft_LH_filter.init( dt, 6, delay, 1)
robot.filters.gyro_filter.init( dt, 3, delay, 1)
robot.filters.acc_filter.init( dt, 3, delay, 1)
robot.filters.estimator_kin.init(dt,30, delay, 2);
robot.filters.ft_RF_filter.init(dt, 6, delay, 1)
robot.filters.ft_LF_filter.init(dt, 6, delay, 1)
robot.filters.ft_RH_filter.init(dt, 6, delay, 1)
robot.filters.ft_LH_filter.init(dt, 6, delay, 1)
robot.filters.gyro_filter.init(dt, 3, delay, 1)
robot.filters.acc_filter.init(dt, 3, delay, 1)
robot.filters.estimator_kin.init(dt, 30, delay, 2)
python/cmd/cmd_current_ctrl_tuning.py
View file @ 67101577
create_topic(robot.ros, robot.device.currents, 'i');
create_topic(robot.ros, robot.device.control, 'ctrl');
create_topic(robot.ros, robot.ctrl_manager.currents_real, 'i_real');
create_topic(robot.ros, robot.ctrl_manager.pwmDes, 'i_des')
create_topic(robot.ros, robot.device.robotState, 'robotState')
create_topic(robot.ros, robot.ctrl_manager.pwmDesSafe, 'ctrl')
create_topic(robot.ros, robot.ctrl_manager.current_errors, 'i_err');
create_topic(robot.ros, robot.estimator_ft.jointsTorques, 'tau');
create_topic(robot.ros, robot.torque_ctrl.jointsTorquesDesired, 'tau_des');
create_topic(robot.ros, robot.device.currents, 'i')
create_topic(robot.ros, robot.device.control, 'ctrl')
create_topic(robot.ros, robot.ctrl_manager.currents_real, 'i_real')
create_topic(robot.ros, robot.ctrl_manager.pwmDes, 'i_des')
create_topic(robot.ros, robot.device.robotState, 'robotState')
create_topic(robot.ros, robot.ctrl_manager.pwmDesSafe, 'ctrl')
create_topic(robot.ros, robot.ctrl_manager.current_errors, 'i_err')
create_topic(robot.ros, robot.estimator_ft.jointsTorques, 'tau')
create_topic(robot.ros, robot.torque_ctrl.jointsTorquesDesired, 'tau_des')
# set dz comp and bemf comp to zero
robot.ctrl_manager.percentageDriverDeadZoneCompensation.value = 30*(0.,)
robot.ctrl_manager.percentage_bemf_compensation.value = 30*(0.,)
robot.ctrl_manager.ki_current.value = 30*(0.,)
robot.ctrl_manager.percentageDriverDeadZoneCompensation.value = 30 * (0., )
robot.ctrl_manager.percentage_bemf_compensation.value = 30 * (0., )
robot.ctrl_manager.ki_current.value = 30 * (0., )
# create trajectory generator for torque
from dynamic_graph.sot.torque_control.nd_trajectory_generator import NdTrajectoryGenerator
torque_traj_gen = NdTrajectoryGenerator('torque_traj_gen')
torque_traj_gen.initial_value.value = 30*(0.0,)
torque_traj_gen.initial_value.value = 30 * (0.0, )
torque_traj_gen.init(robot.timeStep, 30)
plug(torque_traj_gen.x, robot.torque_ctrl.jointsTorquesDesired)
# create trajectory generator for current
from dynamic_graph.sot.torque_control.nd_trajectory_generator import NdTrajectoryGenerator
cur_traj_gen = NdTrajectoryGenerator('cur_traj_gen')
cur_traj_gen.initial_value.value = 30*(0.0,)
cur_traj_gen.initial_value.value = 30 * (0.0, )
cur_traj_gen.init(robot.timeStep, 30)
plug(cur_traj_gen.x, robot.ctrl_manager.ctrl_torque)
......
python/cmd/cmd_test_admittance_ctrl.py
View file @ 67101577
from dynamic_graph.sot.torque_control.main import *
from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal, stop_sot
from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal, stop_sot
from dynamic_graph import plug
import numpy as np
robot.timeStep=0.0015
robot.timeStep = 0.0015
robot = main_v3(robot, startSoT=1, go_half_sitting=1)
robot.torque_ctrl.KpTorque.value = 30*(0.0,)
robot.torque_ctrl.KpTorque.value = 30 * (0.0, )
robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
robot.inv_dyn.kd_com.value = (5.0, 5.0, 0.0)
robot.inv_dyn.kp_posture.value = 30*(30.,)
robot.base_estimator.reset_foot_positions();
robot.inv_dyn.kp_posture.value = 30 * (30., )
robot.base_estimator.reset_foot_positions()
robot.inv_dyn.updateComOffset()
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap', 'torque')
robot.traj_gen.moveJoint('lk', 0.8, 3.0)
robot.com_traj_gen.move(2, 0.85, 3.0)
create_topic(robot.ros, robot.base_estimator.q, 'q');
create_topic(robot.ros, robot.adm_ctrl.fRightFootRef, 'f_des_R')
create_topic(robot.ros, robot.adm_ctrl.fLeftFootRef, 'f_des_L')
create_topic(robot.ros, robot.adm_ctrl.u, 'u_adm')
create_topic(robot.ros, robot.adm_ctrl.dqDes, 'dq_des')
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'f_L');
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_R');
create_topic(robot.ros, robot.inv_dyn.com, 'com')
create_topic(robot.ros, robot.inv_dyn.com_vel, 'com_vel')
create_topic(robot.ros, robot.inv_dyn.com_ref_pos, 'com_ref_pos')
create_topic(robot.ros, robot.inv_dyn.com_ref_vel, 'com_ref_vel')
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'cop_des')
create_topic(robot.ros, robot.inv_dyn.zmp, 'cop')
create_topic(robot.ros, robot.inv_dyn.zmp_right_foot, 'cop_R')
create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'cop_L')
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot, 'cop_des_R')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot, 'cop_des_L')
create_topic(robot.ros, robot.ctrl_manager.u, 'i_des');
create_topic(robot.ros, robot.current_ctrl.i_real, 'i_real');
create_topic(robot.ros, robot.base_estimator.q, 'q')
create_topic(robot.ros, robot.adm_ctrl.fRightFootRef, 'f_des_R')
create_topic(robot.ros, robot.adm_ctrl.fLeftFootRef, 'f_des_L')
create_topic(robot.ros, robot.adm_ctrl.u, 'u_adm')
create_topic(robot.ros, robot.adm_ctrl.dqDes, 'dq_des')
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'f_L')
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_R')
create_topic(robot.ros, robot.inv_dyn.com, 'com')
create_topic(robot.ros, robot.inv_dyn.com_vel, 'com_vel')
create_topic(robot.ros, robot.inv_dyn.com_ref_pos, 'com_ref_pos')
create_topic(robot.ros, robot.inv_dyn.com_ref_vel, 'com_ref_vel')
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'cop_des')
create_topic(robot.ros, robot.inv_dyn.zmp, 'cop')
create_topic(robot.ros, robot.inv_dyn.zmp_right_foot, 'cop_R')
create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'cop_L')
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot, 'cop_des_R')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot, 'cop_des_L')
create_topic(robot.ros, robot.ctrl_manager.u, 'i_des')
create_topic(robot.ros, robot.current_ctrl.i_real, 'i_real')
robot.adm_ctrl.force_integral_saturation.value = (0,0,160.0,20.0,20.0,0)
kp_vel = 12*[0.5,]+18*[0.0,]
robot.adm_ctrl.force_integral_saturation.value = (0, 0, 160.0, 20.0, 20.0, 0)
kp_vel = 12 * [
0.5,
] + 18 * [
0.0,
]
smoothly_set_signal(robot.adm_ctrl.kp_vel, tuple(kp_vel))
#smoothly_set_signal(robot.adm_ctrl.kp_vel, 30*(0.0,))
smoothly_set_signal(robot.adm_ctrl.ki_force, (0, 0, 20.0, 2, 0, 0))
......@@ -55,16 +58,17 @@ robot.filters.ft_LH_filter.switch_filter(b, a)
robot.filters.acc_filter.switch_filter(b, a)
robot.com_traj_gen.move(1, 0.03, 1.5)
robot.com_traj_gen.startSinusoid(1,-0.03,2.0)
robot.com_traj_gen.startSinusoid(1, -0.03, 2.0)
def print_cop_stats(robot):
cop_L = robot.inv_dyn.zmp_left_foot.value
cop_R = robot.inv_dyn.zmp_right_foot.value
cop = robot.inv_dyn.zmp.value
f_L = robot.inv_dyn.wrench_left_foot.value
f_R = robot.inv_dyn.wrench_right_foot.value
print "invdyn cop", cop[1]
print "f_L", f_L[2]
print "f_R", f_R[2]
print "Average cop: ", 0.5*(cop_L[1]+cop_R[1])
print "Weighted average cop: ", (f_L[2]*cop_L[1]+f_R[2]*cop_R[1])/(f_L[2]+f_R[2])
cop_L = robot.inv_dyn.zmp_left_foot.value
cop_R = robot.inv_dyn.zmp_right_foot.value
cop = robot.inv_dyn.zmp.value
f_L = robot.inv_dyn.wrench_left_foot.value
f_R = robot.inv_dyn.wrench_right_foot.value
print "invdyn cop", cop[1]
print "f_L", f_L[2]
print "f_R", f_R[2]
print "Average cop: ", 0.5 * (cop_L[1] + cop_R[1])
print "Weighted average cop: ", (f_L[2] * cop_L[1] + f_R[2] * cop_R[1]) / (f_L[2] + f_R[2])
python/dynamic_graph/sot/torque_control/identification/compress_identification_data.py
View file @ 67101577
......@@ -4,222 +4,244 @@ Created on Mon Feb 23 09:02:21 2015
@author: adelpret
q.shape"""
from IPython import embed
import numpy as np
import matplotlib.pyplot as plt
from dynamic_graph.sot.torque_control.utils.plot_utils import *
from compute_estimates_from_sensors import compute_estimates_from_sensors
import numpy as np
from compute_estimates_from_sensors import compute_estimates_from_sensors
DATA_SET = 1;
FOLDER_ID = 1;
EST_DELAY = 0.1; ''' delay introduced by the estimation in seconds '''
NJ = 30; ''' number of joints '''
DT = 0.001; ''' sampling period '''
PLOT_DATA = False;
FORCE_ESTIMATE_RECOMPUTATION = True;
NEGLECT_GYROSCOPE = True;
NEGLECT_ACCELEROMETER = True;
SET_NORMAL_FORCE_RIGHT_FOOT_TO_ZERO = False;
# from dynamic_graph.sot.torque_control.utils.plot_utils import *
# from IPython import embed
DATA_SET = 1
FOLDER_ID = 1
EST_DELAY = 0.1
''' delay introduced by the estimation in seconds '''
NJ = 30
''' number of joints '''
DT = 0.001
''' sampling period '''
PLOT_DATA = False
FORCE_ESTIMATE_RECOMPUTATION = True
NEGLECT_GYROSCOPE = True
NEGLECT_ACCELEROMETER = True
SET_NORMAL_FORCE_RIGHT_FOOT_TO_ZERO = False
USE_FT_SENSORS = True
JOINT_ID = np.array(range(12)); ''' IDs of the joints to save '''
if(DATA_SET==1):
if(FOLDER_ID==1):
data_folder = '../../../../../../../results/20160923_165916_Joint2_id_Kt/';
JOINT_ID = np.array([2]);
elif(FOLDER_ID==2):
data_folder = '../../../../../../../results/20160923_170659_Joint2_id_Kv/';
JOINT_ID = np.array([2]);
elif(FOLDER_ID==3):
data_folder = '../../../../../../../results/20160923_173001_Joint2_id_Ka/';
JOINT_ID = np.array([2]);
if(DATA_SET==2):
if(FOLDER_ID==1):
#~ data_folder = '../../results/20160720_132041_rsp_torque_id/';
#~ data_folder = '../../results/20160720_132429_rsp_torque_id/';
data_folder = '../../../../../../../results/results/20160720_143905_rsp_torque_id_noGravity/';
JOINT_ID = np.array([16]);
elif(FOLDER_ID==2):
data_folder = '../../../../../../../results/results/20160720_134957_rsp_friction_id_ext/';
#~ data_folder = '../../../../../../../results/results/20160722_144631_rsp_const_vel/';
JOINT_ID = np.array([16]);
elif(FOLDER_ID==3):
data_folder = '../../rien/';
JOINT_ID = np.array([16]);
FILE_READ_SUCCEEDED = False;
DATA_FILE_NAME = 'data';
TEXT_DATA_FILE_NAME = 'data.txt';
N_DELAY = int(EST_DELAY/DT);
file_name_ctrl = 'dg_HRP2LAAS-control.dat';
file_name_enc = 'dg_HRP2LAAS-robotState.dat';
file_name_acc = 'dg_HRP2LAAS-accelerometer.dat';
file_name_gyro = 'dg_HRP2LAAS-gyrometer.dat';
file_name_forceLA = 'dg_HRP2LAAS-forceLARM.dat';
file_name_forceRA = 'dg_HRP2LAAS-forceRARM.dat';
file_name_forceLL = 'dg_HRP2LAAS-forceLLEG.dat';
file_name_forceRL = 'dg_HRP2LAAS-forceRLEG.dat';
file_name_current = 'dg_HRP2LAAS-currents.dat';
JOINT_ID = np.array(range(12))
''' IDs of the joints to save '''
if (DATA_SET == 1):
if (FOLDER_ID == 1):
data_folder = '../../../../../../../results/20160923_165916_Joint2_id_Kt/'
JOINT_ID = np.array([2])
elif (FOLDER_ID == 2):
data_folder = '../../../../../../../results/20160923_170659_Joint2_id_Kv/'
JOINT_ID = np.array([2])
elif (FOLDER_ID == 3):
data_folder = '../../../../../../../results/20160923_173001_Joint2_id_Ka/'
JOINT_ID = np.array([2])
if (DATA_SET == 2):
if (FOLDER_ID == 1):
# ~ data_folder = '../../results/20160720_132041_rsp_torque_id/';
# ~ data_folder = '../../results/20160720_132429_rsp_torque_id/';
data_folder = '../../../../../../../results/results/20160720_143905_rsp_torque_id_noGravity/'
JOINT_ID = np.array([16])
elif (FOLDER_ID == 2):
data_folder = '../../../../../../../results/results/20160720_134957_rsp_friction_id_ext/'
# ~ data_folder = '../../../../../../../results/results/20160722_144631_rsp_const_vel/';
JOINT_ID = np.array([16])
elif (FOLDER_ID == 3):
data_folder = '../../rien/'
JOINT_ID = np.array([16])
FILE_READ_SUCCEEDED = False
DATA_FILE_NAME = 'data'
TEXT_DATA_FILE_NAME = 'data.txt'
N_DELAY = int(EST_DELAY / DT)
file_name_ctrl = 'dg_HRP2LAAS-control.dat'
file_name_enc = 'dg_HRP2LAAS-robotState.dat'
file_name_acc = 'dg_HRP2LAAS-accelerometer.dat'
file_name_gyro = 'dg_HRP2LAAS-gyrometer.dat'
file_name_forceLA = 'dg_HRP2LAAS-forceLARM.dat'
file_name_forceRA = 'dg_HRP2LAAS-forceRARM.dat'
file_name_forceLL = 'dg_HRP2LAAS-forceLLEG.dat'
file_name_forceRL = 'dg_HRP2LAAS-forceRLEG.dat'
file_name_current = 'dg_HRP2LAAS-currents.dat'
''' Load data from file '''
try:
data = np.load(data_folder+DATA_FILE_NAME+'.npz');
time = data['time'];
enc = data['enc'];
acc = data['acc'];
gyro = data['gyro'];
forceLA = data['forceLA'];
forceRA = data['forceRA'];
forceLL = data['forceLL'];
forceRL = data['forceRL'];
N = acc.shape[0];
ctrl = np.empty((N,len(JOINT_ID)));
dq = np.empty((N,len(JOINT_ID)));
ddq = np.empty((N,len(JOINT_ID)));
tau = np.empty((N,len(JOINT_ID)));
# ptorques = np.empty((N,len(JOINT_ID)));
# p_gains = np.empty((N,len(JOINT_ID)));
current = np.empty((N,len(JOINT_ID)));
FILE_READ_SUCCEEDED = True;
data = np.load(data_folder + DATA_FILE_NAME + '.npz')
time = data['time']
enc = data['enc']
acc = data['acc']
gyro = data['gyro']
forceLA = data['forceLA']
forceRA = data['forceRA']
forceLL = data['forceLL']
forceRL = data['forceRL']
N = acc.shape[0]
ctrl = np.empty((N, len(JOINT_ID)))
dq = np.empty((N, len(JOINT_ID)))
ddq = np.empty((N, len(JOINT_ID)))
tau = np.empty((N, len(JOINT_ID)))
# ptorques = np.empty((N,len(JOINT_ID)));
# p_gains = np.empty((N,len(JOINT_ID)));
current = np.empty((N, len(JOINT_ID)))
FILE_READ_SUCCEEDED = True
for i in range(len(JOINT_ID)):
data = np.load(data_folder+DATA_FILE_NAME+'_j'+str(JOINT_ID[i])+'.npz');
ctrl[:,i] = data['ctrl'];
# ptorques[:,i] = data['ptorque'];
# p_gains[:,i] = data['p_gain'];
current[:,i] = data['current'];
if(FORCE_ESTIMATE_RECOMPUTATION==False):
dq [:,i] = data['dq'];
ddq[:,i] = data['ddq'];
tau[:,i] = data['tau'];
data = np.load(data_folder + DATA_FILE_NAME + '_j' + str(JOINT_ID[i]) + '.npz')
ctrl[:, i] = data['ctrl']
# ptorques[:,i] = data['ptorque'];
# p_gains[:,i] = data['p_gain'];
current[:, i] = data['current']
if not FORCE_ESTIMATE_RECOMPUTATION:
dq[:, i] = data['dq']
ddq[:, i] = data['ddq']
tau[:, i] = data['tau']
except (IOError, KeyError):
print 'Gonna read text files...'
ctrl = np.loadtxt(data_folder+file_name_ctrl);
enc = np.loadtxt(data_folder+file_name_enc);
acc = np.loadtxt(data_folder+file_name_acc);
gyro = np.loadtxt(data_folder+file_name_gyro);
forceLA = np.loadtxt(data_folder+file_name_forceLA);
forceRA = np.loadtxt(data_folder+file_name_forceRA);
forceLL = np.loadtxt(data_folder+file_name_forceLL);
forceRL = np.loadtxt(data_folder+file_name_forceRL);
# ptorques = np.loadtxt(data_folder+file_name_ptorque);
current = np.loadtxt(data_folder+file_name_current);
# p_gains = np.loadtxt(data_folder+file_name_p_gain);
print('Gonna read text files...')
ctrl = np.loadtxt(data_folder + file_name_ctrl)
enc = np.loadtxt(data_folder + file_name_enc)
acc = np.loadtxt(data_folder + file_name_acc)
gyro = np.loadtxt(data_folder + file_name_gyro)
forceLA = np.loadtxt(data_folder + file_name_forceLA)
forceRA = np.loadtxt(data_folder + file_name_forceRA)
forceLL = np.loadtxt(data_folder + file_name_forceLL)
forceRL = np.loadtxt(data_folder + file_name_forceRL)
# ptorques = np.loadtxt(data_folder+file_name_ptorque);
current = np.loadtxt(data_folder + file_name_current)
# p_gains = np.loadtxt(data_folder+file_name_p_gain);
# check that largest signal has same length of smallest signal
n_enc = len(enc[:,0]);
n_acc = len(acc[:,0]);
if(n_acc!=n_enc):
print "Reducing size of signals from %d to %d" % (n_acc, n_enc);
N = np.min([n_enc,n_acc]);
time = enc[:N,0];
ctrl = ctrl[:N,1:];
ctrl = ctrl[:,JOINT_ID].reshape(N,len(JOINT_ID));
current = current[:N,1:];
current = current[:,JOINT_ID].reshape(N,len(JOINT_ID));
enc = enc[:N,7:];
acc = acc[:N,1:];
gyro = gyro[:N,1:];
forceLA = forceLA[:N,1:];
forceRA = forceRA[:N,1:];
forceLL = forceLL[:N,1:];
forceRL = forceRL[:N,1:];
# ptorques = ptorques[:N,1:];
# p_gains = p_gains[:N,1:];
n_enc = len(enc[:, 0])
n_acc = len(acc[:, 0])
if (n_acc != n_enc):
print("Reducing size of signals from %d to %d" % (n_acc, n_enc))
N = np.min([n_enc, n_acc])
time = enc[:N, 0]
ctrl = ctrl[:N, 1:]
ctrl = ctrl[:, JOINT_ID].reshape(N, len(JOINT_ID))
current = current[:N, 1:]
current = current[:, JOINT_ID].reshape(N, len(JOINT_ID))
enc = enc[:N, 7:]
acc = acc[:N, 1:]
gyro = gyro[:N, 1:]
forceLA = forceLA[:N, 1:]
forceRA = forceRA[:N, 1:]
forceLL = forceLL[:N, 1:]
forceRL = forceRL[:N, 1:]
# ptorques = ptorques[:N,1:];
# p_gains = p_gains[:N,1:];
# save sensor data
np.savez(data_folder+DATA_FILE_NAME+'.npz',
time=time,
enc=enc.reshape(N,NJ),
acc=acc.reshape(N,3),
gyro=gyro.reshape(N,3),
forceLA=forceLA.reshape(N,6),
forceRA=forceRA.reshape(N,6),
forceLL=forceLL.reshape(N,6),
forceRL=forceRL.reshape(N,6));
N = len(enc[:,0]);
if(FORCE_ESTIMATE_RECOMPUTATION or FILE_READ_SUCCEEDED==False):
print 'Gonna estimate dq, ddq, tau';
dt='f4';
a = np.zeros(N, dtype=[ ('enc',dt,NJ)
,('forceLA',dt,6)
,('forceRA',dt,6)
,('forceLL',dt,6)
,('forceRL',dt,6)
,('acc',dt,3)
,('gyro',dt,3)
,('time',dt,1)]);
a['enc'] = enc;
a['forceLA'] = forceLA;
a['forceRA'] = forceRA;
a['forceLL'] = forceLL;
a['forceRL'] = forceRL;
if(SET_NORMAL_FORCE_RIGHT_FOOT_TO_ZERO):
a['forceRL'][:,2] = 0.0;
if(NEGLECT_ACCELEROMETER):
a['acc'] = np.array([0, 0, 9.81]); #np.mean(acc,0);
np.savez(data_folder + DATA_FILE_NAME + '.npz',
time=time,
enc=enc.reshape(N, NJ),
acc=acc.reshape(N, 3),
gyro=gyro.reshape(N, 3),
forceLA=forceLA.reshape(N, 6),
forceRA=forceRA.reshape(N, 6),
forceLL=forceLL.reshape(N, 6),
forceRL=forceRL.reshape(N, 6))
N = len(enc[:, 0])
if FORCE_ESTIMATE_RECOMPUTATION or not FILE_READ_SUCCEEDED:
print('Gonna estimate dq, ddq, tau')
dt = 'f4'
a = np.zeros(N,
dtype=[('enc', dt, NJ), ('forceLA', dt, 6), ('forceRA', dt, 6), ('forceLL', dt, 6),
('forceRL', dt, 6), ('acc', dt, 3), ('gyro', dt, 3), ('time', dt, 1)])
a['enc'] = enc
a['forceLA'] = forceLA
a['forceRA'] = forceRA
a['forceLL'] = forceLL
a['forceRL'] = forceRL
if (SET_NORMAL_FORCE_RIGHT_FOOT_TO_ZERO):
a['forceRL'][:, 2] = 0.0
if (NEGLECT_ACCELEROMETER):
a['acc'] = np.array([0, 0, 9.81])
# np.mean(acc,0);
else:
a['acc'] = acc;
if(NEGLECT_GYROSCOPE==False):
a['gyro'] = gyro;