Skip to content
GitLab
Commit 82c81563 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[python] Replace Savistsky-Golay filters with Chebishev low-pass filters for... 

[python] Replace Savistsky-Golay filters with Chebishev low-pass filters for all sensors (except current)
parent 3a12ab4c
Hide whitespace changes
Inline Side-by-side
python/cmd/cmd_hrp2_balance_ctrl.py
View file @ 82c81563
......@@ -7,20 +7,23 @@ 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 import plug
robot.timeStep=0.0015
robot = main_v3(robot, startSoT=True, go_half_sitting=False)
go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
robot = main_v3(robot, startSoT=True, go_half_sitting=True)
#go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
#robot.base_estimator.set_imu_weight(0.0)
plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities);
#plug(robot.filters.estimator_kin.dx, robot.current_ctrl.dq);
#plug(robot.filters.estimator_kin.dx, robot.torque_ctrl.jointsVelocities);
robot.inv_dyn.kp_com.value = (30.0, 30.0, 50.0)
#robot.inv_dyn.kd_com.value = (8.0, 8.0, 0.0)
robot.inv_dyn.kd_com.value = (8.0, 8.0, 0.0)
robot.torque_ctrl.KpTorque.value = 30*(0.0,)
#plug(robot.filters_sg.ft_LF_filter.x_filtered, robot.base_estimator.forceLLEG)
#plug(robot.filters_sg.ft_RF_filter.x_filtered, robot.base_estimator.forceRLEG)
# create ros topics
create_topic(robot.ros, robot.device.robotState, 'robotState')
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.robotState, 'robotState')
#create_topic(robot.ros, robot.estimator_ft.jointsTorques, 'tau');
#create_topic(robot.ros, robot.torque_ctrl.jointsTorquesDesired, 'tau_des');
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')
......@@ -28,16 +31,20 @@ create_topic(robot.ros, robot.inv_dyn.com_ref_vel, 'com_ref_vel')
create_topic(robot.ros, robot.inv_dyn.com_acc_des, 'com_acc_des')
create_topic(robot.ros, robot.base_estimator.lf_xyzquat, 'lf_est')
create_topic(robot.ros, robot.base_estimator.rf_xyzquat, 'rf_est')
create_topic(robot.ros, robot.current_ctrl.i_real, 'i_real');
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.ctrl_manager.u, 'i_des')
create_topic(robot.ros, robot.base_estimator.q, 'q');
create_topic(robot.ros, robot.base_estimator.v, 'v');
create_topic(robot.ros, robot.base_estimator.v_flex, 'v_flex');
create_topic(robot.ros, robot.base_estimator.v_kin, 'v_kin');
create_topic(robot.ros, robot.base_estimator.v_gyr, 'v_gyr');
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.filters.ft_LF_filter.x_filtered, 'f_LF_filt')
create_topic(robot.ros, robot.filters_sg.ft_LF_filter.x_filtered, 'f_LF_filt_sg', robot, robot.filters_sg.ft_LF_filter)
# wait until the motion has finished
go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
# put the robot down
robot.imu_offset_compensation.update_offset(5.0)
# wait 5 seconds
......@@ -49,6 +56,7 @@ robot.imu_filter.setBeta(base_est_conf.beta)
go_to_position(robot.traj_gen, robot.halfSitting[6:], 5.0);
# put the robot down
robot.base_estimator.reset_foot_positions();
robot.inv_dyn.updateComOffset()
# start torque control on leg joints
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lk-lar-lap','torque')
......@@ -72,22 +80,22 @@ plug(torque_der.dx, robot.torque_ctrl.jointsTorquesDerivative)
go_to_position(robot.traj_gen, robot.device.robotState.value[6:], 3.0)
robot.ctrl_manager.setCtrlMode('all', 'pos')
create_topic(robot.ros, robot.inv_dyn.f_des_right_foot, 'inv_dyn_f_des_R')
create_topic(robot.ros, robot.inv_dyn.f_des_left_foot, 'inv_dyn_f_des_L')
create_topic(robot.ros, robot.inv_dyn.tau_des, 'inv_dyn_tau_des');
create_topic(robot.ros, robot.inv_dyn.f_des_right_foot, 'f_des_R')
create_topic(robot.ros, robot.inv_dyn.f_des_left_foot, 'f_des_L')
create_topic(robot.ros, robot.inv_dyn.tau_des, 'tau_des');
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'f_LeftSole');
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_RightSole');
create_topic(robot.ros, robot.ctrl_manager.pwmDes, 'i_des');
create_topic(robot.ros, robot.ctrl_manager.ctrl_torque, 'i_des');
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot_local, 'inv_dyn_cop_R')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot_local, 'inv_dyn_cop_L')
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot, 'inv_dyn_cop_R_world')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot, 'inv_dyn_cop_L_world')
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'inv_dyn_cop')
create_topic(robot.ros, robot.inv_dyn.zmp_right_foot, 'inv_dyn_cop_R_mes')
create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'inv_dyn_cop_L_mes')
create_topic(robot.ros, robot.inv_dyn.zmp, 'inv_dyn_cop_mes')
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot_local, 'cop_des_R_local')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot_local, 'cop_des_L_local')
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.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, 'cop_des')
create_topic(robot.ros, robot.inv_dyn.zmp, 'cop')
create_topic(robot.ros, robot.estimator_kin.dx, 'dq_sav');
create_topic(robot.ros, robot.encoder_filter.dx, 'dq');
......@@ -103,7 +111,7 @@ smoothly_set_signal(robot.torque_ctrl.KpTorque,30*(1.,))
smoothly_set_signal(robot.inv_dyn.kp_posture,30*(5.,))
robot.com_traj_gen.stop(-1)
robot.com_traj_gen.move(1, 0.03, 1.5)
robot.com_traj_gen.startSinusoid(1,-0.03,1.5)
robot.com_traj_gen.move(1, 0.05, 1.5)
robot.com_traj_gen.startSinusoid(1,-0.05,1.5)
python/dynamic_graph/sot/torque_control/create_entities_utils.py
View file @ 82c81563
......@@ -171,22 +171,50 @@ def create_trajectory_generator(device, dt=0.001, robot_name="robot"):
return jtg;
def create_estimators(robot, conf, motor_params, dt):
filters = Bunch()
from dynamic_graph.sot.torque_control.utils.filter_utils import create_chebi1_checby2_series_filter
robot.filters_sg = Bunch()
filters = Bunch();
estimator_ft = ForceTorqueEstimator("estimator_ft");
# create low-pass filter for motor currents
filters.current_filter = create_butter_lp_filter_Wn_05_N_3('current_filter', dt, NJ)
#filters.current_filter = NumericalDifference("current_filter");
filters.ft_RF_filter = NumericalDifference("ft_RF_filter");
filters.ft_LF_filter = NumericalDifference("ft_LF_filter");
filters.ft_RH_filter = NumericalDifference("ft_RH_filter");
filters.ft_LH_filter = NumericalDifference("ft_LH_filter");
filters.acc_filter = NumericalDifference("dv_filter");
filters.gyro_filter = NumericalDifference("w_filter");
filters.estimator_kin = NumericalDifference("estimator_kin");
use_sav_gol = True;
if(use_sav_gol):
#filters.current_filter = NumericalDifference("current_filter");
robot.filters_sg.ft_RF_filter = NumericalDifference("ft_RF_sg_filter");
robot.filters_sg.ft_LF_filter = NumericalDifference("ft_LF_sg_filter");
robot.filters_sg.ft_RH_filter = NumericalDifference("ft_RH_sg_filter");
robot.filters_sg.ft_LH_filter = NumericalDifference("ft_LH_sg_filter");
robot.filters_sg.acc_filter = NumericalDifference("dv_sg_filter");
robot.filters_sg.gyro_filter = NumericalDifference("w_sg_filter");
robot.filters_sg.estimator_kin = NumericalDifference("estimator_kin_sg");
#robot.filters_sg.current_filter.init(dt,NJ, conf.DELAY_CURRENT*dt,1)
robot.filters_sg.ft_RF_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
robot.filters_sg.ft_LF_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
robot.filters_sg.ft_RH_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
robot.filters_sg.ft_LH_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
robot.filters_sg.gyro_filter.init(dt, 3, conf.DELAY_GYRO*dt,1)
robot.filters_sg.acc_filter.init(dt, 3, conf.DELAY_ACC*dt,1)
robot.filters_sg.estimator_kin.init(dt,NJ, conf.DELAY_ENC*dt,2);
plug(robot.encoders.sout, robot.filters_sg.estimator_kin.x);
plug(robot.imu_offset_compensation.accelerometer_out, robot.filters_sg.acc_filter.x);
plug(robot.imu_offset_compensation.gyrometer_out, robot.filters_sg.gyro_filter.x);
plug(robot.device.forceRLEG, robot.filters_sg.ft_RF_filter.x);
plug(robot.device.forceLLEG, robot.filters_sg.ft_LF_filter.x);
plug(robot.device.forceRARM, robot.filters_sg.ft_RH_filter.x);
plug(robot.device.forceLARM, robot.filters_sg.ft_LH_filter.x);
filters.ft_RF_filter = create_chebi1_checby2_series_filter("ft_RF_filter", dt, 6);
filters.ft_LF_filter = create_chebi1_checby2_series_filter("ft_LF_filter", dt, 6);
filters.ft_RH_filter = create_chebi1_checby2_series_filter("ft_RH_filter", dt, 6);
filters.ft_LH_filter = create_chebi1_checby2_series_filter("ft_LH_filter", dt, 6);
filters.acc_filter = create_chebi1_checby2_series_filter("dv_filter", dt, 3);
filters.gyro_filter = create_chebi1_checby2_series_filter("w_filter", dt, 3);
filters.estimator_kin = create_chebi1_checby2_series_filter("estimator_kin", dt, NJ);
plug(robot.encoders.sout, filters.estimator_kin.x);
plug(robot.device.robotState, estimator_ft.base6d_encoders);
......@@ -232,15 +260,6 @@ def create_estimators(robot, conf, motor_params, dt):
estimator_ft.gear_ratios.value = motor_params.GEAR_RATIOS;
estimator_ft.init(True);
#filters.current_filter.init(dt,NJ, conf.DELAY_CURRENT*dt,1)
filters.ft_RF_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
filters.ft_LF_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
filters.ft_RH_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
filters.ft_LH_filter.init(dt, 6, conf.DELAY_FORCE*dt,1)
filters.gyro_filter.init(dt, 3, conf.DELAY_GYRO*dt,1)
filters.acc_filter.init(dt, 3, conf.DELAY_ACC*dt,1)
filters.estimator_kin.init(dt,NJ, conf.DELAY_ENC*dt,2);
return (estimator_ft, filters);
......@@ -314,7 +333,6 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
plug(robot.lf_force_traj_gen.x, ctrl.f_ref_left_foot);
# rather than giving to the controller the values of gear ratios and rotor inertias
# it is better to compute directly their product in python and pass the result
# to the C++ entity, because otherwise we get a loss of precision
......
python/dynamic_graph/sot/torque_control/utils/filter_utils.py
View file @ 82c81563
......@@ -59,8 +59,7 @@ def create_chebi1_checby2_series_filter(name, dt, size):
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),
(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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment