From cb7dd835f2a93d777c8cde54bc66708bc92763c3 Mon Sep 17 00:00:00 2001
From: = <=>
Date: Mon, 27 Jan 2025 16:35:37 -0500
Subject: [PATCH] Updated hard stop stuff.

---
 opensourceleg/control/virtual_hard_stop.py | 15 +++++--
 test_hard_stop.py                          | 51 ++++++++++++----------
 2 files changed, 38 insertions(+), 28 deletions(-)

diff --git a/opensourceleg/control/virtual_hard_stop.py b/opensourceleg/control/virtual_hard_stop.py
index 7243ea8..9c259e9 100644
--- a/opensourceleg/control/virtual_hard_stop.py
+++ b/opensourceleg/control/virtual_hard_stop.py
@@ -28,6 +28,11 @@ def __init__(self, joint: Joint, direction:Direction, start_position_rad:float,
         self.start_position  = start_position_rad
         self.stiffness = stiffness
         self.damping = damping
+        self._is_active = False
+
+    @property
+    def is_active(self):
+        return self._is_active
 
     def calculate_hard_stop_torque(self):
         """
@@ -37,7 +42,7 @@ def calculate_hard_stop_torque(self):
         float: The calculated torque to slow the joint
         """
         current_pos = self.joint.output_position
-        delta_theta = current_pos - self.start_position
+        delta_theta = (10*(current_pos - self.start_position))**3
         current_vel = self.joint.output_velocity
 
         # Check the direction of the hard stop and calculate torque accordingly
@@ -45,13 +50,15 @@ def calculate_hard_stop_torque(self):
             if current_pos > self.start_position:
                 torque = -self.stiffness * delta_theta - self.damping * current_vel
             else:
-                return 0
+                torque = 0.0
         else:
             if current_pos < self.start_position:
                 torque = -self.stiffness * delta_theta - self.damping * current_vel
             else:
-                return 0
-
+                torque = 0.0
+            
+        self._is_active = torque!=0.0
+ 
         return torque
     
     def calculate_eq_angle_bias(self, joint_K):
diff --git a/test_hard_stop.py b/test_hard_stop.py
index d26f5c3..bd1e19f 100644
--- a/test_hard_stop.py
+++ b/test_hard_stop.py
@@ -1,6 +1,7 @@
-from opensourceleg.opensourceleg.osl import OpenSourceLeg
-from opensourceleg.opensourceleg.control.virtual_hard_stop import VirtualHardStop, Direction
-from opensourceleg.opensourceleg.tools.units import convert_to_default
+from opensourceleg.osl import OpenSourceLeg
+from opensourceleg.control.virtual_hard_stop import VirtualHardStop, Direction
+from opensourceleg.tools.units import convert_to_default
+import numpy as np
 
 LOOP_FREQUENCY = 200 # Hz
 osl = OpenSourceLeg(frequency=LOOP_FREQUENCY, file_name='./' + 'hardStopTest')
@@ -9,35 +10,37 @@
 osl.add_joint("knee", gear_ratio=9 * 83 / 18, offline_mode=False)
 osl.add_joint("ankle", gear_ratio=9 * 83 / 18, offline_mode=False)
 
-ankle_hard_stop_pos = VirtualHardStop(osl.ankle, Direction.POSITIVE, 5, 10, 5)
-ankle_hard_stop_neg = VirtualHardStop(osl.ankle, Direction.NEGATIVE, -5, 10, 5)
+ankle_hard_stop_pos = VirtualHardStop(osl.ankle, Direction.POSITIVE, np.deg2rad(20), 100, 9)
+ankle_hard_stop_neg = VirtualHardStop(osl.ankle, Direction.NEGATIVE, np.deg2rad(-20), 100, 9)
 
 with osl:
     osl.home()
     osl.update()
 
     # Testing Impedance Mode
-    # ankle_stiffness = 10
-    # osl.knee.set_mode(osl.knee.control_modes.impedance)
-    # osl.knee.set_joint_impedance(K = 10, B = 5)
-    # osl.knee.set_output_position(osl.knee.output_position)
-    # osl.ankle.set_mode(osl.ankle.control_modes.impedance)
-    # osl.ankle.set_joint_impedance(K = ankle_stiffness, B = 5)
-    # osl.ankle.set_output_position(osl.ankle.output_position)
+    ankle_stiffness = 10
+    osl.knee.set_mode(osl.knee.control_modes.impedance)
+    osl.knee.set_joint_impedance(K = 10, B = 5)
+    osl.knee.set_output_position(osl.knee.output_position)
+    osl.ankle.set_mode(osl.ankle.control_modes.impedance)
+    osl.ankle.set_joint_impedance(K = ankle_stiffness, B = 1)
+    osl.ankle.set_output_position(osl.ankle.output_position)
 
 
-    # for time in osl.clock:
-    #     osl.update()
-    #     hard_stop_eq_angle_bias = ankle_hard_stop_pos.calculate_eq_angle_bias(ankle_stiffness) + ankle_hard_stop_neg.calculate_eq_angle_bias(ankle_stiffness)
-    #     osl.ankle.set_output_position(hard_stop_eq_angle_bias)
+    for time in osl.clock:
+        osl.update()
+        hard_stop_eq_angle_bias = ankle_hard_stop_pos.calculate_eq_angle_bias(ankle_stiffness) + ankle_hard_stop_neg.calculate_eq_angle_bias(ankle_stiffness)
+        osl.ankle.set_output_position(hard_stop_eq_angle_bias)
+        print(hard_stop_eq_angle_bias, ankle_hard_stop_neg.is_active, ankle_hard_stop_pos.is_active)
 
     # Testing Torque Mode
-    osl.knee.set_mode(osl.knee.control_modes.torque)
-    osl.knee.set_output_torque(0)
-    osl.ankle.set_mode(osl.ankle.control_modes.torque)
-    osl.ankle.set_output_torque(0)
+    # osl.knee.set_mode(osl.knee.control_modes.current)
+    # osl.knee.set_output_torque(0)
+    # osl.ankle.set_mode(osl.ankle.control_modes.current)
+    # osl.ankle.set_output_torque(0)
 
-    for time in osl.clock:
-        osl.update()
-        hard_stop_torque = ankle_hard_stop_pos.calculate_hard_stop_torque() + ankle_hard_stop_neg.calculate_hard_stop_torque()
-        osl.ankle.set_output_torque(hard_stop_torque)
\ No newline at end of file
+    # for time in osl.clock:
+    #     osl.update()
+    #     hard_stop_torque = ankle_hard_stop_pos.calculate_hard_stop_torque() + ankle_hard_stop_neg.calculate_hard_stop_torque()
+    #     print(hard_stop_torque)
+    #     osl.ankle.set_output_torque(hard_stop_torque)
\ No newline at end of file