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
112
113
114
115
116
117
118
119
120
121
122
123
124
|
#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)
print(c)
if (c < 0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/MPL.xml")
hand = objects[0]
#clamp in range 400-600
#minV = 400
#maxV = 600
minVarray = [275, 280, 350, 290]
maxVarray = [450, 550, 500, 400]
pinkId = 0
middleId = 1
indexId = 2
thumbId = 3
p.setRealTimeSimulation(1)
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
def convertSensor(x, fingerIndex):
minV = minVarray[fingerIndex]
maxV = maxVarray[fingerIndex]
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)
ser = None
portindex = 0
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
if (ser is None):
ser = serial.Serial(port="/dev/cu.usbmodem1421",
baudrate=115200,
parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
pi = 3.141592
if (ser is not None and ser.isOpen()):
while True:
while ser.inWaiting() > 0:
line = str(ser.readline())
words = line.split(",")
if (len(words) == 6):
pink = convertSensor(words[1], pinkId)
middle = convertSensor(words[2], middleId)
index = convertSensor(words[3], indexId)
thumb = convertSensor(words[4], thumbId)
p.setJointMotorControl2(hand, 7, p.POSITION_CONTROL, pi / 4.)
p.setJointMotorControl2(hand, 9, p.POSITION_CONTROL, thumb + pi / 10)
p.setJointMotorControl2(hand, 11, p.POSITION_CONTROL, thumb)
p.setJointMotorControl2(hand, 13, p.POSITION_CONTROL, thumb)
p.setJointMotorControl2(hand, 17, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand, 19, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand, 21, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand, 24, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 26, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 28, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 40, p.POSITION_CONTROL, pink)
p.setJointMotorControl2(hand, 42, p.POSITION_CONTROL, pink)
p.setJointMotorControl2(hand, 44, p.POSITION_CONTROL, pink)
ringpos = 0.5 * (pink + middle)
p.setJointMotorControl2(hand, 32, p.POSITION_CONTROL, ringpos)
p.setJointMotorControl2(hand, 34, p.POSITION_CONTROL, ringpos)
p.setJointMotorControl2(hand, 36, p.POSITION_CONTROL, ringpos)
#print(middle)
#print(pink)
#print(index)
#print(thumb)
else:
print("Cannot find port")
|
