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