def __init__(self, dfile): if isinstance(dfile, ClmcFile): print 'setting ClmcFile from argument' self.dfile=dfile else: print 'loading dfile', dfile self.dfile=ClmcFile(dfile) self.dc_time=self.dfile.get_variables('time') self.base_slck_varnames=['BASE_slck_x', 'BASE_slck_y', 'BASE_slck_z', 'BASE_slck_a', 'BASE_slck_b', 'BASE_slck_g' ] self.swingfoot_right_slck_varnames=['R_FOOT_slck_x', 'R_FOOT_slck_y', 'R_FOOT_slck_z', 'R_FOOT_slck_a', 'R_FOOT_slck_b', 'R_FOOT_slck_g' ] self.swingfoot_left_slck_varnames=['L_FOOT_slck_x', 'L_FOOT_slck_y', 'L_FOOT_slck_z', 'L_FOOT_slck_a', 'L_FOOT_slck_b', 'L_FOOT_slck_g' ] self.momrate_slck_varnames=['MomRate_slck_x','MomRate_slck_y','MomRate_slck_z','MomRate_slck_a','MomRate_slck_b','MomRate_slck_g'] self.leg_dof_varnames = ['HFE', 'HAA', 'HFR', 'KFE', 'AR', 'AFE', 'AAA'] self.torso_dof_varnames = ['B_TR', 'B_TAA', 'B_TFE'] self.dof_varnames = []#['L_'+s+'_th' for s in self.leg_dof_varnames] for prefix in ['L_', 'R_']: self.dof_varnames.extend([prefix+s for s in self.leg_dof_varnames]) self.dof_varnames.extend(self.torso_dof_varnames) return
class FailureHints: def __init__(self, dfile): if isinstance(dfile, ClmcFile): print 'setting ClmcFile from argument' self.dfile=dfile else: print 'loading dfile', dfile self.dfile=ClmcFile(dfile) self.dc_time=self.dfile.get_variables('time') self.base_slck_varnames=['BASE_slck_x', 'BASE_slck_y', 'BASE_slck_z', 'BASE_slck_a', 'BASE_slck_b', 'BASE_slck_g' ] self.swingfoot_right_slck_varnames=['R_FOOT_slck_x', 'R_FOOT_slck_y', 'R_FOOT_slck_z', 'R_FOOT_slck_a', 'R_FOOT_slck_b', 'R_FOOT_slck_g' ] self.swingfoot_left_slck_varnames=['L_FOOT_slck_x', 'L_FOOT_slck_y', 'L_FOOT_slck_z', 'L_FOOT_slck_a', 'L_FOOT_slck_b', 'L_FOOT_slck_g' ] self.momrate_slck_varnames=['MomRate_slck_x','MomRate_slck_y','MomRate_slck_z','MomRate_slck_a','MomRate_slck_b','MomRate_slck_g'] self.leg_dof_varnames = ['HFE', 'HAA', 'HFR', 'KFE', 'AR', 'AFE', 'AAA'] self.torso_dof_varnames = ['B_TR', 'B_TAA', 'B_TFE'] self.dof_varnames = []#['L_'+s+'_th' for s in self.leg_dof_varnames] for prefix in ['L_', 'R_']: self.dof_varnames.extend([prefix+s for s in self.leg_dof_varnames]) self.dof_varnames.extend(self.torso_dof_varnames) return def plot_slacks(self, plot_range = None): if plot_range == None: plot_range = np.s_[:] else: dt=self.dc_time[1]-self.dc_time[0] plot_range=np.s_[int(plot_range[0]/dt):int(plot_range[1]/dt)] walking_state=self.dfile.get_variables('walking_state') base_slacks=self.dfile.get_variables(self.base_slck_varnames) base_slack_sqr_sum=np.zeros(base_slacks[0].size) for i in range(len(base_slacks)): base_slack_sqr_sum += base_slacks[i]**2 swingfoot_right_slacks=self.dfile.get_variables(self.swingfoot_right_slck_varnames) swingfoot_right_slack_sqr_sum=np.zeros(swingfoot_right_slacks[0].size) for i in range(len(swingfoot_right_slacks)): swingfoot_right_slack_sqr_sum += swingfoot_right_slacks[i]**2 swingfoot_left_slacks=self.dfile.get_variables(self.swingfoot_left_slck_varnames) swingfoot_left_slack_sqr_sum=np.zeros(swingfoot_left_slacks[0].size) for i in range(len(swingfoot_left_slacks)): swingfoot_left_slack_sqr_sum += swingfoot_left_slacks[i]**2 posture_slacks_sqr_sum=self.dfile.get_variables('posture_slck') momrate_slacks=self.dfile.get_variables(self.momrate_slck_varnames) momrate_slacks_sqr_sum=np.zeros(momrate_slacks[0].size) for i in range(len(momrate_slacks)): momrate_slacks_sqr_sum += momrate_slacks[i]**2 feet_const_slacks_sqr_sum=self.dfile.get_variables('stat_feet_slck') plt.subplot(2,1,1) plt.plot(self.dc_time[plot_range], base_slack_sqr_sum[plot_range], label='BASE slack') plt.plot(self.dc_time[plot_range], feet_const_slacks_sqr_sum[plot_range], label='feet_const slack') plt.plot(self.dc_time[plot_range], momrate_slacks_sqr_sum[plot_range], label='MomRate slack') plt.plot(self.dc_time[plot_range], posture_slacks_sqr_sum[plot_range], label='posture slack') plt.plot(self.dc_time[plot_range], swingfoot_right_slack_sqr_sum[plot_range], label='SwingRight slack') plt.plot(self.dc_time[plot_range], swingfoot_left_slack_sqr_sum[plot_range], label='SwingLeft slack') plt.legend() plt.subplot(2,1,2) plt.plot(self.dc_time[plot_range], walking_state[plot_range], label='walking state') plt.legend() def plot_com_tracking(self, plot_range = None): if plot_range == None: plot_range = np.s_[:] else: dt=self.dc_time[1]-self.dc_time[0] plot_range=np.s_[int(plot_range[0]/dt):int(plot_range[1]/dt)] coord_names=['x', 'y', 'z'] com_pos = self.dfile.get_variables(['cog_'+coord_names[s] for s in range(3)]) com_vel = self.dfile.get_variables(['com_'+coord_names[s]+'d' for s in range(3)]) des_com_pos = self.dfile.get_variables(['des_com_'+coord_names[s] for s in range(3)]) des_com_vel = self.dfile.get_variables(['des_com_'+coord_names[s]+'d' for s in range(3)]) for i in range(3): plt.subplot(6,1,i+1) plt.plot(self.dc_time[plot_range], com_pos[1][plot_range], label='com_'+coord_names[s]) plt.plot(self.dc_time[plot_range], des_com_pos[1][plot_range], label='des_com_'+coord_names[s]) plt.legend() plt.subplot(6,1,i+1+3) plt.plot(self.dc_time[plot_range], com_vel[1][plot_range], label='com_'+coord_names[s]+'d') plt.plot(self.dc_time[plot_range], des_com_vel[1][plot_range], label='des_com_'+coord_names[s]+'d') plt.legend() def plot_posture(self): data_arrays = self.dfile.get_variables([dof_name+'_th' for dof_name in self.dof_varnames]) for i, dat in enumerate(data_arrays): plt.subplot(math.ceil(len(data_arrays)/2.),2,i+1) plt.plot(self.dc_time, dat, label=self.dof_varnames[i]+'_th') plt.legend() def get_posture_at(self,time): data_arrays = self.dfile.get_variables([dof_name+'_th' for dof_name in self.dof_varnames]) index = int(time/(self.dc_time[1]-self.dc_time[0])) posture = [arr[index] for arr in data_arrays] posture_str = '' for d in posture: posture_str += str(d)+' ' print posture_str return posture