def calibrate_torque(self):
self.proxy.publish_command(self.comp_rt)
self.proxy.publish_param(self.comp_rt)
self.proxy.make_operational(self.name_rt)
self.step()
print 'Make sure other digit is all the way open'
print 'Place digit in zero load condition'
print 'Hit enter when ready'
raw_input()
self.step()
raw_a=int(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
load_a=0
print 'Hang 1Kg weight from gripper near slider'
print 'Hit enter to move joint in first direction.'
raw_input()
self.comp_rt.set_mode_pwm()
print 'Desired pwm? [',self.param_internal['pwm_torque'][0],']?'
p=int(m3t.get_float(self.param_internal['pwm_theta'][0]))
self.comp_rt.set_pwm(p)
self.step()
print 'Hit any key when ready to sample'
raw_input()
raw_b=int(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
print 'Was load in the opening direction [y]?'
if m3t.get_yes_no('y'):
load_b=m3u.g2mN(1000.0)*self.comp_j.config['calib']['cb_drive_radius_m']
else:
load_b=m3u.g2mN(-1000.0)*self.comp_j.config['calib']['cb_drive_radius_m']
print 'Hit enter to move joint in second direction.'
raw_input()
self.comp_rt.set_mode_pwm()
print 'Desired pwm? [',self.param_internal['pwm_torque'][1],']?'
p=int(m3t.get_float(self.param_internal['pwm_theta'][1]))
self.comp_rt.set_pwm(p)
self.step()
print 'Hit any key when ready to sample'
raw_input()
raw_c=int(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
load_c=-1*load_b
log_adc_torque=[raw_a,raw_b,raw_c]
log_load_mNm=[load_a,load_b,load_c]
poly,inv_poly=self.get_polyfit_to_data(x=log_adc_torque,y=log_load_mNm,n=1)
self.write_raw_calibration({'log_adc_torque':log_adc_torque,'log_load_mNm':log_load_mNm,
'cb_torque':poly,'cb_inv_torque':inv_poly})
self.comp_rt.config['calib']['torque']['cb_torque']=poly
self.comp_rt.config['calib']['torque']['cb_inv_torque']=inv_poly
print 'Poly',poly
s=m3tc.PolyEval(poly,[raw_a,raw_b,raw_c])
m3t.mplot2(range(len(log_adc_torque)),log_load_mNm,s,xlabel='Samples',ylabel='Torque (mNm)',
y1name='load',y2name='raw')
def analyze_torque_gravity_comp(self):
print 'This routine requires Torque mode control of a known weighted lever'
print 'Theta = 0 should be calibrated to vertical'
print 'Theta = 90 should be calibrated to horizontal (pointing right when facing actuator output).'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
self.proxy.publish_command(self.comp_rt)
self.proxy.publish_param(self.comp_rt)
self.proxy.subscribe_status(self.comp_rt)
self.proxy.make_operational(self.name_rt)
print 'Enter test lever mass (g) [', self.param_internal[
'calib_lever_mass'], ']'
tlm = m3t.get_float(self.param_internal['calib_lever_mass'])
print 'Enter test lever COM (mm) [', self.param_internal[
'calib_lever_com'], ']'
com = m3t.get_float(self.param_internal['calib_lever_com'])
print 'Enter test lever length (mm) [', self.param_internal[
'calib_lever_len'], ']'
tll = m3t.get_float(self.param_internal['calib_lever_len'])
print 'Enter payload (g)[500]'
tp = m3t.get_float(500)
self.comp_rt.set_mode_torque()
tq_tl = m3u.g2mN(tlm) * com / 1000.0
tq_p = m3u.g2mN(tp) * tll / 1000.0
print 'Test lever torque (at 90 deg) (mNm)', tq_tl
print 'Payload torque (at 90 deg) (mNm)', tq_p
ts = time.time()
while True:
tqg = 1.0 * math.sin(self.comp_rt.get_theta_rad()) * (tq_tl + tq_p)
self.comp_rt.set_torque_mNm(tqg)
self.step()
e = tqg - self.comp_rt.get_torque_mNm()
print 'Tqg', tqg, 'Dt', time.time() - ts, 'Error', e
if time.time() - ts > 20.0:
print 'Continue [y]?'
if m3t.get_yes_no('y'):
print 'Enter payload (g)[', tp, ']'
tp = m3t.get_float(tp)
self.comp_rt.set_mode_torque()
tq_tl = m3u.g2mN(tlm) * tll / 1000.0
tq_p = m3u.g2mN(tp) * tll / 1000.0
ts = time.time()
else:
break
self.comp_rt.set_mode_off()
self.proxy.make_safe_operational(self.name_rt)
self.step()
def calibrate_load_cell(self):
print 'Place sensor in zero load configuration'
print 'Hit any key to continue'
raw_input()
sensors = [
'adc_load_0', 'adc_load_1', 'adc_load_2', 'adc_load_3',
'adc_load_4', 'adc_load_5'
]
self.step()
z = self.get_sensor_list_avg(sensors, 3.0)
print 'Zero'
print z
self.comp_rt.config['calib']['wrench']['cb_adc_bias'][0] = float(
z['adc_load_0'])
self.comp_rt.config['calib']['wrench']['cb_adc_bias'][1] = float(
z['adc_load_1'])
self.comp_rt.config['calib']['wrench']['cb_adc_bias'][2] = float(
z['adc_load_2'])
self.comp_rt.config['calib']['wrench']['cb_adc_bias'][3] = float(
z['adc_load_3'])
self.comp_rt.config['calib']['wrench']['cb_adc_bias'][4] = float(
z['adc_load_4'])
self.comp_rt.config['calib']['wrench']['cb_adc_bias'][5] = float(
z['adc_load_5'])
m = [500.0, 1000.0, 2000.0]
log_mN = []
log_fZ = []
for g in m:
print 'Place ', g, '(g) load on sensor in negative Fz direction'
print 'Hit any key to continue'
raw_input()
sensors = [
'adc_load_0', 'adc_load_1', 'adc_load_2', 'adc_load_3',
'adc_load_4', 'adc_load_5'
]
self.step()
z = self.get_sensor_list_avg(sensors, 2.0)
self.comp_rt.config['calib']['wrench'][
'cb_scale'] = 1.0 #just determin this and zero
w = self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'],
z['adc_load_0'], z['adc_load_1'],
z['adc_load_2'], z['adc_load_3'],
z['adc_load_4'], z['adc_load_5'])
#print 'Enter weight (g)'
mN = -1.0 * m3u.g2mN(g) #m3u.g2mN(m3t.get_float())
log_mN.append(mN)
est = w[2] #current Fz estimation, N
log_fZ.append(est)
print 'Measured (g)', m3u.mN2g(mN)
print 'Estimated (g)', m3u.mN2g(est)
scale = mN / est
print 'Scale', scale
poly, inv_poly = self.get_polyfit_to_data(x=log_fZ, y=log_mN, n=1)
print 'Estimated scale', poly[0]
self.comp_rt.config['calib']['wrench']['cb_scale'] = poly[0]
self.comp_rt.config['calib']['wrench']['cb_bias'] = [0.0] * 6
w = self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'],
z['adc_load_0'], z['adc_load_1'],
z['adc_load_2'], z['adc_load_3'],
z['adc_load_4'], z['adc_load_5'])
def zero_load_cell_with_payload(self):
print 'Place arm with mass pulling straight down on FTS'
print 'Enter payload mass (g) [834g=H2R2wFTS]'
payload = m3u.g2mN(m3t.get_float(834))
sensors = [
'adc_load_0', 'adc_load_1', 'adc_load_2', 'adc_load_3',
'adc_load_4', 'adc_load_5'
]
self.step()
z = self.get_sensor_list_avg(sensors, 3.0)
print 'Sensor readings: ', z
w = self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'],
z['adc_load_0'], z['adc_load_1'],
z['adc_load_2'], z['adc_load_3'],
z['adc_load_4'], z['adc_load_5'])
print 'Old payload', m3u.mN2g(w[2])
w[2] = w[2] - payload
bc = nu.array(self.comp_rt.config['calib']['wrench']['cb_bias'])
self.comp_rt.config['calib']['wrench']['cb_bias'] = list(bc - w)
w = self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'],
z['adc_load_0'], z['adc_load_1'],
z['adc_load_2'], z['adc_load_3'],
z['adc_load_4'], z['adc_load_5'])
print 'New payload: ', m3u.mN2g(w[2])
print 'New wrench', w
def calibrate_torque(self): print 'Place load cell in unloaded configuration. Hit enter when ready' raw_input() self.step() adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque']) print 'adc_zero',adc_zero print 'Number of calibration weights [3]?' nw=m3t.get_int(3) print 'Calibration hub diameter (mm)[70]?' ch=m3t.get_float(70) adc_act=[adc_zero] load_mNm=[0.0] for i in range(nw): print 'Place positive load ',i,'. Hit enter when ready' raw_input() self.step() adc_act.append(float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])) print 'adc_act positive',adc_act[-1] print 'Place negative load ',i,'. Hit enter when ready' raw_input() self.step() adc_act.append(float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])) print 'adc_act negative',adc_act[-1] print 'Enter load weight (g)' w=m3u.g2mN(m3t.get_float())*(ch/1000.0) load_mNm.append(w) load_mNm.append(-w) print 'Calibration load (mNm)',load_mNm[-1] print adc_act print load_mNm poly,inv_poly=self.get_polyfit_to_data(adc_act,load_mNm,n=1) self.comp_rt.config['calib']['torque']['cb_torque']=list(poly) self.comp_rt.config['calib']['torque']['cb_inv_torque']=list(inv_poly)
def zero_load_cell_with_payload(self): print 'Place arm with mass pulling straight down on FTS' print 'Enter payload mass (g) [834g=H2R2wFTS]' payload=m3u.g2mN(m3t.get_float(834)) sensors=['adc_load_0','adc_load_1','adc_load_2','adc_load_3','adc_load_4','adc_load_5'] self.step() z=self.get_sensor_list_avg(sensors,3.0) print 'Sensor readings: ',z w=self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'], z['adc_load_0'], z['adc_load_1'], z['adc_load_2'], z['adc_load_3'], z['adc_load_4'], z['adc_load_5']) print 'Old payload',m3u.mN2g(w[2]) w[2]=w[2]-payload bc=nu.array(self.comp_rt.config['calib']['wrench']['cb_bias']) self.comp_rt.config['calib']['wrench']['cb_bias']=list(bc-w) w=self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'], z['adc_load_0'], z['adc_load_1'], z['adc_load_2'], z['adc_load_3'], z['adc_load_4'], z['adc_load_5']) print 'New payload: ',m3u.mN2g(w[2]) print 'New wrench',w
def analyze_torque_gravity_comp(self):
print 'This routine requires Torque mode control of a known weighted lever'
print 'Theta = 0 should be calibrated to vertical'
print 'Theta = 90 should be calibrated to horizontal (pointing right when facing actuator output).'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
self.proxy.publish_command(self.comp_rt)
self.proxy.publish_param(self.comp_rt)
self.proxy.subscribe_status(self.comp_rt)
self.proxy.make_operational(self.name_rt)
print 'Enter test lever mass (g) [',self.param_internal['calib_lever_mass'],']'
tlm=m3t.get_float(self.param_internal['calib_lever_mass'])
print 'Enter test lever COM (mm) [',self.param_internal['calib_lever_com'],']'
com=m3t.get_float(self.param_internal['calib_lever_com'])
print 'Enter test lever length (mm) [',self.param_internal['calib_lever_len'],']'
tll=m3t.get_float(self.param_internal['calib_lever_len'])
print 'Enter payload (g)[500]'
tp=m3t.get_float(500)
self.comp_rt.set_mode_torque()
tq_tl=m3u.g2mN(tlm)*com/1000.0
tq_p= m3u.g2mN(tp)*tll/1000.0
print 'Test lever torque (at 90 deg) (mNm)',tq_tl
print 'Payload torque (at 90 deg) (mNm)',tq_p
ts=time.time()
while True:
tqg=1.0*math.sin(self.comp_rt.get_theta_rad())*(tq_tl+tq_p)
self.comp_rt.set_torque_mNm(tqg)
self.step()
e=tqg-self.comp_rt.get_torque_mNm()
print 'Tqg',tqg,'Dt',time.time()-ts, 'Error',e
if time.time()-ts>20.0:
print 'Continue [y]?'
if m3t.get_yes_no('y'):
print 'Enter payload (g)[',tp,']'
tp=m3t.get_float(tp)
self.comp_rt.set_mode_torque()
tq_tl=m3u.g2mN(tlm)*tll/1000.0
tq_p= m3u.g2mN(tp)*tll/1000.0
ts=time.time()
else:
break
self.comp_rt.set_mode_off()
self.proxy.make_safe_operational(self.name_rt)
self.step()
def calibrate_torque_servo_theta(self):
print 'This routine requires M3Joint Theta mode control of the hub'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
print 'Enter pose theta [90]'
pose=m3t.get_float(90)
print 'Number of calibration weights [2]?'
nw=m3t.get_int(2)
print 'Calibration hub diameter (mm)[',self.param_internal['calib_hub_diam'],']?'
ch=m3t.get_float(self.param_internal['calib_hub_diam'])/2.0
print 'Place actuator in unloaded configuration. Hit enter when ready'
raw_input()
self.step()
adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
print 'adc_zero',adc_zero
adc_act=[adc_zero]
load_mNm=[0.0]
print 'Turn on power. Hit enter when ready'
raw_input()
self.proxy.subscribe_status(self.comp_j)
self.proxy.publish_command(self.comp_j)
self.proxy.publish_param(self.comp_j)
self.proxy.make_operational(self.name_j)
self.proxy.make_operational(self.name_rt)
self.comp_j.set_mode_theta()
self.comp_j.set_theta_deg(pose)
self.comp_j.set_slew_rate(25.0)
self.step(3.0)
for i in range(nw):
print 'Place positive load ',i,'. Hit enter when ready'
raw_input()
self.step()
adc_act.append(float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque']))
print 'adc_act',adc_act[-1]
print 'Enter load weight (g)'
w=m3u.g2mN(m3t.get_float())*(ch/1000.0)
load_mNm.append(w)
print 'Calibration load (mNm)',load_mNm[-1]
print adc_act
print load_mNm
print 'Enter degree [',self.param_internal['calib_tq_degree'],']?'
n=m3t.get_int(self.param_internal['calib_tq_degree'])
poly,inv_poly=self.get_polyfit_to_data(adc_act,load_mNm,n=n)
self.comp_rt.config['calib']['torque']['cb_torque']=list(poly)
self.comp_rt.config['calib']['torque']['cb_inv_torque']=list(inv_poly)
self.comp_j.set_mode_off()
self.step()
self.proxy.make_safe_operational(self.name_j)
self.proxy.make_safe_operational(self.name_rt)
self.step()
self.write_raw_calibration({'log_adc_torque':adc_act,'log_load_mNm':load_mNm,
'cb_torque':poly,'cb_inv_torque':inv_poly})
def calibrate_load_cell(self): print 'Place sensor in zero load configuration' print 'Hit any key to continue' raw_input() sensors=['adc_load_0','adc_load_1','adc_load_2','adc_load_3','adc_load_4','adc_load_5'] self.step() z=self.get_sensor_list_avg(sensors,3.0) print 'Zero' print z self.comp_rt.config['calib']['wrench']['cb_adc_bias'][0]=float(z['adc_load_0']) self.comp_rt.config['calib']['wrench']['cb_adc_bias'][1]=float(z['adc_load_1']) self.comp_rt.config['calib']['wrench']['cb_adc_bias'][2]=float(z['adc_load_2']) self.comp_rt.config['calib']['wrench']['cb_adc_bias'][3]=float(z['adc_load_3']) self.comp_rt.config['calib']['wrench']['cb_adc_bias'][4]=float(z['adc_load_4']) self.comp_rt.config['calib']['wrench']['cb_adc_bias'][5]=float(z['adc_load_5']) m=[500.0,1000.0,2000.0] log_mN=[] log_fZ=[] for g in m: print 'Place ',g,'(g) load on sensor in negative Fz direction' print 'Hit any key to continue' raw_input() sensors=['adc_load_0','adc_load_1','adc_load_2','adc_load_3','adc_load_4','adc_load_5'] self.step() z=self.get_sensor_list_avg(sensors,2.0) self.comp_rt.config['calib']['wrench']['cb_scale']=1.0 #just determin this and zero w=self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'], z['adc_load_0'], z['adc_load_1'], z['adc_load_2'], z['adc_load_3'], z['adc_load_4'], z['adc_load_5']) #print 'Enter weight (g)' mN=-1.0*m3u.g2mN(g)#m3u.g2mN(m3t.get_float()) log_mN.append(mN) est=w[2] #current Fz estimation, N log_fZ.append(est) print 'Measured (g)',m3u.mN2g(mN) print 'Estimated (g)',m3u.mN2g(est) scale=mN/est print 'Scale',scale poly,inv_poly=self.get_polyfit_to_data(x=log_fZ,y=log_mN,n=1) print 'Estimated scale',poly[0] self.comp_rt.config['calib']['wrench']['cb_scale']=poly[0] self.comp_rt.config['calib']['wrench']['cb_bias']=[0.0]*6 w=self.wrench.raw_2_mNm(self.comp_rt.config['calib']['wrench'], z['adc_load_0'], z['adc_load_1'], z['adc_load_2'], z['adc_load_3'], z['adc_load_4'], z['adc_load_5'])
def calibrate_torque_hub(self, run_neg=True):
print 'This calibration requires a hub with known diameter loaded by a weight'
print 'Place actuator in unloaded configuration. Hit enter when ready'
raw_input()
self.step()
adc_zero = float(
self.get_sensor_list_avg(['adc_torque'], 1.0)['adc_torque'])
print 'adc_zero', adc_zero
print 'Number of calibration weights [3]?'
nw = m3t.get_int(3)
print 'Calibration hub diameter (mm)[', self.param_internal[
'calib_hub_diam'], ']?'
ch = m3t.get_float(self.param_internal['calib_hub_diam']) / 2.0
adc_act = [adc_zero]
load_mNm = [0.0]
for i in range(nw):
print 'Place positive load ', i, '. Hit enter when ready'
raw_input()
self.step()
adc_act.append(
float(
self.get_sensor_list_avg(['adc_torque'],
1.0)['adc_torque']))
print 'adc_act positive', adc_act[-1]
if run_neg:
print 'Place negative load ', i, '. Hit enter when ready'
raw_input()
self.step()
adc_act.append(
float(
self.get_sensor_list_avg(['adc_torque'],
1.0)['adc_torque']))
print 'adc_act negative', adc_act[-1]
print 'Enter load weight (g)'
w = m3u.g2mN(m3t.get_float()) * (ch / 1000.0)
load_mNm.append(w)
if run_neg:
load_mNm.append(-w)
print 'Calibration load (mNm)', load_mNm[-1]
print adc_act
print load_mNm
print 'Enter degree [', self.param_internal['calib_tq_degree'], ']?'
n = m3t.get_int(self.param_internal['calib_tq_degree'])
poly, inv_poly = self.get_polyfit_to_data(adc_act, load_mNm, n=n)
self.comp_rt.config['calib']['torque']['cb_torque'] = list(poly)
self.comp_rt.config['calib']['torque']['cb_inv_torque'] = list(
inv_poly)
def calibrate_torque(self):
print 'Place load cell in unloaded configuration. Hit enter when ready'
raw_input()
self.step()
adc_zero = float(
self.get_sensor_list_avg(['adc_torque'], 1.0)['adc_torque'])
print 'adc_zero', adc_zero
print 'Number of calibration weights [3]?'
nw = m3t.get_int(3)
print 'Calibration hub diameter (mm)[70]?'
ch = m3t.get_float(70)
adc_act = [adc_zero]
load_mNm = [0.0]
for i in range(nw):
print 'Place positive load ', i, '. Hit enter when ready'
raw_input()
self.step()
adc_act.append(
float(
self.get_sensor_list_avg(['adc_torque'],
1.0)['adc_torque']))
print 'adc_act positive', adc_act[-1]
print 'Place negative load ', i, '. Hit enter when ready'
raw_input()
self.step()
adc_act.append(
float(
self.get_sensor_list_avg(['adc_torque'],
1.0)['adc_torque']))
print 'adc_act negative', adc_act[-1]
print 'Enter load weight (g)'
w = m3u.g2mN(m3t.get_float()) * (ch / 1000.0)
load_mNm.append(w)
load_mNm.append(-w)
print 'Calibration load (mNm)', load_mNm[-1]
print adc_act
print load_mNm
poly, inv_poly = self.get_polyfit_to_data(adc_act, load_mNm, n=1)
self.comp_rt.config['calib']['torque']['cb_torque'] = list(poly)
self.comp_rt.config['calib']['torque']['cb_inv_torque'] = list(
inv_poly)
def calibrate_torque_hub(self,run_neg=True): print 'This calibration requires a hub with known diameter loaded by a weight' print 'Place actuator in unloaded configuration. Hit enter when ready' raw_input() self.step() adc_zero=float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque']) print 'adc_zero',adc_zero print 'Number of calibration weights [3]?' nw=m3t.get_int(3) print 'Calibration hub diameter (mm)[',self.param_internal['calib_hub_diam'],']?' ch=m3t.get_float(self.param_internal['calib_hub_diam'])/2.0 adc_act=[adc_zero] load_mNm=[0.0] for i in range(nw): print 'Place positive load ',i,'. Hit enter when ready' raw_input() self.step() adc_act.append(float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])) print 'adc_act positive',adc_act[-1] if run_neg: print 'Place negative load ',i,'. Hit enter when ready' raw_input() self.step() adc_act.append(float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])) print 'adc_act negative',adc_act[-1] print 'Enter load weight (g)' w=m3u.g2mN(m3t.get_float())*(ch/1000.0) load_mNm.append(w) if run_neg: load_mNm.append(-w) print 'Calibration load (mNm)',load_mNm[-1] print adc_act print load_mNm print 'Enter degree [',self.param_internal['calib_tq_degree'],']?' n=m3t.get_int(self.param_internal['calib_tq_degree']) poly,inv_poly=self.get_polyfit_to_data(adc_act,load_mNm,n=n) self.comp_rt.config['calib']['torque']['cb_torque']=list(poly) self.comp_rt.config['calib']['torque']['cb_inv_torque']=list(inv_poly)
def calibrate_torque_weighted_lever(self):
print 'This routine requires M3Joint Theta mode control of a weighted lever'
print 'Theta = 0 should be calibrated to vertical'
print 'Theta = 90 should be calibrated to horizontal (pointing right when facing actuator output).'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
print 'Calibration lever COM (mm)[',self.param_internal['calib_lever_com'],']?'
com=m3t.get_float(self.param_internal['calib_lever_com'])
print 'Calibration lever mass (g)[',self.param_internal['calib_lever_mass'],']?'
cm=m3t.get_float(self.param_internal['calib_lever_mass'])
clq = m3u.g2mN(cm)*(com/1000.0)
print 'Calibration lever len (mm)[',self.param_internal['calib_lever_len'],']?'
cl=m3t.get_float(self.param_internal['calib_lever_len'])
print 'Calibration lever torque (mNm): ',clq
print 'Number of calibration weights [3]?'
nw=m3t.get_int(3)
print 'Place actuator in unloaded configuration'
print 'Give a small impulse in first direction, let return to zero'
print 'Hit any key when ready'
raw_input()
raw_a=int(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
print 'Give a small impulse in second direction, let return to zero'
print 'Hit any key when ready'
raw_input()
raw_b=int(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque'])
adc_zero=raw_a+(raw_b-raw_a)/2.0
print 'adc_zero',adc_zero
print 'Turn on power. Hit enter when ready'
raw_input()
poses=[90,45,-45,-90]
self.proxy.subscribe_status(self.comp_j)
self.proxy.publish_command(self.comp_j)
self.proxy.publish_param(self.comp_j)
self.proxy.make_operational(self.name_j)
self.proxy.make_operational(self.name_rt)
self.comp_j.set_mode_theta()
self.comp_j.set_slew_rate(25.0)
self.step()
adc_act=[adc_zero]
load_mNm=[0.0]
for i in range(nw):
print 'Enter load weight (g)'
w=m3u.g2mN(m3t.get_float())*(cl/1000.0)
for pose in poses:
print 'Posing arm. Hit enter when ready'
raw_input()
self.comp_j.set_theta_deg(pose)
self.step(time_sleep=3.0)
print 'Acquiring torque. Hit enter when ready'
raw_input()
self.step()
adc_act.append(float(self.get_sensor_list_avg(['adc_torque'],1.0)['adc_torque']))
print 'adc_act',adc_act[-1]
gc=math.sin(self.comp_j.get_theta_rad())*(w+clq)
print 'Theta',self.comp_j.get_theta_rad(),'TorqueV',(w+clq)
load_mNm.append(gc)
print 'Calibration load (mNm)',load_mNm[-1]
print adc_act
print load_mNm
print 'Enter degree [',self.param_internal['calib_tq_degree'],']?'
n=m3t.get_int(self.param_internal['calib_tq_degree'])
poly,inv_poly=self.get_polyfit_to_data(adc_act,load_mNm,n=n)
self.comp_rt.config['calib']['torque']['cb_torque']=list(poly)
self.comp_rt.config['calib']['torque']['cb_inv_torque']=list(inv_poly)
self.comp_j.set_mode_off()
self.step()
self.proxy.make_safe_operational(self.name_j)
self.proxy.make_safe_operational(self.name_rt)
self.step()
self.write_raw_calibration({'log_adc_torque':adc_act,'log_load_mNm':load_mNm,
'cb_torque':poly,'cb_inv_torque':inv_poly})
def calibrate_torque_weighted_lever(self):
print 'This routine requires M3Joint Theta mode control of a weighted lever'
print 'Theta = 0 should be calibrated to vertical'
print 'Theta = 90 should be calibrated to horizontal (pointing right when facing actuator output).'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
print 'Calibration lever COM (mm)[', self.param_internal[
'calib_lever_com'], ']?'
com = m3t.get_float(self.param_internal['calib_lever_com'])
print 'Calibration lever mass (g)[', self.param_internal[
'calib_lever_mass'], ']?'
cm = m3t.get_float(self.param_internal['calib_lever_mass'])
clq = m3u.g2mN(cm) * (com / 1000.0)
print 'Calibration lever len (mm)[', self.param_internal[
'calib_lever_len'], ']?'
cl = m3t.get_float(self.param_internal['calib_lever_len'])
print 'Calibration lever torque (mNm): ', clq
print 'Number of calibration weights [3]?'
nw = m3t.get_int(3)
print 'Place actuator in unloaded configuration'
print 'Give a small impulse in first direction, let return to zero'
print 'Hit any key when ready'
raw_input()
raw_a = int(
self.get_sensor_list_avg(['adc_torque'], 1.0)['adc_torque'])
print 'Give a small impulse in second direction, let return to zero'
print 'Hit any key when ready'
raw_input()
raw_b = int(
self.get_sensor_list_avg(['adc_torque'], 1.0)['adc_torque'])
adc_zero = raw_a + (raw_b - raw_a) / 2.0
print 'adc_zero', adc_zero
print 'Turn on power. Hit enter when ready'
raw_input()
poses = [90, 45, -45, -90]
self.proxy.subscribe_status(self.comp_j)
self.proxy.publish_command(self.comp_j)
self.proxy.publish_param(self.comp_j)
self.proxy.make_operational(self.name_j)
self.proxy.make_operational(self.name_rt)
self.comp_j.set_mode_theta()
self.comp_j.set_slew_rate(25.0)
self.step()
adc_act = [adc_zero]
load_mNm = [0.0]
for i in range(nw):
print 'Enter load weight (g)'
w = m3u.g2mN(m3t.get_float()) * (cl / 1000.0)
for pose in poses:
print 'Posing arm. Hit enter when ready'
raw_input()
self.comp_j.set_theta_deg(pose)
self.step(time_sleep=3.0)
print 'Acquiring torque. Hit enter when ready'
raw_input()
self.step()
adc_act.append(
float(
self.get_sensor_list_avg(['adc_torque'],
1.0)['adc_torque']))
print 'adc_act', adc_act[-1]
gc = math.sin(self.comp_j.get_theta_rad()) * (w + clq)
print 'Theta', self.comp_j.get_theta_rad(), 'TorqueV', (w +
clq)
load_mNm.append(gc)
print 'Calibration load (mNm)', load_mNm[-1]
print adc_act
print load_mNm
print 'Enter degree [', self.param_internal['calib_tq_degree'], ']?'
n = m3t.get_int(self.param_internal['calib_tq_degree'])
poly, inv_poly = self.get_polyfit_to_data(adc_act, load_mNm, n=n)
self.comp_rt.config['calib']['torque']['cb_torque'] = list(poly)
self.comp_rt.config['calib']['torque']['cb_inv_torque'] = list(
inv_poly)
self.comp_j.set_mode_off()
self.step()
self.proxy.make_safe_operational(self.name_j)
self.proxy.make_safe_operational(self.name_rt)
self.step()
self.write_raw_calibration({
'log_adc_torque': adc_act,
'log_load_mNm': load_mNm,
'cb_torque': poly,
'cb_inv_torque': inv_poly
})
def calibrate_torque_servo_theta(self):
print 'This routine requires M3Joint Theta mode control of the hub'
print 'Proceed [y]?'
if not m3t.get_yes_no('y'):
return
print 'Enter pose theta [90]'
pose = m3t.get_float(90)
print 'Number of calibration weights [2]?'
nw = m3t.get_int(2)
print 'Calibration hub diameter (mm)[', self.param_internal[
'calib_hub_diam'], ']?'
ch = m3t.get_float(self.param_internal['calib_hub_diam']) / 2.0
print 'Place actuator in unloaded configuration. Hit enter when ready'
raw_input()
self.step()
adc_zero = float(
self.get_sensor_list_avg(['adc_torque'], 1.0)['adc_torque'])
print 'adc_zero', adc_zero
adc_act = [adc_zero]
load_mNm = [0.0]
print 'Turn on power. Hit enter when ready'
raw_input()
self.proxy.subscribe_status(self.comp_j)
self.proxy.publish_command(self.comp_j)
self.proxy.publish_param(self.comp_j)
self.proxy.make_operational(self.name_j)
self.proxy.make_operational(self.name_rt)
self.comp_j.set_mode_theta()
self.comp_j.set_theta_deg(pose)
self.comp_j.set_slew_rate(25.0)
self.step(3.0)
for i in range(nw):
print 'Place positive load ', i, '. Hit enter when ready'
raw_input()
self.step()
adc_act.append(
float(
self.get_sensor_list_avg(['adc_torque'],
1.0)['adc_torque']))
print 'adc_act', adc_act[-1]
print 'Enter load weight (g)'
w = m3u.g2mN(m3t.get_float()) * (ch / 1000.0)
load_mNm.append(w)
print 'Calibration load (mNm)', load_mNm[-1]
print adc_act
print load_mNm
print 'Enter degree [', self.param_internal['calib_tq_degree'], ']?'
n = m3t.get_int(self.param_internal['calib_tq_degree'])
poly, inv_poly = self.get_polyfit_to_data(adc_act, load_mNm, n=n)
self.comp_rt.config['calib']['torque']['cb_torque'] = list(poly)
self.comp_rt.config['calib']['torque']['cb_inv_torque'] = list(
inv_poly)
self.comp_j.set_mode_off()
self.step()
self.proxy.make_safe_operational(self.name_j)
self.proxy.make_safe_operational(self.name_rt)
self.step()
self.write_raw_calibration({
'log_adc_torque': adc_act,
'log_load_mNm': load_mNm,
'cb_torque': poly,
'cb_inv_torque': inv_poly
})
def set_force_g(self,v):
self.set_force_mN(m3u.g2mN(nu.array(v)))
def set_force_g(self,v): self.set_force_mN(m3u.g2mN(nu.array(v)))
