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)
