def do_log(proxy): print 'Starting log...select component to log' proxy.start(start_data_svc=False) comp_name = m3t.user_select_components_interactive( proxy.get_available_components())[0] comp = mcf.create_component(comp_name) proxy.register_log_component(comp) print 'Enter log name [foo]' try_again = True while try_again: logname = m3t.get_string('foo') if not os.path.isdir(m3t.get_m3_log_path() + logname): try_again = False else: print 'Log directory already exists. Please enter another name.' print 'Enter sample frequence (0-X Hz) [250.0]' freq = m3t.get_float(250.0) default_samps = int(2 * freq / 5) print 'Enter samples per file [' + str(default_samps) + ']' samples = m3t.get_int(default_samps) print 'Enter sample time (s) [5.0]' duration = m3t.get_float(5.0) print 'Hit enter to begin log...' raw_input() proxy.start_log_service(logname, sample_freq_hz=freq, samples_per_file=samples) ts = time.time() while time.time() - ts < duration: time.sleep(0.25) print 'Logging ', logname, ' Time: ', time.time() - ts proxy.stop_log_service()
def do_log(proxy): print 'Starting log...select component to log' proxy.start(start_data_svc=False) comp_name=m3t.user_select_components_interactive(proxy.get_available_components())[0] comp=mcf.create_component(comp_name) proxy.register_log_component(comp) print 'Enter log name [foo]' try_again = True while try_again: logname=m3t.get_string('foo') if not os.path.isdir(m3t.get_m3_log_path()+logname): try_again = False else: print 'Log directory already exists. Please enter another name.' print 'Enter sample frequence (0-X Hz) [250.0]' freq=m3t.get_float(250.0) default_samps = int(2*freq/5) print 'Enter samples per file ['+str(default_samps) +']' samples=m3t.get_int(default_samps) print 'Enter sample time (s) [5.0]' duration=m3t.get_float(5.0) print 'Hit enter to begin log...' raw_input() proxy.start_log_service(logname,sample_freq_hz=freq,samples_per_file=samples) ts=time.time() while time.time()-ts<duration: time.sleep(0.25) print 'Logging ',logname,' Time: ',time.time()-ts proxy.stop_log_service()
def fill_cells_random(): scope=m3t.M3Scope2(xwidth=100,yrange=None) ns=30#5#K*2 #Build desired vector log={'ns':ns,'type':'fill_cells_random', 'actuator':'MA12J1','L':L,'K':K,'CW':cw,'a':list(a),'b':list(b),'u_max':u_max,'u_min':u_min,'ut':[],'wt':[],'gt':[],'des':[]} des=[u_min] for nn in range(ns): while True: x=u_min+(u_max-u_min)*(random.random()) #randomly in range if abs(des[-1]-x)>5*cw: #make excursion at least 5 cells des.append(x) break log['des']=des print 'Des',des print 'Enable power. Hit enter to continue' raw_input() for ii in range(len(des)): print ii,'Des: ',des[ii] uu,ww,gg=ramp_to_torque(des[ii],scope) print len(uu),len(ww),len(gg) log['ut'].append(list(uu)) #input log['wt'].append(list(ww)) #output log['gt'].append(list(gg)) #hysteron states print 'Save data [y]?' if m3t.get_yes_no('y'): fn=m3t.get_m3_config_path()+'data/preisach_data_random_ma12j1_'+m3t.time_string()+'.yml' print 'Enter annotation string [None]' note=m3t.get_string('None') log['note']=note f=file(fn,'w') print 'Saving...',fn f.write(yaml.safe_dump(log, default_flow_style=False,width=200)) f.close() return log
def get_client_code(): print 'Enter Client Code (e.g., uta)' while True: s=m3t.get_string() if len(s)!=3: print 'Code must be length 3' else: return s
def get_client_code(): print 'Enter Client Code (e.g., uta)' while True: s = m3t.get_string() if len(s) != 3: print 'Code must be length 3' else: return s
def do_plot(proxy): print 'Starting plot...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' print ' Select field to plot...' comp=mcf.create_component(name) proxy.register_log_component(comp) field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) print 'Fetching data...' data=[] for i in range(start,end): proxy.load_log_sample(logname,i) if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) data.append(v) # m3t.plot(data) m3t.mplot(data,range(len(data)))
def do_plot(proxy): print 'Starting plot...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' print ' Select field to plot...' comp = mcf.create_component(name) proxy.register_log_component(comp) field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) print 'Fetching data...' data = [] for i in range(start, end): proxy.load_log_sample(logname, i) if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) data.append(v) # m3t.plot(data) m3t.mplot(data, range(len(data)))
def run(self): vias = {} for c in self.chains: vias[c] = [] while not self.done: print '--------------' print 'r: record via' print 's: save via file' print 'p: print current pose' print 'q: quit' print '--------------' print k = m3t.get_keystroke() print 'Dbg', dbg if k == 'r': print '-------------' for c in self.chains: vias[c].append(m3t.float_list(self.bot.get_theta_deg(c))) print 'Record of: ', vias[c][-1], 'for', c if k == 'p': print '----------------' for c in self.chains: print 'Chain:', c, ' : ', self.bot.get_theta_deg(c) print '----------------' if k == 's': bot_name = m3t.get_robot_name() fn = bot_name + '_' + m3t.time_string() print 'Enter via file name [', fn, ']' fn = m3t.get_string(fn) fn = m3t.get_m3_animation_path() + fn + '.via' print 'Writing file: ', fn f = file(fn, 'w') d = {} for c in self.chains: ndof = self.bot.get_num_dof(c) if c == 'torso': param = { 'slew': [1.0] * ndof, 'stiffness': [0.8] * ndof, 'velocity': [25.0, 15.0] } else: param = { 'slew': [1.0] * ndof, 'stiffness': [0.4] * ndof, 'velocity': [25.0] * ndof } d[c] = { 'postures': vias[c], 'param': param } #safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == 'q': self.done = True
def do_scope(proxy): print 'Starting scope...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) print 'Num samples available: ', ns if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) comp = mcf.create_component(name) proxy.register_log_component(comp) scope = m3t.M3Scope(xwidth=ns - 1) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' print 'Select field to monitor' field = m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) for i in range(start, end): proxy.load_log_sample(logname, i) if repeated: v = m3t.get_msg_field_value(comp.status, field)[idx] else: v = m3t.get_msg_field_value(comp.status, field) scope.plot(v) time.sleep(0.05)
def do_scope(proxy): print 'Starting scope...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) print 'Num samples available: ',ns if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) comp=mcf.create_component(name) proxy.register_log_component(comp) scope=m3t.M3Scope(xwidth=ns-1) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' print 'Select field to monitor' field=m3t.user_select_msg_field(comp.status) repeated = False idx = 0 if hasattr(m3t.get_msg_field_value(comp.status,field),'__len__'): repeated = True print 'Select index of repeated field to monitor: [0]' idx = m3t.get_int(0) for i in range(start,end): proxy.load_log_sample(logname,i) if repeated: v=m3t.get_msg_field_value(comp.status,field)[idx] else: v=m3t.get_msg_field_value(comp.status,field) scope.plot(v) time.sleep(0.05)
def fill_cells_random(): scope = m3t.M3Scope2(xwidth=100, yrange=None) ns = 30 #5#K*2 #Build desired vector log = { 'ns': ns, 'type': 'fill_cells_random', 'actuator': 'MA12J1', 'L': L, 'K': K, 'CW': cw, 'a': list(a), 'b': list(b), 'u_max': u_max, 'u_min': u_min, 'ut': [], 'wt': [], 'gt': [], 'des': [] } des = [u_min] for nn in range(ns): while True: x = u_min + (u_max - u_min) * (random.random()) #randomly in range if abs(des[-1] - x) > 5 * cw: #make excursion at least 5 cells des.append(x) break log['des'] = des print 'Des', des print 'Enable power. Hit enter to continue' raw_input() for ii in range(len(des)): print ii, 'Des: ', des[ii] uu, ww, gg = ramp_to_torque(des[ii], scope) print len(uu), len(ww), len(gg) log['ut'].append(list(uu)) #input log['wt'].append(list(ww)) #output log['gt'].append(list(gg)) #hysteron states print 'Save data [y]?' if m3t.get_yes_no('y'): fn = m3t.get_m3_config_path( ) + 'data/preisach_data_random_ma12j1_' + m3t.time_string() + '.yml' print 'Enter annotation string [None]' note = m3t.get_string('None') log['note'] = note f = file(fn, 'w') print 'Saving...', fn f.write(yaml.safe_dump(log, default_flow_style=False, width=200)) f.close() return log
def run(self): vias = {} for c in self.chains: vias[c] = [] while not self.done: print "--------------" print "r: record via" print "s: save via file" print "p: print current pose" print "q: quit" print "--------------" print k = m3t.get_keystroke() print "Dbg", dbg if k == "r": print "-------------" for c in self.chains: vias[c].append(m3t.float_list(self.bot.get_theta_deg(c))) print "Record of: ", vias[c][-1], "for", c if k == "p": print "----------------" for c in self.chains: print "Chain:", c, " : ", self.bot.get_theta_deg(c) print "----------------" if k == "s": bot_name = m3t.get_robot_name() fn = bot_name + "_" + m3t.time_string() print "Enter via file name [", fn, "]" fn = m3t.get_string(fn) fn = m3t.get_m3_animation_path() + fn + ".via" print "Writing file: ", fn f = file(fn, "w") d = {} for c in self.chains: ndof = self.bot.get_num_dof(c) if c == "torso": param = {"slew": [1.0] * ndof, "stiffness": [0.8] * ndof, "velocity": [25.0, 15.0]} else: param = {"slew": [1.0] * ndof, "stiffness": [0.4] * ndof, "velocity": [25.0] * ndof} d[c] = {"postures": vias[c], "param": param} # safe defaults f.write(yaml.safe_dump(d)) vias = {} for c in self.chains: vias[c] = [] if k == "q": self.done = True
def record_postures(): #Record posture print 'Enter pose name: (q to quit)' while True: name=m3t.get_string() if postures.has_key(name): print 'Pose ',name,'already exists. Overwrite? ' if m3t.get_yes_no(): break else: break if name!='q' and name !='Q': p=bot.get_theta_deg('head') postures[name]={'theta':list(p),'thetadot':thetadot_default} fn=get_m3_animation_path()+'demo_head_h1_postures.yml' print 'Posture',name,': ',p print 'Writing ',fn f=file(fn,'w') f.write(yaml.safe_dump(postures,width=200)) f.close() else: print 'Record aborted'
def step(self): self.proxy.step() self.status_dict = self.proxy.get_status_dict() self.chain.set_stiffness(self.data['param']['stiffness']) self.chain.set_slew_rate_proportion(self.data['param']['q_slew_rate']) #Do power Grasp if self.grasp and not self.grasp_last: if self.mode[0]: self.chain.set_mode_theta_gc([0]) if self.mode[1]: self.chain.set_mode_torque_gc([1]) if self.mode[2]: self.chain.set_mode_torque_gc([2]) if self.mode[3]: self.chain.set_mode_torque_gc([3]) if self.mode[4]: self.chain.set_mode_torque_gc([4]) self.chain.set_theta_deg(self.chain.get_theta_deg()) self.chain.set_torque_mNm(self.data['param']['grasp_torque']) #if False: #theta open #if not self.grasp and self.grasp_last: #force open #self.grasp_off=True #self.chain.set_torque_mNm(self.data['param']['grasp_torque_off']) #self.grasp_off_ts=time.time() #if self.grasp_off and time.time()-self.grasp_off_ts>2.0: #Open #self.grasp_off=False self.grasp_last = self.grasp #Do joint theta control #if not self.grasp and not self.grasp_off and not self.running: #force open if not self.grasp and not self.running: #theta open for jidx in range(5): if self.mode[jidx]: if jidx == 0: self.chain.set_mode_theta(jidx) else: self.chain.set_mode_theta_gc(jidx) else: self.chain.set_mode_off(jidx) #Record posture if self.record and not self.record_last and not self.grasp and not self.grasp_off: print 'Enter pose name: (q to quit)' while True: name = m3t.get_string() if self.data['postures'].has_key(name): print 'Pose ', name, 'already exists. Overwrite? ' if m3t.get_yes_no(): break else: break if name != 'q' and name != 'Q': p = self.chain.get_theta_deg() self.data['postures'][name] = [float(x) for x in p] self.data['thetadot_avg'][name] = [100, 100, 100, 100, 100] print 'Posture', name, ': ', p else: print 'Record aborted' self.record_last = self.record if self.write and not self.write_last: print 'Writing ', self.posture_filename f = file(self.posture_filename, 'w') f.write(yaml.safe_dump(self.data, width=200)) f.close() self.write_last = self.write #Run Animator if not self.run_last and self.run and not self.running: self.running = True self.pose_idx = -1 self.end_time = 0 self.start_time = 0 if True: for jidx in range(5): if self.mode[jidx]: if jidx == 0: self.chain.set_mode_splined_traj(jidx) else: self.chain.set_mode_splined_traj_gc(jidx) else: self.chain.set_mode_off(jidx) np = max( 0, min(self.data['param']['num_execution_postures'], self.max_num_pose)) for pidx in range(int(np)): pose_name = self.data['postures'].keys()[ self.posture_sel[pidx]] theta_des = self.data['postures'][pose_name] thetadot_avg = self.data['thetadot_avg'][pose_name] self.chain.add_splined_traj_via_deg( theta_des, thetadot_avg) self.chain.add_splined_traj_via_deg( self.data['param']['posture_return'], self.data['param']['posture_return_speed']) if False: for jidx in range(5): if self.mode[jidx]: self.chain.set_mode_theta_gc(jidx) else: self.chain.set_mode_off(jidx) self.pose_idx = 0 self.ts_anim = time.time() if False: #self.running: pose_name = self.data['postures'].keys()[self.posture_sel[ self.pose_idx]] theta_des = self.data['postures'][pose_name] self.chain.set_theta_deg(theta_des) if time.time() - self.ts_anim > 3.0: self.ts_anim = time.time() np = max( 0, min(self.data['param']['num_execution_postures'], self.max_num_pose)) self.pose_idx = self.pose_idx + 1 if self.pose_idx >= np: self.pose_idx = -1 self.running = False print 'Pose', self.pose_idx if self.running: if self.chain.is_splined_traj_complete(): self.running = False print 'Splined traj. complete.' if not self.running: self.chain.set_theta_deg(self.theta_desire) self.run_last = self.run
def do_dump(proxy): print 'Starting dump...' print 'Enter log name [foo]' logname = m3t.get_string('foo') ns = proxy.get_log_num_samples(logname) if ns == 0: return print '-------- Available components --------' names = proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [', names[0], ']' name = m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ', ns print 'Enter start sample idx: [0]' start = max(0, m3t.get_int(0)) print 'Enter end sample idx: [', ns - 1, ']' end = min(ns - 1, m3t.get_int(ns - 1)) print '--------------------------------------' comp = mcf.create_component(name) proxy.register_log_component(comp) fields = [] print 'Dump all data?[n]' if m3t.get_yes_no('n'): '''print 'Fetching data...' fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200)) f.close()''' fields = comp.status.DESCRIPTOR.fields_by_name.keys() print fields else: print ' Select fields to plot...' while True: fields.append(m3t.user_select_msg_field(comp.status)) print 'Another field [n]?' if not m3t.get_yes_no('n'): break print 'Fetching data...' data = {} for k in fields: data[k] = [] print '---------------------------' print k print m3t.get_msg_field_value(comp.status, k) print dir(m3t.get_msg_field_value(comp.status, k)) if hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'): for j in range(len(m3t.get_msg_field_value(comp.status, k))): data[k].append([]) for i in range(start, end): proxy.load_log_sample(logname, i) for k in fields: repeated = False # skip protobuf subclasses (like base) for now. note we'll want this for humanoid if hasattr(m3t.get_msg_field_value(comp.status, k), '__metaclass__'): pass elif hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'): for j in range(len(m3t.get_msg_field_value(comp.status, k))): data[k][j].append( m3t.get_msg_field_value(comp.status, k)[j]) else: data[k].append(m3t.get_msg_field_value(comp.status, k)) fn = m3t.get_m3_log_path() + logname + '/' + logname + '_dump.yaml' f = file(fn, 'w') print 'Saving...', fn print data f.write(yaml.safe_dump(data, default_flow_style=False, width=200)) f.close()
from m3.eeprom import * import m3.toolbox as m3t import time from struct import pack print 'Use this tool to overwrite the EEPROM serial number of an EtherCAT slave' config_path=os.environ['M3_ROBOT']+'/robot_config/eeprom/' print '--------------------------- Slaves -------------------------------' os.system('sudo ethercat slaves') print '------------------------------------------------------------------' print 'Enter slave id' sid=m3t.get_int() print 'Enter serial number' sn=m3t.get_int() print 'Enter slave name (eg MA1J0 or EL4132)' name=m3t.get_string() #Read in eeprom cmd='sudo ethercat sii_read -p'+str(sid) stdout_handle = os.popen(cmd, 'r') eep=stdout_handle.read() if len(eep)==0: print 'Unable to read slave EEPROM.' exit() #Write orig to file fn=config_path+'eeprom_'+name+'_sn_'+str(sn)+'_orig.hex' out_port = open_binary_output_file(fn) for c in eep: write_char(out_port,c) out_port.close() #Update binary sn field
def step(self): self.proxy.step() self.status_dict=self.proxy.get_status_dict() self.chain.set_stiffness(self.data['param']['stiffness']) self.chain.set_slew_rate_proportion(self.data['param']['q_slew_rate']) #Do power Grasp if self.grasp and not self.grasp_last: if self.mode[0]: self.chain.set_mode_theta_gc([0]) if self.mode[1]: self.chain.set_mode_torque_gc([1]) if self.mode[2]: self.chain.set_mode_torque_gc([2]) if self.mode[3]: self.chain.set_mode_torque_gc([3]) if self.mode[4]: self.chain.set_mode_torque_gc([4]) self.chain.set_theta_deg(self.chain.get_theta_deg()) self.chain.set_torque_mNm(self.data['param']['grasp_torque']) #if False: #theta open #if not self.grasp and self.grasp_last: #force open #self.grasp_off=True #self.chain.set_torque_mNm(self.data['param']['grasp_torque_off']) #self.grasp_off_ts=time.time() #if self.grasp_off and time.time()-self.grasp_off_ts>2.0: #Open #self.grasp_off=False self.grasp_last=self.grasp #Do joint theta control #if not self.grasp and not self.grasp_off and not self.running: #force open if not self.grasp and not self.running: #theta open for jidx in range(5): if self.mode[jidx]: if jidx==0: self.chain.set_mode_theta(jidx) else: self.chain.set_mode_theta_gc(jidx) else: self.chain.set_mode_off(jidx) #Record posture if self.record and not self.record_last and not self.grasp and not self.grasp_off: print 'Enter pose name: (q to quit)' while True: name=m3t.get_string() if self.data['postures'].has_key(name): print 'Pose ',name,'already exists. Overwrite? ' if m3t.get_yes_no(): break else: break if name!='q' and name !='Q': p=self.chain.get_theta_deg() self.data['postures'][name]=[float(x) for x in p] self.data['thetadot_avg'][name]=[100,100,100,100,100] print 'Posture',name,': ',p else: print 'Record aborted' self.record_last=self.record if self.write and not self.write_last: print 'Writing ',self.posture_filename f=file(self.posture_filename,'w') f.write(yaml.safe_dump(self.data,width=200)) f.close() self.write_last=self.write #Run Animator if not self.run_last and self.run and not self.running: self.running=True self.pose_idx=-1 self.end_time=0 self.start_time=0 if True: for jidx in range(5): if self.mode[jidx]: if jidx==0: self.chain.set_mode_splined_traj(jidx) else: self.chain.set_mode_splined_traj_gc(jidx) else: self.chain.set_mode_off(jidx) np=max(0,min(self.data['param']['num_execution_postures'],self.max_num_pose)) for pidx in range(int(np)): pose_name=self.data['postures'].keys()[self.posture_sel[pidx]] theta_des=self.data['postures'][pose_name] thetadot_avg=self.data['thetadot_avg'][pose_name] self.chain.add_splined_traj_via_deg(theta_des,thetadot_avg) self.chain.add_splined_traj_via_deg(self.data['param']['posture_return'], self.data['param']['posture_return_speed']) if False: for jidx in range(5): if self.mode[jidx]: self.chain.set_mode_theta_gc(jidx) else: self.chain.set_mode_off(jidx) self.pose_idx=0 self.ts_anim=time.time() if False: #self.running: pose_name=self.data['postures'].keys()[self.posture_sel[self.pose_idx]] theta_des=self.data['postures'][pose_name] self.chain.set_theta_deg(theta_des) if time.time()-self.ts_anim>3.0: self.ts_anim=time.time() np=max(0,min(self.data['param']['num_execution_postures'],self.max_num_pose)) self.pose_idx=self.pose_idx+1 if self.pose_idx>=np: self.pose_idx=-1 self.running=False print 'Pose',self.pose_idx if self.running: if self.chain.is_splined_traj_complete(): self.running=False print 'Splined traj. complete.' if not self.running: self.chain.set_theta_deg(self.theta_desire) self.run_last=self.run
from m3.eeprom import * import m3.toolbox as m3t import time from struct import pack print 'Use this tool to overwrite the EEPROM serial number of an EtherCAT slave' config_path = os.environ['M3_ROBOT'] + '/robot_config/eeprom/' print '--------------------------- Slaves -------------------------------' os.system('sudo ethercat slaves') print '------------------------------------------------------------------' print 'Enter slave id' sid = m3t.get_int() print 'Enter serial number' sn = m3t.get_int() print 'Enter slave name (eg MA1J0 or EL4132)' name = m3t.get_string() #Read in eeprom cmd = 'sudo ethercat sii_read -p' + str(sid) stdout_handle = os.popen(cmd, 'r') eep = stdout_handle.read() if len(eep) == 0: print 'Unable to read slave EEPROM.' exit() #Write orig to file fn = config_path + 'eeprom_' + name + '_sn_' + str(sn) + '_orig.hex' out_port = open_binary_output_file(fn) for c in eep: write_char(out_port, c) out_port.close() #Update binary sn field
def do_dump(proxy): print 'Starting dump...' print 'Enter log name [foo]' logname=m3t.get_string('foo') ns=proxy.get_log_num_samples(logname) if ns==0: return print '-------- Available components --------' names=proxy.get_log_component_names(logname) for i in range(len(names)): print names[i] print '--------------------------------------' print 'Select component: [',names[0],']' name=m3t.get_string(names[0]) print '--------------------------------------' print 'Num samples available: ',ns print 'Enter start sample idx: [0]' start=max(0,m3t.get_int(0)) print 'Enter end sample idx: [',ns-1,']' end=min(ns-1,m3t.get_int(ns-1)) print '--------------------------------------' comp=mcf.create_component(name) proxy.register_log_component(comp) fields=[] print 'Dump all data?[n]' if m3t.get_yes_no('n'): '''print 'Fetching data...' fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200)) f.close()''' fields=comp.status.DESCRIPTOR.fields_by_name.keys() print fields else: print ' Select fields to plot...' while True: fields.append(m3t.user_select_msg_field(comp.status)) print 'Another field [n]?' if not m3t.get_yes_no('n'): break print 'Fetching data...' data={} for k in fields: data[k]=[] print '---------------------------' print k print m3t.get_msg_field_value(comp.status,k) print dir(m3t.get_msg_field_value(comp.status,k)) if hasattr(m3t.get_msg_field_value(comp.status,k),'__len__'): for j in range(len(m3t.get_msg_field_value(comp.status,k))): data[k].append([]) for i in range(start,end): proxy.load_log_sample(logname,i) for k in fields: repeated = False # skip protobuf subclasses (like base) for now. note we'll want this for humanoid if hasattr(m3t.get_msg_field_value(comp.status,k),'__metaclass__'): pass elif hasattr(m3t.get_msg_field_value(comp.status,k),'__len__'): for j in range(len(m3t.get_msg_field_value(comp.status,k))): data[k][j].append(m3t.get_msg_field_value(comp.status,k)[j]) else: data[k].append(m3t.get_msg_field_value(comp.status,k)) fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml' f=file(fn,'w') print 'Saving...',fn print data f.write(yaml.safe_dump(data, default_flow_style=False,width=200)) f.close()