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_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 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 run(self):
try:
rospy.loginfo("Setting up ROS publishers on parent scope: " + str(self.scope))
rospy.loginfo("Publisher details: " + str(self.component.name) + "/" + str(self.field) + " with type " + str(self.dataType) + " and rate " + str(self.rate))
try:
dt = self.dataType
if dt == "WrenchStamped":
dt = WrenchStamped
else:
dt = Floats
except AttributeError:
dt = Floats
except IndexError:
dt = Floats
self.publisher = rospy.Publisher(str(self.scope) + "/" + str(self.component.name) + "/" + str(self.field), dt, queue_size=1)
self.ros_rate = rospy.Rate(self.rate)
self.running = True
while self.running and not rospy.is_shutdown():
try:
PublisherThread.PublisherLock.acquire()
tmp = m3t.get_msg_field_value(self.component.status, self.field)
PublisherThread.PublisherLock.release()
if dt == WrenchStamped:
float_list = map(float, str(tmp).strip('[]').split(','))
msg = WrenchStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = str(self.component.name)
msg.wrench.force.x = float_list[0]/1000
msg.wrench.force.y = float_list[1]/1000
msg.wrench.force.z = float_list[2]/1000
msg.wrench.torque.x = float_list[3]/1000
msg.wrench.torque.y = float_list[4]/1000
msg.wrench.torque.z = float_list[5]/1000
elif dt == Floats:
msg = Floats()
if hasattr(tmp, '__len__'):
for val in tmp:
msg.data.append(val)
else:
msg.data.append(tmp)
else:
rospy.logerr("unknown Data type " + dt)
self.publisher.publish(msg)
self.ros_rate.sleep()
except IndexError:
rospy.logerr("IndexError caught! Skipping publishing...")
except ValueError:
rospy.logerr("ValueError caught! Skipping publishing...")
except ROSException as e:
rospy.logerr("ROS exception caught! " + str(e))
except:
e = sys.exc_info()[0]
rospy.logerr("Exception caught! " + str(e))
def step(self): self.proxy.step() for c in self.actuator_ec: if self.do_scope and self.scope is None: self.scope=m3t.M3Scope2(xwidth=100,yrange=None) self.status_dict=self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) c.command.mode=int(self.mode[idx]) if self.mode[idx]==mec.ACTUATOR_EC_MODE_PWM: c.command.pwm_desired=int(self.pwm_desire[idx]) print 'Desired',c.name,c.command.pwm_desired if self.mode[idx]==mec.ACTUATOR_EC_MODE_CURRENT: c.command.current_desired=int(self.current_desire[idx]) print 'Desired',c.name,c.command.current_desired if self.do_scope and self.scope is not None: f1=self.scope_keys[self.scope_field1[0]] f2=self.scope_keys[self.scope_field2[0]] x1=x2=None if f1!='None' and f1!='base': x1=m3t.get_msg_field_value(self.actuator_ec[0].status,f1) print f1,':',x1 if f2!='None' and f2!='base': x2=m3t.get_msg_field_value(self.actuator_ec[0].status,f2) print f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' if (self.save and not self.save_last): c.write_config() self.save_last=self.save
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)):
log.append([])
try:
ts=time.time()
for i in range(ns):
proxy.step()
idx=0
print '---------'
for c in comps:
v=m3t.get_msg_field_value(c.status,field)
log[idx].append(v)
idx=idx+1
print i,':',v
time.sleep(0.05)
except (KeyboardInterrupt,EOFError):
pass
proxy.stop(force_safeop=False) #allow other clients to continue running
print log
res={}
p2ps=[]
for i in range(len(comps)):
a=na.array(log[i],na.Float32)
avg=float(a.mean())
std=a.std()
p2p=max(a)-min(a)
def do_dump(proxy):
print 'Starting dump...'
print 'Enter log name [foo]'
logname = m3t.get_string('foo')
ns = proxy.get_log_num_samples(logname)
if ns == 0:
return
print '-------- Available components --------'
names = proxy.get_log_component_names(logname)
for i in range(len(names)):
print names[i]
print '--------------------------------------'
print 'Select component: [', names[0], ']'
name = m3t.get_string(names[0])
print '--------------------------------------'
print 'Num samples available: ', ns
print 'Enter start sample idx: [0]'
start = max(0, m3t.get_int(0))
print 'Enter end sample idx: [', ns - 1, ']'
end = min(ns - 1, m3t.get_int(ns - 1))
print '--------------------------------------'
comp = mcf.create_component(name)
proxy.register_log_component(comp)
fields = []
print 'Dump all data?[n]'
if m3t.get_yes_no('n'):
'''print 'Fetching data...'
fn=m3t.get_m3_log_path()+logname+'/'+logname+'_dump.yaml'
f=file(fn,'w')
print 'Saving...',fn
f.write(yaml.safe_dump(comp.status, default_flow_style=False,width=200))
f.close()'''
fields = comp.status.DESCRIPTOR.fields_by_name.keys()
print fields
else:
print ' Select fields to plot...'
while True:
fields.append(m3t.user_select_msg_field(comp.status))
print 'Another field [n]?'
if not m3t.get_yes_no('n'):
break
print 'Fetching data...'
data = {}
for k in fields:
data[k] = []
print '---------------------------'
print k
print m3t.get_msg_field_value(comp.status, k)
print dir(m3t.get_msg_field_value(comp.status, k))
if hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'):
for j in range(len(m3t.get_msg_field_value(comp.status, k))):
data[k].append([])
for i in range(start, end):
proxy.load_log_sample(logname, i)
for k in fields:
repeated = False
# skip protobuf subclasses (like base) for now. note we'll want this for humanoid
if hasattr(m3t.get_msg_field_value(comp.status, k),
'__metaclass__'):
pass
elif hasattr(m3t.get_msg_field_value(comp.status, k), '__len__'):
for j in range(len(m3t.get_msg_field_value(comp.status, k))):
data[k][j].append(
m3t.get_msg_field_value(comp.status, k)[j])
else:
data[k].append(m3t.get_msg_field_value(comp.status, k))
fn = m3t.get_m3_log_path() + logname + '/' + logname + '_dump.yaml'
f = file(fn, 'w')
print 'Saving...', fn
print data
f.write(yaml.safe_dump(data, default_flow_style=False, width=200))
f.close()
print '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
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)
scope=m3t.M3Scope(xwidth=100,yrange=yrange)
try:
ts=time.time()
while True:
proxy.step()
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)
proxy.subscribe_status(c)
log[c.name]={}
for f in fields:
for x in f:
log[c.name][x]=[]
proxy.step()
time.sleep(0.5)
proxy.step()
# ###########################
ns=30
for i in range(ns):
proxy.step()
print '---------'
for c in comp_ec:
for s in sensor:
v=m3t.get_msg_field_value(c.status,s)
log[c.name][s].append(v)
print c.name,':',s,':',v
time.sleep(0.05)
print log
# ###########################
for idx in range(len(comp_ec)):
print '--------',comp_ec[idx].name,'---------'
c=comp_ec[idx]
rt=comp_rt[idx]
if sensor[0]=='adc_current_a':
avg_a=float(na.array(log[c.name]['adc_current_a'][6:],na.Float32).mean())
avg_b=float(na.array(log[c.name]['adc_current_b'][6:],na.Float32).mean())
current=M3CurrentSensor(rt.config['calib']['current']['type'])
if rt.config['calib']['current']['type']=='adc_linear_5V' or rt.config['calib']['current']['type']=='adc_linear_5V_ns':
io=current.raw_2_mA(rt.config['calib']['current'],avg_a,avg_b)
def get_values(self, req):
"""
Callback for the get_values service request
"""
rospy.loginfo("Requesting values for " + str(req.component) +" " + str(req.field) + " with " + str(req.hz) + " Hz.")
values = []
retries = 3
while retries > 0:
try:
if req.component not in self.comps.keys():
comp = mcf.create_component(req.component)
self.rt_proxy.subscribe_status(comp)
self.comps[req.component] = comp
break
except (CannotSendRequest,ResponseNotReady):
rospy.logwarn( "Response not ready, retrying. Retries remaining: " + str(retries))
retries -= 1
time.sleep(1.0)
if retries == 0:
print "Retries exausted, returning.."
return
while self.comps[req.component].status.base.timestamp == 0: #wait for status to be ready
time.sleep(0.2)
rt_field_vals = m3t.get_msg_field_value(self.comps[req.component].status, req.field)
if hasattr(rt_field_vals, '__len__'):
for val in rt_field_vals:
values.append(str(val))
else:
values.append(str(rt_field_vals))
resp = RequestValuesResponse()
resp.values = values
if (req.component, req.field, req.datatype) not in self.publishers.keys():
rospy.loginfo("Adding "+ str(req.hz)+ " Hz publisher thread for " + str((req.component, req.field)) + "...")
t = PublisherThread(self.scope, self.comps[req.component], req.field, req.datatype, req.hz)
t.start()
timeout = 0
while not t.running and timeout <=5:
time.sleep(1) #waiting
timeout += 1
if t.running:
with self.lock:
self.publishers[req.component, req.field, req.datatype] = t
self.set_max_rate()
rospy.loginfo("..done!")
else:
rospy.logerr("Something went wrong, publisher not created")
else:
rospy.loginfo("publisher already exists")
if req.hz != self.publishers[req.component, req.field, req.datatype].rate:
rospy.loginfo("adjusting rate...")
self.publishers[req.component, req.field, req.datatype].set_hz(req.hz)
self.set_max_rate()
return resp
def step(self): # print self.comps['act_ec']['comp'].status.timestamp if self.do_scope and self.scope is None: self.scope=m3t.M3Scope2(xwidth=100,yrange=None) self.proxy.step() self.cnt=self.cnt+1 self.status_dict=self.proxy.get_status_dict() if self.zero_joint_theta and not self.zero_joint_theta_last: self.joint_theta -= self.act.get_joint_theta() print 'New joint_theta zero',self.joint_theta if self.zero_joint_torque and not self.zero_joint_torque_last: self.joint_torque -= self.act.get_joint_torque() print 'New joint_torque zero',self.joint_torque # # if self.zero_joint_torque_lc and not self.zero_joint_torque_lc_last: # self.joint_torque_lc -= self.act.get_joint_torque_lc() # print 'New joint_torque_lc zero',self.joint_torque_lc # # self.param_dict[self.comp_name]['calibration']['zero_motor_theta'] = self.motor_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_theta'] = self.joint_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_torque'] = self.joint_torque # self.param_dict[self.comp_name]['calibration']['zero_joint_torque_lc'] = self.joint_torque_lc # # self.zero_joint_theta_last = self.zero_joint_theta # self.zero_joint_torque_last = self.zero_joint_torque # self.zero_joint_torque_lc_last = self.zero_joint_torque_lc self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: f1=self.scope_keys[self.scope_field1[0]] f2=self.scope_keys[self.scope_field2[0]] x1=x2=None if f1!='None' and f1!='base': x1=m3t.get_msg_field_value(self.act.status,f1) print f1,':',x1 if f2!='None' and f2!='base': x2=m3t.get_msg_field_value(self.act.status,f2) print f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' if self.mode[0] == 0: #Off self.act.set_mode(mec.ACTUATOR_MODE_OFF) elif self.mode[0] == 1: #Pwm self.act.set_mode(mec.ACTUATOR_MODE_PWM) self.act.set_pwm(self.pwm_desired[0]) elif self.mode[0] == 2: #Current self.act.set_mode(mec.ACTUATOR_MODE_CURRENT) self.act.set_i_desired(self.current_desired[0]*1000.0) else: self.act.set_mode(mec.ACTUATOR_MODE_OFF) if (self.save and not self.save_last): self.act.write_config() self.save_last=self.save
def step(self):
if self.do_scope and self.scope is None:
self.scope = m3t.M3Scope2(xwidth=100, yrange=None)
self.proxy.step()
self.cnt = self.cnt + 1
self.status_dict = self.proxy.get_status_dict()
self.proxy.set_param_from_dict(self.param_dict)
if self.do_scope and self.scope is not None:
ctrl_f1 = self.ctrl_scope_keys[self.ctrl_scope_field1[0]]
ctrl_f2 = self.ctrl_scope_keys[self.ctrl_scope_field2[0]]
x1 = x2 = None
if ctrl_f1 != 'None':
x1 = m3t.get_msg_field_value(self.ctrl.status, ctrl_f1)
print ctrl_f1, ':', x1
if ctrl_f2 != 'None':
x2 = m3t.get_msg_field_value(self.ctrl.status, ctrl_f2)
print ctrl_f2, ':', x2
if x1 == None:
x1 = x2
if x2 == None:
x2 = x1
if x1 != None and x2 != None: #Handle only one value or two
self.scope.plot(x1, x2)
print '-----------------'
self.ctrl.set_traj_mode(self.traj[0])
if self.mode[0] == 0:
self.ctrl.set_mode_off()
# elif self.mode[0]==1:
# self.ctrl.set_mode_pwm()
# self.ctrl.set_pwm(self.current[0])
elif self.mode[0] == 1:
self.ctrl.set_mode_current()
self.ctrl.set_current(self.current[0])
elif self.mode[0] == mec.CTRL_MODE_THETA:
self.ctrl.set_mode_theta()
self.ctrl.set_theta_deg(self.theta[0])
#
elif self.mode[0] == mec.CTRL_MODE_TORQUE:
print "kp: " + str(self.ctrl.param.pid_torque.k_p)
self.ctrl.set_mode_torque()
self.ctrl.set_torque(self.torque[0])
#
# elif self.mode[0]==mec.CTRL_MODE_TORQUE_LC:
# self.ctrl.set_mode_torque_lc()
# self.ctrl.set_torque_lc(self.torque_lc[0])
else:
self.ctrl.set_mode_off()
if (self.save and not self.save_last):
self.ctrl.write_config()
self.save_last = self.save
def step(self): if self.do_scope and self.scope is None: self.scope = m3t.M3Scope2(xwidth=100,yrange=None) self.proxy.step() self.cnt = self.cnt+1 self.status_dict = self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: ctrl_f1 = self.ctrl_scope_keys[self.ctrl_scope_field1[0]] ctrl_f2 = self.ctrl_scope_keys[self.ctrl_scope_field2[0]] x1=x2=None if ctrl_f1 != 'None': x1 = m3t.get_msg_field_value(self.ctrl.status,ctrl_f1) print ctrl_f1,':',x1 if ctrl_f2 != 'None': x2 = m3t.get_msg_field_value(self.ctrl.status,ctrl_f2) print ctrl_f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' self.ctrl.set_traj_mode(self.traj[0]) if self.mode[0]==0: self.ctrl.set_mode_off() # elif self.mode[0]==1: # self.ctrl.set_mode_pwm() # self.ctrl.set_pwm(self.current[0]) elif self.mode[0]==1: self.ctrl.set_mode_current() self.ctrl.set_current(self.current[0]) elif self.mode[0]==mec.CTRL_MODE_THETA: self.ctrl.set_mode_theta() self.ctrl.set_theta_deg(self.theta[0]) # elif self.mode[0]==mec.CTRL_MODE_TORQUE: print "kp: " + str(self.ctrl.param.pid_torque.k_p) self.ctrl.set_mode_torque() self.ctrl.set_torque(self.torque[0]) # # elif self.mode[0]==mec.CTRL_MODE_TORQUE_LC: # self.ctrl.set_mode_torque_lc() # self.ctrl.set_torque_lc(self.torque_lc[0]) else: self.ctrl.set_mode_off() if (self.save and not self.save_last): self.ctrl.write_config() self.save_last = self.save
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
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)
scope = m3t.M3Scope(xwidth=100, yrange=yrange)
try:
ts = time.time()
while True:
proxy.step()
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)
#print 'Time: ',60.0-(time.time()-ts),field,':',v
def step(self):
# print self.comps['act_ec']['comp'].status.timestamp
if self.do_scope and self.scope is None:
self.scope = m3t.M3Scope2(xwidth=100, yrange=None)
self.proxy.step()
self.cnt = self.cnt + 1
self.status_dict = self.proxy.get_status_dict()
if self.zero_joint_theta and not self.zero_joint_theta_last:
self.joint_theta -= self.act.get_joint_theta()
print 'New joint_theta zero', self.joint_theta
if self.zero_joint_torque and not self.zero_joint_torque_last:
self.joint_torque -= self.act.get_joint_torque()
print 'New joint_torque zero', self.joint_torque
#
# if self.zero_joint_torque_lc and not self.zero_joint_torque_lc_last:
# self.joint_torque_lc -= self.act.get_joint_torque_lc()
# print 'New joint_torque_lc zero',self.joint_torque_lc
#
# self.param_dict[self.comp_name]['calibration']['zero_motor_theta'] = self.motor_theta
# self.param_dict[self.comp_name]['calibration']['zero_joint_theta'] = self.joint_theta
# self.param_dict[self.comp_name]['calibration']['zero_joint_torque'] = self.joint_torque
# self.param_dict[self.comp_name]['calibration']['zero_joint_torque_lc'] = self.joint_torque_lc
#
# self.zero_joint_theta_last = self.zero_joint_theta
# self.zero_joint_torque_last = self.zero_joint_torque
# self.zero_joint_torque_lc_last = self.zero_joint_torque_lc
self.proxy.set_param_from_dict(self.param_dict)
if self.do_scope and self.scope is not None:
f1 = self.scope_keys[self.scope_field1[0]]
f2 = self.scope_keys[self.scope_field2[0]]
x1 = x2 = None
if f1 != 'None' and f1 != 'base':
x1 = m3t.get_msg_field_value(self.act.status, f1)
print f1, ':', x1
if f2 != 'None' and f2 != 'base':
x2 = m3t.get_msg_field_value(self.act.status, f2)
print f2, ':', x2
if x1 == None:
x1 = x2
if x2 == None:
x2 = x1
if x1 != None and x2 != None: #Handle only one value or two
self.scope.plot(x1, x2)
print '-----------------'
if self.mode[0] == 0: #Off
self.act.set_mode(mec.ACTUATOR_MODE_OFF)
elif self.mode[0] == 1: #Pwm
self.act.set_mode(mec.ACTUATOR_MODE_PWM)
self.act.set_pwm(self.pwm_desired[0])
elif self.mode[0] == 2: #Current
self.act.set_mode(mec.ACTUATOR_MODE_CURRENT)
self.act.set_i_desired(self.current_desired[0] * 1000.0)
else:
self.act.set_mode(mec.ACTUATOR_MODE_OFF)
if (self.save and not self.save_last):
self.act.write_config()
self.save_last = self.save
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()
