using System;<\/p>\n
using System.Collections.Generic;<\/p>\n
using System.Linq;<\/p>\n
using System.Text;<\/p>\n
using System.Windows;<\/p>\n
using System.Windows.Controls;<\/p>\n
using System.Windows.Data;<\/p>\n
using System.Windows.Documents;<\/p>\n
using System.Windows.Input;<\/p>\n
using System.Windows.Media;<\/p>\n
using System.Windows.Media.Imaging;<\/p>\n
using System.Windows.Navigation;<\/p>\n
using System.Windows.Shapes;<\/p>\n
using Microsoft.Kinect;<\/p>\n
using System.Collections;<\/p>\n
using System.IO.Ports;<\/p>\n
<\/p>\n
public class kinect1<\/p>\n
{<\/p>\n
public kinect1()<\/p>\n
{<\/p>\n
InitializeComponent();<\/p>\n
#include <kdl\/frames.hpp> \/\/ For KDL::Vector, KDL:Rotation, etc<\/p>\n
XnFloat* m; \/\/ Temporary variable for accessing matrix elements<\/p>\n
\/\/ Get torso xyz position<\/p>\n
XnSkeletonJointPosition xn_pos_torso;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_TORSO, xn_pos_torso);<\/p>\n
KDL::Vector torso(joint_pos_torso.position.X, joint_pos_torso.position.Y, joint_pos_torso.position.Z);<\/p>\n
\/\/ Get torso rpy rotation<\/p>\n
XnSkeletonJointOrientation xn_orient_torso;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_TORSO, xn_orient_torso);<\/p>\n
m = xn_orient_torso.orientation.elements;<\/p>\n
KDL::Rotation torso_rotation(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);<\/p>\n
\/\/Get right shoulder xyz position<\/p>\n
XnSkeletonJointPosition xn_pos_r_shoulder;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, xn_pos_r_shoulder);<\/p>\n
KDL::Vector right_shoulder(xn_pos_r_shoulder.position.X, xn_pos_r_shoulder.position.Y, xn_pos_r_shoulder.position.Z);<\/p>\n
\/\/ Get left shoulder xyz position<\/p>\n
XnSkeletonJointPosition xn_pos_l_shoulder;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, xn_pos_l_shoulder);<\/p>\n
KDL::Vector left_shoulder(xn_pos_l_shoulder.position.X, xn_pos_l_shoulder.position.Y, xn_pos_l_shoulder.position.Z);<\/p>\n
\/\/ Get left shoulder rpy rotation<\/p>\n
XnSkeletonJointOrientation xn_orient_l_shoulder;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_LEFT_SHOULDER, xn_orient_l_shoulder);<\/p>\n
m = xn_orient_l_shoulder.orientation.elements;<\/p>\n
KDL::Rotation left_shoulder_rotation(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);<\/p>\n
\/\/Get left elbow xyz position’<\/p>\n
XnSkeletonJointPosition xn_pos_l_elbow;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, xn_pos_l_elbow);<\/p>\n
KDL::Vector left_elbow(xn_pos_l_elbow.position.X, xn_pos_l_elbow.position.Y, xn_pos_l_elbow.position.Z);<\/p>\n
\/\/ Get left hand xyz position<\/p>\n
XnSkeletonJointPosition xn_pos_l_hand;<\/p>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HAND, xn_pos_l_hand);<\/p>\n
KDL::Vector left_hand(xn_pos_l_hand.position.X, xn_pos_l_hand.position.Y, xn_pos_l_hand.position.Z);<\/p>\n
\/\/ We need this inverse rotation to correct all of the other joints’ XYZ and RPY into a torso relative space.<\/p>\n
KDL::Rotation torso_rotation_inverse = torso_rotation.Inverse();<\/p>\n
\/\/ Right shoulder position – remove torso rotation and translation<\/p>\n
right_shoulder = right_shoulder – torso;<\/p>\n
right_shoulder = torso_rotation_inverse * right_shoulder;<\/p>\n
\/\/ Left shoulder rotation – remove torso rotation and extract angles<\/p>\n
left_shoulder_rotation = left_shoulder_rotation * torso_rotation_inverse;<\/p>\n
double left_shoulder_roll, left_shoulder_pitch, left_shoulder_yaw;<\/p>\n
left_shoulder_rotation.GetRPY(left_shoulder_roll, left_shoulder_pitch, left_shoulder_yaw);<\/p>\n
\/\/ Left shoulder position – remove torso rotation and translation<\/p>\n
left_shoulder = left_shoulder – torso;<\/p>\n
left_shoulder = torso_rotation_inverse * left_shoulder;<\/p>\n
\/\/ Left elbow position – remove torso rotation and translation<\/p>\n
left_elbow = left_elbow – torso;<\/p>\n
left_elbow = torso_rotation_inverse * left_elbow;<\/p>\n
\/\/ Left hand position – remove torso rotation and translation<\/p>\n
left_hand = left_hand – torso;<\/p>\n
left_hand = torso_rotation_inverse * left_hand;<\/p>\n
\/\/ 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.<\/p>\n
\/\/ These coordinates are now ready to process to create useful joint angles to send to a robot.<\/p>\n
\/\/ 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.<\/p>\n
\/\/ These first two vectors have the shoulder as the origin. Useful for calculating left shoulder pitch and roll<\/p>\n
KDL::Vector left_shoulder_right_shoulder = right_shoulder – left_shoulder;<\/p>\n
left_shoulder_right_shoulder.Normalize();<\/p>\n
KDL::Vector left_shoulder_elbow = left_elbow – left_shoulder;<\/p>\n
left_shoulder_elbow.Normalize();<\/p>\n
\/\/ These last two vectors have the elbow as the origin. Useful for calculating the elbow angle.<\/p>\n
KDL::Vector left_elbow_shoulder = left_shoulder – left_elbow;<\/p>\n
left_elbow_shoulder.Normalize();<\/p>\n
KDL::Vector left_elbow_hand = left_hand – left_elbow;<\/p>\n
left_elbow_hand.Normalize();<\/p>\n
static double robot_left_shoulder_angle_roll = 0;<\/p>\n
if (xn_pos_r_shoulder.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9 && xn_pos_l_shoulder.fConfidence >= 0.9)<\/p>\n
{<\/p>\n
\/\/ Calculate the angle between two vectors. By comparing the left elbow, left shoulder, and right shoulder positions, we can get the inner angle.<\/p>\n
robot_left_shoulder_angle_roll = acos(KDL.dot(left_shoulder_elbow, left_shoulder_right_shoulder));<\/p>\n
\/\/ Shift the angle to consider arms down as zero position<\/p>\n
robot_left_shoulder_angle_roll = robot_left_shoulder_angle_roll – HALFPI;<\/p>\n
}<\/p>\n
static double robot_left_shoulder_angle_pitch = 0;<\/p>\n
\/\/ static: if a confidence check fails, previous iteration’s value is used.<\/p>\n
if (xn_pos_l_shoulder.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9)<\/p>\n
{<\/p>\n
\/\/ Remember, Y from the Kinect coordinate system is actually Z on the robot.<\/p>\n
\/\/ This considers a triangle where the hypotenuse is your arm.<\/p>\n
\/\/ 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.<\/p>\n
robot_left_shoulder_angle_pitch = asin(left_shoulder_elbow.y());<\/p>\n
\/\/ Shift the angle to consider arms down as zero position<\/p>\n
robot_left_shoulder_angle_pitch = robot_left_shoulder_angle_pitch + HALFPI;<\/p>\n
}<\/p>\n
\/\/ left shoulder yaw<\/p>\n
static double left_shoulder_angle_yaw = 0;<\/p>\n
if (xn_pos_l_shoulder.fConfidence >= 0.9)<\/p>\n
{<\/p>\n
\/\/ In this case, we can take the rotation directly from the Kinect data.<\/p>\n
\/\/ However, this method isn’t very reliable.<\/p>\n
\/\/ The rotational information for joints frmo NITE is very unreliable.<\/p>\n
\/\/ In certain positions of the user’s arms, this method will fail.<\/p>\n
\/\/ I suggest that you try to make a better trigonometric solution.<\/p>\n
\/\/ Remember, robot yaw = kinect roll because robot Z = kinect Y<\/p>\n
left_shoulder_angle_yaw = left_shoulder_roll;<\/p>\n
}<\/p>\n
static double robot_left_elbow_angle = 0;<\/p>\n
if (xn_pos_l_hand.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9 && xn_pos_l_shoulder.fConfidence >= 0.9)<\/p>\n
{<\/p>\n
\/\/ calculate the angle between two vectors.<\/p>\n
\/\/ by comparing the left shoulder, left elbow, and left hand positions, we can get the inner angle<\/p>\n
robot_left_elbow_angle = acos(KDL.dot(left_elbow_hand, left_elbow_shoulder));<\/p>\n
\/\/ Shift the angle to consider arms down as zero position<\/p>\n
robot_left_elbow_angle = left_elbow_angle – PI;<\/p>\n
}<\/p>\n
}<\/p>\n
}<\/p>\n
<\/p>\n
<\/p>\n","protected":false},"excerpt":{"rendered":"
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 […]<\/p>\n","protected":false},"author":9,"featured_media":0,"parent":0,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"","meta":{"footnotes":""},"class_list":["post-69","page","type-page","status-publish","hentry","post-preview"],"_links":{"self":[{"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/pages\/69","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/users\/9"}],"replies":[{"embeddable":true,"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/comments?post=69"}],"version-history":[{"count":1,"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/pages\/69\/revisions"}],"predecessor-version":[{"id":71,"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/pages\/69\/revisions\/71"}],"wp:attachment":[{"href":"https:\/\/ece454designprojects.scranton.edu\/robertdudik\/wp-json\/wp\/v2\/media?parent=69"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}