Merge pull request #482 from neurobionics/454-no-easy-way-to-zero-encoders-and-set-their-direction

454 no easy way to zero encoders and set their direction

Author: senthurayyappan

docs/tutorials/robots/homing_joints.md

@@ -10,6 +10,7 @@ This example shows how to:
 - Configure `AS5048B` magnetic absolute encoders via I2C
 - Instantiate the `OpenSourceLeg` platform with the defined components
 - Use the `home()` method to detect joint limits and assign calibrated offsets
+- Use callback functions to execute code at the homed positions
 - Log joint and encoder positions to the terminal in real-time
 
 ## Hardware Setup
@@ -33,7 +34,7 @@ The [tutorial script](https://github.com/neurobionics/opensourceleg/blob/main/tu
 ### 1. Configuration
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:3:14"
+--8<-- "tutorials/robots/osl/homing_joints.py:1:14"
 ```
 
 Key parameters:
@@ -45,7 +46,7 @@ Key parameters:
 ### 2. Real-time loop and Logger
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:17:22"
+--8<-- "tutorials/robots/osl/homing_joints.py:16:22"
 ```
 
 This section:
@@ -56,7 +57,7 @@ This section:
 ### 3. Actuator Initialization
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:24:37"
+--8<-- "tutorials/robots/osl/homing_joints.py:24:39"
 ```
 
 This section:
@@ -67,7 +68,7 @@ This section:
 ### 4. Sensor Initialization
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:41:56"
+--8<-- "tutorials/robots/osl/homing_joints.py:41:58"
 ```
 
 This section:
@@ -76,20 +77,32 @@ This section:
 - Each encoder is tagged and assigned its A1/A2 pin configuration
 - Zero position is temporarily set to 0
 
-### 5. Initialize OSL Platform
+### 5. Using Callbacks for Homing Completion
+
+The `home()` method also supports an optional `callback_functions` argument. This allows you to specify a list of functions (one per actuator) that will be called when each actuator finishes its homing routine. This is useful for custom notifications, logging, or triggering additional actions like zeroing joint encoders.
+
+For example, to print a message when each joint completes homing:
+
+```python
+--8<-- "tutorials/robots/osl/homing_joints.py:60:73"
+```
+
+If you do not wish to use callbacks, you can omit the `callback_functions` argument or pass a list of `None` values.
+
+### 6. Initialize OSL Platform
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:60:63"
+--8<-- "tutorials/robots/osl/homing_joints.py:75:79"
 ```
 
 This section:
 
 - Creates an instance of `OpenSourceLeg` and passes in the actuator and sensor dictionaries
 
-### 6. Run Homing Routine
+### 7. Run Homing Routine
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:67:73"
+--8<-- "tutorials/robots/osl/homing_joints.py:81:90"
 ```
 
 This section:
@@ -100,10 +113,10 @@ This section:
   - Current exceeds `5000 mA`
 - Once stopped, the motor is zeroed based on its position, and a calibrated offset is added (30° for ankle, 0° for knee)
 
-### 7. Reset Torque and Start Logging
+### 8. Reset Torque and Start Logging
 
 ```python
---8<-- "tutorials/robots/osl/homing_joints.py:75:89"
+--8<-- "tutorials/robots/osl/homing_joints.py:92:110"
 ```
 
 This section:

examples/fsm_walking_python_controller.py

@@ -190,15 +190,15 @@ def lswing_to_estance(osl: OpenSourceLeg) -> bool:
     actuators = {
         "knee": DephyActuator(
             tag="knee",
-            port="/dev/ttyACM1",
+            port="/dev/ttyACM0",
             gear_ratio=GEAR_RATIO,
             frequency=FREQUENCY,
             debug_level=0,
             dephy_log=False,
         ),
         "ankle": DephyActuator(
             tag="ankle",
-            port="/dev/ttyACM0",
+            port="/dev/ttyACM1",
             gear_ratio=GEAR_RATIO,
             frequency=FREQUENCY,
             debug_level=0,
@@ -211,21 +211,25 @@ def lswing_to_estance(osl: OpenSourceLeg) -> bool:
         #     calibration_matrix=LOADCELL_CALIBRATION_MATRIX,
         # ),
         "loadcell": NBLoadcellDAQ(
-            LOADCELL_CALIBRATION_MATRIX, tag="loadcell", excitation_voltage=5.0, amp_gain=[34] * 3 + [151] * 3
+            LOADCELL_CALIBRATION_MATRIX,
+            tag="loadcell",
+            excitation_voltage=5.0,
+            amp_gain=[34] * 3 + [151] * 3,
+            spi_bus=1,
         ),
         "joint_encoder_knee": AS5048B(
             tag="joint_encoder_knee",
-            bus=3,
-            A1_adr_pin=True,
+            bus="/dev/i2c-2",
+            A1_adr_pin=False,
             A2_adr_pin=False,
             zero_position=0,
             enable_diagnostics=False,
         ),
         "joint_encoder_ankle": AS5048B(
             tag="joint_encoder_ankle",
-            bus=2,
+            bus="/dev/i2c-3",
             A1_adr_pin=False,
-            A2_adr_pin=True,
+            A2_adr_pin=False,
             zero_position=0,
             enable_diagnostics=False,
         ),
@@ -245,10 +249,25 @@ def lswing_to_estance(osl: OpenSourceLeg) -> bool:
 
     osl_fsm = create_simple_walking_fsm(osl)
 
+    # Zeroing the joint encoders
+    def knee_homing_complete():
+        osl.joint_encoder_knee.update()
+        osl.joint_encoder_knee.zero_position = osl.joint_encoder_knee.counts
+        print("Knee homing complete!")
+
+    def ankle_homing_complete():
+        osl.joint_encoder_ankle.update()
+        # The hard stop for ankle is at 30 deg from the zero position
+        osl.joint_encoder_ankle.zero_position = osl.joint_encoder_ankle.counts - osl.joint_encoder_ankle.deg_to_counts(
+            30
+        )
+        print("Ankle homing complete!")
+
+    callbacks = {"knee": knee_homing_complete, "ankle": ankle_homing_complete}
+
     with osl, osl_fsm:
         osl.update()
-        osl.home()
-
+        osl.home(callbacks=callbacks)
         input("Press Enter to start walking...")
 
         # knee
@@ -261,20 +280,25 @@ def lswing_to_estance(osl: OpenSourceLeg) -> bool:
         osl.ankle.set_impedance_cc_pidf_gains()
         osl.ankle.set_output_impedance()
 
+        osl.loadcell.reset()
+        osl.loadcell.calibrate()
+
         for t in clock:
             osl.update()
+            print("Ankle position", np.rad2deg(osl.sensors["joint_encoder_ankle"].position))
+            print("Knee position", np.rad2deg(osl.sensors["joint_encoder_knee"].position))
             osl_fsm.update(osl=osl)
-            osl.knee.set_output_impedance(
-                k=osl_fsm.current_state.knee_stiffness,
-                b=osl_fsm.current_state.knee_damping,
-            )
-            osl.ankle.set_output_impedance(
-                k=osl_fsm.current_state.ankle_stiffness,
-                b=osl_fsm.current_state.ankle_damping,
-            )
-
-            osl.knee.set_output_position(np.deg2rad(osl_fsm.current_state.knee_theta))
-            osl.ankle.set_output_position(np.deg2rad(osl_fsm.current_state.ankle_theta))
+            # osl.knee.set_output_impedance(
+            #     k=osl_fsm.current_state.knee_stiffness,
+            #     b=osl_fsm.current_state.knee_damping,
+            # )
+            # osl.ankle.set_output_impedance(
+            #     k=osl_fsm.current_state.ankle_stiffness,
+            #     b=osl_fsm.current_state.ankle_damping,
+            # )
+
+            # osl.knee.set_output_position(np.deg2rad(osl_fsm.current_state.knee_theta))
+            # osl.ankle.set_output_position(np.deg2rad(osl_fsm.current_state.ankle_theta))
 
             fsm_logger.info(
                 f"T: {t:.3f}s, "

opensourceleg/actuators/base.py

@@ -834,6 +834,7 @@ def home(
         output_position_offset: float = 0.0,
         current_threshold: int = 5000,
         velocity_threshold: float = 0.001,
+        callback: Optional[Callable[[], None]] = None,
     ) -> None:
         """
         Home the actuator.
@@ -848,6 +849,8 @@ def home(
             output_position_offset (float): Offset to add to the output position.
             current_threshold (int): Current threshold to stop homing.
             velocity_threshold (float): Velocity threshold to stop homing.
+            callback (Optional[Callable[[], None]]): Optional callback function to be
+                        called when homing completes. The function should take no arguments and return None.
 
         Examples:
             >>> actuator.home()

opensourceleg/actuators/dephy.py

@@ -2,7 +2,7 @@
 import os
 import time
 from ctypes import c_int
-from typing import Optional
+from typing import Callable, Optional
 
 import numpy as np
 from flexsea.device import Device
@@ -328,6 +328,7 @@ def home(
         output_position_offset: float = 0.0,
         current_threshold: int = 5000,
         velocity_threshold: float = 0.001,
+        callback: Optional[Callable[[], None]] = None,
     ) -> None:
         """
 
@@ -337,14 +338,22 @@ def home(
         to joint position in radians. This is useful for more accurate joint position estimation.
 
         Args:
-            homing_voltage (int): Voltage in mV to use for homing. Default is 2000 mV.
-            homing_frequency (int): Frequency in Hz to use for homing. Default is the actuator's frequency.
-            homing_direction (int): Direction to move the actuator during homing. Default is -1.
-            output_position_offset (float): Offset in radians to add to the output position. Default is 0.0.
+            homing_voltage (int): Voltage in mV to use for homing.
+                Default is 2000 mV.
+            homing_frequency (int): Frequency in Hz to use for homing.
+                Default is the actuator's frequency.
+            homing_direction (int): Direction to move the actuator during homing.
+                Default is -1.
+            output_position_offset (float): Offset in radians to add to the output position.
+                Default is 0.0.
             current_threshold (int): Current threshold in mA to stop homing the joint or actuator.
-                This is used to detect if the actuator or joint has hit a hard stop. Default is 5000 mA.
+                This is used to detect if the actuator or joint has hit a hard stop.
+                Default is 5000 mA.
             velocity_threshold (float): Velocity threshold in rad/s to stop homing the joint or actuator.
-                This is also used to detect if the actuator or joint has hit a hard stop. Default is 0.001 rad/s.
+                This is also used to detect if the actuator or joint has hit a hard stop.
+                Default is 0.001 rad/s.
+            callback (Optional[Callable[[], None]]): Optional callback function to be called when homing completes.
+                                                        The function should take no arguments and return None.
         Examples:
             >>> actuator = DephyActuator(port='/dev/ttyACM0')
             >>> actuator.start()
@@ -373,6 +382,8 @@ def home(
 
                 if abs(self.output_velocity) <= velocity_threshold or abs(self.motor_current) >= current_threshold:
                     self.set_motor_voltage(value=0)
+                    if callback is not None:
+                        callback()
                     is_homing = False
 
         except KeyboardInterrupt:

opensourceleg/actuators/tmotor.py

@@ -1,7 +1,7 @@
 import time
 import warnings
 from math import isfinite
-from typing import Optional
+from typing import Callable, Optional
 
 import can
 import numpy as np
@@ -25,6 +25,7 @@
     check_actuator_open,
     check_actuator_stream,
 )
+from opensourceleg.logging import LOGGER
 from opensourceleg.math import ThermalModel
 from opensourceleg.utilities import SoftRealtimeLoop
 
@@ -196,7 +197,30 @@ def home(
         output_position_offset: float = 0.0,
         current_threshold: int = 5000,
         velocity_threshold: float = 0.001,
+        callback: Optional[Callable[[], None]] = None,
     ):
+        """
+        Home the actuator and corresponding joint by moving it to the zero position.
+        The zero position is defined as the position where the joint is fully extended.
+
+        Args:
+            homing_voltage (int): Voltage in mV to use for homing.
+                Default is 2000 mV.
+            homing_frequency (Optional[int]): Frequency in Hz to use for homing.
+                Default is the actuator's frequency.
+            homing_direction (int): Direction to move the actuator during homing.
+                Default is -1.
+            output_position_offset (float): Offset in radians to add to the output position.
+                Default is 0.0.
+            current_threshold (int): Current threshold in mA to stop homing the joint or actuator.
+                Default is 5000 mA.
+            velocity_threshold (float): Velocity threshold in rad/s to stop homing the joint or actuator.
+                Default is 0.001 rad/s.
+            callback (Optional[Callable[[], None]]): Optional callback function to be called when homing completes.
+                                                        The function should take no arguments and return None.
+        """
+        # TODO: implement homing
+        LOGGER.info(msg=f"[{self.__repr__()}] Homing not implemented.")
         pass
 
     def update(self):  # noqa: C901

opensourceleg/robots/osl.py

@@ -1,6 +1,6 @@
 import os
 import time
-from typing import Optional, Union
+from typing import Callable, Optional, Union
 
 import numpy as np
 
@@ -43,6 +43,7 @@ def home(
         output_position_offset: Optional[dict[str, float]] = None,
         current_threshold: int = 5000,
         velocity_threshold: float = 0.001,
+        callbacks: Optional[dict[str, Callable]] = None,
     ) -> None:
         """
         Call the home method for all actuators.
@@ -56,19 +57,25 @@ def home(
                 Default is 0.0 for knee and 30.0 for ankle.
             current_threshold: The current threshold to apply to the actuators during homing. Default is 5000.
             velocity_threshold: The velocity threshold to apply to the actuators during homing. Default is 0.001.
+            callbacks  Optional[dict[str, Callable]]:
+                Optional dictionary of callback functions, one per actuator, to be called when each actuator's
+                homing completes. Only one callback per actuator is supported, and the tag must match.
+                Each function should take no arguments and return None. If None, no callbacks are used.
         """
         if output_position_offset is None:
             output_position_offset = {"knee": 0.0, "ankle": np.deg2rad(30.0)}
         if homing_direction is None:
             homing_direction = {"knee": -1, "ankle": -1}
         for actuator in self.actuators.values():
+            callback = callbacks.get(actuator.tag, None) if callbacks is not None else None
             actuator.home(
                 homing_voltage=homing_voltage,
                 homing_frequency=homing_frequency,
                 homing_direction=homing_direction[actuator.tag],
                 output_position_offset=output_position_offset[actuator.tag],
                 current_threshold=current_threshold,
                 velocity_threshold=velocity_threshold,
+                callback=callback,
             )
 
         LOGGER.info(