diff --git a/control.py b/control.py index a580dac..cce115a 100644 --- a/control.py +++ b/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): - max_slider = 0.200 - xboxdata = xbox.get_data() - print(xboxdata) - speed_x = max_slider * xboxdata["x1"] - speed_y = max_slider * xboxdata["y1"] - +def naive_walk(t, speed_x, speed_y): 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) - - 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 + 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): 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_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 + 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)) - 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]})") + real_position[patte][2]) = interpol2(mov_start, mov_end, + get_current_step_advancement(time, movement_duration, step_duration, + 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: @ -# gauche: monde, droite: repere +def walk(t, sx, sy, sr): + 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) - """ - 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] - ]) + num_patte = 6 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") diff --git a/xbox.py b/xbox.py index dafcf48..4974704 100644 --- a/xbox.py +++ b/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)