From 7c6944f597014fe3f77a6d8cc4ff5962df41ee4d Mon Sep 17 00:00:00 2001 From: vaishanth_r Date: Tue, 4 Feb 2025 18:10:15 +0000 Subject: [PATCH 1/6] update --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index 0e203a39..125a9b8c 100644 --- a/.gitignore +++ b/.gitignore @@ -153,3 +153,7 @@ dmypy.json # Cython debug symbols cython_debug/ + + +vaishanthr +lerobot/common/robot_devices/cameras/cam_test.py \ No newline at end of file From 66db8c668c7e0ab82fd9d7b31625cb7eeac00221 Mon Sep 17 00:00:00 2001 From: vaishanth_r Date: Fri, 21 Feb 2025 21:26:14 +0000 Subject: [PATCH 2/6] add smoothen actions function --- lerobot/common/policies/utils.py | 40 ++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/lerobot/common/policies/utils.py b/lerobot/common/policies/utils.py index c06e620b..f13feb23 100644 --- a/lerobot/common/policies/utils.py +++ b/lerobot/common/policies/utils.py @@ -17,6 +17,8 @@ import torch from torch import nn +from scipy.signal import butter, filtfilt +import numpy as np def populate_queues(queues, batch): for key in batch: @@ -65,3 +67,41 @@ def get_output_shape(module: nn.Module, input_shape: tuple) -> tuple: with torch.inference_mode(): output = module(dummy_input) return tuple(output.shape) + +def butterworth_lowpass_filter(data:np.ndarray, cutoff_freq:float = 0.5, sampling_freq:float = 15.0, order=2) -> np.ndarray: + """ + Applies a low-pass Butterworth filter to the input data. + + Parameters: + data (np.array): Input data array. + cutoff (float): Cutoff frequency of the filter (Hz). Smoother for lower values. + fs (float): Sampling frequency of the data (Hz). + order (int): Order of the filter. Higher order may introduce phase distortions. + + Returns: + filtered_data (np.array): Filtered data array with same shape as data. + """ + nyquist = 0.5 * sampling_freq + normal_cutoff = cutoff_freq / nyquist + b, a = butter(order, normal_cutoff, btype='low', analog=False) + + # apply the filter along axis 0 + filtered_data = filtfilt(b, a, data, axis=0) + return filtered_data + +def smoothen_actions(actions: torch.Tensor) -> torch.Tensor: + """ + Smoothens the provided action sequence tensor + Args: + actions (torch.Tensor): actions from policy + """ + if not isinstance(actions, torch.Tensor): + raise ValueError(f"Invalid input type for actions {type(actions)}. Expected torch.Tensor!") + + if len(actions.shape) == 3 and not actions.shape[0] == 1: + raise NotImplementedError(f"Batch processing not implemented!!") + + actions_np = actions.squeeze(0).cpu().numpy() + # apply the low-pass filter + actions_np = butterworth_lowpass_filter(actions_np) + return torch.from_numpy(actions_np.copy()).unsqueeze(0).to(actions.device) From 5d5a4186c2dc2a06d8a7a456624e87bdf4ebd22e Mon Sep 17 00:00:00 2001 From: vaishanth_r Date: Fri, 21 Feb 2025 21:38:01 +0000 Subject: [PATCH 3/6] integrate action smoothing in ACT and Diffusion Policy --- lerobot/common/policies/act/modeling_act.py | 3 +++ lerobot/common/policies/diffusion/modeling_diffusion.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/lerobot/common/policies/act/modeling_act.py b/lerobot/common/policies/act/modeling_act.py index f2b16a1e..3490cd06 100644 --- a/lerobot/common/policies/act/modeling_act.py +++ b/lerobot/common/policies/act/modeling_act.py @@ -36,6 +36,7 @@ from torchvision.ops.misc import FrozenBatchNorm2d from lerobot.common.policies.act.configuration_act import ACTConfig from lerobot.common.policies.normalize import Normalize, Unnormalize from lerobot.common.policies.pretrained import PreTrainedPolicy +from lerobot.common.policies.utils import smoothen_actions class ACTPolicy(PreTrainedPolicy): @@ -138,6 +139,8 @@ class ACTPolicy(PreTrainedPolicy): # TODO(rcadene): make _forward return output dictionary? actions = self.unnormalize_outputs({"action": actions})["action"] + # use low-pass filter to prevent jerky actions + actions = smoothen_actions(actions) # `self.model.forward` returns a (batch_size, n_action_steps, action_dim) tensor, but the queue # effectively has shape (n_action_steps, batch_size, *), hence the transpose. diff --git a/lerobot/common/policies/diffusion/modeling_diffusion.py b/lerobot/common/policies/diffusion/modeling_diffusion.py index 9ecadcb0..615eaf7a 100644 --- a/lerobot/common/policies/diffusion/modeling_diffusion.py +++ b/lerobot/common/policies/diffusion/modeling_diffusion.py @@ -42,6 +42,7 @@ from lerobot.common.policies.utils import ( get_dtype_from_parameters, get_output_shape, populate_queues, + smoothen_actions, ) @@ -137,6 +138,8 @@ class DiffusionPolicy(PreTrainedPolicy): # TODO(rcadene): make above methods return output dictionary? actions = self.unnormalize_outputs({"action": actions})["action"] + # use low-pass filter to prevent jerky actions + actions = smoothen_actions(actions) self._queues["action"].extend(actions.transpose(0, 1)) From e2aa4864e8eed56d307b737041027f5580bc6b21 Mon Sep 17 00:00:00 2001 From: vaishanth_r Date: Wed, 26 Feb 2025 22:11:05 +0000 Subject: [PATCH 4/6] update action smoothing to allow gripper joint passthrough --- lerobot/common/policies/utils.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/lerobot/common/policies/utils.py b/lerobot/common/policies/utils.py index f13feb23..5597c6ec 100644 --- a/lerobot/common/policies/utils.py +++ b/lerobot/common/policies/utils.py @@ -68,7 +68,7 @@ def get_output_shape(module: nn.Module, input_shape: tuple) -> tuple: output = module(dummy_input) return tuple(output.shape) -def butterworth_lowpass_filter(data:np.ndarray, cutoff_freq:float = 0.5, sampling_freq:float = 15.0, order=2) -> np.ndarray: +def butterworth_lowpass_filter(data:np.ndarray, cutoff_freq:float = 1.0, sampling_freq:float = 15.0, order=2) -> np.ndarray: """ Applies a low-pass Butterworth filter to the input data. @@ -103,5 +103,7 @@ def smoothen_actions(actions: torch.Tensor) -> torch.Tensor: actions_np = actions.squeeze(0).cpu().numpy() # apply the low-pass filter - actions_np = butterworth_lowpass_filter(actions_np) - return torch.from_numpy(actions_np.copy()).unsqueeze(0).to(actions.device) + filtered_actions_np = butterworth_lowpass_filter(actions_np.copy()) + # disable filtering for the gripper joint + filtered_actions_np[:, -1] = actions_np[:, -1] + return torch.from_numpy(filtered_actions_np.copy()).unsqueeze(0).to(actions.device) \ No newline at end of file From f71e0a7068444a6aff8648025be172cf736eae2d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 4 Mar 2025 16:41:12 +0000 Subject: [PATCH 5/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .gitignore | 2 +- lerobot/common/policies/utils.py | 22 +++++++++++++--------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index fea4d499..286c14b4 100644 --- a/.gitignore +++ b/.gitignore @@ -160,4 +160,4 @@ cython_debug/ vaishanthr -lerobot/common/robot_devices/cameras/cam_test.py \ No newline at end of file +lerobot/common/robot_devices/cameras/cam_test.py diff --git a/lerobot/common/policies/utils.py b/lerobot/common/policies/utils.py index 5597c6ec..9bb46128 100644 --- a/lerobot/common/policies/utils.py +++ b/lerobot/common/policies/utils.py @@ -14,11 +14,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +import numpy as np import torch +from scipy.signal import butter, filtfilt from torch import nn -from scipy.signal import butter, filtfilt -import numpy as np def populate_queues(queues, batch): for key in batch: @@ -68,27 +68,31 @@ def get_output_shape(module: nn.Module, input_shape: tuple) -> tuple: output = module(dummy_input) return tuple(output.shape) -def butterworth_lowpass_filter(data:np.ndarray, cutoff_freq:float = 1.0, sampling_freq:float = 15.0, order=2) -> np.ndarray: + +def butterworth_lowpass_filter( + data: np.ndarray, cutoff_freq: float = 1.0, sampling_freq: float = 15.0, order=2 +) -> np.ndarray: """ Applies a low-pass Butterworth filter to the input data. - + Parameters: data (np.array): Input data array. cutoff (float): Cutoff frequency of the filter (Hz). Smoother for lower values. fs (float): Sampling frequency of the data (Hz). order (int): Order of the filter. Higher order may introduce phase distortions. - + Returns: filtered_data (np.array): Filtered data array with same shape as data. """ nyquist = 0.5 * sampling_freq normal_cutoff = cutoff_freq / nyquist - b, a = butter(order, normal_cutoff, btype='low', analog=False) + b, a = butter(order, normal_cutoff, btype="low", analog=False) # apply the filter along axis 0 filtered_data = filtfilt(b, a, data, axis=0) return filtered_data + def smoothen_actions(actions: torch.Tensor) -> torch.Tensor: """ Smoothens the provided action sequence tensor @@ -97,13 +101,13 @@ def smoothen_actions(actions: torch.Tensor) -> torch.Tensor: """ if not isinstance(actions, torch.Tensor): raise ValueError(f"Invalid input type for actions {type(actions)}. Expected torch.Tensor!") - + if len(actions.shape) == 3 and not actions.shape[0] == 1: - raise NotImplementedError(f"Batch processing not implemented!!") + raise NotImplementedError("Batch processing not implemented!!") actions_np = actions.squeeze(0).cpu().numpy() # apply the low-pass filter filtered_actions_np = butterworth_lowpass_filter(actions_np.copy()) # disable filtering for the gripper joint filtered_actions_np[:, -1] = actions_np[:, -1] - return torch.from_numpy(filtered_actions_np.copy()).unsqueeze(0).to(actions.device) \ No newline at end of file + return torch.from_numpy(filtered_actions_np.copy()).unsqueeze(0).to(actions.device) From 77dd0e50568e554784d48b14184f0addce62d180 Mon Sep 17 00:00:00 2001 From: Vaishanth Ramaraj <54806061+vaishanth-rmrj@users.noreply.github.com> Date: Tue, 4 Mar 2025 11:59:07 -0500 Subject: [PATCH 6/6] Update .gitignore --- .gitignore | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.gitignore b/.gitignore index 286c14b4..0a0ffe10 100644 --- a/.gitignore +++ b/.gitignore @@ -157,7 +157,3 @@ dmypy.json # Cython debug symbols cython_debug/ - - -vaishanthr -lerobot/common/robot_devices/cameras/cam_test.py