상속: ISerializable, ISkeleton
예제 #1
0
        public void update(NaoSkeleton skeleton)
        {
            // Given 0 to rotational angle for now
            if (!skeleton.isUpperBody)
            {
                NaoPosition nao_pos = skeleton.Position;
                Console.Out.WriteLine("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
                Console.WriteLine("Walking to x: " + -nao_pos.Z + " y: " + -nao_pos.X);
                proxy.walkTo(-nao_pos.Z, -nao_pos.X, 0.0f);
            }
            else
            {
                List<String> angleTypes = new List<String>() {"LShoulderPitch", "RShoulderPitch",
                                      "LShoulderRoll", "RShoulderRoll",
                                      "LElbowYaw", "RElbowYaw",
                                      "LElbowRoll", "RElbowRoll"};
                List<float> angles = new List<float>() { skeleton.LeftShoulder.Pitch, skeleton.RightShoulder.Pitch,
                                 skeleton.LeftShoulder.Roll, skeleton.RightShoulder.Roll,
                                 skeleton.LeftElbow.Yaw, skeleton.RightElbow.Yaw,
                                 skeleton.LeftElbow.Roll, skeleton.RightElbow.Roll};

                proxy.setAngles(angleTypes, angles, SPEED);

                /* //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);

                 //Elbow Angles
                 proxy.setAngles("LElbowYaw", (float)skeleton.LeftElbow.Yaw, SPEED);
                 proxy.setAngles("RElbowYaw", (float)skeleton.RightElbow.Yaw, SPEED);
                 proxy.setAngles("LElbowRoll", (float)skeleton.LeftElbow.Roll, SPEED);
                 proxy.setAngles("RElbowRoll", (float)skeleton.RightElbow.Roll, 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.Yaw);
                Console.Out.WriteLine("RElbowYaw: " + (float)skeleton.RightElbow.Yaw);
                Console.Out.WriteLine("LElbowRoll: " + (float)skeleton.LeftElbow.Roll);
                Console.Out.WriteLine("RElbowRoll: " + (float)skeleton.RightElbow.Roll);

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

                //Hand
                //proxy.setAngles("LHand", (float)skeleton.LeftHand.isOpen(), SPEED);
                //proxy.setAngles("RHand", (float)skeleton.RightHand.isOpen(), SPEED);
            }
        }
예제 #2
0
        public void update(NaoSkeleton skeleton)
        {
            // Given 0 to rotational angle for now
            NaoPosition nao_pos = skeleton.Position;
            NaoPosition nao_pos_prv = lastUpdate.Position;
            proxy.walkTo(nao_pos.X - nao_pos_prv.X, nao_pos.Y - nao_pos_prv.Y, 0.0f);

            String[] angleTypes = {"LShoulderPitch", "RShoulderPitch",
                                      "LShoulderRoll", "RShoulderRoll",
                                      "LElbowYaw", "RElbowYaw",
                                      "LElbowRoll", "RElbowRoll"};
            float[] angles = {skeleton.LeftShoulder.Pitch, skeleton.RightShoulder.Pitch,
                                 skeleton.LeftShoulder.Roll, skeleton.RightShoulder.Roll,
                                 skeleton.LeftElbow.Yaw, skeleton.RightElbow.Yaw,
                                 skeleton.LeftElbow.Roll, skeleton.RightElbow.Roll};

            proxy.angleInterpolationWithSpeed(angleTypes, angles, SPEED);
            /*
            //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);

            //Elbow Angles
            proxy.setAngles("LElbowYaw", (float)skeleton.LeftElbow.Yaw, SPEED);
            proxy.setAngles("RElbowYaw", (float)skeleton.RightElbow.Yaw, SPEED);
            proxy.setAngles("LElbowRoll", (float)skeleton.LeftElbow.Roll, SPEED);
            proxy.setAngles("RElbowRoll", (float)skeleton.RightElbow.Roll, 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.Yaw);
            Console.Out.WriteLine("RElbowYaw: " + (float)skeleton.RightElbow.Yaw);
            Console.Out.WriteLine("LElbowRoll: " + (float)skeleton.LeftElbow.Roll);
            Console.Out.WriteLine("RElbowRoll: " + (float)skeleton.RightElbow.Roll);

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

            //Hand
            //proxy.setAngles("LHand", (float)skeleton.LeftHand.isOpen(), SPEED);
            //proxy.setAngles("RHand", (float)skeleton.RightHand.isOpen(), SPEED);
        }
예제 #3
0
 public NaoController(string ip)
 {
     try
     {
         this.ip = ip;
         lastUpdate = null;
         proxy = new MotionProxy(ip, 9559);
         this.setStiffness(1.0f);
     }
     catch (Exception e)
     {
         Console.Out.WriteLine("HeadPanning.Connect exception: " + e);
     }
 }
예제 #4
0
        public static ActionLibrary load(String path)
        {
            /*ActionLibrary al = new ActionLibrary();

            Stream stream = File.Open(path, FileMode.Open);
            BinaryFormatter bformatter = new BinaryFormatter();

            al = (ActionLibrary)bformatter.Deserialize(stream);
            stream.Close();

            return al;*/

            Dictionary<string, string> actions = new Dictionary<string, string>();
            actions["raise the roof"] = "raisetheroof";
            actions["walk forward"] = "walkforward";
            actions["walk left"] = "walkleft";
            actions["walk back"] = "walkbackward";
            actions["walk right"] = "walkright";
            actions["wave right"] = "waveright";
            actions["wave left"] = "waveleft";
            string filename = "Z:/dev/kinect-nao/recordings/";
            ActionLibrary lib = new ActionLibrary();

            foreach (string actionName in actions.Keys)
            {
                List<HumanSkeleton> seq = new List<HumanSkeleton>();
                using (StreamReader s = new StreamReader(filename + actions[actionName] + "/"
                    + actionName + "1.rec"))
                {
                    while (!s.EndOfStream)
                    {
                        seq.Add(new HumanSkeleton(s.ReadLine()));
                    }
                }

                ActionSequence<NaoSkeleton> naoSeq = new ActionSequence<NaoSkeleton>();
                foreach (HumanSkeleton h in seq)
                {
                    AngleConverter ac = new AngleConverter(h);
                    naoSeq.append(ac.getNaoSkeleton());
                }

                if (actionName.StartsWith("walk"))
                {
                    NaoSkeleton start = naoSeq.get(0);
                    NaoSkeleton end = naoSeq.get(naoSeq.size() - 1);
                    NaoSkeleton diff = new NaoSkeleton(new NaoPosition(end.Position.X - start.Position.X, end.Position.Y - start.Position.Y, end.Position.Z - start.Position.Z));
                    naoSeq = new ActionSequence<NaoSkeleton>();
                    naoSeq.append(diff);
                }
                lib.appendToCache(naoSeq);
                lib.setCachedName(actionName);
                lib.saveCache();
            }

            return lib;
        }