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) """ return 0, 0, 0 from math import * l1h = 0.049 l1v = 0.032 l1 = l1h l2h = 0.0605 l2v = 0.02215 l2 = sqrt(l2h ** 2 + l2v ** 2) l3h = 0.012 l3v = 0.093 l3 = sqrt(l3h ** 2 + l3v ** 2) tete_x = 0.095 patte_y = 0.032 patte_x = 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) z += l1v 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") 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] 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 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 = [0, 0, -1, 0, 0, 1] sin_val = [-1, -1, 0, 1, 1, 0] 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_tmp = cos_val[i] * target_x - sin_val[i] * target_y target_y = sin_val[i] * target_x + cos_val[i] * target_y target_x = target_x_tmp target_x += offset_x[i] target_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.1, 0.15, -0.16], [-0.1, 0.15, -0.16], [-0.2, -0.00, -0.16], [-0.1, -0.15, -0.16], [0.1, -0.15, -0.16], [0.2, 0, -0.16] ]) real_position = np.copy(neutral_position) movement_x = np.array([ [0.0, 0, 0], [0.00, 0, 0], [-0.00, 0, 0], ]) movement_y = np.array([ [0.0, 0, 0], [0, 0.00, 0], [0, -0.00, 0], ]) movement_z = np.array([ [0, 0, 0], [0, 0, 0], [0, 0, 0] ]) 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")