piair/vroom-vroom-vroom
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: exapod now stands

Browse Source
This commit is contained in:
Pierre Tellier 2025-03-31 19:25:20 +02:00
parent 4f6efed9a3
commit 55fa408d62
1 changed files with 35 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
control.py
View File

@ -46,36 +46,28 @@ def direct(alpha, beta, gamma):
- Entrées: alpha, beta, gamma, la cible (radians) des moteurs - 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) - 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""" return 0, 0, 0
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 * from math import *
l1h = 0.049 l1h = 0.049
l1v = 0.032 l1v = 0.032
l1 = sqrt(l1h ** 2 + l1v ** 2) l1 = l1h
l2h = 0.0605 l2h = 0.0605
l2v = 0.02215 l2v = 0.02215
l2 = sqrt(l2h ** 2 + l2v ** 2) l2 = sqrt(l2h ** 2 + l2v ** 2)
l3h = 0.013 l3h = 0.012
l3v = 0.093 l3v = 0.093
l3 = sqrt(l3h ** 2 + l3v ** 2) l3 = sqrt(l3h ** 2 + l3v ** 2)
tete_x = 0.095 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): 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) - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians)
""" """
# Dimensions (m) # Dimensions (m)
z += l1v
theta0 = atan2(y, x) 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) 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) 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): def draw(t):
@ -168,20 +163,21 @@ def legs(targets_robot):
targets = [0] * 18 targets = [0] * 18
cos_val = [1, 1, 0, -1, -1, 0] cos_val = [0, 0, -1, 0, 0, 1]
sin_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_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_y = [patte_y, -patte_y, 0, patte_y, -patte_y, 0]
for i in range(6): for i in range(6):
target_x, target_y, target_z = targets_robot[i] target_x, target_y, target_z = targets_robot[i]
target_x = cos_val[i] * target_x + sin_val[i] * target_x target_x_tmp = cos_val[i] * target_x - sin_val[i] * target_y
target_y = cos_val[i] * target_y + 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_x += offset_x[i]
target_y += patte_y * offset_y[i] target_y += offset_y[i]
alpha, beta, gamma = inverse(target_x, target_y, target_z) alpha, beta, gamma = inverse(target_x, target_y, target_z)
targets[3 * i] = alpha targets[3 * i] = alpha
@ -213,30 +209,32 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
slider_max = 0.200 slider_max = 0.200
neutral_position = np.array([ neutral_position = np.array([
[-0.06, 0.06, -0.13], [0.1, 0.15, -0.16],
[-0.06, -0.06, -0.13], [-0.1, 0.15, -0.16],
[0.06, -0.06, -0.13], # [0.15, 0.15, -0.01], [-0.2, -0.00, -0.16],
[0.06, 0.06, -0.13] [-0.1, -0.15, -0.16],
[0.1, -0.15, -0.16],
[0.2, 0, -0.16]
]) ])
real_position = np.copy(neutral_position) real_position = np.copy(neutral_position)
movement_x = np.array([ movement_x = np.array([
[0.0, 0, 0], [0.0, 0, 0],
[0.06, 0, 0], [0.00, 0, 0],
[-0.06, 0, 0], [-0.00, 0, 0],
]) ])
movement_y = np.array([ movement_y = np.array([
[0.0, 0, 0], [0.0, 0, 0],
[0, 0.06, 0], [0, 0.00, 0],
[0, -0.06, 0], [0, -0.00, 0],
]) ])
movement_z = np.array([ movement_z = np.array([
[0, 0, 0.02], [0, 0, 0],
[0, 0, -0.01], [0, 0, 0],
[0, 0, -0.01] [0, 0, 0]
]) ])
step_duration = np.array([0.05, 0.3, 0.05]) step_duration = np.array([0.05, 0.3, 0.05])