nyx_space/md/opti/
targeter.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use snafu::ResultExt;
20
21use crate::dynamics::guidance::LocalFrame;
22use crate::errors::TargetingError;
23use crate::md::objective::Objective;
24use crate::md::prelude::*;
25use crate::md::AstroSnafu;
26use crate::md::PropSnafu;
27use crate::md::StateParameter;
28pub use crate::md::{Variable, Vary};
29use std::fmt;
30
31use super::solution::TargeterSolution;
32
33/// An optimizer structure with V control variables and O objectives.
34#[derive(Clone)]
35pub struct Targeter<'a, const V: usize, const O: usize> {
36    /// The propagator setup (kind, stages, etc.)
37    pub prop: &'a Propagator<SpacecraftDynamics>,
38    /// The list of objectives of this targeter
39    pub objectives: [Objective; O],
40    /// An optional frame (and Cosm) to compute the objectives in.
41    /// Needed if the propagation frame is separate from objectives frame (e.g. for B Plane targeting).
42    pub objective_frame: Option<Frame>,
43    /// The kind of correction to apply to achieve the objectives
44    pub variables: [Variable; V],
45    /// The frame in which the correction should be applied, must be either a local frame or inertial
46    pub correction_frame: Option<LocalFrame>,
47    /// Maximum number of iterations
48    pub iterations: usize,
49}
50
51impl<const V: usize, const O: usize> fmt::Display for Targeter<'_, V, O> {
52    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
53        let mut objmsg = String::from("");
54        for obj in &self.objectives {
55            objmsg.push_str(&format!("{obj}; "));
56        }
57
58        let mut varmsg = String::from("");
59        for var in &self.variables {
60            varmsg.push_str(&format!("{var}; "));
61        }
62
63        write!(f, "Targeter:\n\tObjectives: {objmsg}\n\tCorrect: {varmsg}")
64    }
65}
66
67impl<'a, const O: usize> Targeter<'a, 3, O> {
68    /// Create a new Targeter which will apply an impulsive delta-v correction.
69    pub fn delta_v(prop: &'a Propagator<SpacecraftDynamics>, objectives: [Objective; O]) -> Self {
70        Self {
71            prop,
72            objectives,
73            variables: [
74                Vary::VelocityX.into(),
75                Vary::VelocityY.into(),
76                Vary::VelocityZ.into(),
77            ],
78            iterations: 100,
79            objective_frame: None,
80            correction_frame: None,
81        }
82    }
83
84    /// Create a new Targeter which will MOVE the position of the spacecraft at the correction epoch
85    pub fn delta_r(prop: &'a Propagator<SpacecraftDynamics>, objectives: [Objective; O]) -> Self {
86        Self {
87            prop,
88            objectives,
89            variables: [
90                Vary::PositionX.into(),
91                Vary::PositionY.into(),
92                Vary::PositionZ.into(),
93            ],
94            iterations: 100,
95            objective_frame: None,
96            correction_frame: None,
97        }
98    }
99
100    /// Create a new Targeter which will apply an impulsive delta-v correction on all components of the VNC frame. By default, max step is 0.5 km/s.
101    pub fn vnc(prop: &'a Propagator<SpacecraftDynamics>, objectives: [Objective; O]) -> Self {
102        Self {
103            prop,
104            objectives,
105            variables: [
106                Vary::VelocityX.into(),
107                Vary::VelocityY.into(),
108                Vary::VelocityZ.into(),
109            ],
110            iterations: 100,
111            objective_frame: None,
112            correction_frame: Some(LocalFrame::VNC),
113        }
114    }
115}
116
117impl<'a, const O: usize> Targeter<'a, 4, O> {
118    /// Create a new Targeter which will apply a continuous thrust for the whole duration of the segment
119    pub fn thrust_dir(
120        prop: &'a Propagator<SpacecraftDynamics>,
121        objectives: [Objective; O],
122    ) -> Self {
123        Self {
124            prop,
125            objectives,
126            variables: [
127                Variable::from(Vary::ThrustX),
128                Variable::from(Vary::ThrustY),
129                Variable::from(Vary::ThrustZ),
130                Variable::from(Vary::ThrustLevel),
131            ],
132            iterations: 20,
133            objective_frame: None,
134            correction_frame: None,
135        }
136    }
137}
138
139impl<'a, const O: usize> Targeter<'a, 7, O> {
140    /// Create a new Targeter which will apply a continuous thrust for the whole duration of the segment
141    pub fn thrust_dir_rate(
142        prop: &'a Propagator<SpacecraftDynamics>,
143        objectives: [Objective; O],
144    ) -> Self {
145        Self {
146            prop,
147            objectives,
148            variables: [
149                Variable::from(Vary::ThrustX),
150                Variable::from(Vary::ThrustY),
151                Variable::from(Vary::ThrustZ),
152                Variable::from(Vary::ThrustLevel),
153                Variable::from(Vary::ThrustRateX),
154                Variable::from(Vary::ThrustRateY),
155                Variable::from(Vary::ThrustRateZ),
156            ],
157            iterations: 50,
158            objective_frame: None,
159            correction_frame: None,
160        }
161    }
162}
163
164impl<'a, const O: usize> Targeter<'a, 10, O> {
165    /// Create a new Targeter which will apply a continuous thrust for the whole duration of the segment
166    pub fn thrust_profile(
167        prop: &'a Propagator<SpacecraftDynamics>,
168        objectives: [Objective; O],
169    ) -> Self {
170        Self {
171            prop,
172            objectives,
173            variables: [
174                Variable::from(Vary::ThrustX),
175                Variable::from(Vary::ThrustY),
176                Variable::from(Vary::ThrustZ),
177                Variable::from(Vary::ThrustLevel),
178                Variable::from(Vary::ThrustRateX),
179                Variable::from(Vary::ThrustRateY),
180                Variable::from(Vary::ThrustRateZ),
181                Variable::from(Vary::ThrustAccelX),
182                Variable::from(Vary::ThrustAccelY),
183                Variable::from(Vary::ThrustAccelZ),
184            ],
185            iterations: 50,
186            objective_frame: None,
187            correction_frame: None,
188        }
189    }
190}
191
192impl<'a, const V: usize, const O: usize> Targeter<'a, V, O> {
193    /// Create a new Targeter which will apply an impulsive delta-v correction.
194    pub fn new(
195        prop: &'a Propagator<SpacecraftDynamics>,
196        variables: [Variable; V],
197        objectives: [Objective; O],
198    ) -> Self {
199        Self {
200            prop,
201            objectives,
202            variables,
203            iterations: 100,
204            objective_frame: None,
205            correction_frame: None,
206        }
207    }
208
209    /// Create a new Targeter which will apply an impulsive delta-v correction.
210    pub fn in_frame(
211        prop: &'a Propagator<SpacecraftDynamics>,
212        variables: [Variable; V],
213        objectives: [Objective; O],
214        objective_frame: Frame,
215    ) -> Self {
216        Self {
217            prop,
218            objectives,
219            variables,
220            iterations: 100,
221            objective_frame: Some(objective_frame),
222            correction_frame: None,
223        }
224    }
225
226    /// Create a new Targeter which will apply an impulsive delta-v correction on the specified components of the VNC frame.
227    pub fn vnc_with_components(
228        prop: &'a Propagator<SpacecraftDynamics>,
229        variables: [Variable; V],
230        objectives: [Objective; O],
231    ) -> Self {
232        Self {
233            prop,
234            objectives,
235            variables,
236            iterations: 100,
237            objective_frame: None,
238            correction_frame: Some(LocalFrame::VNC),
239        }
240    }
241
242    /// Runs the targeter using finite differencing (for now).
243    #[allow(clippy::identity_op)]
244    pub fn try_achieve_from(
245        &self,
246        initial_state: Spacecraft,
247        correction_epoch: Epoch,
248        achievement_epoch: Epoch,
249        almanac: Arc<Almanac>,
250    ) -> Result<TargeterSolution<V, O>, TargetingError> {
251        self.try_achieve_fd(initial_state, correction_epoch, achievement_epoch, almanac)
252    }
253
254    /// Apply a correction and propagate to achievement epoch. Also checks that the objectives are indeed matched
255    pub fn apply(
256        &self,
257        solution: &TargeterSolution<V, O>,
258        almanac: Arc<Almanac>,
259    ) -> Result<Spacecraft, TargetingError> {
260        let (xf, _) = self.apply_with_traj(solution, almanac)?;
261        Ok(xf)
262    }
263
264    /// Apply a correction and propagate to achievement epoch, return the final state and trajectory.
265    /// Also checks that the objectives are indeed matched.
266    pub fn apply_with_traj(
267        &self,
268        solution: &TargeterSolution<V, O>,
269        almanac: Arc<Almanac>,
270    ) -> Result<(Spacecraft, Traj<Spacecraft>), TargetingError> {
271        let (xf, traj) = match solution.to_mnvr() {
272            Ok(mnvr) => {
273                println!("{mnvr}");
274                let mut prop = self.prop.clone();
275                prop.dynamics = prop.dynamics.with_guidance_law(Arc::new(mnvr));
276                prop.with(solution.corrected_state, almanac)
277                    .until_epoch_with_traj(solution.achieved_state.epoch())
278                    .context(PropSnafu)?
279            }
280            Err(_) => {
281                // This isn't a finite burn maneuver, let's just apply the correction
282                // Propagate until achievement epoch
283                self.prop
284                    .with(solution.corrected_state, almanac)
285                    .until_epoch_with_traj(solution.achieved_state.epoch())
286                    .context(PropSnafu)?
287            }
288        };
289
290        // Build the partials
291        let xf_dual = OrbitDual::from(xf.orbit);
292
293        let mut is_bplane_tgt = false;
294        for obj in &self.objectives {
295            if obj.parameter.is_b_plane() {
296                is_bplane_tgt = true;
297            }
298        }
299
300        // Build the B-Plane once, if needed
301        let b_plane = if is_bplane_tgt {
302            Some(BPlane::from_dual(xf_dual).context(AstroSnafu)?)
303        } else {
304            None
305        };
306
307        let mut converged = true;
308        let mut param_errors = Vec::new();
309        for obj in &self.objectives {
310            let partial = if obj.parameter.is_b_plane() {
311                match obj.parameter {
312                    StateParameter::BdotR => b_plane.unwrap().b_r,
313                    StateParameter::BdotT => b_plane.unwrap().b_t,
314                    StateParameter::BLTOF => b_plane.unwrap().ltof_s,
315                    _ => unreachable!(),
316                }
317            } else {
318                xf_dual.partial_for(obj.parameter).context(AstroSnafu)?
319            };
320
321            let param_err = obj.desired_value - partial.real();
322
323            if param_err.abs() > obj.tolerance {
324                converged = false;
325            }
326            param_errors.push(param_err);
327        }
328        if converged {
329            Ok((xf, traj))
330        } else {
331            let mut objmsg = String::from("");
332            for (i, obj) in self.objectives.iter().enumerate() {
333                objmsg.push_str(&format!(
334                    "{:?} = {:.3} BUT should be {:.3} (± {:.1e}) (error = {:.3})",
335                    obj.parameter,
336                    obj.desired_value + param_errors[i],
337                    obj.desired_value,
338                    obj.tolerance,
339                    param_errors[i]
340                ));
341            }
342            Err(TargetingError::Verification { msg: objmsg })
343        }
344    }
345}
nyx_space/md/opti/targeter.rs

nyx_space/md/opti/
targeter.rs
