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