import logging import math import numpy as np from math import * from logs import * from xbox import * import time # DEBUG LEGS_LOG_LEVEL = logging.INFO CONTROLLER_LOG_LEVEL = logging.INFO # Variables configurations l1h = 0.049 l1v = 0.032 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 = 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): 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): 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): """ """ # 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 legs(targets_robot): """ takes a list of target and offsets it to be in the legs referential """ 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 naive_walk(t, speed_x, speed_y): 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): 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)) 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): 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 normalize(matrix, slider_max, speed): return (matrix / slider_max) * speed 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], ]) 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 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(x1, y1, x2) print(x0, y0) num_patte = 6 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_next_step(t): return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count) 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, 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] 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, step_adv) dthteta = x2 * 2 theta = dthteta * (step_adv - 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 return legs(real_position) 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")