def conf_velocity_filter(): conf = Bunch() conf.j_name = "re" conf.x_f = -1.0 conf.w_min = 0.01 conf.w_max = 0.7 conf.deltaT = 1.0 return conf
def get_sim_conf(): import dynamic_graph.sot.torque_control.hrp2.balance_ctrl_sim_conf as balance_ctrl_conf import dynamic_graph.sot.torque_control.hrp2.base_estimator_sim_conf as base_estimator_conf import dynamic_graph.sot.torque_control.hrp2.control_manager_sim_conf as control_manager_conf import dynamic_graph.sot.torque_control.hrp2.current_controller_sim_conf as current_controller_conf import dynamic_graph.sot.torque_control.hrp2.force_torque_estimator_conf as force_torque_estimator_conf import dynamic_graph.sot.torque_control.hrp2.joint_torque_controller_conf as joint_torque_controller_conf import dynamic_graph.sot.torque_control.hrp2.joint_pos_ctrl_gains_sim as pos_ctrl_gains import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params 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 return conf
def get_default_conf(): import dynamic_graph.sot.torque_control.hrp2.inverse_dynamics_controller_gains as inv_dyn_gains import dynamic_graph.sot.torque_control.hrp2.base_estimator_conf as base_estimator_conf import dynamic_graph.sot.torque_control.hrp2.control_manager_conf as control_manager_conf import dynamic_graph.sot.torque_control.hrp2.force_torque_estimator_conf as force_torque_estimator_conf import dynamic_graph.sot.torque_control.hrp2.joint_torque_controller_conf as joint_torque_controller_conf import dynamic_graph.sot.torque_control.hrp2.joint_pos_ctrl_gains as pos_ctrl_gains import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params conf = Bunch() conf.inv_dyn_gains = inv_dyn_gains 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_estimators(robot, conf, motor_params, dt): from dynamic_graph.sot.torque_control.utils.filter_utils import create_chebi1_checby2_series_filter robot.filters_sg = Bunch() filters = Bunch() # create low-pass filter for motor currents filters.current_filter = create_butter_lp_filter_Wn_05_N_3( 'current_filter', dt, NJ) 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.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.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) return
def create_estimators(robot, conf, motor_params, dt): 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") plug(robot.encoders.sout, filters.estimator_kin.x) plug(robot.device.robotState, estimator_ft.base6d_encoders) 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 (estimator_ft, filters)
def conf_filter_list(): conf = Bunch() conf.b_list = [ np.array([0.00902094, 0.01800633, 0.00902094]), np.array([0.00542136, 0.01084273, 0.00542136]), np.array([0.00355661, 0.00711322, 0.00355661]), np.array([0.00208057, 0.00416113, 0.00208057]), np.array([0.03046875, 0.03046875]), np.array([0.0020518, 0.00410359, 0.0020518]), np.array([0.00094469, 0.00188938, 0.00094469]), np.array( [6.70858643e-05, 2.01257593e-04, 2.01257593e-04, 6.70858643e-05]), np.array([0.00010101, 0.00016741, 0.00016741, 0.00010101]), np.array( [9.36150553e-05, 2.80845166e-04, 2.80845166e-04, 9.36150553e-05]), np.array([0.00093575, 0.0018715, 0.00093575]), np.array([ 1.67370188e-07, 1.00422113e-06, 2.51055282e-06, 3.34740376e-06, 2.51055282e-06, 1.00422113e-06, 1.67370188e-07 ]), np.array([ 4.27323622e-06, 1.70929449e-05, 2.56394173e-05, 1.70929449e-05, 4.27323622e-06 ]), np.array([ 1.38055495e-05, -2.98864128e-05, 1.80947937e-05, 1.80947937e-05, -2.98864128e-05, 1.38055495e-05 ]), np.array([ 1.95099715e-07, 9.75498574e-07, 1.95099715e-06, 1.95099715e-06, 9.75498574e-07, 1.95099715e-07 ]), np.array([ 1.70134115e-10, 1.36107292e-09, 4.76375523e-09, 9.52751046e-09, 1.19093881e-08, 9.52751046e-09, 4.76375523e-09, 1.36107292e-09, 1.70134115e-10 ]) ] conf.a_list = [ np.array([1., -1.72610371, 0.76236003]), np.array([1., -1.73969005, 0.7613755]), np.array([1., -1.78994555, 0.80417199]), np.array([1., -1.86689228, 0.87521455]), np.array([1., -0.93906251]), np.array([1., -1.84107589, 0.84928308]), np.array([1., -1.91119707, 0.91497583]), np.array([1., -2.85434518, 2.721757, -0.86687514]), np.array([1., -2.8543607, 2.72178648, -0.86688893]), np.array([1., -2.77503623, 2.57077857, -0.79499342]), np.array([1., -1.89310959, 0.8968526]), np.array([ 1., -5.30603285, 11.7558334, -13.91966085, 9.2894345, -3.31270578, 0.49314228 ]), np.array([1., -3.70990618, 5.16705461, -3.20193343, 0.74485337]), np.array([ 1., -4.80072628, 9.23289417, -8.89170853, 4.28778797, -0.82824331 ]), np.array( [1., -4.64518256, 8.63880318, -8.0399445, 3.74448588, -0.69815575]), np.array([ 1., -7.27133134, 23.1548678, -42.17546448, 48.05912505, -35.08178214, 16.02023822, -4.18419295, 0.47853988 ]) ] #conf.b_list = np.flipud(conf.b_list) #conf.a_list = np.flipud(conf.a_list) return conf