def simulate(self): ''' This method is used to solve the resulting initial value problem after the computation of a solution for the input trajectories. ''' logging.debug("Solving Initial Value Problem") # calulate simulation time T = self.dyn_sys.b - self.dyn_sys.a # get list of start values start = [] if self.constraints is not None: sys = self._dyn_sys_orig else: sys = self.dyn_sys x_vars = sys.states start_dict = dict([(k, v[0]) for k, v in sys.boundary_values.items() if k in x_vars]) ff = sys.f_num for x in x_vars: start.append(start_dict[x]) # create simulation object S = Simulator(ff, T, start, self.eqs.trajectories.u) logging.debug("start: %s" % str(start)) # start forward simulation self.sim_data = S.simulate()
def simulate(self): ''' This method is used to solve the resulting initial value problem after the computation of a solution for the input trajectories. ''' logging.debug("Solving Initial Value Problem") # calulate simulation time T = self.dyn_sys.b - self.dyn_sys.a # get list of start values start = [] if self.constraints is not None: sys = self._dyn_sys_orig else: sys = self.dyn_sys x_vars = sys.states start_dict = dict([(k, v[0]) for k, v in sys.boundary_values.items() if k in x_vars]) ff = sys.f_num for x in x_vars: start.append(start_dict[x]) # create simulation object S = Simulator(ff, T, start, self.eqs.trajectories.u) logging.debug("start: %s"%str(start)) # start forward simulation self.sim_data = S.simulate()
def siumlate_with_input(tp, inputseq, n_parts ): """ :param tp: TransitionProblem :param inputseq: Sequence of input values (will be spline-interpolated) :param n_parts: number of spline parts for the input :return: """ tt = np.linspace(tp.a, tp.b, len(inputseq)) # currently only for single input systems su1 = new_spline(tp.b, n_parts, (tt, inputseq), 'u1') sim = Simulator(tp.dyn_sys.f_num_simulation, tp.b, tp.dyn_sys.xa, x_col_fnc=None, u_col_fnc=su1.f) tt, xx, uu = sim.simulate() return tt, xx, uu
def make_refsol_by_simulation(tp, u_values, plot_u=False, plot_x_idx=0): """ Create a "reference solution" by Simulating the system with a given input signal :param tp: TransitionProblem object (contains system dynamics and boundary values) :param u_values: Sequence of values for the input, will be interpolated by a spline :param plot_u: Flag whether or not plot the input :param plot_x_idx: Index up to which the state should be plotted (default = 0 -> don't plot x) :return: Container with tt, xx, uu and raise_spline_parts """ Ta, Tb = tp.a, tp.b tt1 = np.linspace(Ta, Tb, len(u_values)) uspline = new_spline(Tb, n_parts=10, targetvalues=(tt1, u_values), tag='u1') x_start = tp.dyn_sys.xa ff = tp.eqs.sys.f_num_simulation def ufunc(t): """adapt siganture for broadcasting wrapper """ return uspline.f(t) nu = len(np.atleast_1d(ufunc(Ta))) uref_func = broadcasting_wrapper(ufunc, original_shape=(nu, )) sim = Simulator(ff, Tb, x_start, x_col_fnc=None, u_col_fnc=uref_func) tt, xx, uu = sim.simulate() uu = np.atleast_2d(uu) refsol = Container(tt=tt, xx=xx, uu=uu, n_raise_spline_parts=0, uref_func=uref_func) plot_flag = False if plot_u: # this serves for designing the input signal tt2 = np.linspace(Ta, Tb, 1000) uu = vector_eval(uspline.f, tt2) plt.plot(tt2, uu) plt.title("u") plot_flag = True assert isinstance(plot_x_idx, (int, str)) if plot_x_idx is "all" or plot_x_idx is True: # all is the only allowed string value # type bool is derived from int but we want True to behave like "all" not like 1 plot_x_idx = xx.shape[1] assert isinstance(plot_x_idx, int) if plot_x_idx > 0: assert plot_x_idx <= xx.shape[1] n = plot_x_idx plt.figure() plt.plot(tt, xx[:, :n]) plt.grid(1) plt.legend([str(i+1) for i in xrange(n)]) plot_flag = True if plot_flag: plt.show() raise SystemExit return refsol
class TransitionProblem(Logger): """ Base class of the PyTrajectory project containing all information to model a transition problem of a dynamical system. Parameters ---------- ff : callable Vector field (rhs) of the control system. a : float Left border of the considered time interval. b : float Right border of the considered time interval. xa : list Boundary values at the left border. xb : list Boundary values at the right border. ua : list Boundary values of the input variables at left border. ub : list Boundary values of the input variables at right border. uref : None or callable Vectorized function of reference input, i.e. uref(t). The complete input signal is then uref(t) + ua(t), where ua is the (vector-) spline that is searched. constraints : dict Box-constraints of the state variables. kwargs ============= ============= ============================================================ key default value meaning ============= ============= ============================================================ sx 10 Initial number of spline parts for the system variables su 10 Initial number of spline parts for the input variables kx 2 Factor for raising the number of spline parts maxIt 10 Maximum number of iteration steps (how often raising spline-parts) eps 1e-2 Tolerance for the solution of the initial value problem ierr 1e-1 Tolerance for the error on the whole interval tol 1e-5 Tolerance for the solver of the equation system dt_sim 1e-2 Sample time for integration (initial value problem) reltol 2e-5 Rel. tolerance (for LM-Algorithm to be confident with local minimum) localEsc 0 How often try to escape local minimum without increasing number of spline parts use_chains True Whether or not to use integrator chains sol_steps 50 Maximum number of iteration steps for the eqs solver accIt 5 How often resume the iteration after sol_steps limit (just have a look, in case the ivp is already satisfied) show_ir False Show intermediate result. Plot splines and simulation result after each IVP-solution (usefull for development) first_guess None to initiate free parameters (might be useful: {'seed': value}) refsol Container optional data (C.tt, C.xx, C.uu) for the reference trajectory progress_info (1, 1) 2-tuple which indicates the actual run w.r.t multiprocessing mpc_th np.inf Threshold in which iteration switch on mpc for simulation (default: +inf) ============= ============= ============================================================ """ def __init__(self, ff, a=0., b=1., xa=None, xb=None, ua=None, ub=None, uref=None, constraints=None, **kwargs): self.progress_info = kwargs.get("progress_info", (1, 1)) self.init_logger(self) self.initial_kwargs = kwargs # save all arguments for possible later reference self.all_args = dict(ff=ff, a=a, b=b, xa=xa, xb=xb, ua=ua, ub=ub, uref=uref, constraints=constraints) self.all_args.update(kwargs) if xa is None: xa = [] if xb is None: xb = [] # convenience for single input case: if np.isscalar(ua): ua = [ua] if np.isscalar(ub): ub = [ub] # set method parameters self._parameters = dict() self._parameters['maxIt'] = kwargs.get('maxIt', 10) self._parameters['eps'] = kwargs.get('eps', 1e-2) self._parameters['ierr'] = kwargs.get('ierr', 1e-1) self._parameters['dt_sim'] = kwargs.get('dt_sim', 0.01) self._parameters['accIt'] = kwargs.get('accIt', 5) self._parameters['localEsc'] = kwargs.get('localEsc', 0) self._parameters['reltol'] = kwargs.get('reltol', 2e-5) self._parameters['show_ir'] = kwargs.get('show_ir', False) self._parameters['show_refsol'] = kwargs.get('show_refsol', False) # this serves to reproduce a given trajectory self.refsol = kwargs.get('refsol', None) self.mpc_sim_threshold = kwargs.get( "mpc_th", np.inf) # mpc turned off by default self.tmp_sol = None # place to store the result of the server # if necessary change kwargs such that the seed value is in `first_guess` # (needed before the creation of DynamicalSystem) self._process_seed(kwargs) # create an object for the dynamical system self.dyn_sys = DynamicalSystem(f_sym=ff, masterobject=self, a=a, b=b, xa=xa, xb=xb, ua=ua, ub=ub, uref=uref, **kwargs) # TODO: change default behavior to False (including examples) self.use_chains = kwargs.get('use_chains', True) # 2017-05-09 14:41:14 # Note: there are two kinds of constraints handling: # (1) variable transformation (old, tested, also used by Graichen et al.) # (2) penalty term (new, currently under development)-> seems to work not so good self._preprocess_constraints( constraints) # (constr.-type: "variable transformation") # create an object for the collocation equation system self.eqs = CollocationSystem(masterobject=self, dynsys=self.dyn_sys, **kwargs) # We didn't really do anything yet, so this should be false self.reached_accuracy = False self.nIt = None self.T_sol = None self.tmp_sol_list = None # empty objects to store the simulation results later self.sim_data = None # all results # convenience: self.sim_data_xx = None self.sim_data_uu = None self.sim_data_tt = None self.simulator = None # storage for the error w.r.t desired state self.sim_err = None def set_param(self, param='', value=None): """ Alters the value of the method parameters. Parameters ---------- param : str The method parameter value The new value """ if param in {'maxIt', 'eps', 'ierr', 'dt_sim'}: self._parameters[param] = value elif param in { 'n_parts_x', 'sx', 'n_parts_u', 'su', 'kx', 'use_chains', 'nodes_type', 'use_std_approach' }: if param == 'nodes_type' and value != 'equidistant': raise NotImplementedError() if param == 'sx': param = 'n_parts_x' if param == 'su': param = 'n_parts_u' self.eqs.trajectories._parameters[param] = value elif param in {'tol', 'method', 'coll_type', 'sol_steps', 'k'}: # TODO: unify interface for additional free parameter if param == 'k': param = 'z_par' self.eqs._parameters[param] = value else: raise AttributeError("Invalid method parameter ({})".format(param)) # TODO: get rid of this method, because it is now implemented in ConstraintHandler def _preprocess_constraints(self, constraints=None): """ Preprocessing of projective constraint-data provided by the user. Ensure types and ordering :return: None """ if constraints is None: constraints = dict() con_x = OrderedDict() con_u = OrderedDict() for k, v in constraints.iteritems(): assert isinstance(k, str) if k.startswith('x'): con_x[k] = v elif k.startswith('u'): con_u[k] = v else: msg = "Unexpected key for constraint: %s: %s" % (k, v) raise ValueError(msg) self.constraints = OrderedDict() self.constraints.update(sorted(con_x.iteritems())) self.constraints.update(sorted(con_u.iteritems())) if self.use_chains: msg = "Currently not possible to make use of integrator chains together with " \ "projective constraints." self.log_warn(msg) self.use_chains = False # Note: it should be possible that just those chains are not used # which actually contain a constrained variable self.constraint_handler = ConstraintHandler(self, self.dyn_sys, self.constraints) self.dyn_sys.constraint_handler = self.constraint_handler def get_constrained_spline_fncs(self): """ Map the unconstrained coordinates (y, v) to the original constrained coordinats (x, u). (Use identity map if no constrained was specified for a component) :return: x_fnc, dx_fnc, u_fnc """ # TODO: the attribute names of the splines have to be adjusted y_fncs = self.eqs.trajectories.x_fnc.values() ydot_fncs = self.eqs.trajectories.dx_fnc.values() # sequence of funcs vi(.) v_fncs = self.eqs.trajectories.u_fnc.values() return self.dyn_sys.constraint_handler.get_constrained_spline_fncs( y_fncs, ydot_fncs, v_fncs) def check_refsol_consistency(self): """" Check if the reference solution provided by the user is consistent with boundary conditions """ assert isinstance(self.refsol, auxiliary.Container) tt, xx, uu = self.refsol.tt, self.refsol.xx, self.refsol.uu assert tt[0] == self.a assert tt[-1] == self.b msg = "refsol has the wrong number of states" assert xx.shape[1] == self.dyn_sys.n_states, msg if not np.allclose(xx[0, :], self.dyn_sys.xa): self.log_warn( "boundary values and reference solution not consistent at Ta") if not np.allclose(xx[-1, :], self.dyn_sys.xb): self.log_warn( "boundary values and reference solution not consistent at Tb") def solve(self, tcpport=None, return_format="xup-tuple"): """ This is the main loop. While the desired accuracy has not been reached, the collocation system will be set up and solved with a iteratively raised number of spline parts. Parameters ---------- tcpport: port for interaction with the solution process default: None (no interaction) return_format: specifies the format of the return value (either tuple or container) admitted values: "xup-tuple" (default) or "info_container" Returns ------- callable Callable function for the system state. callable Callable function for the input variables. """ T_start = time.time() if tcpport is not None: assert isinstance(tcpport, int) interfaceserver.listen_for_connections(tcpport) self._process_refsol() self._process_first_guess() self.nIt = 0 self.tmp_sol_list = [] # list to save the "intermediate local optima" def q_finish_loop(): res = self.reached_accuracy or self.nIt >= self._parameters['maxIt'] return res while not q_finish_loop(): if not self.nIt == 0: # raise the number of spline parts (not in the first step) self.eqs.trajectories.raise_spline_parts() msg = "Iteration #{}; spline parts_ {}".format( self.nIt + 1, self.eqs.trajectories.n_parts_x) self.log_info(msg) # start next iteration step try: self._iterate() except auxiliary.NanError: self.log_warn("NanError") return None, None self.log_info('par = {}'.format(self.get_par_values())) # increment iteration number self.nIt += 1 self.tmp_sol_list.append(self.eqs.sol) self.T_sol = time.time() - T_start # return the found solution functions if interfaceserver.running: interfaceserver.stop_listening() return self.return_solution(return_format=return_format) def return_sol_info_container(self): """ Create a data structure which contains all necessary information of the solved TransitionProblem, while consuming only few memory. :return: Conainer """ import pytrajectory # this import is not placed at the to to avoid circular imports msg = "See system.return_sol_info_container for information about the attributes." sol_info = auxiliary.ResultContainer(aaa_info=msg) # The actual solution of optimization sol_info.opt_sol = self.eqs.sol # variables to which the solution belongs sol_info.indep_vars = self.eqs.trajectories.indep_vars """ Note that the curves can be reproduced by creating splines.Spline(...) objects and setting the free coeffs """ # intermediate local optima sol_info.intermediate_solutions = self.tmp_sol_list sol_info.solver_res = self.eqs.solver.res # error wrt. desired final state sol_info.final_state_err = self.sim_err # some meta data sol_info.pytrajectory_version = pytrajectory.__version__ sol_info.pytrajectory_commit_date = pytrajectory.__date__ sol_info.reached_accuracy = self.reached_accuracy sol_info.all_args = self.all_args sol_info.n_parts_x = self.eqs.trajectories.n_parts_x sol_info.n_parts_u = self.eqs.trajectories.n_parts_u sol_info.nIt = self.nIt sol_info.T_sol = self.T_sol # this should be evaluated return sol_info def return_solution(self, return_format="xup-tuple"): """ if return_format == "xup-tuple" (classic behavior) return tuple of callables (xfnc, ufnc) or (xfnc, ufnc, par_values) (depending on the presence of additional free parameters) if return_format == "info_container" return a Container which contains the essential information of the solution consuming few memory. This is usefull for parallelized runs :return: 2-tuple, 3-tuple or Container """ if return_format == "info_container": return self.return_sol_info_container() elif not return_format == "xup-tuple": raise ValueError("Unkown return format: {}".format(return_format)) if self.dyn_sys.n_par == 0: return self.eqs.trajectories.x, self.eqs.trajectories.u else: return self.eqs.trajectories.x, self.eqs.trajectories.u, self.get_par_values( ) ##:: self.eqs.trajectories.x, self.eqs.trajectories.u are functions, ##:: variable is t. x(t), u(t) (value of x and u at t moment, # not all the values (not a list with values for all the time)) def get_spline_values(self, sol, plot=False): """ This function serves for debugging and algorithm investigation. It is supposed to be called from within the solver. It calculates the corresponding curves of x and u w.r.t. the actually best solution (parameter vector) :return: tuple of arrays (t, x(t), u(t)) or None (if plot == True) """ # TODO: add support for additional free parameter self.eqs.trajectories.set_coeffs(sol) # does not work (does not matter, only convenience) # xf = np.vectorize(self.eqs.trajectories.x) # uf = np.vectorize(self.eqs.trajectories.u) dt = 0.01 tt = np.arange(self.a, self.b + dt, dt) xx = np.zeros((len(tt), self.dyn_sys.n_states)) uu = np.zeros((len(tt), self.dyn_sys.n_inputs)) for i, t in enumerate(tt): xx[i, :] = self.eqs.trajectories.x(t) uu[i, :] = self.eqs.trajectories.u(t) return tt, xx, uu def _iterate(self): """ This method is used to run one iteration step. First, new splines are initialised. Then, a start value for the solver is determined and the equation system is set up. Next, the equation system is solved and the resulting numerical values for the free parameters are applied to the corresponding splines. As a last, the resulting initial value problem is simulated. """ # Note: in pytrajectory there are Three main levels of 'iteration' # Level 3: perform one LM-Step (i.e. calculate a new set of parameters) # This is implemented in solver.py. Ends when tolerances are met or # the maximum number of steps is reached # Level 2: restarts the LM-Algorithm with the last values # and stops if the desired accuracy for the initial value problem # is met or if the maximum number of steps solution attempts is reached # Level 1: increasing the spline number. # In Each step solve a nonlinear optimization problem (with LM) # Initialise the spline function objects self.eqs.trajectories.init_splines() # Get an initial value (guess) self.eqs.get_guess() # Build the collocation equations system C = self.eqs.build() F, DF = C.F, C.DF old_res = 1e20 old_sol = None new_solver = True while True: self.tmp_sol = self.eqs.solve(F, DF, new_solver=new_solver) # in the following iterations we want to use the same solver # object (we just had an intermediate look, whether the solution # of the initial value problem is already sufficient accurate.) new_solver = False # Set the found solution self.eqs.trajectories.set_coeffs(self.tmp_sol) # !! dbg # self.eqs.trajectories.set_coeffs(self.eqs.guess) # Solve the resulting initial value problem self.simulate() self._show_intermediate_results() # check if desired accuracy is reached self.check_accuracy() if self.reached_accuracy: # we found a solution break # now decide whether to continue with this solver or not slvr = self.eqs.solver if slvr.cond_external_interrupt: self.log_debug( 'Continue minimization after external interrupt') continue if slvr.cond_num_steps: if slvr.solve_count < self._parameters['accIt']: msg = 'Continue minimization (not yet reached tolerance nor limit of attempts)' self.log_debug(msg) continue else: break if slvr.cond_rel_tol and slvr.solve_count < self._parameters[ 'localEsc']: # we are in a local minimum # > try to jump out by randomly changing the solution # Note: this approach seems not to be successful if self.eqs.trajectories.n_parts_x >= 40: # values between 0.32 and 3.2: scale = 10**(np.random.rand(len(slvr.x0)) - .5) # only use the actual value if slvr.res < old_res: old_sol = slvr.x0 old_res = slvr.res slvr.x0 *= scale else: slvr.x0 = old_sol * scale self.log_debug('Continue minimization with changed x0') continue if slvr.cond_abs_tol or slvr.cond_rel_tol: break else: # IPS() self.log_warn( "unexpected state in mainloop of outer iteration -> break loop" ) break def _process_seed(self, init_kwargs): """ If the Parameter `seed` is passed, this should be the same as first_guess={'seed': xyz}. (Calling convenience) -> update kwargs["first_guess"] if necessary :return: None """ first_guess = init_kwargs.get("first_guess", None) seed = init_kwargs.get("seed", None) # only one of the two arguments is allowed -> at least one must be None assert (first_guess is None) or (seed is None) if seed is None: # leave kwargs unchanged return assert isinstance(seed, numbers.Real) init_kwargs["first_guess"] = {"seed": seed} def _process_first_guess(self): """ In case of a provided guess of all free parameters (coefficients) this is the place to ensure the right number of spline parts. :return: None ------- """ if self.eqs._first_guess is None: return # ensure that either both keys or none are present relevant_keys = {'complete_guess', 'n_spline_parts'} intrsctn = relevant_keys.intersection(self.eqs._first_guess) if len(intrsctn) == 0: # nothing to preprocess return if not len(intrsctn) == 2: missing_key = list(relevant_keys.difference( self.eqs._first_guess))[0] msg = "Missing dict-key in keyword-argument 'first_guess': %s" raise ValueError(msg % missing_key) n_spline_parts = self.eqs._first_guess['n_spline_parts'] self.eqs.trajectories.raise_spline_parts(n_spline_parts) def _process_refsol(self): """ Handle given reference solution and (optionally) visualize it (for debug and development). :return: None """ if self.refsol is None: return self.check_refsol_consistency() auxiliary.make_refsol_callable(self.refsol) # the reference solution specifies how often spline parts should # be raised if not hasattr(self.refsol, 'n_raise_spline_parts'): self.refsol.n_raise_spline_parts = 0 for i in range(self.refsol.n_raise_spline_parts): self.eqs.trajectories.raise_spline_parts() if self._parameters.get('show_refsol', False): # dbg visualization guess = np.empty(0) C = self.eqs.trajectories.init_splines(export=True) self.eqs.guess = None new_params = OrderedDict() tt = self.refsol.tt new_spline_values = [] fnclist = self.refsol.xxfncs + self.refsol.uufncs for i, (key, s) in enumerate(C.splines.iteritems()): coeffs = s.interpolate(fnclist[i], set_coeffs=True) new_spline_values.append(auxiliary.vector_eval(s.f, tt)) guess = np.hstack((guess, coeffs)) if 'u' in key: pass # dbg: # IPS() sym_num_tuples = zip(s._indep_coeffs_sym, coeffs) # List of tuples like (cx1_0_0, 2.41) new_params.update(sym_num_tuples) self.refsol_coeff_guess = guess # IPS() mm = 1. / 25.4 # mm to inch scale = 8 fs = [75 * mm * scale, 35 * mm * scale] rows = np.round( (len(new_spline_values) + 0) / 2.0 + .25) # round up labels = self.dyn_sys.states + self.dyn_sys.inputs plt.figure(figsize=fs) for i in xrange(len(new_spline_values)): plt.subplot(rows, 2, i + 1) plt.plot(tt, self.refsol.xu_list[i], 'k', lw=3, label='sim') plt.plot(tt, new_spline_values[i], label='new') ax = plt.axis() plt.vlines(s.nodes, -1000, 1000, color=(.5, 0, 0, .5)) plt.axis(ax) plt.grid(1) ax = plt.axis() plt.ylabel(labels[i]) plt.legend(loc='best') plt.show() def _show_intermediate_results(self): """ If the appropriate parameters is set this method displays intermediate results. Useful for debugging and development. :return: None (just polt) """ if not self._parameters['show_ir']: return # dbg: create new splines (to interpolate the obtained result) # TODO: spline interpolation of simulation result is not so interesting C = self.eqs.trajectories.init_splines(export=True) new_params = OrderedDict() tt = self.sim_data_tt new_spline_values = [ ] # this will contain the spline interpolation of sim_data actual_spline_values = [] old_spline_values = [] guessed_spline_values = auxiliary.eval_sol(self, self.eqs.guess, tt) data = list(self.sim_data_xx.T) + list(self.sim_data_uu.T) for i, (key, s) in enumerate(C.splines.iteritems()): coeffs = s.interpolate((self.sim_data_tt, data[i]), set_coeffs=True) new_spline_values.append(auxiliary.vector_eval(s.f, tt)) s_actual = self.eqs.trajectories.splines[key] if self.eqs.trajectories.old_splines is None: s_old = splines.get_null_spline(self.a, self.b) else: s_old = self.eqs.trajectories.old_splines[key] actual_spline_values.append(auxiliary.vector_eval(s_actual.f, tt)) old_spline_values.append(auxiliary.vector_eval(s_old.f, tt)) # generate a pseudo "solution" (for dbg) sym_num_tuples = zip(s._indep_coeffs_sym, coeffs) # List of tuples like (cx1_0_0, 2.41) new_params.update(sym_num_tuples) # calculate a new "solution" (sampled simulation result pseudo_sol = [] notfound = [] for key in self.eqs.all_free_parameters: value = new_params.pop(key, None) if value is not None: pseudo_sol.append(value) else: notfound.append(key) # visual comparision: mm = 1. / 25.4 # mm to inch scale = 8 fs = [75 * mm * scale, 35 * mm * scale] rows = np.round((len(data) + 2) / 2.0 + .25) # round up par = self.get_par_values() # this is needed for vectorized evaluation n_tt = len(self.sim_data_tt) assert par.ndim == 1 par = par.reshape(self.dyn_sys.n_par, 1) par = par.repeat(n_tt, axis=1) # input part of the vectorfiled gg = self.eqs.Df_vectorized(self.sim_data_xx.T, self.sim_data_uu.T, self.sim_data_tt.T, par).transpose(2, 0, 1) gg = gg[:, :-1, -1] # drift part of the vf ff = self.eqs.ff_vectorized(self.sim_data_xx.T, self.sim_data_uu.T * 0, self.sim_data_tt.T, par).T[:, :-1] labels = self.dyn_sys.states + self.dyn_sys.inputs plt.figure(figsize=fs) for i in xrange(len(data)): plt.subplot(rows, 2, i + 1) plt.plot(tt, data[i], 'k', lw=3, label='sim') plt.plot(tt, old_spline_values[i], lw=3, label='old') plt.plot(tt, actual_spline_values[i], label='actual') plt.plot(tt, guessed_spline_values[i], label='guessed') # plt.plot(tt, new_spline_values[i], 'r-', label='sim-interp') ax = plt.axis() plt.vlines(s.nodes, -10, 10, color="0.85") plt.axis(ax) plt.grid(1) plt.ylabel(labels[i]) plt.legend(loc='best') # show error between sim and col plt.subplot(rows, 2, i + 2) err = np.linalg.norm(np.array(data) - np.array(actual_spline_values), axis=0) plt.title("log error") plt.semilogy(tt, err) plt.gca().axis([tt[0], tt[-1], 1e-5, 1e2]) plt.grid(1) # plt.subplot(rows, 2, i + 2) # plt.title("vf: f") # plt.plot(tt, ff) # # plt.subplot(rows, 2, i + 3) # plt.title("vf: g") # plt.plot(tt, gg) if 0: fname = auxiliary.datefname(ext="pdf") plt.savefig(fname) self.log_debug(fname + " written.") plt.show() # IPS() def simulate(self): """ This method is used to solve the resulting initial value problem after the computation of a solution for the input trajectories. """ self.log_debug("Solving Initial Value Problem") # calulate simulation time T = self.dyn_sys.b - self.dyn_sys.a ##:ck: obsolete comment? # Todo T = par[0] * T # get list of start values start = self.dyn_sys.xa ff = self.dyn_sys.f_num_simulation par = self.get_par_values() # create simulation object x_fncs, xdot_fncs, u_fnc = self.get_constrained_spline_fncs() mpc_flag = self.nIt >= self.mpc_sim_threshold self.simulator = Simulator(ff, T, start, x_col_fnc=x_fncs, u_col_fnc=u_fnc, z_par=par, dt=self._parameters['dt_sim'], mpc_flag=mpc_flag) self.log_debug("start: %s" % str(start)) # forward simulation self.sim_data = self.simulator.simulate() ##:: S.simulate() is a method, # returns a list [np.array(self.t), np.array(self.xt), np.array(self.ut)] # self.sim_data is a `self.variable?` (initialized with None in __init__(...)) # convenient access self.sim_data_tt, self.sim_data_xx, self.sim_data_uu = self.sim_data def check_accuracy(self): """ Checks whether the desired accuracy for the boundary values was reached. It calculates the difference between the solution of the simulation and the given boundary values at the right border and compares its maximum against the tolerance. If set by the user it also calculates some kind of consistency error that shows how "well" the spline functions comply with the system dynamic given by the vector field. """ # this is the solution of the simulation a = self.sim_data[0][0] b = self.sim_data[0][-1] xt = self.sim_data[1] x_sym = self.dyn_sys.states xb = self.dyn_sys.xb # what is the error self.log_debug(40 * "-") self.log_debug("Ending up with: Should Be: Difference:") err = np.empty(xt.shape[1]) for i, xx in enumerate(x_sym): err[i] = abs(xb[i] - xt[-1][i]) ##:: error (x1, x2) at end time self.log_debug( str(xx) + " : %f %f %f" % (xt[-1][i], xb[i], err[i])) self.log_debug(40 * "-") # if self._ierr: ierr = self._parameters['ierr'] eps = self._parameters['eps'] xfnc, dxfnc, ufnc = self.get_constrained_spline_fncs() if ierr: # calculate maximum consistency error on the whole interval maxH = auxiliary.consistency_error((a, b), xfnc, ufnc, dxfnc, self.dyn_sys.f_num_simulation, par=self.get_par_values()) reached_accuracy = (maxH < ierr) and (max(err) < eps) self.log_debug('maxH = %f' % maxH) else: # just check if tolerance for the boundary values is satisfied reached_accuracy = (max(err) < eps) msg = " --> reached desired accuracy: " + str(reached_accuracy) if reached_accuracy: self.log_info(msg) else: self.log_debug(msg) # save for late reference self.sim_err = err self.reached_accuracy = reached_accuracy def get_par_values(self): """ extract the values of additional free parameters from last solution (self.tmp_sol) """ assert self.tmp_sol is not None N = len(self.tmp_sol) start_idx = N - self.dyn_sys.n_par return self.tmp_sol[start_idx:] def plot(self): """ Plot the calculated trajectories and show interval error functions. This method calculates the error functions and then calls the :py:func:`visualisation.plotsim` function. """ try: import matplotlib except ImportError: self.log_error('Matplotlib is not available for plotting.') return if self.constraints: sys = self._dyn_sys_orig else: sys = self.dyn_sys # calculate the error functions H_i(t) ace = auxiliary.consistency_error max_con_err, error = ace( (sys.a, sys.b), self.eqs.trajectories.x, self.eqs.trajectories.u, self.eqs.trajectories.dx, sys.f_num_simulation, len(self.sim_data[0]), True) H = dict() for i in self.eqs.trajectories._eqind: H[i] = error[:, i] visualisation.plot_simulation(self.sim_data, H) def save(self, fname=None, quiet=False): """ Save data using the python module :py:mod:`pickle`. """ if self.nIt is None: msg = "No Iteration has taken place. Cannot save." raise ValueError(msg) save = dict.fromkeys(['sys', 'eqs', 'traj']) # system state save['sys'] = dict() save['sys']['state'] = dict.fromkeys(['nIt', 'reached_accuracy']) save['sys']['state']['nIt'] = self.nIt save['sys']['state']['reached_accuracy'] = self.reached_accuracy # simulation results save['sys']['sim_data'] = self.sim_data # parameters save['sys']['parameters'] = self._parameters save['eqs'] = self.eqs.save() save['traj'] = self.eqs.trajectories.save() if fname is not None: if not (fname.endswith('.pcl') or fname.endswith('.pcl')): fname += '.pcl' with open(fname, 'wb') as dumpfile: pickle.dump(save, dumpfile) if not quiet: self.log_info("File written: {}".format(fname)) return save def create_new_TP(self, **kwargs): """ Create a new TransitionProblem object with the same data like the present one, except what is specified in kwargs :return: TransitionProblem object """ # DynamicalSystem(f_sym=ff, a=a, b=b, xa=xa, xb=xb, ua=ua, ub=ub, uref=uref, ds = self.dyn_sys new_kwargs = dict(ff=ds.f_sym, a=self.a, b=self.b, xa=ds.xa, xb=ds.xb, ua=ds.ua, ub=ds.ub, uref=ds.uref_fnc, constraints=self.constraints) new_kwargs.update(self.initial_kwargs) # update with the information which was passed to this call new_kwargs.update(kwargs) return TransitionProblem(**new_kwargs) @property def a(self): return self.dyn_sys.a @property def b(self): return self.dyn_sys.b # convencience access to linspace of time values (e.g. for debug-plotting) @property def tt(self): return self.dyn_sys.tt