From 8d540c5cf238301f1805e3c451eb21aee04290fa Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sat, 1 Feb 2025 18:20:06 +0100 Subject: [PATCH 1/7] add some extra utility to the FSR data --- nidhogg/src/types/mod.rs | 42 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/nidhogg/src/types/mod.rs b/nidhogg/src/types/mod.rs index d252db2..dbb5751 100644 --- a/nidhogg/src/types/mod.rs +++ b/nidhogg/src/types/mod.rs @@ -165,7 +165,7 @@ impl ForceSensitiveResistors { } /// Struct representing the force sensitive resistors in one of the feet. -#[derive(Clone, Debug, Default, PartialEq)] +#[derive(Clone, Debug, Default, PartialEq, Filler)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "bevy", derive(Resource))] pub struct ForceSensitiveResistorFoot { @@ -197,6 +197,46 @@ impl ForceSensitiveResistorFoot { pub fn avg(&self) -> f32 { self.sum() / 4.0 } + + /// Compute the pressure leaning forward + pub fn forward_pressure(&self) -> f32 { + self.front_left + self.front_right + } + + /// Compute the pressure leaning backward + pub fn backward_pressure(&self) -> f32 { + self.rear_left + self.rear_right + } + + /// Compute the pressure leaning left + pub fn left_pressure(&self) -> f32 { + self.front_left + self.rear_left + } + + /// Compute the pressure leaning right + pub fn right_pressure(&self) -> f32 { + self.front_right + self.rear_right + } + + /// Compute the element-wise maximum for each sensor value + pub fn max_element(&self, other: &ForceSensitiveResistorFoot) -> Self { + Self { + front_left: self.front_left.max(other.front_left), + front_right: self.front_right.max(other.front_right), + rear_left: self.rear_left.max(other.rear_left), + rear_right: self.rear_right.max(other.rear_right), + } + } + + /// Compute the element-wise minimum for each sensor value + pub fn min_element(&self, other: &ForceSensitiveResistorFoot) -> Self { + Self { + front_left: self.front_left.min(other.front_left), + front_right: self.front_right.min(other.front_right), + rear_left: self.rear_left.min(other.rear_left), + rear_right: self.rear_right.min(other.rear_right), + } + } } /// Values read by the left and right sonar sensor. From 3b39f795f628fe0857a022ef4a404c0b45c81dbd Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sat, 1 Feb 2025 23:04:27 +0100 Subject: [PATCH 2/7] Add `weighted_sum` to fsr --- nidhogg/src/types/mod.rs | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/nidhogg/src/types/mod.rs b/nidhogg/src/types/mod.rs index dbb5751..e911e5a 100644 --- a/nidhogg/src/types/mod.rs +++ b/nidhogg/src/types/mod.rs @@ -158,6 +158,12 @@ impl ForceSensitiveResistors { self.left_foot.sum() + self.right_foot.sum() } + /// Compute the sum of the FSR sensor values, weighted by the provided weights. + pub fn weighted_sum(&self, weights: &ForceSensitiveResistors) -> f32 { + self.left_foot.weighted_sum(&weights.left_foot) + + self.right_foot.weighted_sum(&weights.right_foot) + } + /// Calculates the average weight based on the measurement from the resistors in both feet. pub fn avg(&self) -> f32 { (self.left_foot.avg() + self.right_foot.avg()) / 2.0 @@ -193,6 +199,14 @@ impl ForceSensitiveResistorFoot { self.front_left + self.front_right + self.rear_left + self.rear_right } + /// Compute the sum of the FSR sensor values, weighted by the provided weights. + pub fn weighted_sum(&self, weights: &ForceSensitiveResistorFoot) -> f32 { + (weights.front_left * self.front_left) + + (weights.front_right * self.front_right) + + (weights.rear_left * self.rear_left) + + (weights.rear_right * self.rear_right) + } + /// Calculates the average weight on the foot. pub fn avg(&self) -> f32 { self.sum() / 4.0 From 5d6564765b5e2a9dbba2714921f73cf614d6ffe9 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sat, 1 Feb 2025 23:05:34 +0100 Subject: [PATCH 3/7] Rename `ForceSensitiveResitorFoot` to `FsrFoot` --- nidhogg/src/backend/lola.rs | 6 +++--- nidhogg/src/types/mod.rs | 22 +++++++++++----------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/nidhogg/src/backend/lola.rs b/nidhogg/src/backend/lola.rs index ffc5f27..83eaf52 100644 --- a/nidhogg/src/backend/lola.rs +++ b/nidhogg/src/backend/lola.rs @@ -3,8 +3,8 @@ use crate::{ types::{ - Battery, ForceSensitiveResistorFoot, ForceSensitiveResistors, JointArray, LeftEar, LeftEye, - Rgb, RgbF32, RightEar, RightEye, Skull, SonarEnabled, SonarValues, Touch, + Battery, ForceSensitiveResistors, FsrFoot, JointArray, LeftEar, LeftEye, Rgb, RgbF32, + RightEar, RightEye, Skull, SonarEnabled, SonarValues, Touch, }, DisconnectExt, Error, HardwareInfo, NaoBackend, NaoControlMessage, NaoState, Result, }; @@ -429,7 +429,7 @@ impl FromLoLA<[f32; 8]> for ForceSensitiveResistors { } } -impl FromLoLA<[f32; 4]> for ForceSensitiveResistorFoot { +impl FromLoLA<[f32; 4]> for FsrFoot { fn from_lola(value: [f32; 4]) -> Self { Self { front_left: value[0], diff --git a/nidhogg/src/types/mod.rs b/nidhogg/src/types/mod.rs index e911e5a..1d0b0e8 100644 --- a/nidhogg/src/types/mod.rs +++ b/nidhogg/src/types/mod.rs @@ -141,15 +141,15 @@ pub struct Battery { pub temperature: f32, } -/// Struct containing the [`ForceSensitiveResistorFoot`] value for each foot. +/// Struct containing the [`FsrFoot`] value for each foot. #[derive(Clone, Debug, Default, PartialEq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "bevy", derive(Resource))] pub struct ForceSensitiveResistors { /// FSR values from the four sensors in the left foot. - pub left_foot: ForceSensitiveResistorFoot, + pub left_foot: FsrFoot, /// FSR values from the four sensors in the right foot. - pub right_foot: ForceSensitiveResistorFoot, + pub right_foot: FsrFoot, } impl ForceSensitiveResistors { @@ -174,7 +174,7 @@ impl ForceSensitiveResistors { #[derive(Clone, Debug, Default, PartialEq, Filler)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "bevy", derive(Resource))] -pub struct ForceSensitiveResistorFoot { +pub struct FsrFoot { /// FSR value representing the estimated weight in kilograms on the front left foot sensor. /// /// Please note that this value is approximate. @@ -193,14 +193,14 @@ pub struct ForceSensitiveResistorFoot { pub rear_right: f32, } -impl ForceSensitiveResistorFoot { +impl FsrFoot { /// Computes the sum of the FSR sensor values for the foot. pub fn sum(&self) -> f32 { self.front_left + self.front_right + self.rear_left + self.rear_right } /// Compute the sum of the FSR sensor values, weighted by the provided weights. - pub fn weighted_sum(&self, weights: &ForceSensitiveResistorFoot) -> f32 { + pub fn weighted_sum(&self, weights: &FsrFoot) -> f32 { (weights.front_left * self.front_left) + (weights.front_right * self.front_right) + (weights.rear_left * self.rear_left) @@ -233,7 +233,7 @@ impl ForceSensitiveResistorFoot { } /// Compute the element-wise maximum for each sensor value - pub fn max_element(&self, other: &ForceSensitiveResistorFoot) -> Self { + pub fn max_element(&self, other: &FsrFoot) -> Self { Self { front_left: self.front_left.max(other.front_left), front_right: self.front_right.max(other.front_right), @@ -243,7 +243,7 @@ impl ForceSensitiveResistorFoot { } /// Compute the element-wise minimum for each sensor value - pub fn min_element(&self, other: &ForceSensitiveResistorFoot) -> Self { + pub fn min_element(&self, other: &FsrFoot) -> Self { Self { front_left: self.front_left.min(other.front_left), front_right: self.front_right.min(other.front_right), @@ -398,13 +398,13 @@ mod tests { #[test] fn test_average_force_feet() { - let foot1 = ForceSensitiveResistorFoot { + let foot1 = FsrFoot { front_left: 0.0, front_right: 1.0, rear_left: 0.32, rear_right: 0.76, }; - let foot2 = ForceSensitiveResistorFoot { + let foot2 = FsrFoot { front_left: 0.54, front_right: 1.0, rear_left: 0.32, @@ -419,7 +419,7 @@ mod tests { #[test] fn test_average_weight_foot() { - let foot = ForceSensitiveResistorFoot { + let foot = FsrFoot { front_left: 0.0, front_right: 1.0, rear_left: 0.32, From 86e23a634210ef2ad8f01a9329379d29e8f93bbe Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 2 Feb 2025 15:55:21 +0100 Subject: [PATCH 4/7] apply feedback --- nidhogg/src/types/mod.rs | 176 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 168 insertions(+), 8 deletions(-) diff --git a/nidhogg/src/types/mod.rs b/nidhogg/src/types/mod.rs index 1d0b0e8..ea9c45f 100644 --- a/nidhogg/src/types/mod.rs +++ b/nidhogg/src/types/mod.rs @@ -1,6 +1,8 @@ //! Convenience types used to make interacting with the NAO more convenient. //! +use std::ops::{Add, Div, Mul, Neg, Sub}; + use nidhogg_derive::{Builder, Filler}; #[cfg(feature = "serde")] @@ -170,6 +172,61 @@ impl ForceSensitiveResistors { } } +impl Add for ForceSensitiveResistors { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self::Output { + left_foot: self.left_foot + rhs.left_foot, + right_foot: self.right_foot + rhs.right_foot, + } + } +} + +impl Sub for ForceSensitiveResistors { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self::Output { + left_foot: self.left_foot - rhs.left_foot, + right_foot: self.right_foot - rhs.right_foot, + } + } +} + +impl Mul for ForceSensitiveResistors { + type Output = Self; + + fn mul(self, rhs: Self) -> Self::Output { + Self::Output { + left_foot: self.left_foot * rhs.left_foot, + right_foot: self.right_foot * rhs.right_foot, + } + } +} + +impl Div for ForceSensitiveResistors { + type Output = Self; + + fn div(self, rhs: Self) -> Self::Output { + Self::Output { + left_foot: self.left_foot / rhs.left_foot, + right_foot: self.right_foot / rhs.right_foot, + } + } +} + +impl Neg for ForceSensitiveResistors { + type Output = Self; + + fn neg(self) -> Self::Output { + Self::Output { + left_foot: -self.left_foot, + right_foot: -self.right_foot, + } + } +} + /// Struct representing the force sensitive resistors in one of the feet. #[derive(Clone, Debug, Default, PartialEq, Filler)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] @@ -212,28 +269,48 @@ impl FsrFoot { self.sum() / 4.0 } - /// Compute the pressure leaning forward + /// Computes the total pressure on the front sensors of the foot. + /// + /// # Note + /// + /// Since this value is the sum of two sensors, it can be up to twice as large + /// as the reading from a single sensor. pub fn forward_pressure(&self) -> f32 { self.front_left + self.front_right } - /// Compute the pressure leaning backward + /// Computes the total pressure on the rear sensors of the foot. + /// + /// # Note + /// + /// Since this value is the sum of two sensors, it can be up to twice as large + /// as the reading from a single sensor. pub fn backward_pressure(&self) -> f32 { self.rear_left + self.rear_right } - /// Compute the pressure leaning left + /// Computes the total pressure on the left sensors of the foot. + /// + /// # Note + /// + /// Since this value is the sum of two sensors, it can be up to twice as large + /// as the reading from a single sensor. pub fn left_pressure(&self) -> f32 { self.front_left + self.rear_left } - /// Compute the pressure leaning right + /// Computes the total pressure on the right sensors of the foot. + /// + /// # Note + /// + /// Since this value is the sum of two sensors, it can be up to twice as large + /// as the reading from a single sensor. pub fn right_pressure(&self) -> f32 { self.front_right + self.rear_right } - /// Compute the element-wise maximum for each sensor value - pub fn max_element(&self, other: &FsrFoot) -> Self { + /// Compute the supremum (element-wise maximum) for each sensor value. + pub fn sup(&self, other: &FsrFoot) -> Self { Self { front_left: self.front_left.max(other.front_left), front_right: self.front_right.max(other.front_right), @@ -242,8 +319,17 @@ impl FsrFoot { } } - /// Compute the element-wise minimum for each sensor value - pub fn min_element(&self, other: &FsrFoot) -> Self { + /// Compute the element-wise maximum for each sensor value. + /// + /// # Note + /// + /// This is an alias for [`Self::sup`]. + pub fn max_per_sensor(&self, other: &FsrFoot) -> Self { + self.sup(other) + } + + /// Compute the infimum (element-wise minimum) for each sensor value. + pub fn inf(&self, other: &FsrFoot) -> Self { Self { front_left: self.front_left.min(other.front_left), front_right: self.front_right.min(other.front_right), @@ -251,6 +337,80 @@ impl FsrFoot { rear_right: self.rear_right.min(other.rear_right), } } + + /// Compute the element-wise minimum for each sensor value. + /// + /// # Note + /// + /// This is an alias for [`Self::inf`]. + pub fn min_per_sensor(&self, other: &FsrFoot) -> Self { + self.inf(other) + } +} + +impl Add for FsrFoot { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self::Output { + front_left: self.front_left + rhs.front_left, + front_right: self.front_right + rhs.front_right, + rear_left: self.rear_left + rhs.rear_left, + rear_right: self.rear_right + rhs.rear_right, + } + } +} + +impl Sub for FsrFoot { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self::Output { + front_left: self.front_left - rhs.front_left, + front_right: self.front_right - rhs.front_right, + rear_left: self.rear_left - rhs.rear_left, + rear_right: self.rear_right - rhs.rear_right, + } + } +} + +impl Mul for FsrFoot { + type Output = Self; + + fn mul(self, rhs: Self) -> Self::Output { + Self::Output { + front_left: self.front_left * rhs.front_left, + front_right: self.front_right * rhs.front_right, + rear_left: self.rear_left * rhs.rear_left, + rear_right: self.rear_right * rhs.rear_right, + } + } +} + +impl Div for FsrFoot { + type Output = Self; + + fn div(self, rhs: Self) -> Self::Output { + Self::Output { + front_left: self.front_left / rhs.front_left, + front_right: self.front_right / rhs.front_right, + rear_left: self.rear_left / rhs.rear_left, + rear_right: self.rear_right / rhs.rear_right, + } + } +} + +impl Neg for FsrFoot { + type Output = Self; + + fn neg(self) -> Self::Output { + Self::Output { + front_left: -self.front_left, + front_right: -self.front_right, + rear_left: -self.rear_left, + rear_right: -self.rear_right, + } + } } /// Values read by the left and right sonar sensor. From a15441bbd53402cf7df08facc7ea33f6cbdfad7e Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 2 Feb 2025 15:55:37 +0100 Subject: [PATCH 5/7] increment version --- nidhogg/Cargo.toml | 2 +- nidhogg_derive/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nidhogg/Cargo.toml b/nidhogg/Cargo.toml index ec89417..a7572f5 100644 --- a/nidhogg/Cargo.toml +++ b/nidhogg/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "nidhogg" -version = "0.3.0" +version = "0.4.0" edition = "2021" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/nidhogg_derive/Cargo.toml b/nidhogg_derive/Cargo.toml index c467dcb..b5d16e2 100644 --- a/nidhogg_derive/Cargo.toml +++ b/nidhogg_derive/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "nidhogg_derive" -version = "0.1.0" +version = "0.4.0" edition = "2021" [lib] From a2237ecdb6c498eee94b2bd8646214d3748bb282 Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 2 Feb 2025 15:57:36 +0100 Subject: [PATCH 6/7] Rename `ForceSensitiveResitors` to `Fsr` --- nidhogg/src/backend/lola.rs | 6 +++--- nidhogg/src/lib.rs | 6 +++--- nidhogg/src/types/mod.rs | 18 +++++++++--------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/nidhogg/src/backend/lola.rs b/nidhogg/src/backend/lola.rs index 83eaf52..e4757b4 100644 --- a/nidhogg/src/backend/lola.rs +++ b/nidhogg/src/backend/lola.rs @@ -3,8 +3,8 @@ use crate::{ types::{ - Battery, ForceSensitiveResistors, FsrFoot, JointArray, LeftEar, LeftEye, Rgb, RgbF32, - RightEar, RightEye, Skull, SonarEnabled, SonarValues, Touch, + Battery, Fsr, FsrFoot, JointArray, LeftEar, LeftEye, Rgb, RgbF32, RightEar, RightEye, + Skull, SonarEnabled, SonarValues, Touch, }, DisconnectExt, Error, HardwareInfo, NaoBackend, NaoControlMessage, NaoState, Result, }; @@ -417,7 +417,7 @@ impl FromLoLA<[f32; 4]> for Battery { } } -impl FromLoLA<[f32; 8]> for ForceSensitiveResistors { +impl FromLoLA<[f32; 8]> for Fsr { fn from_lola(value: [f32; 8]) -> Self { let left: [f32; 4] = value[..4].try_into().unwrap(); let right: [f32; 4] = value[4..].try_into().unwrap(); diff --git a/nidhogg/src/lib.rs b/nidhogg/src/lib.rs index f33cc8f..4f58a1b 100644 --- a/nidhogg/src/lib.rs +++ b/nidhogg/src/lib.rs @@ -42,8 +42,8 @@ pub use error::{Error, Result}; use nalgebra::{Vector2, Vector3}; use nidhogg_derive::Builder; use types::{ - color::RgbF32, Battery, FillExt, ForceSensitiveResistors, JointArray, LeftEar, LeftEye, - RightEar, RightEye, Skull, SonarEnabled, SonarValues, Touch, + color::RgbF32, Battery, FillExt, Fsr, JointArray, LeftEar, LeftEye, RightEar, RightEye, Skull, + SonarEnabled, SonarValues, Touch, }; #[cfg(feature = "serde")] @@ -150,7 +150,7 @@ pub struct NaoState { /// /// A more detailed explanation of the different wave kinds can be found [here](http://doc.aldebaran.com/2-8/family/nao_technical/lola/actuator_sensor_names.html#sonars). pub sonar: SonarValues, - pub force_sensitive_resistors: ForceSensitiveResistors, + pub force_sensitive_resistors: Fsr, pub touch: Touch, // Diagnostics diff --git a/nidhogg/src/types/mod.rs b/nidhogg/src/types/mod.rs index ea9c45f..c4a5924 100644 --- a/nidhogg/src/types/mod.rs +++ b/nidhogg/src/types/mod.rs @@ -147,21 +147,21 @@ pub struct Battery { #[derive(Clone, Debug, Default, PartialEq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "bevy", derive(Resource))] -pub struct ForceSensitiveResistors { +pub struct Fsr { /// FSR values from the four sensors in the left foot. pub left_foot: FsrFoot, /// FSR values from the four sensors in the right foot. pub right_foot: FsrFoot, } -impl ForceSensitiveResistors { +impl Fsr { /// Computes the sum of the FSR sensor values for both feet. pub fn sum(&self) -> f32 { self.left_foot.sum() + self.right_foot.sum() } /// Compute the sum of the FSR sensor values, weighted by the provided weights. - pub fn weighted_sum(&self, weights: &ForceSensitiveResistors) -> f32 { + pub fn weighted_sum(&self, weights: &Fsr) -> f32 { self.left_foot.weighted_sum(&weights.left_foot) + self.right_foot.weighted_sum(&weights.right_foot) } @@ -172,7 +172,7 @@ impl ForceSensitiveResistors { } } -impl Add for ForceSensitiveResistors { +impl Add for Fsr { type Output = Self; fn add(self, rhs: Self) -> Self::Output { @@ -183,7 +183,7 @@ impl Add for ForceSensitiveResistors { } } -impl Sub for ForceSensitiveResistors { +impl Sub for Fsr { type Output = Self; fn sub(self, rhs: Self) -> Self::Output { @@ -194,7 +194,7 @@ impl Sub for ForceSensitiveResistors { } } -impl Mul for ForceSensitiveResistors { +impl Mul for Fsr { type Output = Self; fn mul(self, rhs: Self) -> Self::Output { @@ -205,7 +205,7 @@ impl Mul for ForceSensitiveResistors { } } -impl Div for ForceSensitiveResistors { +impl Div for Fsr { type Output = Self; fn div(self, rhs: Self) -> Self::Output { @@ -216,7 +216,7 @@ impl Div for ForceSensitiveResistors { } } -impl Neg for ForceSensitiveResistors { +impl Neg for Fsr { type Output = Self; fn neg(self) -> Self::Output { @@ -570,7 +570,7 @@ mod tests { rear_left: 0.32, rear_right: 0.95, }; - let feet = ForceSensitiveResistors { + let feet = Fsr { left_foot: foot1, right_foot: foot2, }; From 46555cdfb52fce640bbc57d3ac6c8d3c62f21dfb Mon Sep 17 00:00:00 2001 From: Gijs de Jong Date: Sun, 2 Feb 2025 15:58:57 +0100 Subject: [PATCH 7/7] Rename `force_sensitive_resistors` to `fsr` --- nidhogg/src/backend/lola.rs | 2 +- nidhogg/src/lib.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nidhogg/src/backend/lola.rs b/nidhogg/src/backend/lola.rs index e4757b4..212ade2 100644 --- a/nidhogg/src/backend/lola.rs +++ b/nidhogg/src/backend/lola.rs @@ -530,7 +530,7 @@ impl From> for NaoState { gyroscope: value.gyroscope.into_nidhogg(), angles: value.angles.into_nidhogg(), sonar: value.sonar.into_nidhogg(), - force_sensitive_resistors: value.f_s_r.into_nidhogg(), + fsr: value.f_s_r.into_nidhogg(), touch: value.touch.into_nidhogg(), status: value.status.into_nidhogg(), } diff --git a/nidhogg/src/lib.rs b/nidhogg/src/lib.rs index 4f58a1b..980d9ce 100644 --- a/nidhogg/src/lib.rs +++ b/nidhogg/src/lib.rs @@ -150,7 +150,7 @@ pub struct NaoState { /// /// A more detailed explanation of the different wave kinds can be found [here](http://doc.aldebaran.com/2-8/family/nao_technical/lola/actuator_sensor_names.html#sonars). pub sonar: SonarValues, - pub force_sensitive_resistors: Fsr, + pub fsr: Fsr, pub touch: Touch, // Diagnostics