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(0.0, 0.0, 0.0),
75                elevation: CommonPolynomial::Quadratic(0.0, 0.0, 0.0),
76            },
77            frame: LocalFrame::RCN,
78        };
79
80        for (i, var) in self.variables.iter().enumerate() {
81            let corr = self.correction[i];
82
83            // Modify the maneuver, but do not change the epochs of the maneuver unless the change is greater than one millisecond
84            match var.component {
85                Vary::Duration => {
86                    if corr.abs() > 1e-3 {
87                        // Check that we are within the bounds
88                        let init_duration_s = (correction_epoch - achievement_epoch).to_seconds();
89                        let acceptable_corr = var.apply_bounds(init_duration_s).seconds();
90                        mnvr.end = mnvr.start + acceptable_corr;
91                    }
92                }
93                Vary::EndEpoch => {
94                    if corr.abs() > 1e-3 {
95                        // Check that we are within the bounds
96                        let total_end_corr =
97                            (mnvr.end + corr.seconds() - achievement_epoch).to_seconds();
98                        let acceptable_corr = var.apply_bounds(total_end_corr).seconds();
99                        mnvr.end += acceptable_corr;
100                    }
101                }
102                Vary::StartEpoch => {
103                    if corr.abs() > 1e-3 {
104                        // Check that we are within the bounds
105                        let total_start_corr =
106                            (mnvr.start + corr.seconds() - correction_epoch).to_seconds();
107                        let acceptable_corr = var.apply_bounds(total_start_corr).seconds();
108                        mnvr.end += acceptable_corr;
109
110                        mnvr.start += corr.seconds()
111                    }
112                }
113                Vary::MnvrAlpha | Vary::MnvrAlphaDot | Vary::MnvrAlphaDDot => {
114                    match mnvr.representation {
115                        MnvrRepr::Angles { azimuth, elevation } => {
116                            let azimuth = azimuth
117                                .add_val_in_order(corr, var.component.vec_index())
118                                .unwrap();
119                            mnvr.representation = MnvrRepr::Angles { azimuth, elevation };
120                        }
121                        _ => unreachable!(),
122                    };
123                }
124                Vary::MnvrDelta | Vary::MnvrDeltaDot | Vary::MnvrDeltaDDot => {
125                    match mnvr.representation {
126                        MnvrRepr::Angles { azimuth, elevation } => {
127                            let elevation = elevation
128                                .add_val_in_order(corr, var.component.vec_index())
129                                .unwrap();
130                            mnvr.representation = MnvrRepr::Angles { azimuth, elevation };
131                        }
132                        _ => unreachable!(),
133                    };
134                }
135                Vary::ThrustX | Vary::ThrustY | Vary::ThrustZ => {
136                    let mut vector = mnvr.direction();
137                    vector[var.component.vec_index()] += corr;
138                    var.ensure_bounds(&mut vector[var.component.vec_index()]);
139                    mnvr.set_direction(vector).context(GuidanceSnafu)?;
140                }
141                Vary::ThrustRateX | Vary::ThrustRateY | Vary::ThrustRateZ => {
142                    let mut vector = mnvr.rate();
143                    let idx = (var.component.vec_index() - 1) % 3;
144                    vector[idx] += corr;
145                    var.ensure_bounds(&mut vector[idx]);
146                    mnvr.set_rate(vector).context(GuidanceSnafu)?;
147                }
148                Vary::ThrustAccelX | Vary::ThrustAccelY | Vary::ThrustAccelZ => {
149                    let mut vector = mnvr.accel();
150                    let idx = (var.component.vec_index() - 1) % 3;
151                    vector[idx] += corr;
152                    var.ensure_bounds(&mut vector[idx]);
153                    mnvr.set_accel(vector).context(GuidanceSnafu)?;
154                }
155                Vary::ThrustLevel => {
156                    mnvr.thrust_prct += corr;
157                    var.ensure_bounds(&mut mnvr.thrust_prct);
158                }
159                _ => unreachable!(),
160            }
161        }
162
163        Ok(mnvr)
164    }
165}
166
167impl<const V: usize, const O: usize> fmt::Display for TargeterSolution<V, O> {
168    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
169        let mut objmsg = String::from("");
170        for (i, obj) in self.achieved_objectives.iter().enumerate() {
171            objmsg.push_str(&format!(
172                "\n\t\t{:?} = {:.3} (wanted {:.3} ± {:.1e})",
173                obj.parameter,
174                obj.desired_value + self.achieved_errors[i],
175                obj.desired_value,
176                obj.tolerance
177            ));
178        }
179
180        let mut corrmsg = format!("Correction @ {}:", self.corrected_state.epoch());
181        let mut is_only_position = true;
182        let mut is_only_velocity = true;
183        for (i, var) in self.variables.iter().enumerate() {
184            let unit = match var.component {
185                Vary::PositionX | Vary::PositionY | Vary::PositionZ => {
186                    is_only_velocity = false;
187                    "m"
188                }
189                Vary::VelocityX | Vary::VelocityY | Vary::VelocityZ => {
190                    is_only_position = false;
191                    "m/s"
192                }
193                _ => {
194                    is_only_position = false;
195                    is_only_velocity = false;
196                    ""
197                }
198            };
199            corrmsg.push_str(&format!(
200                "\n\t\t{:?} = {:.3} {}",
201                var.component, self.correction[i], unit
202            ));
203        }
204
205        if is_only_position {
206            corrmsg.push_str(&format!(
207                "\n\t\t|Δr| = {:.3} m",
208                self.correction.norm() * 1e3
209            ));
210        } else if is_only_velocity {
211            corrmsg.push_str(&format!(
212                "\n\t\t|Δv| = {:.3} m/s",
213                self.correction.norm() * 1e3
214            ));
215        } else if self.is_finite_burn() {
216            let mnvr = self.to_mnvr().unwrap();
217            corrmsg.push_str(&format!("\n\t\t{mnvr}\n"));
218        }
219
220        writeln!(
221            f,
222            "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}",
223            self.variables.iter().map(|v| format!("{:?}", v.component)).collect::<Vec<String>>(),
224            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
225        )
226    }
227}
nyx_space/md/opti/solution.rs

nyx_space/md/opti/
solution.rs
