def apply_impulse(self) -> None:
dv = (self.body_2.velocity +
cross(self.body_2.angular_velocity, self.r2) -
self.body_1.velocity -
cross(self.body_1.angular_velocity, self.r1))
impulse = self.M @ (self.bias - dv - self.softness * self.P)
self.body_1.velocity -= self.body_1.inv_mass * impulse
self.body_1.angular_velocity -= self.body_1.inv_I * cross(
self.r1, impulse)
self.body_2.velocity += self.body_2.inv_mass * impulse
self.body_2.angular_velocity += self.body_2.inv_I * cross(
self.r2, impulse)
self.P += impulse
def apply_control_torques(self, M_ctrl, w_sc, t, delta_t):
"""Applies the control torques to the modeled reaction wheels
Args:
M_ctrl (numpy ndarray): the control torque (3x1) produced by the
PD controller (N * m)
w_sc (numpy ndarray): the angular velocity (rad/s) (3x1) in body
coordinates of the spacecraft (at a given time)
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 control moment (3x1) as actually applied on
the reaction wheels (the input control torque with some
Gaussian noise applied) (N * m)
numpy ndarray: the angular acceleration of the 3 reaction wheels
applied to achieve the applied torque (rad/s^2)
"""
# take into account the fact that reaction wheels can only apply a certain max torque
M_ctrl_fixed = np.empty((3, ))
M_ctrl_fixed[0] = np.sign(M_ctrl[0]) * min(abs(M_ctrl[0]),
self.rxwl_max_torque)
M_ctrl_fixed[1] = np.sign(M_ctrl[1]) * min(abs(M_ctrl[1]),
self.rxwl_max_torque)
M_ctrl_fixed[2] = np.sign(M_ctrl[2]) * min(abs(M_ctrl[2]),
self.rxwl_max_torque)
w_dot_rxwls = -cross(w_sc, self.w_rxwls) - 1 / self.C_w * M_ctrl_fixed
w_dot_rxwls[0] = self.add_noise(w_dot_rxwls[0], 0, t, delta_t)
w_dot_rxwls[1] = self.add_noise(w_dot_rxwls[1], 1, t, delta_t)
w_dot_rxwls[2] = self.add_noise(w_dot_rxwls[2], 2, t, delta_t)
for i, w_rxwl in enumerate(self.w_rxwls):
# if a wheel is saturated, we can no longer accelerate it above
# its maximum momentum
if ((self.C_w * w_rxwl < -self.rxwl_max_momentum
and w_dot_rxwls[i] < 0)
or (self.C_w * w_rxwl > self.rxwl_max_momentum
and w_dot_rxwls[i] > 0)):
w_dot_rxwls[i] = 0.0
M_applied = -self.C_w * w_dot_rxwls - cross(w_sc,
self.C_w * self.w_rxwls)
return M_applied, w_dot_rxwls
def pre_step(self, inv_dt: float) -> None:
rot_1 = self.body_1.rotation_matrix()
rot_2 = self.body_2.rotation_matrix()
r1 = self.r1 = rot_1 @ self.local_anchor_1
r2 = self.r2 = rot_2 @ self.local_anchor_2
K1 = np.array([[self.body_1.inv_mass + self.body_2.inv_mass, 0.0],
[0.0, self.body_1.inv_mass + self.body_2.inv_mass]])
K2 = np.array([[
self.body_1.inv_I * r1[1] * r1[1],
-self.body_1.inv_I * r1[0] * r1[1]
],
[
-self.body_1.inv_I * r1[0] * r1[1],
self.body_1.inv_I * r1[0] * r1[0]
]])
K3 = np.array([[
self.body_2.inv_I * r2[1] * r2[1],
-self.body_2.inv_I * r2[0] * r2[1]
],
[
-self.body_2.inv_I * r2[0] * r2[1],
self.body_2.inv_I * r2[0] * r2[0]
]])
K = K1 + K2 + K3 + np.eye(2) * self.softness
self.M = np.linalg.inv(K)
p1 = self.body_1.position + r1
p2 = self.body_2.position + r2
dp = p2 - p1
if POSITION_CORRECTION:
self.bias = -self.bias_factor * inv_dt * dp
else:
self.bias = np.zeros(2, dtype=self.body_1.width.dtype)
if WARM_STARTING:
self.body_1.velocity -= self.body_1.inv_mass * self.P
self.body_1.angular_velocity -= self.body_1.inv_I * cross(
r1, self.P)
self.body_2.velocity += self.body_2.inv_mass * self.P
self.body_2.angular_velocity += self.body_2.inv_I * cross(
r2, self.P)
else:
self.P = np.zeros(2, dtype=self.body_1.width.dtype)
def pre_step(self, inv_dt: float) -> None:
k_allowed_penetration = 0.01
k_bias_factor = 0.2 if POSITION_CORRECTION else 0.0
for c in self.contacts:
r1 = c.position - self.body_1.position
r2 = c.position - self.body_2.position
# precompute normal mass, tangent mass, and bias
rn1 = r1 @ c.normal
rn2 = r2 @ c.normal
k_normal = self.body_1.inv_mass + self.body_2.inv_mass
k_normal += (self.body_1.inv_I * ((r1 @ r1) - rn1 * rn1) +
self.body_2.inv_I * ((r2 @ r2) - rn2 * rn2))
c.mass_normal = 1.0 / k_normal
tangent = cross(c.normal, 1.0)
rt1 = r1 @ tangent
rt2 = r2 @ tangent
k_tangent = self.body_1.inv_mass + self.body_2.inv_mass
k_tangent += (self.body_1.inv_I * ((r1 @ r1) - rt1 * rt1) +
self.body_2.inv_I * ((r2 @ r2) - rt2 * rt2))
c.mass_tangent = 1.0 / k_tangent
c.bias = -k_bias_factor * inv_dt * min(
0.0, c.separation + k_allowed_penetration)
if ACCUMULATE_IMPULSES:
# apply normal + friction impulse
P = c.Pn * c.normal + c.Pt * tangent
self.body_1.velocity -= self.body_1.inv_mass * P
self.body_1.angular_velocity -= self.body_1.inv_I * cross(
r1, P)
self.body_2.velocity += self.body_2.inv_mass * P
self.body_2.angular_velocity += self.body_2.inv_I * cross(
r2, P)
def calculate_attitude_rate_error(w_desired, w_estimated, attitude_err):
"""Computes the attitude rate error of a spacecraft at a given time
Args:
w_desired (numpy ndarray): the nominal angular velocity (rad/s) (3x1) in body coordinates of the spacecraft at a given time
w_estimated (numpy ndarray): the estimated angular velocity (rad/s) (3x1) in body coordinates of the spacecraft at a given time
attitude_err (numpy ndarray): the attitude error (3x1) of the spacecraft at a given time
Returns:
attitude_rate_error: the attitude rate error (3x1) of the system at the given time
"""
delta_w = w_estimated - w_desired
attitude_rate_err = -cross(w_estimated, attitude_err) + delta_w
return attitude_rate_err
def angular_velocity_derivative(J, w, M_list=[]):
"""Computes the time derivative of a satellite's angular velocity (in rad/s^2 in body coordinates)
Args:
J (numpy ndarray): the satellite's inertia tensor (3x3) (kg * m^2)
w (numpy ndarray): the angular velocity (rad/s) (3x1) in body
coordinates of the spacecraft
M_list (list): a list of (3x1) numpy ndarrays where each element in the
list is a torque (N * m); an empty list signifies no torques
Returns:
numpy ndarray: the angular acceleration (3x1) produced by the sum of
the torques on the spacecraft
"""
# if the list of torques is empty, the total torque is [0, 0, 0]
if len(M_list) == 0:
M_total = np.array([0, 0, 0])
# else, sum the torques
else:
M_total = np.sum(M_list, axis=0)
J_inv = np.linalg.inv(J)
return (-np.matmul(J_inv, cross(w, np.matmul(J, w))) +
np.matmul(J_inv, M_total))
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 apply_impulse(self) -> None:
b1 = self.body_1
b2 = self.body_2
for c in self.contacts:
c.r1 = c.position - b1.position
c.r2 = c.position - b2.position
# relative velocity at contact
dv = (b2.velocity + cross(b2.angular_velocity, c.r2) -
b1.velocity - cross(b1.angular_velocity, c.r1))
# compute normal impulse
vn = dv @ c.normal
d_Pn = c.mass_normal * (-vn + c.bias)
if ACCUMULATE_IMPULSES:
# clamp the accumulated impulse
Pn_0 = c.Pn
c.Pn = max(Pn_0 + d_Pn, 0.0)
d_Pn = c.Pn - Pn_0
else:
d_Pn = max(d_Pn, 0.0)
# apply contact impulse
Pn = d_Pn * c.normal
b1.velocity -= b1.inv_mass * Pn
b1.angular_velocity -= b1.inv_I * cross(c.r1, Pn)
b2.velocity += b2.inv_mass * Pn
b2.angular_velocity += b2.inv_I * cross(c.r2, Pn)
# relative velocity at contact
dv = (b2.velocity + cross(b2.angular_velocity, c.r2) -
b1.velocity - cross(b1.angular_velocity, c.r1))
tangent = cross(c.normal, 1.0)
vt = dv @ tangent
d_Pt = c.mass_tangent * (-vt)
if ACCUMULATE_IMPULSES:
# compute friction impulse
max_Pt = self.friction * c.Pn
# clamp friction
old_tangent_impulse = c.Pt
c.Pt = np.clip(old_tangent_impulse + d_Pt, -max_Pt, max_Pt)
d_Pt = c.Pt - old_tangent_impulse
else:
max_Pt = self.friction * d_Pn
d_Pt = np.clip(d_Pt, -max_Pt, max_Pt)
# apply contact impulses
Pt = d_Pt * tangent
b1.velocity -= b1.inv_mass * Pt
b1.angular_velocity -= b1.inv_I * cross(c.r1, Pt)
b2.velocity += b2.inv_mass * Pt
b2.angular_velocity += b2.inv_I * cross(c.r2, Pt)