.gitignore

@@ -1,2 +0,0 @@
-models
-__pycache__

control.py

@@ -1,81 +1,87 @@
-import logging
 import math
 import numpy as np
-from math import *
-from logs import *
-from xbox import *
-import time
 
-# DEBUG
+
-LEGS_LOG_LEVEL = logging.INFO
+def sandbox(t):
-CONTROLLER_LOG_LEVEL = logging.INFO
+    """
-# Variables configurations
+    python simulator.py -m sandbox
+
+    Un premier bac à sable pour faire des expériences
+
+    La fonction reçoit le temps écoulé depuis le début (t) et retourne une position cible
+    pour les angles des 12 moteurs
+
+    - Entrée: t, le temps (secondes écoulées depuis le début)
+    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
+    """
+
+    # Par exemple, on envoie un mouvement sinusoidal
+    targets = [0] * 12
+    targets[0] = t ** 3
+    targets[1] = 0
+    targets[2] = 0
+    targets[3] = t ** 3
+    targets[4] = 0
+    targets[5] = 0
+    targets[6] = t ** 3
+    targets[7] = 0
+    targets[8] = 0
+    targets[9] = t ** 3
+    targets[10] = 0
+    targets[11] = 0
+
+    return targets
+
+
+def direct(alpha, beta, gamma):
+    """
+    python simulator.py -m direct
+
+    Le robot est figé en l'air, on ne contrôle qu'une patte
+
+    Reçoit en argument la cible (alpha, beta, gamma) des degrés de liberté de la patte, et produit
+    la position (x, y, z) atteinte par le bout de la patte
+
+    - Sliders: les angles des trois moteurs (alpha, beta, gamma)
+    - Entrées: alpha, beta, gamma, la cible (radians) des moteurs
+    - Sortie: un tableau contenant la position atteinte par le bout de la patte (en mètres)
+    """
+
+    return 0, 0, 0
+
+
+from math import *
 
 l1h = 0.049
 l1v = 0.032
-l1 = l1h  # this is not the real distance as it's not the one needed to calculate position.
+l1 = l1h
 
-# length between motor 2 and motor 3.
 l2h = 0.0605
 l2v = 0.02215
 l2 = sqrt(l2h ** 2 + l2v ** 2)
 
-# length between motor 3 and end of the leg.
 l3h = 0.012
 l3v = 0.093
 l3 = sqrt(l3h ** 2 + l3v ** 2)
 
-# offset of the 'head', the legs isolated at each end.
 tete_x = 0.095
 
-# offset of the legs at the side.
 patte_y = 0.032
 patte_x = 0.079
-num_patte = 6
-
-# Logs functions
-legsLogger = setup_logger("legs", LEGS_LOG_LEVEL)
-controllerLogger = setup_logger("Controller", CONTROLLER_LOG_LEVEL)
-
-# Initialize controller
-xbox = Xbox(controllerLogger)
-CONTROLLER_MODE = xbox.initialized
-xbox.mode_count = 4
-
-neutral_position = np.array([
-    [0.1, 0.15, -0.15],
-    [-0.1, 0.15, -0.15],
-    [-0.2, -0.00, -0.15],
-    [-0.1, -0.15, -0.15],
-    [0.1, -0.15, -0.15],
-    [0.2, 0, -0.15]
-])
-
-
-def interpol2(point2, point1, t):
-    x1, y1, z1 = point1
-    x2, y2, z2 = point2
-    return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
-
-
-def get_current_step(t, step_duration, movement_duration):
-    time_passed = 0
-    for i in range(len(step_duration)):
-        time_passed += step_duration[i]
-        if t % movement_duration < time_passed:
-            return i
-
-
-def get_current_step_advancement(t, movement_duration, step_duration, current_step):
-    current_step = get_current_step(t, step_duration, movement_duration)
-    t = t % movement_duration
-    for i in range(0, current_step):
-        t -= step_duration[i]
-    return t / step_duration[current_step]
 
 
 def inverse(x, y, z):
     """
+    python simulator.py -m inverse
+
+    Le robot est figé en l'air, on ne contrôle qu'une patte
+
+    Reçoit en argument une position cible (x, y, z) pour le bout de la patte, et produit les angles
+    (alpha, beta, gamma) pour que la patte atteigne cet objectif
+
+    - Sliders: la position cible x, y, z du bout de la patte
+    - Entrée: x, y, z, une position cible dans le repère de la patte (mètres), provenant du slider
+    - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians)
     """
     # Dimensions (m)
     z += l1v
@@ -106,9 +112,53 @@ def inverse(x, y, z):
     # return [0, angle1 , angle1 -pi/2 + atan(l3h/l3v)]
 
 
+def draw(t):
+    """
+    python simulator.py -m draw
+
+    Le robot est figé en l'air, on ne contrôle qu'une patte
+
+    Le but est, à partir du temps donné, de suivre une trajectoire de triangle. Pour ce faire, on
+    utilisera une interpolation linéaire entre trois points, et la fonction inverse précédente.
+
+    - Entrée: t, le temps (secondes écoulées depuis le début)
+    - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians)
+    """
+
+    def interpol(x2, y2, z2, x1, y1, z1, t):
+        return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
+
+    p1 = [0.15, 0, 0]
+    p2 = [0.1, 0.1, 0]
+    p3 = [0.1, 0, 0.05]
+
+    positions = [p1, p2, p3]
+
+    ratio = (t % 1)
+    partie = floor((t % 3))
+    partie2 = (partie + 1) % 3
+    # print(f"partie: {partie}, partie2: {partie2}")
+    wanted_origin_x, wanted_origin_y, wanted_origin_z = positions[partie][0], positions[partie][1], positions[partie][
+        2],
+    wanted_dest_x, wanted_dest_y, wanted_dest_z = positions[partie2][0], positions[partie2][1], positions[partie2][2],
+    # print(wanted_origin_x, wanted_origin_y, wanted_origin_z)
+    # print(wanted_dest_x, wanted_dest_y, wanted_dest_z)
+    # print(t)
+    wanted_x, wanted_y, wanted_z = interpol(wanted_origin_x, wanted_origin_y, wanted_origin_z, wanted_dest_x,
+                                            wanted_dest_y, wanted_dest_z, ratio)
+
+    return inverse(wanted_x, wanted_y, wanted_z)
+
+
 def legs(targets_robot):
     """
-    takes a list of target and offsets it to be in the legs referential
+    python simulator.py -m legs
+
+    Le robot est figé en l'air, on contrôle toute les pattes
+
+    - Sliders: les 12 coordonnées (x, y, z) du bout des 4 pattes
+    - Entrée: des positions cibles (tuples (x, y, z)) pour le bout des 4 pattes
+    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
     """
 
     targets = [0] * 18
@@ -137,9 +187,36 @@ def legs(targets_robot):
     return targets
 
 
-def naive_walk(t, speed_x, speed_y):
+def interpol2(point2, point1, t):
+    x1, y1, z1 = point1
+    x2, y2, z2 = point2
+    return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2
+
+
+def walkV1(t, speed_x, speed_y, speed_rotation):
+    """
+    python simulator.py -m walk
+
+    Le but est d'intégrer tout ce que nous avons vu ici pour faire marcher le robot
+
+    - Sliders: speed_x, speed_y, speed_rotation, la vitesse cible du robot
+    - Entrée: t, le temps (secondes écoulées depuis le début)
+            speed_x, speed_y, et speed_rotation, vitesses cibles contrôlées par les sliders
+    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
+    """
+    # t = t*speed_x * 20
+    num_patte = 6
     slider_max = 0.200
 
+    neutral_position = np.array([
+        [0.1, 0.15, -0.15],
+        [-0.1, 0.15, -0.15],
+        [-0.2, -0.00, -0.15],
+        [-0.1, -0.15, -0.15],
+        [0.1, -0.15, -0.15],
+        [0.2, 0, -0.15]
+    ])
+
     real_position = np.copy(neutral_position)
 
     movement_x = np.array([
@@ -169,15 +246,36 @@ def naive_walk(t, speed_x, speed_y):
     assert len(
         step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"
 
+    def get_current_step(t):
+        time_passed = 0
+        for i in range(len(step_duration)):
+            time_passed += step_duration[i]
+            if t % movement_duration < time_passed:
+                return i
+
+    def get_current_step_advancement(t):
+        current_step = get_current_step(t)
+        t = t % movement_duration
+        for i in range(0, current_step):
+            t -= step_duration[i]
+        return t / step_duration[current_step]
+
     def get_next_step(t):
-        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
+        return floor((get_current_step(t) + 1) % step_count)
+
+    def rotate(patte):
+        return [1, -1, 0, -1, 1, 0][
+            patte] * movement_x  # + [-1, 1, -1, 1][patte] * movement_y # mettre des 0 partout sur le Y fait une très belle rotation
+
+    def normalize(matrix, slider_max, speed):
+        return (matrix / slider_max) * speed
 
     offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
     assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
 
     for patte in range(num_patte):
         time = t + offsets[patte]
-        mov_index_start = get_current_step(time, step_duration, movement_duration)
+        mov_index_start = get_current_step(time)
         mov_index_end = get_next_step(time)
 
         mov_start_x = normalize(movement_x[mov_index_start], slider_max, speed_x)
@@ -189,16 +287,17 @@ def naive_walk(t, speed_x, speed_y):
         mov_start_z = movement_z[mov_index_start]
         mov_end_z = movement_z[mov_index_end]
 
-        mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
+        mov_start_rotate = normalize(rotate(patte)[mov_index_start], 0.5, speed_rotation)
-        mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y
+        mov_end_rotate = normalize(rotate(patte)[mov_index_end], 0.5, speed_rotation)
+
+        mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y + mov_start_rotate
+        mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y + mov_end_rotate
 
         (real_position[patte][0],
          real_position[patte][1],
-         real_position[patte][2]) = interpol2(mov_start, mov_end,
+         real_position[patte][2]) = interpol2(mov_start, mov_end, get_current_step_advancement(time))
-                                              get_current_step_advancement(time, movement_duration, step_duration,
+        print(
-                                                                           mov_index_start))
+            f"[{patte}] [{get_current_step(time)}->{get_next_step(time)}], start: {mov_start}, end: {mov_end}, current ({real_position[patte][0]}, {real_position[patte][1]}, {real_position[patte][2]})")
-        legsLogger.debug(
-            f"[{patte}] [{mov_index_start}->{mov_index_end}], start: {mov_start}, end: {mov_end}, current ({real_position[patte][0]}, {real_position[patte][1]}, {real_position[patte][2]})")
 
     return legs(real_position)
 
@@ -212,10 +311,6 @@ def translate(tx, ty, tz):
     ])
 
 
-def normalize(matrix, slider_max, speed):
-    return (matrix / slider_max) * speed
-
-
 def Rx(alpha):
     return np.array([
         [1.0, 0.0, 0.0, 0.0],
@@ -243,39 +338,38 @@ def Rz(alpha):
     ])
 
 
-def walk(t, sx, sy, sr):
+# multiplication de matrices: @
-    xboxdata = xbox.get_data()
+# gauche: monde, droite: repere
-    max_slider = 0.200
-    controllerLogger.debug(xboxdata)
-    if xbox.mode == 0:
-        return static()
-    elif xbox.mode == 1:
-        return jump(xboxdata["y2"])
-    elif xbox.mode == 2:
-        return dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], max_slider * xboxdata["y2"],
-                          max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
-    elif xbox.mode == 3:
-        return dev(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], max_slider * xboxdata["y2"],
-                   max_slider * xboxdata["x2"])
-    else:
-        return naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
 
-
+def walkV2(t, speed_x, speed_y, speed_rotation):
-def static():
-    return legs(neutral_position)
-
-
-# Walk V2
-def dev(t, x1, y1, x2, y2):
     def get_rotation_center(speed_x, speed_y, theta_point):
         direction = np.array([-speed_y, speed_x])
         return direction / max(0.001, theta_point)
 
-    x0, y0 = get_rotation_center(x1, y1, x2)
+    x0, y0 = get_rotation_center(speed_x, speed_y, speed_rotation)
 
     print(x0, y0)
 
-    num_patte = 6
+    """
+    python simulator.py -m walk
+
+    Le but est d'intégrer tout ce que nous avons vu ici pour faire marcher le robot
+
+    - Sliders: speed_x, speed_y, speed_rotation, la vitesse cible du robot
+    - Entrée: t, le temps (secondes écoulées depuis le début)
+            speed_x, speed_y, et speed_rotation, vitesses cibles contrôlées par les sliders
+    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
+    """
+    # t = t*speed_x * 20
+    num_patte = 4
+    slider_max = 0.200
+
+    neutral_position = np.array([
+        [-0.06, 0.06, -0.13],
+        [-0.06, -0.06, -0.13],
+        [0.06, -0.06, -0.13],  # [0.15, 0.15, -0.01],
+        [0.06, 0.06, -0.13]
+    ])
 
     real_position = np.copy(neutral_position)
 
@@ -293,19 +387,31 @@ def dev(t, x1, y1, x2, y2):
     assert len(
         step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"
 
-    def get_next_step(t):
+    def get_current_step(t):
-        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
+        time_passed = 0
+        for i in range(len(step_duration)):
+            time_passed += step_duration[i]
+            if t % movement_duration < time_passed:
+                return i
 
-    offsets = np.array([0, 1 / 2, 2 / 3, 0, 1 / 2, 2 / 3]) * movement_duration  # offset between each leg
+    def get_current_step_advancement(t):
+        current_step = get_current_step(t)
+        t = t % movement_duration
+        for i in range(0, current_step):
+            t -= step_duration[i]
+        return t / step_duration[current_step]
+
+    def get_next_step(t):
+        return floor((get_current_step(t) + 1) % step_count)
+
+    offsets = np.array([0, 0.5, 0, 0.5]) * movement_duration  # offset between each leg
     assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
 
     for patte in range(num_patte):
         time = t + offsets[patte]
-        mov_index_start = get_current_step(time, step_duration, movement_duration)
+        mov_index_start = get_current_step(time)
         mov_index_end = get_next_step(time)
 
-        step_adv = get_current_step_advancement(t, movement_duration, step_duration, mov_index_start)
-
         mov_start_z = movement_z[mov_index_start]
         mov_end_z = movement_z[mov_index_end]
 
@@ -314,11 +420,11 @@ def dev(t, x1, y1, x2, y2):
 
         (real_position[patte][0],
          real_position[patte][1],
-         real_position[patte][2]) = interpol2(mov_start, mov_end, step_adv)
+         real_position[patte][2]) = interpol2(mov_start, mov_end, get_current_step_advancement(time))
 
-        dthteta = x2 * 2
+        dthteta = speed_rotation * 2
 
-        theta = dthteta * (step_adv - 0.5)
+        theta = dthteta * (get_current_step_advancement(time) - 0.5)
         # theta = 0
         print(theta)
         # rotating the vector
@@ -334,95 +440,13 @@ def dev(t, x1, y1, x2, y2):
             real_position[patte][0] = x2
             real_position[patte][1] = y2
 
+        # print(
+        #    f"[{patte}] [{get_current_step(time)}->{get_next_step(time)}], start: {mov_start}, end: {mov_end}, current ({real_position[patte][0]}, {real_position[patte][1]}, {real_position[patte][2]})")
+
     return legs(real_position)
 
 
-def jump(sy):
+walk = walkV1
-    offset = np.array([
-        [0, 0, -0.15],
-        [0, 0, -0.15],
-        [0, 0, -0.15],
-        [0, 0, -0.15],
-        [0, 0, -0.15],
-        [0, 0, -0.15]
-    ])
-    offset *= sy
-    return legs(neutral_position + offset)
-
-
-# based on walk V1
-def dino_naive(t, speed_x, speed_y, hz, hy, hx):
-    slider_max = 0.200
-
-    real_position = np.copy(neutral_position)
-
-    movement_x = np.array([
-        [0.00, 0, 0],
-        [0.04, 0, 0],
-        [-0.04, 0, 0],
-    ])
-
-    movement_y = np.array([
-        [0.0, 0, 0],
-        [0, 0.04, 0],
-        [0, -0.04, 0],
-    ])
-
-    movement_z = np.array([
-        [0, 0, 0.08],
-        [0, 0, -0.02],
-        [0, 0, -0.02]
-    ])
-
-    # duration of each step of the movement
-    step_duration = np.array([0.05, 0.3, 0.05])
-
-    step_count = len(movement_z)
-    movement_duration = np.sum(step_duration)
-
-    assert len(
-        step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"
-
-    def get_next_step(t):
-        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
-
-    offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
-    assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"
-
-    for patte in range(num_patte):
-        if patte in [2, 5]:
-            continue
-        time = t + offsets[patte]
-        mov_index_start = get_current_step(time, step_duration, movement_duration)
-        mov_index_end = get_next_step(time)
-
-        mov_start_x = normalize(movement_x[mov_index_start], slider_max, speed_x)
-        mov_end_x = normalize(movement_x[mov_index_end], slider_max, speed_x)
-
-        mov_start_y = normalize(movement_y[mov_index_start], slider_max, speed_y)
-        mov_end_y = normalize(movement_y[mov_index_end], slider_max, speed_y)
-
-        mov_start_z = movement_z[mov_index_start]
-        mov_end_z = movement_z[mov_index_end]
-
-        mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
-        mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y
-
-        (real_position[patte][0],
-         real_position[patte][1],
-         real_position[patte][2]) = interpol2(mov_start, mov_end,
-                                              get_current_step_advancement(time, movement_duration, step_duration,
-                                                                           mov_index_start))
-
-    real_position[2][2] = -0.1
-    real_position[2][0] = -0.28
-
-    real_position[5][2] = 0.05 + hz
-    real_position[5][0] = 0.25 + hx
-    real_position[5][1] = hy
-    print(hx)
-    return legs(real_position)
-
 
 if __name__ == "__main__":
     print("N'exécutez pas ce fichier, mais simulator.py")

logs.py

@@ -1,47 +0,0 @@
-import logging
-
-
-class ColorFormatter(logging.Formatter):
-    """Custom formatter with colored output based on log level"""
-
-    # ANSI color codes
-    GREY = "\x1b[38;21m"
-    GREEN = "\x1b[92m"
-    YELLOW = "\x1b[93m"
-    RED = "\x1b[91m"
-    BOLD_RED = "\x1b[31;1m"
-    RESET = "\x1b[0m"
-
-    # Format including function name
-    FORMAT = "%(asctime)s | %(funcName)s | %(message)s"
-
-    FORMATS = {
-        logging.DEBUG: GREY + FORMAT + RESET,
-        logging.INFO: GREEN + FORMAT + RESET,
-        logging.WARNING: YELLOW + FORMAT + RESET,
-        logging.ERROR: RED + FORMAT + RESET,
-        logging.CRITICAL: BOLD_RED + FORMAT + RESET
-    }
-
-    def format(self, record):
-        log_format = self.FORMATS.get(record.levelno)
-        formatter = logging.Formatter(log_format)
-        return formatter.format(record)
-
-
-def setup_logger(name, level=logging.INFO):
-    """Set up a logger with the custom color formatter"""
-    logger = logging.getLogger(name)
-    logger.setLevel(level)
-
-    # Create console handler
-    console_handler = logging.StreamHandler()
-    console_handler.setLevel(level)
-
-    # Add formatter to console handler
-    console_handler.setFormatter(ColorFormatter())
-
-    # Add console handler to logger
-    logger.addHandler(console_handler)
-
-    return logger

xbox.py

@@ -1,57 +0,0 @@
-import pygame
-
-
-class Xbox:
-    def __init__(self, controllerLogger):
-        self.logger = controllerLogger
-        self.mode_count = 2
-        self.mode = 0
-        pygame.init()
-        self.controllers = []
-        for i in range(0, pygame.joystick.get_count()):
-            self.controllers.append(pygame.joystick.Joystick(i))
-            self.controllers[-1].init()
-            self.logger.info(f"Detected controller {self.controllers[-1].get_name()}")
-
-        if len(self.controllers) == 0:
-            self.logger.critical("No controllers detected. Can't initialize remote control.")
-            self.initialized = True
-        else:
-            self.initialized = False
-        self.data = {"x1": 0, "x2": 0, "y1": 0, "y2": 0, "up": 0, "down": 0, "left": 0, "right": 0, "r1": 0, "r2": 0,
-                     "r3": 0,
-                     "l1": 0, "l2": 0, "l3": 0}
-
-    def get_data(self):
-        for event in pygame.event.get():
-            event = dict(event.dict)
-            keys = event.keys()
-            try:
-                if "axis" in keys:
-                    axis = event["axis"]
-                    if axis in [1, 4]:
-                        value = -event["value"]
-                        if abs(value) < 0.1:
-                            value = 0
-                        self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
-                    else:
-                        value = event["value"]
-                        if abs(value) < 0.1:
-                            value = 0
-                        self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
-                elif "button" in keys:
-                    pass
-                elif "joy" in keys:  # To manage arrows
-                    data = event["value"][0]
-                    if data != 0:
-                        self.mode += data
-                        self.mode %= self.mode_count
-                        self.logger.info(f"Switched mode ({data}). New mode: {self.mode}")
-                else:
-                    print(event)
-
-            except Exception as e:
-                print(event)
-                print(e)
-
-        return self.data