motor-python 0.0.7__tar.gz → 0.0.8__tar.gz

{motor_python-0.0.7 → motor_python-0.0.8}/.gitignore

@@ -185,5 +185,6 @@ tmp/
 .codex
 CSV/
 data/csv_logs/*.csv
+data/csv_logs/*.png
 !data/csv_logs/.gitkeep
 repo_tree.py

{motor_python-0.0.7 → motor_python-0.0.8}/PKG-INFO

@@ -1,6 +1,6 @@
-Metadata-Version: 2.4
+Metadata-Version: 2.5
 Name: motor_python
-Version: 0.0.7
+Version: 0.0.8
 Summary: CubeMars motor module for Aries exosuits.
 Project-URL: homepage, https://github.com/TUM-Aries-Lab/motor-module
 Author-email: Tsmorz <tony.smoragiewicz@tum.de>, Hannes Nguyen <hannes.nguyen@tum.de>

{motor_python-0.0.7 → motor_python-0.0.8}/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "motor_python"
-version = "0.0.7"
+version = "0.0.8"
 description = "CubeMars motor module for Aries exosuits."
 readme = "README.md"
 authors = [{ name = "Tsmorz", email = "tony.smoragiewicz@tum.de"},{ name = "Hannes Nguyen", email = "hannes.nguyen@tum.de" }]

motor_python-0.0.8/src/motor_python/__init__.py

@@ -0,0 +1,110 @@
+"""Motor control module for CubeMars AK60-6 exosuit actuators.
+
+Primary interface: CAN (CubeMarsAK606v3CAN / Motor).
+Legacy UART interface: CubeMarsAK606v3.
+Base class: BaseMotor (for shared interface & safety logic).
+"""
+
+__version__ = "0.0.8"
+
+from typing import Literal
+
+from motor_python.base_motor import BaseMotor
+from motor_python.cube_mars_motor import CubeMarsAK606v3, CubeMarsAK806v2
+from motor_python.cube_mars_motor_can import (
+    CubeMarsAK606v1CAN,
+    CubeMarsAK606v3CAN,
+    CubeMarsAK806v2CAN,
+    CubeMarsBaseCAN,
+)
+from motor_python.definitions import (
+    AK60_6_V1_1_MOTOR_SPEC,
+    AK60_6_V3_0_MOTOR_SPEC,
+    AK80_6_MOTOR_SPEC,
+    CAN_DEFAULTS,
+    MotorModel,
+    MotorSpec,
+)
+from motor_python.motor_manager import MotorManager
+
+# Convenience alias — CAN is the primary interface. Later can be changed to AK806v2CAN.
+Motor = CubeMarsAK606v3CAN
+
+
+# ruff: noqa: PLR0913
+def create_can_motor(
+    motor_model: str = MotorModel.AK60_6V3,
+    *,
+    motor_can_id: int = CAN_DEFAULTS.motor_can_id,
+    interface: str = CAN_DEFAULTS.interface,
+    bitrate: int = CAN_DEFAULTS.bitrate,
+    feedback_can_id: int | None = None,
+    mit_velocity_kd: float | None = None,
+    motor_spec: MotorSpec | None = None,
+    helper_policy: Literal["strict", "fcfd", "legacy"] = "fcfd",
+    auto_recover_bus: bool = True,
+    allow_legacy_feedback_ids: bool = True,
+    aggressive_bus_reset: bool = False,
+) -> CubeMarsBaseCAN:
+    """Build a CAN motor instance for the requested model."""
+    model = motor_model.strip().upper()
+    if model in {"AK60-6", "AK60_6", "AK60-6_V3.0"}:
+        return CubeMarsAK606v3CAN(
+            motor_can_id=motor_can_id,
+            interface=interface,
+            bitrate=bitrate,
+            feedback_can_id=feedback_can_id,
+            mit_velocity_kd=mit_velocity_kd,
+            motor_spec=motor_spec if motor_spec is not None else AK60_6_V3_0_MOTOR_SPEC,
+            helper_policy=helper_policy,
+            auto_recover_bus=auto_recover_bus,
+            allow_legacy_feedback_ids=True,
+            aggressive_bus_reset=aggressive_bus_reset,
+        )
+    if model in {"AK80-6", "AK80_6"}:
+        return CubeMarsAK806v2CAN(
+            motor_can_id=motor_can_id,
+            interface=interface,
+            bitrate=bitrate,
+            feedback_can_id=feedback_can_id,
+            mit_velocity_kd=mit_velocity_kd,
+            motor_spec=motor_spec if motor_spec is not None else AK80_6_MOTOR_SPEC,
+            helper_policy=helper_policy,
+            auto_recover_bus=auto_recover_bus,
+            allow_legacy_feedback_ids=False,
+            aggressive_bus_reset=aggressive_bus_reset,
+        )
+    if model in {"AK60-V1", "AK60-V1.1", "AK60-6_V1.1"}:
+        return CubeMarsAK606v1CAN(
+            motor_can_id=motor_can_id,
+            interface=interface,
+            bitrate=bitrate,
+            feedback_can_id=feedback_can_id,
+            mit_velocity_kd=mit_velocity_kd,
+            motor_spec=motor_spec if motor_spec is not None else AK60_6_V1_1_MOTOR_SPEC,
+            helper_policy=helper_policy,
+            auto_recover_bus=auto_recover_bus,
+            allow_legacy_feedback_ids=False,
+            aggressive_bus_reset=aggressive_bus_reset,
+        )
+    raise ValueError("Unknown motor model: must be AK60-6 or AK80-6")
+
+
+__all__ = [
+    "AK60_6_V1_1_MOTOR_SPEC",
+    "AK60_6_V3_0_MOTOR_SPEC",
+    "AK80_6_MOTOR_SPEC",
+    "CAN_DEFAULTS",
+    "BaseMotor",
+    "CubeMarsAK606v1CAN",
+    "CubeMarsAK606v3",
+    "CubeMarsAK606v3CAN",
+    "CubeMarsAK806v2",
+    "CubeMarsAK806v2CAN",
+    "CubeMarsBaseCAN",
+    "Motor",
+    "MotorManager",
+    "MotorModel",
+    "MotorSpec",
+    "create_can_motor",
+]

motor_python-0.0.8/src/motor_python/__main__.py

@@ -0,0 +1,183 @@
+"""Motor control main entry point — CAN interface (CubeMarsAK606v3CAN).
+
+Example usage:
+python -m motor_python --motor-ids 0x03
+python -m motor_python --motor-ids 0x03 0x04
+python -m motor_python --discover
+python -m motor_python --dual
+
+"""
+
+import argparse
+
+from loguru import logger
+
+from motor_python.definitions import (
+    CAN_DEFAULTS,
+    DEFAULT_LOG_LEVEL,
+    DEFAULT_MOTOR_SPEC,
+    LogLevel,
+    MotorModel,
+    set_current_motor_model_by_name,
+)
+from motor_python.examples_can import (
+    run_motor_demo_can,
+    run_multi_motor_demo,
+)
+from motor_python.motor_manager import MotorManager
+from motor_python.utils import setup_logger
+
+
+def main(  # noqa: PLR0913
+    log_level: str = DEFAULT_LOG_LEVEL,
+    stderr_level: str = DEFAULT_LOG_LEVEL,
+    dual: bool = False,
+    discover: bool = False,
+    motor_ids: list[int] | None = None,
+    interface: str = CAN_DEFAULTS.interface,
+    motor_model: str = DEFAULT_MOTOR_SPEC.model_name,
+) -> None:
+    """Run the main CAN motor control loop.
+
+    Pre-condition: CAN interface must be up.
+        sudo ip link set can0 up type can bitrate 1000000 berr-reporting on restart-ms 100
+
+    :param log_level: The log level to use.
+    :param stderr_level: The std err level to use.
+    :param dual: If True, run the two-motor synchronized demo instead.
+    :param motor_ids: List of CAN motor IDs to control.
+    :param interface: The CAN interface to use.
+    :return: None
+    """
+    setup_logger(log_level=log_level, stderr_level=stderr_level)
+
+    set_current_motor_model_by_name(motor_model)
+    logger.info(f"Selected motor model: {motor_model}")
+
+    # --- Two-motor mode ---
+    if dual:
+        motor_ids = [CAN_DEFAULTS.motor_can_id, CAN_DEFAULTS.motor_can_id_2]
+
+    if motor_ids is None:
+        motor_ids = [CAN_DEFAULTS.motor_can_id]
+
+    try:
+        if discover:
+            manager = MotorManager.discover(
+                interface=interface, motor_model=motor_model
+            )  # sets the discovered ids as motor_ids
+        else:
+            logger.info(
+                f"Starting CAN motor control loop on interface '{interface}' with IDs: {motor_ids}"
+            )
+            manager = MotorManager(
+                motor_ids=motor_ids, interface=interface, motor_model=motor_model
+            )
+    except Exception as e:
+        logger.error(f"Failed to initialize CAN motor manager: {e}")
+        return
+
+    with manager:
+        first_motor = next(iter(manager))
+        if not first_motor.connected:
+            logger.warning(
+                "CAN bus not available. Run: sudo ip link set can0 up "
+                "type can bitrate 1000000 berr-reporting on restart-ms 100"
+            )
+            return
+
+        manager.send_neutral_commands()  # Send neutral commands to all motors
+
+        status = (
+            manager.check_all()
+        )  # Check communication with all motors before proceeding
+        if not any(status.values()):
+            logger.warning(
+                "No motors responding. Check power, CANH/CANL wiring, "
+                "120 ohm termination, and CAN IDs."
+            )
+            return
+
+        manager.enable_all()  # Enable all motors
+        logger.info("All motors enabled. Checking communication...")
+
+        logger.info(
+            f"Motor communication verified for IDs: {[motor_id for motor_id, ok in status.items() if ok]}"
+        )
+        for motor in manager:
+            motor.get_status()
+
+        try:
+            if len(manager) == 1:
+                run_motor_demo_can(next(iter(manager)))
+            else:
+                run_multi_motor_demo(manager)
+        except KeyboardInterrupt:
+            logger.info("Interrupted by user")
+
+    logger.info("CAN motor control loop complete!")
+
+
+if __name__ == "__main__":  # pragma: no cover
+    parser = argparse.ArgumentParser("Run the pipeline.")
+    parser.add_argument(
+        "--log-level",
+        default=DEFAULT_LOG_LEVEL,
+        choices=list(LogLevel()),
+        help="Set the log level.",
+        required=False,
+        type=str,
+    )
+    parser.add_argument(
+        "--stderr-level",
+        default=DEFAULT_LOG_LEVEL,
+        choices=list(LogLevel()),
+        help="Set the std err level.",
+        required=False,
+        type=str,
+    )
+    parser.add_argument(
+        "--dual",
+        action="store_true",
+        default=False,
+        help="Run the two-motor synchronized demo (requires two motors on the bus).",
+    )
+    parser.add_argument(
+        "--discover",
+        action="store_true",
+        default=False,
+        help="Automatically discover connected motors on the CAN bus.",
+    )
+    parser.add_argument(
+        "--motor-ids",
+        nargs="+",
+        type=lambda x: int(x, 0),
+        default=None,
+        help=(
+            "List of CAN motor IDs to control. "
+            "Use space-separated values like --motor-ids 0x03 0x04."
+        ),
+    )
+    parser.add_argument(
+        "--interface",
+        default=CAN_DEFAULTS.interface,
+        help="SocketCAN interface to use (default: can0).",
+    )
+    parser.add_argument(
+        "--motor-model",
+        type=str,
+        choices=list(MotorModel),
+        default=DEFAULT_MOTOR_SPEC.model_name,
+        help="Select motor type (AK60-6 or AK80-6).",
+    )
+    args = parser.parse_args()
+
+    main(
+        log_level=args.log_level,
+        stderr_level=args.stderr_level,
+        dual=args.dual,
+        discover=args.discover,
+        motor_ids=args.motor_ids,
+        motor_model=args.motor_model,
+        interface=args.interface,
+    )

{motor_python-0.0.7 → motor_python-0.0.8}/src/motor_python/base_motor.py

@@ -8,6 +8,7 @@ must implement the abstract transport-layer methods.
 from __future__ import annotations
 
 import abc
+import math
 import time
 from dataclasses import dataclass
 from typing import Self
@@ -48,6 +49,8 @@ class MotorState:
     current_amps: float  # Phase current in amps
     temperature_celsius: int  # Driver board temperature in °C
     error_code: int  # Fault code (0 = OK)
+    timestamp_monotonic: float = 0.0
+    is_fresh: bool = True
 
     @property
     def error_description(self) -> str:
@@ -137,6 +140,14 @@ class BaseMotor(abc.ABC):
         :param velocity_erpm: Validated and clamped velocity in ERPM.
         """
 
+    def degrees_to_radians(self, degrees: float) -> float:
+        """Convert degrees to radians."""
+        return degrees * math.pi / 180.0
+
+    def radians_to_degrees(self, radians: float) -> float:
+        """Convert radians to degrees."""
+        return radians * 180.0 / math.pi
+
     # ------------------------------------------------------------------
     # Unified Advanced Control Methods (Stubbed by default)
     # ------------------------------------------------------------------
@@ -364,3 +375,195 @@ class BaseMotor(abc.ABC):
     def __exit__(self, exc_type, exc_val, exc_tb) -> None:
         """Context manager exit."""
         self.close()
+
+    def get_timing_stats(self) -> dict:
+        """Return Refresh Loop Statistics.
+
+        Reports:
+        - Loop timing statistics (mean dt, std dt, min dt, max dt, and the effective Hz (1/mean_dt))
+        - Jitter count (Counts how many intervals exceed 2x the expected period)
+        - Cumulative send failures and missed feedback frames (never resets)
+        - CAN error counter deltas (current tx_err/rx_err vs values at start)
+        - TX pacing metrics (if available)
+
+        :return: Dictionary with timing statistics, or minimal dict if unavailable.
+        """
+        stats = {
+            "method": "get_timing_stats",
+            "available": False,
+        }
+
+        # Trying to get loop timing from refresh timestamps
+        refresh_timestamps = getattr(self, "_refresh_timestamps", None)
+        if refresh_timestamps is not None:
+            timestamps = np.array(refresh_timestamps)
+            if len(timestamps) > 1:
+                expected_period = (
+                    getattr(self, "_refresh_interval", 0.01)
+                    if hasattr(self, "_refresh_interval")
+                    else 0.01
+                )
+                dts = np.diff(timestamps)
+                threshold = 2.0 * expected_period
+                jitter_count = np.sum(
+                    dts > threshold
+                )  # Counting intervals that exceed the expected threshold
+
+                stats.update(
+                    {
+                        "available": True,
+                        "loop_period_expected_s": expected_period,
+                        "loop_period_mean_s": float(np.mean(dts)),
+                        "loop_period_std_s": float(np.std(dts)),
+                        "loop_period_min_s": float(np.min(dts)),
+                        "loop_period_max_s": float(np.max(dts)),
+                        "loop_effective_hz": 1.0 / float(np.mean(dts)),
+                        "loop_intervals_total": len(dts),
+                        "loop_jitter_count": int(jitter_count),
+                        "loop_jitter_ratio": float(jitter_count / len(dts))
+                        if len(dts) > 0
+                        else 0.0,
+                    }
+                )
+
+        # Cumulative send failures
+        cumulative_send_failures = getattr(
+            self, "_cumulative_refresh_send_failures", None
+        )
+        if cumulative_send_failures is not None:
+            stats["cumulative_send_failures"] = cumulative_send_failures
+
+        # Cumulative missed feedback
+        cumulative_no_feedback = getattr(self, "_cumulative_refresh_no_feedback", None)
+        if cumulative_no_feedback is not None:
+            stats["cumulative_missed_feedback"] = cumulative_no_feedback
+
+        # CAN error counter deltas
+        initial_can_state = getattr(self, "_initial_can_state", None)
+        last_can_state_cache = getattr(self, "_last_can_state_cache", None)
+        if initial_can_state is not None and last_can_state_cache is not None:
+            stats["can_tx_err_initial"] = initial_can_state.get("tx_err", 0)
+            stats["can_tx_err_final"] = last_can_state_cache.get("tx_err", 0)
+            stats["can_tx_err_delta"] = last_can_state_cache.get(
+                "tx_err", 0
+            ) - initial_can_state.get("tx_err", 0)
+            stats["can_rx_err_initial"] = initial_can_state.get("rx_err", 0)
+            stats["can_rx_err_final"] = last_can_state_cache.get("rx_err", 0)
+            stats["can_rx_err_delta"] = last_can_state_cache.get(
+                "rx_err", 0
+            ) - initial_can_state.get("rx_err", 0)
+
+        # TX pacing metrics tells how often pacing delays were needed and how much time was spent sleeping to pace transmissions.
+        tx_pace_sleep_count = getattr(self, "_tx_pace_sleep_count", None)
+        if tx_pace_sleep_count is not None:
+            stats["tx_pace_sleep_count"] = tx_pace_sleep_count
+        tx_pace_sleep_time_s = getattr(self, "_tx_pace_sleep_time_s", None)
+        if tx_pace_sleep_time_s is not None:
+            stats["tx_pace_sleep_time_s"] = tx_pace_sleep_time_s
+
+        return stats
+
+    def reset_timing_stats(self) -> None:
+        """Reset timing-related diagnostic state used by get_timing_stats().
+
+        This clears any transport-specific timestamp buffers and zeroes
+        counters that are safe to reset for a fresh measurement run.
+        """
+        # Clear refresh timestamps used for jitter analysis (CAN transport)
+        if hasattr(self, "_refresh_timestamps"):
+            try:
+                self._refresh_timestamps.clear()
+            except Exception:
+                try:
+                    self._refresh_timestamps = type(self._refresh_timestamps)()
+                except Exception:
+                    logger.warning("Failed to reset _refresh_timestamps")
+
+        if hasattr(self, "_refresh_send_failures"):
+            try:
+                self._refresh_send_failures = 0
+            except Exception:
+                logger.warning("Failed to reset _refresh_send_failures")
+        if hasattr(self, "_refresh_no_feedback"):
+            try:
+                self._refresh_no_feedback = 0
+            except Exception:
+                logger.warning("Failed to reset _refresh_no_feedback")
+
+
+def print_timing_stats(
+    timing_stats: dict[str, float | int | bool],
+    total_feedback_samples: int,
+    separator: str,
+) -> None:
+    """Print timing and CAN health diagnostics."""
+    if not timing_stats.get("available", False):
+        return
+
+    logger.info(f"\n{separator}")
+    logger.info("Timing & Health Diagnostics")
+    logger.info(separator)
+
+    logger.info(
+        f"Loop effective Hz      : {timing_stats.get('loop_effective_hz', 0):.1f}"
+    )
+    logger.info(
+        f"Loop period (expected) : "
+        f"{timing_stats.get('loop_period_expected_s', 0):.6f} s"
+    )
+    logger.info(
+        f"Loop period (mean)     : {timing_stats.get('loop_period_mean_s', 0):.6f} s"
+    )
+    logger.info(
+        f"Loop period (std)      : {timing_stats.get('loop_period_std_s', 0):.6f} s"
+    )
+    logger.info(
+        f"Loop period (min/max)  : "
+        f"{timing_stats.get('loop_period_min_s', 0):.6f} / "
+        f"{timing_stats.get('loop_period_max_s', 0):.6f} s"
+    )
+
+    logger.info(
+        f"Jitter (>2x period)    : "
+        f"{timing_stats.get('loop_jitter_count', 0)} / "
+        f"{timing_stats.get('loop_intervals_total', 0)} "
+        f"({100.0 * timing_stats.get('loop_jitter_ratio', 0):.1f}%)"
+    )
+
+    logger.info(
+        f"TX pace sleeps         : "
+        f"{timing_stats.get('tx_pace_sleep_count', 0)} times, "
+        f"{timing_stats.get('tx_pace_sleep_time_s', 0):.3f} s total"
+    )
+
+    logger.info(
+        f"Send failures (cumul.) : {timing_stats.get('cumulative_send_failures', 0)}"
+    )
+
+    missed_feedback = timing_stats.get("cumulative_missed_feedback", 0)
+
+    if total_feedback_samples > 0:
+        missed_percentage = (missed_feedback / total_feedback_samples) * 100.0
+        logger.info(
+            f"Missed feedback (cumul) : "
+            f"{missed_feedback}/{total_feedback_samples} "
+            f"({missed_percentage:.1f}%)"
+        )
+    else:
+        logger.info(f"Feedback samples (total): {total_feedback_samples}")
+        logger.info(f"Missed feedback (cumul) : {missed_feedback}")
+
+    can_tx_delta = timing_stats.get("can_tx_err_delta", 0)
+    can_rx_delta = timing_stats.get("can_rx_err_delta", 0)
+
+    logger.info(
+        f"CAN errors             : "
+        f"tx_err {timing_stats.get('can_tx_err_initial', 0)}"
+        f"→{timing_stats.get('can_tx_err_final', 0)} "
+        f"(Δ{can_tx_delta:+d}), "
+        f"rx_err {timing_stats.get('can_rx_err_initial', 0)}"
+        f"→{timing_stats.get('can_rx_err_final', 0)} "
+        f"(Δ{can_rx_delta:+d})"
+    )
+
+    logger.info(separator)

{motor_python-0.0.7 → motor_python-0.0.8}/src/motor_python/cube_mars_motor.py

@@ -11,6 +11,7 @@ from loguru import logger
 
 from motor_python.base_motor import BaseMotor, MotorState
 from motor_python.definitions import (
+    AK80_6_MOTOR_SPEC,
     CRC16_TAB,
     CRC_CONSTANTS,
     FRAME_BYTES,
@@ -429,3 +430,20 @@ class CubeMarsAK606v3(BaseMotor):
             self.stop()
             self.serial.close()
             logger.info("Motor connection closed")
+
+
+class CubeMarsAK806v2(CubeMarsAK606v3):
+    """AK80-6 Motor Controller for CubeMars V3 UART Protocol."""
+
+    def __init__(
+        self,
+        port: Path | str = MOTOR_DEFAULTS.port,
+        baudrate: int = MOTOR_DEFAULTS.baudrate,
+    ) -> None:
+        """Initialize AK80-6 UART motor connection.
+
+        :param port: Serial port path (default: MOTOR_DEFAULTS.port).
+        :param baudrate: Communication baudrate (default: MOTOR_DEFAULTS.baudrate).
+        """
+        self._motor_spec = AK80_6_MOTOR_SPEC
+        super().__init__(port=port, baudrate=baudrate)