Example #1
0
        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);
            }
        }
Example #2
0
        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)";
        }
Example #3
0
        public void update(NaoSkeleton skeleton)
        {
            //Shoulder Angles
            proxy.setAngles("LShoulderPitch", (float)skeleton.LeftShoulder.Pitch, SPEED);
            proxy.setAngles("RShoulderPitch", (float)skeleton.RightShoulder.Pitch, SPEED);
            proxy.setAngles("LShoulderRoll", (float)skeleton.LeftShoulder.Roll, SPEED);
            proxy.setAngles("RShoulderRoll", (float)skeleton.RightShoulder.Roll, SPEED);
            //proxy.setAngles("LShoulderRoll", (float)0.0, SPEED);
            //proxy.setAngles("RShoulderRoll", (float)0.0, SPEED);
            //proxy.setAngles("LShoulderPitch", (float)0, SPEED);
            //proxy.setAngles("RShoulderPitch", (float)0, SPEED);

            //Elbow Angles
            proxy.setAngles("LElbowYaw", (float)skeleton.LeftElbow.getYaw(), SPEED);
            proxy.setAngles("RElbowYaw", (float)skeleton.RightElbow.getYaw(), SPEED);
            proxy.setAngles("LElbowRoll", (float)skeleton.LeftElbow.getRoll(), SPEED);
            proxy.setAngles("RElbowRoll", (float)skeleton.RightElbow.getRoll(), SPEED);

            //proxy.setAngles("LElbowYaw", (float)0.0, SPEED);
            //proxy.setAngles("RElbowYaw", (float)0.0, SPEED);
            //proxy.setAngles("LElbowRoll", (float)-0.04, SPEED);
            //proxy.setAngles("RElbowRoll", (float)0.04, SPEED);

            //Shoulder Angles
            Console.Out.WriteLine("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
            Console.Out.WriteLine("LShoulderPitch: " + (float)skeleton.LeftShoulder.Pitch);
            Console.Out.WriteLine("RShoulderPitch: " + (float)skeleton.RightShoulder.Pitch);
            Console.Out.WriteLine("LShoulderRoll: " + (float)skeleton.LeftShoulder.Roll);
            Console.Out.WriteLine("RShoulderRoll: " + (float)skeleton.RightShoulder.Roll);

            //Elbow Angles

            Console.Out.WriteLine("LElbowYaw: " + (float)skeleton.LeftElbow.getYaw());
            Console.Out.WriteLine("RElbowYaw: " + (float)skeleton.RightElbow.getYaw());
            Console.Out.WriteLine("LElbowRoll: " + (float)skeleton.LeftElbow.getRoll());
            Console.Out.WriteLine("RElbowRoll: " + (float)skeleton.RightElbow.getRoll());

            //Wrist Angles
            //proxy.setAngles("LWristYaw", (float)skeleton.LeftWrist.getYaw(), SPEED);
            //proxy.setAngles("RWristYaw", (float)skeleton.RightWrist.getYaw(), SPEED);

            //Hand Angles
            //proxy.setAngles("LHand", (float)skeleton.LeftHand.isOpen(), SPEED);
            //proxy.setAngles("RHand", (float)skeleton.RightHand.isOpen(), SPEED);
        }
Example #4
0
        public NaoController(string ip)
        {
            try
            {
                lastUpdate = null;
                proxy = new MotionProxy(ip, 9559);
                float stiffness = 1.0f;
                float time = 1.0f;
                /*
                proxy.stiffnessInterpolation("Head", stiffness, time);
                proxy.stiffnessInterpolation("LShoulderPitch", stiffness, time);
                proxy.stiffnessInterpolation("RShoulderPitch", stiffness, time);
                proxy.stiffnessInterpolation("LShoulderRoll", stiffness, time);
                proxy.stiffnessInterpolation("RShoulderRoll", stiffness, time);
                proxy.stiffnessInterpolation("LElbowYaw", stiffness, time);
                proxy.stiffnessInterpolation("RElbowYaw", stiffness, time);
                proxy.stiffnessInterpolation("LElbowRoll", stiffness, time);
                proxy.stiffnessInterpolation("RElbowRoll", stiffness, time);

                proxy.stiffnessInterpolation("RHipYawPitch", stiffness, time);
                proxy.stiffnessInterpolation("LHipYawPitch", stiffness, time);
                proxy.stiffnessInterpolation("LHipPitch", stiffness, time);
                proxy.stiffnessInterpolation("RHipPitch", stiffness, time);
                proxy.stiffnessInterpolation("LKneePitch", stiffness, time);
                proxy.stiffnessInterpolation("RKneePitch", stiffness, time);
                proxy.stiffnessInterpolation("LAnklePitch", stiffness, time);
                proxy.stiffnessInterpolation("RAnklePitch", stiffness, time);
                proxy.stiffnessInterpolation("LHipRoll", stiffness, time);
                proxy.stiffnessInterpolation("RHipRoll", stiffness, time);
                proxy.stiffnessInterpolation("LAnkleRoll", stiffness, time);
                proxy.stiffnessInterpolation("RAnkleRoll", stiffness, time);
                 * */
            }
            catch (Exception e)
            {
                Console.Out.WriteLine("HeadPanning.Connect exception: " + e);
            }
        }