› › ›

Kinect skeleton tracking robot arm questions.

LemonjaminLemonjamin
edited February 2014 in Kinect

So i have this skeleton tracking code on processing, and my goal is to hook that up to my robot arm through arduino. I am already able to control the arm with processing code and uploaded arduino code. Here's the code: Processing test code

import processing.serial.*; PFont font; int pwmValueOne, pwmValueTwo; Serial mySerial; int serialIndex = 0; int motorOneMouse, motorTwoMouse; void setup() { font = createFont("Arial", 32); size(600, 345); mySerial = new Serial(this, Serial.list()[serialIndex], 9600); } // Motor Pair 1: Motor 3 and 4 // Motor Pair 2: Motor 1 and 2 void draw() { background(200); line(300, 0, 300, 345); //The operating window for the pair one motor will be within Y: 89 and 346, X: 0, and 300 //We want to update the speed when it is within the operating range //We want to reset the speed when it is rolled over //We want to save the speed when out of range //Drawing the cursor position is independent of the state if ((mouseY > 90) && (mouseX < 300)) { pwmValueOne = int(constrain(map(mouseY, 95, 340, 255, -255), -255, 255)); // mapping the mouse value to acceptable speed values fill(0); //filling the rectangle differently if outside } else if ((mouseY <= 90) && (mouseX < 300)) { pwmValueOne = 0; fill(70); //filling the rectangle like this if inside } else { fill(0); } motorOneMouse = int(map(pwmValueOne, -255, 255, 340, 95)); line(0, motorOneMouse, 300, motorOneMouse); rect(0, 0, 300, 90); textAlign(LEFT, TOP); textFont(font, 32); fill(255); text(" Motor Pair 1", 9, 9); textFont(font, 16); text("Current Speed: " + str(pwmValueOne), 9, 54); fill(255, 0, 0); text("Rollover to Reset", 160, 54); //------------------------ The same code for the right side if ((mouseY > 89) && (mouseX > 300)) { pwmValueTwo = int(constrain(map(mouseY, 95, 335, 255, -255), -255, 255)); // mapping the mouse value to acceptable speed values fill(0); //filling the rectangle differently if outside } else if ((mouseY <= 90) && (mouseX > 300)) { pwmValueTwo = 0; fill(70); //filling the rectangle like this if inside } else { fill(0); } motorTwoMouse = int(map(pwmValueTwo, -255, 255, 340, 95)); line(300, motorTwoMouse, 600, motorTwoMouse); rect(300, 0, 600, 90); textAlign(LEFT, TOP); textFont(font, 32); fill(255); text(" Motor Pair 2", 309, 9); textFont(font, 16); text("Current Speed: " + str(pwmValueTwo), 309, 54); fill(255, 0, 0); text("Rollover to Reset", 460, 54); //Printing to the Serial Port mySerial.write(str(pwmValueOne)); //motor 3 mySerial.write("#"); mySerial.write(str(pwmValueOne)); //motor 4 mySerial.write("$"); mySerial.write(str(pwmValueTwo)); //motor 1 mySerial.write("!"); } Arduino Code: (#) include <AFMotor.h> //remove paranthesis char inChar; //variable to hold incoming serial byte String inputString; //String to hold one "Strung" number long speedOnThree; //the four different variables long speedOnFour; // long speedOnOne; // AF_DCMotor motorOnThree(3); AF_DCMotor motorOnFour(4); AF_DCMotor motorOnOne(1, MOTOR12_1KHZ); void setup() { Serial.begin(9600); //Initialize Serial Port motorOnThree.run(RELEASE); motorOnFour.run(RELEASE); } void loop() { if(Serial.available()) { //If there is a byte waiting in the serial buffer inChar = Serial.read(); //read that byte, store it in a variable inputString += inChar; //Concatenate the byte with the inputString, // which will be converted into a number /* How "Terminators" correspond with motors: '!' > Motor 1 '/n' > Motor 2 '#' > Motor 3 '$' > Motor 4 */ if((inChar == '#') || (inChar == '!') || (inChar == '$')) { //if the inChar is one of the separators long number = inputString.toInt();//convert the string to an long, store it if(inChar == '#') { //if inChar is a hash, then store it in speedOnThree speedOnThree = number; inputString = ""; //clear the input string, for the accumulation of the next number } else if (inChar == '$') { speedOnFour = number; inputString = ""; //clear the input string, for the accumulation of the next number } else if (inChar == '!') { speedOnOne = number; inputString = ""; //clear the input string, for the accumulation of the next number } } } setMot(motorOnThree, speedOnThree); setMot(motorOnFour, speedOnFour); setMot(motorOnOne, speedOnOne); } void setMot(AF_DCMotor motor, long theSpeed) { if(theSpeed < 0) { motor.run(BACKWARD); } else { motor.run(FORWARD); } motor.setSpeed(constrain(abs(theSpeed), 0, 255)); } So as you can see, the arduino's code only supplies commands for the arduino to follow when given specific pwn inputs. The processing code just sends pwn outputs to the arduino based on the position of the mouse. All I am trying to do is incorporate the pwn outputs into my kinect code

Heres the kinect code: import processing.serial.*; import SimpleOpenNI.*; SimpleOpenNI kinect; float zoomF =0.5f; float rotX = radians(180); // by default rotate the hole scene 180deg around the x-axis, // the data from openni comes upside down float rotY = radians(0); boolean autoCalib=true; int pwmValueOne, pwmValueTwo; Serial mySerial; int serialIndex = 0; PVector bodyCenter = new PVector(); PVector bodyDir = new PVector(); PVector com = new PVector();
PVector com2d = new PVector();
color[] userClr = new color[]{ color(255,0,0), color(0,255,0), color(0,0,255), color(255,255,0), color(255,0,255), color(0,255,255) };
void setup() { size(1024,768,P3D); // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem kinect = new SimpleOpenNI(this); if(kinect.isInit() == false) { println("Can't init SimpleOpenNI, maybe the camera is not connected!"); exit(); return;
} mySerial = new Serial(this, Serial.list()[serialIndex], 9600); // disable mirror kinect.setMirror(false); // enable depthMap generation kinect.enableDepth(); // enable skeleton generation for all joints kinect.enableUser(); stroke(255,255,255); smooth();
perspective(radians(45), float(width)/float(height), 10,150000); } void draw() { // update the cam kinect.update(); background(0,0,0); // set the scene pos translate(width/2, height/2, 0); rotateX(rotX); rotateY(rotY); scale(zoomF); int[] depthMap = kinect.depthMap(); int[] userMap = kinect.userMap(); int steps = 3; // to speed up the drawing, draw every third point int index; PVector realWorldPoint; translate(0,0,-1000); // set the rotation center of the scene 1000 infront of the camera // draw the pointcloud beginShape(POINTS); for(int y=0;y < kinect.depthHeight();y+=steps) { for(int x=0;x < kinect.depthWidth();x+=steps) { index = x + y * kinect.depthWidth(); if(depthMap[index] > 0) { // draw the projected point realWorldPoint = kinect.depthMapRealWorld()[index]; if(userMap[index] == 0) stroke(100); else stroke(userClr[ (userMap[index] - 1) % userClr.length ]);
point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z); } } } endShape(); /if ((jointPos1 > 90) && (jointPos2 < 300)) { pwmValueOne = int(constrain(map(mouseY, 95, 340, 255, -255), -255, 255)); }/ // draw the skeleton if it's available int[] userList = kinect.getUsers(); for(int i=0;i<userList.length;i++) { if(kinect.isTrackingSkeleton(userList[i])) drawSkeleton(userList[i]); // draw the center of mass if(kinect.getCoM(userList[i],com)) { stroke(100,255,0); strokeWeight(1); beginShape(LINES); vertex(com.x - 15,com.y,com.z); vertex(com.x + 15,com.y,com.z); vertex(com.x,com.y - 15,com.z); vertex(com.x,com.y + 15,com.z); vertex(com.x,com.y,com.z - 15); vertex(com.x,com.y,com.z + 15); endShape(); fill(0,255,100); text(Integer.toString(userList[i]),com.x,com.y,com.z); }
}
// draw the kinect cam kinect.drawCamFrustum(); } // draw the skeleton with the selected joints void drawSkeleton(int userId) { strokeWeight(3); // to get the 3d joint data drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK); drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER); drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW); drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND); drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER); drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW); drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND); drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO); drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO); drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP); drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE); drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT); drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP); drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE); drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);
// draw body direction getBodyDirection(userId,bodyCenter,bodyDir); bodyDir.mult(200); // 200mm length bodyDir.add(bodyCenter); stroke(255,200,200); line(bodyCenter.x,bodyCenter.y,bodyCenter.z, bodyDir.x ,bodyDir.y,bodyDir.z); strokeWeight(1); } PVector GlobalVec = new PVector(); void drawLimb(int userId,int jointType1,int jointType2) { PVector jointPos1 = new PVector(); PVector jointPos2 = new PVector(); float confidence; // draw the joint position confidence = kinect.getJointPositionSkeleton(userId,jointType1,jointPos1); confidence = kinect.getJointPositionSkeleton(userId,jointType2,jointPos2); stroke(255,0,0,confidence * 200 + 55); line(jointPos1.x,jointPos1.y,jointPos1.z,jointPos2.x,jointPos2.y,jointPos2.z); PVector limb = PVector.sub(jointPos2, jointPos1); float myAngleOf = PVector.angleBetween(limb, jointPos1); println(jointPos1, jointPos2,limb,GlobalVec); drawJointOrientation(userId,jointType1,jointPos1,50); } float angleOf(PVector one, PVector two, PVector axis) { PVector limb = PVector.sub(two, one); return degrees(PVector.angleBetween(limb, axis)); } void drawJointOrientation(int userId,int jointType,PVector pos,float length) { // draw the joint orientation
PMatrix3D orientation = new PMatrix3D(); float confidence = kinect.getJointOrientationSkeleton(userId,jointType,orientation); if(confidence < 0.001f) // nothing to draw, orientation data is useless return; pushMatrix(); println("before: x " + pos.x + " y " + pos.y + " z " + pos.z); pos.x = pos.x * 2; translate(pos.x ,pos.y,pos.z); println("after: x " + pos.x + " y " + pos.y + " z " + pos.z); // set the local coordsys applyMatrix(orientation); // coordsys lines are 100mm long // x - r stroke(255,0,0,confidence * 200 + 55); line(0,0,0, length,0,0); // y - g stroke(0,255,0,confidence * 200 + 55); line(0,0,0, 0,length,0); // z - b
stroke(0,0,255,confidence * 200 + 55); line(0,0,0, 0,0,length); popMatrix(); } Part 2 of code (still part of it, it just wont format correctly) // ----------------------------------------------------------------- // SimpleOpenNI user events void onNewUser(SimpleOpenNI curkinect,int userId) { println("onNewUser - userId: " + userId); println("\tstart tracking skeleton"); kinect.startTrackingSkeleton(userId); } void onLostUser(SimpleOpenNI curkinect,int userId) { println("onLostUser - userId: " + userId); } void onVisibleUser(SimpleOpenNI curkinect,int userId) { //println("onVisibleUser - userId: " + userId); } // ----------------------------------------------------------------- // Keyboard events void keyPressed() { switch(key) { case ' ': kinect.setMirror(!kinect.mirror()); break; } switch(keyCode) { case LEFT: rotY += 0.1f; break; case RIGHT: // zoom out rotY -= 0.1f; break; case UP: if(keyEvent.isShiftDown()) zoomF += 0.01f; else rotX += 0.1f; break; case DOWN: if(keyEvent.isShiftDown()) { zoomF -= 0.01f; if(zoomF < 0.01) zoomF = 0.01; } else rotX -= 0.1f; break; } } void getBodyDirection(int userId,PVector centerPoint,PVector dir) { PVector jointL = new PVector(); PVector jointH = new PVector(); PVector jointR = new PVector(); float confidence; // draw the joint position confidence = kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER,jointL); confidence = kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_HEAD,jointH); confidence = kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_SHOULDER,jointR); // take the neck as the center point confidence = kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,centerPoint); /* // manually calc the centerPoint PVector shoulderDist = PVector.sub(jointL,jointR); centerPoint.set(PVector.mult(shoulderDist,.5)); centerPoint.add(jointR); */ PVector up = PVector.sub(jointH,centerPoint); PVector left = PVector.sub(jointR,centerPoint); dir.set(up.cross(left)); dir.normalize(); } Suggestions welcome on where I can incorporate pwn outputs. If you need any other information, just ask. Thank you

Tagged:

Answers

Sign In or Register to comment.