def q_err(q_t, q_r):
# """
# Compute angular error between two unit quaternions.
# :param q_t: New quaternion
# :type q_t: ca.DM, list, np.array
# :param q_r: Reference quaternion
# :type q_r: ca.DM, list, np.array
# :return: vector corresponding to SK matrix
# :rtype: ca.DM
# """
q_upper_t = [q_r[3], -q_r[0], -q_r[1], -q_r[2]]
q_lower_t = [q_t[3], q_t[0], q_t[1], q_t[2]]
qd_t = [
q_upper_t[0] * q_lower_t[0] - q_upper_t[1] * q_lower_t[1] -
q_upper_t[2] * q_lower_t[2] - q_upper_t[3] * q_lower_t[3],
q_upper_t[1] * q_lower_t[0] + q_upper_t[0] * q_lower_t[1] +
q_upper_t[2] * q_lower_t[3] - q_upper_t[3] * q_lower_t[2],
q_upper_t[0] * q_lower_t[2] - q_upper_t[1] * q_lower_t[3] +
q_upper_t[2] * q_lower_t[0] + q_upper_t[3] * q_lower_t[1],
q_upper_t[0] * q_lower_t[3] + q_upper_t[1] * q_lower_t[2] -
q_upper_t[2] * q_lower_t[1] + q_upper_t[3] * q_lower_t[0]
]
phi_t = ca.atan2(2 * (qd_t[0] * qd_t[1] + qd_t[2] * qd_t[3]),
1 - 2 * (qd_t[1]**2 + qd_t[2]**2))
theta_t = ca.asin(2 * (qd_t[0] * qd_t[2] - qd_t[3] * qd_t[1]))
psi_t = ca.atan2(2 * (qd_t[0] * qd_t[3] + qd_t[1] * qd_t[2]),
1 - 2 * (qd_t[2]**2 + qd_t[3]**2))
return ca.vertcat(phi_t, theta_t, psi_t)
def from_quat(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
e = ca.SX(3, 1)
a = q[0]
b = q[1]
c = q[2]
d = q[3]
e[0] = ca.atan2(2 * (a * b + c * d), 1 - 2 * (b**2 + c**2))
e[1] = ca.asin(2 * (a * c - d * b))
e[2] = ca.atan2(2 * (a * d + b * c), 1 - 2 * (c**2 + d**2))
return e
def solar_elevation_angle(latitude, day_of_year, time):
"""
Elevation angle of the sun [degrees] for a local observer.
:param latitude: Latitude [degrees]
:param day_of_year: Julian day (1 == Jan. 1, 365 == Dec. 31)
:param time: Time after local solar noon [seconds]
:return: Solar elevation angle [degrees] (angle between horizon and sun). Returns 0 if the sun is below the horizon.
"""
# Solar elevation angle (including seasonality, latitude, and time of day)
# Source: https://www.pveducation.org/pvcdrom/properties-of-sunlight/elevation-angle
declination = declination_angle(day_of_year)
solar_elevation_angle = cas.asin(
sind(declination) * sind(latitude) + cosd(declination) *
cosd(latitude) * cosd(time / 86400 * 360)) * 180 / cas.pi # in degrees
solar_elevation_angle = cas.fmax(solar_elevation_angle, 0)
return solar_elevation_angle
def create_rocket_stage_phase(mocp, phase_name, p, **kwargs):
is_powered = kwargs['is_powered']
has_heating = kwargs['has_heating']
drag_area = kwargs['drag_area']
maxQ = kwargs['maxQ']
init_mass = kwargs['init_mass']
init_position = kwargs['init_position']
init_velocity = kwargs['init_velocity']
if is_powered:
init_steering = kwargs['init_steering']
engine_type = kwargs['engine_type']
engine_count = kwargs['engine_count']
mdot_max = kwargs['mdot_max']
mdot_min = kwargs['mdot_min']
if engine_type == 'vacuum':
effective_exhaust_velocity = kwargs['effective_exhaust_velocity']
else:
exhaust_velocity = kwargs['exhaust_velocity']
exit_area = kwargs['exit_area']
exit_pressure = kwargs['exit_pressure']
duration = mocp.create_phase(phase_name, init=0.001, n_intervals=2)
# Total vessel mass
mass = mocp.add_trajectory(phase_name, 'mass', init=init_mass)
# ECEF position vector
rx = mocp.add_trajectory(phase_name, 'rx', init=init_position[0])
ry = mocp.add_trajectory(phase_name, 'ry', init=init_position[1])
rz = mocp.add_trajectory(phase_name, 'rz', init=init_position[2])
r_vec = casadi.vertcat(rx, ry, rz)
# ECEF velocity vector
vx = mocp.add_trajectory(phase_name, 'vx', init=init_velocity[0])
vy = mocp.add_trajectory(phase_name, 'vy', init=init_velocity[1])
vz = mocp.add_trajectory(phase_name, 'vz', init=init_velocity[2])
v_vec = casadi.vertcat(vx, vy, vz)
if is_powered:
# ECEF steering unit vector
ux = mocp.add_trajectory(phase_name, 'ux', init=init_steering[0])
uy = mocp.add_trajectory(phase_name, 'uy', init=init_steering[1])
uz = mocp.add_trajectory(phase_name, 'uz', init=init_steering[2])
u_vec = casadi.vertcat(ux, uy, uz)
# Engine mass flow for a SINGLE engine
mdot = mocp.add_trajectory(phase_name, 'mdot', init=mdot_max)
mdot_rate = mocp.add_trajectory(phase_name, 'mdot_rate', init=0.0)
if has_heating:
heat = mocp.add_trajectory(phase_name, 'heat', init=0.0)
## Dynamics
r_squared = rx**2 + ry**2 + rz**2 + 1e-8
v_squared = vx**2 + vy**2 + vz**2 + 1e-8
airspeed = v_squared**0.5
r = r_squared**0.5
altitude = r - 1
r3_inv = r_squared**(-1.5)
mocp.add_path_output(
'downrange_distance',
casadi.asin(
casadi.norm_2(
casadi.cross(
r_vec / r,
casadi.SX(
[p.launch_site_x, p.launch_site_y,
p.launch_site_z])))))
mocp.add_path_output('altitude', altitude)
mocp.add_path_output('airspeed', airspeed)
mocp.add_path_output('vertical_speed', (r_vec.T / r) @ v_vec)
mocp.add_path_output(
'horizontal_speed_ECEF',
casadi.norm_2(v_vec - ((r_vec.T / r) @ v_vec) * (r_vec / r)))
# Gravitational acceleration (mu=1 is omitted)
gx = -r3_inv * rx
gy = -r3_inv * ry
gz = -r3_inv * rz
# Atmosphere
atmosphere_fraction = casadi.exp(-altitude / p.scale_height)
air_pressure = p.air_pressure_MSL * atmosphere_fraction
air_density = p.air_density_MSL * atmosphere_fraction
dynamic_pressure = 0.5 * air_density * v_squared
mocp.add_path_output('dynamic_pressure', dynamic_pressure)
if is_powered:
lateral_airspeed = casadi.sqrt(
casadi.sumsqr(v_vec - ((v_vec.T @ u_vec) * u_vec)) + 1e-8)
lateral_dynamic_pressure = 0.5 * air_density * lateral_airspeed * airspeed # Same as Q * sin(alpha), but without trigonometry
mocp.add_path_output('lateral_dynamic_pressure',
lateral_dynamic_pressure)
mocp.add_path_output(
'alpha',
casadi.acos((v_vec.T @ u_vec) / airspeed) * 180.0 / pi)
mocp.add_path_output(
'pitch', 90.0 - casadi.acos((r_vec.T / r) @ u_vec) * 180.0 / pi)
# Thrust force
if is_powered:
if engine_type == 'vacuum':
engine_thrust = effective_exhaust_velocity * mdot
else:
engine_thrust = exhaust_velocity * mdot + exit_area * (
exit_pressure - air_pressure)
total_thrust = engine_thrust * engine_count
mocp.add_path_output('total_thrust', total_thrust)
Tx = total_thrust * ux
Ty = total_thrust * uy
Tz = total_thrust * uz
else:
Tx = 0.0
Ty = 0.0
Tz = 0.0
# Drag force
drag_factor = (-0.5 * drag_area) * air_density * airspeed
Dx = drag_factor * vx
Dy = drag_factor * vy
Dz = drag_factor * vz
# Coriolis Acceleration
ax_Coriolis = 2 * vy * p.body_rotation_speed
ay_Coriolis = -2 * vx * p.body_rotation_speed
az_Coriolis = 0.0
# Centrifugal Acceleration
ax_Centrifugal = rx * p.body_rotation_speed**2
ay_Centrifugal = ry * p.body_rotation_speed**2
az_Centrifugal = 0.0
# Acceleration
ax = gx + ax_Coriolis + ax_Centrifugal + (Dx + Tx) / mass
ay = gy + ay_Coriolis + ay_Centrifugal + (Dy + Ty) / mass
az = gz + az_Coriolis + az_Centrifugal + (Dz + Tz) / mass
if is_powered:
mocp.set_derivative(mass, -engine_count * mdot)
mocp.set_derivative(mdot, mdot_rate)
else:
mocp.set_derivative(mass, 0.0)
mocp.set_derivative(rx, vx)
mocp.set_derivative(ry, vy)
mocp.set_derivative(rz, vz)
mocp.set_derivative(vx, ax)
mocp.set_derivative(vy, ay)
mocp.set_derivative(vz, az)
# Heat flow
heat_flow = 1e-4 * (air_density**0.5) * (airspeed**3.15)
mocp.add_path_output('heat_flow', heat_flow)
if has_heating:
mocp.set_derivative(heat, heat_flow)
## Constraints
# Duration is positive and bounded
mocp.add_constraint(duration > 0.006)
mocp.add_constraint(duration < 10.0)
# Mass is positive
mocp.add_path_constraint(mass > 1e-6)
# maxQ soft constraint
weight_dynamic_pressure_penalty = mocp.get_parameter(
'weight_dynamic_pressure_penalty')
slack_maxQ = mocp.add_trajectory(phase_name, 'slack_maxQ', init=10.0)
mocp.add_path_constraint(slack_maxQ > 0.0)
mocp.add_mean_objective(weight_dynamic_pressure_penalty * slack_maxQ)
mocp.add_path_constraint(dynamic_pressure / maxQ < 1.0 + slack_maxQ)
if is_powered:
# u is a unit vector
mocp.add_path_constraint(ux**2 + uy**2 + uz**2 == 1.0)
# Do not point down
mocp.add_path_constraint((r_vec / r).T @ u_vec > -0.2)
# Engine flow limits
mocp.add_path_constraint(mdot_min < mdot)
mocp.add_path_constraint(mdot < mdot_max)
# Throttle rate reduction
mocp.add_mean_objective(1e-6 * mdot_rate**2)
# Lateral dynamic pressure penalty
weight_lateral_dynamic_pressure_penalty = mocp.get_parameter(
'weight_lateral_dynamic_pressure_penalty')
mocp.add_mean_objective(
weight_lateral_dynamic_pressure_penalty *
(lateral_dynamic_pressure / p.maxQ_sin_alpha)**8)
variables = locals().copy()
RocketStagePhase = collections.namedtuple('RocketStagePhase',
sorted(list(variables.keys())))
return RocketStagePhase(**variables)
