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 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)
