Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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()
Ejemplo n.º 3
0
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 
Ejemplo n.º 4
0
def tune_manual_controller(taskInfo, controllerInfo, saveDir, optMethod='Nelder-Mead', inputData=False, printVerbose=False):   
    (controllerName, taskModel, taskFile) = taskInfo
    (controllerParams, params2tune) = controllerInfo

    # Make deep copy of initial controller parameter dictionary
    import copy
    controllerInit = copy.deepcopy(controllerParams)

    # Make FMUMODEL once, instead of letting run_tracking_simulation() do it every time
    logFile = os.path.join(saveDir, taskModel.replace('.','_') + '_log.txt')
    fmuName = fmi.compile_fmu_openmodelica(taskModel, taskFile, saveDir=saveDir, printVerbose=printVerbose)
    FMUMODEL = fmi.load_fmu_pyfmi(fmuName, logFile, printVerbose=printVerbose)

    # Note that variables in body of tune_manual_controller() below can be accessed in compute_cost_value()
    if inputData: 
        (t_exp, r_exp, y_exp, w_exp, u_exp) = inputData
        matchOutput = True
    else: 
        inputData = False
        matchOutput = False

    # Define initial values for tuned parameters
    x0 = []
    for oneKey in params2tune:
        x0.append(controllerInit[oneKey])
    x0 = numpy.array(x0) # convert to numpy array format for jmodelica

    # Calculate objective function value for current parameter values
    def compute_cost_value(x):
        modifiedParams = {}
        for paramNum in range(len(params2tune)):
            modifiedParams[params2tune[paramNum]] = x[paramNum]

        if printVerbose:
            print "Modified parameters:"
            print modifiedParams
            print ""

        # Run tracking simulation
        mantra.run_tracking_simulation(FMUMODEL=FMUMODEL, modifiedParams=modifiedParams, inputData=inputData, plotResults=False, saveResults=False, runOnce=False)
        (t_sim, r_sim, y_sim, w_sim, u_sim) = read_data_csv(dirName=saveDir, fileName=taskModel.split('.')[-1]+'_sim.csv')      

        # Compute cost value
        if matchOutput: # based on difference between experimental and simulated control signal u(t), given inputs to controller r(t), y(t), and w(t)
            t_intervals = [t_exp[k+1]-t_exp[k] for k in range(len(t_exp)-1)]
            u_simulated = numpy.interp(t_exp, t_sim, u_sim) # interpolate recorded command u at simulated time points
            costVal = numpy.sqrt(numpy.mean(t_intervals*(u_simulated[0:-1] - u_exp[0:-1])**2)) # compare recorded command to simulated command
        else: # based on tracking error
            t_intervals = [t_sim[k+1]-t_sim[k] for k in range(len(t_sim)-1)]
            costVal = numpy.sqrt(numpy.mean(t_intervals*(numpy.array(r_sim[0:-1]) - numpy.array(y_sim[0:-1]))**2)) # compare reference signal to controlled element state

        if printVerbose:
            print '\nCurrent x: '
            print x
            print 'Cost value:'
            print costVal
            print ""

        return costVal

    if not printVerbose: OLDSTDOUT = disable_console_output()
    optParams = fmi.minimize_cost_scipy(compute_cost_value, x0, optMethod=optMethod, printVerbose=printVerbose)
    if not printVerbose: enable_console_output(OLDSTDOUT) 
    
    return optParams