feat: addded 🦖
This commit is contained in:
Pierre Tellier
parent
8479dec345
commit
90d08671d9
254
control.py
254
control.py
@ -40,6 +40,16 @@ 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):
|
||||
@ -48,6 +58,22 @@ def interpol2(point2, point1, t):
|
||||
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):
|
||||
"""
|
||||
"""
|
||||
@ -111,24 +137,9 @@ def legs(targets_robot):
|
||||
return targets
|
||||
|
||||
|
||||
def walkV1(t, speed_x, speed_y, speed_rotation):
|
||||
max_slider = 0.200
|
||||
xboxdata = xbox.get_data()
|
||||
print(xboxdata)
|
||||
speed_x = max_slider * xboxdata["x1"]
|
||||
speed_y = max_slider * xboxdata["y1"]
|
||||
|
||||
def naive_walk(t, speed_x, speed_y):
|
||||
slider_max = 0.200
|
||||
|
||||
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]
|
||||
])
|
||||
|
||||
real_position = np.copy(neutral_position)
|
||||
|
||||
movement_x = np.array([
|
||||
@ -158,36 +169,15 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
|
||||
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, 0, -1, 1, 0][
|
||||
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
|
||||
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)
|
||||
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)
|
||||
@ -199,17 +189,16 @@ def walkV1(t, speed_x, speed_y, speed_rotation):
|
||||
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
|
||||
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))
|
||||
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]})")
|
||||
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)
|
||||
|
||||
@ -223,6 +212,10 @@ def translate(tx, ty, tz):
|
||||
])
|
||||
|
||||
|
||||
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],
|
||||
@ -250,38 +243,39 @@ def Rz(alpha):
|
||||
])
|
||||
|
||||
|
||||
# multiplication de matrices: @
|
||||
# gauche: monde, droite: repere
|
||||
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 walkV2(t, speed_x, speed_y, speed_rotation):
|
||||
|
||||
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(speed_x, speed_y, speed_rotation)
|
||||
x0, y0 = get_rotation_center(x1, y1, x2)
|
||||
|
||||
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]
|
||||
])
|
||||
num_patte = 6
|
||||
|
||||
real_position = np.copy(neutral_position)
|
||||
|
||||
@ -299,31 +293,19 @@ def walkV2(t, speed_x, speed_y, speed_rotation):
|
||||
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)
|
||||
return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)
|
||||
|
||||
offsets = np.array([0, 0.5, 0, 0.5]) * movement_duration # offset between each leg
|
||||
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)
|
||||
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]
|
||||
|
||||
@ -332,11 +314,11 @@ def walkV2(t, speed_x, speed_y, speed_rotation):
|
||||
|
||||
(real_position[patte][0],
|
||||
real_position[patte][1],
|
||||
real_position[patte][2]) = interpol2(mov_start, mov_end, get_current_step_advancement(time))
|
||||
real_position[patte][2]) = interpol2(mov_start, mov_end, step_adv)
|
||||
|
||||
dthteta = speed_rotation * 2
|
||||
dthteta = x2 * 2
|
||||
|
||||
theta = dthteta * (get_current_step_advancement(time) - 0.5)
|
||||
theta = dthteta * (step_adv - 0.5)
|
||||
# theta = 0
|
||||
print(theta)
|
||||
# rotating the vector
|
||||
@ -352,13 +334,95 @@ def walkV2(t, speed_x, speed_y, speed_rotation):
|
||||
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
|
||||
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")
|
||||
|
||||
15
xbox.py
15
xbox.py
@ -3,16 +3,18 @@ import pygame
|
||||
|
||||
class Xbox:
|
||||
def __init__(self, controllerLogger):
|
||||
|
||||
self.logger = controllerLogger
|
||||
self.mode_count = 2
|
||||
self.mode = 0
|
||||
pygame.init()
|
||||
self.controllers = []
|
||||
for i in range(0, pygame.joystick.get_count()):
|
||||
self.controllers.append(pygame.joystick.Joystick(i))
|
||||
self.controllers[-1].init()
|
||||
controllerLogger.info(f"Detected controller {self.controllers[-1].get_name()}")
|
||||
self.logger.info(f"Detected controller {self.controllers[-1].get_name()}")
|
||||
|
||||
if len(self.controllers) == 0:
|
||||
controllerLogger.critical("No controllers detected. Can't initialize remote control.")
|
||||
self.logger.critical("No controllers detected. Can't initialize remote control.")
|
||||
self.initialized = True
|
||||
else:
|
||||
self.initialized = False
|
||||
@ -38,8 +40,13 @@ class Xbox:
|
||||
value = 0
|
||||
self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
|
||||
elif "button" in keys:
|
||||
# TODO
|
||||
pass
|
||||
elif "joy" in keys: # To manage arrows
|
||||
data = event["value"][0]
|
||||
if data != 0:
|
||||
self.mode += data
|
||||
self.mode %= self.mode_count
|
||||
self.logger.info(f"Switched mode ({data}). New mode: {self.mode}")
|
||||
else:
|
||||
print(event)
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user