pySimBlocks 0.1.0__py3-none-any.whl

examples/advanced/hardware/block_diagram.py

@@ -0,0 +1,162 @@
+import time
+from pathlib import Path
+
+import numpy as np
+import parameters as prm
+from emioapi import EmioMotors
+
+from pySimBlocks.project import load_simulator_from_project
+from pySimBlocks.real_time import RealTimeRunner
+
+try:
+    BASE_DIR = Path(__file__).parent.resolve()
+except Exception:
+    BASE_DIR = Path("")
+
+
+# ------------------------------------------------------------------------------
+# Process
+# ------------------------------------------------------------------------------
+def process_block_diagram(shared_markers_pos, shared_ref_ol, shared_ref_cl, 
+                          shared_control_mode, shared_update, 
+                          event_frame, event_measure):
+    """pySimBlocks runner process.
+    """
+    init_angles = np.array([0.7, 0, 0.7, 0])
+    motors = setup_motors(init_angles)
+
+    # init pySimBlocks runner
+    runner = setup_block_diagram()
+
+    # Initialize variables
+    measure = np.zeros((prm.nb_markers * 2, 1))
+    ref_ol = np.zeros((2, 1))
+    ref_cl = np.zeros((2, 1))
+    control_mode = np.zeros((1, 1))
+    command = np.zeros((2, 1))
+
+    # Initial time for dt measurement
+    t = time.perf_counter()
+    dt_init = 1/60
+    dt_list = []
+
+    while True:
+        event_frame.wait()
+        event_frame.clear()
+
+        send_motors_command(motors, command, init_angles)
+
+        event_measure.wait()
+        event_measure.clear()
+
+        if dt_init is not None:
+            t = time.perf_counter()
+            dt = dt_init
+            dt_init = None
+        else:
+            t2 = time.perf_counter()
+            dt = t2 - t
+            t = t2
+        dt_list.append(dt)
+
+        # data from camera
+        with shared_markers_pos.get_lock():
+            measure[:, 0] = shared_markers_pos[:]
+
+        with shared_update.get_lock():
+            if shared_update.value:
+                with shared_ref_ol.get_lock():
+                    ref_ol[:, 0] = shared_ref_ol[:]
+                with shared_ref_cl.get_lock():
+                    ref_cl[:, 0] = shared_ref_cl[:]
+                with shared_control_mode.get_lock():
+                    control_mode[0,0] = shared_control_mode.value
+                shared_update.value = False
+
+        outs = runner.tick(
+            inputs={
+                "Camera": measure,
+                "Ref_ol": ref_ol,
+                "Ref_cl": ref_cl,
+                "Mode": control_mode
+            },
+            dt=dt,
+            pace=False
+        )
+        command = outs["Cmd"]
+
+
+# ------------------------------------------------------------------------------
+# Helpers
+# ------------------------------------------------------------------------------
+def setup_block_diagram():
+    sim, _plot_cfg = load_simulator_from_project(BASE_DIR / "project.yaml")
+    runner = RealTimeRunner(
+        sim,
+        input_blocks=["Camera", "Ref_cl", "Ref_ol", "Mode"],
+        output_blocks=["Cmd"],
+        target_dt=1/60
+    )
+    runner.initialize()
+    return runner
+
+
+# -------------------------------------------------------
+def setup_motors(init_angles=[0, 0, 0, 0]):
+    motors = EmioMotors()
+    while not motors.open():
+        print("Waiting for motors to open...")
+        time.sleep(1)
+    print("Motors opened successfully.")
+    motors.position_p_gain = [2000, 800, 2000, 800]
+    motors.position_i_gain = [0, 0, 0, 0]
+    motors.position_d_gain = [50, 0, 50, 0]
+    time.sleep(1)
+    motors.angles = init_angles
+    return motors
+
+# -------------------------------------------------------
+def send_motors_command(motors, command, init_angles=np.array([0, 0, 0, 0])):
+    command = command.flatten()
+    motors.angles = [command[0] + init_angles[0], init_angles[1],
+                     command[1] + init_angles[2], init_angles[3]]
+
+# -------------------------------------------------------
+def get_motors_position(motors, init_angles=np.array([0, 0, 0, 0])):
+    motors_pos = np.array(motors.angles) - init_angles
+    return motors_pos[[0, 2]].reshape((2, 1))
+
+# ------------------------------------------------------------------------------
+# BLOCK DIAGRAM FUNCTIONS
+# ------------------------------------------------------------------------------
+def select_cmd(t, dt, u_cl, u_ol, mode):
+    try:
+        mode = mode.item()
+    except Exception:
+        return {"u": np.zeros((2,1))}
+
+    if mode == 0:
+        return {"u": u_ol}
+    elif mode == 1:
+        return {"u": u_cl}
+    else:
+        return {"u": np.zeros((2,1))}
+
+# -------------------------------------------------------
+def filter_first_order(t, dt, u, u_prev):
+    """Apply a first-order low-pass filter to the signal."""
+    if dt <= 0:
+        return {"u_filter" : np.zeros((2,1))}
+    cutoffFreq = 30.  # Cutoff frequency in Hz
+    samplingFreq = 1 / dt
+
+    timeConstant = 1 / (2 * np.pi * cutoffFreq)
+    samplingPeriod = 1 / samplingFreq
+    alpha = samplingPeriod / (timeConstant + samplingPeriod)
+    uf = alpha * u + (1 - alpha) * u_prev
+
+    return {"u_filter" : uf}
+
+
+if __name__ == "__main__":
+    setup_block_diagram() 

examples/advanced/hardware/camera.py

@@ -0,0 +1,93 @@
+import json
+
+import cv2 as cv
+import numpy as np
+import parameters as prm
+from emioapi._depthcamera import DepthCamera
+
+
+# ------------------------------------------------------------------------------
+# Process
+# ------------------------------------------------------------------------------
+def process_camera(shared_markers_pos, shared_start, 
+                   event_frame, event_measure):
+    """Update tracker position."""
+    camera = setup_camera()
+
+    init_pos = np.zeros((2 * prm.nb_markers))
+    pos = np.zeros((2 * prm.nb_markers))
+    start = False
+
+    while True:
+        # get frame from camera
+        ret = camera.get_frame()
+        event_frame.set()
+
+        if not ret:
+            raise RuntimeError("Camera frame not received")
+
+        pos = process_frame(camera, pos)
+
+        if start:
+            pos -= init_pos
+            with shared_markers_pos.get_lock():
+                shared_markers_pos[:] = pos
+            event_measure.set()
+
+        else:
+            init_pos = pos
+            with shared_start.get_lock():
+                start = shared_start.value
+
+        k = cv.waitKey(1)
+        if k == ord('q'):
+            camera.quit()
+            break
+
+
+# ------------------------------------------------------------------------------
+# Helpers
+# ------------------------------------------------------------------------------
+def setup_camera():
+    json_path = prm.data_path / "cameraparameter.json"
+    with json_path.open('r') as fp:
+        json_parameters = json.load(fp)
+    camera = DepthCamera(
+        show_video_feed=True,
+        tracking=True,
+        compute_point_cloud=False,
+        parameter=json_parameters)
+    camera.set_fps(60)
+    camera.set_depth_min(0)
+    camera.set_depth_max(1000)
+    camera.open()
+    return camera
+
+# -------------------------------------------------------
+def pixel_to_mm(points, depth):
+    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 = np.column_stack((points[:, 2], -points[:, 1], points[:, 0]))
+    return points.copy()
+
+# -------------------------------------------------------
+def camera_to_sofa_order(points):
+    i_ymax = np.argmax(points[:, 1])
+    i_rest = [i for i in range(prm.nb_markers) if i != i_ymax]
+    i_sorted_z = sorted(i_rest, key=lambda i: points[i, 2])
+    new_order = [i_sorted_z[1], i_sorted_z[0], i_ymax]
+    return points[new_order].flatten()
+
+# -------------------------------------------------------
+def process_frame(camera, last_pos):
+    indices = [1, 2, 4, 5, 7, 8]
+    camera.process_frame()
+    if len(camera.trackers_pos) == prm.nb_markers:
+        pos = np.array(camera.trackers_camera).reshape(prm.nb_markers, 3).copy()
+        pos = pos.astype(np.float64)
+        pos = pixel_to_mm(pos, 249)
+        markers_pos = camera_to_sofa_order(pos)
+        return markers_pos[indices]
+    return last_pos

examples/advanced/hardware/gui_emio.py

@@ -0,0 +1,231 @@
+import tkinter as tk
+
+import numpy as np
+import parameters as prm
+
+
+# -------------------------------------------------------------------------------
+# Process
+# -------------------------------------------------------------------------------
+def process_gui(shared_ref_ol, shared_ref_cl, shared_start,
+                shared_control_mode, shared_update):
+    root = tk.Tk()
+    app = EmioRealTimeGUI(root, shared_ref_ol, shared_ref_cl, 
+                          shared_start, shared_control_mode, shared_update)
+    root.protocol("WM_DELETE_WINDOW", app.close_app)
+    root.mainloop()
+
+# -------------------------------------------------------------------------------
+# GUI Class
+# -------------------------------------------------------------------------------
+class EmioRealTimeGUI:
+    MOTOR_SCALE = np.pi / 2 / 100  # slider [-100..100] -> radians
+
+    def __init__(self, root, shared_ref_ol, shared_ref_cl, 
+                 shared_start, shared_control_mode, shared_update):
+        self.root = root
+        self.root.title('Emio Real Time Control')
+        self.root.geometry("500x350")
+
+        # ------- Variables -------
+        # shared
+        self.shared_ref_ol = shared_ref_ol
+        self.shared_ref_cl = shared_ref_cl
+        self.shared_start = shared_start
+        self.shared_control_mode = shared_control_mode
+        self.shared_update = shared_update
+
+        # local
+        self.start = False
+        self.active = True
+        self.control_mode = prm.ControlMode.OPEN_LOOP
+
+        # ------- Information Frame -------
+        info_frame = tk.Frame(root)
+        info_frame.pack()
+        info = ["Start", "Active", "Control"]
+        info_values = ["False", "True", "Open Loop"]
+        self.label_info = {}
+        for i, item in enumerate(info):
+            self.label_info[item] = tk.Label(
+                    info_frame, 
+                    text=f"{item}: {info_values[i]}", font=("Arial", 12))
+            self.label_info[item].pack(side="left", padx=5, pady=5)
+
+        # ------- Control Frame -------
+        self.build_sliders()
+        self.build_buttons()
+
+
+    # --------------------------------------------------------------------------
+    # Build methods
+    # --------------------------------------------------------------------------
+    def build_sliders(self):
+        self.label_motors = []
+        self.sliders_motor = []
+        motor_label_frame = tk.Frame(self.root)
+        motor_label_frame.pack()
+        motor_slider_frame = tk.Frame(self.root)
+        motor_slider_frame.pack()
+
+        self.label_ref = []
+        self.sliders_ref = []        
+        ref_label_frame = tk.Frame(self.root)
+        ref_label_frame.pack()
+        ref_slider_frame = tk.Frame(self.root)
+        ref_slider_frame.pack()
+
+        for i in range(2):
+            self.label_motors.append(
+                self._build_label(motor_label_frame, text=f"Motor {i + 1}: 0.00 (rad)")
+            )
+            self.sliders_motor.append(
+                self._build_slider(
+                    motor_slider_frame,
+                    command=lambda val, idx=i: self._motor_action(val, idx),
+                )
+            )
+
+            self.label_ref.append(
+                self._build_label(ref_label_frame, text=f"Ref {i + 1}: 0.00 (mm)")
+            )
+            self.sliders_ref.append(
+                self._build_slider(
+                    ref_slider_frame,
+                    command=lambda val, idx=i: self._ref_action(val, idx),
+                )
+            )
+
+        control_label_frame = tk.Frame(self.root)
+        control_label_frame.pack()
+        control_slider_frame = tk.Frame(self.root)
+        control_slider_frame.pack()
+
+        self.label_control = self._build_label(
+            control_label_frame, text="Desired Control Mode: Open Loop"
+ 
+        )
+        self.slider_control = self._build_slider(
+            control_slider_frame,
+            command=self.slider_control_action,
+            from_=0, to=1
+        )
+
+    # ----------------------------------------------------------
+    def build_buttons(self):
+        button_frame = tk.Frame(self.root)
+        button_frame.pack(pady=10)
+
+        buttons = [
+            ("Start", self.start_action),
+            ("Switch Control", self.control_mode_action),
+            ("Active", self.active_action),
+        ]
+        for name, action in buttons:
+            tk.Button(button_frame, text=name, command=action).pack(side="left", padx=3)
+
+
+    # --------------------------------------------------------------------------
+    # Slider actions
+    # --------------------------------------------------------------------------
+    def _motor_action(self, val, index: int):
+        if not self.start:
+            self.sliders_motor[index].set(0)
+            return
+        cmd = float(val) * self.MOTOR_SCALE  # radians
+        self.label_motors[index].config(text=f"Motor {index + 1}: {cmd:.2f} (rad)")
+        if self.active:
+            mode = prm.ControlMode(0)
+            self._push_current_commands(mode)
+
+    # ----------------------------------------------------------
+    def _ref_action(self, val, index: int):
+        if not self.start:
+            self.sliders_ref[index].set(0)
+            return
+        cmd = float(val)  # mm
+        self.label_ref[index].config(text=f"Ref {index + 1}: {cmd:.2f} (mm)")
+        if self.active:
+            mode = prm.ControlMode(1)
+            self._push_current_commands(mode)
+
+    # ----------------------------------------------------------
+    def slider_control_action(self, val):
+        if not self.start:
+            self.slider_control.set(0)
+            return
+        self.control_mode = prm.ControlMode(int(val))
+        self.label_control.config(text=f"Desired Control Mode: {self.control_mode.label}")
+
+    # --------------------------------------------------------------------------
+    # Button actions
+    # --------------------------------------------------------------------------
+    def start_action(self):
+        if not self.start:
+            self.start = True
+            self.label_info["Start"].config(text="Start: True")
+            with self.shared_start.get_lock():
+                self.shared_start.value = self.start
+            mode = prm.ControlMode(0)
+            self._push_current_commands(mode)
+
+    # ----------------------------------------------------------
+    def active_action(self):
+        if not self.start:
+            return
+        self.active = not self.active
+        self.label_info["Active"].config(text=f"Active: {self.active}")
+
+        if self.active:
+            with self.shared_control_mode.get_lock():
+                mode = prm.ControlMode(int(self.shared_control_mode.value)) 
+            self._push_current_commands(mode)
+
+    # ----------------------------------------------------------
+    def control_mode_action(self):
+        if not self.start:
+            return
+        with self.shared_control_mode.get_lock():
+            self.shared_control_mode.value = int(self.control_mode)
+        with self.shared_update.get_lock():
+            self.shared_update.value = True
+
+        if self.control_mode == prm.ControlMode.OPEN_LOOP:
+            self.label_info["Control"].config(text="Control: Open Loop")
+        else:
+            self.label_info["Control"].config(text=f"Control: {self.control_mode.short}")
+
+    # --------------------------------------------------------------------------
+    # Other methods
+    # --------------------------------------------------------------------------
+    def close_app(self):
+        self.root.destroy()
+
+    # ----------------------------------------------------------
+    def _build_slider(self, frame, command, from_=-100, to=100):
+        slider = tk.Scale(frame, from_=from_, to=to, orient="horizontal", command=command)
+        slider.set(0)
+        slider.pack(side="left", padx=3, pady=5)
+        return slider
+
+    # ----------------------------------------------------------
+    def _build_label(self, frame, text):
+        label = tk.Label(frame, text=text)
+        label.pack(side="left", padx=3, pady=5)
+        return label
+
+    # ----------------------------------------------------------
+    def _push_current_commands(self, mode):
+        if mode == prm.ControlMode.OPEN_LOOP:
+            pos = [float(self.sliders_motor[i].get()) for i in range(2)]
+            with self.shared_ref_ol.get_lock():
+                self.shared_ref_ol[0] = pos[0] * self.MOTOR_SCALE
+                self.shared_ref_ol[1] = pos[1] * self.MOTOR_SCALE
+        else:
+            ref = [float(self.sliders_ref[i].get()) for i in range(2)]
+            with self.shared_ref_cl.get_lock():
+                self.shared_ref_cl[0] = ref[0]
+                self.shared_ref_cl[1] = ref[1]
+
+        with self.shared_update.get_lock():
+            self.shared_update.value = True

examples/advanced/hardware/lmi.py

@@ -0,0 +1,69 @@
+import numpy as np
+import cvxpy as cp
+import control as ct
+
+
+def get_lmi_controller(A, B, C):
+    """Compute the LMI controller.
+    """
+    nx = A.shape[0]
+    nu = B.shape[1]
+    ny = C.shape[0]
+
+    # # Define the decay factor and matrices for the LQR problem
+    decay = 0.99
+    Qy = 1e0 * np.diag(ny//2 * [1, 0.5])
+    Q = C.T @ Qy @ C
+    eig = max(abs(np.linalg.eigvals(Q)))
+    Q = Q + 1e-3*eig * np.eye(nx)
+    R = 1e3 * np.eye(nu)
+    Q_inv = np.linalg.inv(Q)
+    R_inv = np.linalg.inv(R)
+
+    # Define the optimization variables
+    X = cp.Variable((nx, nx), symmetric=True)
+    Y = cp.Variable((nu, nx))
+    delta = cp.Variable(nonneg=True)
+
+    gamma_decay = cp.bmat([
+        [-X, 1/decay * (A @ X - B @ Y).T],
+        [1/decay * (A @ X - B @ Y), -X]
+    ])
+    gamma_lqr = cp.bmat([
+        [-X, (A @ X - B @ Y).T, X, Y.T],
+        [(A @ X - B @ Y), -X, np.zeros((nx, nx)), np.zeros((nx, nu))],
+        [X, np.zeros((nx, nx)), -Q_inv, np.zeros((nx, nu))],
+        [Y, np.zeros((nu, nx)), np.zeros((nu, nx)), -R_inv]
+    ])
+
+    matrix = [-X]
+    matrix.append(gamma_decay)
+    matrix.append(gamma_lqr)
+    matrix.append(cp.bmat([[-delta * np.eye(nx), np.eye(nx)], [np.eye(nx), -X]]))
+    constraints = [mat << -1e-5 * np.eye(mat.shape[0]) for mat in matrix]
+
+    # obj = cp.Minimize(0)
+    obj = cp.Minimize(delta)
+    pb = cp.Problem(obj, constraints)
+    pb.solve(solver="MOSEK")
+    print(f"Problem status: {pb.status}")
+    if pb.status != cp.OPTIMAL:
+        raise Exception(f"Control problem not solved. Status: {pb.status}")
+
+    P = np.linalg.inv(X.value)
+    K = Y.value @ P
+    # Feedforward gain
+    C_ctr = C[[0, 1]]
+    G = np.linalg.inv(C_ctr @ np.linalg.inv(np.eye(nx) - A + B @ K) @ B)
+    
+    return K, G
+
+def get_lmi_observer(A, C):
+    """Compute the LMI observer.
+    """
+    nx = A.shape[0]
+    ny = C.shape[0]
+    Q = 8e2 * np.eye(nx)  # State error cost
+    R = 1e1 * np.eye(ny)  # noise cost
+    L = ct.dlqr(A.T, C.T, Q, R)[0].T
+    return L

examples/advanced/hardware/main.py

@@ -0,0 +1,57 @@
+import time
+import multiprocessing
+
+import parameters as prm
+from block_diagram import process_block_diagram
+from camera import process_camera
+from gui_emio import process_gui
+
+
+def main():
+
+    # shared variables
+    shared_markers_pos = multiprocessing.Array("d", 2 * prm.nb_markers * [0.])
+    shared_ref_ol = multiprocessing.Array("d", 2 * [0.0])
+    shared_ref_cl = multiprocessing.Array("d", 2 * [0.0])
+
+    # shared bool
+    shared_start = multiprocessing.Value("b", False)
+    shared_control_mode = multiprocessing.Value("i", 0)
+    shared_update = multiprocessing.Value("b", False)
+
+    # shared event
+    event_frame = multiprocessing.Event()
+    event_measure = multiprocessing.Event()
+
+    # Create processes
+    p1 = multiprocessing.Process(target=process_camera, args=(
+        shared_markers_pos, shared_start, 
+        event_frame, event_measure))
+
+    p2 = multiprocessing.Process(target=process_gui, args=(
+        shared_ref_ol, shared_ref_cl, 
+        shared_start, shared_control_mode, shared_update))
+
+    p3 = multiprocessing.Process(target=process_block_diagram, args=(
+        shared_markers_pos, shared_ref_ol, shared_ref_cl, 
+        shared_control_mode, shared_update, event_frame, event_measure))
+
+    p1.start()
+    p2.start()
+    p3.start()
+
+    try:
+        while True:
+            time.sleep(5)
+    except KeyboardInterrupt:
+        p1.terminate()
+        p2.terminate()
+        p3.terminate()
+
+        p1.join()
+        p2.join()
+        p3.join()
+
+
+if __name__ == "__main__":
+    main()

examples/advanced/hardware/parameters.py

@@ -0,0 +1,54 @@
+from enum import IntEnum
+from pathlib import Path
+
+import numpy as np
+
+from lmi import get_lmi_controller, get_lmi_observer
+
+
+class ControlMode(IntEnum):
+    OPEN_LOOP = 0
+    STATE_FEEDBACK = 1
+
+    @property
+    def short(self) -> str:
+        return {0: "OL", 1: "SF"}[int(self)]
+
+    @property
+    def label(self) -> str:
+        return {
+            0: "Open Loop",
+            1: "State Feedback",
+        }[int(self)]
+
+
+data_path = Path(__file__).parent / "data"
+
+nb_markers = 3
+
+# ctr_data = np.load(data_path / "controller_order5_linear.npz")
+# K = ctr_data["feedback_gain"]
+# G = ctr_data["feedforward_gain"]
+
+obs_data = np.load(data_path / "observer_order5_linear.npz")
+# L = obs_data["observer_gain"]
+A = obs_data["state_matrix"]
+B = obs_data["input_matrix"]
+C = obs_data["output_matrix"]
+
+x0 = np.zeros((A.shape[0], 1))
+Abar = np.block([
+    [A, B],
+    [np.zeros((B.shape[1], A.shape[1] + B.shape[1]))]
+])
+Bbar = np.block([
+    [np.zeros((A.shape[0], B.shape[1]))],
+    [np.eye(B.shape[1])]
+])
+Cbar = np.block([
+    [C, np.zeros((C.shape[0], B.shape[1]))]
+])
+ 
+K, G = get_lmi_controller(Abar, Bbar, Cbar)
+L = get_lmi_observer(A, C)
+

examples/advanced/qp_problem/params.py

@@ -0,0 +1,10 @@
+from numpy import array, dot
+
+M = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]])
+P = M.T @ M
+q = array([[3., 2., 3.]]) @ M
+#
+# from qpsolvers import solve_qp
+# x = solve_qp(P, q, None, None, None, None, None, solver="clarabel")
+# print(f"QP solution: x = {x}")
+#

examples/advanced/qp_problem/run.py

@@ -0,0 +1,14 @@
+from pathlib import Path
+from pySimBlocks.project import load_simulator_from_project
+from pySimBlocks.project.plot_from_config import plot_from_config
+
+try:
+    BASE_DIR = Path(__file__).parent.resolve()
+except Exception:
+    BASE_DIR = Path("")
+
+sim, plot_cfg = load_simulator_from_project(BASE_DIR / "project.yaml")
+
+logs = sim.run()
+if True and plot_cfg is not None:
+    plot_from_config(logs, plot_cfg)