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
