def generate_joint(zlift_name=None,version=None, jid=None, pwr_name=None):
if version is None:
version=get_version(versions)
if zlift_name is None:
zlift_name=get_zlift_name()
if jid is None:
jid=get_joint_id()
if pwr_name is None:
pwr_name=get_pwr_name()
actuator_ec=copy.deepcopy(config_zlift_def[version]['actuator_ec'][jid])
actuator=copy.deepcopy(config_zlift_def[version]['actuator'][jid])
joint=copy.deepcopy(config_zlift_def[version]['joint'][jid])
name_actuator_ec='m3actuator_ec_'+zlift_name+'_j'+str(jid)
name_actuator='m3actuator_'+zlift_name+'_j'+str(jid)
name_joint='m3joint_'+zlift_name+'_j'+str(jid)
print 'Enter Serial Number of EC board for joint ','J'+str(jid),'(e.g. 300 for EC300):'
x=m3t.get_int()
actuator_ec['ethercat']['serial_number']=x
actuator_ec['name']=name_actuator_ec
actuator_ec['pwr_component']=pwr_name
actuator['name']=name_actuator
actuator['ec_component']=name_actuator_ec
actuator['joint_component']=name_joint
joint['name']=name_joint
joint['actuator_component']=name_actuator
joint['transmission']['act_name']=name_actuator
config_dir=m3t.get_m3_config_path()+zlift_name+'/'
return config_dir, [actuator_ec, actuator, joint]
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 generate_joint(base_name=None, version=None, jid=None, pwr_name=None):
if version is None:
version = get_version(versions)
if base_name is None:
base_name = get_base_name()
if jid is None:
jid = get_joint_id()
if pwr_name is None:
pwr_name = get_pwr_name()
actuator_ec = copy.deepcopy(
config_omnibase_def[version]['actuator_ec'][jid])
actuator = copy.deepcopy(config_omnibase_def[version]['actuator'][jid])
joint = copy.deepcopy(config_omnibase_def[version]['joint'][jid])
name_actuator_ec = 'm3actuator_ec_' + base_name + '_j' + str(jid)
name_actuator = 'm3actuator_' + base_name + '_j' + str(jid)
name_joint = 'm3joint_' + base_name + '_j' + str(jid)
print 'Enter Serial Number of EC board for joint ', 'J' + str(
jid), '(e.g. 300 for EC300):'
x = m3t.get_int()
actuator_ec['ethercat']['serial_number'] = x
actuator_ec['name'] = name_actuator_ec
actuator_ec['pwr_component'] = pwr_name
actuator['name'] = name_actuator
actuator['ec_component'] = name_actuator_ec
actuator['joint_component'] = name_joint
joint['name'] = name_joint
joint['actuator_component'] = name_actuator
joint['transmission']['act_name'] = name_actuator
config_dir = m3t.get_m3_config_path() + base_name + '/'
return config_dir, [actuator_ec, actuator, joint]
def generate():
ec=copy.deepcopy(config_loadx6_ec_a2r3)
rt=copy.deepcopy(config_loadx6_a2r3)
arm_name=get_arm_name()
name_ec='m3loadx6_ec_'+arm_name+'_l0'
name_rt='m3loadx6_'+arm_name+'_l0'
print 'Enter Serial Number of EC board (e.g. 300)'
x=m3t.get_int()
print 'Enter Serial Number of Loadcell (e.g. 3333 for FT333)'
y=m3t.get_int()
rt['calib']['wrench']['serial_number']
ec['ethercat']['serial_number']='FT'+str(y)
ec['name']=name_ec
rt['name']=name_rt
rt['ec_component']=name_ec
config_dir=m3t.get_m3_config_path()+arm_name+'/'
return config_dir, [ec, rt]
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 get_component(self):
comps = self.rt_proxy.get_available_components()
print '------- Components ------'
for i in range(len(comps)):
print i, ' : ', comps[i]
print '-------------------------'
print 'Enter component id'
return m3t.get_int()
def get_field(self, comp):
field = m3t.user_select_msg_field(comp.status)
self.idx = 0
if hasattr(m3t.get_msg_field_value(comp.status, field), '__len__'):
self.repeated = True
print 'Select index of repeated fields to monitor: [0]'
self.idx = m3t.get_int(0)
return field
def select_dynamatics(torso_name,version):
dyn=config_torso_def[version]['dynamatics']
print 'Select default dynamatics type for torso'
print '----------------------------------------'
kk=dyn.keys()
for i in range(len(kk)):
print i,':',kk[i]
idx=m3t.get_int()
return 'm3dynamatics_'+kk[idx]+'_'+torso_name
def select_dynamatics(torso_name, version):
dyn = config_torso_def[version]['dynamatics']
print 'Select default dynamatics type for torso'
print '----------------------------------------'
kk = dyn.keys()
for i in range(len(kk)):
print i, ':', kk[i]
idx = m3t.get_int()
return 'm3dynamatics_' + kk[idx] + '_' + torso_name
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 get_version():
versions=['0.3','0.4','0.5']
while True:
print 'Enter version'
for i in range(len(versions)):
print i,' : ',versions[i]
idd=m3t.get_int()
if idd<len(versions):
return versions[idd]
print 'Incorrect value'
def get_version():
versions = ['0.3', '0.4', '0.5']
while True:
print 'Enter version'
for i in range(len(versions)):
print i, ' : ', versions[i]
idd = m3t.get_int()
if idd < len(versions):
return versions[idd]
print 'Incorrect value'
def get_version(versions): if len(versions)==0: return versions[0] while True: print 'Enter version' for i in range(len(versions)): print i,' : ',versions[i] idd=m3t.get_int() if idd<len(versions): return versions[idd] print 'Incorrect value'
def get_limb_name(limbs):
if len(limbs) == 0:
return limbs[0]
while True:
print 'Enter limb name'
for i in range(len(limbs)):
print i, ' : ', limbs[i]
idd = m3t.get_int()
if idd < len(limbs):
return limbs[idd]
print 'Incorrect value'
def get_version(versions):
if len(versions) == 0:
return versions[0]
while True:
print 'Enter version'
for i in range(len(versions)):
print i, ' : ', versions[i]
idd = m3t.get_int()
if idd < len(versions):
return versions[idd]
print 'Incorrect value'
def get_limb_name(limbs): if len(limbs)==0: return limbs[0] while True: print 'Enter limb name' for i in range(len(limbs)): print i,' : ',limbs[i] idd=m3t.get_int() if idd<len(limbs): return limbs[idd] print 'Incorrect value'
def play_posture():
k=postures.keys()
print 'Select posture'
for idx,kk, in zip(range(len(k)),k):
print idx,' : ',k
i=m3t.get_int()
p=postures[k[i]]
print 'Display posture: ',k[i]
done=False
ep_start=time.time()
while not done:
done=execute_posture(postures[k[i]])
def select_dynamatics(arm_name, version, limb_name):
dyn = config_arm_def[version]['dynamatics']
print 'Select default dynamatics type for arm'
print '----------------------------------------'
kk = dyn.keys()
if limb_name == 'right_arm':
xx = [yy for yy in kk if yy.find('right') >= 0]
if limb_name == 'left_arm':
xx = [yy for yy in kk if yy.find('left') >= 0]
for i in range(len(xx)):
print i, ':', xx[i]
idx = m3t.get_int()
return 'm3dynamatics_' + xx[idx] + '_' + arm_name
def select_dynamatics(arm_name,version,limb_name):
dyn=config_arm_def[version]['dynamatics']
print 'Select default dynamatics type for arm'
print '----------------------------------------'
kk=dyn.keys()
if limb_name=='right_arm':
xx=[yy for yy in kk if yy.find('right')>=0]
if limb_name=='left_arm':
xx=[yy for yy in kk if yy.find('left')>=0]
for i in range(len(xx)):
print i,':',xx[i]
idx=m3t.get_int()
return 'm3dynamatics_'+xx[idx]+'_'+arm_name
def generate():
version = get_version()
if version == '0.3':
ec = copy.deepcopy(config_pwr_ec_v0_3)
rt = copy.deepcopy(config_pwr_v0_3)
if version == '0.4':
ec = copy.deepcopy(config_pwr_ec_v0_4)
rt = copy.deepcopy(config_pwr_v0_4)
if version == '0.5':
ec = copy.deepcopy(config_pwr_ec_v0_5)
rt = copy.deepcopy(config_pwr_v0_5)
print 'Enter PWR board ID (e.g., 10 for pwr010)'
x = m3t.get_int()
name_ec = 'm3pwr_ec_pwr' + '0' * (3 - len(str(x))) + str(x)
name_rt = 'm3pwr_pwr' + '0' * (3 - len(str(x))) + str(x)
print 'Enter Serial Number of EC board (e.g. 300)'
x = m3t.get_int()
ec['ethercat']['serial_number'] = x
ec['name'] = name_ec
rt['name'] = name_rt
rt['ec_component'] = name_ec
config_dir = m3t.get_m3_config_path() + 'pwr/'
return config_dir, [ec, rt]
def generate():
version=get_version()
if version=='0.3':
ec=copy.deepcopy(config_pwr_ec_v0_3)
rt=copy.deepcopy(config_pwr_v0_3)
if version=='0.4':
ec=copy.deepcopy(config_pwr_ec_v0_4)
rt=copy.deepcopy(config_pwr_v0_4)
if version=='0.5':
ec=copy.deepcopy(config_pwr_ec_v0_5)
rt=copy.deepcopy(config_pwr_v0_5)
print 'Enter PWR board ID (e.g., 10 for pwr010)'
x=m3t.get_int()
name_ec='m3pwr_ec_pwr'+'0'*(3-len(str(x)))+str(x)
name_rt='m3pwr_pwr'+'0'*(3-len(str(x)))+str(x)
print 'Enter Serial Number of EC board (e.g. 300)'
x=m3t.get_int()
ec['ethercat']['serial_number']=x
ec['name']=name_ec
rt['name']=name_rt
rt['ec_component']=name_ec
config_dir=m3t.get_m3_config_path()+'pwr/'
return config_dir, [ec, rt]
def load_vias():
via_files = m3t.get_via_files()
via_names = m3t.get_via_names()
if len(via_files) == 0:
print 'No via files available'
return
print 'Enter file IDX'
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
def load_vias():
via_files = m3t.get_via_files()
via_names = m3t.get_via_names()
if len(via_files) == 0:
print "No via files available"
return
print "Enter file IDX"
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
def get_sensor_analysis(self,sensors,trial_time):
# sensors e.g., ['qei_on','qei_period']
# trial_time: seconds
sensor_list=[]
while True:
print '---------- Available Sensors----------'
for idx in range(len(sensors)):
print idx,' : ',sensors[idx]
print 'Add sensor? '
id=m3t.get_int()
if id>=0 and id<len(sensors):
sensor_list.append(sensors[id])
break
print 'Measuring sensor',sensor_list
print 'Hit any key to begin'
raw_input()
dt=0
ts=time.time()
log=[[] for x in sensor_list]
self.step()
while dt<trial_time:
self.step()
data=m3t.GetDictFromMsg(self.comp_ec.status)
sample=[]
for idx in range(len(sensor_list)):
print sensor_list[idx],data[sensor_list[idx]]
print 'Timestamp',self.comp_ec.status.base.timestamp
log[idx].append(data[sensor_list[idx]])
print
dt=time.time()-ts
ret={}
for idx in range(len(sensor_list)):
a=nu.array(log[idx],nu.Float32)
avg=nu.average(a)
std=nu.std(a)
p2p=max(a)-min(a)
print '-----------',sensor_list[idx],'--------------'
print 'Mean:',avg
print 'Std Dev:',std
print 'Peak To Peak:',p2p
pyl.title('Noise Distribution for '+sensor_list[idx])
pyl.hist(x=a-avg,bins=pyl.array(range(-10,10,1)))
pyl.show()
ret[sensor_list[idx]]={'avg':avg,'std':std,'p2p':p2p}
print 'Hit enter to continue'
raw_input()
return ret
def get_sensor_analysis(self, sensors, trial_time):
# sensors e.g., ['qei_on','qei_period']
# trial_time: seconds
sensor_list = []
while True:
print '---------- Available Sensors----------'
for idx in range(len(sensors)):
print idx, ' : ', sensors[idx]
print 'Add sensor? '
id = m3t.get_int()
if id >= 0 and id < len(sensors):
sensor_list.append(sensors[id])
break
print 'Measuring sensor', sensor_list
print 'Hit any key to begin'
raw_input()
dt = 0
ts = time.time()
log = [[] for x in sensor_list]
self.step()
while dt < trial_time:
self.step()
data = m3t.GetDictFromMsg(self.comp_ec.status)
sample = []
for idx in range(len(sensor_list)):
print sensor_list[idx], data[sensor_list[idx]]
print 'Timestamp', self.comp_ec.status.base.timestamp
log[idx].append(data[sensor_list[idx]])
print
dt = time.time() - ts
ret = {}
for idx in range(len(sensor_list)):
a = na.array(log[idx], na.Float32)
avg = a.mean()
std = a.stddev()
p2p = max(a) - min(a)
print '-----------', sensor_list[idx], '--------------'
print 'Mean:', avg
print 'Std Dev:', std
print 'Peak To Peak:', p2p
pyl.title('Noise Distribution for ' + sensor_list[idx])
pyl.hist(x=a - avg, bins=pyl.array(range(-10, 10, 1)))
pyl.show()
ret[sensor_list[idx]] = {'avg': avg, 'std': std, 'p2p': p2p}
print 'Hit enter to continue'
raw_input()
return ret
def get_pwr_name():
print 'Enter PWR board ID (e.g., 10 for pwr010)'
x = m3t.get_int()
return 'm3pwr_pwr' + '0' * (3 - len(str(x))) + str(x)
},
'A2.LoadX6': {
'generate': m3l.generate
},
'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...'
def get_head_name():
while True:
print 'Enter Head Number (e.g., 2 for MS2)'
return 'ms' + str(m3t.get_int())
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k != 'm' and self.k != 'v' and self.k != 'b' and self.k != 'q':
self.k = m3t.get_keystroke()
if self.k == 'q':
return
self.proxy = m3p.M3RtProxy()
if self.k == 'm' or self.k == 'b':
self.proxy.start()
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)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,
single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k == 'v' or self.k == 'b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k == 'v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k == 'm' or self.k == 'b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.] * self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.] * self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0] * 7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ', self.vel_avg, ')'
print 's: set stiffness (Current', self.stiffness, ')'
if self.k == 'b' or self.k == 'm':
print 'e: execute vias'
if self.k == 'b' or self.k == 'v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m = m3t.get_keystroke()
if m == 'q':
return
if m == 'v':
print 'Enter avg velocity (0-60 Deg/S) [', self.vel_avg, ']'
self.vel_avg = max(0, min(60, m3t.get_float(self.vel_avg)))
if m == 's':
print 'Enter stiffness (0-1.0) [', self.stiffness, ']'
self.stiffness = max(0, min(1.0,
m3t.get_float(self.stiffness)))
if m == 'g':
self.vias = []
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape = m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10, min(25, m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10, min(25, m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1, min(20, m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m / 2.0
y_right = self.center[1] - length_m / 2.0
z_top = self.center[2] + length_m / 2.0
z_bottom = self.center[2] - length_m / 2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name == 'right_arm':
# first add start point
self.ik_vias.append([
x, y_left, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
else:
# first add start point
self.ik_vias.append([
x, y_right, z_top, self.axis_demo[0],
self.axis_demo[1], self.axis_demo[2]
])
# add top line
for i in range(resolution):
self.ik_vias.append([
x, y_right + (i + 1) * dy, z_top,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add right line
for i in range(resolution):
self.ik_vias.append([
x, y_left, z_top - (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add bottom line
for i in range(resolution):
self.ik_vias.append([
x, y_left - (i + 1) * dy, z_bottom,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
# add left line
for i in range(resolution):
self.ik_vias.append([
x, y_right, z_bottom + (i + 1) * dz,
self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
if shape == 'c':
for i in range(resolution * 4 + 1):
dt = 2 * nu.pi / (nu.float(resolution) * 4)
t = (nu.pi / 2) + i * dt
if t > nu.pi:
t -= 2 * nu.pi
y = self.center[1] + (diameter_m / 2.0) * nu.cos(t)
z = self.center[2] + (diameter_m / 2.0) * nu.sin(t)
self.ik_vias.append([
x, y, z, self.axis_demo[0], self.axis_demo[1],
self.axis_demo[2]
])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref = [0] * self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref = [30, 0, 0, 40, 0, 0, 0]
self.bot.set_theta_sim_deg(self.arm_name, ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(
self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name, theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name, ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m == 'd':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m == 't':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg] * self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(
self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p = m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m == 'e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(
self.arm_name, [self.stiffness] *
self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m == 't':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m == 'e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias = []
#along with m3. If not, see <http://www.gnu.org/licenses/>.
import os
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)
def get_zlift_name():
while True:
print 'Enter ZLift Number (e.g., 1 for MZ1)'
return 'mz' + str(m3t.get_int())
def get_arm_name(): while True: print 'Enter Arm Number (e.g., 6 for MA6)' return 'ma'+str(m3t.get_int())
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:
print 'No components selected'
exit()
for c in comps:
proxy.subscribe_status(c)
proxy.publish_command(c)
field=m3t.user_select_msg_field(comps[0].status)
print 'Number of samples [100]?'
ns=m3t.get_int(100)
log=[]
for i in range(len(comps)):
import glob
path= m3t.get_m3_config_path()+'data/'
files=glob.glob(path+'calibration_data_raw_m3actuator_ec_*.yml')
files.sort()
if len(files)==0:
print 'No calibration files found'
exit()
comps=[]
for f in files:
comps.append(f[f.find('calibration_data_raw')+21:len(f)-4])
print 'Enter component ID'
print '------------------'
for i in range(len(comps)):
print i,' : ',comps[i]
idd=m3t.get_int()
if idd<0 or idd>=len(comps):
print 'Invalid ID'
exit()
fn=files[idd]
print 'Display calibration file: ',fn
try:
f=file(fn,'r')
d= yaml.safe_load(f.read())
f.close()
n=len(d['cb_torque'])-1
#poly,inv_poly=m3tc.get_polyfit_to_data(x,y,n)
s=m3tc.PolyEval(d['cb_torque'],d['log_adc_torque'])
m3t.mplot2(range(len(d['log_adc_torque'])),d['log_load_mNm'],s,xlabel='Samples',ylabel='Torque (mNm)')
except (IOError, EOFError):
def get_base_name(): while True: print 'Enter Base Number (e.g., 1 for MB1)' return 'mb'+str(m3t.get_int())
def get_zlift_name(): while True: print 'Enter ZLift Number (e.g., 1 for MZ1)' return 'mz'+str(m3t.get_int())
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
#! /usr/bin/python
from pylab import *
import time
import m3.rt_proxy as m3p
import m3.toolbox as m3t
import m3.component_factory as mcf
proxy = m3p.M3RtProxy()
proxy.start()
comps = proxy.get_available_components()
print '------- Components ------'
for i in range(len(comps)):
print i, ' : ', comps[i]
print '-------------------------'
print 'Enter component id'
cid = m3t.get_int()
print 'Select Y-Range? [n]'
yrange = None
if m3t.get_yes_no('n'):
yrange = []
print 'Min?'
yrange.append(m3t.get_int())
print 'Max?'
yrange.append(m3t.get_int())
name = comps[cid]
comp = mcf.create_component(name)
proxy.subscribe_status(comp)
#proxy.publish_param(comp)
field = m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
import glob
path = m3t.get_m3_config_path() + 'data/'
files = glob.glob(path + 'calibration_data_raw_m3actuator_ec_*.yml')
files.sort()
if len(files) == 0:
print 'No calibration files found'
exit()
comps = []
for f in files:
comps.append(f[f.find('calibration_data_raw') + 21:len(f) - 4])
print 'Enter component ID'
print '------------------'
for i in range(len(comps)):
print i, ' : ', comps[i]
idd = m3t.get_int()
if idd < 0 or idd >= len(comps):
print 'Invalid ID'
exit()
fn = files[idd]
print 'Display calibration file: ', fn
try:
f = file(fn, 'r')
d = yaml.safe_load(f.read())
f.close()
n = len(d['cb_torque']) - 1
#poly,inv_poly=m3tc.get_polyfit_to_data(x,y,n)
s = m3tc.PolyEval(d['cb_torque'], d['log_adc_torque'])
m3t.mplot2(range(len(d['log_adc_torque'])),
d['log_load_mNm'],
#You should have received a copy of the GNU Lesser General Public License
#along with m3. If not, see <http://www.gnu.org/licenses/>.
import os
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)
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()
def get_joint_id():
print 'Enter Joint ID (e.g., 2 for J2)'
return m3t.get_int()
def get_torso_name():
while True:
print 'Enter Torso Number (e.g., 2 for MT2)'
return 'mt' + str(m3t.get_int())
def get_base_name():
while True:
print 'Enter Base Number (e.g., 1 for MB1)'
return 'mb' + str(m3t.get_int())
from pylab import *
import time
import m3.rt_proxy as m3p
import m3.toolbox as m3t
import m3.component_factory as mcf
proxy = m3p.M3RtProxy()
proxy.start()
comps=proxy.get_available_components()
print '------- Components ------'
for i in range(len(comps)):
print i,' : ',comps[i]
print '-------------------------'
print 'Enter component id'
cid=m3t.get_int()
print 'Select Y-Range? [n]'
yrange=None
if m3t.get_yes_no('n'):
yrange=[]
print 'Min?'
yrange.append(m3t.get_int())
print 'Max?'
yrange.append(m3t.get_int())
name=comps[cid]
comp=mcf.create_component(name)
proxy.subscribe_status(comp)
#proxy.publish_param(comp)
field=m3t.user_select_msg_field(comp.status)
repeated = False
idx = 0
#! /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
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()
def get_robot_name():
while True:
print 'Enter Robot Number (e.g., 7 for MR7)'
return 'mr' + str(m3t.get_int())
def get_arm_name():
while True:
print 'Enter Arm Number (e.g., 6 for MA6)'
return 'ma' + str(m3t.get_int())
def start(self):
print '--------------------------'
print 'm: Target M3 arm'
print 'v: Target RVIZ'
print 'b: Target Both M3 and RVIZ'
print 'q: quit'
print '--------------'
print
self.k = 'a'
while self.k!='m' and self.k!='v' and self.k!='b' and self.k!='q':
self.k=m3t.get_keystroke()
if self.k=='q':
return
self.proxy = m3p.M3RtProxy()
if self.k=='m' or self.k=='b':
self.proxy.start()
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)
arm_names = ['right_arm', 'left_arm']
self.arm_name = m3t.user_select_components_interactive(arm_names,single=True)[0]
if self.arm_name == 'right_arm':
self.center = [0.450, -0.25, -0.1745]
else:
self.center = [0.450, 0.25, -0.1745]
avail_chains = self.bot.get_available_chains()
for c in avail_chains:
if c == 'torso':
self.center[2] += 0.5079
if self.k=='v' or self.k=='b':
self.viz = m3v.M3Viz(self.proxy, self.bot)
self.viz_launched = True
self.viz.turn_sim_on()
if self.k=='v':
self.theta_0[:] = self.bot.get_theta_sim_deg(self.arm_name)[:]
if self.k=='m' or self.k=='b':
self.proxy.subscribe_status(self.bot)
self.proxy.publish_command(self.bot)
self.proxy.make_operational_all()
self.proxy.step()
self.theta_0[:] = self.bot.get_theta_deg(self.arm_name)[:]
self.m3_launched = True
self.theta_soln_deg = [0.]*self.bot.get_num_dof(self.arm_name)
self.thetadot_0 = [0.]*self.bot.get_num_dof(self.arm_name)
self.bot.set_slew_rate_proportion(self.arm_name, [1.0]*7)
while True:
print '--------------------------'
print 'g: generate vias'
print 'd: display vias'
print 'v: set avg velocity (Current ',self.vel_avg,')'
print 's: set stiffness (Current',self.stiffness,')'
if self.k=='b' or self.k=='m':
print 'e: execute vias'
if self.k=='b' or self.k=='v':
print 't: test vias in visualizer'
print 'q: quit'
print '--------------'
print
m=m3t.get_keystroke()
if m=='q':
return
if m=='v':
print 'Enter avg velocity (0-60 Deg/S) [',self.vel_avg,']'
self.vel_avg=max(0,min(60,m3t.get_float(self.vel_avg)))
if m=='s':
print 'Enter stiffness (0-1.0) [',self.stiffness,']'
self.stiffness=max(0,min(1.0,m3t.get_float(self.stiffness)))
if m == 'g':
self.vias=[]
print
print '(s)quare or (c)ircle?'
shape = None
while shape != 's' and shape != 'c':
shape=m3t.get_keystroke()
length_m = 0.0
if shape == 's':
print
print 'Length of square side in cm (10-25) [25]'
length_cm = nu.float(max(10,min(25,m3t.get_int(25))))
length_m = length_cm / 100.0
diameter_m = 0.0
if shape == 'c':
print
print 'Diameter of circle in cm (10-25) [25]'
diameter_cm = nu.float(max(10,min(25,m3t.get_int(25))))
diameter_m = diameter_cm / 100.0
print
print 'Enter shape resolution (1-20 vias/side) [20]'
resolution = max(1,min(20,m3t.get_int(20)))
if self.m3_launched:
self.proxy.step()
x = self.center[0]
if shape == 's':
y_left = self.center[1] + length_m/2.0
y_right = self.center[1] - length_m/2.0
z_top = self.center[2] + length_m/2.0
z_bottom = self.center[2] - length_m/2.0
dy = (y_left - y_right) / nu.float(resolution)
dz = (z_top - z_bottom) / nu.float(resolution)
if self.arm_name=='right_arm':
# first add start point
self.ik_vias.append([x, y_left, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add top line
for i in range(resolution):
self.ik_vias.append([x, y_left - (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
# add right line
for i in range(resolution):
self.ik_vias.append([x, y_right, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add bottom line
for i in range(resolution):
self.ik_vias.append([x, y_right + (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add left line
for i in range(resolution):
self.ik_vias.append([x, y_left, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
else:
# first add start point
self.ik_vias.append([x, y_right, z_top, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add top line
for i in range(resolution):
self.ik_vias.append([x, y_right + (i+1)*dy, z_top, self.axis_demo[0],self.axis_demo[1], self.axis_demo[2]])
# add right line
for i in range(resolution):
self.ik_vias.append([x, y_left, z_top - (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add bottom line
for i in range(resolution):
self.ik_vias.append([x, y_left - (i+1)*dy, z_bottom, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
# add left line
for i in range(resolution):
self.ik_vias.append([x, y_right, z_bottom + (i+1)*dz, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
if shape == 'c':
for i in range(resolution*4 + 1):
dt = 2*nu.pi/(nu.float(resolution)*4)
t = (nu.pi/2) + i*dt
if t > nu.pi:
t -= 2*nu.pi
y = self.center[1] + (diameter_m/2.0) * nu.cos(t)
z = self.center[2] + (diameter_m/2.0) * nu.sin(t)
self.ik_vias.append([x, y, z, self.axis_demo[0], self.axis_demo[1], self.axis_demo[2]])
self.vias.append(self.theta_0[:])
# use zero position as reference for IK solver
ref=[0]*self.bot.get_num_dof(self.arm_name)
# use holdup position as reference
ref= [30,0,0,40,0,0,0]
self.bot.set_theta_sim_deg(self.arm_name,ref)
for ikv in self.ik_vias:
theta_soln = []
print 'solving for ik via:', ikv
if self.bot.get_tool_axis_2_theta_deg_sim(self.arm_name, ikv[:3], ikv[3:], theta_soln):
self.vias.append(theta_soln)
self.bot.set_theta_sim_deg(self.arm_name,theta_soln)
else:
print 'WARNING: no IK solution found for via ', ikv
self.bot.set_theta_sim_deg(self.arm_name,ref)
if self.viz_launched:
self.viz.step()
self.vias.append(self.theta_0[:])
if m=='d':
print
print '--------- IK Vias (', len(self.ik_vias), ')--------'
print '---------------[end_xyz[3], end_axis[3]]-----------'
for ikv in self.ik_vias:
print ikv
print
print '--------- Joint Vias (', len(self.vias), ')--------'
for v in self.vias:
print v
if m == 'e' or m=='t':
if len(self.vias) != 0:
for v in self.vias:
#print 'Adding via',v
self.jt.add_via_deg(v, [self.vel_avg]*self.ndof)
self.jt.start(self.theta_0[:], self.thetadot_0[:])
print
print '--------- Splines (', len(self.jt.splines), ')--------'
print '------------q_0, q_f, qdot_0, qdot_f, tf--------------'
for s in self.jt.splines:
print s.q_0, s.q_f, s.qdot_0, s.qdot_f, s.tf
print
print 'Hit any key to start or (q) to quit execution'
p=m3t.get_keystroke()
if p != 'q':
if self.m3_launched and m=='e':
self.bot.set_motor_power_on()
self.bot.set_mode_theta_gc(self.arm_name)
self.bot.set_stiffness(self.arm_name, [self.stiffness]*self.bot.get_num_dof(self.arm_name))
while not self.jt.is_splined_traj_complete():
q = self.jt.step()
if self.viz_launched and m=='t':
self.bot.set_theta_sim_deg(self.arm_name, q)
self.viz.step()
time.sleep(0.1)
elif self.m3_launched and m=='e':
self.bot.set_theta_deg(self.arm_name, q)
self.proxy.step()
self.ik_vias=[]
# -*- coding: utf-8 -*-
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
from m3qa.calibrate_sensors import *
print 'This script will calibrate the zero-torque of the A2 arm and T2 torso based on the current torque_gravity estimate'
#print 'It will not calibrate joint J5,J6 of the A2 arm'
print 'The robot should be powered off in a neutral pose'
print '----------------------------------------------'
print 'Enter limb type (0: A2R2, A2R3 arm, 1: T2R2 torso, 2: H2 hand, 3: A2R4 arm, 4: T2R3 torso)'
lt = m3t.get_int()
proxy = m3p.M3RtProxy()
proxy.start()
cnames = proxy.get_available_components()
comp = {}
cnames = [q for q in cnames if q.find('actuator_ec') != -1]
if lt == 0 or lt == 3:
cnames = [q for q in cnames if (q.find('_ma') != -1)]
if lt == 1 or lt == 4:
cnames = [q for q in cnames if (q.find('_mt') != -1)]
if lt == 2:
cnames = [q for q in cnames if (q.find('_mh') != -1)]
for c in cnames:
comp[c] = {
def get_head_name(): while True: print 'Enter Head Number (e.g., 2 for MS2)' return 'ms'+str(m3t.get_int())
def get_joint_id(): print 'Enter Joint ID (e.g., 2 for J2)' return m3t.get_int()
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:
print 'No components selected'
exit()
for c in comps:
proxy.subscribe_status(c)
proxy.publish_command(c)
field = m3t.user_select_msg_field(comps[0].status)
print 'Number of samples [100]?'
ns = m3t.get_int(100)
log = []
for i in range(len(comps)):
def get_pwr_name(): print 'Enter PWR board ID (e.g., 10 for pwr010)' x=m3t.get_int() return 'm3pwr_pwr'+'0'*(3-len(str(x)))+str(x)
'B1.OmniBase':{'generate':m3bs.generate_omnibase},
'B1.OmniBase.Joint':{'generate':m3bs.generate_joint},
'Z1.ZLift.Full':{'generate':m3zl.generate_zlift_full},
'H2.Joint':{'generate':m3h.generate_joint},
'H2.Hand.Full':{'generate':m3h.generate_hand_full},
'H2.Hand':{'generate':m3h.generate_hand},
'A2.LoadX6':{'generate':m3l.generate},
'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...'
#! /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
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()
