diff --git a/.gitignore b/.gitignore
index 92ddd5a..3250a5e 100644
--- a/.gitignore
+++ b/.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/
\ No newline at end of file
+html/
diff --git a/emioapi/_depthcamera.py b/emioapi/_depthcamera.py
index 22bf120..3f3adf4 100644
--- a/emioapi/_depthcamera.py
+++ b/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()
diff --git a/emioapi/_positionestimation.py b/emioapi/_positionestimation.py
index 730f645..1da6fa7 100644
--- a/emioapi/_positionestimation.py
+++ b/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]]
-
diff --git a/emioapi/emiocamera.py b/emioapi/emiocamera.py
index cf916c5..84c8034 100644
--- a/emioapi/emiocamera.py
+++ b/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
diff --git a/examples/matlab/Jacobian/jacobian.mat b/examples/matlab/Jacobian/jacobian.mat
new file mode 100644
index 0000000..a60e6de
Binary files /dev/null and b/examples/matlab/Jacobian/jacobian.mat differ
diff --git a/examples/matlab/Jacobian/jacobian.mlx b/examples/matlab/Jacobian/jacobian.mlx
new file mode 100644
index 0000000..a0062a8
Binary files /dev/null and b/examples/matlab/Jacobian/jacobian.mlx differ
diff --git a/examples/matlab/Jacobian/jacobian.slx b/examples/matlab/Jacobian/jacobian.slx
new file mode 100644
index 0000000..40c5961
Binary files /dev/null and b/examples/matlab/Jacobian/jacobian.slx differ
diff --git a/examples/matlab/Jacobian/jacobian.slxc b/examples/matlab/Jacobian/jacobian.slxc
new file mode 100644
index 0000000..9cf5abb
Binary files /dev/null and b/examples/matlab/Jacobian/jacobian.slxc differ
diff --git a/examples/matlab/Jacobian/slprj/_jitprj/s5OXR51N0MTCmAijgIvZlNE.l b/examples/matlab/Jacobian/slprj/_jitprj/s5OXR51N0MTCmAijgIvZlNE.l
new file mode 100644
index 0000000..ddb8858
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_jitprj/s5OXR51N0MTCmAijgIvZlNE.l differ
diff --git a/examples/matlab/Jacobian/slprj/_jitprj/s5OXR51N0MTCmAijgIvZlNE.mat b/examples/matlab/Jacobian/slprj/_jitprj/s5OXR51N0MTCmAijgIvZlNE.mat
new file mode 100644
index 0000000..eb7f581
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_jitprj/s5OXR51N0MTCmAijgIvZlNE.mat differ
diff --git a/examples/matlab/Jacobian/slprj/_sfprj/EMLReport/s5OXR51N0MTCmAijgIvZlNE.mat b/examples/matlab/Jacobian/slprj/_sfprj/EMLReport/s5OXR51N0MTCmAijgIvZlNE.mat
new file mode 100644
index 0000000..25599d7
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_sfprj/EMLReport/s5OXR51N0MTCmAijgIvZlNE.mat differ
diff --git a/examples/matlab/Jacobian/slprj/_sfprj/jacobian/_self/sfun/info/binfo.mat b/examples/matlab/Jacobian/slprj/_sfprj/jacobian/_self/sfun/info/binfo.mat
new file mode 100644
index 0000000..e3ebd16
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_sfprj/jacobian/_self/sfun/info/binfo.mat differ
diff --git a/examples/matlab/Jacobian/slprj/_sfprj/jacobian/amsi_serial.mat b/examples/matlab/Jacobian/slprj/_sfprj/jacobian/amsi_serial.mat
new file mode 100644
index 0000000..52ec2df
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_sfprj/jacobian/amsi_serial.mat differ
diff --git a/examples/matlab/Jacobian/slprj/_sfprj/precompile/WlaHY7ixQkQaG2gOItdvbB.mat b/examples/matlab/Jacobian/slprj/_sfprj/precompile/WlaHY7ixQkQaG2gOItdvbB.mat
new file mode 100644
index 0000000..d28ed67
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_sfprj/precompile/WlaHY7ixQkQaG2gOItdvbB.mat differ
diff --git a/examples/matlab/Jacobian/slprj/_sfprj/precompile/Z9qj3XzcEpjWO87iNQKPFH.mat b/examples/matlab/Jacobian/slprj/_sfprj/precompile/Z9qj3XzcEpjWO87iNQKPFH.mat
new file mode 100644
index 0000000..fe38e8e
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/_sfprj/precompile/Z9qj3XzcEpjWO87iNQKPFH.mat differ
diff --git a/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/checksumOfCache.mat b/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/checksumOfCache.mat
new file mode 100644
index 0000000..1b5d124
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/checksumOfCache.mat differ
diff --git a/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/tmwinternal/simulink_cache.xml b/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/tmwinternal/simulink_cache.xml
new file mode 100644
index 0000000..9b254e8
--- /dev/null
+++ b/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/tmwinternal/simulink_cache.xml
@@ -0,0 +1,6 @@
+
+
+
+ 1CdfPOZQp2C+bnxfUmNTWYKBUzHQfJ90D+MY45thlKRqvvnRI62WZl9tmrGU0JViLHXTLzVjsSVsQ0wLJGdTBg==
+
+
\ No newline at end of file
diff --git a/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/varInfo.mat b/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/varInfo.mat
new file mode 100644
index 0000000..fd2e857
Binary files /dev/null and b/examples/matlab/Jacobian/slprj/sim/varcache/jacobian/varInfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/jitEngineAccessInfo.mat b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/jitEngineAccessInfo.mat
new file mode 100644
index 0000000..dfc46cb
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/jitEngineAccessInfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s0QEhgbdD6HJO2ei765Y2LF.l b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s0QEhgbdD6HJO2ei765Y2LF.l
new file mode 100644
index 0000000..d8b7055
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s0QEhgbdD6HJO2ei765Y2LF.l differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s0QEhgbdD6HJO2ei765Y2LF.mat b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s0QEhgbdD6HJO2ei765Y2LF.mat
new file mode 100644
index 0000000..0d164a0
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s0QEhgbdD6HJO2ei765Y2LF.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s9g6eFMGM1yirYdsP9ROq3C.l b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s9g6eFMGM1yirYdsP9ROq3C.l
new file mode 100644
index 0000000..739290c
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s9g6eFMGM1yirYdsP9ROq3C.l differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s9g6eFMGM1yirYdsP9ROq3C.mat b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s9g6eFMGM1yirYdsP9ROq3C.mat
new file mode 100644
index 0000000..c4e48b1
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/s9g6eFMGM1yirYdsP9ROq3C.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/sYFmWlUFyskuHCmuNU8q8OB.l b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/sYFmWlUFyskuHCmuNU8q8OB.l
new file mode 100644
index 0000000..d8b7055
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/sYFmWlUFyskuHCmuNU8q8OB.l differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_jitprj/sYFmWlUFyskuHCmuNU8q8OB.mat b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/sYFmWlUFyskuHCmuNU8q8OB.mat
new file mode 100644
index 0000000..a150ea5
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_jitprj/sYFmWlUFyskuHCmuNU8q8OB.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat
new file mode 100644
index 0000000..807e39b
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/s0QEhgbdD6HJO2ei765Y2LF.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/s0QEhgbdD6HJO2ei765Y2LF.mat
new file mode 100644
index 0000000..d925ae6
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/s0QEhgbdD6HJO2ei765Y2LF.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/s9g6eFMGM1yirYdsP9ROq3C.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/s9g6eFMGM1yirYdsP9ROq3C.mat
new file mode 100644
index 0000000..ce390fa
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/s9g6eFMGM1yirYdsP9ROq3C.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/sYFmWlUFyskuHCmuNU8q8OB.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/sYFmWlUFyskuHCmuNU8q8OB.mat
new file mode 100644
index 0000000..938179c
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/EMLReport/sYFmWlUFyskuHCmuNU8q8OB.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/D1LamW6c50GVUjdCuCfQKC.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/D1LamW6c50GVUjdCuCfQKC.mat
new file mode 100644
index 0000000..a51d2e1
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/D1LamW6c50GVUjdCuCfQKC.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/PV4bgOV9qZGgyTMusi32nG.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/PV4bgOV9qZGgyTMusi32nG.mat
new file mode 100644
index 0000000..325885a
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/PV4bgOV9qZGgyTMusi32nG.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/autoInferAccessInfo.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/autoInferAccessInfo.mat
new file mode 100644
index 0000000..d6b9626
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/precompile/autoInferAccessInfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/transfert_function/_self/sfun/info/binfo.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/transfert_function/_self/sfun/info/binfo.mat
new file mode 100644
index 0000000..f9b837b
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/transfert_function/_self/sfun/info/binfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/_sfprj/transfert_function/amsi_serial.mat b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/transfert_function/amsi_serial.mat
new file mode 100644
index 0000000..da72aa9
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/_sfprj/transfert_function/amsi_serial.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/checksumOfCache.mat b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/checksumOfCache.mat
new file mode 100644
index 0000000..8fb1ccf
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/checksumOfCache.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml
new file mode 100644
index 0000000..27a34ed
--- /dev/null
+++ b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml
@@ -0,0 +1,6 @@
+
+
+
+ Jhh57wFTTsQB2/9iEyLWm46mwsyh3k2LeYZUtDgDoq3gtr3lhWAK3ItB18HMcTAJmdHu9K913ax+7uvnLohxrg==
+
+
\ No newline at end of file
diff --git a/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/varInfo.mat b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/varInfo.mat
new file mode 100644
index 0000000..e422d39
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/state_space/varInfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/checksumOfCache.mat b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/checksumOfCache.mat
new file mode 100644
index 0000000..efd2a30
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/checksumOfCache.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml
new file mode 100644
index 0000000..d03cf12
--- /dev/null
+++ b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml
@@ -0,0 +1,6 @@
+
+
+
+ BNqY+xVWfqSfLgDHRoG8onrCNVcJaarE3f9pwALr1FtPC49Ts5/QI6qtSOJzrdX6Y0XNSVxKzDUzQWhcZV9oeg==
+
+
\ No newline at end of file
diff --git a/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/varInfo.mat b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/varInfo.mat
new file mode 100644
index 0000000..c54cc89
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/slprj/sim/varcache/transfert_function/varInfo.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/state_space.mat b/examples/matlab/Pendulum/State_Space/state_space.mat
new file mode 100644
index 0000000..2058452
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/state_space.mat differ
diff --git a/examples/matlab/Pendulum/State_Space/state_space.mlx b/examples/matlab/Pendulum/State_Space/state_space.mlx
new file mode 100644
index 0000000..15606ed
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/state_space.mlx differ
diff --git a/examples/matlab/Pendulum/State_Space/state_space.slx b/examples/matlab/Pendulum/State_Space/state_space.slx
new file mode 100644
index 0000000..4aff76c
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/state_space.slx differ
diff --git a/examples/matlab/Pendulum/State_Space/state_space.slxc b/examples/matlab/Pendulum/State_Space/state_space.slxc
new file mode 100644
index 0000000..c6aa46f
Binary files /dev/null and b/examples/matlab/Pendulum/State_Space/state_space.slxc differ
diff --git a/examples/matlab/Pendulum/Transfer_Function/transfert_function.mat b/examples/matlab/Pendulum/Transfer_Function/transfert_function.mat
new file mode 100644
index 0000000..cf501c0
Binary files /dev/null and b/examples/matlab/Pendulum/Transfer_Function/transfert_function.mat differ
diff --git a/examples/matlab/Pendulum/Transfer_Function/transfert_function.mlx b/examples/matlab/Pendulum/Transfer_Function/transfert_function.mlx
new file mode 100644
index 0000000..0513194
Binary files /dev/null and b/examples/matlab/Pendulum/Transfer_Function/transfert_function.mlx differ
diff --git a/examples/matlab/Pendulum/Transfer_Function/transfert_function.slx b/examples/matlab/Pendulum/Transfer_Function/transfert_function.slx
new file mode 100644
index 0000000..cf030cf
Binary files /dev/null and b/examples/matlab/Pendulum/Transfer_Function/transfert_function.slx differ
diff --git a/examples/matlab/Pendulum/Transfer_Function/transfert_function.slxc b/examples/matlab/Pendulum/Transfer_Function/transfert_function.slxc
new file mode 100644
index 0000000..734c4a4
Binary files /dev/null and b/examples/matlab/Pendulum/Transfer_Function/transfert_function.slxc differ
diff --git a/examples/matlab/Pendulum/slprj/sim/varcache/state_space/checksumOfCache.mat b/examples/matlab/Pendulum/slprj/sim/varcache/state_space/checksumOfCache.mat
new file mode 100644
index 0000000..888db72
Binary files /dev/null and b/examples/matlab/Pendulum/slprj/sim/varcache/state_space/checksumOfCache.mat differ
diff --git a/examples/matlab/Pendulum/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml b/examples/matlab/Pendulum/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml
new file mode 100644
index 0000000..e01ca61
--- /dev/null
+++ b/examples/matlab/Pendulum/slprj/sim/varcache/state_space/tmwinternal/simulink_cache.xml
@@ -0,0 +1,6 @@
+
+
+
+ 8qJKTnJiuTH/aGZ17KapfN6U09KZubf0XO7c/dUNBkw9N4OlINRVduUJFv6DJp0w1M4iZSujGkrAS9Osjaccsg==
+
+
\ No newline at end of file
diff --git a/examples/matlab/Pendulum/slprj/sim/varcache/state_space/varInfo.mat b/examples/matlab/Pendulum/slprj/sim/varcache/state_space/varInfo.mat
new file mode 100644
index 0000000..cd599cd
Binary files /dev/null and b/examples/matlab/Pendulum/slprj/sim/varcache/state_space/varInfo.mat differ
diff --git a/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/checksumOfCache.mat b/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/checksumOfCache.mat
new file mode 100644
index 0000000..3fef82f
Binary files /dev/null and b/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/checksumOfCache.mat differ
diff --git a/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml b/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml
new file mode 100644
index 0000000..f2753ff
--- /dev/null
+++ b/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/tmwinternal/simulink_cache.xml
@@ -0,0 +1,6 @@
+
+
+
+ Q83cycAKxswSGwblyt++9jH3boxpVi8p2pBf1zOSA9IdIK5zWmpZikphj/Ycn+E0fwdAmgo4ZB9GeXJsAe985w==
+
+
\ No newline at end of file
diff --git a/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/varInfo.mat b/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/varInfo.mat
new file mode 100644
index 0000000..5ebc5a2
Binary files /dev/null and b/examples/matlab/Pendulum/slprj/sim/varcache/transfert_function/varInfo.mat differ
diff --git a/examples/matlab/README.md b/examples/matlab/README.md
new file mode 100644
index 0000000..6e59709
--- /dev/null
+++ b/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.
diff --git a/examples/matlab/camera.py b/examples/matlab/camera.py
new file mode 100644
index 0000000..a124a41
--- /dev/null
+++ b/examples/matlab/camera.py
@@ -0,0 +1,159 @@
+from multiprocessing.sharedctypes import SynchronizedArray
+from multiprocessing.synchronize import Event
+
+import cv2 as cv
+import numpy as np
+from emioapi.emiocamera import EmioCamera
+
+import params as prm
+
+
+# ------------------------------------------------------------------------------
+# Process
+# ------------------------------------------------------------------------------
+def process_camera(shared_markers_pos: SynchronizedArray,
+ event_frame: Event,
+ event_measure: Event,
+ args) -> None:
+ """Main camera loop: grab frames, track markers, and update shared state.
+
+ Sets ``event_frame`` at each new frame and ``event_measure`` once the
+ marker positions are written to ``shared_markers_pos``.
+ Stops when the user presses ``q``.
+
+ Args:
+ shared_markers_pos: Shared memory array written with marker positions.
+ event_frame: Event set at each new camera frame.
+ event_measure: Event set once marker data is ready.
+ args: Parsed command-line arguments, used for camera configuration
+ """
+ ny = 3 * args.nb_markers
+ camera = setup_camera(args.fps)
+ pos = np.zeros((ny, 1))
+
+ while True:
+ # get frame from camera
+ ret = camera.get_frame()
+ event_frame.set()
+ if ret:
+ pos = process_frame(camera, pos, args.nb_markers, args.side, args.sort)
+
+ with shared_markers_pos.get_lock():
+ shared_markers_pos[:] = pos.flatten()
+ event_measure.set()
+
+ k = cv.waitKey(1)
+ if k == ord('q'):
+ camera.close()
+ break
+
+# ------------------------------------------------------------------------------
+# Helpers
+# ------------------------------------------------------------------------------
+def setup_camera(fps) -> EmioCamera:
+ """Initialise and open the depth camera.
+
+ Args:
+ fps: Desired camera framerate in frames per second.
+
+ Returns:
+ A configured, open DepthCamera instance.
+ """
+ camera = EmioCamera(
+ show=True,
+ track_markers=True,
+ compute_point_cloud=False
+ )
+ camera.fps = fps
+ camera.depth_min = 0
+ camera.depth_max = 1000
+ camera.open()
+ return camera
+
+# -------------------------------------------------------
+def process_frame(camera: EmioCamera,
+ last_pos: np.ndarray,
+ nb_markers: int,
+ side: str,
+ sort: str) -> np.ndarray:
+ """Extract marker positions from the current frame.
+
+ Returns ``last_pos`` unchanged if the expected number of markers is not
+ detected.
+
+ Args:
+ camera: An open, tracking-enabled DepthCamera instance.
+ last_pos: Position array returned on detection failure.
+ nb_markers: Expected number of markers to track.
+ side: Camera side, one of "top", "front", or "plan".
+ sort: Sorting method for front camera, "y" or "z".
+
+ Returns:
+ Marker positions as a column vector, shape ``(ny, 1)``.
+ """
+ camera.process_frame()
+ if len(camera.trackers_pos) == nb_markers:
+ if side == "top":
+ pos = np.array(camera.trackers_pos).reshape(nb_markers, 3).copy()
+ pos = pos.astype(np.float64)
+ return pos
+
+ elif side == "front":
+ p = np.array(camera.trackers_pos_image).reshape(nb_markers, 3).copy()
+ p = p.astype(np.float64)
+ p = pixel_to_mm(p, prm.depth)
+ p = camera_to_sofa_order(p, nb_markers, sort)
+ return p.reshape((-1, 1))
+
+ elif side == "plan":
+ trackers_projected = []
+ for pixel_pos in camera.trackers_pos_image:
+ result = camera._camera.position_estimator.camera_image_to_simulation_plane_intersection(
+ pixel_pos[0],pixel_pos[1],prm.plane_n, prm.plane_d)
+ trackers_projected.append(result)
+ p = np.array(trackers_projected).reshape(nb_markers, 3).copy()
+ p = p.astype(np.float64)
+ p = camera_to_sofa_order(p, nb_markers, sort)
+ return p.reshape((-1, 1))
+
+ return last_pos
+
+# -------------------------------------------------------
+def pixel_to_mm(points: np.ndarray, depth: float) -> np.ndarray:
+ """Project pixel coordinates to millimetres using pinhole intrinsics.
+
+ Args:
+ points: Tracker positions in pixels, shape ``(n, 3)``.
+ depth: Fixed depth in mm used for the projection.
+
+ Returns:
+ Projected points in mm, shape ``(n, 3)``.
+ """
+ ppx, ppy = 319.475, 240.962
+ fx, fy = 382.605, 382.605
+ points[:, 0] = ((points[:, 0] - ppx) / fx) * depth
+ points[:, 1] = ((points[:, 1] - ppy) / fy) * depth
+ points += prm.front2top_offset
+ points = np.column_stack((points[:, 2], -points[:, 1], points[:, 0]))
+ return points.copy()
+
+# -------------------------------------------------------
+def camera_to_sofa_order(points: np.ndarray,
+ nb_markers: int,
+ sort: str) -> np.ndarray:
+ """Reorder markers by ascending coordinate.
+
+ Args:
+ points: Marker positions, shape ``(nb_markers, 3)``.
+ nb_markers: Number of markers.
+ sort: Axis to sort by, "y" or "z".
+
+ Returns:
+ Reordered positions as a flat array.
+ """
+ if sort == "z":
+ i_sorted_z = sorted(range(nb_markers), key=lambda i: points[i, 2])
+ return points[i_sorted_z].flatten()
+ else: # sort by y
+ i_sorted_y = sorted(range(nb_markers), key=lambda i: points[i, 1])
+ return points[i_sorted_y].flatten()
diff --git a/examples/matlab/emio.png b/examples/matlab/emio.png
new file mode 100644
index 0000000..11671c2
Binary files /dev/null and b/examples/matlab/emio.png differ
diff --git a/examples/matlab/launch_python.m b/examples/matlab/launch_python.m
new file mode 100644
index 0000000..15f7f3a
--- /dev/null
+++ b/examples/matlab/launch_python.m
@@ -0,0 +1,42 @@
+function launch_python(remote_ip, remote_port, local_port, bind_port, ...
+ recv_timeout, fps, nb_markers, side, sort_axis)
+% LAUNCH_PYTHON Start the Python bridge in a dedicated xterm terminal.
+%
+% launch_python(REMOTE_IP, REMOTE_PORT, LOCAL_PORT, BIND_PORT,
+% RECV_TIMEOUT, FPS, NB_MARKERS, SIDE, SORT_AXIS)
+%
+% All parameters must match the Simulink UDP Send/Receive block settings.
+%
+% Arguments:
+% remote_ip - IP address of the Python host (string, e.g. '127.0.0.1')
+% remote_port - Port Simulink listens on (Python->Simulink) (int, e.g. 25000)
+% local_port - Port Python listens on (Simulink->Python) (int, e.g. 25001)
+% bind_port - Local port used by Python for sending (int, e.g. 9090)
+% recv_timeout - Socket timeout waiting for Simulink reply (float, e.g. 0.1)
+% fps - Camera framerate [Hz] (int, e.g. 30)
+% nb_markers - Number of markers to track (int, e.g. 1)
+% side - Camera viewpoint: 'top' | 'front' | 'plan' (string)
+% sort_axis - Marker sort axis: 'y' | 'z' (string)
+
+python_env = './env';
+script = './test.py';
+python_bin = fullfile(python_env, 'bin', 'python');
+
+args = sprintf([ ...
+ '--remote_ip %s ' ...
+ '--remote_port %d ' ...
+ '--local_port %d ' ...
+ '--bind_port %d ' ...
+ '--recv_timeout %.3f ' ...
+ '--fps %d ' ...
+ '--nb_markers %d ' ...
+ '--side %s ' ...
+ '--sort %s' ...
+ ], remote_ip, remote_port, local_port, bind_port, ...
+ recv_timeout, fps, nb_markers, side, sort_axis);
+
+system(sprintf( ...
+ 'xterm -title "Bridge Python" -fa "Monospace" -fs 11 -bg black -fg green -e "%s %s %s" &', ...
+ python_bin, script, args));
+
+end
\ No newline at end of file
diff --git a/examples/matlab/main.py b/examples/matlab/main.py
new file mode 100644
index 0000000..aa59316
--- /dev/null
+++ b/examples/matlab/main.py
@@ -0,0 +1,61 @@
+import argparse
+import time
+import multiprocessing
+
+import params as prm
+from camera import process_camera
+from process_motor import process_motors
+
+
+def parse_args():
+ p = argparse.ArgumentParser(description="UDP Bridge - Motor control")
+ p.add_argument("--fps", default=prm.fps, type=int)
+ p.add_argument("--nb_markers", default=prm.nb_markers, type=int)
+ p.add_argument("--side", default=prm.side, type=str,
+ choices=["top", "front", "plan"])
+ p.add_argument("--sort", default=prm.sort, type=str,
+ choices=["y", "z"])
+ p.add_argument("--remote_ip", default=prm.remote_ip, type=str)
+ p.add_argument("--remote_port", default=prm.remote_port, type=int)
+ p.add_argument("--local_port", default=prm.local_port, type=int)
+ p.add_argument("--bind_port", default=prm.bind_port, type=int)
+ p.add_argument("--recv_timeout", default=prm.recv_timeout, type=float)
+
+ return p.parse_args()
+
+def main():
+
+ args = parse_args()
+ ny = 3 * args.nb_markers
+
+ # shared variables
+ shared_markers_pos = multiprocessing.Array("d", ny * [0.])
+
+ # shared event
+ event_frame = multiprocessing.Event()
+ event_measure = multiprocessing.Event()
+
+ # Create processes
+ p1 = multiprocessing.Process(target=process_camera, args=(
+ shared_markers_pos, event_frame, event_measure, args))
+
+ p2 = multiprocessing.Process(target=process_motors, args=(
+ shared_markers_pos, event_frame, event_measure, args))
+
+
+ p1.start()
+ p2.start()
+
+ try:
+ while True:
+ time.sleep(5)
+ except KeyboardInterrupt:
+ p1.terminate()
+ p2.terminate()
+
+ p1.join()
+ p2.join()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/examples/matlab/params.py b/examples/matlab/params.py
new file mode 100644
index 0000000..c0020be
--- /dev/null
+++ b/examples/matlab/params.py
@@ -0,0 +1,23 @@
+import numpy as np
+
+# --- Configurable settings ---
+fps = 30
+nb_markers = 1
+side = "plan" # "top", "front"
+sort = "y" # "y" or "z", only for front camera
+
+# UDP settings
+remote_ip = "127.0.0.1"
+remote_port = 25000
+local_port = 25001
+bind_port = 9090
+recv_timeout = 0.1
+
+# --- Fixed settings (DO NOT CHANGE) ---
+nu = 4 # number of actuators
+
+# camera settings
+depth = 249 # for front camera,
+plane_d = 5
+plane_n = np.array([1,0,0])
+front2top_offset = np.array([-2.2, 195.8, -254])
diff --git a/examples/matlab/process_motor.py b/examples/matlab/process_motor.py
new file mode 100644
index 0000000..cb70a6b
--- /dev/null
+++ b/examples/matlab/process_motor.py
@@ -0,0 +1,130 @@
+import time
+import warnings
+from multiprocessing.sharedctypes import SynchronizedArray
+from multiprocessing.synchronize import Event
+
+import numpy as np
+from emioapi import EmioMotors
+
+import params as prm
+from udp_bridge import UDPBridge, CommStatus
+
+# ------------------------------------------------------------------------------
+# Process
+# ------------------------------------------------------------------------------
+
+def process_motors(shared_markers_pos: SynchronizedArray,
+ event_frame: Event,
+ event_measure: Event,
+ args) -> None:
+ """Motor control loop bridging the remote controller and the physical motors.
+
+ Waits for frame and measure events, reads motor positions and marker data,
+ then exchanges them with the remote host to get the next command. Runs until
+ interrupted by a KeyboardInterrupt (Ctrl-C).
+
+ Args:
+ shared_markers_pos: Shared memory array holding marker positions.
+ event_frame: Event set by the camera process at each new frame.
+ event_measure: Event set when marker measurement is ready.
+ args: Parsed command-line arguments, used for UDP configuration.
+ """
+ ny = 3 * args.nb_markers
+ motors = setup_motors()
+
+ measure = np.zeros((ny, 1))
+ command = np.zeros((prm.nu, 1))
+
+ with UDPBridge(
+ send_size = ny + prm.nu,
+ recv_size = prm.nu,
+ remote_ip = args.remote_ip,
+ remote_port = args.remote_port,
+ local_port = args.local_port,
+ bind_port = args.bind_port,
+ recv_timeout = args.recv_timeout,
+ ) as bridge:
+ bridge.handshake()
+ t = time.perf_counter()
+ dt_expected = 1.0 / args.fps
+
+ while True:
+ # ------------------------------------------------------------------
+ # Wait for new frame and measure events
+ # ------------------------------------------------------------------
+ event_frame.wait()
+ event_frame.clear()
+
+ dt_actual = time.perf_counter() - t
+ t = time.perf_counter()
+ if abs(dt_actual - dt_expected) > 0.1 * dt_expected:
+ warnings.warn(
+ f"[{bridge.seq:04d}] Timing drift: "
+ f"dt={dt_actual * 1000:.1f}ms, "
+ f"expected={dt_expected * 1000:.1f}ms",
+ stacklevel=2,
+ )
+
+ # ------------------------------------------------------------------
+ # Read motors position and send command
+ # ------------------------------------------------------------------
+ motors_pos = get_motors_position(motors)
+ send_motors_command(motors, command)
+
+ event_measure.wait()
+ event_measure.clear()
+
+ # ------------------------------------------------------------------
+ # Read shared marker data
+ # ------------------------------------------------------------------
+ with shared_markers_pos.get_lock():
+ measure[:, 0] = shared_markers_pos[:]
+
+ # ------------------------------------------------------------------
+ # Remote host communication — compute next command
+ # ------------------------------------------------------------------
+ data = np.vstack((measure, motors_pos))
+ command, status = bridge.send_and_receive(data)
+ if status not in (CommStatus.OK, CommStatus.OK_NO_DELAY):
+ print(f"[{bridge.seq}] {status.value}")
+
+# ------------------------------------------------------------------------------
+# Helpers
+# ------------------------------------------------------------------------------
+
+def setup_motors() -> EmioMotors:
+ """Open and return an EmioMotors instance, retrying until successful.
+
+ Returns:
+ An open EmioMotors instance.
+ """
+ motors = EmioMotors()
+ while not motors.open():
+ print("Waiting for motors to open...")
+ time.sleep(1)
+ print("Motors opened successfully.")
+ return motors
+
+
+def send_motors_command(motors: EmioMotors, command: np.ndarray) -> None:
+ """Send a command vector to the motors.
+
+ Args:
+ motors: An open EmioMotors instance.
+ command: Command vector, any shape — will be flattened.
+ """
+ command = command.flatten()
+ motors.angles = command.tolist()
+
+
+def get_motors_position(motors: EmioMotors) -> np.ndarray:
+ """Read current motor angles.
+
+ Args:
+ motors: An open EmioMotors instance.
+
+ Returns:
+ Motor positions as a column vector, shape ``(n_motors, 1)``.
+ """
+ motors_pos = np.array(motors.angles)
+ return motors_pos.reshape((-1, 1))
diff --git a/examples/matlab/test.py b/examples/matlab/test.py
new file mode 100644
index 0000000..5010951
--- /dev/null
+++ b/examples/matlab/test.py
@@ -0,0 +1,88 @@
+import argparse
+import time
+import warnings
+
+import numpy as np
+import params as prm
+from udp_bridge import UDPBridge
+
+
+def parse_args():
+ p = argparse.ArgumentParser(description="UDP Bridge - Motor control")
+ p.add_argument("--fps", default=prm.fps, type=int)
+ p.add_argument("--nb_markers", default=prm.nb_markers, type=int)
+ p.add_argument("--side", default=prm.side, type=str,
+ choices=["top", "front", "plan"])
+ p.add_argument("--sort", default=prm.sort, type=str,
+ choices=["y", "z"])
+ p.add_argument("--remote_ip", default=prm.remote_ip, type=str)
+ p.add_argument("--remote_port", default=prm.remote_port, type=int)
+ p.add_argument("--local_port", default=prm.local_port, type=int)
+ p.add_argument("--bind_port", default=prm.bind_port, type=int)
+ p.add_argument("--recv_timeout", default=prm.recv_timeout, type=float)
+
+ return p.parse_args()
+
+
+def main():
+ """Standalone test loop for the remote bridge without a physical robot.
+
+ Replaces camera and motor data with random vectors, so the bridge can be
+ validated against a remote model without any hardware connected.
+ """
+ args = parse_args()
+ ny = 3 * args.nb_markers
+ measure = np.zeros((ny, 1))
+ command = np.zeros((prm.nu, 1))
+
+ with UDPBridge(
+ send_size = ny + prm.nu,
+ recv_size = prm.nu,
+ remote_ip = args.remote_ip,
+ remote_port = args.remote_port,
+ local_port = args.local_port,
+ bind_port = args.bind_port,
+ recv_timeout = args.recv_timeout,
+ ) as bridge:
+ bridge.handshake()
+ t = time.perf_counter()
+ dt_expected = 1.0 / args.fps
+
+ while True:
+ # ------------------------------------------------------------------
+ # Pace the loop to match args.fps
+ # ------------------------------------------------------------------
+ while time.perf_counter() - t < dt_expected:
+ time.sleep(0.001)
+
+ dt_actual = time.perf_counter() - t
+ t = time.perf_counter()
+ if abs(dt_actual - dt_expected) > 0.1 * dt_expected:
+ warnings.warn(
+ f"[{bridge.seq:04d}] Timing drift: "
+ f"dt={dt_actual * 1000:.1f}ms, "
+ f"expected={dt_expected * 1000:.1f}ms",
+ stacklevel=2,
+ )
+
+ # ------------------------------------------------------------------
+ # Simulated measurements (random stand-in for camera + motors)
+ # ------------------------------------------------------------------
+ motors_pos = np.random.rand(prm.nu, 1)
+ measure = np.random.rand(ny, 1)
+
+ # ------------------------------------------------------------------
+ # Remote host communication — compute next command
+ # ------------------------------------------------------------------
+ data = np.vstack((measure, motors_pos))
+ command, status = bridge.send_and_receive(data)
+ print(
+ f"[{bridge.seq - 1:04d}] {status.value:12s} | "
+ f"dt={dt_actual * 1000:.1f}ms | "
+ f"y={measure.flatten().round(3)} | "
+ f"u={command.flatten().round(3)}"
+ )
+
+
+if __name__ == "__main__":
+ main()
diff --git a/examples/matlab/udp_bridge.py b/examples/matlab/udp_bridge.py
new file mode 100644
index 0000000..ce3685e
--- /dev/null
+++ b/examples/matlab/udp_bridge.py
@@ -0,0 +1,264 @@
+import socket
+import warnings
+from enum import Enum
+
+import numpy as np
+
+
+class CommStatus(Enum):
+ """Status returned by :meth:`UDPBridge.send_and_receive`.
+
+ Attributes:
+ OK: Reply received, sequence numbers match (one-tick delay).
+ OK_NO_DELAY: Reply received with no delay (seq matches exactly).
+ DESYNC: Sequence mismatch detected; bridge is flushing the buffer.
+ TIMEOUT: No reply received within ``recv_timeout``.
+ RECONNECTED: Bridge lost sync and successfully re-handshaked.
+ """
+ OK = "OK"
+ OK_NO_DELAY = "OK_NO_DELAY"
+ DESYNC = "DESYNC"
+ TIMEOUT = "TIMEOUT"
+ RECONNECTED = "RECONNECTED"
+
+
+class UDPBridge:
+ """UDP bridge between Python and a Remote host (e.g. Simulink) for real-time control.
+
+ Python is the clock master: it sends a vector of ``send_size`` doubles at
+ each tick (prepended with a sequence number) and blocks until Remote host
+ replies with a vector of ``recv_size`` doubles (also prepended with a
+ sequence number).
+
+ Wire format (both directions)::
+
+ [ seq (float64) | data[0] | data[1] | ... ]
+ total bytes = (1 + send_size) * 8 (Python -> Remote host)
+ total bytes = (1 + recv_size) * 8 (Remote host -> Python)
+
+ Not thread-safe: ``send_and_receive`` must be called from a single thread.
+
+ Typical usage::
+
+ with UDPBridge(send_size=3, recv_size=2) as bridge:
+ bridge.handshake()
+ while True:
+ reply, status = bridge.send_and_receive(my_data)
+
+ Args:
+ send_size: Number of data doubles sent to UDP each tick.
+ recv_size: Number of data doubles expected from UDP each tick.
+ remote_ip: IP address of the UDP host.
+ remote_port: UDP port Remote host listens on (Python -> Remote host).
+ local_port: UDP port Python listens on (Remote host -> Python).
+ bind_port: Local port used for sending.
+ recv_timeout: Socket timeout in seconds while waiting for a reply.
+ """
+
+ MAX_RECONNECT_ATTEMPTS = 3
+ _MAX_DESYNC_COUNT = 5
+
+ def __init__(
+ self,
+ send_size: int,
+ recv_size: int,
+ remote_ip: str = "127.0.0.1",
+ remote_port: int = 25000,
+ local_port: int = 25001,
+ bind_port: int = 9090,
+ recv_timeout: float = 0.1,
+ ):
+ self.send_size = send_size
+ self.recv_size = recv_size
+ self.remote_addr = (remote_ip, remote_port)
+ self.recv_timeout = recv_timeout
+
+ self._sock_send = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self._sock_send.bind(("0.0.0.0", bind_port))
+
+ self._sock_recv = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self._sock_recv.bind(("0.0.0.0", local_port))
+
+ self._seq = 0
+ self._last_reply = np.zeros((recv_size, 1))
+ self._timeout_count = 0
+ self._desync_count = 0
+
+ # --------------------------------------------------------------------------
+ # Context manager
+ # --------------------------------------------------------------------------
+
+ def __enter__(self) -> "UDPBridge":
+ return self
+
+ def __exit__(self, *_) -> None:
+ self.close()
+
+ # --------------------------------------------------------------------------
+ # Public API
+ # --------------------------------------------------------------------------
+
+ def handshake(self, handshake_timeout: float = 0.05) -> None:
+ """Block until Remote host acknowledges the connection.
+
+ Sends ``seq=-1`` with a zero payload of ``send_size`` doubles so the
+ wire size is identical to the main loop. Resets the sequence counter
+ on success.
+
+ Args:
+ handshake_timeout: Per-attempt socket timeout in seconds.
+ """
+ print(" -> Start the Remote host now (waiting for handshake...)")
+
+ payload = np.zeros(1 + self.send_size, dtype=np.float64)
+ payload[0] = -1.0
+
+ self._sock_recv.settimeout(handshake_timeout)
+
+ while True:
+ self._sock_send.sendto(self._pack(payload), self.remote_addr)
+ try:
+ self._sock_recv.recvfrom((self.recv_size + 1) * 8)
+ print(" Handshake OK - ready!\n")
+ break
+ except socket.timeout:
+ pass
+
+ self._seq = 0
+ self._timeout_count = 0
+ self._desync_count = 0
+ self._last_reply = np.zeros((self.recv_size, 1))
+ self._sock_recv.settimeout(self.recv_timeout)
+
+ def send_and_receive(self, data: np.ndarray) -> tuple[np.ndarray, CommStatus]:
+ """Send a data vector and return the reply from Remote host.
+
+ ``data`` is silently padded with zeros or truncated to match
+ ``send_size`` if needed (a warning is emitted).
+
+ On consecutive timeouts (>= ``MAX_RECONNECT_ATTEMPTS``) or persistent
+ desyncs (>= ``_MAX_DESYNC_COUNT``), a new handshake is triggered
+ automatically.
+
+ Args:
+ data: Vector of doubles, ideally shape ``(send_size,)`` or
+ ``(send_size, 1)``.
+
+ Returns:
+ A tuple ``(reply, status)`` where ``reply`` has shape
+ ``(recv_size, 1)`` and ``status`` is one of:
+ :class:`CommStatus`.
+
+ Raises:
+ RuntimeError: If the packet received from Remote host has an
+ unexpected size (mismatch between ``recv_size`` and the
+ Remote host UDP Send block configuration).
+ """
+ data = self._coerce_send_data(np.asarray(data, dtype=np.float64).flatten())
+ seq = self._seq
+ payload = np.concatenate([[float(seq)], data])
+ self._sock_send.sendto(self._pack(payload), self.remote_addr)
+
+ try:
+ raw, _ = self._sock_recv.recvfrom((self.recv_size + 1) * 8)
+
+ expected = (self.recv_size + 1) * 8
+ if len(raw) != expected:
+ raise RuntimeError(
+ f"Unexpected packet size from Remote host: got {len(raw)} bytes, "
+ f"expected {expected}. "
+ f"Check that recv_size={self.recv_size} matches your "
+ f"Remote host UDP Send block."
+ )
+
+ response = self._unpack(raw, self.recv_size + 1)
+ seq_back = int(response[0, 0])
+ reply = response[1:]
+ self._last_reply = reply
+ self._timeout_count = 0
+ status = self._sync_status(seq_back, seq)
+
+ if status == CommStatus.DESYNC:
+ self._desync_count += 1
+ self._flush_recv_buffer()
+ if self._desync_count >= self._MAX_DESYNC_COUNT:
+ print(f"\n {self._MAX_DESYNC_COUNT} consecutive desyncs "
+ f"- restarting handshake...")
+ self.handshake()
+ return self._last_reply, CommStatus.RECONNECTED
+ else:
+ self._desync_count = 0
+
+ except socket.timeout:
+ reply = self._last_reply
+ self._timeout_count += 1
+ status = CommStatus.TIMEOUT
+
+ if self._timeout_count >= self.MAX_RECONNECT_ATTEMPTS:
+ print(f"\n {self.MAX_RECONNECT_ATTEMPTS} consecutive timeouts "
+ f"- restarting handshake...")
+ self.handshake()
+ return self._last_reply, CommStatus.RECONNECTED
+
+ self._seq += 1
+ return reply, status
+
+ def close(self) -> None:
+ """Release UDP sockets."""
+ self._sock_send.close()
+ self._sock_recv.close()
+
+ @property
+ def seq(self) -> int:
+ """Current sequence counter."""
+ return self._seq
+
+ # --------------------------------------------------------------------------
+ # Private helpers
+ # --------------------------------------------------------------------------
+
+ def _coerce_send_data(self, data: np.ndarray) -> np.ndarray:
+ """Pad or truncate ``data`` to ``send_size``, warning if needed."""
+ n = len(data)
+ if n == self.send_size:
+ return data
+ if n < self.send_size:
+ warnings.warn(
+ f"send_and_receive: data has {n} value(s), expected "
+ f"{self.send_size}. Padding with zeros.",
+ stacklevel=3,
+ )
+ return np.pad(data, (0, self.send_size - n))
+ warnings.warn(
+ f"send_and_receive: data has {n} value(s), expected "
+ f"{self.send_size}. Truncating to first {self.send_size}.",
+ stacklevel=3,
+ )
+ return data[: self.send_size]
+
+ def _flush_recv_buffer(self) -> None:
+ """Drain any stale packets so the next read is up to date."""
+ self._sock_recv.settimeout(0)
+ try:
+ while True:
+ self._sock_recv.recvfrom((self.recv_size + 1) * 8)
+ except (socket.timeout, BlockingIOError):
+ pass
+ finally:
+ self._sock_recv.settimeout(self.recv_timeout)
+
+ @staticmethod
+ def _pack(arr: np.ndarray) -> bytes:
+ return arr.astype(np.float64).flatten().tobytes()
+
+ @staticmethod
+ def _unpack(data: bytes, n: int) -> np.ndarray:
+ return np.frombuffer(data, dtype=np.float64).reshape(n, 1)
+
+ @staticmethod
+ def _sync_status(seq_back: int, seq: int) -> CommStatus:
+ if seq_back == seq - 1:
+ return CommStatus.OK
+ if seq_back == seq:
+ return CommStatus.OK_NO_DELAY
+ return CommStatus.DESYNC