Kinect Code

using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using System.Windows;

using System.Windows.Controls;

using System.Windows.Data;

using System.Windows.Documents;

using System.Windows.Input;

using System.Windows.Media;

using System.Windows.Media.Imaging;

using System.Windows.Navigation;

using System.Windows.Shapes;

using Microsoft.Kinect;

using System.Collections;

using System.IO.Ports;

 

public class kinect1

{

public kinect1()

{

InitializeComponent();

#include <kdl/frames.hpp> // For KDL::Vector, KDL:Rotation, etc

XnFloat* m; // Temporary variable for accessing matrix elements

// Get torso xyz position

XnSkeletonJointPosition xn_pos_torso;

UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_TORSO, xn_pos_torso);

KDL::Vector torso(joint_pos_torso.position.X, joint_pos_torso.position.Y, joint_pos_torso.position.Z);

// Get torso rpy rotation

XnSkeletonJointOrientation xn_orient_torso;

UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_TORSO, xn_orient_torso);

m = xn_orient_torso.orientation.elements;

KDL::Rotation torso_rotation(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);

//Get right shoulder xyz position

XnSkeletonJointPosition xn_pos_r_shoulder;

UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, xn_pos_r_shoulder);

KDL::Vector right_shoulder(xn_pos_r_shoulder.position.X, xn_pos_r_shoulder.position.Y, xn_pos_r_shoulder.position.Z);

// Get left shoulder xyz position

XnSkeletonJointPosition xn_pos_l_shoulder;

UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, xn_pos_l_shoulder);

KDL::Vector left_shoulder(xn_pos_l_shoulder.position.X, xn_pos_l_shoulder.position.Y, xn_pos_l_shoulder.position.Z);

// Get left shoulder rpy rotation

XnSkeletonJointOrientation xn_orient_l_shoulder;

UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_LEFT_SHOULDER, xn_orient_l_shoulder);

m = xn_orient_l_shoulder.orientation.elements;

KDL::Rotation left_shoulder_rotation(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);

//Get left elbow xyz position’

XnSkeletonJointPosition xn_pos_l_elbow;

UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, xn_pos_l_elbow);

KDL::Vector left_elbow(xn_pos_l_elbow.position.X, xn_pos_l_elbow.position.Y, xn_pos_l_elbow.position.Z);

// Get left hand xyz position

XnSkeletonJointPosition xn_pos_l_hand;

UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HAND, xn_pos_l_hand);

KDL::Vector left_hand(xn_pos_l_hand.position.X, xn_pos_l_hand.position.Y, xn_pos_l_hand.position.Z);

// We need this inverse rotation to correct all of the other joints’ XYZ and RPY into a torso relative space.

KDL::Rotation torso_rotation_inverse = torso_rotation.Inverse();

// Right shoulder position – remove torso rotation and translation

right_shoulder = right_shoulder – torso;

right_shoulder = torso_rotation_inverse * right_shoulder;

// Left shoulder rotation – remove torso rotation and extract angles

left_shoulder_rotation = left_shoulder_rotation * torso_rotation_inverse;

double left_shoulder_roll, left_shoulder_pitch, left_shoulder_yaw;

left_shoulder_rotation.GetRPY(left_shoulder_roll, left_shoulder_pitch, left_shoulder_yaw);

// Left shoulder position – remove torso rotation and translation

left_shoulder = left_shoulder – torso;

left_shoulder = torso_rotation_inverse * left_shoulder;

// Left elbow position – remove torso rotation and translation

left_elbow = left_elbow – torso;

left_elbow = torso_rotation_inverse * left_elbow;

// Left hand position – remove torso rotation and translation

left_hand = left_hand – torso;

left_hand = torso_rotation_inverse * left_hand;

// Now all of our data considers their (X,Y,Z) and (R,P,Y) to have their (0,0,0) at the torso rather than at the Kinect camera.

// These coordinates are now ready to process to create useful joint angles to send to a robot.

// The following vectors are normalized so that when we create triangles out of them we can treat them as being within the unit circle, and the trigonometry is simplified.

// These first two vectors have the shoulder as the origin. Useful for calculating left shoulder pitch and roll

KDL::Vector left_shoulder_right_shoulder = right_shoulder – left_shoulder;

left_shoulder_right_shoulder.Normalize();

KDL::Vector left_shoulder_elbow = left_elbow – left_shoulder;

left_shoulder_elbow.Normalize();

// These last two vectors have the elbow as the origin. Useful for calculating the elbow angle.

KDL::Vector left_elbow_shoulder = left_shoulder – left_elbow;

left_elbow_shoulder.Normalize();

KDL::Vector left_elbow_hand = left_hand – left_elbow;

left_elbow_hand.Normalize();

static double robot_left_shoulder_angle_roll = 0;

if (xn_pos_r_shoulder.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9 && xn_pos_l_shoulder.fConfidence >= 0.9)

{

// Calculate the angle between two vectors. By comparing the left elbow, left shoulder, and right shoulder positions, we can get the inner angle.

robot_left_shoulder_angle_roll = acos(KDL.dot(left_shoulder_elbow, left_shoulder_right_shoulder));

// Shift the angle to consider arms down as zero position

robot_left_shoulder_angle_roll = robot_left_shoulder_angle_roll – HALFPI;

}

static double robot_left_shoulder_angle_pitch = 0;

// static: if a confidence check fails, previous iteration’s value is used.

if (xn_pos_l_shoulder.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9)

{

// Remember, Y from the Kinect coordinate system is actually Z on the robot.

// This considers a triangle where the hypotenuse is your arm.

// We want to take the inverse sign of the forward component of your arm to calculate the angle that we should send to the robot’s pitch joint.

robot_left_shoulder_angle_pitch = asin(left_shoulder_elbow.y());

// Shift the angle to consider arms down as zero position

robot_left_shoulder_angle_pitch = robot_left_shoulder_angle_pitch + HALFPI;

}

// left shoulder yaw

static double left_shoulder_angle_yaw = 0;

if (xn_pos_l_shoulder.fConfidence >= 0.9)

{

// In this case, we can take the rotation directly from the Kinect data.

// However, this method isn’t very reliable.

// The rotational information for joints frmo NITE is very unreliable.

// In certain positions of the user’s arms, this method will fail.

// I suggest that you try to make a better trigonometric solution.

// Remember, robot yaw = kinect roll because robot Z = kinect Y

left_shoulder_angle_yaw = left_shoulder_roll;

}

static double robot_left_elbow_angle = 0;

if (xn_pos_l_hand.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9 && xn_pos_l_shoulder.fConfidence >= 0.9)

{

// calculate the angle between two vectors.

// by comparing the left shoulder, left elbow, and left hand positions, we can get the inner angle

robot_left_elbow_angle = acos(KDL.dot(left_elbow_hand, left_elbow_shoulder));

// Shift the angle to consider arms down as zero position

robot_left_elbow_angle = left_elbow_angle – PI;

}

}

}