def is_dpm3_ready(self): print 'This routine requires the actuator torque to be calibrated' print 'This routine requires the actuator to be locked with a DPM3 load-cell in-line' print 'Is DPM3 Loadcell present? [y]' if not m3t.get_yes_no('y'): return False print 'Is the DPM3 resolution set at XXXX.X? [y]' if not m3t.get_yes_no('y'): return False print 'Place actuator in unloaded configuration. Enable Power. Hit enter when ready' raw_input() return True
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 calibrate(self,proxy): print '----------------------------------------------------' if not self.get_encoder_calibrated(): print 'Z-Lift encoder not yet calibrated. Calibrate now[y]?' if not m3t.get_yes_no('y'): return False print 'Disengage E-Stop. Hit enter when ready' raw_input() if self.get_limitswitch_neg(): print 'Already at limit, moving up first...' self.set_mode_pwm() self.set_pwm(self.config['pwm_calibration_up']) proxy.step() time.sleep(4.0) print 'Moving down to limit' print 'Manual assist may be required' self.set_mode_pwm() self.set_pwm(self.config['pwm_calibration_down']) proxy.step() for i in range(20): if self.get_encoder_calibrated(): print 'Z-Lift encoder calibrated. Currently at position: ',self.get_pos_mm(),'(mm)' return True print i,' : ', self.get_theta_deg() time.sleep(1.0) proxy.step() self.set_mode_off() proxy.step() if not self.get_encoder_calibrated(): print 'Calibration failed' return False else: print 'Z-Lift encoder calibrated. Currently at position: ',self.get_pos_mm(),'(mm)' return True
def calibrate(self, proxy): """ Calibrates Omnibase casters if necessary. :param proxy: running proxy :type proxy: M3RtProxy """ need_to_calibrate = False for i in range(self.num_casters): if (not self.is_calibrated(i)): need_to_calibrate = True if need_to_calibrate: print '------------------------------------------------' print 'All casters not calibrated. Do calibration [y]?' if m3t.get_yes_no('y'): print 'Note: Orientations are facing robot' print "Turn power on to robot and press any key." raw_input() self.set_mode_caster() proxy.step() time.sleep(4) caster_names = [ 'FrontRight', 'RearRight', 'RearLeft', 'FrontLeft' ] wiggle = [1, 2, 1, 2] last_calib = -1 repeat_calib = 0 while need_to_calibrate: for i in [1, 2, 3, 0]: if (not self.is_calibrated(i)): print '-------------------------------------------' print 'Calibrating caster: ', caster_names[i], '..' #print 'Manual assist required in CCW direction' if i == last_calib: repeat_calib += 1 if repeat_calib == 0: wiggle = [1, 2, 1, 2] self.home(i, proxy, wiggle[i]) elif repeat_calib == 1: wiggle = [3, 0, 3, 0] self.home(i, proxy, wiggle[i]) elif repeat_calib == 2: wiggle = [2, 3, 0, 1] self.home(i, proxy, wiggle[i]) elif repeat_calib >= 3: raise m3t.M3Exception( 'Error calibrating. Please reposition base and try again.' ) last_calib = i need_to_calibrate = False for i in range(self.num_casters): if (not self.is_calibrated(i)): need_to_calibrate = True self.set_mode_caster_off(range(self.num_casters)) self.set_mode_off() else: print "Skipping Calibration.."
def calibrate(self,proxy): """ Calibrates Omnibase casters if necessary. :param proxy: running proxy :type proxy: M3RtProxy """ need_to_calibrate = False for i in range(self.num_casters): if (not self.is_calibrated(i)): need_to_calibrate = True if need_to_calibrate: print '------------------------------------------------' print 'All casters not calibrated. Do calibration [y]?' if m3t.get_yes_no('y'): print 'Note: Orientations are facing robot' print "Turn power on to robot and press any key." raw_input() self.set_mode_caster() proxy.step() time.sleep(4) caster_names=['FrontRight','RearRight','RearLeft','FrontLeft'] wiggle = [1,2,1,2] last_calib = -1 repeat_calib = 0 while need_to_calibrate: for i in [1,2,3,0]: if (not self.is_calibrated(i)): print '-------------------------------------------' print 'Calibrating caster: ', caster_names[i], '..' #print 'Manual assist required in CCW direction' if i == last_calib: repeat_calib += 1 if repeat_calib == 0: wiggle = [1,2,1,2] self.home(i,proxy, wiggle[i]) elif repeat_calib == 1: wiggle = [3,0,3,0] self.home(i,proxy, wiggle[i]) elif repeat_calib == 2: wiggle = [2,3,0,1] self.home(i,proxy, wiggle[i]) elif repeat_calib >= 3: raise m3t.M3Exception('Error calibrating. Please reposition base and try again.') last_calib = i need_to_calibrate = False for i in range(self.num_casters): if (not self.is_calibrated(i)): need_to_calibrate = True self.set_mode_caster_off(range(self.num_casters)) self.set_mode_off() else: print "Skipping Calibration.."
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 write_raw_calibration(self,data): print 'Save raw data [y]?' if m3t.get_yes_no('y'): fn=m3t.get_m3_config_path()[-1]+'data/calibration_data_raw_'+self.comp_ec.name+'.yml' try: f=file(fn,'r') raw_data= yaml.safe_load(f.read()) f.close() except (IOError, EOFError): raw_data={} pass f=file(fn,'w') for k in data.keys(): raw_data[k]=data[k] print 'Saving...',fn f.write(yaml.safe_dump(data, default_flow_style=False,width=200)) f.close()
def batch_write(bld, boards): print "Ready to write firmware." print "Prompt to write each board?[y]" prompt = m3t.get_yes_no("y") for k in boards.keys(): if boards[k]["connected"]: print "Writing", boards[k]["firmware"], "to ", k if prompt: print "Hit enter to begin" raw_input() if bld.WriteProgramMemory(boards[k]["slave_id"], boards[k]["firmware"]): print "Write success for", k boards[k]["write_success"] = True else: print "Write fail for", k, ". Disconnecting..." boards[k]["connected"] = False boards[k]["write_success"] = False return boards
def batch_write(bld, boards): print 'Ready to write firmware.' print 'Prompt to write each board?[y]' prompt = m3t.get_yes_no('y') for k in boards.keys(): if boards[k]['connected']: print 'Writing', boards[k]['firmware'], 'to ', k if prompt: print 'Hit enter to begin' raw_input() if bld.WriteProgramMemory(boards[k]['slave_id'], boards[k]['firmware']): print 'Write success for', k boards[k]['write_success'] = True else: print 'Write fail for', k, '. Disconnecting...' boards[k]['connected'] = False boards[k]['write_success'] = False return boards
def test_torque_banger(self, auto=False): d = 30.0 if not auto: print '------------- Test Conditions --------------' print '1. Actuator on test-plate' print '2. Rocker arm installed' print '3. PID tuning complete for actuator_ec' print '4. Rocker arm unloaded' print 'Continue? [y]' if not m3t.get_yes_no('y'): return print 'Enter run duration (s) [30.0]' d = m3t.get_float(30.0) print 'Enable power. Hit enter to continue' raw_input() self.step() amplitude = self.test_default[self.jid]['test_banger_amp'] adc_zero = float( self.get_sensor_list_avg(['adc_torque'], 1.0, auto=True, verbose=False)['adc_torque']) print 'Zero of ', adc_zero self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE self.comp_ec.command.t_desire = int(adc_zero) self.step(1.0) nc = 2 period = 5.0 start = time.time() dt = 0 self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_TORQUE while dt < d: dt = time.time() - start a = amplitude[self.jid] * math.sin(2 * math.pi * dt / period) self.comp_ec.command.t_desire = int(adc_zero + a) self.step(time_sleep=0.1) print '----------' print 'DT', dt print 'Amplitude', a print 'Current (mA)', self.comp_rt.get_current_mA() self.comp_ec.command.mode = aec.ACTUATOR_EC_MODE_OFF self.step()
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 calibrate(self, proxy): print '----------------------------------------------------' if not self.get_encoder_calibrated(): print 'Z-Lift encoder not yet calibrated. Calibrate now[y]?' if not m3t.get_yes_no('y'): return False print 'Disengage E-Stop. Hit enter when ready' raw_input() if self.get_limitswitch_neg(): print 'Already at limit, moving up first...' self.set_pwm(self.config['pwm_calibration_up']) self.set_mode_pwm() proxy.step() time.sleep(4.0) print 'Moving down to limit' print 'Manual assist may be required' self.set_pwm(self.config['pwm_calibration_down']) self.set_mode_pwm() proxy.step() for i in range(200): if self.get_encoder_calibrated(): self.set_pwm(self.config['pwm_calibration_hold']) proxy.step() print 'Z-Lift encoder calibrated. Currently at position: ', self.get_pos_mm( ), '(mm)' return True print i, ' : ', self.get_theta_deg( ), 'pwm: ', self.status.pwm_cmd time.sleep(.1) proxy.step() self.set_mode_off() proxy.step() if not self.get_encoder_calibrated(): print 'Calibration failed' return False else: print 'Z-Lift encoder calibrated. Currently at position: ', self.get_pos_mm( ), '(mm)' return True
def test_torque_banger(self,auto=False): d=30.0 if not auto: print '------------- Test Conditions --------------' print '1. Actuator on test-plate' print '2. Rocker arm installed' print '3. PID tuning complete for actuator_ec' print '4. Rocker arm unloaded' print 'Continue? [y]' if not m3t.get_yes_no('y'): return print 'Enter run duration (s) [30.0]' d=m3t.get_float(30.0) print 'Enable power. Hit enter to continue' raw_input() self.step() amplitude=self.test_default[self.jid]['test_banger_amp'] adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0,auto=True,verbose=False)['adc_torque']) print 'Zero of ',adc_zero self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE self.comp_ec.command.t_desire=int(adc_zero) self.step(1.0) nc=2 period=5.0 start=time.time() dt=0 self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE while dt<d: dt=time.time()-start a=amplitude[self.jid]*math.sin(2*math.pi*dt/period) self.comp_ec.command.t_desire=int(adc_zero+a) self.step(time_sleep=0.1) print '----------' print 'DT',dt print 'Amplitude',a print 'Current (mA)',self.comp_rt.get_current_mA() self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF self.step()
def start(self): # ######## Setup Proxy and Components ######################### self.proxy.start() self.current_first = True bot_name = m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_names return self.bot = m3.humanoid.M3Humanoid(bot_name) arm_names = self.bot.get_available_chains() arm_names = [x for x in arm_names if x.find('arm') != -1] if len(arm_names) == 0: print 'No arms found' return if len(arm_names) == 1: self.arm_name = arm_names[0] else: self.arm_name = m3t.user_select_components_interactive( arm_names, single=True)[0] self.jnts = self.bot.get_joint_names(self.arm_name) comp = {} for c in self.jnts: comp[c] = { 'comp_rt': None, 'comp_j': None, 'torque_act': [], 'torque_joint': [], 'torque_gravity': [], 'is_wrist': False } if (c.find('j5') >= 0 or c.find('j6') >= 0): comp[c]['is_wrist'] = True for c in self.jnts: comp[c]['comp_j'] = mcf.create_component(c) comp[c]['comp_rt'] = mcf.create_component( c.replace('joint', 'actuator')) self.proxy.subscribe_status(comp[c]['comp_rt']) self.proxy.subscribe_status(comp[c]['comp_j']) # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof = self.bot.get_num_dof(self.arm_name) humanoid_shm_names = self.proxy.get_available_components( 'm3humanoid_shm') if len(humanoid_shm_names) > 0: self.proxy.make_safe_operational(humanoid_shm_names[0]) self.bot.set_mode_off(self.arm_name) print 'This script will calibrate the zero-torque of the A2 arm while posed using current torque_gravity estimate' print '----------------------------------------------------------------------------------------------------------' print 'With E-Stop down, pose the arm in desired position to calibrate torque zeroes around.' print 'Press any key when ready posed.' raw_input() time.sleep(0.5) self.proxy.step() self.theta_curr = self.bot.get_theta_deg(self.arm_name)[:] self.proxy.step() print 'Posed position set.' print 'Release E-stop and press any key for arm to hold pose.' raw_input() self.bot.set_mode_theta_gc(self.arm_name) self.bot.set_theta_deg(self.arm_name, self.theta_curr) self.bot.set_stiffness(self.arm_name, [1.0] * 7) self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * self.ndof) self.proxy.step() print 'Press any key to start torque calibration for all joints.' raw_input() self.proxy.step() # ########################### ns = 30 for i in range(ns): self.proxy.step() print '---------' for c in comp.keys(): tqj = comp[c]['comp_j'].get_torque_mNm() tqg = comp[c]['comp_j'].get_torque_gravity_mNm() / 1000.0 tqa = comp[c]['comp_rt'].get_torque_mNm() comp[c]['torque_act'].append(tqa) comp[c]['torque_joint'].append(tqj) comp[c]['torque_gravity'].append(tqg) if comp[c]['is_wrist']: print c, ':joint', tqj, ':gravity', tqg, ':actuator', tqa else: print c, ':joint', tqj, ':gravity', tqg, time.sleep(0.05) do_query = True # ########################### for c in comp.keys(): print '--------', c, '---------' tqg = float(na.array(comp[c]['torque_gravity'], na.Float32).mean()) tqj = float(na.array(comp[c]['torque_joint'], na.Float32).mean()) tqa = float(na.array(comp[c]['torque_act'], na.Float32).mean()) if not comp[c]['is_wrist']: bias = tqa + tqg torque = M3TorqueSensor( comp[c]['comp_rt'].config['calib']['torque']['type']) print 'Measured torque:', tqa, 'Torque gravity:', tqg print 'Delta of', bias, 'mNm' comp[c]['comp_rt'].config['calib']['torque']['cb_bias'] = comp[ c]['comp_rt'].config['calib']['torque']['cb_bias'] - bias comp[c]['comp_rt'].config['calibration_date'] = time.asctime() if do_query: print 'Save calibration? [y]' if m3t.get_yes_no('y'): comp[c]['comp_rt'].write_config() else: comp[c]['comp_rt'].write_config() else: print 'Wrist joint...' if c.find('j5') != -1: #do j5/j6 at once cc = None for x in comp.keys(): if x.find('j6') != -1: cc = x if cc is None: print 'Did not find coupled joint to', c tqg_c = float( na.array(comp[cc]['torque_gravity'], na.Float32).mean()) tqj_c = float( na.array(comp[cc]['torque_joint'], na.Float32).mean()) tqa_c = float( na.array(comp[cc]['torque_act'], na.Float32).mean()) x = comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][ 0] #Joint to actuator matrix y = comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][ 1] m = comp[cc]['comp_j'].config['transmission'][ 'tqj_to_tqa'][0] n = comp[cc]['comp_j'].config['transmission'][ 'tqj_to_tqa'][1] tqg_a5 = x * tqg + y * tqg_c tqg_a6 = m * tqg_c + n * tqg bias_5 = tqa + tqg_a5 bias_6 = tqa_c + tqg_a6 torque_5 = M3TorqueSensor( comp[c]['comp_rt'].config['calib']['torque']['type']) torque_6 = M3TorqueSensor( comp[cc]['comp_rt'].config['calib']['torque']['type']) print '------------' print 'J5: Previous joint torque', tqj, 'with joint torque gravity', tqg print 'J5: Previous actuator torque', tqa, 'with actuator torque gravity', tqg_a5 print 'J5: Actuator delta of', bias_5, 'mNm' print '------------' print 'J6: Previous joint torque', tqj_c, 'with joint torque gravity', tqg_c print 'J6: Previous actuator torque', tqa_c, 'with actuator torque gravity', tqg_a6 print 'J6: Actuator delta of', bias_6, 'mNm' print '------------' comp[c]['comp_rt'].config['calib']['torque'][ 'cb_bias'] = comp[c]['comp_rt'].config['calib'][ 'torque']['cb_bias'] - bias_5 comp[c]['comp_rt'].config[ 'calibration_date'] = time.asctime() comp[cc]['comp_rt'].config['calib']['torque'][ 'cb_bias'] = comp[cc]['comp_rt'].config['calib'][ 'torque']['cb_bias'] - bias_6 comp[cc]['comp_rt'].config[ 'calibration_date'] = time.asctime() if do_query: print 'Save calibration? [y]' if m3t.get_yes_no('y'): comp[c]['comp_rt'].write_config() comp[cc]['comp_rt'].write_config() else: comp[c]['comp_rt'].write_config() comp[cc]['comp_rt'].write_config() self.bot.set_mode_off(self.arm_name) self.proxy.stop()
def get_boards(): print "Finding burnable board from m3_config.yml..." name_ec = m3t.get_ec_component_names() boards = {} for n in name_ec: config = m3t.get_component_config(n) try: fw = config["firmware"] + ".hex" except KeyError: print "Missing firmware key for", n, "...skipping" fw = None sn = config["ethercat"]["serial_number"] try: chid = config["chid"] except KeyError: chid = 0 ssn = get_slave_sn() sid = None for s in ssn: if sn == s[0]: sid = s[1] ff = m3t.get_m3_config_path() + "eeprom/eeprom*_sn_" + str(sn) + ".hex" eepf = glob.glob(ff) if len(eepf) == 0: print "EEPROM file not found", ff, "skipping board..." if len(eepf) > 1: print "Multiple eeproms found for", ff print "Select correct eeprom ID" for i in range(len(eepf)): print i, " : ", eepf[i] idx = m3t.get_int() eepf = [eepf[idx]] if sid != None and len(eepf) == 1 and fw is not None and chid == 0: boards[n] = { "firmware": fw, "sn": sn, "slave_id": sid, "eeprom": eepf[0], "connected": False, "write_success": False, } print "Found the following valid configurations: " print boards.keys() print print "Selection type" print "-----------------" print "a: all boards" print "s: single board" print "m: multiple boards" t = raw_input() if t != "a" and t != "s" and t != "m": print "Invalid selection" return {} rem = [] if t == "m": for k in boards.keys(): print "-----------------------" print "Burn ", k, "board [y]?" if not m3t.get_yes_no("y"): rem.append(k) for r in rem: boards.pop(r) if t == "s": k = m3t.user_select_components_interactive(boards.keys(), single=True) if len(k) == 0: return {} boards = {k[0]: boards[k[0]]} print "-------------------------------" print "Burning the following boards: " for k in boards.keys(): print k return boards
def get_boards(): print 'Finding burnable board from m3_config.yml...' name_ec = m3t.get_ec_component_names() boards = {} for n in name_ec: config = m3t.get_component_config(n) try: fw = config['firmware'] + '.hex' except KeyError: print 'Missing firmware key for', n, '...skipping' fw = None sn = config['ethercat']['serial_number'] try: chid = config['chid'] except KeyError: chid = 0 ssn = get_slave_sn() sid = None for s in ssn: if sn == s[0]: sid = s[1] ff = m3t.get_m3_config_path() + 'eeprom/eeprom*_sn_' + str(sn) + '.hex' eepf = glob.glob(ff) if len(eepf) == 0: print 'EEPROM file not found', ff, 'skipping board...' if len(eepf) > 1: print 'Multiple eeproms found for', ff print 'Select correct eeprom ID' for i in range(len(eepf)): print i, ' : ', eepf[i] idx = m3t.get_int() eepf = [eepf[idx]] if sid != None and len(eepf) == 1 and fw is not None and chid == 0: boards[n] = { 'firmware': fw, 'sn': sn, 'slave_id': sid, 'eeprom': eepf[0], 'connected': False, 'write_success': False } print 'Found the following valid configurations: ' print boards.keys() print print 'Selection type' print '-----------------' print 'a: all boards' print 's: single board' print 'm: multiple boards' t = raw_input() if t != 'a' and t != 's' and t != 'm': print 'Invalid selection' return {} rem = [] if t == 'm': for k in boards.keys(): print '-----------------------' print 'Burn ', k, 'board [y]?' if not m3t.get_yes_no('y'): rem.append(k) for r in rem: boards.pop(r) if t == 's': k = m3t.user_select_components_interactive(boards.keys(), single=True) if len(k) == 0: return {} boards = {k[0]: boards[k[0]]} print '-------------------------------' print 'Burning the following boards: ' for k in boards.keys(): print k return boards
'M3Humanoid':{'generate':m3r.generate_humanoid}}#, #'M3.RViz':{'generate':m3f.generate_rviz}} kk=ctypes.keys() kk.sort() print 'Select configuration type' for i in range(len(kk)): print i,':',kk[i] ctype=kk[m3t.get_int()] gen=ctypes[ctype]['generate'] try: config_dir, config_data=apply(gen) print '---------------------------------' print 'Config Dir',config_dir print print 'Config Data',config_data print print '---------------------------------' print print 'Save generated data [y]?' if m3t.get_yes_no('y'): create_config_file(config_dir,config_data) except (KeyboardInterrupt,EOFError): print 'Exception. Exiting now...'
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 hsn = pack('H', sn) eep2 = eep[:28] + hsn + eep[30:] #Write to file fn2 = config_path + 'eeprom_' + name + '_sn_' + str(sn) + '.hex' out_port = open_binary_output_file(fn2) for c in eep2: write_char(out_port, c) out_port.close() #Write to slave print 'Write to slave [y]?' if m3t.get_yes_no('y'): cmd = 'sudo ethercat -p ' + str(sid) + ' sii_write ' + fn2 print 'Executing: ', cmd os.system(cmd) print 'Power cycle and hit return' raw_input() time.sleep(4.0) print '--------------------------- Slaves -------------------------------' os.system('sudo ethercat slaves') print '------------------------------------------------------------------'
def start(self): self.proxy.start() self.proxy.make_operational_all() chain_names=self.proxy.get_available_components('m3hand') if len(chain_names)>1: hand_name=m3t.user_select_components_interactive(chain_names,single=True) else: hand_name=chain_names pwr_name=self.proxy.get_available_components('m3pwr') if len(pwr_name)>1: pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) print 'Position arm [y]?' if m3t.get_yes_no('y'): arm_names=self.proxy.get_available_components('m3arm') if len(arm_names)>1: print 'Select arm: ' arm_name=m3t.user_select_components_interactive(arm_names,single=True)[0] else: arm_name=arm_names[0] self.arm=m3f.create_component(arm_name) self.proxy.publish_command(self.arm) self.arm.set_mode_theta_gc() self.arm.set_theta_deg([30,0,0,110,0,0,0]) self.arm.set_stiffness(0.5) self.arm.set_slew_rate_proportion([0.75]*7) self.chain=m3f.create_component(hand_name[0]) self.proxy.publish_command(self.chain) self.proxy.subscribe_status(self.chain) self.pwr=m3f.create_component(pwr_name[0]) self.proxy.publish_command(self.pwr) self.pwr.set_motor_power_on() #Force safe-op of robot if present hum=self.proxy.get_available_components('m3humanoid') if len(hum)>0: self.proxy.make_safe_operational(hum[0]) #Setup postures self.posture_filename=m3t.get_m3_animation_path()+self.chain.name+'_postures.yml' f=file(self.posture_filename,'r') self.data= yaml.safe_load(f.read()) self.param=self.data['param'] f.close() self.theta_desire=[0,0,0,0,0] self.mode=[1,1,1,1,1] #Create gui self.run=False self.run_last=False self.running=False self.grasp=False self.grasp_last=False self.grasp_off=False self.grasp_off_ts=time.time() self.status_dict=self.proxy.get_status_dict() self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3) self.gui.add('M3GuiTree', 'Param', (self,'param'),[],[],m3g.M3GuiWrite,column=3) self.gui.add('M3GuiToggle', 'Animation', (self,'run'),[],[['Run','Stop']],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiModes', 'Joint', (self,'mode'),range(5),[['Off','Enabled'],1],m3g.M3GuiWrite,column=2) self.gui.add('M3GuiSliders','Theta (Deg)', (self,'theta_desire'),range(5),[0,300],m3g.M3GuiWrite,column=2) self.gui.add('M3GuiToggle', 'Power Grasp', (self,'grasp'),[],[['Run','Stop']],m3g.M3GuiWrite,column=2) self.gui.start(self.step)
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()
print "-----------------------" for i in range(len(via_names)): print i, " : ", via_names[i] idx = m3t.get_int(0) if idx >= 0 and idx < len(via_names): f = file(via_files[idx], "r") d = yaml.safe_load(f.read()) return d return None # ###################################################### print "Enable RVIZ? [n]" pub = None rviz = m3t.get_yes_no("n") # ###################################################### proxy = m3p.M3RtProxy() proxy.start() bot_name = m3t.get_robot_name() if bot_name == "": print "Error: no robot components found:", bot_name quit bot = m3f.create_component(bot_name) if rviz == True: viz = m3v.M3Viz(proxy, bot) proxy.subscribe_status(bot) proxy.publish_command(bot)
def main(): proxy = m3p.M3RtProxy() proxy.start() base_name=proxy.get_available_components('m3omnibase') omni=None if len(base_name)!=1: print 'Omnibase not available. Proceeding without it...' else: print 'Use OmniBase [y]?' if m3t.get_yes_no('y'): omni=m3o.M3OmniBase(base_name[0]) proxy.publish_param(omni) # we need this for calibration proxy.subscribe_status(omni) proxy.publish_command(omni) pwr_name=[m3t.get_omnibase_pwr_component_name(base_name[0])] #proxy.get_available_components('m3pwr') #if len(pwr_name)>1: #pwr_name=m3t.user_select_components_interactive(pwr_name,single=True) pwr=m3f.create_component(pwr_name[0]) proxy.subscribe_status(pwr) proxy.publish_command(pwr) zlift_names=proxy.get_available_components('m3joint_zlift') zl=None if len(zlift_names)==1: print 'Use Zlift [y]?' if m3t.get_yes_no('y'): zl=m3z.M3JointZLift(zlift_names[0]) proxy.subscribe_status(zl) proxy.publish_command(zl) proxy.publish_param(zl) proxy.make_operational(pwr_name[0]) proxy.step() if omni is not None: omni.set_mode_off() pwr.set_motor_power_on() proxy.make_operational_all() zlift_shm_names=proxy.get_available_components('m3joint_zlift_shm') if len(zlift_shm_names) > 0: proxy.make_safe_operational(zlift_shm_names[0]) omnibase_shm_names=proxy.get_available_components('m3omnibase_shm') if len(omnibase_shm_names) > 0: proxy.make_safe_operational(omnibase_shm_names[0]) humanoid_shm_names=proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: proxy.make_safe_operational(humanoid_shm_names[0]) m3ledx2xn_ec_shm_names=proxy.get_available_components('m3ledx2xn_ec_shm') if len(m3ledx2xn_ec_shm_names) > 0: proxy.make_safe_operational(m3ledx2xn_ec_shm_names[0]) m3led_matrix_ec_shm_names=proxy.get_available_components('m3led_matrix_ec_shm') if len(m3led_matrix_ec_shm_names) > 0: proxy.make_safe_operational(m3led_matrix_ec_shm_names[0]) proxy.step() time.sleep(0.5) if omni is not None: proxy.step() omni.calibrate(proxy) time.sleep(0.5) if zl is not None: if not zl.calibrate(proxy): zl=None print 'ZLift failed to calibrate' if omni is None and zl is None: exit() print "Turn motor power on to Omnibase and press any key." raw_input() joy=m3to.M3OmniBaseJoy() joy.start(proxy,omni,zl) k = 0 try: while True: joy.step() #print 'Bus Current:', pwr.get_bus_torque() p = omni.get_local_position() #omni.set_op_space_forces(f.jx*200.0, f.jy*200.0, f.jyaw*50.0) k += 1 if k == 100: print '-----------Local Pos-------' print 'X:', p[0] print 'Y:', p[1] print 'Yaw:', p[2] #print '---------------------------' k = 0 '''print '-------Joystick Pos-------' print 'jx:',f.jx print 'jy:', f.jy print 'jyaw:', f.jyaw print 'button:', f.jbutton print '---------------------------' #print 'Bus voltage',omni.get_bus_voltage()''' '''tqs = omni.get_motor_torques() print tqs[0], tqs[2], tqs[4], tqs[6]''' #print omni.get_steer_torques() proxy.step() except KeyboardInterrupt: pass joy.stop() if omni is not None: omni.set_mode_off() pwr.set_motor_power_off() proxy.step() proxy.stop()
import numpy.numarray as na import math from m3qa.calibrate_sensors import * # ########################### fields=[['adc_ext_temp'],['adc_amp_temp'],['adc_current_a','adc_current_b']] print 'Select sensor to zero' for i in range(len(fields)): print i,' : ',fields[i] sidx=m3t.get_int() sensor=fields[sidx] print 'This assumes ISS version config files' print 'All boards should be at room temp' print 'All motor powers should be off' print 'Continue [y]?' if not m3t.get_yes_no('y'): exit() if sensor[0]=='adc_ext_temp' or sensor[0]=='adc_amp_temp': print 'Enter room temp (C)' ambient=m3t.get_float() # ########################### proxy = m3p.M3RtProxy() proxy.start() cnames=proxy.get_available_components() comp_ec=[] comp_rt=[] cnames=[q for q in cnames if q.find('actuator_ec')!=-1] for c in cnames: comp_ec.append(mcf.create_component(c))
#! /usr/bin/python import pylab as pyl import time import m3.rt_proxy as m3p import m3.toolbox as m3t import m3.component_factory as mcf import numpy.numarray as na import math proxy = m3p.M3RtProxy() proxy.start() cnames = proxy.get_available_components() print 'All actuator_ec [n]?' comps = [] if m3t.get_yes_no('n'): cnames = [q for q in cnames if q.find('actuator_ec') != -1] all_aec = True for c in cnames: comps.append(mcf.create_component(c)) else: all_aec = False print '------- Components ------' for i in range(len(cnames)): print i, ' : ', cnames[i] print '-------------------------' print 'Enter component id' cid = m3t.get_int() name = cnames[cid] comps.append(mcf.create_component(name)) if len(comps) == 0:
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 start(self): # ######## Setup Proxy and Components ######################### self.proxy.start() self.current_first = True bot_name=m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_names return self.bot=m3.humanoid.M3Humanoid(bot_name) arm_names = self.bot.get_available_chains() arm_names = [x for x in arm_names if x.find('arm')!=-1] if len(arm_names)==0: print 'No arms found' return if len(arm_names)==1: self.arm_name=arm_names[0] else: self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0] self.jnts = self.bot.get_joint_names(self.arm_name) comp={} for c in self.jnts: comp[c]={'comp_rt':None,'comp_j':None,'torque_act':[],'torque_joint':[],'torque_gravity':[],'is_wrist':False} if (c.find('j5')>=0 or c.find('j6')>=0): comp[c]['is_wrist']=True for c in self.jnts: comp[c]['comp_j']=mcf.create_component(c) comp[c]['comp_rt']=mcf.create_component(c.replace('joint','actuator')) self.proxy.subscribe_status(comp[c]['comp_rt']) self.proxy.subscribe_status(comp[c]['comp_j']) # ####### Setup Proxy ############# self.proxy.subscribe_status(self.bot) self.proxy.publish_command(self.bot) self.proxy.make_operational_all() self.bot.set_motor_power_on() self.ndof=self.bot.get_num_dof(self.arm_name) humanoid_shm_names=self.proxy.get_available_components('m3humanoid_shm') if len(humanoid_shm_names) > 0: self.proxy.make_safe_operational(humanoid_shm_names[0]) self.bot.set_mode_off(self.arm_name) print 'This script will calibrate the zero-torque of the A2 arm while posed using current torque_gravity estimate' print '----------------------------------------------------------------------------------------------------------' print 'With E-Stop down, pose the arm in desired position to calibrate torque zeroes around.' print 'Press any key when ready posed.' raw_input() time.sleep(0.5) self.proxy.step() self.theta_curr = self.bot.get_theta_deg(self.arm_name)[:] self.proxy.step() print 'Posed position set.' print 'Release E-stop and press any key for arm to hold pose.' raw_input() self.bot.set_mode_theta_gc(self.arm_name) self.bot.set_theta_deg(self.arm_name,self.theta_curr) self.bot.set_stiffness(self.arm_name,[1.0]*7) self.bot.set_slew_rate_proportion(self.arm_name,[1.0]*self.ndof) self.proxy.step() print 'Press any key to start torque calibration for all joints.' raw_input() self.proxy.step() # ########################### ns=30 for i in range(ns): self.proxy.step() print '---------' for c in comp.keys(): tqj=comp[c]['comp_j'].get_torque_mNm() tqg=comp[c]['comp_j'].get_torque_gravity_mNm()/1000.0 tqa=comp[c]['comp_rt'].get_torque_mNm() comp[c]['torque_act'].append(tqa) comp[c]['torque_joint'].append(tqj) comp[c]['torque_gravity'].append(tqg) if comp[c]['is_wrist']: print c,':joint',tqj,':gravity',tqg,':actuator',tqa else: print c,':joint',tqj,':gravity',tqg, time.sleep(0.05) do_query = True # ########################### for c in comp.keys(): print '--------',c,'---------' tqg=float(na.array(comp[c]['torque_gravity'],na.Float32).mean()) tqj=float(na.array(comp[c]['torque_joint'],na.Float32).mean()) tqa=float(na.array(comp[c]['torque_act'],na.Float32).mean()) if not comp[c]['is_wrist']: bias=tqa+tqg torque=M3TorqueSensor(comp[c]['comp_rt'].config['calib']['torque']['type']) print 'Measured torque:',tqa,'Torque gravity:', tqg print 'Delta of',bias,'mNm' comp[c]['comp_rt'].config['calib']['torque']['cb_bias']=comp[c]['comp_rt'].config['calib']['torque']['cb_bias']-bias comp[c]['comp_rt'].config['calibration_date']=time.asctime() if do_query: print 'Save calibration? [y]' if m3t.get_yes_no('y'): comp[c]['comp_rt'].write_config() else: comp[c]['comp_rt'].write_config() else: print 'Wrist joint...' if c.find('j5')!=-1: #do j5/j6 at once cc=None for x in comp.keys(): if x.find('j6')!=-1: cc=x if cc is None: print 'Did not find coupled joint to',c tqg_c=float(na.array(comp[cc]['torque_gravity'],na.Float32).mean()) tqj_c=float(na.array(comp[cc]['torque_joint'],na.Float32).mean()) tqa_c=float(na.array(comp[cc]['torque_act'],na.Float32).mean()) x=comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][0] #Joint to actuator matrix y=comp[c]['comp_j'].config['transmission']['tqj_to_tqa'][1] m=comp[cc]['comp_j'].config['transmission']['tqj_to_tqa'][0] n=comp[cc]['comp_j'].config['transmission']['tqj_to_tqa'][1] tqg_a5= x*tqg+y*tqg_c tqg_a6= m*tqg_c+n*tqg bias_5=tqa+tqg_a5 bias_6=tqa_c+tqg_a6 torque_5=M3TorqueSensor(comp[c]['comp_rt'].config['calib']['torque']['type']) torque_6=M3TorqueSensor(comp[cc]['comp_rt'].config['calib']['torque']['type']) print '------------' print 'J5: Previous joint torque',tqj,'with joint torque gravity', tqg print 'J5: Previous actuator torque',tqa,'with actuator torque gravity', tqg_a5 print 'J5: Actuator delta of',bias_5,'mNm' print '------------' print 'J6: Previous joint torque',tqj_c,'with joint torque gravity', tqg_c print 'J6: Previous actuator torque',tqa_c,'with actuator torque gravity', tqg_a6 print 'J6: Actuator delta of',bias_6,'mNm' print '------------' comp[c]['comp_rt'].config['calib']['torque']['cb_bias']=comp[c]['comp_rt'].config['calib']['torque']['cb_bias']-bias_5 comp[c]['comp_rt'].config['calibration_date']=time.asctime() comp[cc]['comp_rt'].config['calib']['torque']['cb_bias']=comp[cc]['comp_rt'].config['calib']['torque']['cb_bias']-bias_6 comp[cc]['comp_rt'].config['calibration_date']=time.asctime() if do_query: print 'Save calibration? [y]' if m3t.get_yes_no('y'): comp[c]['comp_rt'].write_config() comp[cc]['comp_rt'].write_config() else: comp[c]['comp_rt'].write_config() comp[cc]['comp_rt'].write_config() self.bot.set_mode_off(self.arm_name) self.proxy.stop()
def generate_humanoid(): robot_name=get_robot_name() has_torso=False has_right_arm=False has_left_arm=False has_head=False print 'Torso present [y]?' if m3t.get_yes_no('y'): has_torso=True torso_name=get_torso_name() torso_version=get_version(['T2.R2','T2.R3']) print 'Right arm present [y]?' if m3t.get_yes_no('y'): has_right_arm=True print 'For right arm: ', right_arm_name=get_arm_name() right_arm_version=get_version(['A2.R2','A2.R3']) print 'Left arm present [y]?' if m3t.get_yes_no('y'): has_left_arm=True print 'For left arm: ', left_arm_name=get_arm_name() left_arm_version=get_version(['A2.R2','A2.R3']) print 'Head present [y]?' if m3t.get_yes_no('y'): has_head=True print 'For head: ', head_name=get_head_name() head_version='S2.R1' pwr_name=get_pwr_name() config_dir=m3t.get_m3_config_path()+robot_name+'/' data=[] #descending number of limbs #4 if has_left_arm and has_right_arm and has_torso and has_head: if torso_version=='T2.R3' and right_arm_version=='A2.R3' and left_arm_version=='A2.R3' and head_version=='S2.R1': x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Bimanual.T2R3.S2R1']) x['name']='m3humanoid_bimanual_torso_head_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name x['chains']['left_arm']['chain_component']='m3arm_'+left_arm_name x['chains']['torso']['chain_component']='m3torso_'+torso_name x['chains']['head']['chain_component']='m3head_'+head_name data.append(x) return config_dir,data if torso_version=='T2.R2' and right_arm_version=='A2.R2' and left_arm_version=='A2.R2' and head_version=='S2.R1': x=copy.deepcopy(config_robot_A2T2['Humanoid.A2R2-Bimanual.T2R2.S2R1']) x['name']='m3humanoid_bimanual_torso_head_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name x['chains']['left_arm']['chain_component']='m3arm_'+left_arm_name x['chains']['torso']['chain_component']='m3torso_'+torso_name x['chains']['head']['chain_component']='m3head_'+head_name data.append(x) return config_dir,data #3 if has_right_arm and has_torso and has_head: if torso_version=='T2.R3' and right_arm_version=='A2.R3' and head_version=='S2.R1': x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Right.T2R3.S2R1']) x['name']='m3humanoid_right_arm_torso_head_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name x['chains']['torso']['chain_component']='m3torso_'+torso_name x['chains']['head']['chain_component']='m3head_'+head_name data.append(x) return config_dir,data if torso_version=='T2.R2' and right_arm_version=='A2.R2' and head_version=='S2.R1': x=copy.deepcopy(config_robot_A2T2['Humanoid.A2R2-Right.T2R2.S2R1']) x['name']='m3humanoid_right_arm_torso_head_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name x['chains']['torso']['chain_component']='m3torso_'+torso_name x['chains']['head']['chain_component']='m3head_'+head_name data.append(x) return config_dir,data if has_left_arm and has_right_arm and has_torso: x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Bimanual.T2R3']) x['name']='m3humanoid_bimanual_torso_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name x['chains']['left_arm']['chain_component']='m3arm_'+left_arm_name x['chains']['torso']['chain_component']='m3torso_'+torso_name data.append(x) return config_dir,data #2 if has_left_arm and has_torso: x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Left.T2R3']) x['name']='m3humanoid_left_arm_torso_'+robot_name x['pwr_component']=pwr_name x['chains']['torso']['chain_component']='m3torso_'+torso_name x['chains']['left_arm']['chain_component']='m3arm_'+left_arm_name data.append(x) return config_dir,data if has_right_arm and has_torso: x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Right.T2R3']) x['name']='m3humanoid_right_arm_torso_'+robot_name x['pwr_component']=pwr_name x['chains']['torso']['chain_component']='m3torso_'+torso_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name data.append(x) return config_dir,data if has_left_arm and has_right_arm: x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Bimanual']) x['name']='m3humanoid_bimanual_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name x['chains']['left_arm']['chain_component']='m3arm_'+left_arm_name data.append(x) return config_dir,data #1 if has_torso: x=copy.deepcopy(config_robot_A3T3['Humanoid.T2R3']) x['name']='m3humanoid_torso_'+robot_name x['pwr_component']=pwr_name x['chains']['torso']['chain_component']='m3torso_'+torso_name data.append(x) return config_dir,data if has_right_arm: x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Right']) x['name']='m3humanoid_right_arm_'+robot_name x['pwr_component']=pwr_name x['chains']['right_arm']['chain_component']='m3arm_'+right_arm_name data.append(x) return config_dir,data if has_left_arm: x=copy.deepcopy(config_robot_A3T3['Humanoid.A2R3-Left']) x['name']='m3humanoid_left_arm_'+robot_name x['pwr_component']=pwr_name x['chains']['left_arm']['chain_component']='m3arm_'+left_arm_name data.append(x) return config_dir,data if has_head: x=copy.deepcopy(config_robot_A3T3['Humanoid.S2R1']) x['name']='m3humanoid_head_'+robot_name x['pwr_component']=pwr_name x['chains']['head']['chain_component']='m3head_'+head_name data.append(x) return config_dir,data
def start(self): self.proxy.start() print '--------------------------' print 'Enable RVIZ? (y/n)' print '--------------------------' print self.rviz = False if m3t.get_yes_no(): self.rviz = True sea_joint_names=self.proxy.get_joint_components() sea_joint_names=m3t.user_select_components_interactive(sea_joint_names) self.sea_joint=[] for n in sea_joint_names: self.sea_joint.append(m3f.create_component(n)) self.proxy.subscribe_status(self.sea_joint[-1]) chain_names=self.proxy.get_chain_components() self.chain=[] if len(chain_names)>0: print 'Select kinematic chain' chain_names=m3t.user_select_components_interactive(chain_names) for n in chain_names: self.chain.append(m3f.create_component(n)) self.proxy.subscribe_status(self.chain[-1]) kine_names=self.proxy.get_available_components('m3dynamatics') self.kine = [] if len(kine_names)>0: print 'Select dynamatics controller' kine_names=m3t.user_select_components_interactive(kine_names) for n in kine_names: self.kine.append(m3f.create_component(n)) self.proxy.subscribe_status(self.kine[-1]) bot_name=m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_names return self.bot=m3f.create_component(bot_name) if self.rviz: self.viz = m3v.M3Viz(self.proxy, self.bot) #self.proxy.publish_param(self.bot) #allow to set payload #self.proxy.subscribe_status(self.bot) #self.proxy.publish_command(self.bot) #self.proxy.make_operational_all() self.bot.initialize(self.proxy) self.chain_names = self.bot.get_available_chains() #Create gui self.mode=[0] self.theta_desire_right_arm=[0]*self.bot.get_num_dof('right_arm') self.theta_desire_left_arm=[0]*self.bot.get_num_dof('left_arm') self.theta_desire_torso=[0]*self.bot.get_num_dof('torso') self.theta_desire_head=[0]*self.bot.get_num_dof('head') self.stiffness_right_arm=[0]*self.bot.get_num_dof('right_arm') self.stiffness_left_arm=[0]*self.bot.get_num_dof('left_arm') self.stiffness_torso=[0]*self.bot.get_num_dof('torso') self.stiffness_head=[0]*self.bot.get_num_dof('head') self.stiffness=[0] #self.slew=[0] self.save=False self.save_last=False self.status_dict=self.proxy.get_status_dict() self.param_dict=self.proxy.get_param_dict() self.gui.add('M3GuiTree', 'Status', (self,'status_dict'),[],[],m3g.M3GuiRead,column=3) self.gui.add('M3GuiTree', 'Param', (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3) self.gui.add('M3GuiModes', 'Mode', (self,'mode'),range(1),[['Off','Pwm','Torque','Theta','Torque_GC','Theta_GC','Theta_MJ', 'Theta_GC_MJ'],1],m3g.M3GuiWrite) #self.gui.add('M3GuiSliders','Torque (mNm)', (self,'tq_desire'),range(len(self.bot.right_arm_ndof)),[-8000,8000],m3g.M3GuiWrite) #self.gui.add('M3GuiSliders','Pwm', (self,'pwm_desire'),range(len(self.sea_joint)),[-3200,3200],m3g.M3GuiWrite) #self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(1),[0,100],m3g.M3GuiWrite,column=1) #self.gui.add('M3GuiSliders','Slew ', (self,'slew'),range(0),[0,100],m3g.M3GuiWrite,column=3) self.gui.add('M3GuiSliders','Theta RA (Deg)', (self,'theta_desire_right_arm'),range(len(self.theta_desire_right_arm)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Theta LA (Deg)', (self,'theta_desire_left_arm'),range(len(self.theta_desire_left_arm)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Theta T (Deg)', (self,'theta_desire_torso'),range(len(self.theta_desire_torso)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Theta H (Deg)', (self,'theta_desire_head'),range(len(self.theta_desire_head)),[-45,140],m3g.M3GuiWrite,column=1) self.gui.add('M3GuiSliders','Stiffness RA ', (self,'stiffness_right_arm'),range(len(self.stiffness_right_arm)),[0,100],m3g.M3GuiWrite,column=2) self.gui.add('M3GuiSliders','Stiffness LA )', (self,'stiffness_left_arm'),range(len(self.stiffness_left_arm)),[0,100],m3g.M3GuiWrite,column=2) self.gui.add('M3GuiSliders','Stiffness T ', (self,'stiffness_torso'),range(len(self.stiffness_torso)),[0,100],m3g.M3GuiWrite,column=2) #self.gui.add('M3GuiSliders','Stiffness ', (self,'stiffness'),range(len(self.sea_joint)),[0,100],m3g.M3GuiWrite,column=3) #self.gui.add('M3GuiToggle', 'Save', (self,'save'),[],[['On','Off']],m3g.M3GuiWrite) self.gui.start(self.step)
def start(self): self.proxy.start() self.proxy.make_operational_all() chain_names = self.proxy.get_available_components('m3hand') if len(chain_names) > 1: hand_name = m3t.user_select_components_interactive(chain_names, single=True) else: hand_name = chain_names pwr_name = self.proxy.get_available_components('m3pwr') if len(pwr_name) > 1: pwr_name = m3t.user_select_components_interactive(pwr_name, single=True) print 'Position arm [y]?' if m3t.get_yes_no('y'): arm_names = self.proxy.get_available_components('m3arm') if len(arm_names) > 1: print 'Select arm: ' arm_name = m3t.user_select_components_interactive( arm_names, single=True)[0] else: arm_name = arm_names[0] self.arm = m3f.create_component(arm_name) self.proxy.publish_command(self.arm) self.arm.set_mode_theta_gc() self.arm.set_theta_deg([30, 0, 0, 110, 0, 0, 0]) self.arm.set_stiffness(0.5) self.arm.set_slew_rate_proportion([0.75] * 7) self.chain = m3f.create_component(hand_name[0]) self.proxy.publish_command(self.chain) self.proxy.subscribe_status(self.chain) self.pwr = m3f.create_component(pwr_name[0]) self.proxy.publish_command(self.pwr) self.pwr.set_motor_power_on() #Force safe-op of robot if present hum = self.proxy.get_available_components('m3humanoid') if len(hum) > 0: self.proxy.make_safe_operational(hum[0]) #Setup postures self.posture_filename = m3t.get_m3_animation_path( ) + self.chain.name + '_postures.yml' f = file(self.posture_filename, 'r') self.data = yaml.safe_load(f.read()) self.param = self.data['param'] f.close() self.theta_desire = [0, 0, 0, 0, 0] self.mode = [1, 1, 1, 1, 1] #Create gui self.run = False self.run_last = False self.running = False self.grasp = False self.grasp_last = False self.grasp_off = False self.grasp_off_ts = time.time() self.status_dict = self.proxy.get_status_dict() self.gui.add('M3GuiTree', 'Status', (self, 'status_dict'), [], [], m3g.M3GuiRead, column=3) self.gui.add('M3GuiTree', 'Param', (self, 'param'), [], [], m3g.M3GuiWrite, column=3) self.gui.add('M3GuiToggle', 'Animation', (self, 'run'), [], [['Run', 'Stop']], m3g.M3GuiWrite, column=1) self.gui.add('M3GuiModes', 'Joint', (self, 'mode'), range(5), [['Off', 'Enabled'], 1], m3g.M3GuiWrite, column=2) self.gui.add('M3GuiSliders', 'Theta (Deg)', (self, 'theta_desire'), range(5), [0, 300], m3g.M3GuiWrite, column=2) self.gui.add('M3GuiToggle', 'Power Grasp', (self, 'grasp'), [], [['Run', 'Stop']], m3g.M3GuiWrite, column=2) self.gui.start(self.step)
'torque_gravity': [], 'is_wrist': False } if (c.find('j5') >= 0 or c.find('j6') >= 0) and lt == 0: comp[c]['is_wrist'] = True if len(cnames) == 0: print 'No components found' exit() print 'Using components: ' for i in range(len(cnames)): print i, ':', cnames[i] print 'Continue [y]?' if not m3t.get_yes_no('y'): exit() print 'Query to save each calibration [y]?' do_query = m3t.get_yes_no('y') for c in comp.keys(): comp[c]['comp_rt'] = mcf.create_component(c.replace('_ec', '')) comp[c]['comp_j'] = mcf.create_component(c.replace('actuator_ec', 'joint')) proxy.subscribe_status(comp[c]['comp_rt']) proxy.subscribe_status(comp[c]['comp_j']) #proxy.publish_param(comp[c]['comp_j']) proxy.make_operational_all() proxy.step() # ###########################
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
print if __name__ == "__main__": if len(glob.glob("m3_controller*.hex")) == 0: print "No m3_controller*.hex files available" print "Run from directory with dsPIC hex files" else: try: # print 'Use Port3 [n]?' # p3=m3t.get_yes_no('n') print "Slaves on Port3 not supported (yet)." print "If any Port3 slaves show with lsec, abort now and power cycle" print "Do not run m3rt_bus_init.py" print "Continue [y]?" if not m3t.get_yes_no("y"): exit() p3 = False print "Quick batch write [y]?" if m3t.get_yes_no("y"): easy_batch_write(p3) exit() else: boards = get_boards() if not len(boards): exit() bld = m3.m3_bootloader.M3Bootloader() unload = False while True: print_usage() c = raw_input()
#! /usr/bin/python import pylab as pyl import time import m3.rt_proxy as m3p import m3.toolbox as m3t import m3.component_factory as mcf import numpy.numarray as na import math proxy = m3p.M3RtProxy() proxy.start() cnames=proxy.get_available_components() print 'All actuator_ec [n]?' comps=[] if m3t.get_yes_no('n'): cnames=[q for q in cnames if q.find('actuator_ec')!=-1] all_aec=True for c in cnames: comps.append(mcf.create_component(c)) else: all_aec=False print '------- Components ------' for i in range(len(cnames)): print i,' : ',cnames[i] print '-------------------------' print 'Enter component id' cid=m3t.get_int() name=cnames[cid] comps.append(mcf.create_component(name)) if len(comps)==0:
for c in cnames: comp[c]={'comp_rt':None,'comp_j':None,'torque_act':[],'torque_joint':[],'torque_gravity':[],'is_wrist':False} if (c.find('j5')>=0 or c.find('j6')>=0) and lt==0: comp[c]['is_wrist']=True if len(cnames)==0: print 'No components found' exit() print 'Using components: ' for i in range(len(cnames)): print i,':',cnames[i] print 'Continue [y]?' if not m3t.get_yes_no('y'): exit() print 'Query to save each calibration [y]?' do_query=m3t.get_yes_no('y') for c in comp.keys(): comp[c]['comp_rt']=mcf.create_component(c.replace('_ec','')) comp[c]['comp_j']=mcf.create_component(c.replace('actuator_ec','joint')) proxy.subscribe_status(comp[c]['comp_rt']) proxy.subscribe_status(comp[c]['comp_j']) #proxy.publish_param(comp[c]['comp_j']) proxy.make_operational_all() proxy.step() # ###########################
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()
print '-----------------------' for i in range(len(via_names)): print i, ' : ', via_names[i] idx = m3t.get_int(0) if idx >= 0 and idx < len(via_names): f = file(via_files[idx], 'r') d = yaml.safe_load(f.read()) return d return None # ###################################################### print 'Enable RVIZ? [n]' pub = None rviz = m3t.get_yes_no('n') # ###################################################### proxy = m3p.M3RtProxy() proxy.start() bot_name = m3t.get_robot_name() if bot_name == "": print 'Error: no robot components found:', bot_name quit bot = m3f.create_component(bot_name) if rviz == True: viz = m3v.M3Viz(proxy, bot) proxy.subscribe_status(bot) proxy.publish_command(bot)