gmt_dos-clients_m1-ctrl 6.0.0

GMT DOS M1 Control Client
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103

use gmt_dos_clients_io::gmt_m1::segment::{HardpointsForces, RBM};
#[cfg(all(m1_hp_force_extension, not(feature = "explicit-loadcells")))]
use hardpoints_dynamics::HardpointsDynamics;
#[cfg(any(not(m1_hp_force_extension), feature = "explicit-loadcells"))]
use hardpoints_dynamics_as_design::HardpointsDynamics;
use interface::{Data, Read, Size, Update, Write};

mod loadcell;
pub use loadcell::LoadCells;
use serde::{Deserialize, Serialize};

type M = nalgebra::Matrix6<f64>;
type V = nalgebra::Vector6<f64>;

/// [gmt_dos_actors](https://docs.rs/gmt_dos-actors) client interface for [HardpointsDynamics]
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Hardpoints<const ID: u8> {
    dynamics: HardpointsDynamics,
    // FEM M1 RBM to FEM HP force input
    rbm_2_hp: M,
    // hardpoints stiffness (from FEM static gain)
    m1_hpk: f64,
}
impl<const ID: u8> From<&Calibration> for Hardpoints<ID> {
    fn from(c: &Calibration) -> Self {
        Self {
            dynamics: HardpointsDynamics::new(),
            rbm_2_hp: c.rbm_2_hp[ID as usize - 1],
            m1_hpk: c.stiffness,
        }
    }
}
impl<const ID: u8> Hardpoints<ID> {
    /// Creates a new hardpoints client
    ///
    /// The hardpoints stiffness and the matrix transformation
    /// from rigid body motion to hardpoint motion are provided.
    pub fn new(stiffness: f64, rbm_2_hp: M) -> Self {
        Self {
            dynamics: HardpointsDynamics::new(),
            rbm_2_hp,
            m1_hpk: stiffness,
        }
    }
}

impl<const ID: u8> Size<RBM<ID>> for Hardpoints<ID> {
    fn len(&self) -> usize {
        6
    }
}

impl<const ID: u8> Size<HardpointsForces<ID>> for Hardpoints<ID> {
    fn len(&self) -> usize {
        6
    }
}

impl<const ID: u8> Read<RBM<ID>> for Hardpoints<ID> {
    fn read(&mut self, data: Data<RBM<ID>>) {
        let hp = self.rbm_2_hp * V::from_column_slice(&data);
        self.dynamics.inputs.In1 = hp
            .as_slice()
            .try_into()
            .expect("failed to import `RBM` in `HardpointsDynamics` input");
    }
}

impl<const ID: u8> Update for Hardpoints<ID> {
    fn update(&mut self) {
        self.dynamics.step();
    }
}

impl<const ID: u8> Write<HardpointsForces<ID>> for Hardpoints<ID> {
    fn write(&mut self) -> Option<Data<HardpointsForces<ID>>> {
        let data: Vec<f64> = self
            .dynamics
            .outputs
            .Out1
            .iter()
            .map(|d| *d * self.m1_hpk)
            .collect();
        Some(Data::new(data))
    }
}
#[cfg(m1_hp_force_extension)]
use gmt_dos_clients_io::gmt_m1::segment::HardpointsMotion;

use crate::Calibration;
#[cfg(m1_hp_force_extension)]
impl<const ID: u8> Write<HardpointsMotion<ID>> for Hardpoints<ID> {
    fn write(&mut self) -> Option<Data<HardpointsMotion<ID>>> {
        let data: Vec<f64> = self.dynamics.outputs.Out1.to_vec();
        Some(Data::new(data))
    }
}
#[cfg(m1_hp_force_extension)]
impl<const ID: u8> Size<HardpointsMotion<ID>> for Hardpoints<ID> {
    fn len(&self) -> usize {
        6
    }
}