feat: addded 🦖

control.py

@@ -40,6 +40,16 @@ 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):
@@ -48,6 +58,22 @@ def interpol2(point2, point1, t):
     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):
     """
     """
@@ -111,24 +137,9 @@ def legs(targets_robot):
     return targets
 
 
-def walkV1(t, speed_x, speed_y, speed_rotation):
+def naive_walk(t, speed_x, speed_y):
-    max_slider = 0.200
-    xboxdata = xbox.get_data()
-    print(xboxdata)
-    speed_x = max_slider * xboxdata["x1"]
-    speed_y = max_slider * xboxdata["y1"]
-
     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([
@@ -158,36 +169,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)
@@ -199,17 +189,16 @@ 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)
 
@@ -223,6 +212,10 @@ 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],
@@ -250,38 +243,39 @@ def Rz(alpha):
     ])
 
 
-# multiplication de matrices: @
+def walk(t, sx, sy, sr):
-# gauche: monde, droite: repere
+    xboxdata = xbox.get_data()
+    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(speed_x, speed_y, speed_rotation)
+    x0, y0 = get_rotation_center(x1, y1, x2)
 
     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)
 
@@ -299,31 +293,19 @@ 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 / 2, 2 / 3, 0, 1 / 2, 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)
 
+        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]
 
@@ -332,11 +314,11 @@ 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
+        dthteta = x2 * 2
 
-        theta = dthteta * (get_current_step_advancement(time) - 0.5)
+        theta = dthteta * (step_adv - 0.5)
         # theta = 0
         print(theta)
         # rotating the vector
@@ -352,13 +334,95 @@ def walkV2(t, speed_x, speed_y, speed_rotation):
             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)
 
 
-walk = walkV1
+def jump(sy):
+    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")

xbox.py

@@ -3,16 +3,18 @@ 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()
-            controllerLogger.info(f"Detected controller {self.controllers[-1].get_name()}")
+            self.logger.info(f"Detected controller {self.controllers[-1].get_name()}")
 
         if len(self.controllers) == 0:
-            controllerLogger.critical("No controllers detected. Can't initialize remote control.")
+            self.logger.critical("No controllers detected. Can't initialize remote control.")
             self.initialized = True
         else:
             self.initialized = False
@@ -38,8 +40,13 @@ class Xbox:
                             value = 0
                         self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
                 elif "button" in keys:
-                    # TODO
                     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)