Matlab Emio Communicaiton

.gitignore

@@ -3,6 +3,10 @@ __pycache__/
 *.py[cod]
 *$py.class
 
+# Matlab extensions
+*.slxc
+slprj/
+
 # C extensions
 *.so
 
@@ -176,4 +180,4 @@ cython_debug/
 
 # Custom
 docs/
-html/
+html/

emioapi/_depthcamera.py

@@ -1,6 +1,4 @@
-import os
 import json
-from time import sleep
 import time
 from enum import Enum
 
@@ -9,7 +7,7 @@
 import pyrealsense2 as rs
 
 from ._camerafeedwindow import CameraFeedWindow
-from ._positionestimation import PositionEstimation, image_pixel_to_mm, CONFIG_FILENAME
+from ._positionestimation import PositionEstimation, CONFIG_FILENAME
 from emioapi._logging_config import logger
 
 DEFAULT_CAMERA_PARAMS = {"hue_h": 90, "hue_l": 36, "sat_h": 255, "sat_l": 138, "value_h": 255, "value_l": 35, "erosion_size": 0, "area": 100}
@@ -68,6 +66,7 @@ class DepthCamera:
     parameter = {}
     tracking = False
     trackers_pos = []
+    trackers_pos_image = []
     maskWindow = None
     frameWindow = None
     hsvWindow = None
@@ -77,6 +76,7 @@ class DepthCamera:
     maskFrame = None
     frame: np.ndarray = None
     depth_frame: np.ndarray = None
+    depth_rsframe: np.ndarray = None
     depth_max = 430
     depth_min = 2
     calibration_status = CalibrationStatusEnum.NOT_CALIBRATED
@@ -123,6 +123,7 @@ def __init__(self,
             return
 
         self.trackers_pos = []
+        self.trackers_pos_image = []
 
         if parameter:
             self.parameter = parameter
@@ -290,19 +291,24 @@ def get_frame(self):
         color_frame = frames.get_color_frame()
 
         if not depth_frame or not color_frame:
-            return False, color_frame, depth_frame
+            return False
 
         # Convert images to numpy arrays
-        depth_image = np.asanyarray(depth_frame.get_data())
-        color_image = np.asanyarray(color_frame.get_data())
-        return True, color_image, depth_image, depth_frame
+        self.depth_frame = np.asanyarray(depth_frame.get_data())
+        self.frame = np.asanyarray(color_frame.get_data())
+        self.depth_rsframe = depth_frame
 
+        return True
 
-    def update(self):
-        ret, self.frame, self.depth_frame, depth_rsframe = self.get_frame()
 
+    def update(self):
+        ret = self.get_frame()
         if ret is False:
             return
+        self.process_frame()
+
+    def process_frame(self):
+
         # if frame is read correctly ret is True
 
         self.hsvFrame = cv.cvtColor(self.frame, cv.COLOR_BGR2HSV)
@@ -333,6 +339,7 @@ def update(self):
                 areas = [cv.contourArea(cnt) for cnt in contours]
 
                 self.trackers_pos = []
+                self.trackers_pos_image = []
                 for i, a in enumerate(areas):
                     if a > self.parameter['area']:
                         x, y = compute_contour_center(contours[i])
@@ -341,6 +348,7 @@ def update(self):
                         depth = compute_median_depth(contours[i], self.depth_frame) if self.depth_frame[y, x] == 0 else self.depth_frame[y, x]
                         worldx, worldy, worldz = self.position_estimator.camera_image_to_simulation(x, y, depth)
                         self.trackers_pos.append([worldx, worldy, worldz])
+                        self.trackers_pos_image.append([x, y, depth])
 
                         cv.drawContours(marker_mask, [contours[i]], -1, color=255, thickness=-1)
                         for frame in [self.hsvFrame, self.frame]:
@@ -352,7 +360,7 @@ def update(self):
                             cv.drawContours(self.frame, [contours[i]], -1, (255, 255, 0), 3)
 
         if self.compute_point_cloud:
-            points = self.pc.calculate(depth_rsframe)
+            points = self.pc.calculate(self.depth_rsframe)
             v = points.get_vertices()
             self.point_cloud = np.asanyarray(v).view(np.float32).reshape(-1, 3)  # xyz
 
@@ -370,7 +378,7 @@ def update(self):
                 self.hsvWindow.set_frame(self.hsvFrame)
 
             if self.depthWindow is not None and self.depthWindow.running:
-                colorized = np.asanyarray(rs.colorizer().colorize(depth_rsframe).get_data())
+                colorized = np.asanyarray(rs.colorizer().colorize(self.depth_rsframe).get_data())
                 self.depthWindow.set_frame(colorized)
 
             self.rootWindow.update()

emioapi/_positionestimation.py

@@ -344,6 +344,33 @@ def calibrate(self, frame, depth_image, aggregate, window=None)-> bool:
 
         return True
 
+    def camera_image_to_simulation_plane_intersection(self, x: int, y: int, plane_n: np.ndarray, plane_d: float) -> list[float]:
+        """
+        Calculate the position of the object in our frame space by projecting the ray from the camera to the image point on a plane.
+
+        Args
+        x,y: int
+            The pixel coordinates
+
+        plane_n: np.ndarray
+            The normal vector of the plane
+
+        plane_d: float
+            The distance of the plane from the origin along its normal vector
+
+        Return:
+            position: numpy.ndarray
+                The real world coordinates of the object in the Emio frame space
+        """
+        ray_world = self.camera_image_to_simulation(x, y, 1.0) - self.t
+        camera_pos_world = self.t
+        denom = plane_n.dot(ray_world)
+
+        if abs(denom) < 1e-6:
+            raise ValueError("Ray is parallel to the plane")
+        s = - (plane_n.dot(camera_pos_world) + plane_d) / denom
+        result = camera_pos_world + s * ray_world
+        return [result[0], result[1], result[2]]
 
     def camera_image_to_simulation(self, x: int, y: int, depth: float) -> list[float]:
         """
@@ -373,4 +400,3 @@ def camera_image_to_simulation(self, x: int, y: int, depth: float) -> list[float
         position += self.t
         return [position[0], position[1], position[2]]
 
-

emioapi/emiocamera.py

@@ -61,6 +61,7 @@ class EmioCamera:
     _running: bool = False
     _parameter: dict = None
     _trackers_pos: list = []
+    _trackers_pos_image: list = []
     _point_cloud: np.ndarray = None
     _hsv_frame: np.ndarray = None
     _mask_frame: np.ndarray = None
@@ -261,6 +262,19 @@ def trackers_pos(self) -> list:
             else:
                 return []
 
+    @property
+    def trackers_pos_image(self) -> list:
+        """
+        Get the positions of the trackers in the image frame.
+        Returns:
+            list: The positions of the trackers in the image frame as a list of lists.
+        """
+        with self._lock:
+            if self._tracking:
+                return self._trackers_pos_image
+            else:
+                return []
+
     @property
     def point_cloud(self) -> np.ndarray:
         """
@@ -462,7 +476,17 @@ def update(self):
             Update the camera frames and tracking elements (markers and point cloud)
         """
         if self._camera is not None:
-            self._camera.update()
+            if self.get_frame():
+                self.process_frame()
+
+    def get_frame(self) -> bool:
+        if self._camera is not None:
+            return self._camera.get_frame()
+        return False
+
+    def process_frame(self):
+        if self._camera is not None:
+            self._camera.process_frame()
             with self._lock:
                 self._hsv_frame = self._camera.hsvFrame
                 self._mask_frame = self._camera.maskFrame
@@ -471,6 +495,7 @@ def update(self):
                     for p_camera in self._camera.trackers_pos:
                         p_emio = [p_camera[0], p_camera[1], p_camera[2], 0, 0, 0, 1]
                         self._trackers_pos.append(p_emio[0:3])
+                    self._trackers_pos_image = self._camera.trackers_pos_image.copy()
                     logger.debug(f"Trackers positions in camera frame: {self._camera.trackers_pos}, converted to Emio frame: {self._trackers_pos}")
                 if self._compute_point_cloud:
                         self._point_cloud = self._camera.point_cloud

examples/matlab/Jacobian/slprj/sim/varcache/jacobian/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="c7637f53-da09-4dd7-b4b0-a3ccfd495eec">
+    <checksum>1CdfPOZQp2C+bnxfUmNTWYKBUzHQfJ90D+MY45thlKRqvvnRI62WZl9tmrGU0JViLHXTLzVjsSVsQ0wLJGdTBg==</checksum>
+  </slcache.FileAttributes>
+</MF0>

examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="4fd59ba0-7a75-48fa-bb5a-0c08c968adfb">
+    <checksum>Jhh57wFTTsQB2/9iEyLWm46mwsyh3k2LeYZUtDgDoq3gtr3lhWAK3ItB18HMcTAJmdHu9K913ax+7uvnLohxrg==</checksum>
+  </slcache.FileAttributes>
+</MF0>

examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="ea97faf6-8f70-4221-8aad-4ed8c5c25ada">
+    <checksum>BNqY+xVWfqSfLgDHRoG8onrCNVcJaarE3f9pwALr1FtPC49Ts5/QI6qtSOJzrdX6Y0XNSVxKzDUzQWhcZV9oeg==</checksum>
+  </slcache.FileAttributes>
+</MF0>

examples/matlab/Pendulum/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="94ca11ee-1535-4f1a-8c9e-419ec7eeb144">
+    <checksum>8qJKTnJiuTH/aGZ17KapfN6U09KZubf0XO7c/dUNBkw9N4OlINRVduUJFv6DJp0w1M4iZSujGkrAS9Osjaccsg==</checksum>
+  </slcache.FileAttributes>
+</MF0>

examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MF0 version="1.1" packageUris="http://schema.mathworks.com/mf0/SlCache/19700101">
+  <slcache.FileAttributes type="slcache.FileAttributes" uuid="10d569cb-de26-461b-ac1b-e88403b0a43b">
+    <checksum>Q83cycAKxswSGwblyt++9jH3boxpVi8p2pBf1zOSA9IdIK5zWmpZikphj/Ycn+E0fwdAmgo4ZB9GeXJsAe985w==</checksum>
+  </slcache.FileAttributes>
+</MF0>

examples/matlab/README.md

@@ -0,0 +1,118 @@
+# Compliant Robotics Lab — Python/Simulink Bridge
+
+## Dependencies
+
+Requires modifications to `emioapi/_depthcamera.py`, `emioapi/emiocamera.py`,
+and `emioapi/_positionestimation.py` for:
+
+- Raw camera coordinate access via the `trackers_pos_image` variable
+  (used in `"front"` mode, without processing noise)
+- A `process_frame()` function that separates camera acquisition from data
+  processing (enables event-based inter-process synchronisation).
+  `update()` remains functional and calls `process_frame()` internally.
+- Plane projection of marker positions to reduce camera noise
+  (`"plan"` mode only, for now).
+
+---
+
+## Quickstart
+
+```bash
+python main.py
+```
+
+Then start the Simulink simulation. The connection is established automatically.
+
+To stop: **Ctrl-C** or press **q** in the camera window.
+
+---
+
+## Configuration
+
+The only file you need to edit is **`params.py`**:
+
+| Parameter | Description |
+|---|---|
+| `fps` | Control loop frequency (Hz) |
+| `nb_markers` | Number of markers tracked by the camera |
+| `side` | Camera viewpoint: `"top"`, `"front"`, or `"plan"` (plane projection) |
+| `sort` | Marker sorting axis: `"y"` or `"z"` |
+
+Do not modify any other parameters.
+
+---
+
+## Testing Without Hardware
+
+```bash
+python test.py
+```
+
+Replaces camera and motor data with random values.
+Useful for validating the Simulink model independently.
+
+---
+
+## File Structure
+
+| File | Role |
+|---|---|
+| `main.py` | Entry point |
+| `params.py` | ⚙️ Configuration — **the only file you should edit** |
+| `camera.py` | Marker acquisition and processing |
+| `process_motor.py` | Motor command loop |
+| `udp_bridge.py` | UDP communication layer |
+| `test.py` | Hardware-free test script |
+
+---
+
+## Simulink & MATLAB Setup
+
+**Required toolbox:**
+- Instrument Control Toolbox (for UDP blocks)
+
+**Simulink solver settings:**
+
+| Parameter | Value |
+|---|---|
+| Solver type | `Fixed-step` |
+| Solver | `ode1` (or any other explicit method) |
+| Fixed step size | `1/30` (matches `fps`) |
+| Stop time | `inf` |
+
+**UDP Receive block:**
+
+| Parameter | Value |
+|---|---|
+| Local port | `25000` |
+| Remote port | `9090` |
+| Data size | `[3*nb_markers + 5, 1]` (seq + ny + nu) |
+| Data type | `double` |
+| Byte order | `little-endian` |
+| Blocking mode | ✅ enabled |
+| Timeout | `2` s (or more if Python starts slowly) |
+
+**UDP Send block:**
+
+| Parameter | Value |
+|---|---|
+| Remote port | `25001` |
+| Byte order | `little-endian` |
+| Blocking mode | ✅ enabled |
+
+---
+
+## How It Works
+
+Python is the clock master. At each camera frame (~30 Hz):
+
+1. Camera produces a frame → `event_frame` fires
+2. `process_motors` reads motor positions and sends the previous command
+3. Marker positions are written to shared memory → `event_measure` fires
+4. `process_motors` sends `[seq, y..., motors_pos...]` to Simulink via UDP
+5. Simulink computes the control law and replies with `[seq, u...]`
+6. The new command is applied at the next tick
+
+If Simulink stops responding, the bridge automatically re-initiates the
+handshake after `MAX_RECONNECT_ATTEMPTS` consecutive timeouts — no need to
+restart Python.