AAD_Kinect_UR_Robot

import processing.net.*;
import kinect4WinSDK.*;

Kinect kinect;
ArrayList <SkeletonData> bodies;
PVector[] jointVecs = new PVector[4];

////////////////////////
////// S E T U P ///////
////////////////////////

float[] distanceFromKinect = {1.2, 2.4};                    // How far from the Kinect do you want to track things? (in meters)

// Big Robot Settings:
URobot bigRobot;
String bigRobotIP = "192.168.9.2";
int bigRobotJoint = Kinect.NUI_SKELETON_POSITION_HAND_LEFT; // Which joint do you want to track?
float[] bigRobot_X_limits = {-0.9, 0.9};                    // Limits of the robot on the X axis
float[] bigRobot_Y_limits = {-0.3, -0.8};                    // Limits of the robot on the Y axis
float[] bigRobot_Z_limits = {0.2, 1.2};                     // Limits of the robot on the Z axis

//Small Robot Settings:
URobot smallRobot;
String smallRobotIP = "192.168.9.1";
int smallRobotJoint = Kinect.NUI_SKELETON_POSITION_HAND_RIGHT; // Which joint do you want to track?
float[] smallRobot_X_limits = {-0.6, 0.6};                     // Limits of the robot on the X axis
float[] smallRobot_Y_limits = {-0.1, -0.6};                     // Limits of the robot on the Y axis
float[] smallRobot_Z_limits = {0.2, 0.75};                      // Limits of the robot on the Z axis

////////////////////////
////// S E T U P ///////
////////////////////////

void setup() {
  //size(640, 480);
  fullScreen();

  bigRobot = new URobot(this, bigRobotIP, 30003);
  bigRobot.movej(bigRobot.homePosition, 0.6);
  smallRobot = new URobot(this, smallRobotIP, 30003);
  smallRobot.movej(smallRobot.homePosition, 0.6);

  kinect = new Kinect(this);
  bodies = new ArrayList<SkeletonData>();

  for (int i = 0; i < jointVecs.length; i++) {
    jointVecs[i] = new PVector(width/2, height/2);
  }
}

void draw() {
  background(0);
  sendKinectPoint(smallRobot, smallRobotJoint, #FF00C0);
  sendKinectPoint(bigRobot, bigRobotJoint, #B6FF00);
  drawKinectBodies();
}

void sendKinectPoint(URobot r, int skeletonJoint, color col) {
  if (bodies.size() > 0) {
    SkeletonData s = bodies.get(0);
    if (s.skeletonPositionTrackingState[skeletonJoint] != Kinect.NUI_SKELETON_POSITION_NOT_TRACKED) {
      if(skeletonJoint == Kinect.NUI_SKELETON_POSITION_HAND_LEFT) {
        PVector kinectpt = s.skeletonPositions[skeletonJoint];
        jointVecs[0] = new PVector(map(kinectpt.x * width, 0, width, bigRobot_X_limits[0], bigRobot_X_limits[1]),
                                   map(kinectpt.y * height, 0, height, bigRobot_Z_limits[1], bigRobot_Z_limits[0]),
                                   map(kinectpt.z, distanceFromKinect[0] * 10000, distanceFromKinect[1] * 10000, bigRobot_Y_limits[0], bigRobot_Y_limits[1]));

        float dist = PVector.dist(jointVecs[0], jointVecs[1]);

        if (dist > 0.01) {
          r.movej(new Pose(jointVecs[0].x, jointVecs[0].z, jointVecs[0].y, PI/2, 0, 0), 0.4);
        }
        drawCircle(new PVector(kinectpt.x * width, kinectpt.y * height), col);
        jointVecs[1] = jointVecs[0].copy(); println("left hand: " + jointVecs[0]);
      }
      if(skeletonJoint == Kinect.NUI_SKELETON_POSITION_HAND_RIGHT) {
        PVector kinectpt = s.skeletonPositions[skeletonJoint];
        jointVecs[2] = new PVector(map(kinectpt.x * width, 0, width, smallRobot_X_limits[0], smallRobot_X_limits[1]),
                                   map(kinectpt.y * height, 0, height, smallRobot_Z_limits[1], smallRobot_Z_limits[0]),
                                   map(kinectpt.z, distanceFromKinect[0] * 10000, distanceFromKinect[1] * 10000, smallRobot_Y_limits[0], smallRobot_Y_limits[1]));

        float dist = PVector.dist(jointVecs[2], jointVecs[3]);

        if (dist > 0.01) {
          r.movej(new Pose(jointVecs[2].x, jointVecs[2].z, jointVecs[2].y, PI/2, 0, 0), 0.4);
        }
        drawCircle(new PVector(kinectpt.x * width, kinectpt.y * height), col);
        jointVecs[3] = jointVecs[2].copy(); println("right hand: " + jointVecs[2]);
      }
    }
  }
}

void drawCircle(PVector loc, color col) {
  pushStyle();
  noStroke();
  fill(col);
  ellipse(loc.x, loc.y, 60, 60);
  popStyle();
}

void drawKinectBodies() {
  for (int i=0; i<bodies.size (); i++)
  {
    drawSkeleton(bodies.get(i));
    drawPosition(bodies.get(i));
  }
}

public String movec(Pose p1, Pose p2) {
  return "movec(" + p1.pose + ", " + p2.pose + ")\n";
}

public String movej(Pose p) {
  return "movej(" + p.pose + ")\n";
}

public String movej(JointPose p) {
  return "movej(" + p.pose + ")\n";
}

public String movel(Pose p) {
  return "movel(" + p.pose + ")\n";
}

public String movel(JointPose p) {
  return "movel(" + p.pose + ")\n";
}

public String movep(Pose p) {
  return "movep(" + p.pose + ")\n";
}

public String movep(JointPose p) {
  return "movep(" + p.pose + ")\n";
}

class Pose {
  String pose;

  Pose(float x, float y, float z, float rx, float ry, float rz) {
    pose = String.format("p[%.9s, %.9s, %.9s, %.9s, %.9s, %.9s]", x, y, z, rx, ry, rz);
  }

  Pose(PVector p1, PVector p2) {
  }

  String interpolate_pose(Pose p, float alpha) {
    return "interpolate_pose(" + pose + ", " + p.pose + ", " + alpha + ")\n";
  }
}

class JointPose {
  String pose;

  JointPose(float x, float y, float z, float rx, float ry, float rz) {
    pose = String.format("[%.9s, %.9s, %.9s, %.9s, %.9s, %.9s]", x, y, z, rx, ry, rz);
  }
}

class URobot {
  Client client;
  JointPose homePosition;

  URobot(PApplet parent, String robotIP, int robotPort) {
    client = new Client(parent, robotIP, robotPort);
    homePosition = new JointPose(0, -PI/2, 0, -PI/2, 0, 0);
  }

  public void movec(Pose p1, Pose p2) {
    client.write("movec(" + p1.pose + ", " + p2.pose + ")\n");
  }

  public void movej(Pose p, float speed) {
    client.write("movej(" + p.pose + ", v=" + speed + ")\n");
  }

  public void movej(JointPose p, float speed) {
    client.write("movej(" + p.pose + ", v=" + speed + ")\n");
  }

  public void movel(Pose p) {
    client.write("movel(" + p.pose + ")\n");
  }

  public void movel(JointPose p) {
    client.write("movel(" + p.pose + ")\n");
  }

  public void movep(Pose p) {
    client.write("movep(" + p.pose + ")\n");
  }

  public void movep(JointPose p) {
    client.write("movep(" + p.pose + ")\n");
  }

  public void set_tcp(Pose p) {
    client.write("set_tcp(" + p.pose + ")\n");
  }
}

void drawPosition(SkeletonData _s) {
  noStroke();
  fill(0, 100, 255);
  String s1 = str(_s.dwTrackingID);
  text(s1, _s.position.x*width, _s.position.y*height);
}

void drawSkeleton(SkeletonData _s) {
  // Body
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_HEAD,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_CENTER);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_CENTER,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_LEFT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_CENTER,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_RIGHT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_CENTER,
    Kinect.NUI_SKELETON_POSITION_SPINE);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_LEFT,
    Kinect.NUI_SKELETON_POSITION_SPINE);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_RIGHT,
    Kinect.NUI_SKELETON_POSITION_SPINE);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SPINE,
    Kinect.NUI_SKELETON_POSITION_HIP_CENTER);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_HIP_CENTER,
    Kinect.NUI_SKELETON_POSITION_HIP_LEFT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_HIP_CENTER,
    Kinect.NUI_SKELETON_POSITION_HIP_RIGHT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_HIP_LEFT,
    Kinect.NUI_SKELETON_POSITION_HIP_RIGHT);

  // Left Arm
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_LEFT,
    Kinect.NUI_SKELETON_POSITION_ELBOW_LEFT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_ELBOW_LEFT,
    Kinect.NUI_SKELETON_POSITION_WRIST_LEFT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_WRIST_LEFT,
    Kinect.NUI_SKELETON_POSITION_HAND_LEFT);

  // Right Arm
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_SHOULDER_RIGHT,
    Kinect.NUI_SKELETON_POSITION_ELBOW_RIGHT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_ELBOW_RIGHT,
    Kinect.NUI_SKELETON_POSITION_WRIST_RIGHT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_WRIST_RIGHT,
    Kinect.NUI_SKELETON_POSITION_HAND_RIGHT);

  // Left Leg
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_HIP_LEFT,
    Kinect.NUI_SKELETON_POSITION_KNEE_LEFT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_KNEE_LEFT,
    Kinect.NUI_SKELETON_POSITION_ANKLE_LEFT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_ANKLE_LEFT,
    Kinect.NUI_SKELETON_POSITION_FOOT_LEFT);

  // Right Leg
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_HIP_RIGHT,
    Kinect.NUI_SKELETON_POSITION_KNEE_RIGHT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_KNEE_RIGHT,
    Kinect.NUI_SKELETON_POSITION_ANKLE_RIGHT);
  DrawBone(_s,
    Kinect.NUI_SKELETON_POSITION_ANKLE_RIGHT,
    Kinect.NUI_SKELETON_POSITION_FOOT_RIGHT);
}

void DrawBone(SkeletonData _s, int _j1, int _j2) {
  noFill();
  stroke(255, 255, 0);
  if (_s.skeletonPositionTrackingState[_j1] != Kinect.NUI_SKELETON_POSITION_NOT_TRACKED &&
    _s.skeletonPositionTrackingState[_j2] != Kinect.NUI_SKELETON_POSITION_NOT_TRACKED) {
    line(_s.skeletonPositions[_j1].x*width,
      _s.skeletonPositions[_j1].y*height,
      _s.skeletonPositions[_j2].x*width,
      _s.skeletonPositions[_j2].y*height);
  }
}

void appearEvent(SkeletonData _s) {
  if (_s.trackingState == Kinect.NUI_SKELETON_NOT_TRACKED)
  {
    return;
  }
  synchronized(bodies) {
    bodies.add(_s);
  }
}

void disappearEvent(SkeletonData _s) {
  synchronized(bodies) {
    for (int i=bodies.size ()-1; i>=0; i--)
    {
      if (_s.dwTrackingID == bodies.get(i).dwTrackingID)
      {
        bodies.remove(i);
      }
    }
  }
}

void moveEvent(SkeletonData _b, SkeletonData _a) {
  if (_a.trackingState == Kinect.NUI_SKELETON_NOT_TRACKED)
  {
    return;
  }
  synchronized(bodies) {
    for (int i=bodies.size ()-1; i>=0; i--)
    {
      if (_b.dwTrackingID == bodies.get(i).dwTrackingID)
      {
        bodies.get(i).copy(_a);
        break;
      }
    }
  }
}