diff --git a/control.py b/control.py new file mode 100644 index 0000000..28d5df0 --- /dev/null +++ b/control.py @@ -0,0 +1,453 @@ +import math +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) + """ + l1, l2, l3 = 45, 65, 87 + + """Prend en entrée les angles moteurs et produit la position atteinte""" + xp = l1 + math.cos(beta) * l2 + math.cos(beta + gamma) * l3 + yp = math.sin(beta) * l2 + math.sin(beta + gamma) * l3 + + x = math.cos(alpha) * xp + y = math.sin(alpha) * xp + z = yp + + return x, y, z + + +from math import * + +l1h = 0.049 +l1v = 0.032 +l1 = sqrt(l1h ** 2 + l1v ** 2) + +l2h = 0.0605 +l2v = 0.02215 +l2 = sqrt(l2h ** 2 + l2v ** 2) + +l3h = 0.013 +l3v = 0.093 +l3 = sqrt(l3h ** 2 + l3v ** 2) + +tete_x = 0.095 +patte_x = 0.032 +patte_y = 0.079 + + +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) + + + theta0 = atan2(y, x) + + 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") + 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") + param1 = 1 if param1 > 1 else -1 + + theta1 = acos(param1) + asin(z / l) + + return [-theta0, theta1, theta2] + + +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 + + 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 + + cos_val = [1, 1, 0, -1, -1, 0] + sin_val = [0, 0, 1, 0, 0, -1] + + offset_x = [patte_x, patte_x, tete_x, patte_x, patte_x, tete_x] + offset_y = [patte_y, patte_y, 0, patte_y, patte_y, 0] + + for i in range(6): + target_x, target_y, target_z = targets_robot[i] + + target_x = cos_val[i] * target_x + sin_val[i] * target_x + target_y = cos_val[i] * target_y + sin_val[i] * target_y + + target_x += patte_x * offset_x[i] + target_y += patte_y * offset_y[i] + + alpha, beta, gamma = inverse(target_x, target_y, target_z) + targets[3 * i] = alpha + targets[3 * i + 1] = beta + targets[3 * i + 2] = gamma + + return targets + + +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 = 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) + + movement_x = np.array([ + [0.0, 0, 0], + [0.06, 0, 0], + [-0.06, 0, 0], + ]) + + movement_y = np.array([ + [0.0, 0, 0], + [0, 0.06, 0], + [0, -0.06, 0], + ]) + + movement_z = np.array([ + [0, 0, 0.02], + [0, 0, -0.01], + [0, 0, -0.01] + ]) + + 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_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, -1, 1][ + 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, 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) + 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_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 + + (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]})") + + return legs(real_position) + + +def translate(tx, ty, tz): + return np.array([ + [1.0, 0.0, 0.0, tx], + [0.0, 1.0, 0.0, ty], + [0.0, 0.0, 1.0, tz], + [0.0, 0.0, 0.0, 1.0], + ]) + + +def Rx(alpha): + return np.array([ + [1.0, 0.0, 0.0, 0.0], + [0.0, 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], + ]) + + +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) + + x0, y0 = get_rotation_center(speed_x, speed_y, speed_rotation) + + 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] + ]) + + real_position = np.copy(neutral_position) + + movement_z = np.array([ + [0, 0, 0.02], + [0, 0, -0.01], + [0, 0, -0.01] + ]) + + 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_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) + + 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) + mov_index_end = get_next_step(time) + + 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_end = neutral_position[patte] + mov_end_z + + (real_position[patte][0], + real_position[patte][1], + real_position[patte][2]) = interpol2(mov_start, mov_end, get_current_step_advancement(time)) + + 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( + # 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 + +if __name__ == "__main__": + print("N'exécutez pas ce fichier, mais simulator.py")