def calculate_gains(elementModel, saveDir):
global FMUMODEL
global inputGain
global displayGain
if not autoScale:
inputGain = inputGain
displayGain = displayGain
else:
rmoveSteps = int(rmoveTime/stepTime)
maxMove = 0
for frameNum in range(len(r_hist) - rmoveSteps):
currentMove = abs(r_hist[frameNum + rmoveSteps] - r_hist[frameNum])
maxMove = max([maxMove, currentMove])
displayGain = pixelDist/maxMove # maximum distance reference signal can move in rmoveTime [sec] is pixelDist
resultsFile = os.path.join(saveDir, elementModel.replace('.', '_') + '_step_results.txt')
fmuOpts = fmi.set_fmu_options(False, resultsFile, stepTime, solverName='CVode')
fmuInput = numpy.transpose(numpy.vstack(([0,ymoveTime], [1,1]))) # step input
(FMUMODEL, fmuResults) = fmi.simulate_fmu(FMUMODEL, fmuOpts, 0, ymoveTime, controlInput=(['u'], fmuInput))
ymoveDist = fmuResults['y'][-1] # distance moved by controlled element when step input is applied for ymoveTime
unitDist = pixelDist/displayGain
inputGain = unitDist/ymoveDist # controlled element can move unitDist [scaled pixels] in time ymoveTime [sec]
# Reset and reinitialize the controlled element FMU (otherwise the controlled element starts with a nonzero initial condition)
FMUMODEL.reset()
FMUMODEL.initialize()
def run_tracking_experiment(elementModel, modelicaFile):
global fmuOpts
global FPSCLOCK
global DISPLAYSURF
global JOYOBJECT
global FMUMODEL
global u_hist
global y_hist
# Calculate variables for preview circles
calculate_preview_parameters()
# Compile and initialize controlled element dynamic model
logFile = os.path.join(tempDir, elementModel.replace('.', '_') + '_log.txt')
resultsFile = os.path.join(tempDir, elementModel.replace('.', '_') + '_results.txt')
fmuOpts = fmi.set_fmu_options(False, resultsFile, stepTime, solverName='CVode')
fmuName = fmi.compile_fmu_openmodelica(elementModel, modelicaFile, saveDir=tempDir)
FMUMODEL = fmi.load_fmu_pyfmi(fmuName, logFile)
FMUMODEL.initialize()
# Calculate input and display gains
calculate_gains(elementModel=elementModel, saveDir=tempDir)
# Look for joystick input device
pygame.init()
JOYOBJECT = get_input_device()
# Allow user to initiate experiment
raw_input("Press 'Enter' to bring up the display, then press any key except 'q' to start the experiment.\n")
# Start up pygame window
FPSCLOCK = pygame.time.Clock()
DISPLAYSURF = pygame.display.set_mode((winWidth, winHeight))
pygame.display.set_caption('Python manual tracking (pymantra) task')
# Run each frame of tracking experiment
clear_tracking_display()
import time
a = time.time()
OLDSTDOUT = tools.disable_console_output()
for frame_current in frame_hist[0:-1]:
run_one_frame(frame_current)
tools.enable_console_output(OLDSTDOUT)
print time.time() - a
exit_tracking_program()
return (u_hist, y_hist)
def run_tracking_simulation(FMUMODEL=False, modifiedParams=False, inputData=False, plotResults=False, saveResults=False, runOnce=False):
if not FMUMODEL: # if FMU not provided, then compile FMU
fmuName = fmi.compile_fmu_openmodelica(taskModel, moFile, saveDir=os.environ['MANTRA_TEMP'], printVerbose=printVerbose)
FMUMODEL = fmi.load_fmu_pyfmi(fmuName, logFile, printVerbose=printVerbose)
if modifiedParams: # go through modifiedParams dict
for oneKey in modifiedParams.keys():
paramString = controllerName + '.' + oneKey
FMUMODEL.set(paramString, modifiedParams[oneKey])
if inputData:
(t_data, r_data, y_data, w_data, u_data) = inputData
controlVecs = numpy.transpose(numpy.vstack((t_data, r_data, y_data, w_data)))
controlInput = ([controllerName+'.r', controllerName+'.y', controllerName+'.w'], controlVecs)
else:
controlInput = False
# Simulate model
fmuOpts = fmi.set_fmu_options(True, resultFile, stepTime, solverName=odeSolver)
fmuResults = FMUMODEL.simulate(options=fmuOpts, start_time=0, final_time=finalTime)
FMUMODEL.reset()
# Extract data
t_sim = fmuResults['time']
r_sim = fmuResults[controllerName+'.r']
y_sim = fmuResults[controllerName+'.y']
w_sim = fmuResults[controllerName+'.w']
u_sim = fmuResults[controllerName+'.u']
# Save results
tools.write_data_csv(dirName=os.environ['MANTRA_TEMP'], fileName=taskName+'_sim.csv', dataCols=(t_sim, r_sim, y_sim, w_sim, u_sim))
if saveResults:
functionName = 'RunTrackingSimulation'
timeStamp = time.strftime("%Y.%m.%d-%H.%M.%S", time.localtime())
fileName = eval(saveFormat)
tools.write_data_csv(dirName=os.environ['MANTRA_DATA'], fileName=fileName+'.csv', dataCols=(t_sim, r_sim, y_sim, w_sim, u_sim))
if plotResults:
tools.plot_variable_trajectories(t_sim, ((r_sim, y_sim, 'Reference State', 'Measured State'), (w_sim, u_sim, 'Disturbance Input', 'Control Input')), "Simulated Tracking Task")
# Stop OMC Server if only running once
if runOnce:
fmi.stop_openmodelica_server()
def generate_fmu_signal(modelicaModel, modelicaFile, t_hist, paramDict, fmuMaxh, saveDir, odeSolver, printVerbose=False):
assert t_hist[0] == 0, "Time values must start at t=0."
fmuName = os.path.join(os.environ['MANTRA_TEMP'], modelicaModel.replace('.', '_')+'.fmu')
logFile = os.path.join(saveDir, modelicaModel.replace('.', '_') + '_log.txt')
resultFile = os.path.join(saveDir, modelicaModel.replace('.', '_') + '_results.txt')
if not os.path.isfile(fmuName):
fmuName = fmi.compile_fmu_openmodelica(modelicaModel, modelicaFile, saveDir, printVerbose=printVerbose)
FMUMODEL = fmi.load_fmu_pyfmi(fmuName, logFile, printVerbose=printVerbose)
# Overwrite default parameter values
for oneKey in paramDict.keys():
FMUMODEL.set(oneKey, paramDict[oneKey])
fmuOpts = fmi.set_fmu_options(False, resultFile, fmuMaxh, solverName=odeSolver)
FMUMODEL.initialize()
(FMUMODEL, fmuResults) = fmi.simulate_fmu(FMUMODEL, fmuOpts, 0, t_hist[-1], printVerbose=printVerbose)
t_sig = fmuResults['time']
y_sig = fmuResults['y']
signalOut = numpy.interp(t_hist, t_sig, y_sig)
return signalOut
