Fix safe_action (huggingface#395)

menhguin · Aug 30, 2024 · a62122e · a62122e
1 parent 708dd91
commit a62122e
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/lerobot/common/robot_devices/robots/koch.py b/lerobot/common/robot_devices/robots/koch.py
@@ -554,14 +554,16 @@ def send_action(self, action: torch.Tensor, follower_names: list[str] | None = N
                 safe_diff = torch.minimum(diff, max_relative_target)
                 safe_diff = torch.maximum(safe_diff, -max_relative_target)
                 safe_action = current_pos + safe_diff
-                if not torch.allclose(safe_action, action):
+                if not torch.allclose(safe_action, this_action):
                     logging.warning(
                         "Relative action magnitude had to be clamped to be safe.\n"
                         f"  requested relative action target: {diff}\n"
                         f"    clamped relative action target: {safe_diff}"
                     )
+                follower_goal_pos[name] = safe_action.numpy()
+            else:
+                follower_goal_pos[name] = this_action.numpy()
 
-            follower_goal_pos[name] = safe_action.numpy()
             from_idx = to_idx
 
         for name in self.follower_arms: