I am using scipy.solve_ivp to solve an ODE in a loop, such that in every iteration I change some term in the ODE (which is like the force applied).
In terms of my problem - I just cant seem to understand why when I track events of zero crossing the solution sometimes stalls and essentially zeros out (as the figure below shows).
The code is a bit long, but only a small portion of it is actually relevant to my problem
(1) initialize some constants
MASS = 2.6e-4
WING_LENGTH = 0.07 # meters
GYRATION_RADIUS = 0.6 * WING_LENGTH
MoI = MASS * GYRATION_RADIUS ** 2 # Moment of Inertia
AIR_DENSITY = 1.2
WING_AREA = 0.5 * WING_LENGTH * (0.5 * WING_LENGTH) * np.pi # 1/2 ellipse with minor radios ~ 1/2 major = length/2
# drag coefficients from whitney & wood (JFM 2010):
C_D_MAX = 3.4
C_D_0 = 0.4
C_L_MAX = 1.8
ZERO_CROSSING = 1
RADIAN45 = np.pi / 4
RADIAN135 = 3 * RADIAN45
(2) Define a wrapper which is a RobotSimulation class - it's an easy interface that allows to easily change the values of the ODE. The important function is solve_dynamics (the others are not part of the problem I believe)
def phi_dot_zero_crossing_event(t, y):
"""
this event is given to solve_ivp to track if phi_dot == 0
:param t: unused time variable
:param y: a vector of [phi,phi_dot]
:return:
"""
return y[1]
class RobotSimulation:
def __init__(self, motor_torque=lambda x: 0, alpha=RADIAN45, phi0=0.0, phi_dot0=0.01, start_t=0, end_t=0.05, delta_t=0.001):
self.solution = 0
self.motor_torque = motor_torque
self.alpha = alpha # angle of attack
self.phi0 = phi0
self.phi_dot0 = phi_dot0
self.start_t = start_t
self.end_t = end_t
self.delta_t = delta_t
def set_time(self, new_start, new_end) -> None:
"""
we update the start time to be the end of the prev time and the end time
as the last end time + the window size we wish
"""
self.start_t = new_start
self.end_t = new_end
def set_init_cond(self, new_phi0, new_phi_dot0) -> None:
"""
we update the initial conditions to be the last value of previous iterations
"""
self.phi0 = new_phi0
self.phi_dot0 = new_phi_dot0
def set_motor_torque(self, new_motor_torque) -> None:
"""
sets the moment to the new value
"""
self.motor_torque = new_motor_torque
def flip_alpha(self) -> None:
"""
(Irrelevant for my question) changes the angle of attack's sign
:return:
"""
self.alpha = RADIAN135 if self.alpha == RADIAN45 else RADIAN45
def c_drag(self) -> float:
"""
calculates the drag coefficient based on the angle of attack
"""
return (C_D_MAX + C_D_0) / 2 - (C_D_MAX - C_D_0) / 2 * np.cos(2 * self.alpha)
def drag_torque(self, phi_dot):
"""
the drag moment
"""
return 0.5 * AIR_DENSITY * WING_AREA * self.c_drag() * GYRATION_RADIUS * phi_dot * np.abs(phi_dot)
def phi_derivatives(self, t, y):
"""
A function that defines the ODE that is to be solved: I * phi_ddot = tau_z - tau_drag.
We think of y as a vector y = [phi,phi_dot]. the ode solves dy/dt = f(y,t)
:return:
"""
phi, phi_dot = y[0], y[1]
dy_dt = [phi_dot, (self.motor_torque(t) - self.drag_torque(phi_dot)) / MoI]
return dy_dt
def solve_dynamics(self, phiarr, phidotarr, phiddotarr, angarr, torquearr):
"""
solves the ODE
:return:
"""
phi_0, phi_dot_0 = self.phi0, self.phi_dot0
start_t, end_t, delta_t = self.start_t, self.end_t, self.delta_t
phi_dot_zero_crossing_event.terminal = True
ang = []
times_between_zero_cross = []
sol_between_zero_cross = []
while start_t < end_t:
torquearr.append(self.motor_torque(start_t))
sol = solve_ivp(self.phi_derivatives, t_span=(start_t, end_t), y0=[phi_0,phi_dot_0],events=phi_dot_zero_crossing_event)
self.solution = sol.y
ang.append(self.alpha * np.ones(len(sol.t))) # set alpha for every t based on solution's size
times_between_zero_cross.append(sol.t)
sol_between_zero_cross.append(sol.y)
if sol.status == ZERO_CROSSING:
start_t = sol.t[-1] + delta_t
phi_0, phi_dot_0 = sol.y[0][-1], sol.y[1][-1] # last step is now initial value
self.flip_alpha()
else: # no zero crossing
break
time = np.concatenate(times_between_zero_cross)
phi, phi_dot = np.concatenate(sol_between_zero_cross, axis=1)
ang = np.concatenate(ang)
_, phi_ddot = self.phi_derivatives(time, [phi, phi_dot])
phiarr.append(phi)
phidotarr.append(phi_dot)
phiddotarr.append(phi_ddot)
angarr.append(ang)
(3) Finally the main function calls solve_dynamics sequentially for different values of actions and updates the time window and initial conditions:
phi_arr, phi_dot_arr, phi_ddot_arr, angarr, torquearr = [], [], [], [], []
phi0, phidot0 = 0, 0.01
start_t, end_t, delta_t = 0, 0.05, 0.001
sim = RobotSimulation()
for action in [1, -1, 1, -1, 1, -1]:
sim.set_motor_torque(lambda x: action)
sim.solve_dynamics(phi_arr, phi_dot_arr, phi_ddot_arr, angarr, torquearr)
phi0, phidot0 = sim.solution[0][-1], sim.solution[1][-1]
start_t = end_t
end_t += 0.05
sim.set_init_cond(phi0, phidot0)
sim.set_time(start_t, end_t)
phi_arr = np.concatenate(phi_arr)
phi_dot_arr = np.concatenate(phi_dot_arr)
phi_ddot_arr = np.concatenate(phi_ddot_arr)
angle_arr = np.concatenate(angarr)
Intuitively, as the actions I apply are just repeating +1 and -1, I cannot understand why the blue points in the figure (see below) act so different
Eventually, the problem was with how solve_ivp finds zero crossing points. It seems that the solution kept on finding the same crossings from + to - crossing every time (Thanks Lutz), so I've added a restriction using the .direction variable:
before entering the while loop: phi_dot_zero_crossing_event.direction = -np.sign(phi_dot_0). This makes sure we start by finding a crossing in the direction of the initial velocity.
when identifying an event in if sol.status == 1: I changed the sign of the direction with phi_dot_zero_crossing_event.direction *= -1
Related
I am trying to implement policy iteration from scratch. I have a 2D grid world environment named GridWorld that returns the successor state and reward from a given action, and it also has a function that returns the transition probability. Below is my code for policy iteration:
import matplotlib
matplotlib.use('Agg')
import random
import numpy as np
import matplotlib.pyplot as plt
import gridworld
from tqdm import tqdm
class PolicyIteration:
def __init__(self, env, gamma):
self.env = env
self.num_states = self.env.num_states
self.num_actions = self.env.num_actions
self.max_num_steps = self.env.max_num_steps
self.gamma = gamma #discount factor
self.values = np.zeros(self.num_states) #Initialize `values` as zeros
self.policy = np.random.randint(0, self.num_actions, self.num_states)
def one_policy_evaluation(self):
"""
Runs one iteration of policy evaluation and updates the value function.
:return: the maximum change in value function
"""
delta = 0
for s in range(self.num_states):
v = self.values[s]
a = self.policy[s]
(s_new, r, _) = self.env.step(a)
p = self.env.p(s_new, s, a)
""" update V(s)"""
self.values[s] = np.sum(p * (r + self.gamma * self.values[s_new]))
delta = max(delta, abs(v - self.values[s]))
return delta
def run_policy_evaluation(self, tol = 1e-3):
"""
Runs policy evaluation until convergence.
:param tol: the tolerance level for convergence
:return: the number of iterations of policy evaluation until convergence
"""
delta = self.one_policy_evaluation()
delta_history = [delta]
while delta > tol:
delta = self.one_policy_evaluation()
delta_history.append(delta)
return len(delta_history)
def run_policy_improvement(self):
update_policy_count = 0
for s in range(self.num_states):
temp = self.policy[s]
v_list = np.zeros(self.num_actions)
for a in range(self.num_actions):
(s_new, r, _) = self.env.step(a)
p = self.env.p(s_new, s, a)
v_list[a] = np.sum(p * (r + self.gamma * self.values[s_new]))
self.policy[s] = np.argmax(v_list)
if temp != self.policy[s]:
update_policy_count += 1
return update_policy_count
def train(self, tol=1e-3, max_iters=100, plot=True):
eval_count = self.run_policy_evaluation(tol)
eval_count_history = [eval_count]
policy_change = self.run_policy_improvement()
policy_change_history = [policy_change]
epoch = 0
val_history= []
for i in tqdm(range(max_iters)):
epoch += 1
new_eval_count = self.run_policy_evaluation(tol)
new_policy_change = self.run_policy_improvement()
eval_count_history.append(new_eval_count)
policy_change_history.append(new_policy_change)
val_history.append(np.mean(self.values))
if new_policy_change == 0:
break
print(f'# epoch: {len(policy_change_history)}')
print(f'eval count = {eval_count_history}')
print(f'policy change = {policy_change_history}')
if plot is True:
plt.figure(dpi=200)
plt.plot(val_history)
plt.tight_layout()
plt.savefig('policy_iteration.png')
plt.show()
def main():
env = gridworld.GridWorld(hard_version=False)
agent = PolicyIteration(env, gamma=0.95)
agent.train()
if __name__ == '__main__':
main()
However, based on the figure generated, the sequence of values is oscillating up and down and never converges. I followed the algorithm in the Sutton book step by step, and can't find any issues with my code yet:
Any help is greatly appreciated!
I am working in an undergraduate optics lab and we were given a few codes for data analysis. I need help understanding what two of them are doing exactly. To give some background on the experiment: light from a pulsed laser is injected into the back of a PTFE diffuser. Across from the diffuser is a photomultiplier tube that detects the optical signal. During the experiment, the diffuser is rotated and data is collected about the pulse waveform for every three degrees. I am just not sure what the code does to interpret this data, I hope that makes sense. Thank you in advance.
Here is the first code:
import numpy as np
DEFAULT_FMAX = 0.2
class Waveform(object):
def __init__(self, X, Y, fmax=None, peaknorm=False, areanorm=False):
self.X = np.copy(X)
self.Y = np.copy(Y)
if fmax is not None:
self.lowpass_filter(fmax)
if peaknorm:
self.normalise_peak()
if areanorm:
self.normalise_area()
def lowpass_filter(self, fmax):
# Fourier transform
freq_Y = np.fft.rfft(self.Y)
freq_X = np.fft.rfftfreq(self.Y.size, d=((max(self.X)-min(self.X))/self.Y.size))
# Apply max frequency cut
freq_Y[freq_X>fmax] = 0.0
# Inverse fourier transform
self.Y = np.fft.irfft(freq_Y)
def normalise_peak(self):
self.Y /= abs(np.min(self.Y))
def normalise_area(self):
self.Y /= np.sum(self.Y)
#property
def fft(self):
freq_Y = np.fft.rfft(self.Y)
freq_X = np.fft.rfftfreq(self.Y.size, d=((max(self.X)-min(self.X))/self.Y.size))
return freq_X, freq_Y
#property
def pedestal(self):
return np.mean(self.Y[0:100])
#property
def amplitude(self):
return abs(min(self.Y) - self.pedestal)
#property
def peak_index(self):
return np.argmin(self.Y)
#property
def peak_time(self):
return self.X[self.peak_index]
def _find_half_maxima_indices(self):
peak = self.peak_index
Y = np.copy(self.Y - self.pedestal)
Y += (self.amplitude / 2.0)
# find first half maximum
try:
first = np.argmax(Y[:peak] < 0)
except ValueError:
# can fail eg if peak == 0
first = peak
try:
second = peak + np.argmax(Y[peak:] > 0)
except ValueError:
# can fail eg if peak == last bin
second = peak
return first, second
#property
def fwhm(self):
return self.rise + self.fall
#property
def fall(self):
_, second = self._find_half_maxima_indices()
fall = self.X[second] - self.peak_time
return fall
#property
def rise(self):
first, _ = self._find_half_maxima_indices()
rise = self.peak_time - self.X[first]
return rise
#property
def integralindex_high(self):
pedestal = self.pedestal
#integrate falling edge
for ii in range(self.peak_index, self.Y.shape[0] - 1):
if (self.Y[ii] - pedestal) > 0.0:
break
return ii
#property
def integralindex_low(self):
pedestal = self.pedestal
#integrate falling edge
ii = 0
for ii in reversed(range(0, self.peak_index)):
if (self.Y[ii] - pedestal) > 0.0:
break
return ii
#property
def area(self):
area = 0.0
pedestal = self.pedestal
#integrate falling edge
for ii in range(self.peak_index, self.integralindex_high):
area += (self.Y[ii] - pedestal) * (self.X[ii + 1] - self.X[ii])
#integrate rising edge
for ii in reversed(range(self.integralindex_low, self.peak_index)):
area += (self.Y[ii] - pedestal) * (self.X[ii + 1] - self.X[ii])
return abs(area)
And the second:
import ROOT
import numpy as np
import datetime
from collections import namedtuple
from collections import defaultdict
from .waveform import Waveform
class ScanPoint:
def __init__(self, time_epoch, coord_angle, coord_y, lab_temp, lab_humid, dt, samples_PMT,
samples_PD, num_entry):
self.time_epoch = time_epoch
self.coord_angle = coord_angle
self.coord_y = coord_y
self.lab_temp = lab_temp
self.lab_humid = lab_humid
self.samples_PMT = np.array(list(samples_PMT), dtype=float)
self.samples_PD = np.array(list(samples_PD), dtype=float)
self.axis_time = np.arange(len(samples_PMT), dtype=float) * dt * 1.e9
self.num_entry = num_entry
def getWaveformPMT(self, fmax=None, peaknorm=False, areanorm=False):
return Waveform(self.axis_time, self.samples_PMT, fmax, peaknorm, areanorm)
def getWaveformPD(self, fmax=None, peaknorm=False, areanorm=False):
return Waveform(self.axis_time, self.samples_PD, fmax, peaknorm, areanorm)
def getDatetime(self):
return datetime.datetime.fromtimestamp(self.time_epoch)
# Tuple containing metadata and scanpoints for a full scan
DiffuserScan = namedtuple("DiffuserScan", ["ID_diffuser",
"ID_PMT",
"ID_PD",
"ID_lightsource",
"ID_experimentalist",
"notes",
"pulse_rate",
"pulse_N",
"scanpoints"])
def readscan(filename, treename):
file = ROOT.TFile.Open(filename, "READ")
tree = getattr(file, treename)
tree.GetEntry(0)
formatversion = (
int(tree.version_major),
int(tree.version_minor),
int(tree.version_patch),
)
# Check that input file format is implemented
if formatversion[:2] == (1, 0):
diffuserscan = DiffuserScan(
str(tree.ID_diffuser),
str(tree.ID_PMT),
str(tree.ID_PD),
str(tree.ID_lightsource),
str(tree.ID_experimentalist),
str(tree.notes),
float(tree.pulse_rate),
int(tree.pulse_N),
[],
)
for i, entry in enumerate(tree):
scanpoint = ScanPoint(entry.time_epoch,
entry.coord_angle,
entry.coord_y,
entry.lab_temp,
entry.lab_humid,
entry.dt,
entry.waveform_PMT,
entry.waveform_PD,
i)
diffuserscan.scanpoints.append(scanpoint)
else:
raise NotImplementedError("Input format version not implemented")
return diffuserscan
def combinefiles(scans):
combined_scan = scans[0]
compare_vars = ["ID_PMT", "ID_PD", "ID_lightsource", "pulse_rate", "pulse_N"]
for scan in scans[1:]:
combined_scan.scanpoints.extend(scan.scanpoints)
if scan.ID_diffuser != combined_scan.ID_diffuser:
combined_scan.ID_diffuser = "multi"
for var in compare_vars:
if getattr(scan, var) != getattr(combined_scan, var):
print('WARNING: Variable "{}" does not match between files! Should we be comparing
them?'.format(var))
return combined_scan
I need to simulate a penalty where the ball shoots in every direction. For this I use solve_ivp and I terminate the integration when the ball crosses the backline. When this happens I want to use the values found when the integration stops to see if the ball at that point is within the dimensions of the goal (and count it as a goal). However, solution.y does not give the required precision that I want. The desired values are within solution.y_events, however, I can't seem to be able to get receive them. I also can't find information about this online. My code:
import numpy as np
from scipy import integrate
from scipy import constants
import matplotlib.pyplot as plt
#### Constants
# Number of simulations
number_of_penalty_shots = 10
# Angle of the shots
theta = np.random.uniform(0, 2.0*np.pi, number_of_penalty_shots)
phi = np.random.uniform(0, np.pi, number_of_penalty_shots)
# Velocity of the ball
v_magnitude = 80
### Starting Position Ball (defined as the penalty stip)
pos_x = 0.0
pos_y = 0.0
pos_z = 0.0
in_position = np.array([pos_x, pos_y, pos_z]) # Inital position in m
def homo_magnetic_field(t, vector):
vx = vector[3] # dx/dt = vx
vy = vector[4] # dy/dt = vy
vz = vector[5] # dz/dt = vz
# ax = -0.05*vector[3] # dvx/dt = ax
# ay = -0.05*vector[4] # dvy/dy = ay
# az = -0.05*vector[5] - constants.g #dvz/dt = az
ax = 0
ay = 0
az = 0
dvectordt = (vx,vy,vz,ax,ay,az)
return(dvectordt)
def goal(t, vector):
return vector[1] - 11
def own_goal(t,vector):
return vector[1] + 100
def ground(t,vector):
return vector[2]
goal.terminal=True
own_goal.terminal=True
def is_it_goal(vector):
if vector.status == 1:
if (vector.y[1][len(vector.y[1])-1] > 0) and (-3.36 < vector.y[0][len(vector.y[1])-1] < 3.36) and (vector.y[2][len(vector.y[1])-1] < 2.44):
print("GOAAAAAAAAAAAAL!")
elif (vector.y[1][len(vector.y[1])-1] < 0) and (-3.36 < vector.y[0][len(vector.y[1])-1] < 3.36) and (vector.y[2][len(vector.y[1])-1] < 2.44):
print("Own goal?! Why?")
else: print("Awwwwh")
else: print("Not even close, lol")
# Integrating
time_range = (0.0, 10**2)
for i in range(number_of_penalty_shots):
v_x = v_magnitude*np.sin(phi[i])*np.cos(theta[i])
v_y = v_magnitude*np.sin(phi[i])*np.sin(theta[i])
v_z = v_magnitude*np.cos(phi[i])
in_velocity = np.array([v_x, v_y, v_z])
initial_point = np.array([in_position, in_velocity])
start_point = initial_point.reshape(6,)
solution = integrate.solve_ivp(homo_magnetic_field , time_range, start_point,events=(goal, own_goal))
is_it_goal(solution)
Here I want to change vector.y[1][len(vector.y[1])-1] into something like vector.y_events...
Excuse me for the length of the title please but this is a pretty specific question. I'm currently simulating a launch of a rocket to mars in the 2022 launch window and I noticed that my rocket is a far distance away from Mars, even though it's traveling in the right direction. After simplifying my code to narrow down the problem, I simply plotted the orbits of the Earth and Mars (Using data from NASA's SPICE library) and propagated the position and velocity given to me by the lambert solver I implemented (Universal variables) to plot the final orbit of the rocket.
I'm only letting the Sun's gravity effect the rocket, not the Earth or Mars, to minimize my problem space. Yet even though I've simplified my problem so far, the intersection between Mars' and my rocket's orbits happens well before the time of flight has been simulated all the way, and the minimum distance between the two bodies is more than a million kilometers at all times.
That being said, something must be wrong but I cannot find the problem. I've made sure the lambert solver code I copied is correct by comparing it to Dario Izzo's method and both gave the same results. Furthermore, I've also checked that my orbit propagator works by propagating Mars' and the Earth's orbits and comparing those ellipses to the data from SPICE.
In conclusion, I assume this must be a stupid little mistake I made somewhere, but cannot find because I lack experience in this field. Thank you for any help! :)
This is the JupyterLab notebook I used:
import numpy as np
import matplotlib.pyplot as plt
import json
import math
import spiceypy as spice
# Physics
G = 6.6741e-11
class Entity:
def __init__(self, x, v, m, do_gravity):
self.x = x
self.v = v
self.a = np.array([0,0,0])
self.m = m
self.do_gravity = do_gravity
def apply_force(self, f):
self.a = np.add(self.a, f / self.m);
def step(self, dt):
self.v = np.add(self.v, self.a * dt)
self.x = np.add(self.x, self.v * dt)
self.a = np.array([0,0,0])
class StaticEntity(Entity):
def __init__(self, body, t, do_gravity):
super().__init__(self.get_state(body, t)[:3], self.get_state(body, t)[3:], self.get_mass(body), do_gravity)
self.body = body
self.t = t
def step(self, dt):
self.t += dt
state = self.get_state(self.body, self.t)
self.x = state[:3]
self.v = state[3:]
#classmethod
def get_state(self, body, t):
[state, lt] = spice.spkezr(body, t, "J2000", "NONE", "SSB")
return state * 1000
#classmethod
def get_mass(self, body):
[dim, gm] = spice.bodvrd(body, "GM", 1)
return gm * 1e9 / G
def get_position(self, t):
return self.get_state(self.body, t)[:3]
def get_velocity(self, t):
return self.get_state(self.body, t)[3:]
class Propagator:
def __init__(self, entities):
self.entities = entities
def step(self, dt):
for e1 in self.entities:
for e2 in self.entities:
if (e1 is e2) or (not e1.do_gravity) or isinstance(e2, StaticEntity):
continue
diff = np.subtract(e1.x, e2.x)
fg = G * e1.m * e2.m / np.dot(diff, diff)
force = fg * diff / np.linalg.norm(diff)
e2.apply_force(force)
for entity in self.entities:
entity.step(dt)
# Lambert solver
def C2(psi):
if psi >= 0.0:
sp = math.sqrt(psi)
return (1 - math.cos(sp)) / psi
else:
sp = math.sqrt(-psi)
return (1 - math.cosh(sp)) / psi
def C3(psi):
if psi >= 0.0:
sp = math.sqrt(psi)
return (sp - math.sin(sp)) / (psi * sp)
else:
sp = math.sqrt(-psi)
return (sp - math.sinh(sp)) / (psi * sp)
def lambert_solve(r1, r2, tof, mu, iterations, tolerance):
R1 = np.linalg.norm(r1)
R2 = np.linalg.norm(r2)
cos_a = np.dot(r1, r2) / (R1 * R2)
A = math.sqrt(R1 * R2 * (1.0 + cos_a))
sqrt_mu = math.sqrt(mu)
if A == 0.0:
return None
psi = 0.0
psi_lower = -4.0 * math.pi * math.pi
psi_upper = 4.0 * math.pi * math.pi
c2 = 1.0 / 2.0
c3 = 1.0 / 6.0
for i in range(iterations):
B = R1 + R2 + A * (psi * c3 - 1.0) / math.sqrt(c2)
if A > 0.0 and B < 0.0:
psi_lower += math.pi
B = -B
chi = math.sqrt(B / c2)
chi3 = chi * chi * chi
tof_new = (chi3 * c3 + A * math.sqrt(B)) / sqrt_mu
if math.fabs(tof_new - tof) < tolerance:
f = 1.0 - B / R1
g = A * math.sqrt(B / mu)
g_dot = 1.0 - B / R2
v1 = (r2 - f * r1) / g
v2 = (g_dot * r2 - r1) / g
return (v1, v2)
if tof_new <= tof:
psi_lower = psi
else:
psi_upper = psi
psi = (psi_lower + psi_upper) * 0.5
c2 = C2(psi)
c3 = C3(psi)
return None
# Set up solar system
spice.furnsh('solar_system.tm')
inject_time = spice.str2et('2022 Sep 28 00:00:00')
exit_time = spice.str2et('2023 Jun 1 00:00:00')
sun = StaticEntity("Sun", inject_time, True)
earth = StaticEntity("Earth", inject_time, False)
mars = StaticEntity("Mars Barycenter", inject_time, False)
(v1, v2) = lambert_solve(earth.get_position(inject_time), mars.get_position(exit_time), exit_time - inject_time, G * sun.m, 1000, 1e-4)
rocket = Entity(earth.x, v1, 100000, False)
propagator = Propagator([sun, earth, mars, rocket])
# Generate data
earth_pos = [[], [], []]
mars_pos = [[], [], []]
rocket_pos = [[], [], []]
t = inject_time
dt = 3600 # seconds
while t < exit_time:
propagator.step(dt)
earth_pos[0].append(earth.x[0])
earth_pos[1].append(earth.x[1])
earth_pos[2].append(earth.x[2])
mars_pos[0].append(mars.x[0])
mars_pos[1].append(mars.x[1])
mars_pos[2].append(mars.x[2])
rocket_pos[0].append(rocket.x[0])
rocket_pos[1].append(rocket.x[1])
rocket_pos[2].append(rocket.x[2])
t += dt
# Plot data
plt.figure()
plt.title('Transfer orbit')
plt.xlabel('x-axis')
plt.ylabel('y-axis')
plt.plot(earth_pos[0], earth_pos[1], color='blue')
plt.plot(mars_pos[0], mars_pos[1], color='orange')
plt.plot(rocket_pos[0], rocket_pos[1], color='green')
EDIT:
I recently remodeled my code so that it uses orbit class to represent the entities. This actually gave me acceptable results, even though the code is, in theory, not doing anything differently (as far as I can tell; obviously something must be different)
def norm(a):
return np.dot(a, a)**0.5
def fabs(a):
return -a if a < 0 else a
def newton_raphson(f, f_dot, x0, n):
res = x0
for i in range(n):
res = res - f(res) / f_dot(res)
return res
def get_ephemeris(body, time):
state, _ = sp.spkezr(body, time, "J2000", "NONE", "SSB")
return np.array(state[:3]) * ap.units.km, np.array(state[3:]) * ap.units.km / ap.units.s
def get_mu(body):
_, mu = sp.bodvrd(body, "GM", 1)
return mu * ap.units.km**3 / ap.units.s**2
class orbit:
def __init__(self, position, velocity, mu):
self.position = position
self.velocity = velocity
self.mu = mu
#staticmethod
def from_body(name, center, time):
return static_orbit(name, center, time)
def get_ephemerides(self, t, dt):
time = 0
positions = []
velocities = []
#M = self.M
position = self.position
velocity = self.velocity
delta_t = dt * ap.units.s
t1 = t * ap.units.s
while time < t1:
g = self.mu / np.dot(position, position)
g_vec = g * -position / norm(position)
velocity = np.add(velocity, g_vec * delta_t)
position = np.add(position, velocity * delta_t)
positions.append(position)
velocities.append(velocity)
time = time + delta_t
return positions, velocities
class static_orbit(orbit):
def __init__(self, name, center, time):
p, v = get_ephemeris(name, time)
pc, vc = get_ephemeris(center, time)
super().__init__(p - pc, v - vc, get_mu(center))
self.name = name
self.center = center
self.time = time
def get_ephemerides(self, t, dt):
time = 0
positions = []
velocities = []
while time < t:
p, v = get_ephemeris(self.name, time + self.time)
pc, vc = get_ephemeris(self.center, time + self.time)
positions.append(p - pc)
velocities.append(v - vc)
time += dt
return positions, velocities
sp.furnsh('solar_system.tm')
t1 = sp.str2et('2022 Sep 28 00:00:00')
t2 = sp.str2et('2023 Jun 10 00:00:00')
eo = orbit.from_body("Earth", "Sun", t1)
mo = orbit.from_body("Mars Barycenter", "Sun", t1)
earth_x, earth_v = eo.get_ephemerides(t2 - t1, 3600)
mars_x, mars_v = mo.get_ephemerides(t2 - t1, 3600)
l = lambert(earth_x[0], mars_x[-1], t2 - t1, get_mu("Sun"), 1000, 1e-6)
ro = orbit(earth_x[0], l.v1, get_mu("Sun"))
rocket_x, rocket_v = ro.get_ephemerides(t2 - t1, 60)
earth_x = np.array(earth_x)
mars_x = np.array(mars_x)
rocket_x = np.array(rocket_x)
fig = go.Figure()
fig.add_trace(go.Scatter3d(x=earth_x[:,0], y=earth_x[:,1], z=earth_x[:,2], marker_size=1, marker_color='blue'))
fig.add_trace(go.Scatter3d(x=mars_x[:,0], y=mars_x[:,1], z=mars_x[:,2], marker_size=1, marker_color='orange'))
fig.add_trace(go.Scatter3d(x=rocket_x[:,0], y=rocket_x[:,1], z=rocket_x[:,2], marker_size=1, marker_color='green'))
fig.show()
This method generated following plot:
Also, before this is mentioned again, I have varied my integration step size and lambert solver tolerance to no avail, the result was qualitatively different.
So, I managed to figure out what the problem was after much head-scratching. I was simply not taking into account that the Sun is not located at (0,0,0) in my coordinate system. I thought this was negligible, but that is what made the difference. In the end, I simply passed the difference between the Earth and Mars's and the Sun's position vectors and passed those into the Lambert solver. This finally gave me the desired results.
The reason that the error ended up being so "small" (It didn't seem like an obvious bug at first) was because my coordinates are centered at the solar system barycenter which is a few million kilometers away from the Sun, as one would expect.
Thanks for the comments!
My code dies after about 140+ iterations, and I don't know why. I guess memory leak is a possibility, but I couldn't find it. I also found out that changing some arithmetic constants can prolong the time until the crash.
I have a genetic algorithm that tries to find best (i.e. minimal steps) route from point A (src) to point B (dst).
I create a list of random chromosomes, where each chromosome has:
src + dst [always the same]
list of directions (random)
I then run the algorithm:
find best route and draw it (for visualization purposes)
Given a probability P - replace the chromosomes with cross-overs (i.e. pick 2, and take the "end" of one's directions, and replace the "end" of the second's)
Given probability Q - mutate (replace the next direction with a random direction)
This all goes well, and most of the times I do find a route (usually not the ideal one), but sometimes, when it searches for a long time (say, about 140+ iterations) it just crushes. No warning. No error.
How can I prevent that (a simple iteration limit can work, but I do want the algorithm to run for a long time [~2000+ iterations])?
I think the relevant parts of the code are:
update function inside GUI class
which calls to cross_over
When playing with the update_fitness() score values (changing score -= (weight+1)*2000*(shift_x + shift_y) to score -= (weight+1)*2*(shift_x + shift_y) it runs for a longer time. Could be some kind of an arithmetic overflow?
import tkinter as tk
from enum import Enum
from random import randint, sample
from copy import deepcopy
from time import sleep
from itertools import product
debug_flag = False
class Direction(Enum):
Up = 0
Down = 1
Left = 2
Right = 3
def __str__(self):
return str(self.name)
def __repr__(self):
return str(self.name)[0]
# A chromosome is a list of directions that should lead the way from src to dst.
# Each step in the chromosome is a direction (up, down, right ,left)
# The chromosome also keeps track of its route
class Chromosome:
def __init__(self, src = None, dst = None, length = 10, directions = None):
self.MAX_SCORE = 1000000
self.route = [src]
if not directions:
self.directions = [Direction(randint(0,3)) for i in range(length)]
else:
self.directions = directions
self.src = src
self.dst = dst
self.fitness = self.MAX_SCORE
def __str__(self):
return str(self.fitness)
def __repr__(self):
return self.__str__()
def set_src(self, pixel):
self.src = pixel
def set_dst(self, pixel):
self.dst = pixel
def set_directions(self, ls):
self.directions = ls
def update_fitness(self):
# Higher score - a better fitness
score = self.MAX_SCORE - len(self.route)
score += 4000*(len(set(self.route)) - len(self.route)) # penalize returning to the same cell
score += (self.dst in self.route) * 500 # bonus routes that get to dst
for weight,cell in enumerate(self.route):
shift_x = abs(cell[0] - self.dst[0])
shift_y = abs(cell[1] - self.dst[1])
score -= (weight+1)*2000*(shift_x + shift_y) # penalize any wrong turn
self.fitness = max(score, 0)
def update(self, mutate_chance = 0.9):
# mutate #
self.mutate(chance = mutate_chance)
# move according to direction
last_cell = self.route[-1]
try:
direction = self.directions[len(self.route) - 1]
except IndexError:
print('No more directions. Halting')
return
if direction == Direction.Down:
x_shift, y_shift = 0, 1
elif direction == Direction.Up:
x_shift, y_shift = 0, -1
elif direction == Direction.Left:
x_shift, y_shift = -1, 0
elif direction == Direction.Right:
x_shift, y_shift = 1, 0
new_cell = last_cell[0] + x_shift, last_cell[1] + y_shift
self.route.append(new_cell)
self.update_fitness()
def cross_over(p1, p2, loc = None):
# find the cross_over point
if not loc:
loc = randint(0,len(p1.directions))
# choose one of the parents randomly
x = randint(0,1)
src_parent = (p1, p2)[x]
dst_parent = (p1, p2)[1 - x]
son = deepcopy(src_parent)
son.directions[loc:] = deepcopy(dst_parent.directions[loc:])
return son
def mutate(self, chance = 1):
if 100*chance > randint(0,99):
self.directions[len(self.route) - 1] = Direction(randint(0,3))
class GUI:
def __init__(self, rows = 10, cols = 10, iteration_timer = 100, chromosomes = [], cross_over_chance = 0.5, mutation_chance = 0.3, MAX_ITER = 100):
self.rows = rows
self.cols = cols
self.canv_w = 800
self.canv_h = 800
self.cell_w = self.canv_w // cols
self.cell_h = self.canv_h // rows
self.master = tk.Tk()
self.canvas = tk.Canvas(self.master, width = self.canv_w, height = self.canv_h)
self.canvas.pack()
self.rect_dict = {}
self.iteration_timer = iteration_timer
self.iterations = 0
self.MAX_ITER = MAX_ITER
self.chromosome_list = chromosomes
self.src = chromosomes[0].src # all chromosomes share src + dst
self.dst = chromosomes[0].dst
self.prev_best_route = []
self.cross_over_chance = cross_over_chance
self.mutation_chance = mutation_chance
self.no_obstacles = True
# init grid #
for r in range(rows):
for c in range(cols):
self.rect_dict[(r, c)] = self.canvas.create_rectangle(r *self.cell_h, c *self.cell_w,
(1+r)*self.cell_h, (1+c)*self.cell_w,
fill="gray")
# init grid #
# draw src + dst #
self.color_src_dst()
# draw src + dst #
# after + mainloop #
self.master.after(iteration_timer, self.start_gui)
tk.mainloop()
# after + mainloop #
def start_gui(self):
self.start_msg = self.canvas.create_text(self.canv_w // 2,3*self.canv_h // 4, fill = "black", font = "Times 25 bold underline",
text="Starting new computation.\nPopulation size = %d\nCross-over chance = %.2f\nMutation chance = %.2f" %
(len(self.chromosome_list), self.cross_over_chance, self.mutation_chance))
self.master.after(2000, self.update)
def end_gui(self, msg="Bye Bye!"):
self.master.wm_attributes('-alpha', 0.9) # transparency
self.canvas.create_text(self.canv_w // 2,3*self.canv_h // 4, fill = "black", font = "Times 25 bold underline", text=msg)
cell_ls = []
for idx,cell in enumerate(self.prev_best_route):
if cell in cell_ls:
continue
cell_ls.append(cell)
self.canvas.create_text(cell[0]*self.cell_w, cell[1]*self.cell_h, fill = "purple", font = "Times 16 bold italic", text=str(idx+1))
self.master.after(3000, self.master.destroy)
def color_src_dst(self):
r_src = self.rect_dict[self.src]
r_dst = self.rect_dict[self.dst]
c_src = 'blue'
c_dst = 'red'
self.canvas.itemconfig(r_src, fill=c_src)
self.canvas.itemconfig(r_dst, fill=c_dst)
def color_route(self, route, color):
for cell in route:
try:
self.canvas.itemconfig(self.rect_dict[cell], fill=color)
except KeyError:
# out of bounds -> ignore
continue
# keep the src + dst
self.color_src_dst()
# keep the src + dst
def compute_shortest_route(self):
if self.no_obstacles:
return (1 +
abs(self.chromosome_list[0].dst[0] - self.chromosome_list[0].src[0]) +
abs(self.chromosome_list[0].dst[1] - self.chromosome_list[0].src[1]))
else:
return 0
def create_weighted_chromosome_list(self):
ls = []
for ch in self.chromosome_list:
tmp = [ch] * (ch.fitness // 200000)
ls.extend(tmp)
return ls
def cross_over(self):
new_chromosome_ls = []
weighted_ls = self.create_weighted_chromosome_list()
while len(new_chromosome_ls) < len(self.chromosome_list):
try:
p1, p2 = sample(weighted_ls, 2)
son = Chromosome.cross_over(p1, p2)
if son in new_chromosome_ls:
continue
else:
new_chromosome_ls.append(son)
except ValueError:
continue
return new_chromosome_ls
def end_successfully(self):
self.end_gui(msg="Got to destination in %d iterations!\nBest route length = %d" % (len(self.prev_best_route), self.compute_shortest_route()))
def update(self):
# first time #
self.canvas.delete(self.start_msg)
# first time #
# end #
if self.iterations >= self.MAX_ITER:
self.end_gui()
return
# end #
# clean the previously best chromosome route #
self.color_route(self.prev_best_route[1:], 'gray')
# clean the previously best chromosome route #
# cross over #
if 100*self.cross_over_chance > randint(0,99):
self.chromosome_list = self.cross_over()
# cross over #
# update (includes mutations) all chromosomes #
for ch in self.chromosome_list:
ch.update(mutate_chance=self.mutation_chance)
# update (includes mutations) all chromosomes #
# show all chromsome fitness values #
if debug_flag:
fit_ls = [ch.fitness for ch in self.chromosome_list]
print(self.iterations, sum(fit_ls) / len(fit_ls), fit_ls)
# show all chromsome fitness values #
# find and display best chromosome #
best_ch = max(self.chromosome_list, key=lambda ch : ch.fitness)
self.prev_best_route = deepcopy(best_ch.route)
self.color_route(self.prev_best_route[1:], 'gold')
# find and display best chromosome #
# check if got to dst #
if best_ch.dst == best_ch.route[-1]:
self.end_successfully()
return
# check if got to dst #
# after + update iterations #
self.master.after(self.iteration_timer, self.update)
self.iterations += 1
# after + update iterations #
def main():
iter_timer, ITER = 10, 350
r,c = 20,20
s,d = (13,11), (7,8)
population_size = [80,160]
cross_over_chance = [0.2,0.4,0.5]
for pop_size, CO_chance in product(population_size, cross_over_chance):
M_chance = 0.7 - CO_chance
ch_ls = [Chromosome(src=s, dst=d, directions=[Direction(randint(0,3)) for i in range(ITER)]) for i in range(pop_size)]
g = GUI(rows=r, cols=c, chromosomes = ch_ls, iteration_timer=iter_timer,
cross_over_chance=CO_chance, mutation_chance=M_chance, MAX_ITER=ITER-1)
del(ch_ls)
del(g)
if __name__ == "__main__":
main()
I do not know if you know the Python Profiling tool of Visual Studio, but it is quite useful in cases as yours (though I usually program with editors, like VS Code).
I have run your program and, as you said, it sometimes crashes. I have analyzed the code with the profiling tool and it seems that the problem is the function cross_over, specifically the random function:
I would strongly suggest reviewing your cross_over and mutation functions. The random function should not be called so many times (2 millions).
I have previously programmed Genetic Algorithms and, to me, it seems that your program is falling into a local minimum. What is suggested in these cases is playing with the percentage of mutation. Try to increase it a little bit so that you could get out of the local minimum.