Add WrenchSmoother (or more general ArraySmoother)?

[Victorlouisdg]

Describe the feature you'd like
For the cloth competition, I noticed the FT-measurements can be quite noisy, so I implemented with quick "smoother" class (see below). I believe a more general and configurable version of this class could be useful.

Use cases

• People how want to do simple logic with the force-torque measurements e.g. check if tension > 4.0 N.
• People working with other low-dimensional sensors with continuous measurements.

Discussion points

• Do we want this in airo-mono
• Where would we put it? Maybe a new airo-sensors package?
• Maybe make it an "array smoother"? This can support scalar, 1D, 2D data.
• Sample timestamps and smooth over timeframe or last N measurements? (e.g. for non-consistent data streams)

wrench_smoother.py

from collections import deque

import scipy
from airo_typing import WrenchType


class WrenchSmoother:
    def __init__(self, history: int):
        self.history = history
        self.sigma = 2 * history + 1

        self.forces_x = deque(maxlen=history)
        self.forces_y = deque(maxlen=history)
        self.forces_z = deque(maxlen=history)
        self.torques_x = deque(maxlen=history)
        self.torques_y = deque(maxlen=history)
        self.torques_z = deque(maxlen=history)

        self.wrench_deques = [
            self.forces_x,
            self.forces_y,
            self.forces_z,
            self.torques_x,
            self.torques_y,
            self.torques_z,
        ]

    def smooth(self, wrench_deque: deque) -> float:
        return scipy.ndimage.gaussian_filter1d(wrench_deque, sigma=self.sigma)[-1]

    def smooth_wrench(self) -> WrenchType:
        wrench = []

        for wrench_deque in self.wrench_deques:
            wrench.append(self.smooth(wrench_deque))

        return wrench

    def add_wrench(self, wrench: WrenchType) -> WrenchType:
        for i, component in enumerate(wrench):
            self.wrench_deques[i].append(component)

        return self.smooth_wrench()

[tlpss]

I'm not against it, but should we make a new package for this? For me this can be included in airo-robots? And for that matter, do we already support the FT sensor in the first place?

[tlpss]

And I would make this a little more generic by calling the class 'Filter' and then have a subclass for your specific choice?
Could indeed also make it more generic by just working on ND numpy arrays.