gfold 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- gfold/__init__.py +6 -0
- gfold/cli.py +49 -0
- gfold/config.py +235 -0
- gfold/solver.py +242 -0
- gfold/visualization.py +90 -0
- gfold-0.1.0.dist-info/METADATA +105 -0
- gfold-0.1.0.dist-info/RECORD +10 -0
- gfold-0.1.0.dist-info/WHEEL +5 -0
- gfold-0.1.0.dist-info/entry_points.txt +2 -0
- gfold-0.1.0.dist-info/top_level.txt +1 -0
gfold/__init__.py
ADDED
gfold/cli.py
ADDED
|
@@ -0,0 +1,49 @@
|
|
|
1
|
+
import argparse
|
|
2
|
+
import os
|
|
3
|
+
|
|
4
|
+
from .config import GFoldConfig
|
|
5
|
+
from .solver import GFoldSolver
|
|
6
|
+
from .visualization import plot_results
|
|
7
|
+
|
|
8
|
+
def main():
|
|
9
|
+
"""Command-line interface for the G-FOLD solver."""
|
|
10
|
+
parser = argparse.ArgumentParser(description="G-FOLD: Fuel Optimal Large Divert Guidance Algorithm")
|
|
11
|
+
parser.add_argument('-n', type=int,
|
|
12
|
+
help="Determines the amount of steps and thus determines dt (dt = tf/N)",
|
|
13
|
+
default=100)
|
|
14
|
+
parser.add_argument('-g', '--generate',
|
|
15
|
+
help="Generate C++/Python code to solve problems faster",
|
|
16
|
+
action="store_true")
|
|
17
|
+
parser.add_argument('-o', '--output',
|
|
18
|
+
help="Output directory for generated code or plot",
|
|
19
|
+
default=".")
|
|
20
|
+
parser.add_argument('--no-plot',
|
|
21
|
+
help="Don't display the plot",
|
|
22
|
+
action="store_true")
|
|
23
|
+
parser.add_argument('-s', '--save-plot',
|
|
24
|
+
help="Save the plot to a file (default: don't save)",
|
|
25
|
+
action="store_true")
|
|
26
|
+
|
|
27
|
+
args = parser.parse_args()
|
|
28
|
+
|
|
29
|
+
# Initialize solver
|
|
30
|
+
solver = GFoldSolver(GFoldConfig(n=args.n))
|
|
31
|
+
|
|
32
|
+
# Generate code if requested
|
|
33
|
+
if args.generate:
|
|
34
|
+
output_dir = os.path.join(args.output, "code")
|
|
35
|
+
code_dir = solver.generate_code(code_dir=output_dir)
|
|
36
|
+
print(f"Generated code in {code_dir}")
|
|
37
|
+
return
|
|
38
|
+
|
|
39
|
+
# Otherwise solve and visualize
|
|
40
|
+
print("Solving G-FOLD optimization problem...")
|
|
41
|
+
solution = solver.solve(verbose=True)
|
|
42
|
+
print(f"Final mass: {solution['final_mass']:.2f} kg")
|
|
43
|
+
|
|
44
|
+
# Plot results
|
|
45
|
+
save_path = os.path.join(args.output, "gfold_plot.png") if args.save_plot else None
|
|
46
|
+
plot_results(solution, save_path=save_path, show=not args.no_plot)
|
|
47
|
+
|
|
48
|
+
if __name__ == "__main__":
|
|
49
|
+
main()
|
gfold/config.py
ADDED
|
@@ -0,0 +1,235 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
|
|
3
|
+
class SpacecraftConfig:
|
|
4
|
+
"""Configuration for spacecraft parameters."""
|
|
5
|
+
|
|
6
|
+
def __init__(self, **kwargs):
|
|
7
|
+
"""
|
|
8
|
+
Initialize spacecraft configuration with default values.
|
|
9
|
+
|
|
10
|
+
Args:
|
|
11
|
+
**kwargs: Any parameter can be overridden during initialization
|
|
12
|
+
"""
|
|
13
|
+
# Mass parameters
|
|
14
|
+
self.wet_mass = 2000 # wet mass of the rocket (kg)
|
|
15
|
+
self.fuel = 1700 # weight of fuel (kg)
|
|
16
|
+
|
|
17
|
+
# Thrust parameters
|
|
18
|
+
self.real_max_thrust = 24000 # maximum possible thrust (N)
|
|
19
|
+
self.min_thrust_pct = 0.2 # percentage of max thrust for minimum
|
|
20
|
+
self.max_thrust_pct = 0.8 # percentage of max thrust for maximum
|
|
21
|
+
|
|
22
|
+
# Motion constraints
|
|
23
|
+
self.max_velocity = 1000 # maximum velocity (m/s)
|
|
24
|
+
|
|
25
|
+
# Initial conditions
|
|
26
|
+
self.initial_position = [450, -330, 2400] # (m)
|
|
27
|
+
self.initial_velocity = [-40, 10, -10] # (m/s)
|
|
28
|
+
self.target_velocity = [0, 0, 0] # (m/s)
|
|
29
|
+
self.target_position = [0, 0, 0] # (m)
|
|
30
|
+
|
|
31
|
+
# Physics parameters
|
|
32
|
+
self.fuel_consumption = 5e-4 # fuel consumption rate (kg/N/s)
|
|
33
|
+
|
|
34
|
+
self._update_from_kwargs(kwargs)
|
|
35
|
+
|
|
36
|
+
def _update_from_kwargs(self, kwargs):
|
|
37
|
+
"""Update attributes from keyword arguments."""
|
|
38
|
+
for key, value in kwargs.items():
|
|
39
|
+
if hasattr(self, key):
|
|
40
|
+
setattr(self, key, value)
|
|
41
|
+
else:
|
|
42
|
+
raise ValueError(f"Unknown spacecraft parameter: {key}")
|
|
43
|
+
|
|
44
|
+
@property
|
|
45
|
+
def log_wet_mass(self):
|
|
46
|
+
"""Natural logarithm of wet mass."""
|
|
47
|
+
return np.log(self.wet_mass)
|
|
48
|
+
|
|
49
|
+
@property
|
|
50
|
+
def log_dry_mass(self):
|
|
51
|
+
"""Natural logarithm of dry mass."""
|
|
52
|
+
return np.log(self.wet_mass - self.fuel)
|
|
53
|
+
|
|
54
|
+
@property
|
|
55
|
+
def min_thrust(self):
|
|
56
|
+
"""Minimum thrust value."""
|
|
57
|
+
return self.real_max_thrust * self.min_thrust_pct
|
|
58
|
+
|
|
59
|
+
@property
|
|
60
|
+
def max_thrust(self):
|
|
61
|
+
"""Maximum thrust value."""
|
|
62
|
+
return self.real_max_thrust * self.max_thrust_pct
|
|
63
|
+
|
|
64
|
+
def to_dict(self):
|
|
65
|
+
"""Convert configuration to dictionary."""
|
|
66
|
+
return {k: v for k, v in self.__dict__.items() if not k.startswith('_')}
|
|
67
|
+
|
|
68
|
+
|
|
69
|
+
class EnvironmentConfig:
|
|
70
|
+
"""Configuration for environment parameters."""
|
|
71
|
+
|
|
72
|
+
def __init__(self, **kwargs):
|
|
73
|
+
"""
|
|
74
|
+
Initialize environment configuration with default Mars values.
|
|
75
|
+
|
|
76
|
+
Args:
|
|
77
|
+
**kwargs: Any parameter can be overridden during initialization
|
|
78
|
+
"""
|
|
79
|
+
# Gravitational parameters
|
|
80
|
+
self.gravity = [0, 0, -3.71] # gravity vector (m/s²), default is Mars
|
|
81
|
+
|
|
82
|
+
# Landing constraints
|
|
83
|
+
self.glide_slope_angle = 0 # in degrees
|
|
84
|
+
self.max_angle = 90 # maximum angle for approach (degrees)
|
|
85
|
+
|
|
86
|
+
self._update_from_kwargs(kwargs)
|
|
87
|
+
|
|
88
|
+
def _update_from_kwargs(self, kwargs):
|
|
89
|
+
"""Update attributes from keyword arguments."""
|
|
90
|
+
for key, value in kwargs.items():
|
|
91
|
+
if hasattr(self, key):
|
|
92
|
+
setattr(self, key, value)
|
|
93
|
+
else:
|
|
94
|
+
raise ValueError(f"Unknown environment parameter: {key}")
|
|
95
|
+
|
|
96
|
+
@property
|
|
97
|
+
def sin_glide_slope(self):
|
|
98
|
+
"""Sine of the glide slope angle."""
|
|
99
|
+
return np.sin(np.radians(self.glide_slope_angle))
|
|
100
|
+
|
|
101
|
+
@property
|
|
102
|
+
def cos_max_angle(self):
|
|
103
|
+
"""Cosine of the maximum angle."""
|
|
104
|
+
return np.cos(np.radians(self.max_angle))
|
|
105
|
+
|
|
106
|
+
@classmethod
|
|
107
|
+
def mars(cls):
|
|
108
|
+
"""Create Mars environment configuration."""
|
|
109
|
+
return cls()
|
|
110
|
+
|
|
111
|
+
@classmethod
|
|
112
|
+
def moon(cls):
|
|
113
|
+
"""Create Moon environment configuration."""
|
|
114
|
+
return cls(gravity=[0, 0, -1.62])
|
|
115
|
+
|
|
116
|
+
@classmethod
|
|
117
|
+
def earth(cls):
|
|
118
|
+
"""Create Earth environment configuration."""
|
|
119
|
+
return cls(gravity=[0, 0, -9.81])
|
|
120
|
+
|
|
121
|
+
def to_dict(self):
|
|
122
|
+
"""Convert configuration to dictionary."""
|
|
123
|
+
return {k: v for k, v in self.__dict__.items() if not k.startswith('_')}
|
|
124
|
+
|
|
125
|
+
|
|
126
|
+
class SolverConfig:
|
|
127
|
+
"""Configuration for solver parameters."""
|
|
128
|
+
|
|
129
|
+
def __init__(self, **kwargs):
|
|
130
|
+
"""
|
|
131
|
+
Initialize solver configuration with default values.
|
|
132
|
+
|
|
133
|
+
Args:
|
|
134
|
+
**kwargs: Any parameter can be overridden during initialization
|
|
135
|
+
"""
|
|
136
|
+
# Solver parameters
|
|
137
|
+
self.n = 100 # number of discrete timesteps
|
|
138
|
+
self.time_of_flight = 44.63 # pre-calculated time of flight (s)
|
|
139
|
+
|
|
140
|
+
self._update_from_kwargs(kwargs)
|
|
141
|
+
|
|
142
|
+
def _update_from_kwargs(self, kwargs):
|
|
143
|
+
"""Update attributes from keyword arguments."""
|
|
144
|
+
for key, value in kwargs.items():
|
|
145
|
+
if hasattr(self, key):
|
|
146
|
+
setattr(self, key, value)
|
|
147
|
+
else:
|
|
148
|
+
raise ValueError(f"Unknown solver parameter: {key}")
|
|
149
|
+
|
|
150
|
+
def to_dict(self):
|
|
151
|
+
"""Convert configuration to dictionary."""
|
|
152
|
+
return {k: v for k, v in self.__dict__.items() if not k.startswith('_')}
|
|
153
|
+
|
|
154
|
+
|
|
155
|
+
class GFoldConfig:
|
|
156
|
+
"""Main configuration class for G-FOLD solver parameters."""
|
|
157
|
+
|
|
158
|
+
def __init__(self, spacecraft=None, environment=None, solver=None, **kwargs):
|
|
159
|
+
"""
|
|
160
|
+
Initialize configuration with default values that can be overridden.
|
|
161
|
+
|
|
162
|
+
Args:
|
|
163
|
+
spacecraft (SpacecraftConfig): Spacecraft configuration
|
|
164
|
+
environment (EnvironmentConfig): Environment configuration
|
|
165
|
+
solver (SolverConfig): Solver configuration
|
|
166
|
+
**kwargs: Any parameter can be overridden during initialization
|
|
167
|
+
"""
|
|
168
|
+
# Initialize sub-configurations
|
|
169
|
+
self.spacecraft = spacecraft if spacecraft is not None else SpacecraftConfig()
|
|
170
|
+
self.environment = environment if environment is not None else EnvironmentConfig.mars()
|
|
171
|
+
self.solver = solver if solver is not None else SolverConfig()
|
|
172
|
+
|
|
173
|
+
# Process any remaining kwargs by trying to assign them to the appropriate sub-config
|
|
174
|
+
self._process_kwargs(kwargs)
|
|
175
|
+
|
|
176
|
+
def _process_kwargs(self, kwargs):
|
|
177
|
+
"""Process keyword arguments and assign to appropriate sub-config."""
|
|
178
|
+
for key, value in kwargs.items():
|
|
179
|
+
assigned = False
|
|
180
|
+
|
|
181
|
+
for config_name in ['spacecraft', 'environment', 'solver']:
|
|
182
|
+
config = getattr(self, config_name)
|
|
183
|
+
print(config.to_dict())
|
|
184
|
+
if hasattr(config, key):
|
|
185
|
+
setattr(config, key, value)
|
|
186
|
+
assigned = True
|
|
187
|
+
break
|
|
188
|
+
|
|
189
|
+
if not assigned:
|
|
190
|
+
raise ValueError(f"Unknown configuration parameter: {key}")
|
|
191
|
+
|
|
192
|
+
# Properties to maintain backward compatibility
|
|
193
|
+
@property
|
|
194
|
+
def log_wet_mass(self):
|
|
195
|
+
return self.spacecraft.log_wet_mass
|
|
196
|
+
|
|
197
|
+
@property
|
|
198
|
+
def log_dry_mass(self):
|
|
199
|
+
return self.spacecraft.log_dry_mass
|
|
200
|
+
|
|
201
|
+
@property
|
|
202
|
+
def sin_glide_slope(self):
|
|
203
|
+
return self.environment.sin_glide_slope
|
|
204
|
+
|
|
205
|
+
@property
|
|
206
|
+
def cos_max_angle(self):
|
|
207
|
+
return self.environment.cos_max_angle
|
|
208
|
+
|
|
209
|
+
@property
|
|
210
|
+
def min_thrust(self):
|
|
211
|
+
return self.spacecraft.min_thrust
|
|
212
|
+
|
|
213
|
+
@property
|
|
214
|
+
def max_thrust(self):
|
|
215
|
+
return self.spacecraft.max_thrust
|
|
216
|
+
|
|
217
|
+
@property
|
|
218
|
+
def time_of_flight(self):
|
|
219
|
+
return self.solver.time_of_flight
|
|
220
|
+
|
|
221
|
+
@property
|
|
222
|
+
def wet_mass(self):
|
|
223
|
+
return self.spacecraft.wet_mass
|
|
224
|
+
|
|
225
|
+
@property
|
|
226
|
+
def real_max_thrust(self):
|
|
227
|
+
return self.spacecraft.real_max_thrust
|
|
228
|
+
|
|
229
|
+
def to_dict(self):
|
|
230
|
+
"""Convert all configuration to a flat dictionary."""
|
|
231
|
+
result = {}
|
|
232
|
+
for config_name in ['spacecraft', 'environment', 'solver']:
|
|
233
|
+
config = getattr(self, config_name)
|
|
234
|
+
result.update(config.to_dict())
|
|
235
|
+
return result
|
gfold/solver.py
ADDED
|
@@ -0,0 +1,242 @@
|
|
|
1
|
+
import cvxpy as cp
|
|
2
|
+
import numpy as np
|
|
3
|
+
import os
|
|
4
|
+
from cvxpygen import cpg
|
|
5
|
+
from .config import GFoldConfig
|
|
6
|
+
|
|
7
|
+
class GFoldSolver:
|
|
8
|
+
"""
|
|
9
|
+
G-FOLD solver that implements the Fuel Optimal Large Divert Guidance Algorithm.
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
def __init__(self, config=None):
|
|
13
|
+
"""
|
|
14
|
+
Initialize the G-FOLD solver.
|
|
15
|
+
|
|
16
|
+
Args:
|
|
17
|
+
config (GFoldConfig): Configuration object with problem parameters
|
|
18
|
+
"""
|
|
19
|
+
self.config = config if config is not None else GFoldConfig()
|
|
20
|
+
|
|
21
|
+
self.parameters = {}
|
|
22
|
+
self.variables = {}
|
|
23
|
+
self.constraints = []
|
|
24
|
+
self.problem = None
|
|
25
|
+
self._setup_problem()
|
|
26
|
+
|
|
27
|
+
def _calculate_parameter(self, expression, *args, **kwargs):
|
|
28
|
+
"""Helper function to create parameters from expressions with values"""
|
|
29
|
+
return cp.Parameter(*args, value=expression.value, **kwargs)
|
|
30
|
+
|
|
31
|
+
def _setup_problem(self):
|
|
32
|
+
"""Set up the G-FOLD optimization problem."""
|
|
33
|
+
config = self.config
|
|
34
|
+
n = config.solver.n
|
|
35
|
+
t = config.solver.time_of_flight
|
|
36
|
+
|
|
37
|
+
# Variables
|
|
38
|
+
x = cp.Variable((n, 6), "x") # position(3), speed(3)
|
|
39
|
+
u = cp.Variable((n, 3), "u") # acc due to rocket engine
|
|
40
|
+
s = cp.Variable(n, "s") # slack variable, equal to |u|
|
|
41
|
+
z = cp.Variable(n, "z") # ln(mass)
|
|
42
|
+
|
|
43
|
+
self.variables = {
|
|
44
|
+
"x": x,
|
|
45
|
+
"u": u,
|
|
46
|
+
"s": s,
|
|
47
|
+
"z": z
|
|
48
|
+
}
|
|
49
|
+
|
|
50
|
+
# Parameters
|
|
51
|
+
log_mass = cp.Parameter(name="log_mass", value=config.spacecraft.log_wet_mass)
|
|
52
|
+
max_vel = cp.Parameter(name="max_vel", value=config.spacecraft.max_velocity)
|
|
53
|
+
sin_glide_slope = cp.Parameter(name="sin_glide_slope", nonneg=True, value=config.environment.sin_glide_slope)
|
|
54
|
+
log_dry_mass = cp.Parameter(name="log_dry_mass", value=config.spacecraft.log_dry_mass)
|
|
55
|
+
min_t = cp.Parameter(nonneg=True, name="min_thrust", value=config.spacecraft.min_thrust)
|
|
56
|
+
max_t = cp.Parameter(nonneg=True, name="max_thrust", value=config.spacecraft.max_thrust)
|
|
57
|
+
dt = cp.Parameter(name="dt", value=t/n)
|
|
58
|
+
a = cp.Parameter(name="fuel_consumption", value=config.spacecraft.fuel_consumption)
|
|
59
|
+
a_dt = self._calculate_parameter(a*dt, name="fuel_consumption_dt")
|
|
60
|
+
|
|
61
|
+
# Derived parameters
|
|
62
|
+
z0 = cp.Parameter(n, name="z0")
|
|
63
|
+
exp_z0 = cp.Parameter(n, name="exp_z0", nonneg=True)
|
|
64
|
+
max_exp = cp.Parameter(n, name="max_exp_z0", nonneg=True)
|
|
65
|
+
min_exp = cp.Parameter(n, name="min_exp_z0", nonneg=True)
|
|
66
|
+
|
|
67
|
+
# Calculate values
|
|
68
|
+
c_z0 = []
|
|
69
|
+
c_exp_z0 = []
|
|
70
|
+
c_max_exp = []
|
|
71
|
+
c_min_exp = []
|
|
72
|
+
|
|
73
|
+
for i in range(n):
|
|
74
|
+
z00 = np.log(config.spacecraft.wet_mass - a.value*dt.value*max_t.value*i)
|
|
75
|
+
c_z0.append(z00)
|
|
76
|
+
c_exp_z0.append(np.exp(-z00))
|
|
77
|
+
c_max_exp.append(1/(np.exp(-z00) * max_t.value))
|
|
78
|
+
if min_t.value != 0:
|
|
79
|
+
c_min_exp.append(1/(np.exp(-z00) * min_t.value))
|
|
80
|
+
|
|
81
|
+
z0.value = c_z0
|
|
82
|
+
exp_z0.value = c_exp_z0
|
|
83
|
+
max_exp.value = c_max_exp
|
|
84
|
+
min_exp.value = c_min_exp
|
|
85
|
+
|
|
86
|
+
# More parameters
|
|
87
|
+
max_angle = cp.Parameter(name="max_angle", value=config.environment.cos_max_angle)
|
|
88
|
+
dt_squared = self._calculate_parameter(dt**2, name="dt_squared")
|
|
89
|
+
initial_pos = cp.Parameter(3, name="initial_position", value=config.spacecraft.initial_position)
|
|
90
|
+
initial_vel = cp.Parameter(3, name="initial_vel", value=config.spacecraft.initial_velocity)
|
|
91
|
+
target_vel = cp.Parameter(3, name="target_velocity", value=config.spacecraft.target_velocity)
|
|
92
|
+
g = cp.Parameter(3, name="gravity", value=config.environment.gravity)
|
|
93
|
+
g_dt = self._calculate_parameter(g*dt, 3, name="gravity_dt")
|
|
94
|
+
g_dt_sq = self._calculate_parameter(g*dt*dt, 3, name="gravity_dt_squared")
|
|
95
|
+
|
|
96
|
+
# Store parameters
|
|
97
|
+
self.parameters = {
|
|
98
|
+
"log_mass": log_mass,
|
|
99
|
+
"max_vel": max_vel,
|
|
100
|
+
"sin_glide_slope": sin_glide_slope,
|
|
101
|
+
"log_dry_mass": log_dry_mass,
|
|
102
|
+
"min_thrust": min_t,
|
|
103
|
+
"max_thrust": max_t,
|
|
104
|
+
"dt": dt,
|
|
105
|
+
"fuel_consumption": a,
|
|
106
|
+
"fuel_consumption_dt": a_dt,
|
|
107
|
+
"z0": z0,
|
|
108
|
+
"exp_z0": exp_z0,
|
|
109
|
+
"max_exp_z0": max_exp,
|
|
110
|
+
"min_exp_z0": min_exp,
|
|
111
|
+
"max_angle": max_angle,
|
|
112
|
+
"dt_squared": dt_squared,
|
|
113
|
+
"initial_position": initial_pos,
|
|
114
|
+
"initial_vel": initial_vel,
|
|
115
|
+
"target_velocity": target_vel,
|
|
116
|
+
"gravity": g,
|
|
117
|
+
"gravity_dt": g_dt,
|
|
118
|
+
"gravity_dt_squared": g_dt_sq,
|
|
119
|
+
}
|
|
120
|
+
|
|
121
|
+
# Constraints
|
|
122
|
+
constraints = [
|
|
123
|
+
x[0, :3] == initial_pos,
|
|
124
|
+
x[0, 3:] == initial_vel,
|
|
125
|
+
z[0] == log_mass,
|
|
126
|
+
]
|
|
127
|
+
|
|
128
|
+
# Timestep constraints
|
|
129
|
+
for i in range(n):
|
|
130
|
+
constraints.append(cp.norm(x[i, 3:]) <= max_vel) # never exceed the maximum velocity
|
|
131
|
+
constraints.append(x[i, 2] >= cp.norm(x[i, :3]) * sin_glide_slope) # glide slope constraint
|
|
132
|
+
constraints.append(s[i] >= cp.norm(u[i, :])) # |u| = s
|
|
133
|
+
constraints.append((1 - (z[i]-z0[i]) + cp.square(z[i]-z0[i])/2) <= s[i] * min_exp[i])
|
|
134
|
+
constraints.append(s[i] * max_exp[i] <= (1 - (z[i]-z0[i]))) # upper bound for s
|
|
135
|
+
if i != n - 1:
|
|
136
|
+
acc = (u[i+1, :] + u[i, :])/2
|
|
137
|
+
constraints += [
|
|
138
|
+
x[i+1, :3] == x[i, :3] + (x[i, 3:] + x[i+1, 3:]) * dt / 2 + (acc*dt_squared+g_dt_sq) * (1/2), # position update
|
|
139
|
+
x[i+1, 3:] == x[i, 3:] + acc*dt + g_dt, # velocity update
|
|
140
|
+
z[i+1] == z[i] - (s[i] + s[i+1]) / 2 * a_dt # mass update
|
|
141
|
+
]
|
|
142
|
+
|
|
143
|
+
# Constraints on the last step
|
|
144
|
+
constraints += [
|
|
145
|
+
x[n-1, :3] == config.spacecraft.target_position, # landing site
|
|
146
|
+
x[n-1, 3:] == target_vel,
|
|
147
|
+
z[n-1] >= log_dry_mass,
|
|
148
|
+
]
|
|
149
|
+
|
|
150
|
+
self.constraints = constraints
|
|
151
|
+
|
|
152
|
+
# Objective: maximize final mass
|
|
153
|
+
obj = cp.Maximize(z[n-1])
|
|
154
|
+
self.problem = cp.Problem(obj, constraints)
|
|
155
|
+
|
|
156
|
+
def solve(self, verbose=False):
|
|
157
|
+
"""
|
|
158
|
+
Solve the G-FOLD optimization problem.
|
|
159
|
+
|
|
160
|
+
Args:
|
|
161
|
+
verbose (bool): Whether to print verbose output
|
|
162
|
+
|
|
163
|
+
Returns:
|
|
164
|
+
dict: Solution containing positions, velocities, thrusts, and other data
|
|
165
|
+
"""
|
|
166
|
+
if not self.problem:
|
|
167
|
+
raise ValueError("Problem not initialized properly")
|
|
168
|
+
|
|
169
|
+
solution_val = self.problem.solve(verbose=verbose)
|
|
170
|
+
|
|
171
|
+
# Extract solution data
|
|
172
|
+
x_val = self.variables["x"].value
|
|
173
|
+
u_val = self.variables["u"].value
|
|
174
|
+
z_val = self.variables["z"].value
|
|
175
|
+
s_val = self.variables["s"].value
|
|
176
|
+
|
|
177
|
+
positions = x_val[:, :3]
|
|
178
|
+
velocities = x_val[:, 3:]
|
|
179
|
+
thrusts = np.array([np.linalg.norm(u) for u in u_val])
|
|
180
|
+
|
|
181
|
+
# Adjust thrust for mass
|
|
182
|
+
for i in range(self.config.solver.n):
|
|
183
|
+
thrusts[i] *= np.exp(z_val[i])
|
|
184
|
+
|
|
185
|
+
return {
|
|
186
|
+
"solution_value": solution_val,
|
|
187
|
+
"positions": positions,
|
|
188
|
+
"velocities": velocities,
|
|
189
|
+
"thrusts": thrusts,
|
|
190
|
+
"normalized_thrusts": thrusts / self.config.spacecraft.real_max_thrust,
|
|
191
|
+
"final_mass": np.exp(z_val[-1]),
|
|
192
|
+
"z_values": z_val,
|
|
193
|
+
"x_values": x_val,
|
|
194
|
+
"u_values": u_val,
|
|
195
|
+
"s_values": s_val,
|
|
196
|
+
"time_points": np.arange(0, self.parameters["dt"].value * self.config.solver.n, self.parameters["dt"].value)
|
|
197
|
+
}
|
|
198
|
+
|
|
199
|
+
def generate_code(self, code_dir="code"):
|
|
200
|
+
"""
|
|
201
|
+
Generate C++/Python code for the solver using cvxpygen.
|
|
202
|
+
|
|
203
|
+
Args:
|
|
204
|
+
code_dir (str): Directory to save the generated code
|
|
205
|
+
|
|
206
|
+
Returns:
|
|
207
|
+
str: Path to the generated code
|
|
208
|
+
"""
|
|
209
|
+
if not self.problem:
|
|
210
|
+
raise ValueError("Problem not initialized properly")
|
|
211
|
+
|
|
212
|
+
# Create directory if it doesn't exist
|
|
213
|
+
os.makedirs(code_dir, exist_ok=True)
|
|
214
|
+
|
|
215
|
+
# Generate code
|
|
216
|
+
cpg.generate_code(self.problem, code_dir=code_dir, solver=cp.CLARABEL, wrapper=False)
|
|
217
|
+
return code_dir
|
|
218
|
+
|
|
219
|
+
def update_parameter(self, param_name, new_value):
|
|
220
|
+
"""
|
|
221
|
+
Update a parameter value.
|
|
222
|
+
|
|
223
|
+
Args:
|
|
224
|
+
param_name (str): Name of the parameter to update
|
|
225
|
+
new_value: New value for the parameter
|
|
226
|
+
"""
|
|
227
|
+
if param_name not in self.parameters:
|
|
228
|
+
raise ValueError(f"Parameter {param_name} not found")
|
|
229
|
+
|
|
230
|
+
self.parameters[param_name].value = new_value
|
|
231
|
+
|
|
232
|
+
def update_config(self, **kwargs):
|
|
233
|
+
"""
|
|
234
|
+
Update configuration parameters and rebuild the problem.
|
|
235
|
+
|
|
236
|
+
Args:
|
|
237
|
+
**kwargs: Configuration parameters to update
|
|
238
|
+
"""
|
|
239
|
+
self.config._process_kwargs(kwargs)
|
|
240
|
+
|
|
241
|
+
# Rebuild the problem with updated configuration
|
|
242
|
+
self._setup_problem()
|
gfold/visualization.py
ADDED
|
@@ -0,0 +1,90 @@
|
|
|
1
|
+
import matplotlib.pyplot as plt
|
|
2
|
+
import numpy as np
|
|
3
|
+
|
|
4
|
+
def plot_results(solution, save_path=None, show=True):
|
|
5
|
+
"""
|
|
6
|
+
Plot the results of the G-FOLD solver.
|
|
7
|
+
|
|
8
|
+
Args:
|
|
9
|
+
solution (dict): Solution from GFoldSolver.solve()
|
|
10
|
+
save_path (str, optional): Path to save the figure
|
|
11
|
+
show (bool): Whether to show the plot
|
|
12
|
+
|
|
13
|
+
Returns:
|
|
14
|
+
matplotlib.figure.Figure: The created figure
|
|
15
|
+
"""
|
|
16
|
+
positions = solution["positions"]
|
|
17
|
+
velocities = solution["velocities"]
|
|
18
|
+
thrusts = solution["normalized_thrusts"]
|
|
19
|
+
time_points = solution["time_points"]
|
|
20
|
+
|
|
21
|
+
x = positions[:, 0]
|
|
22
|
+
y = positions[:, 1]
|
|
23
|
+
z = positions[:, 2]
|
|
24
|
+
|
|
25
|
+
fig = plt.figure(figsize=(12, 10))
|
|
26
|
+
|
|
27
|
+
# 3D trajectory plot
|
|
28
|
+
ax = fig.add_subplot(221, projection='3d')
|
|
29
|
+
ax.plot(x, y, z, "red", label="path")
|
|
30
|
+
|
|
31
|
+
# Calculate and plot the glide slope cone
|
|
32
|
+
initial_height = positions[0, 2]
|
|
33
|
+
sin_glide_slope = 0 # Default value, can be passed from the solver
|
|
34
|
+
|
|
35
|
+
if initial_height > 0:
|
|
36
|
+
cosx = np.sqrt(1-np.square(sin_glide_slope))
|
|
37
|
+
theta = np.arange(0, 2*np.pi, np.pi/100)
|
|
38
|
+
radius = np.sqrt(1-np.square(cosx))/cosx * initial_height if cosx > 0 else initial_height
|
|
39
|
+
z_values = np.arange(0, initial_height, initial_height/20)
|
|
40
|
+
|
|
41
|
+
for count, zval in enumerate(z_values):
|
|
42
|
+
x_vals = np.cos(theta) * (count+1)/20 * radius
|
|
43
|
+
y_vals = np.sin(theta) * (count+1)/20 * radius
|
|
44
|
+
ax.plot(x_vals, y_vals, zval, '#0000FF55')
|
|
45
|
+
|
|
46
|
+
ax.legend(loc="center left", bbox_to_anchor=(2, 0.5))
|
|
47
|
+
ax.set_xlim3d(-initial_height*0.6, initial_height*0.6)
|
|
48
|
+
ax.set_ylim3d(-initial_height*0.6, initial_height*0.6)
|
|
49
|
+
ax.set_zlim3d(0, initial_height*1.2)
|
|
50
|
+
ax.set_xlabel('X')
|
|
51
|
+
ax.set_ylabel('Y')
|
|
52
|
+
ax.set_zlabel('Z')
|
|
53
|
+
ax.set_title('3D Trajectory')
|
|
54
|
+
|
|
55
|
+
# Velocity plot
|
|
56
|
+
ax = fig.add_subplot(222)
|
|
57
|
+
v = np.linalg.norm(velocities, axis=1)
|
|
58
|
+
ax.plot(time_points, velocities[:, 2], label="Z velocity")
|
|
59
|
+
ax.plot(time_points, v, label="Total velocity")
|
|
60
|
+
ax.set_xlabel('Time (s)')
|
|
61
|
+
ax.set_ylabel('Velocity (m/s)')
|
|
62
|
+
ax.set_title('Velocity Profile')
|
|
63
|
+
ax.legend()
|
|
64
|
+
|
|
65
|
+
# Thrust plot
|
|
66
|
+
ax = fig.add_subplot(223)
|
|
67
|
+
ax.plot(time_points, thrusts*100, label="thrust %")
|
|
68
|
+
ax.set_ylim(0, 100)
|
|
69
|
+
ax.set_xlabel('Time (s)')
|
|
70
|
+
ax.set_ylabel('Thrust (%)')
|
|
71
|
+
ax.set_title('Thrust Profile')
|
|
72
|
+
ax.legend()
|
|
73
|
+
|
|
74
|
+
# Ground trajectory
|
|
75
|
+
ax = fig.add_subplot(224)
|
|
76
|
+
ax.plot(positions[:, 0], positions[:, 1], label="ground trajectory")
|
|
77
|
+
ax.set_xlabel('X (m)')
|
|
78
|
+
ax.set_ylabel('Y (m)')
|
|
79
|
+
ax.set_title('Ground Track')
|
|
80
|
+
ax.legend()
|
|
81
|
+
|
|
82
|
+
plt.tight_layout()
|
|
83
|
+
|
|
84
|
+
if save_path:
|
|
85
|
+
plt.savefig(save_path)
|
|
86
|
+
|
|
87
|
+
if show:
|
|
88
|
+
plt.show()
|
|
89
|
+
|
|
90
|
+
return fig
|
|
@@ -0,0 +1,105 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: gfold
|
|
3
|
+
Version: 0.1.0
|
|
4
|
+
Summary: G-FOLD: Fuel Optimal Large Divert Guidance Algorithm
|
|
5
|
+
Author-email: Samu Toljamo <samu.toljamo@gmail.com>
|
|
6
|
+
Project-URL: Homepage, https://github.com/samutoljamo/g-fold
|
|
7
|
+
Project-URL: Bug Tracker, https://github.com/samutoljamo/g-fold/issues
|
|
8
|
+
Classifier: Programming Language :: Python :: 3
|
|
9
|
+
Classifier: License :: OSI Approved :: MIT License
|
|
10
|
+
Classifier: Operating System :: OS Independent
|
|
11
|
+
Requires-Python: >=3.7
|
|
12
|
+
Description-Content-Type: text/markdown
|
|
13
|
+
Requires-Dist: cvxpy
|
|
14
|
+
Requires-Dist: cvxpygen
|
|
15
|
+
Requires-Dist: matplotlib
|
|
16
|
+
Requires-Dist: numpy
|
|
17
|
+
Requires-Dist: jinja2
|
|
18
|
+
|
|
19
|
+
# G-FOLD Python Generator
|
|
20
|
+
|
|
21
|
+
This is the Python implementation of the G-FOLD algorithm with code generation capabilities. The problem is described using CVXPY/Python and C/C++ code is generated using CVXPYGen.
|
|
22
|
+
|
|
23
|
+
## Installation
|
|
24
|
+
|
|
25
|
+
### Option 1: Install from source
|
|
26
|
+
|
|
27
|
+
#### Prerequisites
|
|
28
|
+
|
|
29
|
+
You need to install [Rust](https://www.rust-lang.org/tools/install) and [Eigen](https://github.com/oxfordcontrol/Clarabel.cpp#installation) for the code generation feature.
|
|
30
|
+
|
|
31
|
+
Clone the repository:
|
|
32
|
+
|
|
33
|
+
```bash
|
|
34
|
+
git clone https://github.com/samutoljamo/g-fold.git
|
|
35
|
+
cd g-fold/generator
|
|
36
|
+
```
|
|
37
|
+
|
|
38
|
+
Install the package in development mode:
|
|
39
|
+
|
|
40
|
+
```bash
|
|
41
|
+
pip install -e .
|
|
42
|
+
```
|
|
43
|
+
|
|
44
|
+
On WSL, make sure you've installed Tkinter (version depends on the python version you're using):
|
|
45
|
+
```bash
|
|
46
|
+
sudo apt-get install python3.12-tk
|
|
47
|
+
```
|
|
48
|
+
|
|
49
|
+
### Option 2: Install from PyPI (coming soon)
|
|
50
|
+
|
|
51
|
+
```bash
|
|
52
|
+
pip install gfold
|
|
53
|
+
```
|
|
54
|
+
|
|
55
|
+
## Usage
|
|
56
|
+
|
|
57
|
+
### As a command-line tool
|
|
58
|
+
|
|
59
|
+
After installation, you can run G-FOLD from the command line:
|
|
60
|
+
|
|
61
|
+
```bash
|
|
62
|
+
# Solve the example problem with 100 steps and display graphs
|
|
63
|
+
gfold -n 100
|
|
64
|
+
|
|
65
|
+
# Generate C++ code
|
|
66
|
+
gfold -g -n 100 -o output_directory
|
|
67
|
+
|
|
68
|
+
# Save the plot to a file without displaying it
|
|
69
|
+
gfold -n 100 --save-plot --no-plot
|
|
70
|
+
```
|
|
71
|
+
|
|
72
|
+
### As a Python library
|
|
73
|
+
|
|
74
|
+
```python
|
|
75
|
+
from gfold import GFoldSolver
|
|
76
|
+
from gfold.visualization import plot_results
|
|
77
|
+
|
|
78
|
+
# Create a solver with 100 steps
|
|
79
|
+
solver = GFoldSolver()
|
|
80
|
+
|
|
81
|
+
# Solve the problem
|
|
82
|
+
solution = solver.solve(verbose=True)
|
|
83
|
+
print(f"Final mass: {solution['final_mass']:.2f} kg")
|
|
84
|
+
|
|
85
|
+
# Plot the results
|
|
86
|
+
plot_results(solution, save_path="gfold_plot.png")
|
|
87
|
+
|
|
88
|
+
# Generate C++ code
|
|
89
|
+
solver.generate_code(code_dir="generated_code")
|
|
90
|
+
```
|
|
91
|
+
|
|
92
|
+
### Example scripts
|
|
93
|
+
|
|
94
|
+
Check the `examples` directory for more usage examples:
|
|
95
|
+
|
|
96
|
+
- `simple_example.py`: Basic usage of the solver and visualization
|
|
97
|
+
|
|
98
|
+
## Development
|
|
99
|
+
|
|
100
|
+
To set up the development environment:
|
|
101
|
+
|
|
102
|
+
```bash
|
|
103
|
+
cd g-fold/generator
|
|
104
|
+
pip install -e .
|
|
105
|
+
```
|
|
@@ -0,0 +1,10 @@
|
|
|
1
|
+
gfold/__init__.py,sha256=yaVDQITf3aXyJQ3VGc5_1nAhkrF1nXYZC_L5PtahtPE,148
|
|
2
|
+
gfold/cli.py,sha256=2MtcnkIVqYj-6RyMMRWlLEu2QfDSKS9Iiq2LGlZN5uw,1887
|
|
3
|
+
gfold/config.py,sha256=ByXu8UKvXpps0QiKZdjds4RC5O_Eq18aGHj7svUg9d0,7797
|
|
4
|
+
gfold/solver.py,sha256=do1zHlXCHOiDiVa-Ppm1KYnak9KIL2Vi7VTD-KIFUgI,9159
|
|
5
|
+
gfold/visualization.py,sha256=WQudybLpB84IU1t5wCFY3QRgH9WzLkMBTxV4_3Ob7-Y,2795
|
|
6
|
+
gfold-0.1.0.dist-info/METADATA,sha256=Ko3xl8DIMu6QUiWjrlX-Y_DhfJ0exRFT7YLex-Xyq8g,2483
|
|
7
|
+
gfold-0.1.0.dist-info/WHEEL,sha256=DK49LOLCYiurdXXOXwGJm6U4DkHkg4lcxjhqwRa0CP4,91
|
|
8
|
+
gfold-0.1.0.dist-info/entry_points.txt,sha256=4dEXUic5CtbWhA8R0iAzZdejerZVrJiAypGu29bKWz4,41
|
|
9
|
+
gfold-0.1.0.dist-info/top_level.txt,sha256=jBTUZKxNhllKQx0iJklVZaGOF6mASvtHK5C00VrSpwk,6
|
|
10
|
+
gfold-0.1.0.dist-info/RECORD,,
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
gfold
|