================================================================================
  MSI Demo - User Documentation
  Weiss Robotics GmbH & Co. KG
  Python Library for the Motion Stream Interface (MSI) of Servo Grippers
================================================================================

OVERVIEW
--------
This directory contains a Python library and demo scripts for controlling
Weiss Robotics servo grippers via two complementary protocols:

  - GRIPLINK  : TCP-based configuration and command interface
  - MSI       : UDP-based high-speed Motion Stream Interface for real-time
                trajectory streaming and continuous position control

The library is structured as two core modules (griplink_client.py,
msi_client.py) and three demo applications (main_*.py). A test script
(griplink_test.py) is included to verify the GRIPLINK connection.

--------------------------------------------------------------------------------

REQUIREMENTS
------------
  - Python 3.10 or later (uses built-in socket, struct, threading, queue)
  - No third-party packages required
  - Network access to the gripper device(s) on the configured IP address(es)

--------------------------------------------------------------------------------

FILE DESCRIPTIONS
-----------------

1. griplink_client.py
   -------------------
   Core library module. Implements a TCP client for the GRIPLINK Unified
   Command Set (Protocol V3).

   Key classes and types:
     GriplinkStatus (IntEnum)
         Enumeration of all GRIPLINK status/error codes returned by the
         controller (e.g., E_SUCCESS, E_TIMEOUT, E_AXIS_BLOCKED, ...).

     GriplinkError (Exception)
         Custom exception raised when a GRIPLINK command returns an error.
         Carries both a human-readable message and a GriplinkStatus code.

     DevState (IntEnum)
         Enumeration of device operational states:
         NOT_INITIALIZED, DISABLED, RELEASED, NO_PART, HOLDING, OPERATING,
         FAULT. Provides helper methods such as is_fault(), is_holding(), etc.

     GriplinkClient
         TCP client class. Supports the context manager protocol (with statement).

         Constructor:
           GriplinkClient(host: str, timeout_s: float = 10.0)
             host        - IP address of the GRIPLINK controller
             timeout_s   - socket timeout in seconds

         Low-level communication:
           cmd(s)            - send raw ASCII command, return raw response line
           function(cmd, …)  - send COMMAND(p1,p2,…), raise on ERR
           query(cmd, …)     - send COMMAND[p]?, return value string
           assign(cmd, v, …) - send COMMAND[p]=value, return value string

         System information (controller):
           id()              - controller identity string
           protocol()        - protocol name and version
           sn()              - controller serial number
           label()           - user-defined controller label
           ver()             - firmware version string
           verbose(enable)   - get or set verbose mode
           protassert(name, min_ver) - assert protocol name and minimum version

         Device commands (per port):
           devvid(port)      - vendor ID
           devpid(port)      - product ID
           devname(port)     - product name
           devvendor(port)   - vendor name
           devsn(port)       - device serial number
           devtag(port, tag) - get or set application tag
           devassert(port, vid, pid) - assert device identity
           devstate(port)    - current DevState
           value(port, idx)  - read a device value (e.g., current position)
           enable(port)      - enable the device
           disable(port)     - disable the device
           home(port)        - start homing procedure
           grip(port, idx, do_block)    - execute grip command
           release(port, idx, do_block) - execute release command
           gripcfg(port, idx, config)   - get or set grip configuration

   Usage example:
     from griplink_client import GriplinkClient, GriplinkError

     with GriplinkClient("192.168.1.40") as grip:
         print(grip.devstate())    # e.g. OPERATING
         grip.home(0)
         pos = grip.value(0, 0)   # read position in mm
         print(f"Position: {pos:.2f} mm")


2. msi_client.py
   --------------
   Core library module. Implements a UDP client for the Motion Stream
   Interface (MSI). The MSI allows real-time trajectory point streaming
   at a 1 ms resolution.

   The MsiClient runs two background daemon threads:
     - Receiver thread: continuously receives feedback UDP packets from the
       gripper and updates internal state variables.
     - Sender thread:   continuously transmits queued MSI command packets
       to the gripper via UDP.

   Key classes and types:
     OperatingState (IntEnum)
         MSI operating states: UNDEFINED, CLOSED, OPENED, ENABLED,
         DISABLED, FAULT.

     MsiClient
         Constructor:
           MsiClient(gripper_ip: str,
                     receiver_port: int = 5006,
                     feedback_callback = None)
             gripper_ip        - IP address of the gripper
             receiver_port     - local UDP port to listen on for feedback
             feedback_callback - optional callable(dict) invoked on every
                                 received feedback packet; the dict contains:
                                 position [mm], speed [mm/s], motor_current [A],
                                 force [N], flags, ack_number, time_stamp [ms],
                                 operating_state, status_code, points_buffered

         State machine methods (call in order):
           open(period_ticks_receiver, host_ip, timeout_s)
               Activate the MSI on the gripper, set the feedback period
               (in 1 ms ticks), and specify the host IP to send feedback to.
               Waits until OperatingState.OPENED is reached.

           enable(period_ticks_sender, timeout_s)
               Enable the MSI for trajectory streaming. Sets the expected
               command period (in 1 ms ticks). Waits for ENABLED state.

           point(pos_mm, force_n, sequence_number)
               Queue a single trajectory point. Each point specifies a target
               position [mm] and a force limit [N]. The sequence number is
               auto-incremented if not provided.

           disable(timeout_s)
               Gracefully disable trajectory streaming. Waits for DISABLED.

           close(timeout_s)
               Deactivate the MSI. Waits for CLOSED state.

         Feedback getters (thread-safe reads of last received values):
           get_current_position()  -> float  [mm]
           get_current_speed()     -> float  [mm/s]
           get_motor_current()     -> float  [A]
           get_estimated_force()   -> float  [N]
           get_ack_number()        -> int
           get_time_stamp()        -> int    [ms]
           get_operating_state()   -> OperatingState
           get_status_code()       -> GriplinkStatus
           get_points_buffered()   -> int    [ms of buffered motion]
           get_all()               -> dict   (all of the above)

         Status flag helpers:
           is_referenced()         -> bool   (gripper is homed)
           is_temperature_warning()-> bool
           is_service_required()   -> bool
           is_current_fault()      -> bool

   Important constants:
     SENDER_PORT = 5005   Fixed UDP port the gripper listens on (not configurable).

   Trajectory buffer:
     The controller buffers up to 500 trajectory points (= 500 ms of motion).
     get_points_buffered() returns the current fill level. The application must
     continuously replenish the buffer to prevent an underrun.

   Usage example:
     from msi_client import MsiClient

     msi = MsiClient("192.168.1.40", receiver_port=5006)
     msi.open(10, "192.168.1.1")   # feedback every 10 ms
     msi.enable(10)                 # command period 10 ms
     for i in range(500):
         msi.point(pos_mm=20.0 + i * 0.1, force_n=50.0)
     msi.disable()
     msi.close()


3. griplink_test.py
   -----------------
   Standalone test script. Connects to a GRIPLINK controller and exercises
   all available query and information methods, printing results to the
   console. Useful for verifying connectivity and inspecting controller/
   device parameters.

   Configuration (edit at the top of the file):
     IP_ADDR = "192.168.1.40"   # IP address of the GRIPLINK controller
     PORT    = 0                # Device port number

   Run:
     python griplink_test.py

   The script does NOT require any MSI setup. It uses only TCP/GRIPLINK.


4. main_sin2x.py
   --------------
   Demo application. Executes a sin²(x) position trajectory on a single
   gripper using the MSI. The trajectory oscillates the gripper fingers
   with a smooth, continuously differentiable motion profile.

   Configuration constants:
     AMPLITUDE_MM       = -30.0    Amplitude relative to the homed position [mm]
     PERIOD_SECS        = 3.0      Duration of one sin² cycle [s]
     TOTAL_TIME_SECS    = 10.0     Total execution time [s]
     INTERVAL_SECS      = 0.010    Trajectory sample interval (10 ms) [s]
     IP_ADDR_GRIPPER    = "192.168.1.40"   Change to your gripper IP
     IP_ADDR_HOST       = "192.168.1.1"    Change to your computer IP

   Execution steps performed by the script:
     1. Creates an MsiClient with a feedback callback that prints every
        received feedback packet.
     2. Connects via GRIPLINK (TCP) to home the gripper.
     3. Pre-computes the complete sin²x trajectory (position values at each
        10 ms sample).
     4. Opens and enables the MSI.
     5. Streams trajectory points in a loop, keeping the controller buffer
        filled (max 500 points), paced by real-time sleep.
     6. Disables and closes the MSI after all points are sent.

   Run:
     python main_sin2x.py


5. main_trajectory.py
   -------------------
   Demo application. Moves the gripper from the homed position to a
   configurable target using a trapezoidal velocity profile (constant
   acceleration, cruise, deceleration). Implements the trajectory generator
   class TrajectoryPy inline.

   Configuration constants:
     GOAL_POSITION_MM   = 60.0     Target position [mm]
     MAX_SPEED          = 100.0    Maximum speed [mm/s]
     MAX_ACCEL          = 300.0    Maximum acceleration [mm/s²]
     MAX_FORCE          = 50       Force limit [N]
     INTERVAL_SECS      = 0.010    Trajectory sample interval (10 ms) [s]
     IP_ADDR_GRIPPER    = "192.168.1.40"   Change to your gripper IP
     IP_ADDR_HOST       = "192.168.1.1"    Change to your computer IP

   Execution steps performed by the script:
     1. Creates an MsiClient instance.
     2. Homes the gripper via GRIPLINK.
     3. Computes a trapezoidal velocity trajectory from the homed position
        to GOAL_POSITION_MM.
     4. Opens and enables the MSI.
     5. Streams all pre-computed trajectory points while maintaining the
        buffer fill level.
     6. Disables and closes the MSI.

   Run:
     python main_trajectory.py


6. main_teleoperation.py
   ----------------------
   Demo application. Implements real-time teleoperation between two grippers:
   the position of a master gripper is continuously mirrored to a slave
   gripper via MSI.

   Configuration constants:
     IP_ADDR_MASTER_GRIPPER = "192.168.1.40"   Change to your master gripper IP
     IP_ADDR_SLAVE_GRIPPER  = "192.168.1.30"   Change to your slave gripper IP
     IP_ADDR_HOST           = "192.168.1.1"    Change to your computer IP
     MASTER_HOST_PORT       = 5006             Local UDP port for master feedback
     SLAVE_HOST_PORT        = 5007             Local UDP port for slave feedback
     UPDATE_INTERVAL_MS     = 10              Control loop period [ms]
     FORCE_N                = 100             Force limit for slave motion [N]

   Execution steps performed by the script:
     1. Homes both master and slave grippers via GRIPLINK (TCP).
     2. Creates two MsiClient instances (one per gripper).
     3. Opens and enables the MSI on both grippers.
     4. In a continuous loop at UPDATE_INTERVAL_MS: reads the master position
        and sends it as the next trajectory point to the slave.
     5. On KeyboardInterrupt: disables and closes both MSI connections cleanly.

   Note: Two separate local UDP ports are required because both feedback
   streams are received on the same host.

   Run:
     python main_teleoperation.py

--------------------------------------------------------------------------------

NETWORK SETUP
-------------
  Default ports used:

    Protocol   Direction          Port    Transport
    ---------  -----------------  ------  ---------
    GRIPLINK   Host → Gripper     10001   TCP
    MSI        Host → Gripper     5005    UDP  (fixed, cannot be changed)
    MSI        Gripper → Host     5006+   UDP  (configurable per demo script)

  All demo scripts require the host IP address to be set correctly so the
  gripper knows where to send MSI feedback packets.

--------------------------------------------------------------------------------

TYPICAL USAGE SEQUENCE
-----------------------
  For any MSI demo, the standard sequence is:

    1. Home the gripper using GriplinkClient over TCP.
    2. Create an MsiClient with the gripper IP and a local receiver port.
    3. Call msi.open(feedback_period_ticks, host_ip) to activate the MSI.
    4. Call msi.enable(command_period_ticks) to start trajectory streaming.
    5. Continuously call msi.point(pos_mm, force_n) to feed the buffer.
    6. Call msi.disable() followed by msi.close() to shut down cleanly.

--------------------------------------------------------------------------------

COPYRIGHT
---------
  Copyright (c) 2026, Weiss Robotics GmbH & Co. KG
  D-71640 Ludwigsburg, Germany
  www.weiss-robotics.com

================================================================================
