def get_default_conf():
import dynamic_graph.sot.torque_control.talos.balance_ctrl_conf as balance_ctrl_conf
import dynamic_graph.sot.torque_control.talos.admittance_ctrl_conf as admittance_ctrl_conf
import dynamic_graph.sot.torque_control.talos.base_estimator_conf as base_estimator_conf
import dynamic_graph.sot.torque_control.talos.control_manager_conf as control_manager_conf
import dynamic_graph.sot.torque_control.talos.force_torque_estimator_conf as force_torque_estimator_conf
import dynamic_graph.sot.torque_control.talos.joint_torque_controller_conf as joint_torque_controller_conf
import dynamic_graph.sot.torque_control.talos.joint_pos_ctrl_gains as pos_ctrl_gains
import dynamic_graph.sot.torque_control.talos.motors_parameters as motor_params
conf = Bunch()
conf.balance_ctrl = balance_ctrl_conf
conf.adm_ctrl = admittance_ctrl_conf
conf.base_estimator = base_estimator_conf
conf.control_manager = control_manager_conf
conf.force_torque_estimator = force_torque_estimator_conf
conf.joint_torque_controller = joint_torque_controller_conf
conf.pos_ctrl_gains = pos_ctrl_gains
conf.motor_params = motor_params
return conf
def create_filters(robot, conf, motor_params, dt):
filters = Bunch()
# 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")
plug(robot.encoders.sout, filters.estimator_kin.x)
plug(robot.imu_offset_compensation.accelerometer_out, filters.acc_filter.x)
plug(robot.imu_offset_compensation.gyrometer_out, filters.gyro_filter.x)
plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
plug(robot.device.forceRARM, filters.ft_RH_filter.x)
plug(robot.device.forceLARM, filters.ft_LH_filter.x)
plug(robot.device.currents, filters.current_filter.x)
'''plug(filters.acc_filter.x_filtered, estimator_ft.accelerometer);
plug(filters.gyro_filter.x_filtered, estimator_ft.gyro);
plug(filters.gyro_filter.dx, estimator_ft.dgyro);
plug(filters.ft_RF_filter.x_filtered, estimator_ft.ftSensRightFoot);
plug(filters.ft_LF_filter.x_filtered, estimator_ft.ftSensLeftFoot);
plug(filters.ft_RH_filter.x_filtered, estimator_ft.ftSensRightHand);
plug(filters.ft_LH_filter.x_filtered, estimator_ft.ftSensLeftHand);
plug(filters.current_filter.x_filtered, estimator_ft.current);'''
'''plug(filters.estimator_kin.x_filtered, estimator_ft.q_filtered);
plug(filters.estimator_kin.dx, estimator_ft.dq_filtered);
plug(filters.estimator_kin.ddx, estimator_ft.ddq_filtered);
try:
plug(robot.traj_gen.dq, estimator_ft.dqRef);
plug(robot.traj_gen.ddq, estimator_ft.ddqRef);
except:
pass;
estimator_ft.wCurrentTrust.value = tuple(NJ*[conf.CURRENT_TORQUE_ESTIMATION_TRUST,])
estimator_ft.saturationCurrent.value = tuple(NJ*[conf.SATURATION_CURRENT,])
estimator_ft.motorParameterKt_p.value = tuple(motor_params.Kt_p)
estimator_ft.motorParameterKt_n.value = tuple(motor_params.Kt_n)
estimator_ft.motorParameterKf_p.value = tuple(motor_params.Kf_p)
estimator_ft.motorParameterKf_n.value = tuple(motor_params.Kf_n)
estimator_ft.motorParameterKv_p.value = tuple(motor_params.Kv_p)
estimator_ft.motorParameterKv_n.value = tuple(motor_params.Kv_n)
estimator_ft.motorParameterKa_p.value = tuple(motor_params.Ka_p)
estimator_ft.motorParameterKa_n.value = tuple(motor_params.Ka_n)
estimator_ft.rotor_inertias.value = motor_params.ROTOR_INERTIAS;
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 filters
def get_sim_conf():
import dynamic_graph.sot.torque_control.talos.balance_ctrl_sim_conf as balance_ctrl_conf
import dynamic_graph.sot.torque_control.talos.base_estimator_sim_conf as base_estimator_conf
import dynamic_graph.sot.torque_control.talos.control_manager_sim_conf as control_manager_conf
import dynamic_graph.sot.torque_control.talos.current_controller_sim_conf as current_controller_conf
import dynamic_graph.sot.torque_control.talos.force_torque_estimator_conf as force_torque_estimator_conf
import dynamic_graph.sot.torque_control.talos.joint_torque_controller_conf as joint_torque_controller_conf
import dynamic_graph.sot.torque_control.talos.joint_pos_ctrl_gains_sim as pos_ctrl_gains
import dynamic_graph.sot.torque_control.talos.motors_parameters as motor_params
import dynamic_graph.sot.torque_control.talos.ddp_controller_conf as ddp_controller_conf
conf = Bunch()
conf.balance_ctrl = balance_ctrl_conf
conf.base_estimator = base_estimator_conf
conf.control_manager = control_manager_conf
conf.current_ctrl = current_controller_conf
conf.force_torque_estimator = force_torque_estimator_conf
conf.joint_torque_controller = joint_torque_controller_conf
conf.pos_ctrl_gains = pos_ctrl_gains
conf.motor_params = motor_params
conf.ddp_controller = ddp_controller_conf
return conf