1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
#script to control a simulated robot hand using a VR glove
#see https://twitter.com/erwincoumans/status/821953216271106048
#and https://www.youtube.com/watch?v=I6s37aBXbV8
#vr glove was custom build using Spectra Symbolflex sensors (4.5")
#inside a Under Armour Batting Glove, using DFRobot Bluno BLE/Beetle
#with BLE Link to receive serial (for wireless bluetooth serial)
import serial
import time
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.resetSimulation()
p.setGravity(0,0,-10)
print(c)
if (c<0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand=objects[0]
ho = p.getQuaternionFromEuler([0,3.14,0])
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.1,0,0],[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
maxV = 600
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 (1.0-b)
controllerId = -1
serialSteps = 0
serialStepsUntilCheckVREvents = 3
ser = serial.Serial(port='COM9',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
if (ser.isOpen()):
while True:
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]&p.VR_BUTTON_IS_DOWN):
controllerId = e[0]
if (e[0] == controllerId):
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):
middle = convertSensor(words[1])
pink = convertSensor(words[2])
index = convertSensor(words[3])
thumb = convertSensor(words[4])+0.2
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)
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)
|
