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

nyx_space/md/opti/
targeter.rs
