add yapf style and apply yapf to format all Python files
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
@@ -11,38 +11,66 @@ import pybullet as p
|
||||
|
||||
#first try to connect to shared memory (VR), if it fails use local GUI
|
||||
c = p.connect(p.SHARED_MEMORY)
|
||||
if (c<0):
|
||||
c = p.connect(p.GUI)
|
||||
|
||||
p.setInternalSimFlags(0)#don't load default robot assets etc
|
||||
if (c < 0):
|
||||
c = p.connect(p.GUI)
|
||||
|
||||
p.setInternalSimFlags(0) #don't load default robot assets etc
|
||||
p.resetSimulation()
|
||||
|
||||
#p.resetSimulation()
|
||||
p.setGravity(0,0,-10)
|
||||
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
|
||||
p.setGravity(0, 0, -10)
|
||||
objects = [
|
||||
p.loadURDF("plane.urdf", 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000)
|
||||
]
|
||||
|
||||
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
|
||||
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
|
||||
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
|
||||
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
|
||||
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
|
||||
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
|
||||
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
|
||||
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
|
||||
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
|
||||
objects = [
|
||||
p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("jenga/jenga.urdf", 1.200000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("jenga/jenga.urdf", 1.100000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("jenga/jenga.urdf", 1.000000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("jenga/jenga.urdf", 0.900000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("jenga/jenga.urdf", 0.800000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("table/table.urdf", 1.000000, -0.200000, 0.000000, 0.000000, 0.000000, 0.707107,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("cube_small.urdf", 0.950000, -0.100000, 0.700000, 0.000000, 0.000000, 0.707107,
|
||||
0.707107)
|
||||
]
|
||||
objects = [
|
||||
p.loadURDF("sphere_small.urdf", 0.850000, -0.400000, 0.700000, 0.000000, 0.000000, 0.707107,
|
||||
0.707107)
|
||||
]
|
||||
|
||||
|
||||
#load the MuJoCo MJCF hand
|
||||
objects = p.loadMJCF("MPL/mpl2.xml")
|
||||
|
||||
hand=objects[0]
|
||||
ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
|
||||
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
|
||||
print ("hand_cid")
|
||||
print (hand_cid)
|
||||
for i in range (p.getNumJoints(hand)):
|
||||
p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
|
||||
|
||||
hand = objects[0]
|
||||
ho = p.getQuaternionFromEuler([3.14, -3.14 / 2, 0])
|
||||
hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [-0.05, 0, 0.02],
|
||||
[0.500000, 0.300006, 0.700000], ho)
|
||||
print("hand_cid")
|
||||
print(hand_cid)
|
||||
for i in range(p.getNumJoints(hand)):
|
||||
p.setJointMotorControl2(hand, i, p.POSITION_CONTROL, 0, 0)
|
||||
|
||||
#clamp in range 400-600
|
||||
#minV = 400
|
||||
@@ -50,100 +78,105 @@ for i in range (p.getNumJoints(hand)):
|
||||
minV = 250
|
||||
maxV = 450
|
||||
|
||||
|
||||
POSITION=1
|
||||
ORIENTATION=2
|
||||
BUTTONS=6
|
||||
POSITION = 1
|
||||
ORIENTATION = 2
|
||||
BUTTONS = 6
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
|
||||
def convertSensor(x):
|
||||
v = minV
|
||||
try:
|
||||
v = float(x)
|
||||
except ValueError:
|
||||
v = minV
|
||||
if (v<minV):
|
||||
v=minV
|
||||
if (v>maxV):
|
||||
v=maxV
|
||||
b = (v-minV)/float(maxV-minV)
|
||||
return (b)
|
||||
|
||||
v = minV
|
||||
try:
|
||||
v = float(x)
|
||||
except ValueError:
|
||||
v = minV
|
||||
if (v < minV):
|
||||
v = minV
|
||||
if (v > maxV):
|
||||
v = maxV
|
||||
b = (v - minV) / float(maxV - minV)
|
||||
return (b)
|
||||
|
||||
|
||||
controllerId = -1
|
||||
|
||||
serialSteps = 0
|
||||
serialStepsUntilCheckVREvents = 3
|
||||
|
||||
|
||||
def getSerialOrNone(portname):
|
||||
try:
|
||||
return serial.Serial(port=portname,baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
|
||||
except:
|
||||
return None
|
||||
try:
|
||||
return serial.Serial(port=portname,
|
||||
baudrate=115200,
|
||||
parity=serial.PARITY_ODD,
|
||||
stopbits=serial.STOPBITS_TWO,
|
||||
bytesize=serial.SEVENBITS)
|
||||
except:
|
||||
return None
|
||||
|
||||
|
||||
ser = None
|
||||
|
||||
portindex = 10
|
||||
while (ser is None and portindex < 30):
|
||||
portname = 'COM'+str(portindex)
|
||||
print(portname)
|
||||
ser = getSerialOrNone(portname)
|
||||
if (ser is None):
|
||||
portname = "/dev/cu.usbmodem14"+str(portindex)
|
||||
print(portname)
|
||||
ser = getSerialOrNone(portname)
|
||||
if (ser is not None):
|
||||
print("COnnected!")
|
||||
portindex = portindex+1
|
||||
portname = 'COM' + str(portindex)
|
||||
print(portname)
|
||||
ser = getSerialOrNone(portname)
|
||||
if (ser is None):
|
||||
portname = "/dev/cu.usbmodem14" + str(portindex)
|
||||
print(portname)
|
||||
ser = getSerialOrNone(portname)
|
||||
if (ser is not None):
|
||||
print("COnnected!")
|
||||
portindex = portindex + 1
|
||||
|
||||
p.saveWorld("setupTrackerWorld.py")
|
||||
|
||||
|
||||
if (ser.isOpen()):
|
||||
while True:
|
||||
while True:
|
||||
|
||||
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER)
|
||||
for e in (events):
|
||||
p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50)
|
||||
|
||||
serialSteps = 0
|
||||
while ser.inWaiting() > 0:
|
||||
serialSteps=serialSteps+1
|
||||
if (serialSteps>serialStepsUntilCheckVREvents):
|
||||
ser.flushInput()
|
||||
break
|
||||
line = str(ser.readline())
|
||||
words = line.split(",")
|
||||
if (len(words)==6):
|
||||
pink = convertSensor(words[1])
|
||||
middle = convertSensor(words[2])
|
||||
index = convertSensor(words[3])
|
||||
thumb = convertSensor(words[4])+0.2
|
||||
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER)
|
||||
for e in (events):
|
||||
p.changeConstraint(hand_cid, e[POSITION], e[ORIENTATION], maxForce=50)
|
||||
|
||||
p.setJointMotorControl2(hand,5,p.POSITION_CONTROL,1.3)
|
||||
p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,thumb)
|
||||
p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb)
|
||||
p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
|
||||
|
||||
p.setJointMotorControl2(hand,15,p.POSITION_CONTROL,index)
|
||||
p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
|
||||
p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
|
||||
serialSteps = 0
|
||||
while ser.inWaiting() > 0:
|
||||
serialSteps = serialSteps + 1
|
||||
if (serialSteps > serialStepsUntilCheckVREvents):
|
||||
ser.flushInput()
|
||||
break
|
||||
line = str(ser.readline())
|
||||
words = line.split(",")
|
||||
if (len(words) == 6):
|
||||
pink = convertSensor(words[1])
|
||||
middle = convertSensor(words[2])
|
||||
index = convertSensor(words[3])
|
||||
thumb = convertSensor(words[4]) + 0.2
|
||||
|
||||
p.setJointMotorControl2(hand,22,p.POSITION_CONTROL,middle)
|
||||
p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
|
||||
p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
|
||||
|
||||
p.setJointMotorControl2(hand,38,p.POSITION_CONTROL,pink)
|
||||
p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
|
||||
p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
|
||||
p.setJointMotorControl2(hand, 5, p.POSITION_CONTROL, 1.3)
|
||||
p.setJointMotorControl2(hand, 7, p.POSITION_CONTROL, thumb)
|
||||
p.setJointMotorControl2(hand, 9, p.POSITION_CONTROL, thumb)
|
||||
p.setJointMotorControl2(hand, 11, p.POSITION_CONTROL, thumb)
|
||||
|
||||
ringpos = 0.5*(pink+middle)
|
||||
p.setJointMotorControl2(hand,30,p.POSITION_CONTROL,ringpos)
|
||||
p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos)
|
||||
p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos)
|
||||
|
||||
#print(middle)
|
||||
#print(pink)
|
||||
#print(index)
|
||||
#print(thumb)
|
||||
p.setJointMotorControl2(hand, 15, p.POSITION_CONTROL, index)
|
||||
p.setJointMotorControl2(hand, 17, p.POSITION_CONTROL, index)
|
||||
p.setJointMotorControl2(hand, 19, p.POSITION_CONTROL, index)
|
||||
|
||||
p.setJointMotorControl2(hand, 22, p.POSITION_CONTROL, middle)
|
||||
p.setJointMotorControl2(hand, 24, p.POSITION_CONTROL, middle)
|
||||
p.setJointMotorControl2(hand, 26, p.POSITION_CONTROL, middle)
|
||||
|
||||
p.setJointMotorControl2(hand, 38, p.POSITION_CONTROL, pink)
|
||||
p.setJointMotorControl2(hand, 40, p.POSITION_CONTROL, pink)
|
||||
p.setJointMotorControl2(hand, 42, p.POSITION_CONTROL, pink)
|
||||
|
||||
ringpos = 0.5 * (pink + middle)
|
||||
p.setJointMotorControl2(hand, 30, p.POSITION_CONTROL, ringpos)
|
||||
p.setJointMotorControl2(hand, 32, p.POSITION_CONTROL, ringpos)
|
||||
p.setJointMotorControl2(hand, 34, p.POSITION_CONTROL, ringpos)
|
||||
|
||||
#print(middle)
|
||||
#print(pink)
|
||||
#print(index)
|
||||
#print(thumb)
|
||||
|
||||
Reference in New Issue
Block a user