def process_diffeqn(item, work):
target = item[1]
logger.message("Processing differential equation for variable: " + target)
symbol = declare_symbol(target, work)
if work['docstack']:
symbol['docs'].extend(work['docstack'])
work['docstack'] = []
if target in work['roots']:
symbol['conflicts'] = symbol['conflicts'] + 1
else:
work['roots'].append(target)
if target not in work['diffs']:
work['diffs'].append(target)
i_expr, expr, depends = process_mathterm(item[2][1], work)
symbol['diffs'].append({'expr':expr, 'i_expr':i_expr, 'depends':depends, 'mathterm':item[2][1]})
symbol['depends'] |= depends
# process auxiliary terms, if any
auxterm = item[4]
offset = 1
auxes = []
while len(auxterm) >= offset + 3:
mass = auxterm[offset + 1]
if auxterm[offset] == '-':
mass = -1 * mass
auxes.append((mass, auxterm[offset + 2]))
offset = offset + 3
work['auxiliaries'][item[1]] = auxes
if offset > 1:
work['diagonal'] = False
def transform_reactions(work):
reacs = work['reactions']
if len(reacs) > 0:
logger.message('Transforming reactions to ODEs')
else:
return
chems = work['chemicals']
syms = work['symbols']
roots = work['roots']
# construct an ODE for each chemical
for chem in chems.keys():
sym = declare_symbol(chem, work)
if (chem in roots):
sym['conflicts'] = sym['conflicts'] + 1
else:
roots.append(chem)
work['diffs'].append(chem)
if chem not in work['auxiliaries']:
work['auxiliaries'][chem] = []
expr = ''
i_expr = ()
deps = set()
mathterm = ()
for reactlabel in reacs.keys():
reaction = reacs[reactlabel]
if chem in reaction['chems'].keys():
deps |= reaction['rate']['depends']
deps |= reaction['chems'][chem]['depends']
if len(expr) > 0: expr = expr + ' + '
if len(i_expr) > 0: i_expr = i_expr + (('literal', ' + '), )
expr = expr + ('(' + reaction['chems'][chem]['stoich'] + '*' +
reaction['rate']['expr'] + ')')
i_expr = i_expr + (('literal','('),) \
+ reaction['chems'][chem]['i_stoich'] \
+ (('literal','*'),) \
+ reaction['rate']['i_expr'] \
+ (('literal',')'),)
subterm = ('arithmetic', '*',
('mathterm', reaction['chems'][chem]['mathterm']),
('mathterm', reaction['rate']['mathterm']))
if mathterm:
mathterm = ('arithmetic', '+', ('mathterm', mathterm),
('mathterm', subterm))
else:
mathterm = subterm
sym['depends'] |= deps
sym['diffs'].append({
'depends': deps,
'expr': expr,
'i_expr': i_expr,
'mathterm': mathterm
})
def __init__(self):
lg.message(logging.INFO, 'Initialising ' + self.__class__.__name__)
self.object_types = {
0: 'shape',
1: 'joint',
2: 'graph',
3: 'camera',
4: 'dummy',
5: 'proximitysensor',
6: 'reserved1',
7: 'reserved2',
8: 'path_type',
9: 'visionsensor',
10: 'volume',
11: 'mill',
12: 'forcesensor',
13: 'light',
14: 'mirror'
}
self.api_server = {
'connectionAddress': '127.0.0.1',
'connectionPort': 19999,
'waitUntilConnected': True,
'doNotReconnectOnceDisconnected': True,
'timeOutInMs': 5000,
'commThreadCycleInMs': 5
}
self.client_id = -1
self.connected = self.connect_client()
self.object_tree = {}
def parse_file(filename):
try:
f = open(filename)
data = f.read()
f.close()
except IOError as e:
logger.error("I/O error({0}): {1}".format(e.errno, e.strerror))
return None
logger.message("Processing file: " + filename)
bcmd_yacc.currentFile = filename
errsBefore = len(bcmd_yacc.compilationInfo['errors'])
lp = bcmd_yacc.get_lexer_parser()
result = lp[1].parse(data, lexer=lp[0])
fileErrs = len(bcmd_yacc.compilationInfo['errors']) - errsBefore
bcmd_yacc.currentFile = None
if fileErrs == 1:
logger.error('Compilation failed with 1 syntax error')
elif fileErrs > 1:
logger.error('Compilation failed with %d syntax errors' % fileErrs)
return fileErrs, result
def parse_file ( filename ):
try:
f = open(filename)
data = f.read()
f.close()
except IOError as e:
logger.error("I/O error({0}): {1}".format(e.errno, e.strerror))
return None
logger.message("Processing file: " + filename)
bcmd_yacc.currentFile = filename
errsBefore = len(bcmd_yacc.compilationInfo['errors'])
lp = bcmd_yacc.get_lexer_parser()
result = lp[1].parse(data, lexer=lp[0])
fileErrs = len(bcmd_yacc.compilationInfo['errors']) - errsBefore
bcmd_yacc.currentFile = None
if fileErrs == 1:
logger.error('Compilation failed with 1 syntax error')
elif fileErrs > 1:
logger.error('Compilation failed with %d syntax errors' % fileErrs)
return fileErrs, result
def calibrate_all_motors(self):
try:
# Launch motor calibration
for positioner in self.positioners:
positioner.calibrate_motor(self.canUSB)
# Wait until all the positioners finished their calibration
finishedIDs = []
while len(finishedIDs) < len(self.positioners):
for positioner in self.positioners:
if positioner.ID in finishedIDs:
continue
status = positioner.get_status(self.canUSB)
if not (status & (canUSB._OPT.STREG.MOTOR_CALIBRATION)):
finishedIDs.append(positioner.ID)
# Check the calibration went well
for positioner in self.positioners:
result = positioner.get_motor_calibration_error(self.canUSB)
if abs(
result[0]
) > DEFINES.CALIBRATION_MOTOR_MAXIMAL_ERROR or abs(
result[1]) > DEFINES.CALIBRATION_MOTOR_MAXIMAL_ERROR:
raise errors.OutOfRangeError(
f'Motor calibration error of positioner {positioner.ID:04d} is too big'
) from None
except (errors.CANError, errors.PositionerError,
errors.OutOfRangeError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners motor calibration failed") from None
def init_positioners(self, genericPositioner=pos.Positioner()):
try:
response = self.canUSB.CAN_write(0, 'askID', [])
for i in range(0, len(response)):
self.positioners.append(copy.deepcopy(genericPositioner))
self.positioners[i].init(response[i],
self.canUSB,
waitInitComplete=False)
if len(self.positioners) < 1:
raise errors.PositionerError(
"No positioner in the bench") from None
for positioner in self.positioners:
#manually initialize each positioner's position
while not positioner.datum_initialized(self.canUSB):
time.sleep(0.005)
#Set the current position as the hardstop position
initial_position = {'Actual_alpha_pos': int(round(positioner.physics.incrementsPerRotation*positioner.model.offsetAlpha*180/np.pi/(DEFINES.DEGREES_PER_ROTATION),0)), \
'Actual_beta_pos': int(round(positioner.physics.incrementsPerRotation*positioner.model.offsetBeta*180/np.pi/(DEFINES.DEGREES_PER_ROTATION),0))}
self.canUSB.CAN_write(positioner.ID, 'set_actual_position',
initial_position)
positioner.initialized = True
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners initialization failed") from None
def load(self, fileName):
#Load all the data in the file, exculding the fileInfos
try:
with open(os.path.join(fileName), 'r') as inFile:
variablesToLoad = json.load(inFile)
for key in variablesToLoad.keys():
if key in type(self).__slots__:
setattr(self, key, variablesToLoad[key])
else:
log.message(
DEFINES.LOG_MESSAGE_PRIORITY_DEBUG_WARNING, 1,
f'Unexpected data was encountered during the loading of the testbench parameters. Faulty key: {key}'
)
if DEFINES.RAISE_ERROR_ON_UNEXPECTED_KEY:
raise errors.IOError(
'Unexpected data was encountered during the loading of the testbench parameters'
) from None
self.slotsCenters = np.array(self.slotsCenters)
self.nbSlots = len(self.slotsCenters[:, 0])
self.maxSlots = copy.deepcopy(self.nbSlots)
self.originalSlotsCenters = copy.deepcopy(self.slotsCenters)
self.clear_slots()
except OSError:
raise errors.IOError(
'The testbench parameters file could not be found') from None
def turn(self, step_status, world_props, args):
"""
Turn robot task - calculate new bearing relative to current orientation and turn cw or ccw at constant speed
"""
if step_status['complete'] is None:
if 'degrees' in args:
#
self.state['int']['compass'].set_to_bearing(args['degrees'])
lg.message(
logging.DEBUG, 'Turn bearing from {} to {}'.format(
self.state['int']['compass'].last_read,
self.state['int']['compass'].to_bearing))
# Turn cw or ccw at slow speed to prevent overshooting
if args['degrees'] > 0:
self.state['int']['motor_l_v'] = -0.1
self.state['int']['motor_r_v'] = 0.1
else:
self.state['int']['motor_l_v'] = 0.1
self.state['int']['motor_r_v'] = -0.1
self.set_motor_v()
step_status['complete'] = False
# Subtract max from min between last compass state and target bearing. Turn threshold is 0.5 degrees
radius_threshold = 0.5
diff = max(self.state['int']['compass'].last_read, self.state['int']['compass'].to_bearing) - \
min(self.state['int']['compass'].last_read, self.state['int']['compass'].to_bearing)
if diff < radius_threshold:
step_status['complete'] = True
lg.message(logging.INFO, 'Turn event complete')
def testAverage(self):
if self.started and self.value / self.propCom.nAvg < self.propCom.MIN_ADC_PERIOD and self.propCom.nAvg != 1:
# notify user of change
self.propCom.nAvg = int(self.value / self.propCom.MIN_ADC_PERIOD)
logger.message("Average filter is too high. \n Setting to " +
str(self.propCom.nAvg) + " sample average.")
self.propCom.send("avg", self.propCom.nAvg)
def __init__(self, world):
lg.message(logging.INFO, 'Initialising ' + self.__class__.__name__)
self.world_props = world['props']
self.world_events = world['events']
self.start_time = 0
self.scenario_mapping_enabled = False
self.step_mapping_enabled = False
# Instantiate helper and connect to VREP
self.h = Helper()
if not self.h.connected:
sys.exit()
# Get all objects from running VREP scene
self.full_scene_object_list, self.simple_object_list = self.h.get_objects()
# Instantiate robot
self.robot = PioneerP3dx(self.h, self.simple_object_list)
self.loop()
self.stats()
# Disconnect from VREP
self.h.disconnect_client()
def transform_reactions(work):
reacs = work['reactions']
if len(reacs) > 0:
logger.message('Transforming reactions to ODEs')
else:
return
chems = work['chemicals']
syms = work['symbols']
roots = work['roots']
# construct an ODE for each chemical
for chem in chems.keys():
sym = declare_symbol(chem, work)
if ( chem in roots ):
sym['conflicts'] = sym['conflicts'] + 1
else:
roots.append(chem)
work['diffs'].append(chem)
if chem not in work['auxiliaries']:
work['auxiliaries'][chem] = []
expr = ''
i_expr = ()
deps = set()
mathterm = ()
for reactlabel in reacs.keys():
reaction = reacs[reactlabel]
if chem in reaction['chems'].keys():
deps |= reaction['rate']['depends']
deps |= reaction['chems'][chem]['depends']
if len(expr) > 0: expr = expr + ' + '
if len(i_expr) > 0: i_expr = i_expr + (('literal',' + '),)
expr = expr + ( '(' + reaction['chems'][chem]['stoich']
+ '*' + reaction['rate']['expr'] + ')' )
i_expr = i_expr + (('literal','('),) \
+ reaction['chems'][chem]['i_stoich'] \
+ (('literal','*'),) \
+ reaction['rate']['i_expr'] \
+ (('literal',')'),)
subterm = ( 'arithmetic',
'*',
('mathterm', reaction['chems'][chem]['mathterm']),
('mathterm', reaction['rate']['mathterm']) )
if mathterm:
mathterm = ( 'arithmetic',
'+',
('mathterm', mathterm),
('mathterm', subterm) )
else:
mathterm = subterm
sym['depends'] |= deps
sym['diffs'].append({'depends':deps, 'expr':expr, 'i_expr':i_expr, 'mathterm':mathterm})
def set_speed_all_positioners(self, alphaSpeed, betaSpeed):
try:
for positioner in self.positioners:
positioner.set_speed(self.canUSB, alphaSpeed, betaSpeed)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners speed setting failed") from None
def set_current_all_positioners(self, alphaCurrent, betaCurrent):
try:
for positioner in self.positioners:
positioner.set_current(self.canUSB, alphaCurrent, betaCurrent)
except (errors.CANError, errors.PositionerError) as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners current setting failed") from None
def stop_all(self):
try:
self.testBench.stop_all_positioners()
except errors.Error as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
self.processManager.stop_livePlot_process()
self.processManager.stop_centroid_processes()
self.testBench.close_handles()
def disconnect_client(self):
"""
Disconnect this client from the V-REP server
"""
# Close the connection to VREP
vrep.simxStopSimulation(self.client_id, vrep.simx_opmode_blocking)
vrep.simxFinish(self.client_id)
lg.message(logging.INFO, 'Disconnected from V-REP')
def process_reaction(item, work):
logger.message("Processing reaction '" + item[2][1] + "' of type: " + item[1])
{
'influx' : process_flux,
'outflux' : process_flux,
'oneway' : process_oneway,
'twoway' : process_twoway
}.get(item[1], unknown_reaction)(item, work)
def set_state_pos_start(self):
"""
Set starting position as external, absolute position
"""
res, self.state['ext']['abs_pos_s'] = vrep.simxGetObjectPosition(
self.h.client_id, self.handle, -1, vrep.simx_opmode_buffer)
lg.message(logging.DEBUG,
'Start point set ' + str(self.state['ext']['abs_pos_s']))
def dbgHandler(propCom, v1=None, v2=None, v3=None, v4=None, v5=None):
strFmt = "Debug Message:\n"
strFmt += str(v1) + "\n"
strFmt += str(v2) + "\n"
strFmt += str(v3) + "\n"
strFmt += str(v4) + "\n"
strFmt += str(v5) + "\n"
logger.message(strFmt)
def stop_all_positioners(self):
try:
for positioner in self.positioners:
positioner.stop(self.canUSB)
except errors.CANError as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise errors.PositionerError(
"Positioners could not be stopped. A manual shutdown is recommended"
) from None
def init_canUSB(self):
if self.canUSBSerialNo is '':
return
try:
self.canUSB.init(self.canUSBSerialNo)
except errors.CANError as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise e from None
def process_reaction(item, work):
logger.message("Processing reaction '" + item[2][1] + "' of type: " +
item[1])
{
'influx': process_flux,
'outflux': process_flux,
'oneway': process_oneway,
'twoway': process_twoway
}.get(item[1], unknown_reaction)(item, work)
def request_invitation_token(self):
response = requests.post(f'{CPM_HUB_AUTH_URL}/otp/{self.name}')
if response.status_code == HTTPStatus.OK:
self.invitation_token = response.text
else:
logger.message(
f'BitDeveloperAgent({self.name}): FAIL request_invitation_token'
)
self.current_state = 'idle'
def get_object_handle_by_name(self, object_name):
"""
Return V-REP object handle for a given object name
"""
lg.message(logging.DEBUG, 'Getting V-REP object handle for ' + object_name)
error_code, handle = vrep.simxGetObjectHandle(self.client_id, object_name, vrep.simx_opmode_oneshot_wait)
if error_code == 0:
return handle
else:
return -1
def go_home_stats(self, ss, ta):
lg.message(logging.INFO, 'Completed path to home')
lg.message(logging.INFO, 'Stop task triggered')
self.robot.stop(ss, self.world_props, ta)
lg.message(logging.INFO, 'GH Distance travelled - {}m'.format(
self.robot.get_distance() - self.robot.state['int']['gh_status']['start_m']))
lg.message(logging.INFO, 'GH time taken - {}s'.format(
round(time.time() - self.robot.state['int']['gh_status']['start_t'], 2)))
lg.message(logging.INFO, 'GH number of route steps taken - {}'.format(
self.robot.state['int']['gh_status']['steps_count']))
def setExposure(self, exposure):
if self.connected:
try:
self.camHandle.ExposureTime = exposure
except genicam.GenericException:
self.connected = False
raise errors.CameraError("Camera communication failed")
else:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_DEBUG_WARNING, 0,
'Trying to set the exposure of an unconnected camera')
def within_dist(self, point_a, point_b, dist_threshold=0.08):
"""
Determine if point A within distance threshold of point B
"""
distance = self.get_euclidean_distance(point_a, point_b)
lg.message(logging.DEBUG, 'Distance between point A and B is ' + str(distance))
if distance <= dist_threshold:
return True
else:
return False
def run(self):
while True:
event, values = self.window.Read() #timeout=10)
if event is None:
break
elif event == 'testbenchChoice1':
self.window['testbenchText'].Update(value='Testbench 1',
background_color='blue',
text_color='white')
self.general.config.currentTestBenchFile = DEFINES.GUI_TB1_FILENAME
self.window['start'].Update(text='Start',
button_color=('white', 'blue'))
log.message(DEFINES.LOG_MESSAGE_PRIORITY_INFO, 0,
'Testbench 1 selected')
elif event == 'testbenchChoice2':
self.window['testbenchText'].Update(value='Testbench 2',
background_color='yellow',
text_color='black')
self.general.config.currentTestBenchFile = DEFINES.GUI_TB2_FILENAME
self.window['start'].Update(text='Start',
button_color=('black', 'yellow'))
log.message(DEFINES.LOG_MESSAGE_PRIORITY_INFO, 0,
'Testbench 2 selected')
elif event == 'start':
self.window['Log_testbench'].Update(value="")
self.window['testbenchChoice1'].Update(disabled=True)
self.window['testbenchChoice2'].Update(disabled=True)
self.window['start'].Update(disabled=True)
self.window.Refresh()
# try:
self.general.run_main()
# except KeyboardInterrupt:
# try:
# self.general.stop_all()
# except:
# pass
# log.message(DEFINES.LOG_MESSAGE_PRIORITY_CRITICAL,0,"Program manually interrupted")
# except Exception as e:
# # print(e)
# log.message(DEFINES.LOG_MESSAGE_PRIORITY_CRITICAL,0,"Program failed")
# except OSError:
# try:
# self.general.stop_all()
# except:
# pass
# log.message(DEFINES.LOG_MESSAGE_PRIORITY_CRITICAL,0,"Program failed")
self.window.Refresh()
self.window['testbenchChoice1'].Update(disabled=False)
self.window['testbenchChoice2'].Update(disabled=False)
self.window['start'].Update(disabled=False)
self.window.Refresh()
self.window.Close()
def get_components(self, tag, simple_object_list):
"""
Get component tree for vrep object
"""
lg.message(logging.DEBUG, 'Getting V-REP components for ' + tag)
components = {}
for comp in self.component_tree:
for child in self.component_tree[comp].children:
if tag in simple_object_list[child]:
components[child] = simple_object_list[child]
return components
def init_cameraTilt(self, pathToFile):
if self.TiltCameraID is None:
return
try:
self.cameraTilt = cam.Camera(DEFINES.PC_CAMERA_TYPE_TILT,
self.TiltCameraID)
self.cameraTilt.setDistortionCorrection(pathToFile)
except Exception as e:
log.message(DEFINES.LOG_MESSAGE_PRIORITY_ERROR, 0, str(e))
raise e from None
def write_tree(config, work):
if not config['treefile'] is None:
if config['treefile'] == '':
config['treefile'] = config['name'] + TREE_EXT
treePath = os.path.join(config['outdir'], config['treefile'])
logger.message("Attempting to write parse tree to " + treePath)
try:
treeStream = open(treePath, 'w')
pprint.pprint(work['merged'], stream=treeStream)
treeStream.close()
except IOError as e:
logger.error("Error writing file ({0}): {1}".format(e.errno, e.strerror))
def write_comp(config, processed):
if not config['compfile'] is None:
if config['compfile'] == '':
config['compfile'] = config['name'] + COMPILE_EXT
compPath = os.path.join(config['outdir'], config['compfile'])
logger.message("Attempting to write compilation structure to " + compPath)
try:
compStream = open(compPath, 'w')
pprint.pprint(processed, stream=compStream)
compStream.close()
except IOError as e:
logger.error("Error writing file ({0}): {1}".format(e.errno, e.strerror))
def write_graph(config, model):
if not config['graph'] is None:
if config['graph'] == '':
config['graph'] = config['name'] + GRAPHVIZ_EXT
graphPath = os.path.join(config['outdir'], config['graph'])
logger.message("Attempting to write dependency graph to " + graphPath)
try:
stream = open(graphPath, 'w')
print >> stream, info.generateGraphViz(model, config)
stream.close()
except IOError as e:
logger.error("Error writing file ({0}): {1}".format(e.errno, e.strerror))
def openFile(self):
"""opens this channels output file for writing"""
if self.filename is not None:
try:
self.outfile = open(self.filename, "a")
logger.log("file opened", self.filename, logger.INFO)
self.clearTime()
except ioerror:
logger.log("io error opening file", self.filename, logger.error)
logger.message("can't open file " + self.filename + "\n\nis the file already open?", logger.error)
else:
logger.log("no file selected. cant open", self.filename, logger.INFO)
def write_tree(config, work):
if not config['treefile'] is None:
if config['treefile'] == '':
config['treefile'] = config['name'] + TREE_EXT
treePath = os.path.join(config['outdir'], config['treefile'])
logger.message("Attempting to write parse tree to " + treePath)
try:
treeStream = open(treePath, 'w')
pprint.pprint(work['merged'], stream=treeStream)
treeStream.close()
except IOError as e:
logger.error("Error writing file ({0}): {1}".format(
e.errno, e.strerror))
def write_graph(config, model):
if not config['graph'] is None:
if config['graph'] == '':
config['graph'] = config['name'] + GRAPHVIZ_EXT
graphPath = os.path.join(config['outdir'], config['graph'])
logger.message("Attempting to write dependency graph to " + graphPath)
try:
stream = open(graphPath, 'w')
print >> stream, info.generateGraphViz(model, config)
stream.close()
except IOError as e:
logger.error("Error writing file ({0}): {1}".format(
e.errno, e.strerror))
def load_sources(config):
sources = config['sources']
srcIndex = 0
parsedSources = []
failedSources = []
merged = []
while srcIndex < len(sources):
logger.message("Searching for source file: " + sources[srcIndex])
src = search_file(sources[srcIndex], config['modelpath'])
if (src is None) and (not sources[srcIndex].endswith(MODELDEF_EXT)):
logger.message("Not found, trying with added extension: " +
sources[srcIndex] + MODELDEF_EXT)
src = search_file(sources[srcIndex] + MODELDEF_EXT,
config['modelpath'])
if src is None:
logger.warn("File not found: " + sources[srcIndex])
failedSources.append(sources[srcIndex])
else:
nErrs, ast = parse_file(src)
if nErrs > 0 or ast is None:
failedSources.append(src)
else:
ast = list(ast)
# add imports that are not already in the source list to it
for imp in list(
sum([x[1:] for x in ast if x[0] == 'import'], ())):
if imp not in sources and imp + MODELDEF_EXT not in sources:
sources.append(imp)
logger.detail(ast, prettify=True)
parsedSources.append((sources[srcIndex], src))
merged = merged + ast
srcIndex = srcIndex + 1
logger.message("Total number of attempted source files: %d" % srcIndex)
logger.message("%d parsed, %d failed" %
(len(parsedSources), len(failedSources)))
for failed in failedSources:
logger.message(" -> %s" % failed)
return {
'sources': sources,
'parsed': parsedSources,
'failed': failedSources,
'merged': merged
}
def setFile(self, fname): # opens a file used to write values into.
"""sets this channels output file and overwrites its contents"""
self.filename = fname
logger.log("New file for channel " + str(self.idx), self.filename, logger.INFO)
try:
self.writeHeader()
except IOError:
logger.log("IO Error opening file", self.filename, logger.ERROR)
logger.message("Can't open file " + self.filename + "\n\nis the file already open?", logger.ERROR)
self.filename = None
return False
else:
self.openFile() # open file for data.
return True
def stats(self):
"""
Show simulation statistics
"""
time_taken = round(time.time() - self.start_time, 2)
avg_joint_dist = 0
for m in self.robot.state['int']['motors']:
avg_joint_dist += self.robot.state['int']['jpos'][str(
self.robot.state['int']['motors'][m] + '_dist')]
avg_joint_dist = round(avg_joint_dist / 2, 2)
avg_speed_ms = round(avg_joint_dist / time_taken, 2)
lg.message(logging.INFO,
'Distance travelled - {}m'.format(avg_joint_dist))
lg.message(logging.INFO, 'Average speed - {}m/s'.format(avg_speed_ms))
lg.message(
logging.INFO, 'Nav dist diff error - {}cm'.format(
round(self.robot.state['int']['err_corr_count'], 2)))
lg.message(
logging.INFO, 'Controller loop complete - time taken - {}s'.format(
round(time.time() - self.start_time, 2)))
with open('output/abs_pos_all.csv', 'w', newline='') as out:
csv_output = csv.writer(out)
csv_output.writerows(self.robot.state['ext']['abs_pos_all'])
with open('output/motor_all_v', 'w', newline='') as out:
csv_output = csv.writer(out)
csv_output.writerows(self.robot.state['int']['motor_all_v'])
def sort_assignments(work):
independent = []
ind_expr = []
dependent_init = []
init_expr = []
dependent_step = []
step_expr = []
dependent_run = []
run_expr = []
symbols = work['symbols']
for name in work['assigned']:
init, run = choose_assignments(symbols[name])
if len(init['depends']) > 0:
dependent_init.append(name)
init_expr.append(init)
else:
independent.append(name)
ind_expr.append(init)
if run:
if name in work['intermeds']:
dependent_run.append(name)
run_expr.append(run)
else:
dependent_step.append(name)
step_expr.append(run)
else:
# intermeds is filled before we've determined
# whether there's any runtime assignment to do
# - now correct any earlier misapprehensions...
if name in work['intermeds']:
logger.message('reclassifying symbol %s as parameter' % name)
work['intermeds'].remove(name)
if name not in work['params']:
work['params'].append(name)
result = { 'independent': { 'names':independent, 'exprs':ind_expr } }
names, exprs = dependency_sort(dependent_init, init_expr)
result['dependent'] = { 'names': names, 'exprs': exprs }
names, exprs = dependency_sort(dependent_step, step_expr)
result['step'] = { 'names': names, 'exprs': exprs }
names, exprs = dependency_sort(dependent_run, run_expr)
result['runtime'] = { 'names':names, 'exprs': exprs }
return result
def OnSave2(self,event):
filetypes = "CSV files (*.csv)|*.csv|Text files (*.txt)|*.txt|All files|*"
dlg = wx.FileDialog(self, "Choose a file", style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, wildcard=filetypes)
outFile = None
if dlg.ShowModal()==wx.ID_OK:
try:
filename=dlg.GetFilename()
dirname=dlg.GetDirectory()
fullPath = os.path.join(dirname, filename)
date = '"' + time.asctime() + '"'
self.SetTitle( self.title + " - " + filename )
outFile = open(fullPath, "w")
# write header info
outFile.write( date )
for idx in self.data:
outFile.write("," + str(idx) )
outFile.write( "\n" )
# write data
nData = 0
for idx in self.data:
if len(self.data[idx]) > nData:
nData = len( self.data[idx] )
xList = []
x = self.initVal
for n in range( nData ):
xList.append((n, x))
x += self.inc
for n, x in sorted(xList):
outFile.write( str(x) + "," )
for idx in self.data:
try:
outFile.write( str(self.data[idx][n][1]) )
except IndexError:
pass
outFile.write( "," )
outFile.write( "\n")
outFile.close()
except IOError as e:
logger.log("Error opening file", e, logger.WARNING)
logger.message("Error opening file.", logger.ERROR)
except ValueError as e:
logger.log("Error writing file", e, logger.WARNING)
logger.message("Error writing file.", logger.ERROR)
dlg.Destroy()
def process_algeqn(item, work):
target = item[1]
logger.message("Processing algebraic relation for variable: " + target)
symbol = declare_symbol(target, work)
if work['docstack']:
symbol['docs'].extend(work['docstack'])
work['docstack'] = []
if target in work['roots']:
symbol['conflicts'] = symbol['conflicts'] + 1
else:
work['roots'].append(target)
if target not in work['algs']:
work['algs'].append(target)
i_expr, expr, depends = process_mathterm(item[2][1], work)
symbol['algs'].append({'expr':expr, 'i_expr':i_expr, 'depends':depends, 'mathterm':item[2][1]})
symbol['depends'] |= depends
def OnSave(self, event):
filetypes = "CSV files (*.csv)|*.csv|Text files (*.txt)|*.txt|All files|*"
dlg = wx.FileDialog(self, "Choose a file", style=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, wildcard=filetypes)
outFile = None
if dlg.ShowModal()==wx.ID_OK:
try:
filename=dlg.GetFilename()
dirname=dlg.GetDirectory()
fullPath = os.path.join(dirname, filename)
self.SetTitle( self.title + " - " + filename )
outFile = open(fullPath, "w")
# write header info
outFile.write( self.header )
outFile.write( "\n" )
for idx in self.data:
outFile.write(", X" + str(idx) )
outFile.write(", Y" + str(idx) )
if self.POINTDEBUG:
outFile.write(", DEBUG" + str(idx) )
outFile.write( "\n" )
# write data
nData = 0
for idx in self.data:
if len(self.data[idx]) > nData:
nData = len( self.data[idx] )
for n in range( nData ):
for idx in self.data:
try:
strfmt = ",{0:.5f},{1}".format(self.data[idx][n][0], self.data[idx][n][1])
if self.POINTDEBUG:
strfmt += "," + str(self.data[idx][n][2])
outFile.write(strfmt)
except IndexError:
pass
outFile.write( "\n")
outFile.close()
except IOError as e:
logger.log("Error opening file", e, logger.WARNING)
logger.message("Error opening file.", logger.ERROR)
except ValueError as e:
logger.log("Error writing file", e, logger.WARNING)
logger.message("Error writing file.", logger.ERROR)
dlg.Destroy()
def choose_assignments(symbol):
assigns = symbol['assigns']
if len(assigns) == 1:
if assigns[0]['init'] or len(assigns[0]['depends']) == 0:
return assigns[0], False
else:
return assigns[0], assigns[0]
if len(assigns) > 2:
logger.warn('Warning: too many assignments for symbol ' + symbol['id'] + ':')
logger.message(assigns, True)
lo = 1e6
hi = -1
init = False
noinit = False
# we use a rule of thumb that the expression with fewer dependencies is
# the initialisation; in the case of a tie, we take the later one as the init
# (which is why we have <= vs > in the if clauses below)
for ass in assigns:
if ass['init']:
init = ass
else:
noinit = ass
ndeps = len(ass['depends'])
if ndeps <= lo:
lo = ndeps
lo_expr = ass
if ndeps > hi:
hi = ndeps
hi_expr = ass
if init:
return init, noinit
if hi == lo:
logger.warn('Ambiguous dependencies in assignment for ' + symbol['id'])
return lo_expr, hi_expr
def load_sources(config):
sources = config['sources']
srcIndex = 0
parsedSources = []
failedSources = []
merged = []
while srcIndex < len(sources):
logger.message("Searching for source file: " + sources[srcIndex])
src = search_file(sources[srcIndex], config['modelpath'])
if ( src is None ) and ( not sources[srcIndex].endswith(MODELDEF_EXT) ):
logger.message("Not found, trying with added extension: " + sources[srcIndex] + MODELDEF_EXT)
src = search_file(sources[srcIndex] + MODELDEF_EXT, config['modelpath'])
if src is None:
logger.warn("File not found: " + sources[srcIndex])
failedSources.append(sources[srcIndex])
else:
nErrs, ast = parse_file(src)
if nErrs > 0 or ast is None:
failedSources.append(src)
else:
ast = list(ast)
# add imports that are not already in the source list to it
for imp in list(sum([x[1:] for x in ast if x[0]=='import'], ())):
if imp not in sources and imp + MODELDEF_EXT not in sources:
sources.append(imp)
logger.detail(ast, prettify=True)
parsedSources.append((sources[srcIndex],src))
merged = merged + ast
srcIndex = srcIndex + 1
logger.message("Total number of attempted source files: %d" % srcIndex)
logger.message("%d parsed, %d failed" % (len(parsedSources), len(failedSources)))
for failed in failedSources:
logger.message(" -> %s" % failed)
return {'sources':sources, 'parsed':parsedSources, 'failed':failedSources, 'merged':merged}
def onNAvg(self, event):
# ask user for starting X value
done = False
initVal = 0
while not done:
initVal = wx.GetTextFromUser(message="Enter a new sample size ", caption="Sample Size", default_value="1" )
if initVal == "":
initVal = 1
return
else:
try:
initVal = float(initVal)
done = True
if initVal < 1:
initVal = 1
except ValueError:
logger.message("Please enter a number", logger.WARNING)
self.nAvg = initVal
def merge_columns(full_table, merge_array):
#assuming that the merge function is "MEAN"
table = full_table['table']
col_ids = full_table['col_ids']
row_ids = full_table['row_ids']
cols = table[:, merge_array]
merged_col = cols.mean(axis=1)
#change the cols IDs
ids = [col_ids[i] for i in merge_array]
merged_id = "+".join(ids)
#todo now we are assuming that we merge the columns to the place of the first col
#remove the cols
for dc in sorted(merge_array, reverse=True):
table = del_col(table, dc)
col_ids.pop(dc)
#add the new col
table = add_col(table, merge_array[0], merged_col)
#update the IDs
col_ids.insert(merge_array[0], merged_id)
log.message("merge","column", merged_id ,merge_array)
print(merged_id, cols, merged_col, table)
return {"table": table, "col_ids": col_ids, "row_ids": row_ids}
def merge_rows(full_table, merge_array):
#assuming that the merge function is "MEAN"
table = full_table['table']
col_ids = full_table['col_ids']
row_ids = full_table['row_ids']
rows = table[merge_array, :]
merged_row = rows.mean(axis=0)
#change the rows IDs
ids = [row_ids[i] for i in merge_array]
merged_id = "+".join(ids)
#todo now we are assuming that we merge the rows to the place of the first row
#remove the rows
for dr in sorted(merge_array, reverse=True):
table = del_row(table, dr)
row_ids.pop(dr)
#add the new row
table = add_row(table, merge_array[0], merged_row)
#update the IDs
row_ids.insert(merge_array[0], merged_id)
log.message("merge","row", merged_id, merge_array)
print(merged_id, rows, merged_row, table)
return {"table": table, "col_ids": col_ids, "row_ids": row_ids}
def process_version(item, work):
if ( work['version'] ):
logger.warn('Ignoring additional @version directive: ' + item[1])
else:
logger.message('Model version is: ' + item[1])
work['version'] = item[1]
def unknown_reaction(item, work):
logger.message("Ignoring unknown reaction type: " + item[1])
def logModelInfo(model, config):
logger.detail('\n\n** work **\n', False)
logger.detail(model)
# log the main actual equation system
logger.message("\n\n** equations **")
for name in model['diffs']:
sym = model['symbols'][name]
if sym['conflicts'] > 0: tag = '[CONFLICT-?] '
else: tag = ''
lhs = name + "'"
for aux in model['auxiliaries'][name]:
mass = aux[0]
if mass < 0:
mass = mass * -1
op = ' - '
else:
op = ' + '
lhs = lhs + op + str(mass) + ' ' + aux[1] + "'"
for diff in sym['diffs']:
logger.message(tag + lhs + ' = ' + diff['expr'])
for name in model['algs']:
sym = model['symbols'][name]
if sym['conflicts'] > 0: tag = '[CONFLICT-?] '
else: tag = ''
for alg in sym['algs']:
logger.message(tag + 'f(' + name + ') : 0 = ' + alg['expr'])
# log dependency info
logger.message("\n\n** dependency analysis **")
logger.message("\nThe following solver variables are used in the model:")
for name in sorted(model['diffs'], key=str.lower): logger.message(name)
for name in sorted(model['algs'], key=str.lower): logger.message(name)
logger.detail("\nThe following intermediate variables or parameters are used by the model:", False)
for name in sorted(model['required'], key=str.lower):
if name in model['assigned']:
logger.detail(name + ' (assigned)')
else:
logger.detail(name + ' (unassigned)')
if model['inputs']:
logger.message('\nThe following symbols are declared as inputs:')
for name in sorted(model['inputs'], key=str.lower):
logger.message(name)
if model['params']:
logger.message('\nThe following symbols are parameters, independent of the solver variables:')
for name in sorted(model['params'], key=str.lower):
logger.message(name)
logger.message('\nThe following parameters have no dependencies at all:')
for name in sorted(model['params'], key=str.lower):
if len(model['symbols'][name]['depends']) == 0:
logger.message(name)
if model['intermeds']:
logger.message('\nThe following symbols are intermediates, with solver variable dependencies:')
for name in sorted(model['intermeds'], key=str.lower):
logger.message(name)
if model['unused']:
logger.message('\nThe following intermediate variables or parameters are declared but unused:')
for name in sorted(model['unused'], key=str.lower): logger.message(name)
if config['unused']:
logger.message('(NB: unused variables will still be calculated)')
else:
logger.message('(NB: unused variables will NOT be calculated)')
undoc = [ x for x in model['symbols'] if not [y for y in model['symbols'][x]['docs'] if not y.startswith('+')] ]
if undoc:
logger.message('\nThe following symbols are not documented:')
for name in sorted(undoc, key=str.lower): logger.message(name)
unassigned = set(model['symlist']) - model['assigned']
if unassigned:
logger.warn('\nThe following symbols are never explicitly assigned (will default to 0):')
for name in sorted(unassigned, key=str.lower):
logger.warn(name)
if model['extern']:
logger.warn('\nThe following external dependencies are declared but unsatisfied:\n')
for name in sorted(model['extern'], key=str.lower): logger.warn(name)
if model['unknown']:
logger.warn("\nThe model makes use of the following non-standard functions:")
for name in model['unknown']: logger.warn(name)
# examine circular dependencies
logger.detail("\nCircular dependencies:")
for name in model['symbols'].keys():
if model['symbols'][name]['circular']:
if name in model['roots']:
logger.detail(name + " (is a solver var)")
elif name in model['unused']:
logger.detail(name + ' (unused)')
else:
LHS = model['symbols'][name]['depends'] & set(model['roots'])
if len(LHS) == 0:
logger.detail(name + " (no LHS dependencies)")
else:
logger.detail(name + ' ' + str(LHS))
logger.message('')
logger.message('** summary for model %s **' % config['name'])
logger.message('%d model variables (%d differential, %d algebraic)' % (len(model['roots']), len(model['diffs']), len(model['algs'])))
logger.message('%d intermediate variables (%d unused)' % (len(model['intermeds']), len([x for x in model['intermeds'] if x in model['unused']])))
logger.message('%d parameters (%d unused)' % (len(model['params']), len([x for x in model['params'] if x in model['unused']])))
logger.message('%d unsatisfied external dependencies\n' % len(model['extern']))
logger.message('')
def finalise_externs(work):
logger.message(work['extern'], True)
work['extern'] = [ x for x in work['extern'] if x in work['symbols'] and x not in work['assigned'] ]
def run_tool(window_parent, device):
global graph_windows
# ask user for input selection
i = 0
choices = []
#for c in device.analogIn:
for key, c in device.analogIn.iteritems():
choices.append( "Analog input " + str(c.idx) )
dlg = wx.MultiChoiceDialog(window_parent, message="Select which inputs to sample", caption="Input Selection", choices=choices)
dlg.SetSelections([0])
started_channels = []
if dlg.ShowModal()== wx.ID_OK:
selection = dlg.GetSelections()
for idx in selection:
started_channels.append(device.channels[idx])
if len(selection) <= 0:
logger.log("No inputs selected", "nothing to average", logger.INFO)
else:
return
dlg.Destroy()
# ask user for input selection for trigger
i = 0
choices = []
for n in range(4):
choices.append( "Digital input " + str(n) )
dlg = wx.SingleChoiceDialog(window_parent, message="Select input trigger", caption="Trigger Selection", choices=choices)
dlg.SetSelection(0)
triggerPin = 0
if dlg.ShowModal()== wx.ID_OK:
triggerPin = dlg.GetSelection() + 4
else:
return
dlg.Destroy()
# ask user for starting X value
done = False
initVal = 0
while not done:
initVal = wx.GetTextFromUser(message="Enter the initial X axis value ", caption="Initial Value", default_value="0" )
if initVal == "":
initVal = 0
return
else:
try:
initVal = float(initVal)
done = True
except ValueError:
logger.message("Please enter a number", logger.WARNING)
# ask user for increment size
done = False
inc = 0
while not done:
inc = wx.GetTextFromUser(message="Enter a number for the sample step", caption="Sample Step", default_value="10" )
if inc == "":
inc = 0
return
else:
try:
inc = float(inc)
done = True
except ValueError:
logger.message("Please enter a number", logger.WARNING)
done = False
# ask user for sample size
done = False
sample_size = 0
while not done:
sample_size = wx.GetTextFromUser(message="Enter the number of samples to be averaged", caption="Sample Size", default_value="100" )
if sample_size == "":
return
else:
try:
sample_size = float(sample_size)
done = True
except ValueError:
logger.message("Please enter a number", logger.WARNING)
# ask user for increment size
#inc = wx.GetTextFromUser(message="Enter an integer for the sample step (Integers Only)", caption="Sampling Step", value=10 )
#if inc == "":
# inc = 1
# return
# ask user for sample time
#sample_size = wx.GetTextFromUser(message="Enter the number of samples to be averaged", caption="Sample Size", value=100 )
#if sample_size == "":
# sample_size = 1
# return
# disable any active windows
for w in graph_windows:
if w.recording:
w.stop()
window = NewGraphFrame(window_parent, device, sample_size, initVal, inc, started_channels, triggerPin)
window.Show()
graph_windows.append(window)
def randomly_change_table(full_table, min_data, max_data, change_type, new_id = None):
table = full_table['table']
row_ids = full_table['row_ids']
col_ids =full_table['col_ids']
#just in case of an empty table
largest_row = 10
largest_col = 3
#shape of the table without ids
table_height = table.shape[0]
table_width = table.shape[1]
if change_type == ADD_ROW:
index = random.randint(0, table_height)
if table_height > 0:
if table_width > 0:
new_row = gen.random_floats_array(min_data, max_data, table_width)
else:
#i don't know if this is possible in anyway!!
new_row = gen.random_floats_array(min_data, max_data, 1)
else:
# table is empty
# todo recheck
new_id = 1 #or?
new_row = gen.random_floats_array(min_data, max_data, random.randint(1, largest_row))
log.message("add", "row", "row"+str(new_id), index, new_row)
row_ids.insert(index, "row"+str(new_id))
new_id += 1
table = add_row(table, index, new_row)
elif change_type == ADD_COL:
if table_height > 0:
index = random.randint(0, table_width)
new_col = gen.random_floats_array(min_data, max_data, table_height)
else:
#this is the first column or what?
index = 0
new_id = 1 #?
new_col = gen.random_floats_array(min_data, max_data, random.randint(1, largest_col))
log.message("add", "column", "col"+str(new_id), index, new_col)
col_ids.insert(index, "col"+str(new_id))
table = add_col(table, index, new_col)
elif change_type == CH_CELL:
if table_height > 0:
i = random.randint(0, table_height - 1)
j = random.randint(0, table_width - 1)
new_value = random.uniform(min_data, max_data)
log.message("change", "cell", row_ids[i]+','+col_ids[j], str(i)+','+str(j), new_value)
table = change_cell(table, i, j, new_value)
else:
print("log: there's nothing to change")
elif change_type == DEL_ROW:
index = random.randint(0, table_height - 1)
log.message("delete","row", row_ids[index] ,index, table[index])
row_ids.pop(index)
table = del_row(table, index)
elif change_type == DEL_COL:
if table_width > 0:
index = random.randint(0, table_width - 1)
log.message("delete","column", col_ids[index] ,index, table[:,index])
col_ids.pop(index)
table = del_col(table, index)
else:
print("Error: no columns to delete")
return {'table': np.array(table), 'col_ids': col_ids, 'row_ids': row_ids}
# main entry point of this compiler script
if __name__ == '__main__':
config = process_args()
if not config: sys.exit(2)
work = load_sources(config)
if len(work['failed']) > 0:
print_errors()
sys.exit(1)
write_tree(config, work)
processed = ast.process(work['merged'], work['parsed'], config['independent'])
info.logModelInfo(processed, config)
write_comp(config, processed)
write_graph(config, processed)
source = codegen.generateSource(processed, config)
codepath = os.path.join(config['outdir'], config['name'] + CODE_EXT)
logger.message("Attempting to write C code to " + codepath)
try:
cfile = open(codepath, 'w')
cfile.write(source)
cfile.close()
except IOError as e:
logger.error("Error writing file ({0}): {1}".format(e.errno, e.strerror))
sys.exit(1)