From 55fa408d620e112575b8a461e0301dc426b48efc Mon Sep 17 00:00:00 2001 From: Pierre Tellier Date: Mon, 31 Mar 2025 19:25:20 +0200 Subject: [PATCH] feat: exapod now stands --- control.py | 72 ++++++++++++++++++++++++++---------------------------- 1 file changed, 35 insertions(+), 37 deletions(-) diff --git a/control.py b/control.py index 28d5df0..91c7938 100644 --- a/control.py +++ b/control.py @@ -46,36 +46,28 @@ def direct(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 + return 0, 0, 0 from math import * l1h = 0.049 l1v = 0.032 -l1 = sqrt(l1h ** 2 + l1v ** 2) +l1 = l1h l2h = 0.0605 l2v = 0.02215 l2 = sqrt(l2h ** 2 + l2v ** 2) -l3h = 0.013 +l3h = 0.012 l3v = 0.093 l3 = sqrt(l3h ** 2 + l3v ** 2) tete_x = 0.095 -patte_x = 0.032 -patte_y = 0.079 + +patte_y = 0.032 +patte_x = 0.079 def inverse(x, y, z): @@ -92,12 +84,12 @@ def inverse(x, y, z): - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians) """ # Dimensions (m) - + z += l1v theta0 = atan2(y, x) - l = sqrt((x - l1h*cos(theta0)) ** 2 + (y - l1h*sin(theta0)) ** 2 + (z + l1v) ** 2) - + 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) @@ -114,7 +106,10 @@ def inverse(x, y, z): theta1 = acos(param1) + asin(z / l) - return [-theta0, theta1, theta2] + # 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): @@ -168,20 +163,21 @@ def legs(targets_robot): targets = [0] * 18 - cos_val = [1, 1, 0, -1, -1, 0] - sin_val = [0, 0, 1, 0, 0, -1] + 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] + 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_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 += patte_x * offset_x[i] - target_y += patte_y * offset_y[i] + target_x += offset_x[i] + target_y += offset_y[i] alpha, beta, gamma = inverse(target_x, target_y, target_z) targets[3 * i] = alpha @@ -213,30 +209,32 @@ def walkV1(t, speed_x, speed_y, speed_rotation): 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] + [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.06, 0, 0], - [-0.06, 0, 0], + [0.00, 0, 0], + [-0.00, 0, 0], ]) movement_y = np.array([ [0.0, 0, 0], - [0, 0.06, 0], - [0, -0.06, 0], + [0, 0.00, 0], + [0, -0.00, 0], ]) movement_z = np.array([ - [0, 0, 0.02], - [0, 0, -0.01], - [0, 0, -0.01] + [0, 0, 0], + [0, 0, 0], + [0, 0, 0] ]) step_duration = np.array([0.05, 0.3, 0.05])