feat: finised project
This commit is contained in:
Pierre Tellier
parent
90d08671d9
commit
6b0f2c2170
190
control.py
190
control.py
@ -1,14 +1,15 @@
|
||||
import logging
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
from math import *
|
||||
from logs import *
|
||||
from dynamixel_sdk import *
|
||||
from xbox import *
|
||||
import time
|
||||
|
||||
# DEBUG
|
||||
LEGS_LOG_LEVEL = logging.INFO
|
||||
CONTROLLER_LOG_LEVEL = logging.INFO
|
||||
LEGS_LOG_LEVEL = logging.DEBUG
|
||||
CONTROLLER_LOG_LEVEL = logging.CRITICAL
|
||||
# Variables configurations
|
||||
|
||||
l1h = 0.049
|
||||
@ -40,17 +41,39 @@ controllerLogger = setup_logger("Controller", CONTROLLER_LOG_LEVEL)
|
||||
# Initialize controller
|
||||
xbox = Xbox(controllerLogger)
|
||||
CONTROLLER_MODE = xbox.initialized
|
||||
xbox.mode_count = 4
|
||||
xbox.mode_count = 5
|
||||
|
||||
neutral_position = np.array([
|
||||
[0.1, 0.15, -0.15],
|
||||
[-0.1, 0.15, -0.15],
|
||||
[0.1, 0.16, -0.15],
|
||||
[-0.1, 0.16, -0.15],
|
||||
[-0.2, -0.00, -0.15],
|
||||
[-0.1, -0.15, -0.15],
|
||||
[0.1, -0.15, -0.15],
|
||||
[-0.1, -0.16, -0.15],
|
||||
[0.1, -0.16, -0.15],
|
||||
[0.2, 0, -0.15]
|
||||
])
|
||||
|
||||
REAL = 0
|
||||
|
||||
# Robot setup
|
||||
if REAL == 1:
|
||||
portHandler = PortHandler("/dev/ttyUSB0")
|
||||
packetHandler = PacketHandler(1.0)
|
||||
|
||||
portHandler.openPort()
|
||||
portHandler.setBaudRate(1000000)
|
||||
|
||||
ADDR_GOAL_POSITION = 30
|
||||
|
||||
ids = []
|
||||
for id in range(0, 100):
|
||||
model_number, result, error = packetHandler.ping(portHandler, id)
|
||||
if model_number == 12:
|
||||
print(f"Found AX-12 with id: {id}")
|
||||
ids.append(id)
|
||||
time.sleep(0.01)
|
||||
|
||||
|
||||
# Done
|
||||
|
||||
def interpol2(point2, point1, t):
|
||||
x1, y1, z1 = point1
|
||||
@ -88,14 +111,14 @@ def inverse(x, y, z):
|
||||
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")
|
||||
legsLogger.warning("fTentative d'acces a une position impossible (param2) ({x}, {y}, {z})")
|
||||
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")
|
||||
legsLogger.warning(f"Tentative d'acces a une position impossible (param1) ({x}, {y}, {z})")
|
||||
param1 = 1 if param1 > 1 else -1
|
||||
|
||||
theta1 = acos(param1) + asin(z / l)
|
||||
@ -203,46 +226,10 @@ def naive_walk(t, speed_x, speed_y):
|
||||
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
|
||||
@ -255,10 +242,10 @@ def walk(t, sx, sy, sr):
|
||||
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"])
|
||||
elif xbox.mode == 4:
|
||||
return dev(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], xboxdata["x2"] * max_slider,
|
||||
max_slider * xboxdata["y2"])
|
||||
|
||||
|
||||
def static():
|
||||
@ -266,16 +253,21 @@ def static():
|
||||
|
||||
|
||||
# Walk V2
|
||||
def dev(t, x1, y1, x2, y2):
|
||||
def dev(t, speed_x, speed_y, speed_r, y2):
|
||||
"""
|
||||
x1: speed along the x-axis
|
||||
y1: speed along the y-axis
|
||||
x2: rotational speed along the z-axis
|
||||
"""
|
||||
max_dist = 0.3
|
||||
|
||||
def get_rotation_center(speed_x, speed_y, theta_point):
|
||||
direction = np.array([-speed_y, speed_x])
|
||||
return direction / max(0.001, theta_point)
|
||||
return direction / max(0.00001, theta_point)
|
||||
|
||||
x0, y0 = get_rotation_center(x1, y1, x2)
|
||||
center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r)
|
||||
|
||||
print(x0, y0)
|
||||
|
||||
num_patte = 6
|
||||
legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
|
||||
|
||||
real_position = np.copy(neutral_position)
|
||||
|
||||
@ -285,7 +277,7 @@ def dev(t, x1, y1, x2, y2):
|
||||
[0, 0, -0.01]
|
||||
])
|
||||
|
||||
step_duration = np.array([0.05, 0.3, 0.05])
|
||||
step_duration = np.array([0.15, 0.3, 0.15])
|
||||
|
||||
step_count = len(movement_z)
|
||||
movement_duration = np.sum(step_duration)
|
||||
@ -296,15 +288,21 @@ def dev(t, x1, y1, x2, y2):
|
||||
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
|
||||
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"
|
||||
|
||||
max_radius = 0
|
||||
for patte in range(num_patte):
|
||||
x1, y1 = real_position[patte][0], real_position[patte][1]
|
||||
circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
|
||||
max_radius = max(max_radius, circle_radius)
|
||||
|
||||
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)
|
||||
step_adv = get_current_step_advancement(time, movement_duration, step_duration, mov_index_start)
|
||||
|
||||
mov_start_z = movement_z[mov_index_start]
|
||||
mov_end_z = movement_z[mov_index_end]
|
||||
@ -316,25 +314,37 @@ def dev(t, x1, y1, x2, y2):
|
||||
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)
|
||||
circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
|
||||
cur_theta = acos((x1 - center_x) / circle_radius)
|
||||
|
||||
if y1 < center_y:
|
||||
cur_theta *= -1
|
||||
|
||||
speed = abs(speed_x) + abs(speed_y)
|
||||
if speed == 0:
|
||||
speed = speed_r
|
||||
dthteta = speed * (1 / max_radius * max_dist)
|
||||
legsLogger.info(f"speed: {speed}, dthteta: {dthteta}")
|
||||
if mov_index_start == 0:
|
||||
# midair part 1
|
||||
theta_offset = dthteta * (- step_adv)
|
||||
elif mov_index_start == 1:
|
||||
# moving part
|
||||
theta_offset = dthteta * (step_adv - 0.5) * 2
|
||||
else:
|
||||
# midair part 1
|
||||
theta_offset = dthteta * (1 - step_adv)
|
||||
wanted_theta = cur_theta + theta_offset
|
||||
|
||||
wanted_x = center_x + circle_radius * cos(wanted_theta)
|
||||
wanted_y = center_y + circle_radius * sin(wanted_theta)
|
||||
|
||||
real_position[patte][0] = wanted_x
|
||||
real_position[patte][1] = wanted_y
|
||||
|
||||
return legs(real_position), [center_x, center_y]
|
||||
|
||||
|
||||
def jump(sy):
|
||||
@ -424,5 +434,35 @@ def dino_naive(t, speed_x, speed_y, hz, hy, hx):
|
||||
return legs(real_position)
|
||||
|
||||
|
||||
def robot_input():
|
||||
print("Send motors wave (press enter)")
|
||||
input()
|
||||
for id in ids:
|
||||
packetHandler.write2ByteTxRx(portHandler, id, ADDR_GOAL_POSITION, 512)
|
||||
input()
|
||||
angle = [512, 624, 400, 512, 624, 400]
|
||||
for id in range(1, 7):
|
||||
packetHandler.write2ByteTxRx(portHandler, id * 10 + 1, ADDR_GOAL_POSITION, angle[id - 1])
|
||||
input()
|
||||
|
||||
# Dino mode
|
||||
packetHandler.write2ByteTxRx(portHandler, 12, ADDR_GOAL_POSITION, 650)
|
||||
packetHandler.write2ByteTxRx(portHandler, 13, ADDR_GOAL_POSITION, 400)
|
||||
packetHandler.write2ByteTxRx(portHandler, 42, ADDR_GOAL_POSITION, 650)
|
||||
packetHandler.write2ByteTxRx(portHandler, 43, ADDR_GOAL_POSITION, 400)
|
||||
|
||||
input()
|
||||
for id in ids:
|
||||
packetHandler.write2ByteTxRx(portHandler, id, ADDR_GOAL_POSITION, 512)
|
||||
# t = 0.0
|
||||
# while True:
|
||||
# print("Send motors wave (press enter)")
|
||||
# angle = 512 + int(50 * np.sin(t))
|
||||
# print(angle)
|
||||
# packetHandler.write2ByteTxRx(portHandler, 53, ADDR_GOAL_POSITION, angle)
|
||||
# time.sleep(0.001)
|
||||
# t += 0.01
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print("N'exécutez pas ce fichier, mais simulator.py")
|
||||
|
||||
20
simulator.py
20
simulator.py
@ -3,6 +3,7 @@ import argparse
|
||||
import math
|
||||
import time
|
||||
import pybullet as p
|
||||
|
||||
if __package__ is None or __package__ == '':
|
||||
import control
|
||||
else:
|
||||
@ -14,6 +15,7 @@ dirname = os.path.dirname(__file__) + '/models/'
|
||||
n_motors = 0
|
||||
jointsMap = []
|
||||
|
||||
|
||||
def init():
|
||||
"""Initialise le simulateur"""
|
||||
# Instanciation de Bullet
|
||||
@ -60,6 +62,7 @@ def inverseControls(name='', x_default=0.15, y_default=0.):
|
||||
|
||||
return target_x, target_y, target_z, target
|
||||
|
||||
|
||||
def directControls():
|
||||
alpha = p.addUserDebugParameter('alpha', -math.pi, math.pi, 0)
|
||||
beta = p.addUserDebugParameter('beta', -math.pi, math.pi, 0)
|
||||
@ -78,6 +81,7 @@ def inverseUpdate(controls):
|
||||
|
||||
return x, y, z
|
||||
|
||||
|
||||
def tick():
|
||||
global t
|
||||
|
||||
@ -85,6 +89,7 @@ def tick():
|
||||
p.stepSimulation()
|
||||
time.sleep(dt)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# Arguments
|
||||
parser = argparse.ArgumentParser(prog="Quadruped")
|
||||
@ -161,6 +166,8 @@ if __name__ == "__main__":
|
||||
|
||||
robot = loadModel(robot, fixed, startPos, startOrientation)
|
||||
|
||||
target = loadModel('target2', True)
|
||||
|
||||
# Boucle principale
|
||||
while True:
|
||||
if mode == 'motors':
|
||||
@ -181,10 +188,12 @@ if __name__ == "__main__":
|
||||
elif mode == 'draw':
|
||||
joints = control.draw(t) + [0] * (n_motors - 3)
|
||||
|
||||
|
||||
def getLegTip():
|
||||
res = p.getLinkState(robot, 3)
|
||||
return res[0]
|
||||
|
||||
|
||||
if lastLine is None:
|
||||
lastLine = time.time(), getLegTip()
|
||||
elif time.time() - lastLine[0] > 0.1:
|
||||
@ -201,7 +210,16 @@ if __name__ == "__main__":
|
||||
x = p.readUserDebugParameter(speed_x)
|
||||
y = p.readUserDebugParameter(speed_y)
|
||||
rotation = p.readUserDebugParameter(speed_rotation)
|
||||
joints = control.walk(t, x, y, rotation)
|
||||
data = control.walk(t, x, y, rotation)
|
||||
if len(data) == 2: # When we receive the position of a target
|
||||
joints = data[0]
|
||||
x, y = data[1]
|
||||
z = 0
|
||||
else:
|
||||
joints = data
|
||||
x, y, z = 0, 0, -5 # We hide the target under the map
|
||||
p.resetBasePositionAndOrientation(target, [x, y, z],
|
||||
p.getQuaternionFromEuler([0, 0, 0]))
|
||||
elif mode == 'sandbox':
|
||||
joints = control.sandbox(t)
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user