From f358b3766006ce244b490353dcee08f79ddb53b2 Mon Sep 17 00:00:00 2001
From: Clemens Christoph <cchristoph@student.ethz.ch>
Date: Tue, 12 May 2026 16:31:50 +0200
Subject: [PATCH] Improve tensioning: move motors by default, stall detection,
 gradual torque release

- Default to move_motors=True so tension.py moves motors without needing --move-motors
- Add --no-move-motors flag to skip motor movement
- Replace fixed-duration movement loops with stall detection (1s hold after stall)
- Gradually ramp down torque before hold phase to prevent tendon snap-back
---
 orca_core/hardware_hand.py | 50 ++++++++++++++++++++++++++------------
 scripts/tension.py         |  5 ++--
 2 files changed, 37 insertions(+), 18 deletions(-)

diff --git a/orca_core/hardware_hand.py b/orca_core/hardware_hand.py
index e1732af2..4da34d6f 100644
--- a/orca_core/hardware_hand.py
+++ b/orca_core/hardware_hand.py
@@ -1193,7 +1193,7 @@ def _jitter(
         with self._motor_lock:
             self._motor_client.write_desired_pos(motor_ids, start_pos_array)
 
-    def _tension(self, move_motors: bool = False):
+    def _tension(self, move_motors: bool = True):
         # TODO(fracapuano): Move this to a standard stateless function
         control_mode = self.config.control_mode
         self.set_control_mode(CURRENT_BASED_POSITION)
@@ -1205,7 +1205,7 @@ def _tension(self, move_motors: bool = False):
             ]
             self.set_max_current(self.config.calibration_current)
 
-            duration = 8
+            duration = 5
             increment_per_step = 0.1
             motor_increments_right = {
                 motor_id: increment_per_step for motor_id in motors_to_move
@@ -1214,21 +1214,39 @@ def _tension(self, move_motors: bool = False):
                 motor_id: -increment_per_step for motor_id in motors_to_move
             }
 
-            start_time = time.time()
-            while time.time() - start_time < duration:
-                if self._task_stop_event.is_set():
-                    break
-                self._set_motor_pos(motor_increments_left, rel_to_current=True)
-                time.sleep(0.1)
-
-            start_time = time.time()
-            while time.time() - start_time < duration:
-                if self._task_stop_event.is_set():
-                    break
-                self._set_motor_pos(motor_increments_right, rel_to_current=True)
-                time.sleep(0.1)
+            stall_threshold = 0.01
+            stall_hold = 1.0
+
+            for increments in (motor_increments_left, motor_increments_right):
+                stall_start = None
+                prev_pos = self.get_motor_pos()
+                while not self._task_stop_event.is_set():
+                    self._set_motor_pos(increments, rel_to_current=True)
+                    time.sleep(0.1)
+                    cur_pos = self.get_motor_pos()
+                    if np.max(np.abs(cur_pos - prev_pos)) < stall_threshold:
+                        if stall_start is None:
+                            stall_start = time.time()
+                        elif time.time() - stall_start >= stall_hold:
+                            break
+                    else:
+                        stall_start = None
+                    prev_pos = cur_pos
 
-        self.set_max_current(self.config.max_current)
+        # Gradually release torque so tendons don't snap back, then
+        # re-engage at the relaxed position for a stable hold.
+        max_cur = self.config.max_current
+        self.set_max_current(max_cur)
+        self.enable_torque()
+        steps = 20
+        for i in range(steps):
+            if self._task_stop_event.is_set():
+                break
+            self.set_max_current(max_cur * (1 - (i + 1) / steps))
+            time.sleep(1.0 / steps)
+        self.disable_torque()
+        time.sleep(0.05)
+        self.set_max_current(max_cur)
         self.enable_torque()
         print("Holding motors. Please tension carefully. Press Ctrl+C to exit.")
         try:
diff --git a/scripts/tension.py b/scripts/tension.py
index 0b3a8270..5e60d9a5 100644
--- a/scripts/tension.py
+++ b/scripts/tension.py
@@ -12,8 +12,9 @@ def main() -> int:
     add_hand_arguments(parser)
     parser.add_argument(
         "--move-motors",
-        action="store_true",
-        help="Run the built-in preconditioning motion before holding tension.",
+        action=argparse.BooleanOptionalAction,
+        default=True,
+        help="Run the built-in preconditioning motion before holding tension (default: True).",
     )
     args = parser.parse_args()