def get_avoid_dir(self): direction = np.zeros(2) # force-field away from incoming attacks gos = utils.get_game_objects() for key in gos: go = gos[key] # different team if go.team_id != self.parent.team_id: # calc dist to parent com_vector = go.com_vector(self.parent) dist_to_dmg = go.dist_to_other(self.parent) if dist_to_dmg > self.dist_tol: continue else: # normalize, inversely proportional mult = np.clip( math_utils.zero_protection_divide( self.dist_multiplier, dist_to_dmg), -1.0, 1.0) direction += mult * math_utils.normalize(com_vector) # print("direction: {}".format(direction)) out = math_utils.normalize(direction) return out
def rate2iGluSnFR(trace, rec_time=None, dt=None, T=0.5, n_drop=0, kind='single'): ''' Convolve rate trace with iGluSnFR kernel. ''' trace = np.array(trace) dt = time_utils.get_and_test_dt(dt=dt, t_array=rec_time) # iGluSnFR kernel. if kind == 'double': kernel = get_iGluSnFR_kernel_double_exp(duration=T, dt=dt) elif kind == 'single': kernel = get_iGluSnFR_kernel_exp_decay(duration=T, dt=dt) else: raise NotImplementedError # Convolve. iGluSnFR = convolve(trace, kernel, mode='full')[0:trace.size] # Drop values at the beginning were convolution isn't meaningful. if n_drop > 0: iGluSnFR[0:n_drop] = iGluSnFR[n_drop] # Normalize. iGluSnFR = math_utils.normalize(iGluSnFR) return iGluSnFR
def __plot_data(rec_data, rec_time_plot, rec_stim, original_model_output, rate_name, Vm_name): fig, axs = plt.subplots(3, 1, figsize=(12, 4), sharex=True) axs[0].plot(rec_time_plot, math_utils.normalize(rec_stim), label='This model') axs[0].plot(rec_time_plot, interpolation_utils.in_ex_polate( x_old=original_model_output['Stimulus']['Time'], y_old=original_model_output['Stimulus']['Stim'], x_new=rec_time_plot), '--', label='Target') rate_data = rec_data[rate_name] if rate_data.values.ndim > 1: rate_data = rate_data.mean(axis=1) axs[1].plot(rec_time_plot, rate_data) axs[1].plot(original_model_output['Time'], original_model_output['rate'], '--') Vm_data = rec_data[Vm_name] if Vm_data.values.ndim > 1: Vm_data = Vm_data.mean(axis=1) axs[2].plot(rec_time_plot, Vm_data) axs[2].plot(original_model_output['Time'], original_model_output['Vm'], '--') axs[0].legend() plt.tight_layout() plt.show()
def estimate_earth_direction(self, q_actual, r, t, delta_t): """Estimates the direction vector to the Earth Args: q_actual (numpy ndarray): the actual quaternion representing the attitude (from the inertial to body frame) of the spacecraft r (numpy ndarray): the inertial position of the spacecraft (m) t (float): the current simulation time in seconds delta_t (float): the time between user-defined integrator steps (not the internal/adaptive integrator steps) in seconds Returns: numpy ndarray: the direction vector to the Earth as measured by the sensor in body coordinates """ d_earth_inertial = compute_earth_direction(r) T_actual = quaternion_to_dcm(q_actual) noise = self.get_noise(t, delta_t) theta = np.linalg.norm(noise) e = normalize(noise) e_x = skew_symmetric(e) T_err = (np.diag([1, 1, 1]) - 2 * np.sin(theta) * e_x + (1 - np.cos(theta)) * np.matmul(e_x, e_x)) d_earth_body = np.matmul(T_err, np.matmul(T_actual, d_earth_inertial)) return d_earth_body
def OrthonormalBasis_HughesMoller(n): if np.abs(n[0]) > np.abs(n[2]): u = np.array([-n[1], n[0], 0.0]) else: u = np.array([0.0, -n[2], n[1]]) b2 = normalize(u) b1 = np.cross(b2, n) return np.array([b1, b2, n])
def OrthonormalBasis_HughesMoller(n): if np.abs(n[0]) > np.abs(n[2]): u = np.array([-n[1], n[0], 0.0], dtype=np.float32) else: u = np.array([0.0, -n[2], n[1]], dtype=np.float32) b2 = normalize(u) b1 = np.cross(b2, n) return (n, b1, b2)
def get_dir_to_move(self): """ from game-objects handle, get CG of all attacks """ direction, dist = super().get_dir_to_move() direction += self.get_avoid_dir() unit_direction = math_utils.normalize(direction) return unit_direction, dist
def find_peaks( trace, prom=None, prom_pos=None, prom_neg=None, c='k', plot=False, verbose=True, height=None, height_pos=None, height_neg=None, xlims=[], time=None, label='unknown', ): # Get prominence of peaks. prom_pos = prom_pos or prom prom_neg = prom_neg or prom height_pos = height_pos or height height_neg = height_neg or height trace = np.asarray(trace) time = np.asarray(time) # Compute. peak_indices_pos = f_peaks(trace, prominence=prom_pos, height=height_pos)[0] peak_indices_neg = f_peaks(-trace, prominence=prom_neg, height=height_neg)[0] peak_indices = np.append(peak_indices_pos, peak_indices_neg) if verbose: print('Found ' + str(len(peak_indices_pos)) + ' positive peaks for ' + label) print('Found ' + str(len(peak_indices_neg)) + ' negative peaks for ' + label) # Plot? if plot: plt.figure(figsize=(12, len(xlims) * 1.5)) for idx, xlim in enumerate(xlims): plt.subplot(len(xlims), 1, idx + 1) if idx == 0: plt.title(label) plt.xlim(xlim) plt.plot(time, math_utils.normalize(trace), label='rate', c=c) for peak_index in peak_indices_pos: plt.axvline(time[peak_index], c=c, alpha=0.5) for peak_index in peak_indices_neg: plt.axvline(time[peak_index], c=c, linestyle='--', alpha=0.5) plt.tight_layout() plt.show() return peak_indices, peak_indices_pos, peak_indices_neg
def compute_earth_direction(r): """Estimates the direction vector to the Earth Args: r (numpy ndarray): the inertial position of the spacecraft (m) Returns: numpy ndarray: the direction vector to the Earth as measured by the sensor in inertial coordinates """ return normalize(-r)
def get_action(self): """ get direction from heading and project velocity in that direction """ pivot_pt = self.parent.go_position( ) # recall, the parent for the AI is the DamageObj pos = self.parent.get_center() orient = m2d.Orientation(np.pi / 2.0) # 90 deg turn vec = (orient * m2d.Vector(pos - pivot_pt)).array unit = math_utils.normalize(vec) dv = self.parent.max_velocity * unit out = {'dv': dv} cbs = [] return out, cbs
def init_component(self, **kwargs): self.target_pos = kwargs.get("target_pos") self.dir = math_utils.normalize( (self.target_pos[0] - self.game_object.pos[0], self.target_pos[1] - self.game_object.pos[1])) # print("dir: " + str(self.dir)) # print("sqr mag: " + str(math_utils.sqr_magnitude((0, 0), self.dir))) angle = -math.atan2(self.dir[1], self.dir[0]) * 57.2957795 + 180 self.game_object.set_rotation(angle) self.speed = kwargs.get("speed") self.damages = kwargs.get("damages") # tests """
def derivatives_func(t, x, satellite, nominal_state_func, perturbations_func, position_velocity_func, delta_t): """Computes the derivative of the spacecraft state Args: t (float): the time (in seconds) x (numpy ndarray): the state (10x1) where the elements are: [0, 1, 2, 3]: the quaternion describing the spacecraft attitude [4, 5, 6]: the angular velocity of the spacecraft [7, 8, 9]: the angular velocities of the reaction wheels satellite (Spacecraft): the Spacecraft object that represents the satellite being modeled nominal_state_func (function): the function that should compute the nominal attitude (in DCM form) and angular velocity; its header must be (t) perturbations_func (function): the function that should compute the perturbation torques (N * m); its header must be (satellite) position_velocity_func (function): the function that should compute the position and velocity; its header must be (t) delta_t (float): the time between user-defined integrator steps (not the internal/adaptive integrator steps) in seconds Returns: numpy ndarray: the derivative of the state (10x1) with respect to time """ r, v = position_velocity_func(t) satellite.q = normalize(x[0:4]) satellite.w = x[4:7] satellite.r = r satellite.v = v # only set if the satellite has actuators try: satellite.actuators.w_rxwls = x[7:10] except AttributeError: pass M_applied, w_dot_rxwls, _ = simulate_estimation_and_control( t, satellite, nominal_state_func, delta_t) # calculate the perturbing torques on the satellite M_perturb = perturbations_func(satellite) dx = np.empty((10, )) dx[0:4] = quaternion_derivative(satellite.q, satellite.w) dx[4:7] = angular_velocity_derivative(satellite.J, satellite.w, [M_applied, M_perturb]) dx[7:10] = w_dot_rxwls return dx
def CorrectPointToRobotEnvelope2D_2J(self, links, target, robot_offset=np.array([0, 0, 0])): """ Returns nearest point to 'target' point, robot can reach """ l1_length = np.linalg.norm(links[:, 0]) l2_length = np.linalg.norm(links[:, 1]) target_length = np.linalg.norm(target - robot_offset) inner_radius = np.abs(l1_length - l2_length) outer_radius = l1_length + l2_length if target_length <= outer_radius and target_length >= inner_radius: return target dir_vec = normalize(target - robot_offset) if target_length > outer_radius: return robot_offset + outer_radius * dir_vec elif target_length < inner_radius: return robot_offset + inner_radius * dir_vec else: raise Exception("Shouldn't happen")
def simulate_adcs(satellite, nominal_state_func, perturbations_func, position_velocity_func, start_time=0, delta_t=1, stop_time=6000, verbose=False): """Simulates an attitude determination and control system over a period of time Args: satellite (Spacecraft): the Spacecraft object that represents the satellite being modeled nominal_state_func (function): the function that should compute the nominal attitude (in DCM form) and angular velocity; its header must be (t) perturbations_func (function): the function that should compute the perturbation torques (N * m); its header must be (satellite) position_velocity_func (function): the function that should compute the position and velocity; its header must be (t) start_time (float, optional): Defaults to 0. The start time of the simulation in seconds delta_t (float, optional): Defaults to 1. The time between user-defined integrator steps (not the internal/adaptive integrator steps) in seconds stop_time (float, optional): Defaults to 6000. The end time of the simulation in seconds verbose (bool, optional). Defaults to False. Whether or not to print integrator output to the console while running. Returns: dict: a dictionary of simulation results. Each value is an NxM numpy ndarray where N is the number of time steps taken and M is the size of the data being represented at that time (M=1 for time, 3 for angular velocity, 4 for quaternion, etc.) Contains: - times (numpy ndarray): the times of all associated data - q_actual (numpy ndarray): actual quaternion - w_actual (numpy ndarray): actual angular velocity - w_rxwls (numpy ndarray): angular velocity of the reaction wheels - DCM_estimated (numpy ndarray): estimated DCM - w_estimated (numpy ndarray): estimated angular velocity - DCM_desired (numpy ndarray): desired DCM - w_desired (numpy ndarray): desired angular velocity - attitude_err (numpy ndarray): attitude error - attitude_rate_err (numpy ndarray): attitude rate error - M_ctrl (numpy ndarray): control torque - M_applied (numpy ndarray): applied control torque - w_dot_rxwls (numpy ndarray): angular acceleration of reaction wheels - M_perturb (numpy ndarray): sum of perturbation torques - positions (numpy ndarray): inertial positions - velocities (numpy ndarray): inertial velocities """ try: init_state = [*satellite.q, *satellite.w, *satellite.actuators.w_rxwls] except AttributeError: init_state = [*satellite.q, *satellite.w, 0, 0, 0] solver = ode(derivatives_func) solver.set_integrator('lsoda', rtol=(1e-10, 1e-10, 1e-10, 1e-10, 1e-10, 1e-10, 1e-10, 1e-6, 1e-6, 1e-6), atol=(1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-8, 1e-8, 1e-8), nsteps=10000) solver.set_initial_value(y=init_state, t=start_time) solver.set_f_params(satellite, nominal_state_func, perturbations_func, position_velocity_func, delta_t) length = int((stop_time - 0) / delta_t + 1) times = np.empty((length, )) q_actual = np.empty((length, 4)) w_actual = np.empty((length, 3)) w_rxwls = np.empty((length, 3)) DCM_estimated = np.empty((length, 3, 3)) w_estimated = np.empty((length, 3)) DCM_desired = np.empty((length, 3, 3)) w_desired = np.empty((length, 3)) attitude_err = np.empty((length, 3)) attitude_rate_err = np.empty((length, 3)) M_ctrl = np.empty((length, 3)) M_applied = np.empty((length, 3)) w_dot_rxwls = np.empty((length, 3)) M_perturb = np.empty((length, 3)) positions = np.empty((length, 3)) velocities = np.empty((length, 3)) i = 0 if verbose: print("Starting propagation at time: {}".format(start_time)) while solver.successful() and solver.t <= stop_time: if verbose: print("Time: {}\nState: {}\n".format(solver.t, solver.y)) # this section currently duplicates calculations for logging purposes t = solver.t q = normalize(solver.y[0:4]) w = solver.y[4:7] r, v = position_velocity_func(t) satellite.q = q satellite.w = w satellite.r = r satellite.v = v _, _, log = simulate_estimation_and_control(t, satellite, nominal_state_func, delta_t, log=True) times[i] = t q_actual[i] = q w_actual[i] = w w_rxwls[i] = solver.y[7:10] DCM_estimated[i] = log["DCM_estimated"] w_estimated[i] = log["w_estimated"] DCM_desired[i] = log["DCM_desired"] w_desired[i] = log["w_desired"] attitude_err[i] = log["attitude_err"] attitude_rate_err[i] = log["attitude_rate_err"] M_ctrl[i] = log["M_ctrl"] M_applied[i] = log["M_applied"] w_dot_rxwls[i] = log["w_dot_rxwls"] M_perturb[i] = perturbations_func(satellite) positions[i] = r velocities[i] = v i += 1 # continue integrating solver.integrate(solver.t + delta_t) results = {} results["times"] = times results["q_actual"] = q_actual results["w_actual"] = w_actual results["w_rxwls"] = w_rxwls results["DCM_estimated"] = DCM_estimated results["w_estimated"] = w_estimated results["DCM_desired"] = DCM_desired results["w_desired"] = w_desired results["attitude_err"] = attitude_err results["attitude_rate_err"] = attitude_rate_err results["M_ctrl"] = M_ctrl results["M_applied"] = M_applied results["M_perturb"] = M_perturb return results
def main(): # Define 6U CubeSat mass, dimensions, drag coefficient sc_mass = 8 sc_dim = [226.3e-3, 100.0e-3, 366.0e-3] J = 1 / 12 * sc_mass * np.diag([ sc_dim[1]**2 + sc_dim[2]**2, sc_dim[0]**2 + sc_dim[2]**2, sc_dim[0]**2 + sc_dim[1]**2 ]) sc_dipole = np.array([0, 0.018, 0]) # Define two `PDController` objects—one to represent no control and one # to represent PD control with the specified gains no_controller = PDController(k_d=np.diag([0, 0, 0]), k_p=np.diag([0, 0, 0])) controller = PDController(k_d=np.diag([.01, .01, .01]), k_p=np.diag([.1, .1, .1])) # Northrop Grumman LN-200S Gyros gyros = Gyros(bias_stability=1, angular_random_walk=0.07) perfect_gyros = Gyros(bias_stability=0, angular_random_walk=0) # NewSpace Systems Magnetometer magnetometer = Magnetometer(resolution=10e-9) perfect_magnetometer = Magnetometer(resolution=0) # Adcole Maryland Aerospace MAI-SES Static Earth Sensor earth_horizon_sensor = EarthHorizonSensor(accuracy=0.25) perfect_earth_horizon_sensor = EarthHorizonSensor(accuracy=0) # Sinclair Interplanetary 60 mNm-sec RXWLs actuators = Actuators(rxwl_mass=226e-3, rxwl_radius=0.5 * 65e-3, rxwl_max_torque=20e-3, rxwl_max_momentum=0.18, noise_factor=0.03) perfect_actuators = Actuators(rxwl_mass=226e-3, rxwl_radius=0.5 * 65e-3, rxwl_max_torque=np.inf, rxwl_max_momentum=np.inf, noise_factor=0.0) # define some orbital parameters mu_earth = 3.986004418e14 R_e = 6.3781e6 orbit_radius = R_e + 400e3 orbit_w = np.sqrt(mu_earth / orbit_radius**3) period = 2 * np.pi / orbit_w # define a function that returns the inertial position and velocity of the # spacecraft (in m & m/s) at any given time def position_velocity_func(t): r = orbit_radius / np.sqrt(2) * np.array([ -np.cos(orbit_w * t), np.sqrt(2) * np.sin(orbit_w * t), np.cos(orbit_w * t), ]) v = orbit_w * orbit_radius / np.sqrt(2) * np.array([ np.sin(orbit_w * t), np.sqrt(2) * np.cos(orbit_w * t), -np.sin(orbit_w * t), ]) return r, v # compute the initial inertial position and velocity r_0, v_0 = position_velocity_func(0) # define the body axes in relation to where we want them to be: # x = Earth-pointing # y = pointing along the velocity vector # z = normal to the orbital plane b_x = -normalize(r_0) b_y = normalize(v_0) b_z = cross(b_x, b_y) # construct the nominal DCM from inertial to body (at time 0) from the body # axes and compute the equivalent quaternion dcm_0_nominal = np.stack([b_x, b_y, b_z]) q_0_nominal = dcm_to_quaternion(dcm_0_nominal) # compute the nominal angular velocity required to achieve the reference # attitude; first in inertial coordinates then body w_nominal_i = 2 * np.pi / period * normalize(cross(r_0, v_0)) w_nominal = np.matmul(dcm_0_nominal, w_nominal_i) # provide some initial offset in both the attitude and angular velocity q_0 = quaternion_multiply( np.array( [0, np.sin(2 * np.pi / 180 / 2), 0, np.cos(2 * np.pi / 180 / 2)]), q_0_nominal) w_0 = w_nominal + np.array([0.005, 0, 0]) # define a function that will model perturbations def perturb_func(satellite): return (satellite.approximate_gravity_gradient_torque() + satellite.approximate_magnetic_field_torque()) # define a function that returns the desired state at any given point in # time (the initial state and a subsequent rotation about the body x, y, or # z axis depending upon which nominal angular velocity term is nonzero) def desired_state_func(t): if w_nominal[0] != 0: dcm_nominal = np.matmul(t1_matrix(w_nominal[0] * t), dcm_0_nominal) elif w_nominal[1] != 0: dcm_nominal = np.matmul(t2_matrix(w_nominal[1] * t), dcm_0_nominal) elif w_nominal[2] != 0: dcm_nominal = np.matmul(t3_matrix(w_nominal[2] * t), dcm_0_nominal) return dcm_nominal, w_nominal # construct three `Spacecraft` objects composed of all relevant spacecraft # parameters and objects that resemble subsystems on-board # 1st Spacecraft: no controller # 2nd Spacecraft: PD controller with perfect sensors and actuators # 3rd Spacecraft: PD controller with imperfect sensors and actuators satellite_no_control = Spacecraft( J=J, controller=no_controller, gyros=perfect_gyros, magnetometer=perfect_magnetometer, earth_horizon_sensor=perfect_earth_horizon_sensor, actuators=perfect_actuators, q=q_0, w=w_0, r=r_0, v=v_0) satellite_perfect = Spacecraft( J=J, controller=controller, gyros=perfect_gyros, magnetometer=perfect_magnetometer, earth_horizon_sensor=perfect_earth_horizon_sensor, actuators=perfect_actuators, q=q_0, w=w_0, r=r_0, v=v_0) satellite_noise = Spacecraft(J=J, controller=controller, gyros=gyros, magnetometer=magnetometer, earth_horizon_sensor=earth_horizon_sensor, actuators=actuators, q=q_0, w=w_0, r=r_0, v=v_0) # Simulate the behavior of all three spacecraft over time simulate(satellite=satellite_no_control, nominal_state_func=desired_state_func, perturbations_func=perturb_func, position_velocity_func=position_velocity_func, stop_time=6000, tag=r"(No Control)") simulate(satellite=satellite_perfect, nominal_state_func=desired_state_func, perturbations_func=perturb_func, position_velocity_func=position_velocity_func, stop_time=6000, tag=r"(Perfect Estimation \& Control)") simulate(satellite=satellite_noise, nominal_state_func=desired_state_func, perturbations_func=perturb_func, position_velocity_func=position_velocity_func, stop_time=6000, tag=r"(Actual Estimation \& Control)")
def calcAngleBetween(self, searchDir, grad): return scalar_prod(normalize(searchDir), normalize(grad))
def isSearchDirectionDescent(self, searchDir, grad): return scalar_prod(normalize(searchDir), normalize(grad)) < 0
def get_normal(p_vs): #TODO: only 3D triangles supported at the moment return normalize(np.cross(p_vs[1]-p_vs[0], p_vs[2]-p_vs[0]))