feat: v4
This commit is contained in:
Pierre Tellier
212
simulator.py
Normal file
212
simulator.py
Normal file
@@ -0,0 +1,212 @@
|
||||
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)
|
||||
|
||||
# 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)
|
||||
joints = control.walk(t, x, y, rotation)
|
||||
elif mode == 'sandbox':
|
||||
joints = control.sandbox(t)
|
||||
|
||||
# Envoi des positions cibles au simulateur
|
||||
setJoints(robot, joints)
|
||||
|
||||
# Mise à jour de la simulation
|
||||
tick()
|
||||
Reference in New Issue
Block a user