ML Fall state detection

Cargo.toml

@@ -216,6 +216,7 @@ structopt = "0.3.26"
 syn = { version = "2.0.98", features = ["extra-traits", "full"] }
 systemd = "0.10.0"
 tempfile = "3.17.0"
+tflite = { path = "../../tflite-rs" }
 thiserror = "2.0.11"
 threadbound = "0.1.7"
 tokio = { version = "1.43.0", features = ["full"] }

crates/bevyhavior_simulator/build.rs

@@ -31,6 +31,7 @@ fn main() -> Result<()> {
                     "control::primary_state_filter",
                     "control::referee_position_provider",
                     "control::ready_signal_detection_filter",
+                    "control::fall_state_detection",
                     "control::role_assignment",
                     "control::rule_obstacle_composer",
                     "control::search_suggestor",

crates/bevyhavior_simulator/src/fake_data.rs

@@ -1,7 +1,7 @@
 use std::{net::SocketAddr, time::Duration};
 
 use color_eyre::Result;
-use linear_algebra::Isometry2;
+use linear_algebra::{Isometry2, Orientation3};
 use projection::camera_matrices::CameraMatrices;
 use serde::{Deserialize, Serialize};
 
@@ -67,6 +67,7 @@ pub struct MainOutputs {
     pub calibration_command: MainOutput<Option<CalibrationCommand>>,
     pub stand_up_front_estimated_remaining_duration: MainOutput<Option<Duration>>,
     pub camera_matrices: MainOutput<Option<CameraMatrices>>,
+    pub robot_orientation: MainOutput<Option<Orientation3<Field>>>,
 }
 
 impl FakeData {
@@ -107,6 +108,7 @@ impl FakeData {
                 .into(),
             calibration_command: last_database.calibration_command.into(),
             camera_matrices: last_database.camera_matrices.clone().into(),
+            robot_orientation: last_database.robot_orientation.into(),
         })
     }
 }

crates/bevyhavior_simulator/src/interfake.rs

@@ -1,9 +1,11 @@
 use std::{
     mem::take,
+    path::PathBuf,
     sync::Arc,
     time::{SystemTime, UNIX_EPOCH},
 };
 
+use hula_types::hardware::Paths;
 use parking_lot::Mutex;
 
 use buffered_watch::{Receiver, Sender};
@@ -70,7 +72,11 @@ impl SpeakerInterface for Interfake {
 
 impl PathsInterface for Interfake {
     fn get_paths(&self) -> hula_types::hardware::Paths {
-        unimplemented!()
+        Paths {
+            motions: PathBuf::from("etc/motions"),
+            neural_networks: PathBuf::from("etc/neural_networks"),
+            sounds: PathBuf::from("etc/sounds"),
+        }
     }
 }
 

crates/control/Cargo.toml

@@ -38,5 +38,6 @@ serde = { workspace = true }
 smallvec = { workspace = true }
 spl_network_messages = { workspace = true }
 splines = { workspace = true }
+tflite = { workspace = true }
 types = { workspace = true }
 walking_engine = { workspace = true }

crates/control/src/fall_state_detection.rs

@@ -0,0 +1,138 @@
+use std::collections::VecDeque;
+
+use color_eyre::Result;
+use hardware::PathsInterface;
+use serde::{Deserialize, Serialize};
+
+use context_attribute::context;
+use framework::{deserialize_not_implemented, MainOutput};
+use tflite::{ops::builtin::BuiltinOpResolver, FlatBufferModel, InterpreterBuilder};
+use types::{
+    fall_state::{FallState, FallStateTinyML},
+    sensor_data::SensorData,
+};
+
+#[derive(Deserialize, Serialize)]
+pub struct FallStateDetection {
+    last_fall_state: FallState,
+    #[serde(skip, default = "deserialize_not_implemented")]
+    model: FlatBufferModel,
+    datas: Vec<VecDeque<f32>>,
+}
+
+#[context]
+pub struct CreationContext {
+    hardware_interface: HardwareInterface,
+}
+
+#[context]
+pub struct CycleContext {
+    sensor_data: Input<SensorData, "sensor_data">,
+    has_ground_contact: Input<bool, "has_ground_contact">,
+}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+    pub fall_state_tinyml: MainOutput<FallStateTinyML>,
+    pub soft_fall_state_tinyml: MainOutput<f32>,
+}
+
+impl FallStateDetection {
+    pub fn new(context: CreationContext<impl PathsInterface>) -> Result<Self> {
+        let paths = context.hardware_interface.get_paths();
+        let neural_network_folder = paths.neural_networks;
+
+        let model_path = neural_network_folder.join("fall_detection_shift_23.tflite");
+
+        let model = FlatBufferModel::build_from_file(model_path)?;
+
+        Ok(Self {
+            last_fall_state: Default::default(),
+            model,
+            datas: vec![VecDeque::new(); 6],
+        })
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        // TODO: hadle primary state unstiff
+
+        let inertial_measurement_unit = context.sensor_data.inertial_measurement_unit;
+        let linear_accelerations = inertial_measurement_unit.linear_acceleration.inner;
+        let roll_pitch = inertial_measurement_unit.roll_pitch.inner;
+        let has_ground_contact = if *context.has_ground_contact {
+            1.0
+        } else {
+            0.0
+        };
+
+        let resolver = BuiltinOpResolver::default();
+
+        let builder = InterpreterBuilder::new(&self.model, &resolver)?;
+        let mut interpreter = builder.build()?;
+        interpreter.set_num_threads(1);
+
+        interpreter.allocate_tensors()?;
+
+        let inputs = interpreter.inputs().to_vec();
+        assert_eq!(inputs.len(), 1);
+
+        let input_index = inputs[0];
+
+        let outputs = interpreter.outputs().to_vec();
+        assert_eq!(outputs.len(), 1);
+
+        let output_index = outputs[0];
+
+        let input_tensor = interpreter.tensor_info(input_index).unwrap();
+
+        self.datas[0].push_back(linear_accelerations.x);
+        self.datas[1].push_back(linear_accelerations.y);
+        self.datas[2].push_back(linear_accelerations.z);
+        self.datas[3].push_back(roll_pitch.x);
+        self.datas[4].push_back(roll_pitch.y);
+        self.datas[5].push_back(has_ground_contact);
+
+        let max_size = input_tensor.dims[1];
+        for i in 0..input_tensor.dims[2] {
+            while self.datas[i].len() > max_size {
+                self.datas[i].pop_front();
+            }
+        }
+
+        dbg!(input_tensor.dims);
+
+        if self.datas[0].len() == max_size {
+            interpreter.tensor_data_mut(input_index).unwrap()[0..max_size]
+                .copy_from_slice(self.datas[0].make_contiguous());
+            interpreter.tensor_data_mut(input_index).unwrap()[max_size..max_size * 2]
+                .copy_from_slice(self.datas[1].make_contiguous());
+            interpreter.tensor_data_mut(input_index).unwrap()[max_size * 2..max_size * 3]
+                .copy_from_slice(self.datas[2].make_contiguous());
+            interpreter.tensor_data_mut(input_index).unwrap()[max_size * 3..max_size * 4]
+                .copy_from_slice(self.datas[3].make_contiguous());
+            interpreter.tensor_data_mut(input_index).unwrap()[max_size * 4..max_size * 5]
+                .copy_from_slice(self.datas[4].make_contiguous());
+            interpreter.tensor_data_mut(input_index).unwrap()[max_size * 5..max_size * 6]
+                .copy_from_slice(self.datas[5].make_contiguous());
+        }
+
+        interpreter.invoke()?;
+
+        let output: &[f32] = interpreter.tensor_data(output_index)?;
+        assert!(output.len() == 1);
+
+        let guess = *output.first().unwrap();
+
+        let fall_state = if guess <= 5.0 {
+            FallStateTinyML::Stable
+        } else {
+            FallStateTinyML::SoonToBeUnstable
+        };
+
+        Ok(MainOutputs {
+            fall_state_tinyml: fall_state.into(),
+            soft_fall_state_tinyml: guess.into(),
+        })
+    }
+}

crates/control/src/lib.rs

@@ -8,6 +8,7 @@ pub mod calibration_controller;
 pub mod camera_matrix_calculator;
 pub mod center_of_mass_provider;
 pub mod dribble_path_planner;
+pub mod fall_state_detection;
 pub mod fall_state_estimation;
 pub mod filtered_game_controller_state_timer;
 pub mod foot_bumper_filter;

crates/hulk_manifest/src/lib.rs

@@ -56,6 +56,7 @@ pub fn collect_hulk_cyclers(root: impl AsRef<Path>) -> Result<Cyclers, Error> {
                     "control::center_of_mass_provider",
                     "control::dribble_path_planner",
                     "control::fall_state_estimation",
+                    "control::fall_state_detection",
                     "control::filtered_game_controller_state_timer",
                     "control::foot_bumper_filter",
                     "control::free_kick_signal_filter",

crates/types/src/fall_state.rs

@@ -95,3 +95,21 @@ pub enum FallState {
         kind: Kind,
     },
 }
+
+#[derive(
+    Clone,
+    Copy,
+    Debug,
+    Default,
+    Deserialize,
+    PartialEq,
+    Serialize,
+    PathSerialize,
+    PathDeserialize,
+    PathIntrospect,
+)]
+pub enum FallStateTinyML {
+    #[default]
+    Stable,
+    SoonToBeUnstable,
+}

tools/machine-learning/fall_detection/.gitignore

@@ -0,0 +1,5 @@
+*.parquet
+*.tflite
+*.keras
+wandb/
+confusion_matrix.html

tools/machine-learning/fall_detection/.python-version

@@ -0,0 +1 @@
+3.12