feat: added dimensions
This commit is contained in:
Pierre Tellier
453
control.py
Normal file
453
control.py
Normal file
@@ -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")
|
||||
Reference in New Issue
Block a user