import os import argparse import math import time import pybullet as p if __package__ is None or __package__ == '': import control else: from . import control t = 0. dt = 0.01 dirname = os.path.dirname(__file__) + '/models/' n_motors = 0 jointsMap = [] def init(): """Initialise le simulateur""" # Instanciation de Bullet physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -10) # Chargement du sol planeId = p.loadURDF(dirname + '/plane.urdf') p.setPhysicsEngineParameter(fixedTimeStep=dt) 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", startPos, startOrientation, useFixedBase=fixed) return model def setJoints(robot, joints): """Définis les angles cibles pour les moteurs du robot Arguments: int -- identifiant du robot joints {list} -- liste des positions cibles (rad) """ global jointsMap for k in range(len(joints)): jointInfo = p.getJointInfo(robot, jointsMap[k]) p.setJointMotorControl2( robot, jointInfo[0], p.POSITION_CONTROL, joints[k]) def inverseControls(name='', x_default=0.15, y_default=0.): target_x = p.addUserDebugParameter( '%starget_x' % name, -0.3, 0.3, x_default) target_y = p.addUserDebugParameter( '%starget_y' % name, -0.3, 0.3, y_default) target_z = p.addUserDebugParameter('%starget_z' % name, -0.3, 0.3, 0.) target = loadModel('target2', True) 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) gamma = p.addUserDebugParameter('gamma', -math.pi, math.pi, 0) target = loadModel('target2', True) return alpha, beta, gamma, target def inverseUpdate(controls): x = p.readUserDebugParameter(controls[0]) y = p.readUserDebugParameter(controls[1]) z = p.readUserDebugParameter(controls[2]) p.resetBasePositionAndOrientation( controls[3], [x, y, z + floatHeight], p.getQuaternionFromEuler([0, 0, 0])) return x, y, z def tick(): global t t += dt p.stepSimulation() time.sleep(dt) if __name__ == "__main__": # Arguments parser = argparse.ArgumentParser(prog="Quadruped") parser.add_argument('-m', type=str, help='Mode', default='motors') parser.add_argument('-r', type=str, help='Robot', default='quadruped') args = parser.parse_args() mode = args.m robot = args.r if mode not in ['motors', 'sandbox', 'direct', 'inverse', 'draw', 'legs', 'walk']: print('Le mode %s est inconnu' % mode) exit(1) init() fixed = False floatHeight = 0.15 if robot == 'quadruped': oneLegStartPos = [-0.04, 0., floatHeight] 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] oneLegStartPos = [-0.079, 0.032, floatHeight] n_motors = 18 jointsMap = list(range(18)) startPos = [0., 0., floatHeight] startOrientation = [0., 0., 0.] if mode == 'motors': motors_sliders = [] for k in range(n_motors): motors_sliders.append(p.addUserDebugParameter( "motor_%d" % k, -math.pi, math.pi, 0.)) elif mode == 'inverse' or mode == 'direct': fixed = True startOrientation = oneLegstartOrientation startPos = oneLegStartPos leg = inverseControls() if mode == 'inverse' else directControls() elif mode == 'draw': fixed = True startOrientation = oneLegstartOrientation startPos = oneLegStartPos lastLine = None elif mode == 'legs': fixed = True legs = [] if robot == 'quadruped': legs = [ inverseControls('leg1_', -0.15, 0.15), inverseControls('leg2_', -0.15, -0.15), inverseControls('leg3_', 0.15, -0.15), inverseControls('leg4_', 0.15, 0.15) ] else: legs = [ inverseControls('leg1_', 0.05, 0.24), inverseControls('leg2_', -0.05, 0.24), inverseControls('leg3_', -0.24, 0), 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) speed_rotation = p.addUserDebugParameter( 'speed_rotation', -0.5, 0.5, 0.) elif mode == 'sandbox': print('Mode bac à sable...') robot = loadModel(robot, fixed, startPos, startOrientation) target = loadModel('target2', True) # Boucle principale while True: if mode == 'motors': joints = [] for entry in motors_sliders: joints.append(p.readUserDebugParameter(entry)) elif mode == 'inverse': 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) x, y, z = control.direct(alpha, beta, gamma) p.resetBasePositionAndOrientation(target, [x, y, z + floatHeight], p.getQuaternionFromEuler([0, 0, 0])) 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: tip = getLegTip() p.addUserDebugLine(lastLine[1], tip, (1., 0, 0), 2., 10.) lastLine = time.time(), tip elif mode == 'legs': legs_xyz = [ inverseUpdate(leg) for leg in legs ] joints = control.legs(legs_xyz) elif mode == 'walk': x = p.readUserDebugParameter(speed_x) y = p.readUserDebugParameter(speed_y) rotation = p.readUserDebugParameter(speed_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) # Envoi des positions cibles au simulateur setJoints(robot, joints) # Mise à jour de la simulation tick()