def test_torque_tracking(self, auto=False):
if not auto and not self.is_dpm3_ready():
return
res = {}
for k in self.test_default[self.jid]['test_torques'].keys():
amp = self.test_default[self.jid]['test_torques'][k]
print '------------------'
print 'Tracking test:', k, ':', amp
log_torque_mNm, log_load_mNm = self.sweep_torque_sine(
amp, zero=True, loadcell=True)
err = utt.measure_avg_error(log_torque_mNm, log_load_mNm)
acc = utt.measure_accuracy(log_torque_mNm, log_load_mNm)
print 'Average err', err
print 'Accuracy (%)', acc
res[k] = {
'tracking_err_mNm': err,
'tracking_accuracy_pct': acc,
'log_torque_mNm': log_torque_mNm,
'log_load_mNm': log_load_mNm
}
m3t.mplot2(range(len(log_torque_mNm)),
log_load_mNm,
log_torque_mNm,
xlabel='Samples',
ylabel='Torque (mNm)',
y1name='loadcell',
y2name='actuator')
self.write_test_results({'test_torque_tracking': res})
def experiment_b(self):
print 'Hello B!'
# Command motor to run sine curvature
print 'Prepare experiment. Hit enter when ready'
raw_input()
print 'Enable power. Hit enter when ready'
raw_input()
self.step() #send current commands out to dsp and read sensors
adc_zero=self.comp_ec.status.adc_torque
ns=100.0
amp = 100;
# Generate Sine curvature as
x_temp=nu.arange(0,50.0,1/ns)
sin_curvature = nu.sin(x_temp)*amp
#Place actuator in torque mode
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
log_adc_torque=[]
log_load_mNm=[]
self.comp_ec.command.t_desire=0 #Initial desired torque
#Send out intial configuration (zero torque) and wait 1 second
self.step(time_sleep=1.0)
for f in sin_curvature:
self.comp_ec.command.t_desire=int(f+adc_zero) #send out command torque
self.step(time_sleep=0.05) #send new desired out and wait
lmNm=self.dpm3.get_load_mNm()#read load-cell
tq=self.comp_ec.status.adc_torque #read raw torque sensor (ticks)
print '------------------------'
print 'Fwd',f
print 'Adc SEA',tq
print 'mNm Load Cell',lmNm
log_adc_torque.append(tq) #log data
log_load_mNm.append(lmNm)
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF #Turn off actuator
self.step()#send out command
#Least squares fit to get nominal calibration
poly,inv_poly=self.get_polyfit_to_data(x=log_adc_torque,y=log_load_mNm,n=1,draw=False)
#Compute calibrated view of raw torque data
s=m3tc.PolyEval(poly,log_adc_torque)
#Draw the comparison between loadcell and torque sensor
m3t.mplot2(range(len(log_adc_torque)),log_load_mNm,s,xlabel='Samples',ylabel='Torque (mNm)',
y1name='loadcell',y2name='actuator')
#Save raw data
self.write_raw_calibration({'log_adc_torque':log_adc_torque,\
'log_load_mNm':log_load_mNm})
def test_torque_tracking(self,auto=False):
if not auto and not self.is_dpm3_ready():
return
res={}
for k in self.test_default[self.jid]['test_torques'].keys():
amp=self.test_default[self.jid]['test_torques'][k]
print '------------------'
print 'Tracking test:',k,':',amp
log_torque_mNm, log_load_mNm=self.sweep_torque_sine(amp,zero=True,loadcell=True)
err=utt.measure_avg_error(log_torque_mNm, log_load_mNm)
acc=utt.measure_accuracy(log_torque_mNm, log_load_mNm)
print 'Average err',err
print 'Accuracy (%)',acc
res[k]={'tracking_err_mNm':err,'tracking_accuracy_pct':acc,'log_torque_mNm':log_torque_mNm,'log_load_mNm':log_load_mNm}
m3t.mplot2(range(len(log_torque_mNm)),log_load_mNm,log_torque_mNm,xlabel='Samples',ylabel='Torque (mNm)',
y1name='loadcell',y2name='actuator')
self.write_test_results({'test_torque_tracking':res})
exit() comp=m3f.create_component(name_ec) proxy.subscribe_status(comp) proxy.make_operational(name_ec) q_log=[] tq_log=[] print 'Ready to generate motion? Hit any key to start' m3t.get_keystroke() ts=time.time() try: while time.time()-ts>5.0: proxy.step() q=comp.status.qei_on tq=comp.status.adc_torque time.sleep(0.25) print 'DT',time.time()-ts, 'Q',q,'TQ',tq q_log.append(q) tq_log.append(tq) except (KeyboardInterrupt,EOFError): proxy.stop() poly,inv_poly=m3tc.get_polyfit_to_data(q_log,tq_log,n=1) print 'Poly',poly s=m3t.PolyEval(poly,log_tq) m3t.mplot2(range(len(log_q)),log_load_mNm,s,xlabel='Samples',ylabel='Torque (mNm)', y1name='loadcell',y2name='actuator')
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):
print 'Raw data file', fn, 'not available'
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):
print 'Raw data file',fn,'not available'
def experiment_a(self):
print 'Prepare experiment. Hit enter when ready'
raw_input()
print 'Enable power. Hit enter when ready'
raw_input()
self.step() #send current commands out to dsp and read sensors
adc_zero=self.comp_ec.status.adc_torque
print 'Enter amplitude (ticks)[100]'
amp=m3t.get_float(100)
ns=100.0
#Sawtooth torque profile
fwd=nu.arange(0,1.0,1/ns)*amp
rev=nu.arange(0,-1.0,-1/ns)*amp
zero=nu.zeros(int(ns))
fwd=fwd.tolist()+zero.tolist()
rev=rev.tolist()+zero.tolist()
#Place actuator in torque mode
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_TORQUE
log_adc_torque=[]
log_load_mNm=[]
self.comp_ec.command.t_desire=0 #Initial desired torque
#Send out intial configuration (zero torque) and wait 1 second
self.step(time_sleep=1.0)
for f in fwd:
self.comp_ec.command.t_desire=int(f+adc_zero) #send out command torque
self.step(time_sleep=0.05) #send new desired out and wait
lmNm=self.dpm3.get_load_mNm()#read load-cell
tq=self.comp_ec.status.adc_torque #read raw torque sensor (ticks)
print '------------------------'
print 'Fwd',f
print 'Adc SEA',tq
print 'mNm Load Cell',lmNm
log_adc_torque.append(tq) #log data
log_load_mNm.append(lmNm)
for r in rev:
self.comp_ec.command.t_desire=int(r+adc_zero) #send out command torque
self.step(time_sleep=0.05)
lmNm=self.dpm3.get_load_mNm()#-load_zero
tq=self.comp_ec.status.adc_torque
print '------------------------'
print 'Rev',r
print 'Adc SEA',tq
print 'mNm Load Cell',lmNm
log_adc_torque.append(tq)
log_load_mNm.append(lmNm)
self.comp_ec.command.mode=aec.ACTUATOR_EC_MODE_OFF #Turn off actuator
self.step()#send out command
#Least squares fit to get nominal calibration
poly,inv_poly=self.get_polyfit_to_data(x=log_adc_torque,y=log_load_mNm,n=1,draw=False)
#Compute calibrated view of raw torque data
s=m3tc.PolyEval(poly,log_adc_torque)
#Draw the comparison between loadcell and torque sensor
m3t.mplot2(range(len(log_adc_torque)),log_load_mNm,s,xlabel='Samples',ylabel='Torque (mNm)',
y1name='loadcell',y2name='actuator')
#Save raw data
self.write_raw_calibration({'log_adc_torque':log_adc_torque,\
'log_load_mNm':log_load_mNm})
comp = m3f.create_component(name_ec)
proxy.subscribe_status(comp)
proxy.make_operational(name_ec)
q_log = []
tq_log = []
print 'Ready to generate motion? Hit any key to start'
m3t.get_keystroke()
ts = time.time()
try:
while time.time() - ts > 5.0:
proxy.step()
q = comp.status.qei_on
tq = comp.status.adc_torque
time.sleep(0.25)
print 'DT', time.time() - ts, 'Q', q, 'TQ', tq
q_log.append(q)
tq_log.append(tq)
except (KeyboardInterrupt, EOFError):
proxy.stop()
poly, inv_poly = m3tc.get_polyfit_to_data(q_log, tq_log, n=1)
print 'Poly', poly
s = m3t.PolyEval(poly, log_tq)
m3t.mplot2(range(len(log_q)),
log_load_mNm,
s,
xlabel='Samples',
ylabel='Torque (mNm)',
y1name='loadcell',
y2name='actuator')
