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()
