def axis_angle_from_matrix(rotation_matrix):
"""
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
rm = rotation_matrix
angle = (trace(rm[:3, :3]) - 1) / 2
angle = Min(angle, 1)
angle = Max(angle, -1)
angle = acos(angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = sqrt(x * x + y * y + z * z)
m = if_eq_zero(n, 1, n)
axis = Matrix([
if_eq_zero(n, 0, x / m),
if_eq_zero(n, 0, y / m),
if_eq_zero(n, 1, z / m)
])
sign = if_eq_zero(angle, 1, diffable_sign(angle))
axis *= sign
angle = sign * angle
return axis, angle
def asTwist(self):
acosarg = (casadi.trace(self.R) - 1) / 2
theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg))
w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]),
(self.R[0, 2] - self.R[2, 0]),
(self.R[1, 0] - self.R[0, 1]))
return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def quaternion_slerp(q1, q2, t):
"""
spherical linear interpolation that takes into account that q == -q
:param q1: 4x1 Matrix
:type q1: Matrix
:param q2: 4x1 Matrix
:type q2: Matrix
:param t: float, 0-1
:type t: Union[float, Symbol]
:return: 4x1 Matrix; Return spherical linear interpolation between two quaternions.
:rtype: Matrix
"""
cos_half_theta = dot(q1.T, q2)
if0 = -cos_half_theta
q2 = if_greater_zero(if0, -q2, q2)
cos_half_theta = if_greater_zero(if0, -cos_half_theta, cos_half_theta)
if1 = Abs(cos_half_theta) - 1.0
# enforce acos(x) with -1 < x < 1
cos_half_theta = Min(1, cos_half_theta)
cos_half_theta = Max(-1, cos_half_theta)
half_theta = acos(cos_half_theta)
sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta)
if2 = 0.001 - Abs(sin_half_theta)
ratio_a = save_division(sin((1.0 - t) * half_theta), sin_half_theta)
ratio_b = save_division(sin(t * half_theta), sin_half_theta)
return if_greater_eq_zero(
if1, Matrix(q1),
if_greater_zero(if2, 0.5 * q1 + 0.5 * q2, ratio_a * q1 + ratio_b * q2))
def axis_angle_from_matrix(rotation_matrix):
"""
MAKE SURE MATRIX IS NORMALIZED
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
q = quaternion_from_matrix(rotation_matrix)
return axis_angle_from_quaternion(q[0], q[1], q[2], q[3])
# TODO use 'if' to make angle always positive?
rm = rotation_matrix
cos_angle = (trace(rm[:3, :3]) - 1) / 2
cos_angle = Min(cos_angle, 1)
cos_angle = Max(cos_angle, -1)
angle = acos(cos_angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = sqrt(x * x + y * y + z * z)
axis = Matrix([
if_eq(Abs(cos_angle), 1, 0, x / n),
if_eq(Abs(cos_angle), 1, 0, y / n),
if_eq(Abs(cos_angle), 1, 1, z / n)
])
return axis, angle
def diffable_axis_angle_from_matrix(rotation_matrix):
"""
MAKE SURE MATRIX IS NORMALIZED
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
# TODO nan if angle 0
# TODO buggy when angle == pi
# TODO use 'if' to make angle always positive?
rm = rotation_matrix
cos_angle = (trace(rm[:3, :3]) - 1) / 2
cos_angle = diffable_min_fast(cos_angle, 1)
cos_angle = diffable_max_fast(cos_angle, -1)
angle = acos(cos_angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = sqrt(x * x + y * y + z * z)
axis = Matrix([
if_eq(Abs(cos_angle), 1, 0, x / n),
if_eq(Abs(cos_angle), 1, 0, y / n),
if_eq(Abs(cos_angle), 1, 1, z / n)
])
return axis, angle
def get_angle_casadi(point1, point2, point3):
v12 = vector3(*point1) - vector3(*point2)
v13 = vector3(*point1) - vector3(*point3)
v23 = vector3(*point2) - vector3(*point3)
d12 = norm(v12)
d13 = norm(v13)
d23 = norm(v23)
#return w.acos(w.dot(v12, v13.T)[0] / (d12 * d13))
return acos((d12**2 + d13**2 - d23**2) / (2 * d12 * d13))
def terminal_constraints3(x, t, x0, t0):
# https://space.stackexchange.com/questions/1904/how-to-programmatically-calculate-orbital-elements-using-position-velocity-vecto
# http://control.asu.edu/Classes/MAE462/462Lecture06.pdf
h = ca.vertcat(x[1] * x[5] - x[4] * x[2], x[3] * x[2] - x[0] * x[5],
x[0] * x[4] - x[1] * x[3])
n = ca.vertcat(-h[1], h[0], 0)
r = ca.sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2])
e = ca.vertcat(
1 / muE * (x[4] * h[2] - x[5] * h[1]) - x[0] / r,
1 / muE * (x[5] * h[0] - x[3] * h[2]) - x[1] / r,
1 / muE * (x[3] * h[1] - x[4] * h[0]) - x[2] / r,
)
e_mag = ca.sqrt(e[0] * e[0] + e[1] * e[1] + e[2] * e[2])
h_sq = h[0] * h[0] + h[1] * h[1] + h[2] * h[2]
v_mag = ca.sqrt(x[3] * x[3] + x[4] * x[4] + x[5] * x[5])
a = -muE / (v_mag * v_mag - 2.0 * muE / r)
i = ca.acos(h[2] / ca.sqrt(h_sq))
n_mag = ca.sqrt(n[0] * n[0] + n[1] * n[1])
node_asc = ca.acos(n[0] / n_mag)
# if n[1] < -1e-12:
node_asc = 2 * np.pi - node_asc
argP = ca.acos((n[0] * e[0] + n[1] * e[1]) / (n_mag * e_mag))
# if e[2] < 0:
# argP = 2*np.pi - argP
a_req = 24361140.0
e_req = 0.7308
i_req = 28.5 * np.pi / 180.0
node_asc_req = 269.8 * np.pi / 180.0
argP_req = 130.5 * np.pi / 180.0
return [
(a - a_req) / (Re),
e_mag - e_req,
i - i_req,
node_asc - node_asc_req,
argP - argP_req,
]
def slerp(v1, v2, t):
"""
spherical linear interpolation
:param v1: any vector
:param v2: vector of same length as v1
:param t: value between 0 and 1. 0 is v1 and 1 is v2
:return:
"""
angle = acos(dot(v1.T, v2)[0])
return (sin((1-t)*angle)/sin(angle))*v1 + (sin(t*angle)/sin(angle))*v2
def log(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
v = ca.SX(3, 1)
norm_q = ca.norm_2(q)
theta = 2 * ca.acos(q[0])
c = ca.sin(theta / 2)
v[0] = theta * q[1] / c
v[1] = theta * q[2] / c
v[2] = theta * q[3] / c
return ca.if_else(ca.fabs(c) > eps, v, ca.SX([0, 0, 0]))
def rotation_distance(a_R_b, a_R_c):
"""
:param a_R_b: 4x4 or 3x3 Matrix
:type a_R_b: Matrix
:param a_R_c: 4x4 or 3x3 Matrix
:type a_R_c: Matrix
:return: angle of axis angle representation of b_R_c
:rtype: Union[float, Symbol]
"""
difference = dot(a_R_b.T, a_R_c)
angle = (trace(difference[:3, :3]) - 1) / 2
angle = Min(angle, 1)
angle = Max(angle, -1)
return acos(angle)
def axis_angle_from_quaternion(x, y, z, w):
"""
:type x: Union[float, Symbol]
:type y: Union[float, Symbol]
:type z: Union[float, Symbol]
:type w: Union[float, Symbol]
:return: 4x1 Matrix
:rtype: Matrix
"""
l = norm(Matrix([x, y, z, w]))
x, y, z, w = x / l, y / l, z / l, w / l
w2 = sqrt(1 - w**2)
angle = (2 * acos(Min(Max(-1, w), 1)))
m = if_eq_zero(w2, 1, w2) # avoid /0
x = if_eq_zero(w2, 0, x / m)
y = if_eq_zero(w2, 0, y / m)
z = if_eq_zero(w2, 1, z / m)
return Matrix([x, y, z]), angle
def diffable_axis_angle_from_matrix_stable(rotation_matrix):
"""
:param rotation_matrix: 4x4 or 3x3 Matrix
:type rotation_matrix: Matrix
:return: 3x1 Matrix, angle
:rtype: (Matrix, Union[float, Symbol])
"""
# TODO buggy when angle == pi
rm = rotation_matrix
angle = (trace(rm[:3, :3]) - 1) / 2
angle = diffable_min_fast(angle, 1)
angle = diffable_max_fast(angle, -1)
angle = acos(angle)
x = (rm[2, 1] - rm[1, 2])
y = (rm[0, 2] - rm[2, 0])
z = (rm[1, 0] - rm[0, 1])
n = sqrt(x * x + y * y + z * z)
m = diffable_if_eq_zero(n, 1, n)
axis = Matrix([
diffable_if_eq_zero(n, 0, x / m),
diffable_if_eq_zero(n, 0, y / m),
diffable_if_eq_zero(n, 1, z / m)
])
return axis, angle
def diffable_slerp(q1, q2, t):
"""
!takes a long time to compile!
:param q1: 4x1 Matrix
:type q1: Matrix
:param q2: 4x1 Matrix
:type q2: Matrix
:param t: float, 0-1
:type t: Union[float, Symbol]
:return: 4x1 Matrix; Return spherical linear interpolation between two quaternions.
:rtype: Matrix
"""
cos_half_theta = dot(q1.T, q2)
if0 = -cos_half_theta
q2 = diffable_if_greater_zero(if0, -q2, q2)
cos_half_theta = diffable_if_greater_zero(if0, -cos_half_theta,
cos_half_theta)
if1 = diffable_abs(cos_half_theta) - 1.0
# enforce acos(x) with -1 < x < 1
cos_half_theta = diffable_min_fast(1, cos_half_theta)
cos_half_theta = diffable_max_fast(-1, cos_half_theta)
half_theta = acos(cos_half_theta)
sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta)
if2 = 0.001 - diffable_abs(sin_half_theta)
ratio_a = save_division(sin((1.0 - t) * half_theta), sin_half_theta)
ratio_b = save_division(sin(t * half_theta), sin_half_theta)
return diffable_if_greater_eq_zero(
if1, Matrix(q1),
diffable_if_greater_zero(if2, 0.5 * q1 + 0.5 * q2,
ratio_a * q1 + ratio_b * q2))
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)
def rotationAngle(mx_R):
acosarg = (casadi.trace(mx_R) - 1.) / 2.
return casadi.if_else(casadi.fabs(acosarg) >= 1., 0., casadi.acos(acosarg))
def create_rocket_stage_phase(mocp, phase_name, p, **kwargs):
engine_parameters = kwargs['engine_parameters']
atmosphere_parameters = kwargs['atmosphere_parameters']
init_mass = kwargs['init_mass']
has_steering_unit_vector_constraint = kwargs.get(
'has_steering_unit_vector_constraint', True)
if init_mass is None:
assert atmosphere_parameters is None # Must have mass for drag-acceleration
assert engine_parameters is None # Must have mass for thrust-acceleration
if phase_name == 'target':
init_position = [
p.target_position_x, p.target_position_y, p.target_position_z
]
init_velocity = [
p.target_velocity_x, p.target_velocity_y, p.target_velocity_z
]
else:
init_position = [p.launch_site_x, p.launch_site_y, p.launch_site_z]
init_velocity = [0.0, 0.0, 0.0]
duration = mocp.create_phase(phase_name, init=0.001, n_intervals=3)
if init_mass is not None:
# 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)
mocp.add_path_constraint(sumsqr(r_vec) > 0.99)
# 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 engine_parameters is not None:
launch_site_param = DM(
[p.launch_site_x, p.launch_site_y, p.launch_site_z])
init_up_direction = normalize(launch_site_param)
# ECEF steering unit vector
ux = mocp.add_trajectory(phase_name, 'ux', init=init_up_direction[0])
uy = mocp.add_trajectory(phase_name, 'uy', init=init_up_direction[1])
uz = mocp.add_trajectory(phase_name, 'uz', init=init_up_direction[2])
u_vec = casadi.vertcat(ux, uy, uz)
if has_steering_unit_vector_constraint:
# u is a unit vector
mocp.add_constraint(sumsqr(mocp.start(u_vec)) == 1.0)
omega_x = mocp.add_trajectory(phase_name, 'omega_x', init=0.0)
omega_y = mocp.add_trajectory(phase_name, 'omega_y', init=0.0)
omega_z = mocp.add_trajectory(phase_name, 'omega_z', init=0.0)
omega_vec = casadi.vertcat(omega_x, omega_y, omega_z)
alpha_x = mocp.add_trajectory(phase_name, 'alpha_x', init=0.0)
alpha_y = mocp.add_trajectory(phase_name, 'alpha_y', init=0.0)
alpha_z = mocp.add_trajectory(phase_name, 'alpha_z', init=0.0)
alpha_vec = casadi.vertcat(alpha_x, alpha_y, alpha_z)
du_dt = casadi.cross(omega_vec,
u_vec) + u_vec * 10 * (1.0 - sumsqr(u_vec))
mocp.set_derivative(ux, du_dt[0])
mocp.set_derivative(uy, du_dt[1])
mocp.set_derivative(uz, du_dt[2])
mocp.set_derivative(omega_x, alpha_x)
mocp.set_derivative(omega_y, alpha_y)
mocp.set_derivative(omega_z, alpha_z)
mocp.add_path_constraint(dot(u_vec, omega_vec) == 1e-20)
mocp.add_mean_objective(1e-6 * sumsqr(alpha_vec))
mocp.add_mean_objective(1e-6 * sumsqr(omega_vec))
mocp.add_path_output('u_mag_err', sumsqr(u_vec) - 1)
throttle = mocp.add_trajectory(phase_name, 'throttle', init=1.0)
throttle_rate = mocp.add_trajectory(phase_name,
'throttle_rate',
init=0.0)
r_squared = sumsqr(r_vec) + 1e-8
v_squared = sumsqr(v_vec) + 1e-8
airspeed = v_squared**0.5
orbital_radius = r_squared**0.5
up_direction = r_vec / orbital_radius
altitude = orbital_radius - 1.0
planet_angular_velocity = casadi.DM(
[0.0, -p.body_angular_rate, 0.0]
) # KSP coordinates: planet angular velocity points in negative y direction
a_gravity = -(r_squared**(
-1.5)) * r_vec # Gravitational acceleration (mu=1 is omitted)
a_coriolis = -2 * cross(planet_angular_velocity,
v_vec) # Coriolis acceleration
a_centrifugal = -cross(planet_angular_velocity,
cross(planet_angular_velocity,
r_vec)) # Centrifugal acceleration
a_vec = a_gravity + a_coriolis + a_centrifugal # Total acceleration on vehicle
# Atmosphere
if atmosphere_parameters is not None:
atmosphere_fraction = casadi.exp(-altitude /
atmosphere_parameters.scale_height)
air_density = atmosphere_parameters.air_density_MSL * atmosphere_fraction
dynamic_pressure = 0.5 * air_density * v_squared
mocp.add_path_output('dynamic_pressure', dynamic_pressure)
# Drag force
drag_force_vector = (-0.5 * atmosphere_parameters.drag_area *
atmosphere_parameters.air_density_MSL
) * atmosphere_fraction * airspeed * v_vec
a_vec += (drag_force_vector / mass)
# Engine thrust
if engine_parameters is not None:
if atmosphere_parameters is None:
exhaust_velocity = engine_parameters.exhaust_velocity_vacuum
else:
exhaust_velocity = engine_parameters.exhaust_velocity_vacuum + \
atmosphere_fraction * (engine_parameters.exhaust_velocity_sea_level - engine_parameters.exhaust_velocity_vacuum)
engine_mass_flow = engine_parameters.max_mass_flow * throttle
thrust = exhaust_velocity * engine_mass_flow
mocp.add_path_output('thrust', thrust)
thrust_acceleration = thrust / mass
mocp.add_path_output('thrust_acceleration', thrust_acceleration)
thrust_force_vector = thrust * u_vec
a_vec += (thrust_force_vector / mass)
## Differential equations
if engine_parameters is not None:
mocp.set_derivative(mass, -engine_mass_flow)
mocp.set_derivative(throttle, throttle_rate)
elif init_mass is not None:
mocp.set_derivative(mass, 0.0) # Atmospheric coast: mass is constant
for i in range(3):
mocp.set_derivative(r_vec[i], v_vec[i])
mocp.set_derivative(v_vec[i], a_vec[i])
## Constraints
# Duration is positive and bounded
mocp.add_constraint(duration > 0.004)
if phase_name == 'target':
mocp.add_constraint(duration < 20.0)
else:
mocp.add_constraint(duration < 10.0)
# Mass is positive
if init_mass is not None:
mocp.add_path_constraint(mass > 1e-6)
if engine_parameters is not None:
# Do not point down
mocp.add_path_constraint(dot(up_direction, u_vec) > -0.2)
# Throttle limits
mocp.add_path_constraint(throttle < 0.90)
mocp.add_path_constraint(throttle > 0.05)
# Throttle rate reduction
mocp.add_mean_objective(1e-5 * throttle_rate**2)
# Q alpha constraint
if (atmosphere_parameters is not None) and (engine_parameters is not None):
lateral_airspeed = casadi.sqrt(
casadi.sumsqr(v_vec - (dot(v_vec, 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_mean_objective(
(lateral_dynamic_pressure /
atmosphere_parameters.lateral_dynamic_pressure_limit)**8)
## Outputs
mocp.add_path_output('altitude', altitude)
mocp.add_path_output('airspeed', airspeed)
mocp.add_path_output('vertical_speed', dot(up_direction, v_vec))
mocp.add_path_output(
'horizontal_speed_ECEF',
casadi.norm_2(v_vec - (dot(up_direction, v_vec) * (up_direction))))
if engine_parameters is not None:
mocp.add_path_output(
'AoA',
casadi.acos(dot(v_vec, u_vec) / airspeed) * 180.0 / pi)
mocp.add_path_output(
'pitch', 90.0 - casadi.acos(dot(up_direction, u_vec)) * 180.0 / pi)
variables = locals().copy()
RocketStagePhase = collections.namedtuple('RocketStagePhase',
sorted(list(variables.keys())))
return RocketStagePhase(**variables)
def angle_between_vector(v1, v2):
v1 = v1[:3]
v2 = v2[:3]
return acos(dot(v1.T, v2) / (norm(v1) * norm(v2)))
def __setPhysics__(self):
# Generalized Coordinates, q = [x_B, z_B, theta_H, theta_K]
q = ca.SX.sym('q', self.n_generalized, 1)
dq = ca.SX.sym('dq', self.n_generalized, 1)
# ddq = ca.SX.sym('ddq', self.n_generalized, 1)
# Inputs and external force, u = [u_H, u_K]
u = ca.SX.sym('u', self.dof, 1)
lam = ca.SX.sym('lambda', 3, self.n_contact)
"""Hopper Dynamics"""
"Joint positions from Base to end effector"
p = ca.SX.zeros(2, self.n_joints)
p[0, 0], p[1, 0] = q[0] + (self.length[1, 0]) * ca.sin(
q[2]), q[1] - (self.length[1, 0]) * ca.cos(q[2])
p[0, 1], p[1,
1] = p[0, 0] + self.length[2, 0] * ca.sin(q[2] + q[3]), p[
1, 0] - self.length[2, 0] * ca.cos(q[2] + q[3])
# COG
g = ca.SX.zeros(2, self.n_joints + 1)
g[0, 0], g[1, 0] = q[0], q[1]
g[0, 1], g[1, 1] = p[0, 0] / 2, p[1, 0] / 2
g[0, 2], g[
1, 2] = p[0, 0] + (self.length[2, 0] / 2) * ca.sin(q[2] + q[3]), p[
1, 0] - (self.length[2, 0] / 2) * ca.cos(q[2] + q[3])
J_ang = ca.DM([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 1., 0],
[0, 0, 1., 1.]])
a = (J_ang @ q).T
J_ee = ca.jacobian(p[:, -1], q)
"For inertia matrix"
H = ca.SX.zeros(self.n_generalized, self.n_generalized)
for i in range(self.dof):
J_l = ca.jacobian(p[:, i], q)
J_a = ca.jacobian(a[:, i], q)
I = (1 / 12) * self.mass[i] * (self.length[i, 0]**2) # Rod
M = self.mass[i]
H += M * J_l.T @ J_l + I * J_a.T @ J_a
"For coriolis + centrifugal matrix"
C = ca.SX.zeros(self.n_generalized, self.n_generalized)
for i in range(self.n_generalized):
for j in range(self.n_generalized):
sum_ = 0
for k in range(self.n_generalized):
c_ijk = ca.jacobian(H[i, j], q[k]) + ca.jacobian(
H[i, j], q[j]) - ca.jacobian(H[k, j], q[i])
sum_ += c_ijk @ dq[k]
C[i, j] = (1 / 2) * sum_
# sum_ = 0
# for k in range(self.dof):
# c_ijk = ca.jacobian(H[i, j], q[k]) - (1/2)*ca.jacobian(H[j, k], q[i])
# sum_ += c_ijk @ dq[j] @ dq[k]
# C[i, j] = sum_
"For G matrix"
V = self.gravity * ca.sum1(self.mass * g[1, :].T)
G = ca.jacobian(V, q).T
"For B matrix"
B = ca.DM([[0., 0.], [0., 0.], [1., 0.], [0., 1.]])
"For external force"
pe_terrain = ca.SX.zeros(2, 1)
pe_terrain[0, 0] = p[0, -1]
pe_terrain[1, 0] = self.terrain.heightMap(p[0, -1])
phi = p[:, -1] - pe_terrain
B_J_C = ca.jacobian(phi, q)
theta_contact = ca.acos(
ca.dot(self.terrain.heightMapNormalVector(p[0, -1]), ca.DM([0,
1])))
# rotate lambda force represented in contact frame to world frame
B_Rot_C = ca.SX.zeros(2, 2)
B_Rot_C[0,
0], B_Rot_C[0,
1] = ca.cos(theta_contact), ca.sin(theta_contact)
B_Rot_C[1,
0], B_Rot_C[1,
1] = -ca.sin(theta_contact), ca.cos(theta_contact)
C_lam = ca.SX.zeros(2, 1)
C_lam[0, 0], C_lam[1, 0] = lam[0, 0] - lam[1, 0], lam[2, 0]
B_lam = B_Rot_C @ C_lam
dp = J_ee @ dq
# print(dp.shape)
psi = ca.dot(self.terrain.heightMapTangentVector(pe_terrain[0, 0]), dp)
# psi = dp[0, 0]
self.kinematics = ca.Function('Kinematics', [q], [p, g], ['q'],
['p', 'g'])
self.dynamics = ca.Function('Dynamics', [q, dq, u, lam], [
H, C, B, G, phi, J_ee, B_J_C, C_lam, B_lam, psi
], ['q', 'dq', 'u', 'lam'], [
'H', 'C', 'B', 'G', 'phi', 'J_ee', 'B_J_C', 'C_lam', 'B_lam', 'psi'
])
