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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

// Copyright (c) 2021 Marco Boneberger
// Licensed under the EUPL-1.2-or-later

//! Contains functions for filtering signals with a low-pass filter.
#![allow(non_upper_case_globals)]

use crate::utils::array_to_isometry;
use std::f64::consts::PI;

/// Maximum cutoff frequency: 1000 Hz
pub static kMaxCutoffFrequency: f64 = 1000.0;
///  Default cutoff frequency: 100 Hz
pub static kDefaultCutoffFrequency: f64 = 100.0;

/// Applies a first-order low-pass filter
///
/// # Arguments
/// * `sample_time` - Sample time constant
/// * `y` - Current value of the signal to be filtered
/// * `y_last` - Value of the signal to be filtered in the previous time step
/// * `cutoff_frequency` -Cutoff frequency of the low-pass filter
/// # Panics
/// This function panics if:
/// * y is infinite or NaN.
/// * y_last is infinite or NaN.
/// * cutoff_frequency is zero, negative, infinite or NaN.
/// * sample_time is negative, infinite or NaN.
/// # Return
/// Filtered value.
pub fn low_pass_filter(sample_time: f64, y: f64, y_last: f64, cutoff_frequency: f64) -> f64 {
    assert!(sample_time.is_sign_positive() && sample_time.is_finite());
    assert!(cutoff_frequency.is_sign_positive() && cutoff_frequency.is_finite());
    assert!(y.is_finite() && y_last.is_finite());
    let gain = sample_time / (sample_time + (1.0 / (2.0 * PI * cutoff_frequency)));
    gain * y + (1. - gain) * y_last
}

/// Applies a first-order low-pass filter to the translation and spherical linear interpolation
/// to the rotation of a transformation matrix which represents a Cartesian Motion.
///
/// # Arguments
/// * `sample_time` - Sample time constant
/// * `y` - Current Cartesian transformation matrix to be filtered
/// * `y_last` - Cartesian transformation matrix from the previous time step
/// * `cutoff_frequency` - Cutoff frequency of the low-pass filter
/// # Panics
/// This function panics if:
/// * elements of y is infinite or NaN.
/// * elements of y_last is infinite or NaN.
/// * cutoff_frequency is zero, negative, infinite or NaN.
/// * sample_time is negative, infinite or NaN.
/// # Return
/// Filtered Cartesian transformation matrix.
pub fn cartesian_low_pass_filter(
    sample_time: f64,
    y: &[f64; 16],
    y_last: &[f64; 16],
    cutoff_frequency: f64,
) -> [f64; 16] {
    assert!(sample_time.is_sign_positive() && sample_time.is_finite());
    assert!(cutoff_frequency.is_sign_positive() && cutoff_frequency.is_finite());
    y.iter()
        .zip(y_last.iter())
        .for_each(|(i, j)| assert!(i.is_finite() && j.is_finite()));
    let mut transform = array_to_isometry(&y);
    let transform_last = array_to_isometry(&y_last);
    // let mut orientation = transform.rotation;
    // let orientation_last = transform.rotation;
    let gain = sample_time / (sample_time + (1.0 / (2.0 * PI * cutoff_frequency)));
    transform.translation.vector =
        gain * transform.translation.vector + (1. - gain) * transform_last.translation.vector;
    transform.rotation = transform_last.rotation.slerp(&transform.rotation, gain);

    let mut out = [0.; 16];
    for (i, &x) in transform.to_homogeneous().iter().enumerate() {
        out[i] = x;
    }
    out
}

#[cfg(test)]
mod tests {
    use crate::robot::low_pass_filter::{cartesian_low_pass_filter, low_pass_filter};

    #[test]
    fn low_pass_test() {
        assert!(f64::abs(low_pass_filter(0.001, 1.0, 1.0, 100.0) - 1.) < 0.000001);
        assert!(f64::abs(low_pass_filter(0.001, 1.0, 1.0, 500.0) - 1.) < 0.000001);
        assert!(f64::abs(low_pass_filter(0.001, 1.0, 1.0, 1000.0) - 1.) < 0.000001);
        assert!(f64::abs(low_pass_filter(0.001, 1.0, 0.0, 100.0) - 0.3859) < 0.0001);
        assert!(f64::abs(low_pass_filter(0.001, 1.0, 0.0, 500.0) - 0.7585) < 0.0001);
        assert!(f64::abs(low_pass_filter(0.001, 1.0, 0.0, 900.0) - 0.8497) < 0.0001);
    }

    #[test]
    fn low_pass_cartesian_test() {
        let sample_time = 0.001;
        let cutoff_frequency = 100.;
        // let y = [0.9999903734042686, 0.000000010688512130929695, -0.0000000038600565380547064, 0.0, 0.000000010688512130929695, -0.9999903734042686, 0.000000001402494408653832, 0.0, -0.00000000386009369835821, -0.000000001402507951289548, -1.0, 0.0, 0.30689056676830223, 0.0000000030382738099574655, 0.48688205090223136, 1.0];
        // let y_last = [0.9999903734042686, 0.000000010688512130929695, -0.0000000038600565380547064, 0.0, 0.000000010688512130929695, -0.9999903734042686, 0.000000001402494408653832, 0.0, -0.00000000386009369835821, -0.000000001402507951289548, -1.0, 0.0, 0.30689056676830223, 0.0000000030382738099574655, 0.48688205090223136, 1.0];
        let y = [
            0.9999903734042686,
            0.0000000002540163079878255,
            -0.00000000012581154368346085,
            0.0,
            0.0000000002540163079878255,
            -0.9999903734042686,
            0.00000000004614105974113725,
            0.0,
            -0.00000000012581275483128821,
            -0.000000000046141503958700795,
            -1.0,
            0.0,
            0.30689056659844144,
            0.0000000000692086410879149,
            0.4868820527992277,
            1.0,
        ];
        let y_last = [
            0.9999903734042686,
            0.0000000002540163079878255,
            -0.00000000012581154368346085,
            0.0,
            0.0000000002540163079878255,
            -0.9999903734042686,
            0.00000000004614105974113725,
            0.0,
            -0.00000000012581275483128821,
            -0.000000000046141503958700795,
            -1.0,
            0.0,
            0.30689056659844144,
            0.0000000000692086410879149,
            0.4868820527992277,
            1.0,
        ];

        println!(
            "{:?}",
            cartesian_low_pass_filter(sample_time, &y, &y_last, cutoff_frequency)
        );
    }
}