Only move the filter when the simulator is stepped.

omnigibson/controllers/controller_base.py

@@ -233,6 +233,12 @@ def compute_control(self, control_dict):
         """
         control = self._command_to_control(command=self._command, control_dict=control_dict)
         return self.clip_control(control=control)
+
+    def step(self):
+        """
+        Steps the internal state of this controller, e.g. filters. Can be implemented by subclass.
+        """
+        pass
 
     def reset(self):
         """

omnigibson/controllers/ik_controller.py

@@ -173,6 +173,10 @@ def limiter(command_pos: Array[float], command_quat: Array[float], control_dict:
             command_output_limits=command_output_limits,
         )
 
+    def step(self):
+        if self.control_filter is not None:
+            self.control_filter.step()
+
     def reset(self):
         # Reset the filter and clear internal control state
         if self.control_filter is not None:

omnigibson/objects/controllable_object.py

@@ -338,6 +338,8 @@ def step(self):
                 "value": controller.compute_control(control_dict=control_dict),
                 "type": controller.control_type,
             }
+            # Step the controller to update internal state
+            controller.step()
             # Update idx
             idx += controller.command_dim
 

omnigibson/utils/processing_utils.py

@@ -8,7 +8,8 @@ class Filter(Serializable):
     """
     def estimate(self, observation):
         """
-        Takes an observation and returns a de-noised estimate.
+        Takes an observation and returns a de-noised estimate. Should return the same value if
+        called multiple times in a row without an intervening step() call.
 
         Args:
             observation (n-array): A current observation.
@@ -18,6 +19,10 @@ def estimate(self, observation):
         """
         raise NotImplementedError
 
+    def step(self):
+        """Updates the filter's internal state after a simulation step. Default is no-op."""
+        pass
+
     def reset(self):
         """
         Resets this filter. Default is no-op.
@@ -69,7 +74,9 @@ def __init__(self, obs_dim, filter_width):
 
     def estimate(self, observation):
         """
-        Do an online hold for state estimation given a recent observation.
+        Do an online hold for state estimation given a recent observation. Note that this will
+        repeatedly update the same current step's observation until the current index is moved
+        along by the step function.
 
         Args:
             observation (n-array): New observation to hold internal estimate of state.
@@ -89,10 +96,11 @@ def estimate(self, observation):
         else:
             val = self.past_samples.mean(axis=0)
 
-        # Increment the index to write the next sample to
-        self.current_idx = (self.current_idx + 1) % self.filter_width
-
         return val
+
+    def step(self):
+        """Increment the index to write the next sample to"""
+        self.current_idx = (self.current_idx + 1) % self.filter_width
 
     def reset(self):
         # Clear internal state