{"id":40,"date":"2016-05-21T18:22:01","date_gmt":"2016-05-21T22:22:01","guid":{"rendered":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/?page_id=40"},"modified":"2021-05-24T15:08:15","modified_gmt":"2021-05-24T19:08:15","slug":"electronic-portfolio","status":"publish","type":"page","link":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/electronic-portfolio\/","title":{"rendered":"Electronic Portfolio"},"content":{"rendered":"

Below is most of the written work for the robot as well as the code and links to the Kinect libraries we used for reference.\"Resized_20160420_111130\"<\/p>\n

\"IMG_1812\"<\/p>\n

\"IMG_1813\"<\/p>\n

Code:<\/p>\n

using System;<\/div>\n
using System.Collections.Generic;<\/div>\n
using System.Linq;<\/div>\n
using System.Text;<\/div>\n
using System.Windows;<\/div>\n
using System.Windows.Controls;<\/div>\n
using System.Windows.Data;<\/div>\n
using System.Windows.Documents;<\/div>\n
using System.Windows.Input;<\/div>\n
using System.Windows.Media;<\/div>\n
using System.Windows.Media.Imaging;<\/div>\n
using System.Windows.Navigation;<\/div>\n
using System.Windows.Shapes;<\/div>\n
using Microsoft.Kinect;<\/div>\n
using System.Collections;<\/div>\n
using System.IO.Ports;<\/div>\n
<\/div>\n
\/\/namespace kinect1<\/div>\n
\/\/{<\/div>\n
public class kinect1<\/div>\n
{<\/div>\n
public kinect1()<\/div>\n
{<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 InitializeComponent();<\/div>\n
\/\/}<\/div>\n
#include <kdl\/frames.hpp> \/\/ For KDL::Vector, KDL:Rotation, etc<\/div>\n
<\/div>\n
XnFloat* m; \/\/ Temporary variable for accessing matrix elements<\/div>\n
<\/div>\n
\/\/ Get torso xyz position<\/div>\n
XnSkeletonJointPosition xn_pos_torso;<\/div>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_TORSO, xn_pos_torso);<\/div>\n
KDL::Vector torso(joint_pos_torso.position.X, joint_pos_torso.position.Y, joint_pos_torso.position.Z);<\/div>\n
<\/div>\n
\/\/ Get torso rpy rotation<\/div>\n
XnSkeletonJointOrientation xn_orient_torso;<\/div>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_TORSO, xn_orient_torso);<\/div>\n
m = xn_orient_torso.orientation.elements;<\/div>\n
KDL::Rotation torso_rotation(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);<\/div>\n
<\/div>\n
\/\/Get right shoulder xyz position<\/div>\n
XnSkeletonJointPosition xn_pos_r_shoulder;<\/div>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, xn_pos_r_shoulder);<\/div>\n
KDL::Vector right_shoulder(xn_pos_r_shoulder.position.X, xn_pos_r_shoulder.position.Y, xn_pos_r_shoulder.position.Z);<\/div>\n
<\/div>\n
\/\/ Get left shoulder xyz position<\/div>\n
XnSkeletonJointPosition xn_pos_l_shoulder;<\/div>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, xn_pos_l_shoulder);<\/div>\n
KDL::Vector left_shoulder(xn_pos_l_shoulder.position.X, xn_pos_l_shoulder.position.Y, xn_pos_l_shoulder.position.Z);<\/div>\n
<\/div>\n
\/\/ Get left shoulder rpy rotation<\/div>\n
XnSkeletonJointOrientation xn_orient_l_shoulder;<\/div>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_LEFT_SHOULDER, xn_orient_l_shoulder);<\/div>\n
m = xn_orient_l_shoulder.orientation.elements;<\/div>\n
KDL::Rotation left_shoulder_rotation(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);<\/div>\n
<\/div>\n
\/\/Get left elbow xyz position’<\/div>\n
XnSkeletonJointPosition xn_pos_l_elbow;<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, xn_pos_l_elbow);<\/div>\n
KDL::Vector left_elbow(xn_pos_l_elbow.position.X, xn_pos_l_elbow.position.Y, xn_pos_l_elbow.position.Z);<\/div>\n
<\/div>\n
\/\/ Get left hand xyz position<\/div>\n
XnSkeletonJointPosition xn_pos_l_hand;<\/div>\n
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HAND, xn_pos_l_hand);<\/div>\n
KDL::Vector left_hand(xn_pos_l_hand.position.X, xn_pos_l_hand.position.Y, xn_pos_l_hand.position.Z);<\/div>\n
<\/div>\n
\/\/ We need this inverse rotation to correct all of the other joints’ XYZ and RPY into a torso relative space.<\/div>\n
KDL::Rotation torso_rotation_inverse = torso_rotation.Inverse();<\/div>\n
<\/div>\n
\/\/ Right shoulder position – remove torso rotation and translation<\/div>\n
right_shoulder = right_shoulder – torso;<\/div>\n
right_shoulder = torso_rotation_inverse * right_shoulder;<\/div>\n
<\/div>\n
\/\/ Left shoulder rotation – remove torso rotation and extract angles<\/div>\n
left_shoulder_rotation = left_shoulder_rotation * torso_rotation_inverse;<\/div>\n
double left_shoulder_roll, left_shoulder_pitch, left_shoulder_yaw;<\/div>\n
left_shoulder_rotation.GetRPY(left_shoulder_roll, left_shoulder_pitch, left_shoulder_yaw);<\/div>\n
<\/div>\n
\/\/ Left shoulder position – remove torso rotation and translation<\/div>\n
left_shoulder = left_shoulder – torso;<\/div>\n
left_shoulder = torso_rotation_inverse * left_shoulder;<\/div>\n
<\/div>\n
\/\/ Left elbow position – remove torso rotation and translation<\/div>\n
left_elbow = left_elbow – torso;<\/div>\n
left_elbow = torso_rotation_inverse * left_elbow;<\/div>\n
<\/div>\n
\/\/ Left hand position – remove torso rotation and translation<\/div>\n
left_hand = left_hand – torso;<\/div>\n
left_hand = torso_rotation_inverse * left_hand;<\/div>\n
<\/div>\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.<\/div>\n
\/\/ These coordinates are now ready to process to create useful joint angles to send to a robot.<\/div>\n
<\/div>\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.<\/div>\n
\/\/ These first two vectors have the shoulder as the origin. Useful for calculating left shoulder pitch and roll<\/div>\n
<\/div>\n
KDL::Vector left_shoulder_right_shoulder = right_shoulder – left_shoulder;<\/div>\n
left_shoulder_right_shoulder.Normalize();<\/div>\n
<\/div>\n
KDL::Vector left_shoulder_elbow = left_elbow – left_shoulder;<\/div>\n
left_shoulder_elbow.Normalize();<\/div>\n
<\/div>\n
\/\/ These last two vectors have the elbow as the origin. Useful for calculating the elbow angle.<\/div>\n
<\/div>\n
KDL::Vector left_elbow_shoulder = left_shoulder – left_elbow;<\/div>\n
left_elbow_shoulder.Normalize();<\/div>\n
<\/div>\n
KDL::Vector left_elbow_hand = left_hand – left_elbow;<\/div>\n
left_elbow_hand.Normalize();<\/div>\n
<\/div>\n
static double robot_left_shoulder_angle_roll = 0;<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 if (xn_pos_r_shoulder.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9 && xn_pos_l_shoulder.fConfidence >= 0.9)<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 {<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \/\/ Calculate the angle between two vectors. By comparing the left elbow, left shoulder, and right shoulder positions, we can get the inner angle.<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 robot_left_shoulder_angle_roll = acos(KDL.dot(left_shoulder_elbow, left_shoulder_right_shoulder));<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \/\/ Shift the angle to consider arms down as zero position<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 robot_left_shoulder_angle_roll = robot_left_shoulder_angle_roll – HALFPI;<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 }<\/div>\n
<\/div>\n
static double robot_left_shoulder_angle_pitch = 0;<\/div>\n
\/\/ static: if a confidence check fails, previous iteration’s value is used.<\/div>\n
<\/div>\n
if (xn_pos_l_shoulder.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9)<\/div>\n
{<\/div>\n
\u00a0 \u00a0 \/\/ Remember, Y from the Kinect coordinate system is actually Z on the robot.<\/div>\n
\u00a0 \u00a0 \/\/ This considers a triangle where the hypotenuse is your arm.<\/div>\n
\u00a0 \u00a0 \/\/ 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.<\/div>\n
<\/div>\n
\u00a0 \u00a0 robot_left_shoulder_angle_pitch = asin(left_shoulder_elbow.y());<\/div>\n
<\/div>\n
\u00a0 \u00a0 \/\/ Shift the angle to consider arms down as zero position<\/div>\n
<\/div>\n
\u00a0 \u00a0 robot_left_shoulder_angle_pitch = robot_left_shoulder_angle_pitch + HALFPI;<\/div>\n
}<\/div>\n
<\/div>\n
\/\/ left shoulder yaw<\/div>\n
static double left_shoulder_angle_yaw = 0;<\/div>\n
if (xn_pos_l_shoulder.fConfidence >= 0.9)<\/div>\n
{<\/div>\n
\u00a0 \u00a0 \/\/ In this case, we can take the rotation directly from the Kinect data.<\/div>\n
\u00a0 \u00a0 \/\/ However, this method isn’t very reliable.<\/div>\n
\u00a0 \u00a0 \/\/ The rotational information for joints frmo NITE is very unreliable.<\/div>\n
\u00a0 \u00a0 \/\/ In certain positions of the user’s arms, this method will fail.<\/div>\n
\u00a0 \u00a0 \/\/ I suggest that you try to make a better trigonometric solution.<\/div>\n
\u00a0 \u00a0 \/\/ Remember, robot yaw = kinect roll because robot Z = kinect Y<\/div>\n
<\/div>\n
\u00a0 \u00a0 left_shoulder_angle_yaw = left_shoulder_roll;<\/div>\n
}<\/div>\n
<\/div>\n
static double robot_left_elbow_angle = 0;<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 if (xn_pos_l_hand.fConfidence >= 0.9 && xn_pos_l_elbow.fConfidence >= 0.9 && xn_pos_l_shoulder.fConfidence >= 0.9)<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 {<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \/\/ calculate the angle between two vectors.<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \/\/ by comparing the left shoulder, left elbow, and left hand positions, we can get the inner angle<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 robot_left_elbow_angle = acos(KDL.dot(left_elbow_hand, left_elbow_shoulder));<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \/\/ Shift the angle to consider arms down as zero position<\/div>\n
<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 robot_left_elbow_angle = left_elbow_angle – PI;<\/div>\n
\u00a0 \u00a0 \u00a0 \u00a0 }<\/div>\n
<\/div>\n
}<\/div>\n
}<\/div>\n
<\/div>\n
Reference library used for program was KDL and can be found here: https:\/\/github.com\/orocos\/orocos_kinematics_dynamics\/blob\/master\/orocos_kdl\/src\/frames.hpp<\/div>\n
<\/div>\n
Below is photos of the robot program and Kinect program running:<\/div>\n
\"program\" \"robot \"action<\/div>\n","protected":false},"excerpt":{"rendered":"

Below is most of the written work for the robot as well as the code and links to the Kinect libraries we used for reference. 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 […]<\/p>\n","protected":false},"author":6,"featured_media":0,"parent":0,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"","meta":{"footnotes":""},"class_list":["post-40","page","type-page","status-publish","hentry"],"_links":{"self":[{"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/pages\/40","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/users\/6"}],"replies":[{"embeddable":true,"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/comments?post=40"}],"version-history":[{"count":1,"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/pages\/40\/revisions"}],"predecessor-version":[{"id":42,"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/pages\/40\/revisions\/42"}],"wp:attachment":[{"href":"https:\/\/ece454designprojects.scranton.edu\/kinectrobot\/wp-json\/wp\/v2\/media?parent=40"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}