API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 16th, 2015
465
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 10.61 KB | None | 0 0
raw download clone embed print report
  1. from __future__ import division
  2. import math, time
  3.  
  4.  
  5.  
  6. leap = Runtime.createAndStart("leap","LeapMotion2")
  7. #inmoov = Runtime.createAndStart("inmoov", "InMoovHand")
  8. #inmoov.connect("COM3")
  9. arduinoServiceName = "arduino"
  10. arduino = Runtime.createAndStart(arduinoServiceName, "Arduino")
  11. arduino.connect("COM3")
  12.  
  13. start_time = time.time()
  14. counter = 0
  15.  
  16. leap.addFrameListener(python)
  17.  
  18. class LeapFrame:
  19.     def __init__(self, frame):
  20.         self.pointableOrder = [0 for x in range (5)]
  21.         self.frame = frame
  22.         if self.isValid():
  23.             self.valid = True
  24.             print "Hands count : %d" % self.frame.hands().count()
  25.             self.palmPosition = self._palmPosition(self.frame)
  26.             self.palmDirection = self._palmDirection(self.frame)
  27.             self.palmNormal = self._palmNormal(self.frame)
  28.            
  29.             self.thumbDirection = self._thumbDirection(self.frame)
  30.             self.indexDirection = self._indexDirection(self.frame)
  31.             self.middleDirection = self._middleDirection(self.frame)
  32.             self.ringDirection = self._ringDirection(self.frame)
  33.             self.littleDirection = self._littleDirection(self.frame)
  34.            
  35.             self.thumbAngleY = self._thumbAngleY(self.frame)
  36.             self.indexAngleY = self._indexAngleY(self.frame)
  37.             self.middleAngleY = self._middleAngleY(self.frame)
  38.             self.ringAngleY = self._ringAngleY(self.frame)
  39.             self.littleAngleY = self._littleAngleY(self.frame)
  40.            
  41.            
  42.             self.thumbAngleX = self._thumbAngleX(self.frame)
  43.             self.indexAngleX = self._indexAngleX(self.frame)
  44.             self.middleAngleX = self._middleAngleX(self.frame)
  45.             self.ringAngleX = self._ringAngleX(self.frame)
  46.             self.littleAngleX = self._littleAngleX(self.frame)
  47.            
  48.             self.deltaHandFingerThumb = self._deltaHandFingerThumb(self.frame)
  49.             self.deltaHandFingerIndex = self._deltaHandFingerIndex(self.frame)
  50.             self.deltaHandFingerMiddle = self._deltaHandFingerMiddle(self.frame)
  51.             self.deltaHandFingerRing = self._deltaHandFingerRing(self.frame)
  52.             self.deltaHandFingerLittle = self._deltaHandFingerLittle(self.frame)
  53.         else:
  54.             self.valid = False
  55.  
  56.     def isValid(self):
  57.         # Check valid condition if 1 hand is verified
  58.         if self.frame.hands().count() >= 1 and self.frame.hands().get(0):
  59.             print "Hands count 2 : %d " % self.frame.hands().count()
  60.             # Check valid condition if 5 fingers are verified
  61.             if self.frame.pointables().get(0).isValid() and self.frame.pointables().get(1).isValid() and self.frame.pointables().get(2).isValid() and self.frame.pointables().get(3).isValid() and self.frame.pointables().get(4).isValid():               
  62.                 return True
  63.         return False   
  64.    
  65.     def _vectorAngle(self, v1_x, v1_y, v1_z, v2_x, v2_y, v2_z):
  66.         vectorProduct = v1_x*v2_x + v1_y*v2_y + v1_z*v2_z
  67.         v1Norm = math.sqrt(v1_x*v1_x + v1_y*v1_y + v1_z*v1_z)
  68.         v2Norm = math.sqrt(v2_x*v2_x + v2_y*v2_y + v2_z*v2_z)
  69.         #print "v1_x %f v1_y %f v1_z %f -- v2_x %f v2_y %f v2_z %f" % (v1_x, v1_y, v1_z, v2_x, v2_y, v2_z)
  70.         cos = math.acos(vectorProduct / (v1Norm * v2Norm))
  71.         return (cos * 180 / math.pi)
  72.        
  73.     def _palmPosition(self, frame):
  74.         return frame.hands().get(0).palmPosition().getX(), frame.hands().get(0).palmPosition().getY(), frame.hands().get(0).palmPosition().getZ()
  75.        
  76.     def _palmDirection(self, frame):
  77.         return frame.hands().get(0).direction().getX(), frame.hands().get(0).direction().getY(), frame.hands().get(0).direction().getZ()
  78.    
  79.     def _palmNormal(self, frame):
  80.         return frame.hands().get(0).palmNormal().getX(), frame.hands().get(0).palmNormal().getY(), frame.hands().get(0).palmNormal().getZ()
  81.        
  82.     def _thumbPosition(self, frame):
  83.         return frame.hands().get(0).fingers().get(0).tipPosition().getX(), frame.hands().get(0).fingers().get(0).tipPosition().getY(), frame.hands().get(0).fingers().get(0).tipPosition().getZ()
  84.        
  85.     def _thumbDirection(self, frame):
  86.         return frame.hands().get(0).fingers().get(0).direction().getX(), frame.hands().get(0).fingers().get(0).direction().getY(), frame.hands().get(0).fingers().get(0).direction().getZ()
  87.    
  88.     def _indexPosition(self, frame):
  89.         return frame.hands().get(0).fingers().get(1).tipPosition().getX(), frame.hands().get(0).fingers().get(1).tipPosition().getY(), frame.hands().get(0).fingers().get(1).tipPosition().getZ()
  90.        
  91.     def _indexDirection(self, frame):
  92.         return frame.hands().get(0).fingers().get(1).direction().getX(), frame.hands().get(0).fingers().get(1).direction().getY(), frame.hands().get(0).fingers().get(1).direction().getZ()
  93.    
  94.     def _middlePosition(self, frame):
  95.         return frame.hands().get(0).fingers().get(2).tipPosition().getX(), frame.hands().get(0).fingers().get(2).tipPosition().getY(), frame.hands().get(0).fingers().get(2).tipPosition().getZ()
  96.    
  97.     def _middleDirection(self, frame):
  98.         return frame.hands().get(0).fingers().get(2).direction().getX(), frame.hands().get(0).fingers().get(2).direction().getY(), frame.hands().get(0).fingers().get(2).direction().getZ()
  99.        
  100.     def _ringPosition(self, frame):
  101.         return frame.hands().get(0).fingers().get(3).tipPosition().getX(), frame.hands().get(0).fingers().get(3).tipPosition().getY(), frame.hands().get(0).fingers().get(3).tipPosition().getZ()
  102.        
  103.     def _ringDirection(self, frame):
  104.         return frame.hands().get(0).fingers().get(3).direction().getX(), frame.hands().get(0).fingers().get(3).direction().getY(), frame.hands().get(0).fingers().get(3).direction().getZ()
  105.        
  106.     def _littlePosition(self, frame):
  107.         return frame.hands().get(0).fingers().get(4).tipPosition().getX(), frame.hands().get(0).fingers().get(4).tipPosition().getY(), frame.hands().get(0).fingers().get(4).tipPosition().getZ()
  108.        
  109.     def _littleDirection(self, frame):
  110.         return frame.hands().get(0).fingers().get(4).direction().getX(), frame.hands().get(0).fingers().get(4).direction().getY(), frame.hands().get(0).fingers().get(4).direction().getZ()
  111.        
  112.     def _thumbAngleY(self, frame):
  113.         x1, y1, z1 = self._palmNormal(frame)
  114.         x2, y2, z2 = self._thumbDirection(frame)
  115.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  116.        
  117.        
  118.     def _thumbAngleX(self, frame):
  119.         x1, y1, z1 = self._palmDirection(frame)
  120.         x2, y2, z2 = self._thumbDirection(frame)
  121.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  122.        
  123.     def _indexAngleY(self, frame):
  124.         x1, y1, z1 = self._palmNormal(frame)
  125.         x2, y2, z2 = self._indexDirection(frame)
  126.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  127.        
  128.        
  129.     def _indexAngleX(self, frame):
  130.         x1, y1, z1 = self._palmDirection(frame)
  131.         x2, y2, z2 = self._indexDirection(frame)
  132.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  133.    
  134.     def _middleAngleY(self, frame):
  135.         x1, y1, z1 = self._palmNormal(frame)
  136.         x2, y2, z2 = self._middleDirection(frame)
  137.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  138.        
  139.     def _middleAngleX(self, frame):
  140.         x1, y1, z1 = self._palmDirection(frame)
  141.         x2, y2, z2 = self._middleDirection(frame)
  142.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  143.        
  144.        
  145.     def _ringAngleY(self, frame):
  146.         x1, y1, z1 = self._palmNormal(frame)
  147.         x2, y2, z2 = self._ringDirection(frame)
  148.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  149.        
  150.     def _ringAngleX(self, frame):
  151.         x1, y1, z1 = self._palmDirection(frame)
  152.         x2, y2, z2 = self._ringDirection(frame)
  153.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  154.        
  155.     def _littleAngleY(self, frame):
  156.         x1, y1, z1 = self._palmNormal(frame)
  157.         x2, y2, z2 = self._littleDirection(frame)
  158.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  159.  
  160.    
  161.     def _littleAngleX(self, frame):
  162.         x1, y1, z1 = self._palmDirection(frame)
  163.         x2, y2, z2 = self._littleDirection(frame)
  164.         return self._vectorAngle(x1, y1, z1, x2, y2, z2)
  165.        
  166.     def _deltaHandFingerThumb(self, frame):
  167.         x1, y1, z1 = self._palmPosition(frame)
  168.         x2, y2, z2 = self._thumbPosition(frame)
  169.         return (x1-x2), (y1-y2), (z1-z2)
  170.        
  171.     def _deltaHandFingerIndex(self, frame):
  172.         x1, y1, z1 = self._palmPosition(frame)
  173.         x2, y2, z2 = self._indexPosition(frame)
  174.         return (x1-x2), (y1-y2), (z1-z2)
  175.    
  176.     def _deltaHandFingerMiddle(self, frame):
  177.         x1, y1, z1 = self._palmPosition(frame)
  178.         x2, y2, z2 = self._middlePosition(frame)
  179.         return (x1-x2), (y1-y2), (z1-z2)
  180.    
  181.     def _deltaHandFingerRing(self, frame):
  182.         x1, y1, z1 = self._palmPosition(frame)
  183.         x2, y2, z2 = self._ringPosition(frame)
  184.         return (x1-x2), (y1-y2), (z1-z2)
  185.        
  186.     def _deltaHandFingerLittle(self, frame):
  187.         x1, y1, z1 = self._palmPosition(frame)
  188.         x2, y2, z2 = self._littlePosition(frame)
  189.         return (x1-x2), (y1-y2), (z1-z2)
  190.  
  191.  
  192.        
  193.        
  194. class Joint:
  195.     def __init__(self, servo, minPos, maxPos, pin, range, arduinoServiceName):
  196.         self.servo = servo
  197.         self.minPos = minPos
  198.         self.maxPos = maxPos
  199.         self.pin = pin
  200.         self.range = range
  201.         self.servo.attach(arduinoServiceName, self.pin)
  202.         self.servo.setMinMax(self.range[0], self.range[1])
  203.        
  204.     def translate(self, value, leftMin, leftMax, rightMin, rightMax):
  205.         # Figure out how 'wide' each range is
  206.         leftSpan = leftMax - leftMin
  207.         rightSpan = rightMax - rightMin
  208.  
  209.         # Convert the left range into a 0-1 range (float)
  210.         valueScaled = float(value - leftMin) / float(leftSpan)
  211.  
  212.         # Convert the 0-1 range into a value in the right range.
  213.         return rightMin + (valueScaled * rightSpan)
  214.  
  215.     def scale(self, pos):
  216.         if  pos < self.minPos:
  217.             pos = self.minPos
  218.         elif pos > self.maxPos:
  219.             pos = self.maxPos
  220.         return math.floor(self.translate(pos, self.minPos, self.maxPos, self.range[0], self.range[1]))
  221.    
  222.     def move(self, pos):
  223.         """if constraint in locals():
  224.             pos = constraint(pos)"""
  225.         angle = self.scale(pos)
  226.         self.servo.moveTo(angle)
  227.  
  228. # variables
  229. pouce = Runtime.createAndStart("pouce", "Servo")
  230. index = Runtime.createAndStart("index", "Servo")
  231. majeur = Runtime.createAndStart("majeur", "Servo")
  232. annulaire = Runtime.createAndStart("annulaire", "Servo")
  233. auriculaire = Runtime.createAndStart("auriculaire", "Servo")
  234. paume = Runtime.createAndStart("paume", "Servo")
  235.  
  236.  
  237. # servo, minPos, maxPos, pin, range
  238. j_pouce = Joint(pouce, 0, 50, 2, [50, 170], arduinoServiceName)
  239. j_index = Joint(index, 0, 50, 3, [50, 170], arduinoServiceName)
  240. j_majeur = Joint(majeur, 0, 50, 4, [50, 170], arduinoServiceName)
  241. j_annulaire = Joint(annulaire, 0, 50, 5, [50, 170], arduinoServiceName)
  242. j_auriculaire = Joint(auriculaire, 0, 50, 6, [50, 170], arduinoServiceName)
  243. j_paume = Joint(paume, -30, 30, 7, [50, 150], arduinoServiceName)
  244.  
  245.    
  246.        
  247. def onFrame(fram):
  248.     global start_time
  249.     global counter
  250.    
  251.     # Track only 40 frames/s
  252.     elapsed_time = time.time() - start_time
  253.     if elapsed_time >= 1 and counter <= 120:
  254.         start_time = time.time()
  255.         counter += 1
  256.     elif counter >= 120:
  257.         counter = 0
  258.     frame = LeapFrame(fram)
  259.     if frame.valid:
  260.         j_pouce.move(int(frame.deltaHandFingerThumb[1]))
  261.         j_index.move(int(frame.deltaHandFingerIndex[1]))
  262.         j_majeur.move(int(frame.deltaHandFingerMiddle[1]))
  263.         j_annulaire.move(int(frame.deltaHandFingerRing[1]))
  264.         j_auriculaire.move(int(frame.deltaHandFingerLittle[1]))
  265.        
  266.         j_paume.move(int(frame.palmNormal[0] * 50))
  267.    
  268. leap.startTracking()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!