nyx_space/md/opti/
solution.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 hifitime::TimeUnits;
20use snafu::{ensure, ResultExt};
21
22use crate::dynamics::guidance::{LocalFrame, Maneuver, MnvrRepr};
23use crate::linalg::SVector;
24use crate::md::objective::Objective;
25use crate::md::{prelude::*, GuidanceSnafu, NotFiniteSnafu, TargetingError};
26pub use crate::md::{Variable, Vary};
27use crate::polyfit::CommonPolynomial;
28use std::fmt;
29use std::time::Duration;
30
31/// Defines a targeter solution
32#[derive(Clone, Debug)]
33pub struct TargeterSolution<const V: usize, const O: usize> {
34    /// The corrected spacecraft state at the correction epoch
35    pub corrected_state: Spacecraft,
36    /// The state at which the objectives are achieved
37    pub achieved_state: Spacecraft,
38    /// The correction vector applied
39    pub correction: SVector<f64, V>,
40    /// The kind of correction (position or velocity)
41    pub variables: [Variable; V],
42    /// The errors achieved
43    pub achieved_errors: SVector<f64, O>,
44    /// The objectives set in the targeter
45    pub achieved_objectives: [Objective; O],
46    /// The number of iterations required
47    pub iterations: usize,
48    /// Computation duration
49    pub computation_dur: Duration,
50}
51
52impl<const V: usize, const O: usize> TargeterSolution<V, O> {
53    /// Returns whether this solution is a finite burn solution or not
54    pub fn is_finite_burn(&self) -> bool {
55        for var in &self.variables {
56            if var.component.is_finite_burn() {
57                return true;
58            }
59        }
60        false
61    }
62
63    /// Returns a maneuver if targeter solution was a finite burn maneuver
64    pub fn to_mnvr(&self) -> Result<Maneuver, TargetingError> {
65        ensure!(self.is_finite_burn(), NotFiniteSnafu);
66
67        let correction_epoch = self.corrected_state.epoch();
68        let achievement_epoch = self.achieved_state.epoch();
69        let mut mnvr = Maneuver {
70            start: correction_epoch,
71            end: achievement_epoch,
72            thrust_prct: 1.0,
73            representation: MnvrRepr::Angles {
74                azimuth: CommonPolynomial::Quadratic {
75                    a: 0.0,
76                    b: 0.0,
77                    c: 0.0,
78                },
79                elevation: CommonPolynomial::Quadratic {
80                    a: 0.0,
81                    b: 0.0,
82                    c: 0.0,
83                },
84            },
85            frame: LocalFrame::RCN,
86        };
87
88        for (i, var) in self.variables.iter().enumerate() {
89            let corr = self.correction[i];
90
91            // Modify the maneuver, but do not change the epochs of the maneuver unless the change is greater than one millisecond
92            match var.component {
93                Vary::Duration => {
94                    if corr.abs() > 1e-3 {
95                        // Check that we are within the bounds
96                        let init_duration_s = (correction_epoch - achievement_epoch).to_seconds();
97                        let acceptable_corr = var.apply_bounds(init_duration_s).seconds();
98                        mnvr.end = mnvr.start + acceptable_corr;
99                    }
100                }
101                Vary::EndEpoch => {
102                    if corr.abs() > 1e-3 {
103                        // Check that we are within the bounds
104                        let total_end_corr =
105                            (mnvr.end + corr.seconds() - achievement_epoch).to_seconds();
106                        let acceptable_corr = var.apply_bounds(total_end_corr).seconds();
107                        mnvr.end += acceptable_corr;
108                    }
109                }
110                Vary::StartEpoch => {
111                    if corr.abs() > 1e-3 {
112                        // Check that we are within the bounds
113                        let total_start_corr =
114                            (mnvr.start + corr.seconds() - correction_epoch).to_seconds();
115                        let acceptable_corr = var.apply_bounds(total_start_corr).seconds();
116                        mnvr.end += acceptable_corr;
117
118                        mnvr.start += corr.seconds()
119                    }
120                }
121                Vary::MnvrAlpha | Vary::MnvrAlphaDot | Vary::MnvrAlphaDDot => {
122                    match mnvr.representation {
123                        MnvrRepr::Angles { azimuth, elevation } => {
124                            let azimuth = azimuth
125                                .add_val_in_order(corr, var.component.vec_index())
126                                .unwrap();
127                            mnvr.representation = MnvrRepr::Angles { azimuth, elevation };
128                        }
129                        _ => unreachable!(),
130                    };
131                }
132                Vary::MnvrDelta | Vary::MnvrDeltaDot | Vary::MnvrDeltaDDot => {
133                    match mnvr.representation {
134                        MnvrRepr::Angles { azimuth, elevation } => {
135                            let elevation = elevation
136                                .add_val_in_order(corr, var.component.vec_index())
137                                .unwrap();
138                            mnvr.representation = MnvrRepr::Angles { azimuth, elevation };
139                        }
140                        _ => unreachable!(),
141                    };
142                }
143                Vary::ThrustX | Vary::ThrustY | Vary::ThrustZ => {
144                    let mut vector = mnvr.direction();
145                    vector[var.component.vec_index()] += corr;
146                    var.ensure_bounds(&mut vector[var.component.vec_index()]);
147                    mnvr.set_direction(vector).context(GuidanceSnafu)?;
148                }
149                Vary::ThrustRateX | Vary::ThrustRateY | Vary::ThrustRateZ => {
150                    let mut vector = mnvr.rate();
151                    let idx = (var.component.vec_index() - 1) % 3;
152                    vector[idx] += corr;
153                    var.ensure_bounds(&mut vector[idx]);
154                    mnvr.set_rate(vector).context(GuidanceSnafu)?;
155                }
156                Vary::ThrustAccelX | Vary::ThrustAccelY | Vary::ThrustAccelZ => {
157                    let mut vector = mnvr.accel();
158                    let idx = (var.component.vec_index() - 1) % 3;
159                    vector[idx] += corr;
160                    var.ensure_bounds(&mut vector[idx]);
161                    mnvr.set_accel(vector).context(GuidanceSnafu)?;
162                }
163                Vary::ThrustLevel => {
164                    mnvr.thrust_prct += corr;
165                    var.ensure_bounds(&mut mnvr.thrust_prct);
166                }
167                _ => unreachable!(),
168            }
169        }
170
171        Ok(mnvr)
172    }
173}
174
175impl<const V: usize, const O: usize> fmt::Display for TargeterSolution<V, O> {
176    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
177        let mut objmsg = String::from("");
178        for (i, obj) in self.achieved_objectives.iter().enumerate() {
179            objmsg.push_str(&format!(
180                "\n\t\t{:?} = {:.3} (wanted {:.3} ± {:.1e})",
181                obj.parameter,
182                obj.desired_value + self.achieved_errors[i],
183                obj.desired_value,
184                obj.tolerance
185            ));
186        }
187
188        let mut corrmsg = format!("Correction @ {}:", self.corrected_state.epoch());
189        let mut is_only_position = true;
190        let mut is_only_velocity = true;
191        for (i, var) in self.variables.iter().enumerate() {
192            let (unit, mulp) = match var.component {
193                Vary::PositionX | Vary::PositionY | Vary::PositionZ => {
194                    is_only_velocity = false;
195                    ("m", 1e3)
196                }
197                Vary::VelocityX | Vary::VelocityY | Vary::VelocityZ => {
198                    is_only_position = false;
199                    ("m/s", 1e3)
200                }
201                _ => {
202                    is_only_position = false;
203                    is_only_velocity = false;
204                    ("", 1.0)
205                }
206            };
207            corrmsg.push_str(&format!(
208                "\n\t\t{:?} = {:.3} {}",
209                var.component,
210                self.correction[i] * mulp,
211                unit
212            ));
213        }
214
215        if is_only_position {
216            corrmsg.push_str(&format!(
217                "\n\t\t|Δr| = {:.3} m",
218                self.correction.norm() * 1e3
219            ));
220        } else if is_only_velocity {
221            corrmsg.push_str(&format!(
222                "\n\t\t|Δv| = {:.3} m/s",
223                self.correction.norm() * 1e3
224            ));
225        } else if self.is_finite_burn() {
226            let mnvr = self.to_mnvr().unwrap();
227            corrmsg.push_str(&format!("\n\t\t{mnvr}\n"));
228        }
229
230        writeln!(
231            f,
232            "Targeter solution correcting {:?} (converged in {:.3} seconds, {} iterations):\n\t{}\n\tAchieved @ {}:{}\n\tCorrected state:\n\t\t{}\n\t\t{:x}\n\tAchieved state:\n\t\t{}\n\t\t{:x}",
233            self.variables.iter().map(|v| format!("{:?}", v.component)).collect::<Vec<String>>(),
234            self.computation_dur.as_secs_f64(), self.iterations, corrmsg, self.achieved_state.epoch(), objmsg, self.corrected_state, self.corrected_state, self.achieved_state, self.achieved_state
235        )
236    }
237}
nyx_space/md/opti/solution.rs

nyx_space/md/opti/
solution.rs
