feat: added comments

.gitignore

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

control.py

@@ -1,87 +1,118 @@
-import math
+import logging
+
 import numpy as np
-
-
-def sandbox(t):
-    """
-    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 *
+from logs import *
+from dynamixel_sdk import *
+from xbox import *
+import time
+
+# DEBUG
+LEGS_LOG_LEVEL = logging.WARNING
+CONTROLLER_LOG_LEVEL = logging.CRITICAL
+# Variables configurations
 
 l1h = 0.049
 l1v = 0.032
-l1 = l1h
+l1 = l1h  # this is not the real distance as it's not the one needed to calculate position.
 
+# 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 = 5
+
+neutral_position = np.array([
+    [0.08, 0.16, -0.12],
+    [-0.08, 0.16, -0.12],
+    [-0.2, -0.00, -0.12],
+    [-0.08, -0.16, -0.12],
+    [0.08, -0.16, -0.12],
+    [0.2, 0, -0.12]
+])
+
+REAL = 0
+
+# Robot setup
+if REAL == 1:
+    try:
+        portHandler = PortHandler("/dev/ttyUSB0")
+    except:
+        portHandler = PortHandler("/dev/ttyUSB1")
+
+    packetHandler = PacketHandler(1.0)
+
+    portHandler.openPort()
+    portHandler.setBaudRate(1000000)
+    ADDR_GOAL_POSITION = 30
+
+    ids = []
+    for id in range(0, 100):
+        model_number, result, error = packetHandler.ping(portHandler, id)
+        if model_number == 12:
+            print(f"Found AX-12 with id: {id}")
+            ids.append(id)
+        time.sleep(0.01)
+    if len(ids) != 18:
+        legsLogger.error("Not all motor found. Press enter to continue")
+        input()
+
+
+def interpol2(point2, point1, t):
+    """
+    Linear interpolation between two points in space
+    """
+    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):
+    """
+    helper function used to calculate the current step of the movement i.e. what part of the movement.
+    """
+    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):
+    """
+    helper function used to calculate the current advancement of the movement step i.e. the percentage of progression of said movement.
+    """
+    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
+    Calculate the angle of each motor to have the end of the feet at a specified position
-
-    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
@@ -89,76 +120,29 @@ def inverse(x, y, z):
     theta0 = atan2(y, x)
 
     l = sqrt((sqrt(x ** 2 + y ** 2) - l1) ** 2 + z ** 2)
-    # l = sqrt((x - l1h*cos(theta0)) ** 2 + (y - l1h*sin(theta0)) ** 2 + (z + l1v) ** 2)
 
     param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)
 
     if param2 > 1 or param2 < -1:
-        print("\033[94m" + f"Tentative d'acces a une position impossible (param2) ({x}, {y}, {z})" + "\033[0m")
+        legsLogger.warning("fTentative d'acces a une position impossible (param2) ({x}, {y}, {z})")
         param2 = 1 if param2 > 1 else -1
 
     theta2 = acos(param2)
 
     param1 = (-l3 ** 2 + l2 ** 2 + l ** 2) / (2 * l2 * l)
     if param1 > 1 or param1 < -1:
-        print("\033[94m" + f"Tentative d'acces a une position impossible (param1) ({x}, {y}, {z})" + "\033[0m")
+        legsLogger.warning(f"Tentative d'acces a une position impossible (param1) ({x}, {y}, {z})")
         param1 = 1 if param1 > 1 else -1
 
     theta1 = acos(param1) + asin(z / l)
 
-    # return [-theta0, theta1, theta2]
     angle1 = atan(l2v / l2h)
     return [-theta0, theta1 + angle1, theta2 + angle1 - pi / 2 + atan(l3h / l3v)]
-    # 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):
     """
-    python simulator.py -m legs
+    takes a list of target and offsets them to be in the legs referential
-
-    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
@@ -187,36 +171,74 @@ def legs(targets_robot):
     return targets
 
 
-def interpol2(point2, point1, t):
+def normalize(matrix, slider_max, speed):
-    x1, y1, z1 = point1
+    return (matrix / slider_max) * speed
-    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):
+counter = 0
+
+
+def convert_to_robot(positions):
     """
-    python simulator.py -m walk
+    Send a robot position to the real robot.
-
+    """
-    Le but est d'intégrer tout ce que nous avons vu ici pour faire marcher le robot
+    global counter
-
+    counter += 1
-    - Sliders: speed_x, speed_y, speed_rotation, la vitesse cible du robot
+    if counter % 2 != 0:
-    - Entrée: t, le temps (secondes écoulées depuis le début)
+        return
-            speed_x, speed_y, et speed_rotation, vitesses cibles contrôlées par les sliders
+    index = [
-    - Sortie: un tableau contenant les 12 positions angulaires cibles (radian) pour les moteurs
+        11, 12, 13,
+        41, 42, 43,
+        21, 22, 23,
+        51, 52, 53,
+        61, 62, 63,
+        31, 32, 33]
+    dir = [-1] * 18
+
+    for i in range(len(positions)):
+        value = int(512 + positions[i] * 150 * dir[i])
+        packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
+
+
+def walk(t, sx, sy, sr):
+    """
+    Choose the algorithm depending on input of xbox controller
+    """
+    xboxdata = xbox.get_data()
+    if xbox.initialized:
+        max_slider = 0.200
+        controllerLogger.debug(xboxdata)
+        if xbox.mode == 0:
+            positions = static()
+        elif xbox.mode == 1:
+            positions = jump(xboxdata["y2"])
+        elif xbox.mode == 2:
+            positions = 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:
+            positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
+        elif xbox.mode == 4:
+            positions = walk_v2(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"],
+                                xboxdata["x2"] * max_slider,
+                                max_slider * xboxdata["y2"])
+        # print(positions)
+        if REAL == 1:
+            convert_to_robot(positions)
+
+        return positions
+    else:
+        return walk_v2(t, sx, sy, sr, None)
+
+
+# different move algorithm
+
+def naive_walk(t, speed_x, speed_y):
+    """
+    First version of walk, using triangle for each step
     """
-    # 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([
@@ -246,36 +268,15 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
     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) + 1) % step_count)
+        return floor((get_current_step(t, step_duration, movement_duration) + 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)
+        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)
@@ -287,99 +288,58 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
         mov_start_z = movement_z[mov_index_start]
         mov_end_z = movement_z[mov_index_end]
 
-        mov_start_rotate = normalize(rotate(patte)[mov_index_start], 0.5, speed_rotation)
+        mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
-        mov_end_rotate = normalize(rotate(patte)[mov_index_end], 0.5, speed_rotation)
+        mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y
-
-        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, get_current_step_advancement(time))
+         real_position[patte][2]) = interpol2(mov_start, mov_end,
-        print(
+                                              get_current_step_advancement(time, movement_duration, step_duration,
-            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]})")
+                                                                           mov_index_start))
+        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)
 
 
-def translate(tx, ty, tz):
+def static():
-    return np.array([
+    """
-        [1.0, 0.0, 0.0, tx],
+    Function to have the robot stand at the neutral position
-        [0.0, 1.0, 0.0, ty],
+    """
-        [0.0, 0.0, 1.0, tz],
+    return legs(neutral_position)
-        [0.0, 0.0, 0.0, 1.0],
-    ])
 
 
-def Rx(alpha):
+# Walk V2
-    return np.array([
+def walk_v2(t, speed_x, speed_y, speed_r, y2):
-        [1.0, 0.0, 0.0, 0.0],
+    """
-        [0.0, np.cos(alpha), -np.sin(alpha), 0.0],
+    Second version of the walking algorithm, using circle for each leg
-        [0.0, np.sin(alpha), np.cos(alpha), 0.0],
+    This is the main algorithm.
-        [0.0, 0.0, 0.0, 1.0],
+    """
-    ])
+    max_dist = 0.3
 
-
-def Ry(alpha):
-    return np.array([
-        [np.cos(alpha), 0.0, -np.sin(alpha), 0.0],
-        [0.0, 1.0, 0.0, 0.0],
-        [np.sin(alpha), 0.0, np.cos(alpha), 0.0],
-        [0.0, 0.0, 0.0, 1.0],
-    ])
-
-
-def Rz(alpha):
-    return np.array([
-        [np.cos(alpha), -np.sin(alpha), 0.0, 0.0],
-        [np.sin(alpha), np.cos(alpha), 0.0, 0.0],
-        [0.0, 0.0, 1.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0],
-    ])
-
-
-# multiplication de matrices: @
-# gauche: monde, droite: repere
-
-def walkV2(t, speed_x, speed_y, speed_rotation):
     def get_rotation_center(speed_x, speed_y, theta_point):
-        direction = np.array([-speed_y, speed_x])
+        """
-        return direction / max(0.001, theta_point)
+        Helper function that calculate the center of rotation of the robot based on the current parameters
+        """
+        norm = sqrt(speed_x ** 2 + speed_y ** 2)
 
-    x0, y0 = get_rotation_center(speed_x, speed_y, speed_rotation)
+        r = 1000 if theta_point == 0 else norm / theta_point
-
+        if norm != 0:
-    print(x0, y0)
+            return np.array([speed_y, -speed_x]) / norm * r
-
+        else:
-    """
+            return 0, 0
-    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]
-    ])
 
+    center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r)
+    legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
     real_position = np.copy(neutral_position)
 
     movement_z = np.array([
-        [0, 0, 0.02],
+        [0, 0, 0.03],
-        [0, 0, -0.01],
+        [0, 0, -0.02],
-        [0, 0, -0.01]
+        [0, 0, -0.02]
     ])
 
-    step_duration = np.array([0.05, 0.3, 0.05])
+    step_duration = np.array([0.15, 0.3, 0.15])
 
     step_count = len(movement_z)
     movement_duration = np.sum(step_duration)
@@ -387,31 +347,26 @@ def walkV2(t, speed_x, speed_y, speed_rotation):
     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) + 1) % step_count)
+        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
 
-    offsets = np.array([0, 0.5, 0, 0.5]) * movement_duration  # offset between each leg
+    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"
 
+    # used to cap the speed to a max
+    max_radius = 0
+    for patte in range(num_patte):
+        x1, y1 = real_position[patte][0], real_position[patte][1]
+        circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
+        max_radius = max(max_radius, circle_radius)
+
     for patte in range(num_patte):
         time = t + offsets[patte]
-        mov_index_start = get_current_step(time)
+        mov_index_start = get_current_step(time, step_duration, movement_duration)
         mov_index_end = get_next_step(time)
 
+        step_adv = get_current_step_advancement(time, movement_duration, step_duration, mov_index_start)
+
         mov_start_z = movement_z[mov_index_start]
         mov_end_z = movement_z[mov_index_end]
 
@@ -420,33 +375,137 @@ def walkV2(t, speed_x, speed_y, speed_rotation):
 
         (real_position[patte][0],
          real_position[patte][1],
-         real_position[patte][2]) = interpol2(mov_start, mov_end, get_current_step_advancement(time))
+         real_position[patte][2]) = interpol2(mov_start, mov_end, step_adv)
 
-        dthteta = speed_rotation * 2
-
-        theta = dthteta * (get_current_step_advancement(time) - 0.5)
-        # theta = 0
-        print(theta)
-        # rotating the vector
         x1, y1 = real_position[patte][0], real_position[patte][1]
-        print(f"x1: {x1}, y1: {y1}")
-        x1t, y1t = x1 - x0, y1 - y0
-        x2t, y2t = x1t * cos(theta) + y1t * sin(theta), x1t * sin(theta) + y1t * cos(theta)
-        x2, y2 = x2t + x0, y2t + y0
-        print(f"x2: {x2}, y2: {y2}")
-        if mov_index_start == 1:
-            # theta += time
-            # xp = d * cos(theta) + real_position[patte][0]
-            real_position[patte][0] = x2
-            real_position[patte][1] = y2
 
-        # print(
+        circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
-        #    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]})")
+        cur_theta = acos((x1 - center_x) / circle_radius)
+
+        if y1 < center_y:
+            cur_theta *= -1
+        if speed_r != 0:
+            speed = sqrt(speed_x ** 2 + speed_y ** 2) * (speed_r / abs(speed_r))
+        else:
+            speed = sqrt(speed_x ** 2 + speed_y ** 2)
+
+        if speed == 0:
+            speed = speed_r
+        dthteta = speed * (1 / max_radius * max_dist)
+        legsLogger.info(f"speed: {speed}, dthteta: {dthteta}")
+        if mov_index_start == 0:
+            # midair part 1
+            theta_offset = dthteta * (- step_adv)
+        elif mov_index_start == 1:
+            # moving part
+            theta_offset = dthteta * (step_adv - 0.5) * 2
+        else:
+            # midair part 1
+            theta_offset = dthteta * (1 - step_adv)
+        wanted_theta = cur_theta + theta_offset
+
+        wanted_x = center_x + circle_radius * cos(wanted_theta)
+        wanted_y = center_y + circle_radius * sin(wanted_theta)
+
+        real_position[patte][0] = wanted_x
+        real_position[patte][1] = wanted_y
+
+    if REAL:
+        return legs(real_position)
+    return legs(real_position), [center_x, center_y]
+
+
+def jump(sy):
+    """
+    Make the robot jump (at least we wished)
+    """
+    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)
+
+
+def dino_naive(t, speed_x, speed_y, hz, hy, hx):
+    """
+    based on walk naive, but with a head and a tail
+    """
+    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
 
     return legs(real_position)
 
 
-walk = walkV1
-
 if __name__ == "__main__":
     print("N'exécutez pas ce fichier, mais simulator.py")

logs.py

@@ -0,0 +1,47 @@
+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

simulator.py

@@ -3,6 +3,7 @@ import argparse
 import math
 import time
 import pybullet as p
+
 if __package__ is None or __package__ == '':
     import control
 else:
@@ -14,6 +15,7 @@ dirname = os.path.dirname(__file__) + '/models/'
 n_motors = 0
 jointsMap = []
 
+
 def init():
     """Initialise le simulateur"""
     # Instanciation de Bullet
@@ -21,7 +23,7 @@ def init():
     p.setGravity(0, 0, -10)
 
     # Chargement du sol
-    planeId = p.loadURDF(dirname+'/plane.urdf')
+    planeId = p.loadURDF(dirname + '/plane.urdf')
 
     p.setPhysicsEngineParameter(fixedTimeStep=dt)
 
@@ -29,7 +31,7 @@ def init():
 def loadModel(name, fixed=False, startPos=[0., 0., 0.1], startOrientation=[0., 0., 0.]):
     """Charge un modèle"""
     startOrientation = p.getQuaternionFromEuler(startOrientation)
-    model = p.loadURDF(dirname+"/"+name+"/robot.urdf",
+    model = p.loadURDF(dirname + "/" + name + "/robot.urdf",
                        startPos, startOrientation, useFixedBase=fixed)
 
     return model
@@ -43,7 +45,7 @@ def setJoints(robot, joints):
         joints {list} -- liste des positions cibles (rad)
     """
     global jointsMap
-    
+
     for k in range(len(joints)):
         jointInfo = p.getJointInfo(robot, jointsMap[k])
         p.setJointMotorControl2(
@@ -60,6 +62,7 @@ def inverseControls(name='', x_default=0.15, y_default=0.):
 
     return target_x, target_y, target_z, target
 
+
 def directControls():
     alpha = p.addUserDebugParameter('alpha', -math.pi, math.pi, 0)
     beta = p.addUserDebugParameter('beta', -math.pi, math.pi, 0)
@@ -78,6 +81,7 @@ def inverseUpdate(controls):
 
     return x, y, z
 
+
 def tick():
     global t
 
@@ -85,6 +89,7 @@ def tick():
     p.stepSimulation()
     time.sleep(dt)
 
+
 if __name__ == "__main__":
     # Arguments
     parser = argparse.ArgumentParser(prog="Quadruped")
@@ -104,11 +109,11 @@ if __name__ == "__main__":
 
     if robot == 'quadruped':
         oneLegStartPos = [-0.04, 0., floatHeight]
-        oneLegstartOrientation = [0., 0., math.pi + math.pi/4]
+        oneLegstartOrientation = [0., 0., math.pi + math.pi / 4]
         n_motors = 12
         jointsMap = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
     else:
-        oneLegstartOrientation = [0., 0., -math.pi/2]
+        oneLegstartOrientation = [0., 0., -math.pi / 2]
         oneLegStartPos = [-0.079, 0.032, floatHeight]
         n_motors = 18
         jointsMap = list(range(18))
@@ -149,8 +154,8 @@ if __name__ == "__main__":
                 inverseControls('leg4_', -0.05, -0.24),
                 inverseControls('leg5_', 0.05, -0.24),
                 inverseControls('leg6_', 0.24, 0.)
-            ]       
+            ]
-        
+
     elif mode == 'walk':
         speed_x = p.addUserDebugParameter('speed_x', -0.2, 0.2, 0.0)
         speed_y = p.addUserDebugParameter('speed_y', -0.2, 0.2, 0.0)
@@ -161,6 +166,8 @@ if __name__ == "__main__":
 
     robot = loadModel(robot, fixed, startPos, startOrientation)
 
+    target = loadModel('target2', True)
+
     # Boucle principale
     while True:
         if mode == 'motors':
@@ -168,23 +175,25 @@ if __name__ == "__main__":
             for entry in motors_sliders:
                 joints.append(p.readUserDebugParameter(entry))
         elif mode == 'inverse':
-            joints = control.inverse(*inverseUpdate(leg)) + [0]*(n_motors-3)
+            joints = control.inverse(*inverseUpdate(leg)) + [0] * (n_motors - 3)
         elif mode == 'direct':
             alpha_slider, beta_slider, gamma_slider, target = leg
             alpha = p.readUserDebugParameter(alpha_slider)
             beta = p.readUserDebugParameter(beta_slider)
             gamma = p.readUserDebugParameter(gamma_slider)
-            joints = [alpha, beta, gamma] + [0]*(n_motors-3)
+            joints = [alpha, beta, gamma] + [0] * (n_motors - 3)
             x, y, z = control.direct(alpha, beta, gamma)
             p.resetBasePositionAndOrientation(target, [x, y, z + floatHeight],
-                p.getQuaternionFromEuler([0, 0, 0]))
+                                              p.getQuaternionFromEuler([0, 0, 0]))
         elif mode == 'draw':
-            joints = control.draw(t) + [0]*(n_motors-3)
+            joints = control.draw(t) + [0] * (n_motors - 3)
+
 
             def getLegTip():
                 res = p.getLinkState(robot, 3)
                 return res[0]
 
+
             if lastLine is None:
                 lastLine = time.time(), getLegTip()
             elif time.time() - lastLine[0] > 0.1:
@@ -201,7 +210,16 @@ if __name__ == "__main__":
             x = p.readUserDebugParameter(speed_x)
             y = p.readUserDebugParameter(speed_y)
             rotation = p.readUserDebugParameter(speed_rotation)
-            joints = control.walk(t, x, y, rotation)
+            data = control.walk(t, x, y, rotation)
+            if len(data) == 2:  # When we receive the position of a target
+                joints = data[0]
+                x, y = data[1]
+                z = 0
+            else:
+                joints = data
+                x, y, z = 0, 0, -5  # We hide the target under the map
+            p.resetBasePositionAndOrientation(target, [x, y, z],
+                                              p.getQuaternionFromEuler([0, 0, 0]))
         elif mode == 'sandbox':
             joints = control.sandbox(t)
 

xbox.py

@@ -0,0 +1,57 @@
+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 = False
+        else:
+            self.initialized = True
+        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.2:
+                            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.2:
+                            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