parse_monitors/monitors/
mirror.rs

1//! M1 and M2 segments center of pressure, forces and moments
2
3use crate::Vector;
4use crate::{Exertion, Monitors, MonitorsLoader};
5#[cfg(feature = "plot")]
6use plotters::prelude::*;
7use std::{
8    collections::{BTreeMap, VecDeque},
9    fs::File,
10    path::Path,
11};
12
13/// Mirror data loader
14pub struct MirrorLoader<P: AsRef<Path>> {
15    mirror: Mirror,
16    path: P,
17    time_range: (f64, f64),
18    net_force: bool,
19}
20impl<P: AsRef<Path>> MirrorLoader<P> {
21    fn new(mirror: Mirror, path: P) -> Self {
22        MirrorLoader {
23            mirror,
24            path,
25            time_range: (0f64, f64::INFINITY),
26            net_force: false,
27        }
28    }
29    pub fn start_time(self, time: f64) -> Self {
30        Self {
31            time_range: (time, self.time_range.1),
32            ..self
33        }
34    }
35    pub fn end_time(self, time: f64) -> Self {
36        Self {
37            time_range: (self.time_range.0, time),
38            ..self
39        }
40    }
41    pub fn net_force(self) -> Self {
42        Self {
43            net_force: true,
44            ..self
45        }
46    }
47    pub fn load(self) -> Result<Mirror, Box<dyn std::error::Error>> {
48        let mut mirror = self.mirror;
49        let (filename, time, force) = match &mut mirror {
50            Mirror::M1 { time, force } => ("center_of_pressure.csv", time, force),
51            Mirror::M2 { time, force } => ("M2_segments_force.csv", time, force),
52        };
53        let path = Path::new(self.path.as_ref());
54        if let Ok(csv_file) = File::open(&path.join(filename)) {
55            let mut rdr = csv::Reader::from_reader(csv_file);
56            for result in rdr.deserialize() {
57                let record: (f64, Vec<([f64; 3], ([f64; 3], [f64; 3]))>) = result?;
58                let t = record.0;
59                if t < self.time_range.0 - 1. / 40. || t > self.time_range.1 + 1. / 40. {
60                    continue;
61                };
62                let mut record_iter = record.1.into_iter();
63                if let Some(t_b) = time.back() {
64                    if t >= *t_b {
65                        time.push_back(t);
66                        for fm in force.values_mut() {
67                            fm.push_back(record_iter.next().unwrap().into())
68                        }
69                    } else {
70                        if let Some(index) = time.iter().rposition(|&x| x < t) {
71                            time.insert(index + 1, t);
72                            for fm in force.values_mut() {
73                                fm.insert(index + 1, record_iter.next().unwrap().into())
74                            }
75                        } else {
76                            time.push_front(t);
77                            for fm in force.values_mut() {
78                                fm.push_front(record_iter.next().unwrap().into())
79                            }
80                        }
81                    }
82                } else {
83                    time.push_back(t);
84                    for fm in force.values_mut() {
85                        fm.push_back(record_iter.next().unwrap().into())
86                    }
87                }
88            }
89            if self.net_force {
90                if let Mirror::M1 { time, force } = &mut mirror {
91                    let ts = *time.front().unwrap();
92                    let te = *time.back().unwrap();
93                    let monitors = MonitorsLoader::<2021>::default()
94                        .data_path(path)
95                        .header_filter("M1cell".to_string())
96                        .start_time(ts)
97                        .end_time(te)
98                        .load()?;
99                    let m1_cell = &monitors.forces_and_moments["M1cell"];
100                    assert_eq!(
101                        time.len(),
102                        m1_cell.len(),
103                        "{:?} {:?}/{:?}: M1 segments and M1 cell # of sample do not match",
104                        (ts, te),
105                        (monitors.time[0], monitors.time.last().unwrap()),
106                        path
107                    );
108                    for v in force.values_mut() {
109                        for (e, cell) in v.iter_mut().zip(m1_cell) {
110                            let mut f = &mut e.force;
111                            f += &(&cell.force / 7f64).unwrap();
112                            let mut m = &mut e.moment;
113                            m += &(&cell.moment / 7f64).unwrap();
114                        }
115                    }
116                }
117            }
118            Ok(mirror)
119        } else {
120            Err(format!("Cannot open {:?}", &path).into())
121        }
122    }
123}
124
125/// Mirror type
126#[derive(Debug)]
127pub enum Mirror {
128    M1 {
129        time: VecDeque<f64>,
130        force: BTreeMap<String, VecDeque<Exertion>>,
131    },
132    M2 {
133        time: VecDeque<f64>,
134        force: BTreeMap<String, VecDeque<Exertion>>,
135    },
136}
137impl Mirror {
138    pub fn m1<P: AsRef<Path>>(path: P) -> MirrorLoader<P> {
139        let mut force: BTreeMap<String, VecDeque<Exertion>> = BTreeMap::new();
140        (1..=7).for_each(|k| {
141            force.entry(format!("S{}", k)).or_default();
142        });
143        MirrorLoader::new(
144            Mirror::M1 {
145                time: VecDeque::new(),
146                force,
147            },
148            path,
149        )
150    }
151    pub fn m2<P: AsRef<Path>>(path: P) -> MirrorLoader<P> {
152        let mut force: BTreeMap<String, VecDeque<Exertion>> = BTreeMap::new();
153        (1..=7).for_each(|k| {
154            force.entry(format!("S{}", k)).or_default();
155        });
156        MirrorLoader::new(
157            Mirror::M2 {
158                time: VecDeque::new(),
159                force,
160            },
161            path,
162        )
163    }
164    /// Keeps only the last `period` seconds of the monitors
165    pub fn keep_last(&mut self, period: usize) -> &mut Self {
166        let i = self.len() - period * crate::FORCE_SAMPLING_FREQUENCY as usize;
167        let _: Vec<_> = self.time_mut().drain(..i).collect();
168        for value in self.exertion_mut() {
169            let _: Vec<_> = value.drain(..i).collect();
170        }
171        self
172    }
173    pub fn summary(&self) {
174        let (mirror, time, force) = match self {
175            Mirror::M1 { time, force } => ("M1", time, force),
176            Mirror::M2 { time, force } => ("M2", time, force),
177        };
178        println!("{} SUMMARY:", mirror);
179        println!(" - # of records: {}", time.len());
180        println!(
181            " - time range: [{:8.3}-{:8.3}]s",
182            time.front().unwrap(),
183            time.back().unwrap()
184        );
185        println!("    {:^16}: [{:^12}], [{:^12}])", "ELEMENT", "MEAN", "STD");
186        for (key, value) in force.iter() {
187            let force: Vec<Vector> = value.iter().map(|e| e.force.clone()).collect();
188            Monitors::display(key, &force);
189        }
190    }
191    pub fn len(&self) -> usize {
192        self.time().len()
193    }
194    pub fn time(&self) -> &VecDeque<f64> {
195        match self {
196            Mirror::M1 { time, .. } => time,
197            Mirror::M2 { time, .. } => time,
198        }
199    }
200    pub fn time_mut(&mut self) -> &mut VecDeque<f64> {
201        match self {
202            Mirror::M1 { time, .. } => time,
203            Mirror::M2 { time, .. } => time,
204        }
205    }
206    pub fn forces_and_moments(&self) -> &BTreeMap<String, VecDeque<Exertion>> {
207        match self {
208            Mirror::M1 { force, .. } => force,
209            Mirror::M2 { force, .. } => force,
210        }
211    }
212    pub fn forces_and_moments_mut(&mut self) -> &mut BTreeMap<String, VecDeque<Exertion>> {
213        match self {
214            Mirror::M1 { force, .. } => force,
215            Mirror::M2 { force, .. } => force,
216        }
217    }
218    pub fn exertion(&self) -> impl Iterator<Item = &VecDeque<Exertion>> {
219        match self {
220            Mirror::M1 { force, .. } => force.values(),
221            Mirror::M2 { force, .. } => force.values(),
222        }
223    }
224    pub fn exertion_mut(&mut self) -> impl Iterator<Item = &mut VecDeque<Exertion>> {
225        match self {
226            Mirror::M1 { force, .. } => force.values_mut(),
227            Mirror::M2 { force, .. } => force.values_mut(),
228        }
229    }
230    /// Return a latex table with force monitors summary
231    pub fn force_latex_table(&self, stats_duration: f64) -> Option<String> {
232        let max_value = |x: &[f64]| x.iter().cloned().fold(std::f64::NEG_INFINITY, f64::max);
233        let min_value = |x: &[f64]| x.iter().cloned().fold(std::f64::INFINITY, f64::min);
234        let minmax = |x: &[f64]| (min_value(x), max_value(x));
235        let stats = |x: &[f64]| {
236            let n = x.len() as f64;
237            let mean = x.iter().sum::<f64>() / n;
238            let std = (x.iter().map(|x| x - mean).fold(0f64, |s, x| s + x * x) / n).sqrt();
239            (mean, std)
240        };
241        if self.forces_and_moments().is_empty() {
242            None
243        } else {
244            let duration = self.time().back().unwrap();
245            let time_filter: Vec<_> = self
246                .time()
247                .iter()
248                .map(|t| t - duration + stats_duration - crate::FORCE_SAMPLING > 0f64)
249                .collect();
250            let data: Vec<_> = self
251                .forces_and_moments()
252                .iter()
253                .map(|(key, value)| {
254                    let force_magnitude: Option<Vec<f64>> = value
255                        .iter()
256                        .zip(time_filter.iter())
257                        .filter(|(_, t)| **t)
258                        .map(|(e, _)| e.force.magnitude())
259                        .collect();
260                    match force_magnitude {
261                        Some(ref value) => {
262                            let (mean, std) = stats(value);
263                            let (min, max) = minmax(value);
264                            format!(
265                                " {:} & {:.3} & {:.3} & {:.3} & {:.3} \\\\",
266                                key.replace("_", " "),
267                                mean,
268                                std,
269                                min,
270                                max
271                            )
272                        }
273                        None => format!(" {:} \\\\", key.replace("_", " ")),
274                    }
275                })
276                .collect();
277            Some(data.join("\n"))
278        }
279    }
280    /// Return a latex table with moment monitors summary
281    pub fn moment_latex_table(&self, stats_duration: f64) -> Option<String> {
282        let max_value = |x: &[f64]| x.iter().cloned().fold(std::f64::NEG_INFINITY, f64::max);
283        let min_value = |x: &[f64]| x.iter().cloned().fold(std::f64::INFINITY, f64::min);
284        let minmax = |x: &[f64]| (min_value(x), max_value(x));
285        let stats = |x: &[f64]| {
286            let n = x.len() as f64;
287            let mean = x.iter().sum::<f64>() / n;
288            let std = (x.iter().map(|x| x - mean).fold(0f64, |s, x| s + x * x) / n).sqrt();
289            (mean, std)
290        };
291        if self.forces_and_moments().is_empty() {
292            None
293        } else {
294            let duration = self.time().back().unwrap();
295            let time_filter: Vec<_> = self
296                .time()
297                .iter()
298                .map(|t| t - duration + stats_duration - crate::FORCE_SAMPLING > 0f64)
299                .collect();
300            let data: Vec<_> = self
301                .forces_and_moments()
302                .iter()
303                .map(|(key, value)| {
304                    let moment_magnitude: Option<Vec<f64>> = value
305                        .iter()
306                        .zip(time_filter.iter())
307                        .filter(|(_, t)| **t)
308                        .map(|(e, _)| e.moment.magnitude())
309                        .collect();
310                    match moment_magnitude {
311                        Some(ref value) => {
312                            let (mean, std) = stats(value);
313                            let (min, max) = minmax(value);
314                            format!(
315                                " {:} & {:.3} & {:.3} & {:.3} & {:.3} \\\\",
316                                key.replace("_", " "),
317                                mean,
318                                std,
319                                min,
320                                max
321                            )
322                        }
323                        None => format!(" {:} \\\\", key.replace("_", " ")),
324                    }
325                })
326                .collect();
327            Some(data.join("\n"))
328        }
329    }
330    #[cfg(feature = "plot")]
331    pub fn plot_forces(&self, filename: Option<&str>) {
332        if self.forces_and_moments().is_empty() {
333            println!("Empty mirror");
334            return;
335        }
336
337        let max_value = |x: &[f64]| -> f64 {
338            x.iter()
339                .cloned()
340                .rev()
341                .take(400 * 20)
342                .fold(std::f64::NEG_INFINITY, f64::max)
343        };
344        let min_value = |x: &[f64]| -> f64 {
345            x.iter()
346                .cloned()
347                .rev()
348                .take(400 * 20)
349                .fold(std::f64::INFINITY, f64::min)
350        };
351
352        let plot =
353            BitMapBackend::new(filename.unwrap_or("FORCE.png"), (768, 512)).into_drawing_area();
354        plot.fill(&WHITE).unwrap();
355
356        let (min_values, max_values): (Vec<_>, Vec<_>) = self
357            .forces_and_moments()
358            .values()
359            .map(|values| {
360                let force_magnitude: Option<Vec<f64>> =
361                    values.iter().map(|e| e.force.magnitude()).collect();
362                (
363                    min_value(force_magnitude.as_ref().unwrap()),
364                    max_value(force_magnitude.as_ref().unwrap()),
365                )
366            })
367            .unzip();
368        let xrange = (*self.time().front().unwrap(), *self.time().back().unwrap());
369        let minmax_padding = 0.1;
370        let mut chart = ChartBuilder::on(&plot)
371            .set_label_area_size(LabelAreaPosition::Left, 60)
372            .set_label_area_size(LabelAreaPosition::Bottom, 40)
373            .margin(10)
374            .build_cartesian_2d(
375                xrange.0..xrange.1 * (1. + 1e-2),
376                min_value(&min_values) * (1. - minmax_padding)
377                    ..max_value(&max_values) * (1. + minmax_padding),
378            )
379            .unwrap();
380        chart
381            .configure_mesh()
382            .x_desc("Time [s]")
383            .y_desc("Force [N]")
384            .draw()
385            .unwrap();
386
387        let mut colors = colorous::TABLEAU10.iter().cycle();
388
389        for (key, values) in self.forces_and_moments().iter() {
390            let color = colors.next().unwrap();
391            let rgb = RGBColor(color.r, color.g, color.b);
392            chart
393                .draw_series(LineSeries::new(
394                    self.time()
395                        .iter()
396                        .zip(values.iter())
397                        //.skip(10 * 20)
398                        .map(|(&x, y)| (x, y.force.magnitude().unwrap())),
399                    &rgb,
400                ))
401                .unwrap()
402                .label(key)
403                .legend(move |(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &rgb));
404        }
405        chart
406            .configure_series_labels()
407            .border_style(&BLACK)
408            .background_style(&WHITE.mix(0.8))
409            .position(SeriesLabelPosition::UpperRight)
410            .draw()
411            .unwrap();
412    }
413    #[cfg(feature = "plot")]
414    pub fn plot_moments(&self, filename: Option<&str>) {
415        if self.forces_and_moments().is_empty() {
416            println!("Empty mirror");
417            return;
418        }
419
420        let max_value = |x: &[f64]| -> f64 {
421            x.iter()
422                .cloned()
423                .rev()
424                .take(400 * 20)
425                .fold(std::f64::NEG_INFINITY, f64::max)
426        };
427        let min_value = |x: &[f64]| -> f64 {
428            x.iter()
429                .cloned()
430                .rev()
431                .take(400 * 20)
432                .fold(std::f64::INFINITY, f64::min)
433        };
434
435        let plot =
436            BitMapBackend::new(filename.unwrap_or("MOMENT.png"), (768, 512)).into_drawing_area();
437        plot.fill(&WHITE).unwrap();
438
439        let (min_values, max_values): (Vec<_>, Vec<_>) = self
440            .forces_and_moments()
441            .values()
442            .map(|values| {
443                let moment_magnitude: Option<Vec<f64>> =
444                    values.iter().map(|e| e.moment.magnitude()).collect();
445                (
446                    min_value(moment_magnitude.as_ref().unwrap()),
447                    max_value(moment_magnitude.as_ref().unwrap()),
448                )
449            })
450            .unzip();
451        let xrange = (*self.time().front().unwrap(), *self.time().back().unwrap());
452        let minmax_padding = 0.1;
453        let mut chart = ChartBuilder::on(&plot)
454            .set_label_area_size(LabelAreaPosition::Left, 60)
455            .set_label_area_size(LabelAreaPosition::Bottom, 40)
456            .margin(10)
457            .build_cartesian_2d(
458                xrange.0..xrange.1 * (1. + 1e-2),
459                min_value(&min_values) * (1. - minmax_padding)
460                    ..max_value(&max_values) * (1. + minmax_padding),
461            )
462            .unwrap();
463        chart
464            .configure_mesh()
465            .x_desc("Time [s]")
466            .y_desc("Force [N]")
467            .draw()
468            .unwrap();
469
470        let mut colors = colorous::TABLEAU10.iter().cycle();
471
472        for (key, values) in self.forces_and_moments().iter() {
473            let color = colors.next().unwrap();
474            let rgb = RGBColor(color.r, color.g, color.b);
475            chart
476                .draw_series(LineSeries::new(
477                    self.time()
478                        .iter()
479                        .zip(values.iter())
480                        //.skip(10 * 20)
481                        .map(|(&x, y)| (x, y.moment.magnitude().unwrap())),
482                    &rgb,
483                ))
484                .unwrap()
485                .label(key)
486                .legend(move |(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], &rgb));
487        }
488        chart
489            .configure_series_labels()
490            .border_style(&BLACK)
491            .background_style(&WHITE.mix(0.8))
492            .position(SeriesLabelPosition::UpperRight)
493            .draw()
494            .unwrap();
495    }
496}
parse_monitors/monitors/mirror.rs

parse_monitors/monitors/
mirror.rs
