def create_butter_lp_filter_Wn_05_N_3(name, dt, size): lp_filter = FilterDifferentiator(name) # (b,a) = butter(N=3, Wn=0.05) # delay about 13*dt lp_filter.init(dt, size, (0.00041655, 0.00124964, 0.00124964, 0.00041655), (1., -2.6861574, 2.41965511, -0.73016535)) return lp_filter
def create_chebi2_lp_filter_Wn_03_N_4(name, dt, size): lp_filter = FilterDifferentiator(name); # (b,a) = cheby2(4, 20, 0.03) # delay about 23*dt lp_filter.init(dt, size, (0.09147425, -0.35947268, 0.53605377, -0.35947268, 0.09147425), ( 1. , -3.7862251 , 5.38178322, -3.40348456, 0.80798333)); return lp_filter;
def create_butter_lp_filter_Wn_03_N_4(name, dt, size): lp_filter = FilterDifferentiator(name); # (b,a) = butter(N=4, Wn=0.03) # delay about 28*dt lp_filter.init(dt, size, (4.37268880e-06, 1.74907552e-05, 2.62361328e-05, 1.74907552e-05, 4.37268880e-06), (1., -3.75376276, 5.29115258, -3.3189386 , 0.78161874)); return lp_filter;
def create_butter_lp_filter_Wn_05_N_2(name, dt, size): lp_filter = FilterDifferentiator(name); # from scipy.signal import butter # (b,a) = butter(N=2, Wn=0.05) # delay about 9*dt lp_filter.init(dt, size, (0.00554272, 0.01108543, 0.00554272), (1., -1.77863178, 0.80080265)); return lp_filter;
def create_butter_lp_filter_Wn_05_N_4(name, dt, size): lp_filter = FilterDifferentiator(name); # (b,a) = butter(N=4, Wn=0.05) # delay about 16*dt lp_filter.init(dt, size, (3.12389769e-05, 1.24955908e-04, 1.87433862e-04, 1.24955908e-04, 3.12389769e-05), (1., -3.58973389, 4.85127588, -2.92405266, 0.66301048)); return lp_filter;
def create_butter_lp_filter_Wn_03_N_3(name, dt, size): lp_filter = FilterDifferentiator(name) # (b,a) = butter(N=3, Wn=0.03) # delay about 20*dt lp_filter.init( dt, size, (9.54425084e-05, 2.86327525e-04, 2.86327525e-04, 9.54425084e-05), (1., -2.81157368, 2.64048349, -0.82814628)) return lp_filter
def create_chebi1_checby2_series_filter(name, dt, size): #b1,a1=cheby2(2, 20,0.05); #b2,a2 = cheby1(4,0.05,0.08); #(b,a) = filter_series(b1,a1,b2,a2); lp_filter = FilterDifferentiator(name); lp_filter.init(dt, size, (2.16439898e-05, 4.43473520e-05, -1.74065002e-05, -8.02197247e-05, -1.74065002e-05, 4.43473520e-05, 2.16439898e-05), (1.,-5.32595322,11.89749109,-14.26803139, 9.68705647, -3.52968633, 0.53914042)) return lp_filter;
def setup_velocity_filter(robot, conf, filter_b, filter_a): main_v3(robot, startSoT=False, go_half_sitting=False, conf=None) robot.estimator_fd = FilterDifferentiator("fd_filter") dt = robot.timeStep robot.estimator_fd.init(dt, NJ, filter_b, filter_a) robot.inv_dyn_ctrl = create_inverse_dynamics(robot, conf.inv_dyn_gains, conf.motor_params, dt=dt) replug_inv_dyn(robot, robot.inv_dyn_ctrl) replug_estimator_kin(robot, robot.estimator_fd)
from dynamic_graph.sot.core.operator import Selec_of_vector, Stack_of_vector #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 # Replace force sensor filters # Compute finite differences of base position 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) 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) plug(robot.v.sout, robot.inv_dyn.v) 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') robot.force_LF_filter = create_chebi2_lp_filter_Wn_03_N_4( 'force_LF_filter', robot.timeStep, 6) robot.force_RF_filter = create_chebi2_lp_filter_Wn_03_N_4( 'force_RF_filter', robot.timeStep, 6) plug(robot.device.forceLLEG, robot.force_LF_filter.x) plug(robot.force_LF_filter.x_filtered, robot.base_estimator.forceLLEG) plug(robot.force_LF_filter.dx, robot.base_estimator.dforceLLEG) plug(robot.device.forceRLEG, robot.force_RF_filter.x) plug(robot.force_RF_filter.x_filtered, robot.base_estimator.forceRLEG) plug(robot.force_RF_filter.dx, robot.base_estimator.dforceRLEG)