Hello everyone!

I'm currently working on arm tracking with openni and kinect (i know there is a built in service but as an intern i must do one myself). My code crashes sometimes and i don't know why, here is how i handle the initialisation and the user tracking.

I feel like this is not the right way, i heard something about some method onLostUser and onFoundUser.

def init():

  print "start initialise"

  users = []

  image = []

  myWrapper = SimpleOpenNI(ni)

  myWrapper.setMirror(True)

  myWrapper.enableIR(640, 640, 30)

  myWrapper.enableDepth(640, 640, 30)

  myWrapper.enableUser()

  print "initialised : "+str(myWrapper.isInit())

  if myWrapper.isInit():

    return myWrapper

  else:

    return Exception

 

def schedule(myWrapper, duree, periode):

  print "start schedule"

  i = 1

  userFound = False

  while i <= 100:

    try:

      user = myWrapper.getUsers()[0]

      myWrapper.startTrackingSkeleton(user)

      print "user found"

      userFound = True

      i = 101

    except Exception:

      myWrapper.update()

      i=i+1

      print i

      sleep(0.1)

  if i >= 100 and userFound == False:

    print "failed to find user"

    return

 

  startingTime = time.time()

  lastTime = time.time() - startingTime

  while (time.time() - startingTime) < duree: 

    if (time.time() - lastTime - startingTime) > periode:

      updatePosition(myWrapper, user)

      myWrapper.update()

      lastTime = time.time() - startingTime

  myWrapper.stopTrackingSkeleton(user)

  print "stop schedule"

 

mywrapper = init()

schedule(mywrapper, 30, 0.1)

 

nicolas taguet

6 years 8 months ago

If it interests someone here is how i get the angles and the position of the different arm parts :

 

#get the rotate angle and convert it to motor value

def rotateToMotor(rElbowPosition, rShoulderPosition, rShoulderOrientation):

 

#initialise the system : repere = system 

  repereBicep = PMatrix3D(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1)

  repereShoulder = PMatrix3D(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1)

   

  rShoulderOrientation.m03 = 0

  rShoulderOrientation.m13 = 0

  ShoulderOrientation.m23 = 0

           

  repereShoulder.apply(rShoulderOrientation)

   

  rShoulderRepereX = PVector(repereShoulder.m00, repereShoulder.m10, repereShoulder.m20)

  rShoulderRepereY = PVector(repereShoulder.m01, repereShoulder.m11, repereShoulder.m21)

  rShoulderRepereZ = PVector(repereShoulder.m02, repereShoulder.m12, repereShoulder.m22)

  repereShoulderAxis = [rShoulderRepereX, rShoulderRepereY, rShoulderRepereZ]

 

  vectBicep = PVector(rElbowPosition.x - rShoulderPosition.x, rElbowPosition.y -  rShoulderPosition.y, rElbowPosition.z - rShoulderPosition.z)

 

  repereBicepX = PVector(repereBicep.m00, repereBicep.m10, repereBicep.m20)

  repereBicepY = PVector(repereBicep.m01, repereBicep.m11, repereBicep.m21)

  repereBicepZ = PVector(repereBicep.m02, repereBicep.m12, repereBicep.m22)

  repereBicepAxis = [repereBicepX, repereBicepX, repereBicepZ]

 

#-------------------------------------------------------------------------------

#align the axis x of the system repereBicepAxis with the vector "shoulder to elbow"

  i = 0

  u = repereBicepAxis[i].cross(vectBicep)

  det = PMatrix3D(vectBicep.x, vectBicep.y, vectBicep.z, 0, repereBicepAxis[i].x, repereBicepAxis[i].y, repereBicepAxis[i].z, 0, u.x, u.y, u.z, 0, 0, 0, 0, 1).determinant()

  rotateAxisU = math.copysign(PVector.angleBetween(vectBicep, repereBicepAxis[i]), -det)

  repereBicep.rotate(rotateAxisU, u.x, u.y, u.z)

#-------------------------------------------------------------------------------

  repereBicepX = PVector(repereBicep.m00, repereBicep.m10, repereBicep.m20)

  repereBicepY = PVector(repereBicep.m01, repereBicep.m11, repereBicep.m21)

  repereBicepZ = PVector(repereBicep.m02, repereBicep.m12, repereBicep.m22)

 

#-------------------------------------------------------------------------------

#align the axis z of the system repereBicepAxis with the axis z of the shoulder's    orientation

  i = 2

  j = 2

  u = repereBicepAxis[i].cross(repereShoulderAxis[j])

  det = PMatrix3D(repereShoulderAxis[j].x, repereShoulderAxis[j].y, repereShoulderAxis[j].z, 0, repereBicepAxis[i].x, repereBicepAxis[i].y, repereBicepAxis[i].z, 0, u.x, u.y, u.z, 0, 0, 0, 0, 1).determinant()

 

  rotateAxisU = math.copysign(PVector.angleBetween(repereShoulderAxis[j],   repereBicepAxis[i]), -det)

  repereBicep.rotate(rotateAxisU, u.x, u.y, u.z)

#-------------------------------------------------------------------------------

           

  repereBicepX = PVector(repereBicep.m00, repereBicep.m10, repereBicep.m20)

  repereBicepY = PVector(repereBicep.m01, repereBicep.m11, repereBicep.m21)

  repereBicepZ = PVector(repereBicep.m02, repereBicep.m12, repereBicep.m22)

  value = angleBetween(repereBicepX, rShoulderRepereZ)

  value = ((value)*100 + 54)*1.56

  if value < 0:

    return 0

  elif value > 180:

    return 180

  else:return myround(value, rounding)

 

 

#get the omoplate angle and convert it to motor value

 

def homoplateToMotor(rShoulderPosition, rElbowPosition, lHipPosition, rHipPosition):

  value = projectToBodyReferential(rShoulderPosition, rElbowPosition, lHipPosition, rHipPosition).x

  #value = (value*100-20)*(1.3)+80

  value = (value*100-20)*(1.1)+80

  if value < 90:

    return 90

  elif value > 180:

    return 180

  else: return myround(value, rounding)

 

 

#get the shoulder angle and convert it to motor value

 

def shoulderToMotor(rShoulderPosition, rElbowPosition, torsoPosition, headPosition):

  value = projectToBodyReferential(rShoulderPosition, rElbowPosition, torsoPosition,  headPosition).z

  #value = value*400+90

  value = value*375+90

  if value < 0:

    return 0

  elif value > 180:

    return 180

  else:return myround(value, rounding)

 

#get the bicep angle and convert it to motor value

 

def bicepToMotor(vectBicep, vectForeArm):

  value = angleBetween(vectBicep, vectForeArm)*100+90

  if value < 0 :

    return 0

  elif value > 90 :

    return 90

  else:return myround(value, 5)

GroG

6 years 8 months ago

Hi Nicolas,

Thanks for sharing your work.  Its been a while since I worked on the OpenNi module, but its nice to see another implementation in Python.

Not sure about the crashing, if I remember correctly we hooked events in the OpenNi service when the skeleton or hand was lost.  If your forcing the computation of something that isn't there - I could imagine that might cause things to go badly.

Keep us posted.

Cheers

   GroG

Hello Grog,

So i tried to put the initalisation of the services (OpenNi and inmoov) in another script that i execute before using the tracking one, it seems to help a little.

Otherwise here is my result, the code i gave above is not the one used in the video, if this one interests you i'll post it here :)

Cheers!

<iframe width="560" height="315" src="https://www.youtube.com/embed/Hj7x7wHPy40&quot; frameborder="0" allowfullscreen></iframe>

GroG

6 years 8 months ago

In reply to by nicolas taguet

Great Video Nicolas !

I really like the way you mounted the arm.  It reminds me of "Robot Kitchen" - only what you've done is "real" :)

Yes I be interested in the code.  Do you get a big error message when it crashes ?  Hope you don't mind me re-posting your video, it just "too" good :)

Here is the code used to get the different position from the arm

import SimpleOpenNI.ContextWrapper as ContextWrapper

import SimpleOpenNI.SimpleOpenNI as SimpleOpenNI

import SimpleOpenNI.SimpleOpenNIConstants as SimpleOpenNIConstants

from org.myrobotlab.service import OpenNi

import org.myrobotlab.openni.PMatrix3D as PMatrix3D

import org.myrobotlab.openni.PVector as PVector

import org.myrobotlab.openni

import threading

from threading import Thread as Thread

import time

import math

#to use this code you have to execute initArmNi before.

 

def attachArm():

  inmoov.rightArm.bicep.attach("inmoov.right",2)

  inmoov.rightArm.rotate.attach("inmoov.right",3)

  inmoov.rightArm.shoulder.attach("inmoov.right",4)

  inmoov.rightArm.omoplate.attach("inmoov.right",5)

 

def detachArm():

  inmoov.rightArm.bicep.detach()

  inmoov.rightArm.rotate.detach()

  inmoov.rightArm.omoplate.detach()

  inmoov.rightArm.shoulder.detach()

 

def positionRepo():

  inmoov.rightArm.bicep.moveTo(0)

  inmoov.rightArm.rotate.moveTo(100)

  inmoov.rightArm.omoplate.moveTo(90)

  inmoov.rightArm.shoulder.moveTo(90)

 

#variables used to get kinect data declaration.

rShoulderPosition = PVector()

lShoulderPosition = PVector()

rShoulderOrientation = PMatrix3D()

rElbowPosition = PVector()

headPosition = PVector()

torsoPosition = PVector()

lHipPosition = PVector()

rHipPosition = PVector()

rHandPosition = PVector()

 

 

#-----------------------------Function used for calculus--------------------------------

 

def angle(a,b,c):

  angle01 = math.atan2(a.y - b.y, a.x - b.x)

  angle02 = math.atan2(b.y - c.y, b.x - c.x)

  return (angle01 - angle02)

 

def projectToBodyReferential(A,B, vecHip1, vecHip2):

  normeA = math.sqrt(math.pow(A.x - B.x, 2) + math.pow(A.y - B.y, 2) + math.pow(A.z - B.z, 2))

  normeVecHip = math.sqrt(math.pow(vecHip1.x - vecHip2.x, 2) + math.pow(vecHip1.y - vecHip2.y, 2) + math.pow(vecHip1.z - vecHip2.z, 2))

  newAx = (((A.x - B.x)/normeA) * ((vecHip1.x - vecHip2.x)/normeVecHip) + ((A.z - B.z)/normeA) * ((vecHip1.z - vecHip2.z)/normeVecHip))

  newAy = (A.y - B.y)

  newAz = (((A.x - B.x)/normeA) * ((vecHip1.z - vecHip2.z)/normeVecHip) + ((A.z - B.z)/normeA) * ((vecHip1.x - vecHip2.x)/normeVecHip))

  return PVector(newAx,newAy,newAz)

 

def myround(x, base):

  return int(base * round(float(x)/base))

 

def angleBetween(v1, v2):

    dot = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z

    v1mag = math.sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z)

    v2mag = math.sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z)

    return (float)(dot / (v1mag * v2mag))

 

#---------------------------------------------------------------------------------------

#rounding is the variable to round value : if rounding = 2 => the values will be 10 12 14 16 18 20....

#rounding is used to ignore noise on values.

rounding = 1

 

 

#-----------------------------Functions getting arm positio-----------------------------

 

#Function getting from kinect the value for the movement made by the rotate motor.

def rotateToMotor(rElbowPosition, rShoulderPosition, rShoulderOrientation, homoplate):

  repereBicep = PMatrix3D(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1)

  y = PVector(0,1,0)

  normalY = PVector(1,1,1)

  rShoulderOrientation.mult(y,normalY)

 

  vectBicep = PVector(rShoulderPosition.x - rElbowPosition.x, rShoulderPosition.y -  rElbowPosition.y, rShoulderPosition.z - rElbowPosition.z)

  repereBicepX = PVector(repereBicep.m00, repereBicep.m10, repereBicep.m20)

  repereBicepY = PVector(repereBicep.m01, repereBicep.m11, repereBicep.m21)

  repereBicepZ = PVector(repereBicep.m02, repereBicep.m12, repereBicep.m22)

  repereBicepAxis = [repereBicepX, repereBicepY, repereBicepZ]

 

  i = 2

  u = repereBicepAxis[i].cross(vectBicep)

  det = PMatrix3D(vectBicep.x, vectBicep.y, vectBicep.z, 0, repereBicepAxis[i].x, repereBicepAxis[i].y, repereBicepAxis[i].z, 0, u.x, u.y, u.z, 0, 0, 0, 0, 1).determinant()

  rotateAxisU = math.copysign(PVector.angleBetween(vectBicep, repereBicepAxis[i]), -det)

  repereBicep.rotate(rotateAxisU, u.x, u.y, u.z)

 

  repereBicepX = PVector(repereBicep.m00, repereBicep.m10, repereBicep.m20)

  repereBicepY = PVector(repereBicep.m01, repereBicep.m11, repereBicep.m21)

  repereBicepZ = PVector(repereBicep.m02, repereBicep.m12, repereBicep.m22)

  repereBicepAxis = [repereBicepX, repereBicepY, repereBicepZ]

  value = 180 - int((angleBetween(normalY, repereBicepY)*30+18)*3.8)

  value = int(value + homoplate/2)

#print "test normalY bicepY is : "+str(value)

  if value < 0:

    return 0

  elif value > 180:

    return 180

  else:return myround(value, 5)

 

#Function getting from kinect the value for the movement made by the omoplate motor.

def homoplateToMotor(rShoulderPosition, rElbowPosition, lHipPosition, rHipPosition):

  value = projectToBodyReferential(rShoulderPosition, rElbowPosition, lHipPosition, rHipPosition).x

  value = (value*100-20)*(1.3)+60

  #value = (value*100-20)*(1.1)+70

  if value < 90:

    return 90

  elif value > 180:

    return 180

  else: return myround(value, rounding)

 

#Function getting from kinect the value for the movement made by the shoulder motor.

def shoulderToMotor(rElbowPosition, rShoulderPosition, rShoulderOrientation):

  repere = PMatrix3D(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1)

  x = PVector(1,0,0)

  normalX = PVector(1,1,1)

  rShoulderOrientation.mult(x,normalX)

  repereZ = PVector(repere.m02, repere.m12, repere.m22)

  value = 180-int(((angleBetween(normalX, repereZ)+1)*10)*9.5)

  if value < 0:

    return 0

  elif value > 180:

    return 180

  else:return myround(value, rounding)

 

#Function getting from kinect the value for the movement made by the bicep motor.

def bicepToMotor(vectBicep, vectForeArm):

  value = angleBetween(vectBicep, vectForeArm)*100+90

  if value < 0 :

    return 0

  elif value > 90 :

    return 90

  else:return myround(value, 1)

 

#---------------------------------------------------------------------------------------

 

#function called by the scheduler to get the joint's data and set the motor to the right value

def updatePosition(myWrapper, user):

  if myWrapper.isTrackingSkeleton(user):

    lHip = SimpleOpenNIConstants.SKEL_LEFT_HIP

    myWrapper.getJointPositionSkeleton(user,lHip ,lHipPosition)

 

    rHip = SimpleOpenNIConstants.SKEL_RIGHT_HIP

    myWrapper.getJointPositionSkeleton(user,rHip ,rHipPosition)

 

    head = SimpleOpenNIConstants.SKEL_HEAD

    myWrapper.getJointPositionSkeleton(user,head ,headPosition)

 

    torso = SimpleOpenNIConstants.SKEL_TORSO

    myWrapper.getJointPositionSkeleton(user,torso ,torsoPosition)

 

    rShoulder = SimpleOpenNIConstants.SKEL_RIGHT_SHOULDER

    myWrapper.getJointPositionSkeleton(user,rShoulder ,rShoulderPosition)

    probaShoulderOrientation = myWrapper.getJointOrientationSkeleton(user,rShoulder ,rShoulderOrientation)

 

    lShoulder = SimpleOpenNIConstants.SKEL_LEFT_SHOULDER

    myWrapper.getJointPositionSkeleton(user,lShoulder ,lShoulderPosition)

 

    rElbow = SimpleOpenNIConstants.SKEL_RIGHT_ELBOW

    probaElbowPosition = myWrapper.getJointPositionSkeleton(user,rElbow ,rElbowPosition)

 

    rHand = SimpleOpenNIConstants.SKEL_RIGHT_HAND

    probaHandPosition = myWrapper.getJointPositionSkeleton(user,rHand ,rHandPosition)

 

 

    vectBicep = PVector(rElbowPosition.x - rShoulderPosition.x, rElbowPosition.y - rShoulderPosition.y, rElbowPosition.z - rShoulderPosition.z)

    vectForeArm = PVector(rElbowPosition.x - rHandPosition.x, rElbowPosition.y - rHandPosition.y, rElbowPosition.z - rHandPosition.z)

 

    if probaShoulderOrientation >= 0.5 and probaHandPosition >= 0.5 and probaElbowPosition >= 0.5:

      try:

        bicep = bicepToMotor(vectBicep, vectForeArm)

        homoplate = homoplateToMotor(rShoulderPosition, rElbowPosition, lHipPosition, rHipPosition)

        rotate = rotateToMotor(rElbowPosition, rShoulderPosition, rShoulderOrientation, homoplate)

        shoulder = shoulderToMotor(rElbowPosition, rShoulderPosition, rShoulderOrientation)

        print "angle bicep is : "+str(int(bicep))

        #inmoov.rightArm.bicep.moveTo(int(bicep))

        print "angle rotate : "+str(int(rotate))

        #inmoov.rightArm.rotate.moveTo(int(rotate))

        print "angle homoplate : "+str(homoplate)

        #inmoov.rightArm.omoplate.moveTo(int(homoplate))

        print "angle shoulder : "+str((shoulder))

        #inmoov.rightArm.shoulder.moveTo(shoulder)

        print ""

      except Exception:

        pass

  else:

    print "user lost"

 

#function used to itialise simpleOpenNi wrapper.

#The wrapper is the object allowing us to use simpleOpenNi

def init():

  print "start initialise"

  users = []

  image = []

  myWrapper = SimpleOpenNI(ni)

  myWrapper.setMirror(True)

  myWrapper.enableIR(640, 640, 30)

  myWrapper.enableDepth(640, 640, 30)

  myWrapper.enableUser()

  print "initialised : "+str(myWrapper.isInit())

  if myWrapper.isInit():

    return myWrapper

  else:

    return Exception

 

#Scheduler is finding someone and the calling the update position with period specified

def schedule(myWrapper, duree, periode):

  print "start schedule"

  i = 1

  userFound = False

  user = False

  while i <= 100:

  try:

    user = myWrapper.getUsers()[0]

    myWrapper.startTrackingSkeleton(user)

    print "user found"

    userFound = True

    i = 101

  except Exception:

    myWrapper.update()

    i=i+1

    print i

    sleep(0.1)

  if i >= 100 and userFound == False:

    print "failed to find user"

    return

 

  startingTime = time.time()

  lastTime = time.time() - startingTime

  while (time.time() - startingTime) < duree: 

    if (time.time() - lastTime - startingTime) > periode:

      updatePosition(myWrapper, user)

      myWrapper.update()

      lastTime = time.time() - startingTime

  myWrapper.stopTrackingSkeleton(user)

  #myWrapper.close()

  print "stop schedule"

 

inmoov.rightArm.omoplate.setMinMax(90,180)

inmoov.rightArm.setVelocity(-1,-1,-1,-1)

#sleep(0.1)

#positionRepo()

#attachArm()

#sleep(2)

a = init()

#sleep(2)

#schedule(a, 10, 0.001)

#sleep(0.1)

#positionRepo()

#sleep(4)

#detachArm()

#print "stop"

 

#---------------------------------------------------------------------------------------

 

#-----------------------------Using udp to control the code-----------------------------

 

import socket

import threading

from threading import Thread as Thread

 

UDP_IP = "127.0.0.1"

UDP_PORT = 5019

 

#receiver, if receive track start tracking

def receive(ip, port, myWrapper):

  living = True

  UDP_IP = ip

  UDP_PORT = port

 

  sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

  sock.bind((UDP_IP, UDP_PORT))

  while living:

    print "is working"

    data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes

    print "received message:", data

    if data == "stop":

      living = False

    elif data == "track":

      print "start track"

      positionRepo()

      attachArm()

      sleep(2)

      schedule(myWrapper, 10, 0.001)

      sleep(0.1)

      positionRepo()

      sleep(4)

      detachArm()

      print "stop track"

 

Thread(target=lambda: receive(UDP_IP,UDP_PORT, a)).start()

 

import socket

 

UDP_IP = "127.0.0.1"

UDP_PORT = 5019

MESSAGE = "track"

 

#print "UDP target IP:", UDP_IP

#print "UDP target port:", UDP_PORT

#print "message:", MESSAGE

 

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # interet,UDP

sock.sendto(MESSAGE, (UDP_IP, UDP_PORT))

It was crashing because i was initiating a simpleOpenNi wrapper at every execution, now i create one and i control the tracking via udp and it does not crash anymore