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

feat: finised project

Browse Source
This commit is contained in:
Pierre Tellier 2025-04-07 14:53:38 +02:00
parent 90d08671d9
commit 6b0f2c2170
2 changed files with 145 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

190
control.py
View File

@ -1,14 +1,15 @@
import logging import logging
import math
import numpy as np import numpy as np
from math import * from math import *
from logs import * from logs import *
from dynamixel_sdk import *
from xbox import * from xbox import *
import time import time
# DEBUG # DEBUG
LEGS_LOG_LEVEL = logging.INFO LEGS_LOG_LEVEL = logging.DEBUG
CONTROLLER_LOG_LEVEL = logging.INFO CONTROLLER_LOG_LEVEL = logging.CRITICAL
# Variables configurations # Variables configurations
l1h = 0.049 l1h = 0.049
@ -40,17 +41,39 @@ controllerLogger = setup_logger("Controller", CONTROLLER_LOG_LEVEL)
# Initialize controller # Initialize controller
xbox = Xbox(controllerLogger) xbox = Xbox(controllerLogger)
CONTROLLER_MODE = xbox.initialized CONTROLLER_MODE = xbox.initialized
xbox.mode_count = 4 xbox.mode_count = 5
neutral_position = np.array([ neutral_position = np.array([
[0.1, 0.15, -0.15], [0.1, 0.16, -0.15],
[-0.1, 0.15, -0.15], [-0.1, 0.16, -0.15],
[-0.2, -0.00, -0.15], [-0.2, -0.00, -0.15],
[-0.1, -0.15, -0.15], [-0.1, -0.16, -0.15],
[0.1, -0.15, -0.15], [0.1, -0.16, -0.15],
[0.2, 0, -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): def interpol2(point2, point1, t):
x1, y1, z1 = point1 x1, y1, z1 = point1
@ -88,14 +111,14 @@ def inverse(x, y, z):
param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3) param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)
if param2 > 1 or param2 < -1: 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 param2 = 1 if param2 > 1 else -1
theta2 = acos(param2) theta2 = acos(param2)
param1 = (-l3 ** 2 + l2 ** 2 + l ** 2) / (2 * l2 * l) param1 = (-l3 ** 2 + l2 ** 2 + l ** 2) / (2 * l2 * l)
if param1 > 1 or param1 < -1: 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 param1 = 1 if param1 > 1 else -1
theta1 = acos(param1) + asin(z / l) theta1 = acos(param1) + asin(z / l)
@ -203,46 +226,10 @@ def naive_walk(t, speed_x, speed_y):
return legs(real_position) 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): def normalize(matrix, slider_max, speed):
return (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): def walk(t, sx, sy, sr):
xboxdata = xbox.get_data() xboxdata = xbox.get_data()
max_slider = 0.200 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"], 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)) max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
elif xbox.mode == 3: 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"]) 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(): def static():
@ -266,16 +253,21 @@ def static():
# Walk V2 # 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): def get_rotation_center(speed_x, speed_y, theta_point):
direction = np.array([-speed_y, speed_x]) 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) legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")
num_patte = 6
real_position = np.copy(neutral_position) real_position = np.copy(neutral_position)
@ -285,7 +277,7 @@ def dev(t, x1, y1, x2, y2):
[0, 0, -0.01] [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) step_count = len(movement_z)
movement_duration = np.sum(step_duration) movement_duration = np.sum(step_duration)
@ -296,15 +288,21 @@ def dev(t, x1, y1, x2, y2):
def get_next_step(t): def get_next_step(t):
return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count) 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" 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): for patte in range(num_patte):
time = t + offsets[patte] time = t + offsets[patte]
mov_index_start = get_current_step(time, step_duration, movement_duration) mov_index_start = get_current_step(time, step_duration, movement_duration)
mov_index_end = get_next_step(time) 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_start_z = movement_z[mov_index_start]
mov_end_z = movement_z[mov_index_end] 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][1],
real_position[patte][2]) = interpol2(mov_start, mov_end, step_adv) 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] 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): def jump(sy):
@ -424,5 +434,35 @@ def dino_naive(t, speed_x, speed_y, hz, hy, hx):
return legs(real_position) 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__": if __name__ == "__main__":
print("N'exécutez pas ce fichier, mais simulator.py") print("N'exécutez pas ce fichier, mais simulator.py")

42
simulator.py
View File

@ -3,6 +3,7 @@ import argparse
import math import math
import time import time
import pybullet as p import pybullet as p
if __package__ is None or __package__ == '': if __package__ is None or __package__ == '':
import control import control
else: else:
@ -14,6 +15,7 @@ dirname = os.path.dirname(__file__) + '/models/'
n_motors = 0 n_motors = 0
jointsMap = [] jointsMap = []
def init(): def init():
"""Initialise le simulateur""" """Initialise le simulateur"""
# Instanciation de Bullet # Instanciation de Bullet
@ -21,7 +23,7 @@ def init():
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
# Chargement du sol # Chargement du sol
planeId = p.loadURDF(dirname+'/plane.urdf') planeId = p.loadURDF(dirname + '/plane.urdf')
p.setPhysicsEngineParameter(fixedTimeStep=dt) p.setPhysicsEngineParameter(fixedTimeStep=dt)
@ -29,7 +31,7 @@ def init():
def loadModel(name, fixed=False, startPos=[0., 0., 0.1], startOrientation=[0., 0., 0.]): def loadModel(name, fixed=False, startPos=[0., 0., 0.1], startOrientation=[0., 0., 0.]):
"""Charge un modèle""" """Charge un modèle"""
startOrientation = p.getQuaternionFromEuler(startOrientation) startOrientation = p.getQuaternionFromEuler(startOrientation)
model = p.loadURDF(dirname+"/"+name+"/robot.urdf", model = p.loadURDF(dirname + "/" + name + "/robot.urdf",
startPos, startOrientation, useFixedBase=fixed) startPos, startOrientation, useFixedBase=fixed)
return model return model
@ -43,7 +45,7 @@ def setJoints(robot, joints):
joints {list} -- liste des positions cibles (rad) joints {list} -- liste des positions cibles (rad)
""" """
global jointsMap global jointsMap
for k in range(len(joints)): for k in range(len(joints)):
jointInfo = p.getJointInfo(robot, jointsMap[k]) jointInfo = p.getJointInfo(robot, jointsMap[k])
p.setJointMotorControl2( p.setJointMotorControl2(
@ -60,6 +62,7 @@ def inverseControls(name='', x_default=0.15, y_default=0.):
return target_x, target_y, target_z, target return target_x, target_y, target_z, target
def directControls(): def directControls():
alpha = p.addUserDebugParameter('alpha', -math.pi, math.pi, 0) alpha = p.addUserDebugParameter('alpha', -math.pi, math.pi, 0)
beta = p.addUserDebugParameter('beta', -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 return x, y, z
def tick(): def tick():
global t global t
@ -85,6 +89,7 @@ def tick():
p.stepSimulation() p.stepSimulation()
time.sleep(dt) time.sleep(dt)
if __name__ == "__main__": if __name__ == "__main__":
# Arguments # Arguments
parser = argparse.ArgumentParser(prog="Quadruped") parser = argparse.ArgumentParser(prog="Quadruped")
@ -104,11 +109,11 @@ if __name__ == "__main__":
if robot == 'quadruped': if robot == 'quadruped':
oneLegStartPos = [-0.04, 0., floatHeight] oneLegStartPos = [-0.04, 0., floatHeight]
oneLegstartOrientation = [0., 0., math.pi + math.pi/4] oneLegstartOrientation = [0., 0., math.pi + math.pi / 4]
n_motors = 12 n_motors = 12
jointsMap = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14] jointsMap = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
else: else:
oneLegstartOrientation = [0., 0., -math.pi/2] oneLegstartOrientation = [0., 0., -math.pi / 2]
oneLegStartPos = [-0.079, 0.032, floatHeight] oneLegStartPos = [-0.079, 0.032, floatHeight]
n_motors = 18 n_motors = 18
jointsMap = list(range(18)) jointsMap = list(range(18))
@ -149,8 +154,8 @@ if __name__ == "__main__":
inverseControls('leg4_', -0.05, -0.24), inverseControls('leg4_', -0.05, -0.24),
inverseControls('leg5_', 0.05, -0.24), inverseControls('leg5_', 0.05, -0.24),
inverseControls('leg6_', 0.24, 0.) inverseControls('leg6_', 0.24, 0.)
] ]
elif mode == 'walk': elif mode == 'walk':
speed_x = p.addUserDebugParameter('speed_x', -0.2, 0.2, 0.0) speed_x = p.addUserDebugParameter('speed_x', -0.2, 0.2, 0.0)
speed_y = p.addUserDebugParameter('speed_y', -0.2, 0.2, 0.0) speed_y = p.addUserDebugParameter('speed_y', -0.2, 0.2, 0.0)
@ -161,6 +166,8 @@ if __name__ == "__main__":
robot = loadModel(robot, fixed, startPos, startOrientation) robot = loadModel(robot, fixed, startPos, startOrientation)
target = loadModel('target2', True)
# Boucle principale # Boucle principale
while True: while True:
if mode == 'motors': if mode == 'motors':
@ -168,23 +175,25 @@ if __name__ == "__main__":
for entry in motors_sliders: for entry in motors_sliders:
joints.append(p.readUserDebugParameter(entry)) joints.append(p.readUserDebugParameter(entry))
elif mode == 'inverse': elif mode == 'inverse':
joints = control.inverse(*inverseUpdate(leg)) + [0]*(n_motors-3) joints = control.inverse(*inverseUpdate(leg)) + [0] * (n_motors - 3)
elif mode == 'direct': elif mode == 'direct':
alpha_slider, beta_slider, gamma_slider, target = leg alpha_slider, beta_slider, gamma_slider, target = leg
alpha = p.readUserDebugParameter(alpha_slider) alpha = p.readUserDebugParameter(alpha_slider)
beta = p.readUserDebugParameter(beta_slider) beta = p.readUserDebugParameter(beta_slider)
gamma = p.readUserDebugParameter(gamma_slider) gamma = p.readUserDebugParameter(gamma_slider)
joints = [alpha, beta, gamma] + [0]*(n_motors-3) joints = [alpha, beta, gamma] + [0] * (n_motors - 3)
x, y, z = control.direct(alpha, beta, gamma) x, y, z = control.direct(alpha, beta, gamma)
p.resetBasePositionAndOrientation(target, [x, y, z + floatHeight], p.resetBasePositionAndOrientation(target, [x, y, z + floatHeight],
p.getQuaternionFromEuler([0, 0, 0])) p.getQuaternionFromEuler([0, 0, 0]))
elif mode == 'draw': elif mode == 'draw':
joints = control.draw(t) + [0]*(n_motors-3) joints = control.draw(t) + [0] * (n_motors - 3)
def getLegTip(): def getLegTip():
res = p.getLinkState(robot, 3) res = p.getLinkState(robot, 3)
return res[0] return res[0]
if lastLine is None: if lastLine is None:
lastLine = time.time(), getLegTip() lastLine = time.time(), getLegTip()
elif time.time() - lastLine[0] > 0.1: elif time.time() - lastLine[0] > 0.1:
@ -201,7 +210,16 @@ if __name__ == "__main__":
x = p.readUserDebugParameter(speed_x) x = p.readUserDebugParameter(speed_x)
y = p.readUserDebugParameter(speed_y) y = p.readUserDebugParameter(speed_y)
rotation = p.readUserDebugParameter(speed_rotation) 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': elif mode == 'sandbox':
joints = control.sandbox(t) joints = control.sandbox(t)