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):
estimator_kin = VelAccEstimator("estimator_kin");
estimator_ft = ForceTorqueEstimator("estimator_ft");
plug(robot.encoders.sout, estimator_kin.x);
plug(robot.device.robotState, estimator_ft.base6d_encoders);
plug(robot.imu_offset_compensation.accelerometer_out, estimator_ft.accelerometer);
plug(robot.imu_offset_compensation.gyrometer_out, estimator_ft.gyroscope);
plug(robot.device.forceRLEG, estimator_ft.ftSensRightFoot);
plug(robot.device.forceLLEG, estimator_ft.ftSensLeftFoot);
plug(robot.device.forceRARM, estimator_ft.ftSensRightHand);
plug(robot.device.forceLARM, estimator_ft.ftSensLeftHand);
plug(robot.device.currents, estimator_ft.currentMeasure);
plug(estimator_kin.x_filtered, estimator_ft.q_filtered);
plug(estimator_kin.dx, estimator_ft.dq_filtered);
plug(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.init(dt, conf.DELAY_ACC*dt, conf.DELAY_GYRO*dt, conf.DELAY_FORCE*dt, conf.DELAY_CURRENT*dt, True);
estimator_kin.init(dt,NJ, conf.DELAY_ENC*dt);
return (estimator_ft, estimator_kin);
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-lar-lap','torque')
# enable integral feedback in torque control
import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params
robot.torque_ctrl.torque_integral_saturation.value = tuple(0.5*motor_params.Kf_n / motor_params.Kt_n)
robot.torque_ctrl.KiTorque.value = 30*(5.0,)
# enable foot pose update in base estimator
robot.base_estimator.K_fb_feet_poses.value = 1e-3
# compute derivatives of joint torques
from dynamic_graph.sot.torque_control.numerical_difference import NumericalDifference
torque_der = NumericalDifference("torque_der");
plug(robot.torque_ctrl.jointsTorques, torque_der.x)
torque_der.init(robot.timeStep, 30, 0.015, 2);
create_topic(robot.ros, torque_der.dx, 'dtau')
plug(torque_der.dx, robot.torque_ctrl.jointsTorquesDerivative)
# switch to position control
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, '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_L');
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_R');
create_topic(robot.ros, robot.ctrl_manager.u, 'i_des');
def compute_estimates_from_sensors(sensors, delay, ftSensorOffsets=None, USE_FT_SENSORS=True):
NJ = 30; # number of joints
m = sensors['time'].shape[0]; # number of time steps
dt = float(np.mean(sensors['time'][1:]-sensors['time'][:-1])); # sampling period
# Create and initialize estimator
estimator_ft = ForceTorqueEstimator("estimator_ft"+str(np.random.rand()));
estimator_kin = VelAccEstimator("estimator_kin"+str(np.random.rand()));
plug(estimator_kin.x, estimator_ft.q_filtered);
plug(estimator_kin.dx, estimator_ft.dq_filtered);
plug(estimator_kin.ddx, estimator_ft.ddq_filtered);
estimator_ft.dqRef.value = NJ*(0.0,);
estimator_ft.ddqRef.value = NJ*(0.0,);
#Only use inertia model (not current) to estimate torques.
estimator_ft.wCurrentTrust.value = NJ*(0.0,);
estimator_ft.currentMeasure.value = NJ*(0.0,);
estimator_ft.saturationCurrent.value = NJ*(0.0,);
estimator_ft.motorParameterKt_p.value = tuple(30*[1.,])
estimator_ft.motorParameterKt_n.value = tuple(30*[1.,])
estimator_ft.motorParameterKf_p.value = tuple(30*[0.,])
estimator_ft.motorParameterKf_n.value = tuple(30*[0.,])
estimator_ft.motorParameterKv_p.value = tuple(30*[0.,])
estimator_ft.motorParameterKv_n.value = tuple(30*[0.,])
estimator_ft.motorParameterKa_p.value = tuple(30*[0.,])
estimator_ft.motorParameterKa_n.value = tuple(30*[0.,])
set_sensor_data_in_estimator(estimator_ft, estimator_kin, sensors[0]);
print "Time step: %f" % dt;
print "Estimation delay: %f" % delay;
if(ftSensorOffsets==None):
estimator_ft.init(dt,delay,delay,delay,delay,True);
else:
estimator_ft.init(dt,delay,delay,delay,delay,False);
estimator_ft.setFTsensorOffsets(tuple(ftSensorOffsets));
estimator_kin.init(dt,NJ, delay);
estimator_ft.setUseRawEncoders(False);
estimator_ft.setUseRefJointVel(False);
estimator_ft.setUseRefJointAcc(False);
estimator_ft.setUseFTsensors(USE_FT_SENSORS);
torques = np.zeros((m,NJ));
dq = np.zeros((m,NJ));
ddq = np.zeros((m,NJ));
if(USE_ROBOT_VIEWER):
viewer=robotviewer.client('XML-RPC');
for i in range(m):
if(USE_ROBOT_VIEWER and i%10==0):
viewer.updateElementConfig('hrp_device', [0,0,0.7,0,0,0] + sensors['enc'][i,:].tolist());
set_sensor_data_in_estimator(estimator_ft, estimator_kin, sensors[i]);
estimator_ft.jointsTorques.recompute(i);
torques[i,:] = np.array(estimator_ft.jointsTorques.value);
dq[i,:] = np.array(estimator_kin.dx.value);
ddq[i,:] = np.array(estimator_kin.ddx.value);
if(i==2):
print ("F/T sensor offsets are: ", estimator_ft.getFTsensorOffsets());
if(i%1000==0):
print 'Estimation time: \t %.3f' % (i*dt);
return (torques, dq, ddq);
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 compute_estimates_from_sensors(sensors,
delay,
ftSensorOffsets=None,
USE_FT_SENSORS=True):
NJ = 30
# number of joints
m = sensors['time'].shape[0]
# number of time steps
dt = float(np.mean(sensors['time'][1:] - sensors['time'][:-1]))
# sampling period
# Create and initialize estimator
estimator_ft = ForceTorqueEstimator("estimator_ft" + str(np.random.rand()))
estimator_kin = NumericalDifference("estimator_kin" +
str(np.random.rand()))
ft_RF_filter = NumericalDifference("ft_RF_filter" + str(np.random.rand()))
ft_LF_filter = NumericalDifference("ft_LF_filter" + str(np.random.rand()))
ft_RH_filter = NumericalDifference("ft_RH_filter" + str(np.random.rand()))
ft_LH_filter = NumericalDifference("ft_LH_filter" + str(np.random.rand()))
acc_filter = NumericalDifference("dv_filter" + str(np.random.rand()))
gyro_filter = NumericalDifference("w_filter" + str(np.random.rand()))
estimator_kin = NumericalDifference("estimator_kin" +
str(np.random.rand()))
plug(estimator_kin.x, estimator_ft.q_filtered)
plug(estimator_kin.dx, estimator_ft.dq_filtered)
plug(estimator_kin.ddx, estimator_ft.ddq_filtered)
plug(acc_filter.x_filtered, estimator_ft.accelerometer)
plug(gyro_filter.x_filtered, estimator_ft.gyro)
plug(gyro_filter.dx, estimator_ft.dgyro)
plug(ft_RF_filter.x_filtered, estimator_ft.ftSensRightFoot)
plug(ft_LF_filter.x_filtered, estimator_ft.ftSensLeftFoot)
plug(ft_RH_filter.x_filtered, estimator_ft.ftSensRightHand)
plug(ft_LH_filter.x_filtered, estimator_ft.ftSensLeftHand)
estimator_ft.rotor_inertias.value = NJ * (0., )
estimator_ft.gear_ratios.value = NJ * (0., )
estimator_ft.dqRef.value = NJ * (0.0, )
estimator_ft.ddqRef.value = NJ * (0.0, )
# Only use inertia model (not current) to estimate torques.
estimator_ft.wCurrentTrust.value = NJ * (0.0, )
estimator_ft.current.value = NJ * (0.0, )
estimator_ft.saturationCurrent.value = NJ * (0.0, )
estimator_ft.motorParameterKt_p.value = tuple(30 * [1.])
estimator_ft.motorParameterKt_n.value = tuple(30 * [1.])
estimator_ft.motorParameterKf_p.value = tuple(30 * [0.])
estimator_ft.motorParameterKf_n.value = tuple(30 * [0.])
estimator_ft.motorParameterKv_p.value = tuple(30 * [0.])
estimator_ft.motorParameterKv_n.value = tuple(30 * [0.])
estimator_ft.motorParameterKa_p.value = tuple(30 * [0.])
estimator_ft.motorParameterKa_n.value = tuple(30 * [0.])
set_sensor_data_in_estimator(estimator_ft, estimator_kin, acc_filter,
gyro_filter, ft_RH_filter, ft_LF_filter,
ft_LH_filter, ft_RF_filter, sensors[0])
print("Time step: %f" % dt)
print("Estimation delay: %f" % delay)
if ftSensorOffsets is None:
estimator_ft.init(True)
else:
estimator_ft.init(False)
estimator_ft.setFTsensorOffsets(tuple(ftSensorOffsets))
estimator_kin.init(dt, NJ, delay, 2)
ft_RF_filter.init(dt, 6, delay, 1)
ft_LF_filter.init(dt, 6, delay, 1)
ft_RH_filter.init(dt, 6, delay, 1)
ft_LH_filter.init(dt, 6, delay, 1)
gyro_filter.init(dt, 3, delay, 1)
acc_filter.init(dt, 3, delay, 1)
estimator_ft.setUseRawEncoders(False)
estimator_ft.setUseRefJointVel(False)
estimator_ft.setUseRefJointAcc(False)
estimator_ft.setUseFTsensors(USE_FT_SENSORS)
torques = np.zeros((m, NJ))
dq = np.zeros((m, NJ))
ddq = np.zeros((m, NJ))
if (USE_ROBOT_VIEWER):
viewer = robotviewer.client('XML-RPC')
for i in range(m):
if USE_ROBOT_VIEWER and i % 10 == 0:
viewer.updateElementConfig('hrp_device', [0, 0, 0.7, 0, 0, 0] +
sensors['enc'][i, :].tolist())
set_sensor_data_in_estimator(estimator_ft, estimator_kin, acc_filter,
gyro_filter, ft_RH_filter, ft_LF_filter,
ft_LH_filter, ft_RF_filter, sensors[i])
estimator_ft.jointsTorques.recompute(i)
torques[i, :] = np.array(estimator_ft.jointsTorques.value)
dq[i, :] = np.array(estimator_kin.dx.value)
ddq[i, :] = np.array(estimator_kin.ddx.value)
if i == 2:
print("F/T sensor offsets are: ",
estimator_ft.getFTsensorOffsets())
if i % 1000 == 0:
print('Estimation time: \t %.3f' % (i * dt))
return (torques, dq, ddq)