From 6b0f2c21707345a51d1d27504464ff112badccd3 Mon Sep 17 00:00:00 2001 From: Pierre Tellier Date: Mon, 7 Apr 2025 14:53:38 +0200 Subject: [PATCH] feat: finised project --- control.py | 190 +++++++++++++++++++++++++++++++-------------------- simulator.py | 42 ++++++++---- 2 files changed, 145 insertions(+), 87 deletions(-) diff --git a/control.py b/control.py index cce115a..84c1b03 100644 --- a/control.py +++ b/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") diff --git a/simulator.py b/simulator.py index 492af7a..c03227f 100644 --- a/simulator.py +++ b/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 @@ -21,7 +23,7 @@ def init(): p.setGravity(0, 0, -10) # Chargement du sol - planeId = p.loadURDF(dirname+'/plane.urdf') + planeId = p.loadURDF(dirname + '/plane.urdf') p.setPhysicsEngineParameter(fixedTimeStep=dt) @@ -29,7 +31,7 @@ def init(): def loadModel(name, fixed=False, startPos=[0., 0., 0.1], startOrientation=[0., 0., 0.]): """Charge un modèle""" startOrientation = p.getQuaternionFromEuler(startOrientation) - model = p.loadURDF(dirname+"/"+name+"/robot.urdf", + model = p.loadURDF(dirname + "/" + name + "/robot.urdf", startPos, startOrientation, useFixedBase=fixed) return model @@ -43,7 +45,7 @@ def setJoints(robot, joints): joints {list} -- liste des positions cibles (rad) """ global jointsMap - + for k in range(len(joints)): jointInfo = p.getJointInfo(robot, jointsMap[k]) p.setJointMotorControl2( @@ -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") @@ -104,11 +109,11 @@ if __name__ == "__main__": if robot == 'quadruped': oneLegStartPos = [-0.04, 0., floatHeight] - oneLegstartOrientation = [0., 0., math.pi + math.pi/4] + oneLegstartOrientation = [0., 0., math.pi + math.pi / 4] n_motors = 12 jointsMap = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14] else: - oneLegstartOrientation = [0., 0., -math.pi/2] + oneLegstartOrientation = [0., 0., -math.pi / 2] oneLegStartPos = [-0.079, 0.032, floatHeight] n_motors = 18 jointsMap = list(range(18)) @@ -149,8 +154,8 @@ if __name__ == "__main__": inverseControls('leg4_', -0.05, -0.24), inverseControls('leg5_', 0.05, -0.24), inverseControls('leg6_', 0.24, 0.) - ] - + ] + elif mode == 'walk': speed_x = p.addUserDebugParameter('speed_x', -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) + target = loadModel('target2', True) + # Boucle principale while True: if mode == 'motors': @@ -168,23 +175,25 @@ if __name__ == "__main__": for entry in motors_sliders: joints.append(p.readUserDebugParameter(entry)) elif mode == 'inverse': - joints = control.inverse(*inverseUpdate(leg)) + [0]*(n_motors-3) + joints = control.inverse(*inverseUpdate(leg)) + [0] * (n_motors - 3) elif mode == 'direct': alpha_slider, beta_slider, gamma_slider, target = leg alpha = p.readUserDebugParameter(alpha_slider) beta = p.readUserDebugParameter(beta_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) p.resetBasePositionAndOrientation(target, [x, y, z + floatHeight], - p.getQuaternionFromEuler([0, 0, 0])) + p.getQuaternionFromEuler([0, 0, 0])) elif mode == 'draw': - joints = control.draw(t) + [0]*(n_motors-3) + 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)