diff options
Diffstat (limited to 'examples/pybullet/examples/hand.py')
| -rw-r--r-- | examples/pybullet/examples/hand.py | 157 |
1 files changed, 84 insertions, 73 deletions
diff --git a/examples/pybullet/examples/hand.py b/examples/pybullet/examples/hand.py index 277e8ec79..5f1b0793a 100644 --- a/examples/pybullet/examples/hand.py +++ b/examples/pybullet/examples/hand.py @@ -12,18 +12,18 @@ 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) - +if (c < 0): + p.connect(p.GUI) + #load the MuJoCo MJCF hand objects = p.loadMJCF("MPL/MPL.xml") -hand=objects[0] +hand = objects[0] #clamp in range 400-600 #minV = 400 #maxV = 600 -minVarray = [275,280,350,290] -maxVarray = [450,550,500,400] +minVarray = [275, 280, 350, 290] +maxVarray = [450, 550, 500, 400] pinkId = 0 middleId = 1 @@ -32,82 +32,93 @@ 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 + 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) + 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 + 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 + 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) + 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") + print("Cannot find port") |