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