void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e) { SkeletonFrame allSkeletons = e.SkeletonFrame; //get the first tracked skeleton SkeletonData skeleton = (from s in allSkeletons.Skeletons where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault(); if (skeleton != null) { this._skel = skeleton; Joint head = skeleton.Joints[JointID.Head]; HumanSkeleton hs = new HumanSkeleton(skeleton.Joints); NaoSkeleton ns = new NaoSkeleton(hs); UpdateTextBoxes(ns, hs); controller.update(ns); SetLineAngle(RightArmTopDown, Util.radToDeg(hs.RightShoulderYaw)); SetLineAngle(LeftArmTopDown, Util.radToDeg(hs.LeftShoulderYaw)); SetLineAngle(RightArmFront, Util.radToDeg(-hs.RightShoulderPitch) + 90); SetLineAngle(LeftArmFront, Util.radToDeg(hs.LeftShoulderPitch) - 90); SetLineAngle(RightArm, Util.radToDeg(-hs.RightShoulderRoll) + 90); SetLineAngle(LeftArm, Util.radToDeg(hs.LeftShoulderRoll) - 90); } }
private void UpdateTextBoxes(NaoSkeleton ns, HumanSkeleton hs) { NaoElbowRoll.Text = "Left: " + ns.LeftElbow.getRoll() + "(NaoRoll) - " + hs.LeftElbowYaw + "(HumanYaw) || Right: " + ns.RightElbow.getRoll() + "(NaoRoll) - " + hs.RightElbowYaw + "(HumanYaw)"; NaoElbowYaw.Text = "Left: " + ns.LeftElbow.getYaw() + "(NaoYaw) - " + hs.LeftShoulderRoll + "(HumanRoll) || Right: " + ns.RightElbow.getYaw() + "(NaoYaw) - " + hs.RightShoulderRoll + "(HumanRoll)"; NaoShoulderRoll.Text = "Left: " + ns.LeftShoulder.Roll + "(NaoRoll) - " + hs.LeftShoulderPitch + "(HumanPitch) || Right: " + ns.RightShoulder.Roll + "(NaoRoll) - " + hs.RightShoulderPitch + "(HumanPitch)"; NaoShoulderPitch.Text = "Left: " + ns.LeftShoulder.Pitch + "(NaoPitch) - " + hs.LeftShoulderYaw + "(HumanYaw) || Right: " + ns.RightShoulder.Pitch + "(NaoPitch) - " + hs.RightShoulderYaw + "(HumanYaw)"; }
public NaoSkeleton(HumanSkeleton skeleton) { this.LeftShoulder = new NaoShoulder(skeleton.LeftShoulderYaw, skeleton.LeftShoulderPitch, true); this.RightShoulder = new NaoShoulder(skeleton.RightShoulderYaw, skeleton.RightShoulderPitch, false); this.LeftWrist = new NaoWrist(0.0); this.RightWrist = new NaoWrist(0.0); this.LeftElbow = new NaoElbow(skeleton.LeftShoulderRoll, skeleton.LeftElbowYaw, true); this.RightElbow = new NaoElbow(skeleton.RightShoulderRoll, skeleton.RightElbowYaw, false); this.LeftHand = new NaoHand(true); this.RightHand = new NaoHand(true); }