def test_scipy_RK45(self): t0 = 0 y0 = np.array([1, 0, 1, 0]) tmax1 = np.pi * 2 # period/2 ==> opposite position tmax2 = np.pi * 5 # period/2 ==> opposite position h = 0.01 integrator = RK45(_mass_spring_sp, t0, y0, rtol=h, atol=10**-6, t_bound=tmax1) while not (integrator.status == 'finished'): integrator.step() Y1 = integrator.y T1 = integrator.t integrator = RK45(_mass_spring_sp, T1, Y1, rtol=h, atol=10**-6, t_bound=tmax2) while not (integrator.status == 'finished'): integrator.step() Y2 = integrator.y T2 = integrator.t self.assertAlmostEqual(Y1[0], -1, places=2) self.assertAlmostEqual(Y2[2], -1, places=2) self.assertAlmostEqual(T1, tmax1, places=4) self.assertAlmostEqual(T2, tmax2, places=4)
def RunF16Sim(initialState, tMax, der_func, F16Model, ap, llc, pass_fail, sim_step=0.01, multipliers=None): 'Simulates and analyzes autonomous F-16 maneuvers' # append integral error states to state vector initialState = np.array(initialState, dtype=float) x0 = np.zeros((initialState.shape[0] + llc.get_num_integrators() + ap.get_num_integrators(),)) x0[:initialState.shape[0]] = initialState # run the numerical simulation times = [0] states = [x0] modes = [ap.state] _, u, Nz, ps, _ = controlledF16(times[-1], states[-1], F16Model, ap, llc, multipliers=multipliers) Nz_list = [Nz] ps_list = [ps] u_list = [u] rk45 = RK45(der_func, times[-1], states[-1], tMax) while rk45.status == 'running': rk45.step() if rk45.t > times[-1] + sim_step: dense_output = rk45.dense_output() while rk45.t > times[-1] + sim_step: t = times[-1] + sim_step times.append(t) states.append(dense_output(t)) updated = ap.advance_discrete_state(times[-1], states[-1]) modes.append(ap.state) # re-run dynamics function at current state to get non-state variables xd, u, Nz, ps, Ny_r = controlledF16(times[-1], states[-1], F16Model, ap, llc, multipliers=multipliers) pass_fail.advance(times[-1], states[-1], ap.state, xd, u, Nz, ps, Ny_r) Nz_list.append(Nz) ps_list.append(ps) u_list.append(u) if updated: rk45 = RK45(der_func, times[-1], states[-1], tMax) break if pass_fail.break_on_error and not pass_fail.result(): break if pass_fail.break_on_error and not pass_fail.result(): break result = pass_fail.result() # make sure the solver didn't fail if rk45.status != 'finished': result = False # fail return result, times, states, modes, ps_list, Nz_list, u_list
def one_time_step_function(self,rho0,t0,tf,*,manifold_key = None):#,RWA_gap = 0): # dL = self.set_dL(RWA_gap) # rho0_rotated = rho0 * np.exp(1j*RWA_gap * t0) # rk45 = RK45(dL,t0,rho0_rotated,tf) num_steps = 0 if manifold_key == None: rk45 = RK45(self.dL,t0,rho0,tf,atol=self.atol,rtol=self.rtol) else: dL = self.get_dL_manual(manifold_key) rk45 = RK45(dL,t0,rho0,tf,atol=self.atol,rtol=self.rtol) while rk45.t < tf: rk45.step() num_steps += 1 rho_final = rk45.y# * np.exp(-1j*RWA_gap * tf) return rho_final
def simulate(initial_state, name=""): current = lower_bound res = initial_state.copy() total_tol = [0, 0, 0, 0] t = numpy.arange(lower_bound, upper_bound + TOLERANCE, step) x, dx, y, dy = [initial_state[0]], [initial_state[1] ], [initial_state[2] ], [initial_state[3]] while numpy.abs(current - upper_bound) > TOLERANCE: integrator = RK45(fun=motion_equation, t0=current, y0=res, t_bound=current + step, rtol=TOLERANCE) while integrator.status == "running": integrator.step() previous_res = res res = integrator.y x_, dx_, y_, dy_ = res x.append(x_) dx.append(dx_) y.append(y_) dy.append(dy_) current += step tolerance = abs(res - previous_res) * TOLERANCE total_tol += tolerance print("{0:1.2f} : {1}\n ±{2}".format(current, res, total_tol)) space_plot.plot(x, y, label="{0} : (x, y)".format(name)) velocities_plot.plot(t, dx, label="{0}: dx/dt(t)".format(name)) velocities_plot.plot(t, dy, label="{0}: dy/dt(t)".format(name))
def _show_chaos(xi, library, normalized_rsi): def f(t, y): library_values = [] for i in range(len(library)): if type(library[i]) == int: library_values.append(library[i]) elif len(library[i]) == 1: library_values.append(y[library[i]]) else: library_values.append(y[library[i][0]] / (1 + abs(y[library[i][1]]))) y_dot = np.matmul(np.array(library_values), xi.T) return y_dot t0 = 0 max_t = 80 ts = [] y0 = normalized_rsi[0] ys = [y0] distances = [] integrator = RK45(f, t0, y0, max_t) while integrator.status != 'finished': integrator.step() distance = np.linalg.norm(integrator.y - ys[-1]) print(distance) ts.append(integrator.t) ys.append(integrator.y) distances.append(distance) _draw_distance_time_series(ts, distances)
def RK45_wrapper(x0, time_checkpoints, f, stepsize=None): if stepsize is None: stepsize = 0.01 * (time_checkpoints[-1] - time_checkpoints[0]) t_set = [time_checkpoints[0]] x_set = [x0] t_checkpoint_new = [time_checkpoints[0]] x_checkpoint = [x0] for i in range(len(time_checkpoints) - 1): t1 = time_checkpoints[i + 1] if i > 0: x0 = ox.y t0 = ox.t else: t0 = time_checkpoints[i] ox = RK45(f, t0, x0, t1, max_step=stepsize) while ox.t < t1: msg = ox.step() t_set.append(ox.t) x_set.append(ox.y) x_checkpoint.append(ox.y) t_checkpoint_new.append(ox.t) return x_set, t_set, x_checkpoint, t_checkpoint_new
def run_interval(count, times, conditions, results): # set up this leg of the simulation simulator = RK45(model, times[0], conditions, t_bound=5, max_step = 0.01) # set the desired points global Q1_DESIRED, Q2_DESIRED, Q4_DESIRED, Q5_DESIRED, q_desireds Q1_DESIRED = q_desireds[count, 0] Q2_DESIRED = q_desireds[count, 1] Q4_DESIRED = q_desireds[count, 2] Q5_DESIRED = q_desireds[count, 3] # run this leg of the simulation while True: # attempt to step try: simulator.step() # if the solver is finished except Exception as e: break # if it's close enough to the target if (abs(float(Q1_DESIRED - Q1_CURRENT)) <= eps) and (abs(float(Q4_DESIRED - Q4_CURRENT)) <= eps): break # store the current state conditions = simulator.y # append step results to simulation results times = vstack([times, simulator.t]) results = vstack([results, (Q1_CURRENT, Q2_CURRENT, Q4_CURRENT, Q5_CURRENT)]) # return this leg of the simulation return times, conditions, results
def scipy_runge_kutta(self, fun, y0, t0=0, t_bound=10): return RK45(fun, t0, y0, t_bound, rtol=self.time_span / self.number_iterations, atol=1e-4)
def run_interval(theta, times, conditions, results): # set up this leg of the simulation simulator = RK45(model, times[0], conditions, t_bound=0.1, max_step = 0.1) # set the desired points global X_DESIRED, Y_DESIRED X_DESIRED = xh(theta) Y_DESIRED = yh(theta) # run this leg of the simulation while True: # attempt to step try: simulator.step() # if the solver is finished except Exception as e: break # if it's close enough to the target if linalg.norm([float(X_DESIRED - X_CURRENT), float(Y_DESIRED - Y_CURRENT)]) <= eps: break # store the current state conditions = simulator.y # append step results to simulation results times = vstack([times, simulator.t]) results = vstack([results, (X_CURRENT, Y_CURRENT, *conditions)]) # return this leg of the simulation return theta, times, conditions, results
def RKU_onetimestep(self,psi0,t0,tf,*,manifold_num = 0): dH = [self.dH0,self.dH1,self.dH2] fun = dH[manifold_num] rk45 = RK45(fun,t0,psi0,tf,atol=self.atol,rtol=self.rtol,vectorized=True) while rk45.t < tf: rk45.step() psi_final = rk45.y return psi_final
def dist(t, s, v, w): global V0, S0, W0 S0 = s W0 = w V0 = v var = RK45(vel, 0, S0, t, max_step=(1.0 / 20.0)) return var
def advance(self, t, f): r = RK45(self.diffEq, self.last_t, f, t, vectorized=True) self.last_t = t while r.status is not 'finished': r.step() return r.t, r.y
def get_trajectory( self, initial_speed: float, launch_angle: float, launch_direction_angle: float, initial_spin: float, spin_angle: float, delta_time: float = 0.01, ) -> pd.DataFrame: # TODO: make the return value a trajectory object """ computes a batted ball trajectory. speed is in miles-per-hour, angles in degrees, and spin in revolutions per minute :param initial_speed: float :param launch_angle: float :param launch_direction_angle: float :param initial_spin: float :param spin_angle: float :param delta_time: float :return: pandas data frame """ initial_velocity = ( initial_speed * self.env_parameters.unit_conversions.MPH_TO_FTS # type: ignore # TODO: https://github.com/python/mypy/issues/5439 Remove the ^ type: ignore after this is fixed in mypy * unit_vector(np.float64(launch_angle), np.float64(launch_direction_angle))) initial_conditions = np.concatenate( (self.initial_position, initial_velocity), axis=0) rk_solution = RK45( partial( self.trajectory_fun, launch_angle=launch_angle, launch_direction_angle=launch_direction_angle, spin=initial_spin, spin_angle=spin_angle, ), 0, initial_conditions, t_bound=1000, max_step=delta_time, ) ans = [] z = self.initial_position[2] while z >= 0: rk_solution.step() res = rk_solution.y z = res[2] ans.append([rk_solution.t] + list(res)) result_df = pd.DataFrame(np.array(ans).reshape(-1, 7)) result_df.columns = pd.Index(["t", "x", "y", "z", "vx", "vy", "vz"]) return result_df
def test_k2a(self): # Test to compare simulation of K2A system with PowerFactory results. # Error should be bounded by specified value. import ps_models.k2a as model_data model = model_data.load() ps = dps.PowerSystemModel(model=model) ps.avr['SEXS']['T_a'] = 2 ps.avr['SEXS']['T_e'] = 0.1 ps.gov['TGOV1']['T_1'] = 0.5 ps.gov['TGOV1']['T_2'] = 1 ps.gov['TGOV1']['T_3'] = 2 ps.power_flow() ps.init_dyn_sim() t_end = 10 max_step = 5e-3 # PowerFactory result pf_res = load_pf_res('k2a/powerfactory_res.csv') # Python result x0 = ps.x0 sol = RK45(ps.ode_fun, 0, x0, t_end, max_step=max_step) t = 0 result_dict = defaultdict(list) print('Running dynamic simulation') while t < t_end: # print(t) # Simulate next step result = sol.step() t = sol.t if t >= 1 and t <= 1.1: # print('Event!') ps.y_bus_red_mod[0, 0] = 1e6 else: ps.y_bus_red_mod[0, 0] = 0 # Store result variables result_dict['Global', 't'].append(sol.t) [ result_dict[tuple(desc)].append(state) for desc, state in zip(ps.state_desc, sol.y) ] index = pd.MultiIndex.from_tuples(result_dict) result = pd.DataFrame(result_dict, columns=index) error = compute_error(ps, result, pf_res, max_step) self.assertLessEqual(error, 0.02)
def test_ieee39(self): # Test to compare simulation of IEEE 39 bus system with PowerFactory results. # Error should be bounded by specified value. import ps_models.ieee39 as model_data model = model_data.load() ps = dps.PowerSystemModel(model=model) ps.power_flow() ps.init_dyn_sim() t_end = 10 max_step = 5e-3 # PowerFactory result pf_res = load_pf_res('ieee39/powerfactory_res.csv') x0 = ps.x0 sol = RK45(ps.ode_fun, 0, x0, t_end, max_step=max_step) t = 0 result_dict = defaultdict(list) # monitored_variables = ['e_q', 'v_g', 'v_g_dev', 'v_pss'] print('Running dynamic simulation') while t < t_end: # print(t) # Simulate next step result = sol.step() t = sol.t if t >= 1 and t <= 1.05: # print('Event!') ps.y_bus_red_mod[0, 0] = 1e6 else: ps.y_bus_red_mod[0, 0] = 0 # Store result variables result_dict['Global', 't'].append(sol.t) # for var in monitored_variables: # [result_dict[(gen_name, var)].append(var_) for i, (var_, gen_name) in # enumerate(zip(getattr(ps, var), ps.generators['name']))] [ result_dict[tuple(desc)].append(state) for desc, state in zip(ps.state_desc, sol.y) ] index = pd.MultiIndex.from_tuples(result_dict) result = pd.DataFrame(result_dict, columns=index) error = compute_error(ps, result, pf_res, max_step) self.assertLessEqual(error, 0.02)
def vel(t, Vn): global t0, V0 v1 = RK45(acc, t0, V0, t) while v1.status != "finished": v1.step() V0 = v1.y t0 = t return V0
def ps_state(self, step_n): t0 = 0 y = self.get_y0() rk = RK45(self.multiply_vector, t0, y, 10000, vectorized=True) for i in range(step_n): rk.step() res = rk.y res = res / np.linalg.norm(res, ord=1) ps = list(map(lambda x: 'P{}'.format(x), list(range(self.n)))) plt.bar(ps, res, align='center', width=0.5) plt.show()
def lorenz_sys_rk(koef): a1, a2, a3, c1, c2, c3, c4, d1, d2 = koef[0], koef[1], koef[2], koef[ 3], koef[4], koef[5], koef[6], koef[7], koef[8] tmax, n = t_len, t_len t = np.linspace(0, tmax, n) f = RK45(lorenzsys, 0.0, (x_00, y_00, z_00), t_len, f_args=(a1, a2, a3, c1, c2, c3, c4, d1, d2)) X, Y, Z = f.T return X, Y, Z
def export_solution(self): t0 = 0 y = self.get_y0() rk = RK45(self.multiply_vector, t0, y, 10000, vectorized=True) self.folder_name = datetime.now().strftime('%Y%m%d%H%M%S') os.mkdir('result/{}'.format(self.folder_name)) for i in range(10): res = rk.y res = res / np.linalg.norm(res, ord=1) df = pd.DataFrame(res) filename = './result/{}/step_{}.csv'.format(self.folder_name, i) df.to_csv(filename, index=True, header=False) rk.step()
def reset(self, reset_state=None, dock_port=None): """ Reset state, control and integrator """ if reset_state is not None: self.initial_state = reset_state if dock_port is not None: self.dock_port_inB_pos = dock_port self.state = self.initial_state self.u = np.zeros(self.dim_u) self.integrator = RK45(self.f, self.t0, self.state, self.dt) return self.state
def simulador(puntos, angulos): ''' esta función devuelve dos solver de scipy sol2 solo se ha construido para depurar, el que usa el simulador es sol''' global cCartesianas global headings cCartesianas = puntos headings = angulos sol2 =RK45(lambda t,y:dt(t,y,taun,M,m,b,J,Ji),0.,y0,Tf,max_step=0.01) sol = solve_ivp(lambda t,y:dt(t,y,taun,M,m,b,J,Ji),\ [0.,Tf],y0,t_eval=np.arange(0,Tf+0.1,0.1)) return sol, sol2
def get_solutions(self, step_n, pis): t0 = 0 y = self.get_y0(pis=pis) rk = RK45(self.multiply_vector, t0, y, 10000, vectorized=True) y_res = [] x_res = [] for i in range(step_n): sum = 0 res = rk.y res = res / np.linalg.norm(res, ord=1) for i, r in enumerate(res, start=0): if i in pis: sum = sum + r y_res.append(sum) x_res.append(rk.t) rk.step() return [x_res, y_res]
def solve_dynamic_system(system, args, max_step, t_bound, x_0, t_0=0, bc_imposer=None, bc_args=None): """ Solves a dynamical system using Dormand–Prince with 4th order error control and 5th order stepping "RK45". :param system: A callable dynamic system taking (t,x,J) :param args: Arguments to be passed to the system :param max_step: The maximum allowed length of a step :param t_bound: The time at which to stop integration :param x_0: The initial state :param t_0: The initial time :param bc_imposer: Callable that can be used to modify x every timestep in order to impose bc. :param bc_args: Args for the bc_imposer :return: An array of states and an array of timestamps """ x = x_0 t_arr = np.zeros((1, 1)) x = np.expand_dims(x, axis=1) ivp = RK45(fun=lambda t, y: system(t, y, args), t0=t_0, y0=x_0, t_bound=t_bound, max_step=max_step, rtol=0.001, atol=1e-06, vectorized=False) while True: if (ivp.t >= t_bound): break ivp.step() if bc_imposer is not None: ivp.y = bc_imposer(ivp.y, ivp.t, bc_args) x = np.append(x, np.expand_dims(ivp.y, axis=1), axis=1) t_arr = np.append(t_arr, np.ones((1, 1)) * ivp.t, axis=1) print("[Info] Made " + str(t_arr.shape[1]) + " timesetps") return x, t_arr
def reliability(self, step_n, pis): t0 = 0 y = self.get_y0(pis=pis) rk = RK45(self.multiply_vector, t0, y, 10000, vectorized=True) y_res = [] x_res = [] for i in range(step_n): sum = 0 rk.step() res = rk.y res = res / np.linalg.norm(res, ord=1) for i, r in enumerate(res, start=0): if i in pis: sum = sum + r y_res.append(sum) x_res.append(rk.t) plt.plot(x_res, y_res) plt.grid(True) plt.show()
def solve_time(ode, t, x_initial, x_dot_initial, F_load, V): end_time = 250e-6 states = [] success = False solve_time = -1 solver = RK45(ode, 0, (x_initial, x_dot_initial, F_load, V), .5) # y = odeint(ode, (x_initial, x_dot_initial, F_load, V), t) while solver.t < end_time: solver.step() solver_state = solver.dense_output() if solver_state.y_old[0] >= 3.833e-6: success = True states.append(solver_state.y_old) solve_time = solver.dense_output().t break else: states.append(solver_state.y_old) return states, success, solve_time
def step(self, action): """ 进行一次初值的求解,从lambda0到lambda_n action:lambda_0 , t_f return: 返回state,ceq, done, info """ lambda_0 = action[:-1] t_f = action[-1] x_goal = 0.45 t_f = abs(t_f) # 微分方程 X0 = np.hstack([self.state, lambda_0]) t = np.linspace(0, t_f, 101) # Old API # X = odeint(self.motionequation, X0, t, rtol=1e-12, atol=1e-12) # New API ode = RK45(self.motionequation,t[0],X0,t_f) X = [X0] while True: ode.step() X.append(ode.y) if ode.status != 'running': break X = np.array(X) # X = self.odeint(self.motionequation, X0, t) # 末端Hamilton函数值 X_end = X[-1, :] H_end = self.motionequation(0,X_end, end=True) ceq = np.array([H_end,X_end[0]-x_goal,X_end[3]]) done = True info = {} info['X'] = X.copy() info['t'] = t.copy() info['ceq'] = ceq return self.state.copy(), ceq, done, info
def advance(self, dt, f): """Runge-Kutta differential equation solver advance function Uses an adaptive RK4 routine plus some fancy RK5 error approximation or something. This is currently only set up to work with relative time steps rather than absolute times since the simulation began. Parameters ---------- dt : float The amount of time by which to advance the system. Can be positive or negative. f : 1D numpy array Dependent variables """ if not self.initialized: self.r = RK45(self.diffEq, 0, f, dt, rtol=self.tolerance, vectorized=True) self.initialized = True else: self.r.t = 0 self.r.t_bound = dt self.r.status = 'running' # i=0 while self.r.status is not 'finished': self.r.step() # i+=1 # self.r.status = 'finished' return self.r.t, self.r.y
def solve_ode(self): ''' solve the ordinary differential equations defined by func, using RK45 ''' solver = RK45(self.func, t0=0., y0=self.initial_conditions, t_bound=self.t_bound, max_step=self.dt_max, rtol=1e-6, atol=1e-9) solution_list = [] # Maximum t_bound increase iteration is 10 for i in range(10): while 'running' == solver.status: solution_list.append(np.append(solver.t, solver.y)) solver.step() # Check momentum difference of last two results p_diff = max([abs(solution_list[-1][4]-solution_list[-2][4]), abs(solution_list[-1][5]-solution_list[-2][5]), abs(solution_list[-1][6]-solution_list[-2][6])]) # Continue running until momentum difference is negligible if p_diff < very_small: break else: solver.t_bound *= 1.2 solver.status = 'running' if p_diff>=very_small: warnings.warn('Momentum change at t_bound is still not negligible. Maybe you need a larger t_bound.') self.solution = np.array(solution_list)
def step(self, u): """ RK45 integrator for one step/dt :param u:Control Command--[F,Mx,My,Mz] :return: System State """ while not (self.integrator.status == 'finished'): self.integrator.step() self.state = self.integrator.y att_flag, att_lim = self.attitude_limit() if att_flag: self.state[6:10] = att_lim self.state[10:] = np.array([0.0, 0.0, 0.0]) self.u = self.u_limit(u) self.t = self.integrator.t self.integrator = RK45(self.f, self.integrator.t, self.state, self.integrator.t + self.dt) return self.state
def reset(self): if self.has_integrator: if self.integrator_type == Integrators.lsoda: self.integrator = LSODA(self.dydt, self.t0, self.dydt_y0, np.inf, max_step=self.dydt_max_step, **self.dydt_solver_args) elif self.integrator_type == Integrators.dop853: self.integrator = DOP853(lambda t, y: self.dydt_y0, self.t0, self.dydt_y0, np.inf, max_step=self.dydt_max_step, **self.dydt_solver_args) self.integrator.fun = self.dydt elif self.integrator_type == Integrators.rk45: self.integrator = RK45(lambda t, y: self.dydt_y0, self.t0, self.dydt_y0, np.inf, max_step=self.dydt_max_step, **self.dydt_solver_args) self.integrator.fun = self.dydt elif self.integrator_type == Integrators.implicit_midpoint: self.integrator = ImplicitMidpoint(self.dydt, self.t0, self.dydt_y0, np.inf, max_step=self.dydt_max_step, **self.dydt_solver_args) else: raise ValueError( f'Unsupported integrator type: {self.integrator_type}') for sys in self.systems[IterationMode.cvx] + self.systems[ IterationMode.map]: sys.reset() self.discrete_y[sys.uuid] = sys._y.copy() for sys in self.systems[IterationMode.traj]: sys.reset()