update
This commit is contained in:
zhouyr9
@@ -1,345 +1,5 @@
|
|||||||
operating_history_raw = """
|
operating_history_raw = """
|
||||||
2050-09-15 First module delivered to LEO via Vulture Shuttle
|
|
||||||
2053-02-02 Assembly Halted
|
|
||||||
|
|
||||||
2053-05-11 Reconfiguring to ST-01 Stellaria
|
|
||||||
Only fitted with the Light Speed Engine. Other systems are still Xihe standard. Awaiting further upgrades.
|
|
||||||
2054-4-28 Reconfiguration Complete
|
|
||||||
|
|
||||||
2054-05-12 Begin Trial Run in Earth Orbit, testing all systems
|
|
||||||
2054-08-22 Trial Run complete, all systems perform as expected. Including multiple run to GEO and Lunar Orbit before returning to LEO.
|
|
||||||
|
|
||||||
2054-09-07 Begin Deep Space Test Mission
|
|
||||||
Uncrewed mission to Earth-Sun Lagrange Point 2 to test long-duration operations
|
|
||||||
2054-11-28 Test Mission Complete, all systems nominal. Returning to LEO.
|
|
||||||
|
|
||||||
2054-12-01 Start maintenance
|
|
||||||
2055-02-28 Maintenance complete, all systems operational, ready for commissioning.
|
|
||||||
|
|
||||||
2055-03-20 Picking up crew for Inner Solar System Exploration Mission 1
|
|
||||||
2055-03-22 Earth Departure for Mecury
|
|
||||||
Use standard trajectory with 127km/s of departure burn
|
|
||||||
2055-04-17 Mercury Arrival
|
|
||||||
First crewed mission to Mercury
|
|
||||||
2055-04-18 Orbital Survey of Mercury begins
|
|
||||||
2055-06-14 First Crewed Surface Landing on Mercury
|
|
||||||
2055-06-15 EVA operations commence
|
|
||||||
2055-06-30 Surface operations complete, returning to orbit
|
|
||||||
2055-08-15 Departure from Mercury for Venus
|
|
||||||
Use standard trajectory with 85km/s of departure burn
|
|
||||||
2055-10-01 Venus Arrival
|
|
||||||
First crewed mission to Venus, no crewed landing attempted due to extreme surface conditions. Probe deployment only.
|
|
||||||
2055-10-02 Orbital Survey of Venus begins
|
|
||||||
2055-10-10 Landing Probe Deployment
|
|
||||||
2055-11-10 Begin Upper Atmosphere Survey via Callisto MPV
|
|
||||||
2056-03-24 Departure from Venus for Earth
|
|
||||||
Use standard trajectory with 75km/s of departure burn
|
|
||||||
2056-04-12 Earth Arrival
|
|
||||||
|
|
||||||
2056-04-15 Start maintenance
|
|
||||||
2056-05-31 Maintenance complete.
|
|
||||||
|
|
||||||
2056-06-02 Picking up crew for Mars One Construction Mission 2
|
|
||||||
2056-06-04 Picking up Mars One construction cargo
|
|
||||||
2056-06-06 Earth Departure for Mars
|
|
||||||
Use standard trajectory with 126km/s of departure burn
|
|
||||||
2056-07-01 Mars Arrival, First crewed mission to Mars using Stellaria
|
|
||||||
2056-07-03 Crew descend to Martian Surface, Continuing the construction of Mars One Base
|
|
||||||
2056-07-20 Heavy Cargo Delivery to Surface via Callisto MPV
|
|
||||||
2056-09-17 Mars One Construction Mission 3 Crew Arrival via XH-01, joining Stellaria crew. First Crew Handover on Mars.
|
|
||||||
2056-10-12 Mars One Construction Mission 2 Crew ascend to Stellaria
|
|
||||||
2056-10-15 Departure from Mars for Earth
|
|
||||||
Use Fast Return trajectory with 212km/s of departure burn
|
|
||||||
2056-11-03 Earth Arrival
|
|
||||||
|
|
||||||
2056-11-04 Start maintenance
|
|
||||||
2056-12-10 Maintenance complete.
|
|
||||||
|
|
||||||
2056-12-15 Start preparations for Europa Outpost Construction Mission 1
|
|
||||||
2056-12-20 Europa Outpost Construction Package arrival via Qingtian Cargo Shuttle
|
|
||||||
2056-12-31 Picking up crew for Europa Outpost Construction Mission 1
|
|
||||||
2057-01-02 Earth Departure for Europa
|
|
||||||
Use standard trajectory with 165km/s of departure burn
|
|
||||||
2057-02-25 Europa Arrival, First crewed mission to Europa
|
|
||||||
2057-02-27 Heavy Cargo Delivery to Surface via Qingtian Cargo Shuttle
|
|
||||||
2057-03-15 Crew descend to Europan Surface, Beginning construction of Europa Outpost
|
|
||||||
6 crewed ascend and descend operations to Europa Surface via Callisto MPV between 2057-03-15 and 2057-09-15
|
|
||||||
2057-10-01 Europa Outpost Construction Mission 1 Crew ascend to Stellaria, wrapping up construction phase 1
|
|
||||||
2057-10-04 Europa Departure for Earth
|
|
||||||
Use Fast trajectory with 216km/s of departure burn
|
|
||||||
2057-12-20 Earth Arrival
|
|
||||||
2058-01-05 Start maintenance and upgrades, extensive overhauls to bring ST-01 closer to full Stellaria specifications
|
|
||||||
Add more greenhouse modules, upgrade life support systems, add another Rotating Habitat Ring
|
|
||||||
2058-05-15 Maintenance and upgrades complete.
|
|
||||||
|
|
||||||
2058-06-01 Picking up crew for Saturn Exploration Mission 1
|
|
||||||
2058-06-03 Earth Departure for Saturn
|
|
||||||
Use Fast trajectory with 210km/s of departure burn
|
|
||||||
2058-10-12 Saturn Arrival, First crewed mission to Saturn
|
|
||||||
|
|
||||||
2059-05-31 Parked into Iapetus orbit, orbital science completed
|
|
||||||
2059-06-15 Start Iapetus Surface operation
|
|
||||||
2059-07-01 Wrapping up Saturn Exploration Mission. Saturn Departure for Earth
|
|
||||||
Use standard trajectory with 162km/s of departure burn
|
|
||||||
2059-11-24 Back from Saturn, Earth Arrival
|
|
||||||
|
|
||||||
2059-11-25 Parking to Star Port, Starting to upgrade
|
|
||||||
2059-02-10 Upgrades complete, ST-01 now fully meets Stellaria specifications.
|
|
||||||
|
|
||||||
2060-02-11 Out of Star Port, config for Outer Solar System Exploration
|
|
||||||
2060-02-12 Pick up Outer Solar System Exploration Crew 1 and supply
|
|
||||||
2060-02-12 Earth Departure for Neptune
|
|
||||||
160km/s departure burn
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
operating_history = [
|
|
||||||
{
|
|
||||||
"log_name": "Construction",
|
|
||||||
"start_time": "2050-09-15",
|
|
||||||
"end_time": "2053-02-02",
|
|
||||||
"mission_detail": "Construction",
|
|
||||||
"sub_log":
|
|
||||||
[
|
|
||||||
"2050-09-15 First module delivered to LEO via Vulture Shuttle",
|
|
||||||
"2053-02-02 Assembly Halted",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"log_name": "Reconfiguring to ST-01 Stellaria",
|
|
||||||
"start_time": "2053-05-11",
|
|
||||||
"end_time": "2054-4-28",
|
|
||||||
"mission_detail": "Only fitted with the Light Speed Engine. Other systems are still Xihe standard. Awaiting further upgrades.",
|
|
||||||
"sub_log":[]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Trial Run",
|
|
||||||
'start_time': "2054-05-12",
|
|
||||||
'end_time': "2054-8-22",
|
|
||||||
'mission_detail': "Trial Run before commissioning",
|
|
||||||
'sub_log': [
|
|
||||||
"2054-05-12 Begin Trial Run in Earth Orbit, testing all systems",
|
|
||||||
"2054-08-22 Trial Run complete, all systems perform as expected. Including multiple run to GEO and Lunar Orbit before returning to LEO.",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Deep Space Test Mission",
|
|
||||||
'start_time': "2054-09-07",
|
|
||||||
'end_time': "2054-11-28",
|
|
||||||
'mission_detail': "Uncrewed mission to Earth-Sun Lagrange Point 2 to test long-duration operations",
|
|
||||||
'sub_log': [
|
|
||||||
"2054-09-07 Begin Deep Space Test Mission",
|
|
||||||
"2054-11-28 Test Mission Complete, all systems nominal. Returning to LEO.",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Maintenance before Commissioning",
|
|
||||||
'start_time': "2055-12-01",
|
|
||||||
'end_time': "2056-02-28",
|
|
||||||
'mission_detail': "",
|
|
||||||
'sub_log': []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': 'Inner Solar System Exploration Mission 1 outbound to Mecury',
|
|
||||||
'start_time': "2055-03-20",
|
|
||||||
'end_time': "2055-04-17",
|
|
||||||
'mission_detail': "Earth-Mecury transfer, using standard trajectory with 127km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2055-03-20 Picking up crew for Inner Solar System Exploration Mission 1",
|
|
||||||
"2055-03-22 Earth Departure for Mecury",
|
|
||||||
"2055-04-17 Mercury Arrival",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Inner Solar System Exploration Mission 1 Mecury Operations",
|
|
||||||
'start_time': "2055-04-18",
|
|
||||||
'end_time': "2055-06-30",
|
|
||||||
'mission_detail': "First crewed mission to Mercury",
|
|
||||||
'sub_log': [
|
|
||||||
"2055-04-17 Mercury Arrival",
|
|
||||||
"2055-04-18 Orbital Survey of Mercury begins",
|
|
||||||
"2055-06-14 First Crewed Surface Landing on Mercury",
|
|
||||||
"2055-06-15 EVA operations commence",
|
|
||||||
"2055-06-30 Surface operations complete, returning to orbit",
|
|
||||||
"2055-08-15 Departure from Mercury for Venus",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"log_name": "Inner SolarSystem Exploration Mission 1 outbound to Venus",
|
|
||||||
"start_time": "2055-08-15",
|
|
||||||
"end_time": "2055-10-01",
|
|
||||||
"mission_detail": "Mecury-Venus transfer, using standard trajectory with 85km/s of departure burn",
|
|
||||||
"sub_log": [
|
|
||||||
"2055-08-15 Departure from Mercury for Venus",
|
|
||||||
"2055-10-01 Venus Arrival",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': 'Inner Solar System Exploration Mission 1 Venus Operations',
|
|
||||||
'start_time': "2055-10-01",
|
|
||||||
'end_time': "2056-03-24",
|
|
||||||
'mission_detail': "First crewed mission to Venus, no crewed landing attempted due to extreme surface conditions. Probe deployment only.",
|
|
||||||
'sub_log': [
|
|
||||||
"2055-10-01 Venus Arrival",
|
|
||||||
"2055-10-02 Orbital Survey of Venus begins",
|
|
||||||
"2055-10-10 Landing Probe Deployment",
|
|
||||||
"2055-11-10 Begin Upper Atmosphere Survey via Callisto MPV",
|
|
||||||
"2056-03-24 Departure from Venus for Earth",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': 'Inner Solar System Exploration Mission 1 return to Earth',
|
|
||||||
'start_time': "2056-03-24",
|
|
||||||
'end_time': "2056-04-12",
|
|
||||||
'mission_detail': "Venus-Earth transfer, using standard trajectory with 75km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2056-03-24 Departure from Venus for Earth",
|
|
||||||
"2056-04-12 Earth Arrival",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Maintenance",
|
|
||||||
'start_time': "2056-04-15",
|
|
||||||
'end_time': "2056-05-31",
|
|
||||||
'mission_detail': "",
|
|
||||||
'sub_log': []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Mars One Construction Mission 2 outbound to Mars",
|
|
||||||
'start_time': "2056-06-02",
|
|
||||||
'end_time': "2056-07-01",
|
|
||||||
'mission_detail': "Earth-Mars transfer, using standard trajectory with 126km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2056-06-02 Picking up crew for Mars One Construction Mission 2",
|
|
||||||
"2056-06-04 Picking up Mars One construction cargo",
|
|
||||||
"2056-06-06 Earth Departure for Mars",
|
|
||||||
"2056-07-01 Mars Arrival, First crewed mission to Mars using Stellaria",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Mars One Construction Mission 2 Mars Operations",
|
|
||||||
'start_time': "2056-07-03",
|
|
||||||
'end_time': "2056-10-12",
|
|
||||||
'mission_detail': "Continuing the construction of Mars One Base",
|
|
||||||
'sub_log': [
|
|
||||||
"2056-07-03 Crew descend to Martian Surface, Continuing the construction of Mars One Base",
|
|
||||||
"2056-07-20 Heavy Cargo Delivery to Surface via Callisto MPV",
|
|
||||||
"2056-09-17 Mars One Construction Mission 3 Crew Arrival via XH-01, joining Stellaria crew. First Crew Handover on Mars.",
|
|
||||||
"2056-10-12 Mars One Construction Mission 2 Crew ascend to Stellaria",
|
|
||||||
"2056-10-15 Departure from Mars for Earth",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Mars One Construction Mission 2 return to Earth",
|
|
||||||
'start_time': "2056-10-15",
|
|
||||||
'end_time': "2056-11-03",
|
|
||||||
'mission_detail': "Mars-Earth transfer, using Fast Return trajectory with 212km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2056-10-15 Departure from Mars for Earth",
|
|
||||||
"2056-11-03 Earth Arrival",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Maintenance",
|
|
||||||
'start_time': "2056-11-04",
|
|
||||||
'end_time': "2056-12-10",
|
|
||||||
'mission_detail': "",
|
|
||||||
'sub_log': []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Europa Outpost Construction Mission 1 outbound to Europa",
|
|
||||||
'start_time': "2056-12-15",
|
|
||||||
'end_time': "2057-02-25",
|
|
||||||
'mission_detail': "Earth-Europa transfer, using standard trajectory with 165km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2056-12-15 Start preparations for Europa Outpost Construction Mission 1",
|
|
||||||
"2056-12-20 Europa Outpost Construction Package arrival via Qingtian Cargo Shuttle",
|
|
||||||
"2056-12-31 Picking up crew for Europa Outpost Construction Mission 1 ",
|
|
||||||
"2057-01-02 Earth Departure for Europa",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Europa Outpost Construction Mission 1 Europa Operations",
|
|
||||||
'start_time': "2057-02-27",
|
|
||||||
'end_time': "2057-10-01",
|
|
||||||
'mission_detail': "First crewed mission to Europa",
|
|
||||||
'sub_log': [
|
|
||||||
"2057-02-27 Heavy Cargo Delivery to Surface via Qingtian Cargo Shuttle",
|
|
||||||
"2057-03-15 Crew descend to Europan Surface, Beginning construction of Europa Outpost",
|
|
||||||
"6 crewed ascend and descend operations to Europa Surface via Callisto MPV between 2057-03-15 and 2057-09-15",
|
|
||||||
"2057-10-01 Europa Outpost Construction Mission 1 Crew ascend to Stellaria, wrapping up construction phase 1",
|
|
||||||
"2057-10-04 Europa Departure for Earth",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Europa Outpost Construction Mission 1 return to Earth",
|
|
||||||
'start_time': "2057-10-04",
|
|
||||||
'end_time': "2057-12-20",
|
|
||||||
'mission_detail': "Europa-Earth transfer, using Fast trajectory with 216km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2057-10-04 Europa Departure for Earth",
|
|
||||||
"2057-12-20 Earth Arrival",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Maintenance and Upgrades",
|
|
||||||
'start_time': "2058-01-05",
|
|
||||||
'end_time': "2058-05-15",
|
|
||||||
'mission_detail': "Extensive overhauls to bring ST-01 closer to full Stellaria specifications. Add more greenhouse modules, upgrade life support systems, add another Rotating Habitat Ring",
|
|
||||||
'sub_log': []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Saturn Exploration Mission 1 outbound to Saturn",
|
|
||||||
'start_time': "2058-06-01",
|
|
||||||
'end_time': "2058-10-12",
|
|
||||||
'mission_detail': "Earth-Saturn transfer, using Fast trajectory with 210km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2058-06-01 Picking up crew for Saturn Exploration Mission 1",
|
|
||||||
"2058-06-03 Earth Departure for Saturn",
|
|
||||||
"2058-10-12 Saturn Arrival, First crewed mission to Saturn",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Saturn Exploration Mission 1 Saturn Operations",
|
|
||||||
'start_time': "2059-05-31",
|
|
||||||
'end_time': "2059-07-01",
|
|
||||||
'mission_detail': "First crewed mission to Saturn",
|
|
||||||
'sub_log': [
|
|
||||||
"2059-05-31 Parked into Iapetus orbit, orbital science completed",
|
|
||||||
"2059-06-15 Start Iapetus Surface operation",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Saturn Exploration Mission 1 return to Earth",
|
|
||||||
'start_time': "2059-07-01",
|
|
||||||
'end_time': "2059-11-24",
|
|
||||||
'mission_detail': "Saturn-Earth transfer, using standard trajectory with 162km/s of departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2059-07-01 Wrapping up Saturn Exploration Mission. Saturn Departure for Earth",
|
|
||||||
"2059-11-24 Back from Saturn, Earth Arrival",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Upgrades at Star Port",
|
|
||||||
'start_time': "2059-11-25",
|
|
||||||
'end_time': "2060-02-10",
|
|
||||||
'mission_detail': "Upgrades complete, ST-01 now fully meets Stellaria specifications.",
|
|
||||||
'sub_log': []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
'log_name': "Outer Solar System Exploration Mission 1 outbound to Neptune",
|
|
||||||
'start_time': "2060-02-11",
|
|
||||||
'end_time': None,
|
|
||||||
'mission_detail': "Earth-Neptune transfer, using 160km/s departure burn",
|
|
||||||
'sub_log': [
|
|
||||||
"2060-02-11 Out of Star Port, config for Outer Solar System Exploration",
|
|
||||||
"2060-02-12 Pick up Outer Solar System Exploration Crew 1 and supply",
|
|
||||||
"2060-02-12 Earth Departure for Neptune",
|
|
||||||
]
|
|
||||||
},
|
|
||||||
]
|
|
||||||
@@ -0,0 +1,289 @@
|
|||||||
|
# import krpc
|
||||||
|
#
|
||||||
|
# conn = krpc.connect(name='KSP Explore')
|
||||||
|
# vessel = conn.space_center.active_vessel
|
||||||
|
# orbit = vessel.orbit
|
||||||
|
# surface = vessel.surface_velocity_reference_frame
|
||||||
|
# print(f'Vessel Name: {vessel.name}')
|
||||||
|
# print(f'Orbit Apoapsis: {orbit.apoapsis} m')
|
||||||
|
# print(f'Orbit Periapsis: {orbit.periapsis} m')
|
||||||
|
# print(f'Surface Speed: {vessel.surface_velocity(surface).magnitude} m/s')
|
||||||
|
|
||||||
|
import time
|
||||||
|
|
||||||
|
import cvxpy as cp
|
||||||
|
import krpc
|
||||||
|
import numpy as np
|
||||||
|
from krpc.services.spacecenter import ReferenceFrame, Vessel
|
||||||
|
from scipy.spatial.transform import Rotation as R
|
||||||
|
|
||||||
|
from src.optimization import optimize_fuel
|
||||||
|
|
||||||
|
landing_platform = np.array([159780.5, -1018.1, -578410.4])
|
||||||
|
origin = np.array([-0.0021359772614235606, -0.004701372718752435, 0.152749662369653])
|
||||||
|
|
||||||
|
def optimize_fuel(
|
||||||
|
p_target: np.ndarray,
|
||||||
|
g: float,
|
||||||
|
m: float,
|
||||||
|
p0: np.ndarray,
|
||||||
|
v0: np.ndarray,
|
||||||
|
K: int,
|
||||||
|
h: float,
|
||||||
|
F_max: float,
|
||||||
|
alpha: float,
|
||||||
|
gamma: float,
|
||||||
|
**kwargs: dict,
|
||||||
|
) -> Tuple[np.ndarray, np.ndarray, float, cp.Problem]:
|
||||||
|
"""
|
||||||
|
|
||||||
|
Minimize fuel consumption for a rocket to land on a target
|
||||||
|
|
||||||
|
:param p_target: landing target in m
|
||||||
|
:param g: gravitational acceleration in m/s^2
|
||||||
|
:param m: mass in kg
|
||||||
|
:param p0: position in m
|
||||||
|
:param v0: velocity in m/s
|
||||||
|
:param K: Number of discretization steps
|
||||||
|
:param h: discretization step in s
|
||||||
|
:param F_max: maximum thrust of engine in kg*m/s^2 (Newton)
|
||||||
|
:param alpha: Glide path angle in radian
|
||||||
|
:param gamma: converts fuel consumption to liters of fuel consumption
|
||||||
|
:return: position, thrust, fuel consumption, problem
|
||||||
|
"""
|
||||||
|
|
||||||
|
P_min = p_target[2]
|
||||||
|
|
||||||
|
# Variables
|
||||||
|
V = cp.Variable((K + 1, 3)) # velocity
|
||||||
|
P = cp.Variable((K + 1, 3)) # position
|
||||||
|
F = cp.Variable((K, 3)) # thrust
|
||||||
|
|
||||||
|
# Constraints
|
||||||
|
# Match initial position and initial velocity
|
||||||
|
constraints = [
|
||||||
|
V[0] == v0,
|
||||||
|
P[0] == p0,
|
||||||
|
P[:, 2] >= P_min,
|
||||||
|
# P[:, 2] <= P_max,
|
||||||
|
F[:, 2] >= 0,
|
||||||
|
V[:, 2] <= 0,
|
||||||
|
]
|
||||||
|
|
||||||
|
# Match final position and 0 velocity
|
||||||
|
constraints += [
|
||||||
|
V[K] == [0, 0, 0],
|
||||||
|
P[K] == p_target,
|
||||||
|
]
|
||||||
|
|
||||||
|
# Physics dynamics for velocity
|
||||||
|
constraints += [V[1:, :2] == V[:-1, :2] + h * (F[:, :2] / m)]
|
||||||
|
constraints += [V[1:, 2] == V[:-1, 2] + h * (F[:, 2] / m - g)]
|
||||||
|
|
||||||
|
# Physics dynamics for position
|
||||||
|
constraints += [P[1:] == P[:-1] + (h / 2) * (V[:-1] + V[1:])]
|
||||||
|
|
||||||
|
# Maximum thrust constraint
|
||||||
|
constraints += [cp.norm(F, 2, axis=1) <= F_max]
|
||||||
|
|
||||||
|
fuel_consumption = gamma * cp.sum(cp.norm(F, axis=1))
|
||||||
|
|
||||||
|
# Regularization
|
||||||
|
height_regularization = cp.sum(cp.abs(P[:, 2] - P_min))
|
||||||
|
xy_regularization = cp.sum(cp.abs(P[:, :2] - p_target[:2].reshape((1, -1))))
|
||||||
|
glide_violation = cp.pos(np.tan(alpha) * cp.norm(P[:, :2], axis=1) - P[:, 2])
|
||||||
|
glide_violation = cp.sum(glide_violation)
|
||||||
|
gamma_height = 1
|
||||||
|
gamma_xy = 1
|
||||||
|
gamma_glide = 1000
|
||||||
|
reg = (
|
||||||
|
gamma_height * height_regularization
|
||||||
|
+ gamma_xy * xy_regularization
|
||||||
|
+ gamma_glide * glide_violation
|
||||||
|
)
|
||||||
|
|
||||||
|
problem = cp.Problem(cp.Minimize(fuel_consumption + reg), constraints)
|
||||||
|
problem.solve(**kwargs)
|
||||||
|
return P.value, F.value, fuel_consumption.value, problem
|
||||||
|
|
||||||
|
|
||||||
|
def run() -> None:
|
||||||
|
"""
|
||||||
|
The main function containing the MPC loop
|
||||||
|
"""
|
||||||
|
|
||||||
|
conn = krpc.connect(name="Landing")
|
||||||
|
vessel = conn.space_center.active_vessel
|
||||||
|
print(f"the vessel's name is: {vessel.name}")
|
||||||
|
|
||||||
|
ref_frame = get_reference_frame(conn, vessel)
|
||||||
|
|
||||||
|
position = conn.add_stream(vessel.position, ref_frame)
|
||||||
|
velocity = conn.add_stream(vessel.velocity, ref_frame)
|
||||||
|
|
||||||
|
relative_frame = ref_frame.create_relative(
|
||||||
|
ref_frame, rotation=R.from_euler("yz", [-90, -90], degrees=True).as_quat()
|
||||||
|
)
|
||||||
|
vessel.auto_pilot.reference_frame = relative_frame
|
||||||
|
vessel.auto_pilot.target_pitch_and_heading(60, 0)
|
||||||
|
|
||||||
|
launch_rocket(vessel)
|
||||||
|
|
||||||
|
while True:
|
||||||
|
|
||||||
|
time.sleep(0.1)
|
||||||
|
|
||||||
|
p = np.array(position())
|
||||||
|
v = np.array(velocity())
|
||||||
|
max_thrust = vessel.max_thrust
|
||||||
|
|
||||||
|
if vessel.situation.name == "landed":
|
||||||
|
vessel.control.throttle = 0
|
||||||
|
break
|
||||||
|
|
||||||
|
# Update the glide angle
|
||||||
|
alpha = np.arctan(p[2] * 1.05 / np.linalg.norm(p[:2]))
|
||||||
|
|
||||||
|
try:
|
||||||
|
P, F, _, _ = optimize_fuel(
|
||||||
|
origin,
|
||||||
|
9.81,
|
||||||
|
vessel.mass,
|
||||||
|
p,
|
||||||
|
v,
|
||||||
|
100,
|
||||||
|
1,
|
||||||
|
max_thrust,
|
||||||
|
alpha,
|
||||||
|
1,
|
||||||
|
)
|
||||||
|
except (cp.SolverError, AssertionError) as e:
|
||||||
|
print(e)
|
||||||
|
if v[2] > 0:
|
||||||
|
vessel.control.throttle = 0
|
||||||
|
continue
|
||||||
|
|
||||||
|
if F is None:
|
||||||
|
print("no solution found")
|
||||||
|
vessel.control.throttle = 0
|
||||||
|
continue
|
||||||
|
else:
|
||||||
|
target_F = F[0]
|
||||||
|
thrust_level = np.linalg.norm(target_F) / max_thrust
|
||||||
|
|
||||||
|
direction = target_F / np.linalg.norm(target_F)
|
||||||
|
xy_len = np.linalg.norm(direction[[0, 1]])
|
||||||
|
|
||||||
|
# pitch between -90 and 90
|
||||||
|
target_pitch = np.arctan2(direction[2], xy_len) * 180 / np.pi
|
||||||
|
|
||||||
|
# heading between 0 and 360
|
||||||
|
target_heading = (np.arctan2(direction[1], direction[0]) * 180 / np.pi) % 360
|
||||||
|
|
||||||
|
vessel.auto_pilot.target_pitch_and_heading(target_pitch, target_heading)
|
||||||
|
vessel.auto_pilot.engage()
|
||||||
|
|
||||||
|
if thrust_level < 0.01:
|
||||||
|
thrust_level = 0
|
||||||
|
vessel.control.throttle = thrust_level
|
||||||
|
|
||||||
|
conn.drawing.clear()
|
||||||
|
draw_trajectory(conn, P, ref_frame)
|
||||||
|
draw_F(conn, target_F, ref_frame, vessel.reference_frame)
|
||||||
|
draw_autopilot_direction(
|
||||||
|
conn, vessel.auto_pilot.target_direction, relative_frame, vessel.reference_frame
|
||||||
|
)
|
||||||
|
|
||||||
|
print("thrust: ", round(thrust_level, 2))
|
||||||
|
|
||||||
|
conn.close()
|
||||||
|
|
||||||
|
|
||||||
|
def draw_trajectory(conn: krpc.Client, P: np.ndarray, ref_frame: ReferenceFrame) -> None:
|
||||||
|
"""
|
||||||
|
Draw the next 10 points of the trajectory
|
||||||
|
"""
|
||||||
|
for i in range(1, P.shape[0]):
|
||||||
|
p = P[i]
|
||||||
|
p0 = P[i - 1]
|
||||||
|
conn.drawing.add_line(p0, p, ref_frame)
|
||||||
|
if i > 10:
|
||||||
|
break
|
||||||
|
|
||||||
|
|
||||||
|
def draw_F(
|
||||||
|
conn: krpc.Client,
|
||||||
|
target_F: np.ndarray,
|
||||||
|
ref_frame: ReferenceFrame,
|
||||||
|
vessel_ref_frame: ReferenceFrame,
|
||||||
|
) -> None:
|
||||||
|
"""
|
||||||
|
Draw the target force vector in blue
|
||||||
|
"""
|
||||||
|
transformed_F = conn.space_center.transform_direction(target_F, ref_frame, vessel_ref_frame)
|
||||||
|
line = conn.drawing.add_line([0, 0, 0], transformed_F, vessel_ref_frame)
|
||||||
|
line.color = (0, 0, 1)
|
||||||
|
|
||||||
|
|
||||||
|
def draw_autopilot_direction(
|
||||||
|
conn: krpc.Client,
|
||||||
|
direction: np.ndarray,
|
||||||
|
ref_frame: ReferenceFrame,
|
||||||
|
vessel_ref_frame: ReferenceFrame,
|
||||||
|
) -> None:
|
||||||
|
"""
|
||||||
|
Draw the autopilot direction in red
|
||||||
|
"""
|
||||||
|
transformed_direction = conn.space_center.transform_direction(
|
||||||
|
direction, ref_frame, vessel_ref_frame
|
||||||
|
)
|
||||||
|
line = conn.drawing.add_line([0, 0, 0], np.array(transformed_direction) * 100, vessel_ref_frame)
|
||||||
|
line.color = (1, 0, 0)
|
||||||
|
|
||||||
|
|
||||||
|
def draw_coordinate_system(conn: krpc.Client, ref_frame: ReferenceFrame) -> None:
|
||||||
|
"""
|
||||||
|
Draw the coordinate system of the reference frame
|
||||||
|
"""
|
||||||
|
line = conn.drawing.add_line([0, 0, 0], [10, 0, 0], ref_frame)
|
||||||
|
line.color = (1, 0, 0)
|
||||||
|
line = conn.drawing.add_line([0, 0, 0], [0, 10, 0], ref_frame)
|
||||||
|
line.color = (0, 1, 0)
|
||||||
|
line = conn.drawing.add_line([0, 0, 0], [0, 0, 10], ref_frame)
|
||||||
|
line.color = (0, 0, 1)
|
||||||
|
|
||||||
|
|
||||||
|
def launch_rocket(vessel: Vessel) -> None:
|
||||||
|
"""
|
||||||
|
Launch the rocket on a hard-coded trajectory
|
||||||
|
"""
|
||||||
|
vessel.auto_pilot.engage()
|
||||||
|
vessel.control.throttle = 1
|
||||||
|
vessel.control.activate_next_stage()
|
||||||
|
time.sleep(2)
|
||||||
|
vessel.auto_pilot.target_pitch_and_heading(60, 180)
|
||||||
|
time.sleep(6)
|
||||||
|
vessel.control.throttle = 0
|
||||||
|
|
||||||
|
|
||||||
|
def get_reference_frame(conn: krpc.Client, vessel: Vessel) -> ReferenceFrame:
|
||||||
|
"""
|
||||||
|
Get the reference frame of the landing platform
|
||||||
|
"""
|
||||||
|
|
||||||
|
z = landing_platform
|
||||||
|
z = z / np.linalg.norm(z)
|
||||||
|
x = np.array([1, 0, 0])
|
||||||
|
y = np.cross(z, x)
|
||||||
|
y = y / np.linalg.norm(y)
|
||||||
|
x = np.cross(y, z)
|
||||||
|
x = x / np.linalg.norm(x)
|
||||||
|
q = R.from_matrix(np.array([x, y, z]).T).as_quat()
|
||||||
|
ref_frame = conn.space_center.ReferenceFrame.create_relative(
|
||||||
|
vessel.orbit.body.reference_frame, position=landing_platform, rotation=q
|
||||||
|
)
|
||||||
|
return ref_frame
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
run()
|
||||||
Reference in New Issue
Block a user