P7 Utilisation d'un Robot Nao pour les enfants autistes : Différence entre versions

De Wiki de Projets IMA
Ligne 174 : Ligne 174 :
  
 
expérience avec les enfants
 
expérience avec les enfants
 +
 +
===Mois de Janvier===
 +
Après avoir fait plusieurs exercices avec les enfants en touchant divers stimulis, nous avons donc pris la décision d'axer des exercices que sur l'audition et de mettre au point un remote controlable par un éducateur
 +
 +
Nous avons donc créé des fonctions de base de mouvements en langage c# afin de mettre une application intuitive sous Windows.
 +
Fonction StandUp "Robot se lève peu importe sa position de base"
 +
 +
"using System;
 +
using System.Collections.Generic;
 +
using System.Linq;
 +
using System.Text;
 +
using Aldebaran.Proxies;
 +
 +
namespace StandUp
 +
{
 +
    class StandUpExecuter
 +
    {
 +
        MotionProxy motionProxy;
 +
        RobotPoseProxy robotPoseProxy;
 +
 +
        public StandUpExecuter(MotionProxy motionProxy, RobotPoseProxy robotPoseProxy)
 +
        {
 +
            this.motionProxy = motionProxy;
 +
            this.robotPoseProxy = robotPoseProxy;
 +
        }
 +
 +
        public bool Start()
 +
        {
 +
 +
            NAOPositions curPosition = GetPosition();
 +
            if (curPosition == null || curPosition == NAOPositions.Unknown || curPosition == NAOPositions.HeadBack)
 +
            {
 +
                return false;
 +
            }
 +
 +
            motionProxy.stiffnessInterpolation("Body", 1, 1);
 +
            while (curPosition != NAOPositions.Stand)
 +
            {
 +
                switch (curPosition)
 +
                {
 +
                    case NAOPositions.Sit:
 +
                        ChangePositionFromSitToCrouch();
 +
                        ChangePositionFromCrouchToStand();
 +
                        break;
 +
                    case NAOPositions.Crouch:
 +
                        ChangePositionFromCrouchToStand();
 +
                        break;
 +
                    case NAOPositions.Belly:
 +
                    case NAOPositions.Frog:
 +
                    case NAOPositions.Knee:
 +
                        ChangePositionFromBellyToStand();
 +
                        break;
 +
                    case NAOPositions.Back:
 +
                        ChangePositionFromBackToStand();
 +
                        break;
 +
                    case NAOPositions.Right:
 +
                    case NAOPositions.Left:
 +
                        ChangePositionFromSideToBelly();
 +
                        break;
 +
                    case NAOPositions.HeadBack:
 +
                        break;
 +
                    case NAOPositions.Unknown:
 +
                        return false;
 +
                    default:
 +
                        return false;
 +
                }
 +
                curPosition = GetPosition();
 +
            }
 +
            return true;
 +
        }
 +
 +
        private void ChangePositionFromSideToBelly()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.72094f, 0.72094f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.14876f, 0.14876f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 1.89752f, 1.89752f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.80224f, 0.80224f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.99254f, -0.99254f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.46783f, -0.46783f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.67807f, -0.67807f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.00350f, 0.00350f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 1.97891f, 1.97891f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.77931f, -0.77931f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.92343f, 0.92343f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.34059f, 0.34059f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.64577f, 0.64577f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.00256f, 0.00256f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -1.14432f, -1.14432f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.25162f, 0.25162f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.03532f, 0.03532f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.09208f, -0.09208f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.35585f, 0.35585f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.34366f, 0.34366f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.03064f, -0.03064f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.41567f, 0.41567f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.09200f, -0.09200f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.52314f, 0.52314f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.06745f, -0.06745f });
 +
 +
           
 +
            ChangePosition(names, times, keys);
 +
 +
        }
 +
 +
        private void ChangePositionFromBackToStand()
 +
        {
 +
            var bodyPart = NAOBodyParts.Arms.ToString();
 +
            motionProxy.setCollisionProtectionEnabled(bodyPart, false);
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -0.02459f, -0.00000f, 0.04138f, 0.00000f, 0.00000f, 0.00000f, -0.51393f, -0.30224f, 0.26354f, 0.12043f, -0.00940f, 0.06592f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.06285f, 0.00000f, 0.51490f, -0.67200f, 0.00000f, 0.34907f, 0.17177f, 0.34511f, 0.01047f, 0.02967f, 0.37886f, -0.03993f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 2.08560f, 2.08567f, 2.03865f, 2.08560f, 2.08560f, 2.08560f, 2.03251f, 1.28698f, 0.95717f, 0.73321f, 0.73321f, 1.59225f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.00000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.98018f, 0.36652f, 0.34907f, 0.34907f, 0.36652f, 0.00870f, 0.29909f, 0.44331f, 0.07052f, 0.26687f, 0.49851f, 0.49851f, 0.21932f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -0.76091f, 0.15708f, 0.12268f, 0.08727f, 0.08727f, 0.08727f, -1.96049f, -0.85448f, -0.25008f, 0.08308f, 0.08308f, -1.03089f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.20000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -0.62430f, -0.03491f, -1.54462f, -1.56210f, -0.69813f, -0.00870f, -0.19199f, -1.04615f, -0.87280f, -0.82985f, -0.80535f, -0.80535f, -0.68105f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
 +
            keys.Add(new[] { -0.70568f, -1.78868f, -0.72562f, -0.71949f, -0.72256f, -0.71182f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
 +
            keys.Add(new[] { 0.00088f, 0.00133f, 0.00219f, 0.00269f, 0.00290f, 0.00451f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 2.08560f, 2.08567f, 1.95283f, 2.08560f, 2.08560f, 2.08560f, 2.07708f, 2.03720f, 1.44047f, 0.89131f, 0.89131f, 1.46961f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -1.07384f, -0.36652f, -0.34907f, -0.34907f, -0.36652f, -0.00870f, -0.45717f, -0.56455f, -0.70568f, -0.87266f, -0.68068f, -0.15037f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.76542f, -0.15708f, -0.25775f, -0.08727f, -0.08727f, -0.08727f, -0.08134f, -0.06907f, -0.02612f, 0.00062f, 0.00062f, 0.91269f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.71028f, 0.03491f, 1.54462f, 1.56210f, 0.69813f, 0.00870f, 0.09668f, 0.02152f, 0.19026f, 0.45379f, 0.55995f, 0.38047f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
 +
            keys.Add(new[] { 1.15199f, 1.72571f, 1.07989f, 1.07836f, 1.07989f, 1.08450f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 4.30000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
 +
            keys.Add(new[] { 0.00000f, 0.00000f, 0.00032f, 0.00000f, 0.00287f, 0.00300f, 0.00308f, 0.00376f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -0.47090f, 0.00000f, 0.00771f, -0.00000f, -0.65498f, -0.49851f, -0.85900f, -0.69639f, -0.40225f, -0.40225f, -0.23006f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.04453f, 0.00000f, 0.05833f, 0.00000f, -0.06478f, 0.54105f, 0.15498f, -0.17483f, 0.00925f, 0.19199f, 0.17453f, 0.15191f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.25008f, -0.17453f, 0.37434f, -0.17453f, -1.17808f, -1.57080f, -0.85746f, -0.05672f, -0.45556f, -0.85521f, -0.83599f, 0.21327f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.09200f, 1.67552f, 2.02024f, 1.67552f, 1.07777f, 1.67552f, 2.11075f, 2.11227f, 2.11253f, 2.11253f, 2.11253f, -0.08901f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.82525f, 0.24435f, -0.45411f, 0.24435f, 0.67748f, 0.66323f, -0.45564f, -1.10145f, -1.18952f, -1.18952f, -1.18952f, 0.08126f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -0.03371f, 0.00000f, -0.03371f, 0.00000f, 0.17109f, -0.10472f, -0.39573f, -0.09967f, 0.04299f, 0.11810f, 0.08727f, -0.13648f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { -0.14876f, 0.00000f, -0.02297f, 0.00000f, 0.06190f, -0.54105f, -0.55833f, -0.61049f, -0.62657f, -0.29671f, -0.01745f, -0.06285f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.18557f, -0.17453f, 0.28221f, -0.17453f, -1.16456f, -1.57080f, -1.52484f, -1.56012f, -1.02974f, -0.90583f, -0.90583f, 0.20858f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.19179f, 1.67552f, 2.02645f, 1.67552f, 1.09386f, 1.67552f, 1.22111f, 1.09992f, 1.08385f, 0.87616f, 1.76278f, -0.07666f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.46638f, 0.24435f, -0.37425f, 0.24435f, 0.71737f, 0.66323f, 0.78545f, 0.78392f, 0.44157f, 0.40317f, -0.57945f, 0.08134f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
 +
            keys.Add(new[] { 0.16725f, 0.00000f, -0.03984f, 0.00000f, -0.16084f, 0.10472f, 0.00925f, -0.09813f, 0.44331f, 0.67960f, 0.27751f, 0.06294f });
 +
 +
 +
            ChangePosition(names, times, keys);
 +
            motionProxy.setCollisionProtectionEnabled(bodyPart, true);
 +
        }
 +
 +
        private void ChangePositionFromBellyToStand()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00000f, -0.22689f, 0.28623f, 0.29671f, -0.49567f, -0.29671f, 0.05236f, -0.39095f, 0.06745f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.67195f, -0.54768f, 0.10734f, 0.51487f, 0.38048f, 0.37119f, -0.10472f, -0.53387f, 0.12741f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.08433f, -1.51146f, -1.25025f, 0.07206f, 1.27409f, 0.75573f, 0.75049f, 1.29154f, 1.59378f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 1.55390f, 0.01683f, 0.07666f, 0.07052f, 0.15643f, 0.93899f, 0.67719f, 0.84648f, 0.21779f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -2.07694f, -1.58006f, -1.60461f, -1.78715f, -1.32695f, -1.24791f, -0.97260f, -0.95993f, -1.01862f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.00873f, -0.35278f, -0.63810f, -0.85133f, -1.55083f, -0.73304f, -0.73653f, -1.15506f, -0.68105f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -1.55092f, -1.55092f, -1.51717f, -1.44814f, -1.40519f, -1.40825f, -1.40825f, -0.54454f, -0.71028f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00102f, 0.00034f, 0.00071f, 0.00095f, 0.00100f, 0.00100f, 0.00096f, 0.00100f, 0.00455f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.02757f, -1.51146f, -1.22256f, -0.23619f, 0.21787f, 0.44950f, 0.91431f, 0.96033f, 1.47882f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -1.53558f, -0.19199f, -0.08288f, -0.08288f, -0.22707f, -0.18259f, -0.00870f, -0.13197f, -0.14884f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 2.07694f, 1.56157f, 1.61373f, 1.68582f, 1.96041f, 1.95121f, 0.66571f, 0.39573f, 0.90962f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.10472f, 0.38201f, 0.63512f, 1.55705f, 0.00870f, 0.00870f, 0.42343f, 0.64926f, 0.39275f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 1.50941f, 1.50941f, 1.49714f, 1.42811f, 1.43578f, 1.44038f, 1.44345f, 1.44038f, 1.08603f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00032f, 0.00022f, 0.00046f, 0.00082f, 0.00086f, 0.00084f, 0.00083f, 0.00084f, 0.00380f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.02269f, 0.03491f, -0.43561f, -0.77923f, -1.04154f, -1.14530f, -1.14530f, -0.56754f, -0.23619f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00000f, 0.00004f, 0.00158f, -0.37732f, -0.29755f, -0.29755f, 0.19486f, 0.12736f, 0.14884f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.80460f, 0.00004f, -1.56924f, -1.28085f, -1.03694f, -1.15966f, -1.18267f, -1.27011f, 0.21327f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.09233f, 1.98968f, 2.11253f, 0.28221f, 0.40493f, 0.35738f, 0.71940f, 2.01409f, -0.09055f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.64403f, -1.02974f, -1.15054f, 0.21625f, 0.71020f, 0.92275f, 0.82525f, -0.50166f, 0.07666f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00000f, 0.00004f, -0.00149f, -0.45249f, -0.30062f, -0.11808f, -0.04138f, -0.12114f, -0.13495f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00000f, 0.00004f, 0.00158f, 0.31144f, 0.25469f, 0.32065f, 0.22707f, -0.07512f, -0.05978f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.80460f, -0.00004f, -1.57699f, -1.66136f, -1.19963f, -1.59847f, -0.32218f, -0.71028f, 0.21012f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.09233f, 1.98968f, 2.08628f, 1.74267f, 2.12019f, 2.12019f, 2.12019f, 2.12019f, -0.07666f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.64403f, -1.02974f, -1.13819f, -1.18645f, -1.18645f, -0.58901f, -1.18645f, -1.18645f, 0.08595f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.00000f, 0.00004f, 0.00618f, -0.00456f, -0.09813f, -0.01376f, -0.09507f, 0.03532f, 0.06140f });
 +
 +
 +
            ChangePosition(names, times, keys);
 +
        }
 +
 +
        private void ChangePositionFromCrouchToStand()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.06745f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.04146f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 1.58611f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.22392f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -1.02169f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.65958f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.75170f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.00456f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 1.46808f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.14731f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.90962f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.37127f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 1.05535f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.00381f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.23926f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.14884f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.21173f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.09055f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.08126f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.13342f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.05825f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.21165f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { -0.07666f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.07981f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 1.50000f });
 +
            keys.Add(new[] { 0.05987f });
 +
 +
            ChangePosition(names, times, keys);
 +
        }
 +
 +
        private NAOPositions GetPosition()
 +
        {
 +
            var curPositionAndTime = (System.Collections.ArrayList)robotPoseProxy.getActualPoseAndTime();
 +
            string curPositionStr = curPositionAndTime[0].ToString();
 +
            NAOPositions curPosition;
 +
            Enum.TryParse<NAOPositions>(curPositionStr, true, out curPosition);
 +
            return curPosition;
 +
        }
 +
 +
        private void ChangePositionFromSitToCrouch()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 0.00149f, -0.27925f, -0.34826f, -0.47124f, -0.52360f, -0.28690f, -0.00940f, -0.00940f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 0.37119f, 0.36965f, 0.22689f, 0.27576f, 0.15708f, 0.34971f, 0.37886f, 0.37886f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 0.49567f, 0.57421f, 0.62657f, 1.01393f, 0.65101f, 0.75162f, 1.12050f, 1.17286f, 1.40848f, 1.06814f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 0.60000f, 0.84000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 0.24386f, 0.49393f, 0.41567f, 0.50964f, 0.75398f, 0.61087f, 0.51078f, 0.07666f, 0.15489f, 0.68872f, 0.59208f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { -0.54461f, -0.71182f, -0.73636f, -0.81766f, -0.29764f, -0.16265f, -0.86982f, -1.01402f, -0.92044f, -0.36513f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { -1.06302f, -0.93416f, -0.79304f, -0.94183f, -1.06762f, -0.62430f, -0.89121f, -0.85133f, -1.23636f, -1.07836f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
 +
            keys.Add(new[] { -0.74250f, -0.77164f, -0.73636f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
 +
            keys.Add(new[] { 0.00032f, 0.00116f, 0.00126f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 0.60000f, 0.84000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 0.56609f, 0.73304f, 1.32695f, 1.96664f, 2.08560f, 2.06787f, 2.08560f, 2.02799f, 1.61381f, 1.42359f, 1.21650f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 1.92000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { -0.35133f, -0.67500f, -0.63819f, -0.62832f, -0.00873f, -0.50933f, -0.55382f, -0.56455f, -0.66580f, -0.57683f, -0.26236f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 0.48010f, 1.82235f, 2.05399f, -0.08727f, -0.08134f, -0.07674f, -0.06600f, 0.31596f, 1.67969f, 0.96638f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 1.92000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
 +
            keys.Add(new[] { 1.21650f, 1.02936f, 0.68114f, 0.85521f, 0.00873f, 0.07981f, 0.05987f, 0.01692f, 0.33292f, 0.49706f, 0.97260f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
 +
            keys.Add(new[] { 1.07529f, 1.09523f, 1.08756f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
 +
            keys.Add(new[] { 0.00032f, 0.00221f, 0.00244f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { -0.67645f, -0.65173f, -0.75144f, -0.49909f, -0.64559f, -0.85897f, -0.43408f, -0.28221f, -0.17330f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { 0.13964f, 0.14501f, 0.14808f, 0.54105f, 0.15498f, -0.02526f, -0.17453f, 0.18259f, 0.44030f, 0.13043f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 4.36000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { -1.57077f, -1.57080f, -1.57080f, -1.57080f, -0.85706f, -0.59586f, -0.05236f, -0.40143f, -0.69026f, -0.75469f, -0.73014f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { 0.72401f, 0.95923f, 1.18473f, 1.67552f, 2.11255f, 2.11255f, 2.11255f, 2.00182f, 2.11253f, 1.61833f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { 0.77616f, 0.75542f, 0.75542f, 0.66323f, -0.45379f, -0.68068f, -1.09956f, -1.18944f, -1.18944f, -0.73636f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { -0.05058f, -0.00550f, 0.02671f, -0.17453f, -0.39573f, -0.08681f, -0.10379f, 0.01078f, -0.01683f, -0.08893f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { -0.14415f, -0.13336f, -0.21006f, -0.54105f, -0.55842f, -0.73468f, -0.61087f, -0.25460f, -0.08126f, -0.07512f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { -1.55245f, -1.57080f, -1.57080f, -1.57080f, -1.52484f, -1.57080f, -1.55965f, -0.84988f, -0.67807f, -0.64892f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { 0.84374f, 0.95426f, 1.21504f, 1.67552f, 1.22173f, 1.27333f, 1.09956f, 0.59523f, 1.44047f, 1.58006f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 4.36000f, 4.72000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { 0.78545f, 0.76027f, 0.76027f, 0.66323f, 0.78540f, 0.66976f, 0.78540f, 0.78540f, 0.54105f, 0.48172f, -0.36965f, -0.78843f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.52000f, 3.76000f, 4.36000f, 4.72000f, 5.04000f, 5.80000f, 7.20000f });
 +
            keys.Add(new[] { 0.04913f, 0.00073f, -0.00081f, 0.17453f, 0.00929f, 0.07436f, -0.15708f, -0.10472f, 0.24435f, 0.34907f, 0.44030f, 0.42343f, 0.09055f });
 +
 +
            ChangePosition(names, times, keys);
 +
 +
        }
 +
 +
        private void ChangePosition(List<string> names, List<float[]> times, List<float[]> keys)
 +
        {
 +
            motionProxy.angleInterpolation(names, keys, times, true);
 +
        }
 +
    }
 +
}"
 +
 +
Fonction SitDown " Le robot s'asseoit peu importe sa position de base"
 +
"using System;
 +
using System.Collections.Generic;
 +
using System.Linq;
 +
using System.Text;
 +
using Aldebaran.Proxies;
 +
 +
namespace StandUp
 +
{
 +
    class SitDownExecuter
 +
    {
 +
        MotionProxy motionProxy;
 +
        RobotPoseProxy robotPoseProxy;
 +
 +
        public SitDownExecuter(MotionProxy motionProxy, RobotPoseProxy robotPoseProxy)
 +
        {
 +
            this.motionProxy = motionProxy;
 +
            this.robotPoseProxy = robotPoseProxy;
 +
        }
 +
    public bool Start()
 +
        {
 +
 +
            NAOPositions curPosition = GetPosition();
 +
            if (curPosition == NAOPositions.Unknown || curPosition == NAOPositions.HeadBack)
 +
            {
 +
                return false;
 +
            }
 +
 +
            motionProxy.stiffnessInterpolation(NAOBodyParts.Body.ToString(), 1, 1);
 +
            while (curPosition != NAOPositions.Sit)
 +
            {
 +
                switch (curPosition)
 +
                {
 +
                    case NAOPositions.Belly:
 +
                    case NAOPositions.Frog:
 +
                    case NAOPositions.Knee:
 +
                        ChangePositionFromBellyToCrouch();
 +
                        break;
 +
                    case NAOPositions.Back:
 +
                        ChangePositionFromBackToSitDown();
 +
                        break;
 +
                    case NAOPositions.Right:
 +
                    case NAOPositions.Left:
 +
                        ChangePositionFromSideToBelly();
 +
                        break;
 +
                    case NAOPositions.Stand:
 +
                    case NAOPositions.Crouch:
 +
                        ChangePositionFromCrouchToSitDown();
 +
                        break;
 +
                    case NAOPositions.HeadBack:
 +
                        return false;
 +
                        break;
 +
                    case NAOPositions.Unknown:
 +
                        return false;
 +
                    default:
 +
                        return false;
 +
                }
 +
                curPosition = GetPosition();
 +
            }
 +
            //motionProxy.stiffnessInterpolation(NAOBodyParts.Body.ToString(), 0, 1);//Release stiffness
 +
            return true;
 +
        }
 +
 +
        private void ChangePositionFromCrouchToSitDown()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.31605f, -0.44030f, -0.63358f, -0.62745f, -0.40962f, -0.18412f, -0.00004f, -0.02152f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.37119f, 0.37579f, 0.36812f, 0.35738f, 0.36812f, 0.36658f, 0.37886f, -0.03379f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 2.90000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 1.04371f, 0.72955f, 0.72955f, 1.01086f, 1.50481f, 2.03404f, 1.53764f, 0.67719f, 0.91269f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.33590f, 0.53532f, 0.48171f, 0.83776f, 0.44331f, 1.55683f, 0.56907f, 0.21932f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.56302f, -0.08552f, 0.10580f, -1.55334f, -0.32985f, -1.20777f, -0.76091f, -0.43723f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 4.70000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.95871f, -1.20253f, -0.69813f, -0.90234f, -0.48695f, -0.68591f, -0.95993f, -1.32227f, -1.28085f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.08740f, -0.71642f, -0.71642f, -0.71642f, -0.72869f, -0.73023f, -0.72256f, 0.05978f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.00404f, 0.00117f, 0.00116f, 0.00116f, 0.00128f, 0.00121f, 0.00106f, 0.00524f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.80809f, 0.59341f, 1.74420f, 2.07694f, 2.08560f, 1.88839f, 0.78191f, 0.93118f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 2.90000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.56762f, -0.93026f, -0.76271f, -0.55327f, -0.52534f, -0.55327f, -1.55683f, -0.54921f, -0.26389f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.13963f, 0.00456f, -0.06907f, -0.14731f, 0.11694f, 1.75179f, 1.12745f, 0.51845f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.83776f, 0.47865f, 0.07828f, 0.10129f, 0.59167f, 1.03856f, 1.29320f, 1.28247f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.18864f, 1.05535f, 1.05228f, 1.04308f, 1.04308f, 1.04768f, 1.06149f, -0.05987f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.00709f, 0.00149f, 0.00154f, 0.00180f, 0.00206f, 0.00201f, 0.00211f, 0.00523f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.39726f, -0.42334f, -0.90809f, -0.58595f, -0.53993f, -0.79304f, -0.77770f, -0.61356f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.18259f, 0.31416f, -0.26227f, 0.29304f, 0.52927f, 0.23321f, 0.35746f, 0.27923f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.59055f, -0.86974f, -0.00916f, -0.62123f, -1.55390f, -1.59378f, -1.46493f, -1.57998f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 2.11253f, 2.11253f, 1.81774f, 1.98189f, 1.07687f, 1.15813f, 1.08756f, 1.39743f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -1.18952f, -1.18952f, -1.18952f, -0.52314f, 0.73935f, 0.92275f, 0.92189f, 0.83599f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.04299f, 0.07981f, 0.04146f, -0.45402f, -0.18097f, -0.04751f, -0.05058f, -0.00916f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.13802f, -0.25614f, -0.38039f, -0.57521f, -0.53532f, -0.21472f, -0.32210f, -0.26841f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.76551f, -0.85908f, -1.36530f, -1.65063f, -1.54171f, -1.60921f, -1.41746f, -1.58620f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 1.89300f, 0.77471f, 1.14287f, 1.14594f, 1.03847f, 1.19810f, 0.98027f, 1.40672f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { -0.73014f, 0.33445f, 0.45717f, 0.77164f, 0.71335f, 0.93206f, 0.93206f, 0.84988f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
 +
            keys.Add(new[] { 0.21020f, 0.50626f, 0.05833f, -0.05518f, 0.18872f, 0.03532f, 0.04760f, 0.01845f });
 +
 +
            ChangePosition(names, times, keys);
 +
        }
 +
 +
        private void ChangePositionFromBackToSitDown()
 +
        {
 +
            var bodyPart = NAOBodyParts.Arms.ToString();
 +
            motionProxy.setCollisionProtectionEnabled(bodyPart, false);
 +
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { -0.02459f, -0.00000f, 0.04138f, 0.00000f, -0.00618f, 0.01376f, 0.02450f, -0.02305f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { 0.06285f, 0.00000f, 0.51490f, -0.67200f, -0.02152f, 0.19478f, 0.07359f, -0.03532f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 5.90000f, 6.20000f });
 +
            keys.Add(new[] { 2.08560f, 2.09440f, 2.03865f, 2.08560f, 2.08567f, 1.30846f, 0.90348f, 0.70337f, 0.90195f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { 0.98018f, 0.36652f, 0.34907f, 0.34907f, 0.40143f, 1.17193f, 0.39726f, 0.21625f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { -0.76091f, 0.15708f, 0.12268f, 0.08727f, 0.05978f, -0.95419f, -0.96339f, -0.43877f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 4.60000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { -0.62430f, 0.00000f, -1.54462f, -1.56210f, -0.87441f, -1.03498f, -0.67952f, -1.12285f, -1.23483f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { -0.70568f, -1.78868f, -0.71949f, -0.04760f, 0.05978f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.00088f, 0.00133f, 0.00158f, 0.00022f, 0.00524f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 5.90000f, 6.20000f });
 +
            keys.Add(new[] { 2.08560f, 2.09440f, 1.95283f, 2.08560f, 2.08567f, 1.91908f, 1.03703f, 0.72955f, 0.92504f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { -1.07384f, -0.36652f, -0.34907f, -0.34907f, -0.41015f, -0.89897f, -0.74096f, -0.24241f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { 0.76542f, -0.15708f, -0.25775f, -0.08727f, -0.06600f, 1.57691f, 1.06762f, 0.52459f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 4.60000f, 5.00000f, 5.60000f, 6.20000f });
 +
            keys.Add(new[] { 0.71028f, 0.00000f, 1.54462f, 1.56210f, 0.71384f, 1.05418f, 0.95726f, 1.14594f, 1.27019f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 1.15199f, 1.72571f, 1.11057f, 0.24693f, -0.05680f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.00000f, 0.00000f, 0.00032f, 0.00005f, 0.00221f, 0.00523f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { -0.47090f, 0.00000f, 0.00771f, -0.00000f, -0.65498f, -0.70407f, -0.61816f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.04453f, 0.00000f, 0.05833f, 0.00000f, -0.06439f, 0.14083f, 0.27923f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.25008f, -0.17453f, 0.37434f, -0.17453f, -1.17807f, -1.57080f, -1.57998f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.09200f, 1.67552f, 2.02024f, 1.67552f, 1.07683f, 1.40161f, 1.39590f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.82525f, 0.24435f, -0.45411f, 0.24435f, 0.67799f, 0.77609f, 0.83599f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { -0.03371f, 0.00000f, -0.03371f, 0.00000f, 0.17032f, 0.09083f, -0.00916f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { -0.14876f, 0.00000f, -0.02297f, 0.00000f, 0.06140f, -0.12200f, -0.26841f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.18557f, -0.17453f, 0.28221f, -0.17453f, -1.16435f, -1.57080f, -1.59233f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.19179f, 1.67552f, 2.02645f, 1.67552f, 1.09532f, 1.37682f, 1.40825f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.46638f, 0.24435f, -0.37425f, 0.24435f, 0.71795f, 0.76985f, 0.84988f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
 +
            keys.Add(new[] { 0.16725f, 0.00000f, -0.03984f, 0.00000f, -0.14415f, -0.11992f, 0.01845f });
 +
 +
 +
 +
            ChangePosition(names, times, keys);
 +
            motionProxy.setCollisionProtectionEnabled(bodyPart, true);
 +
        }
 +
 +
        private void ChangePositionFromBellyToCrouch()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.17453f, -0.22689f, 0.28623f, 0.29671f, -0.49567f, -0.29671f, 0.05236f, -0.39095f, -0.01745f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.57683f, -0.54768f, 0.10734f, 0.51487f, 0.38048f, 0.37119f, -0.10472f, -0.53387f, 0.00698f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.08433f, -1.51146f, -1.25025f, 0.07206f, 1.27409f, 0.75573f, 0.75049f, 1.29154f, 1.51708f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 1.55390f, 0.01683f, 0.07666f, 0.07052f, 0.15643f, 0.93899f, 0.67719f, 0.84648f, 0.08433f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -2.07694f, -1.58006f, -1.60461f, -1.78715f, -1.32695f, -1.24791f, -0.97260f, -0.95993f, -0.90203f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.00873f, -0.35278f, -0.63810f, -0.85133f, -1.55083f, -0.73304f, -0.73653f, -1.15506f, -1.09984f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
 +
            keys.Add(new[] { -1.55092f, -1.55092f, -1.51717f, -1.44814f, -1.40519f, -1.40825f, -1.40825f, -0.54454f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
 +
            keys.Add(new[] { 0.00102f, 0.00034f, 0.00071f, 0.00095f, 0.00100f, 0.00100f, 0.00096f, 0.00100f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.02757f, -1.51146f, -1.22256f, -0.23619f, 0.21787f, 0.44950f, 0.91431f, 0.96033f, 1.35303f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -1.53558f, -0.19199f, -0.08288f, -0.08288f, -0.22707f, -0.18259f, -0.00870f, -0.13197f, -0.01385f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 2.07694f, 1.56157f, 1.61373f, 1.68582f, 1.96041f, 1.95121f, 0.66571f, 0.39573f, 0.86667f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.10472f, 0.38201f, 0.63512f, 1.55705f, 0.00870f, 0.00870f, 0.42343f, 0.64926f, 0.83454f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
 +
            keys.Add(new[] { 1.50941f, 1.50941f, 1.49714f, 1.42811f, 1.43578f, 1.44038f, 1.44345f, 1.44038f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
 +
            keys.Add(new[] { 0.00032f, 0.00022f, 0.00046f, 0.00082f, 0.00086f, 0.00084f, 0.00083f, 0.00084f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.03371f, 0.03491f, -0.43561f, -0.77923f, -1.04154f, -1.14530f, -1.14530f, -0.56754f, 0.00000f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.06294f, 0.00004f, 0.00158f, -0.37732f, -0.29755f, -0.29755f, 0.19486f, 0.12736f, -0.01047f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.06140f, 0.00004f, -1.56924f, -1.28085f, -1.03694f, -1.15966f, -1.18267f, -1.27011f, -1.04720f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.12043f, 1.98968f, 2.11253f, 0.28221f, 0.40493f, 0.35738f, 0.71940f, 2.01409f, 2.09440f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.92189f, -1.02974f, -1.15054f, 0.21625f, 0.71020f, 0.92275f, 0.82525f, -0.50166f, -1.04720f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.00149f, 0.00004f, -0.00149f, -0.45249f, -0.30062f, -0.11808f, -0.04138f, -0.12114f, 0.00000f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.03142f, 0.00004f, 0.00158f, 0.31144f, 0.25469f, 0.32065f, 0.22707f, -0.07512f, 0.01047f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.07666f, -0.00004f, -1.57699f, -1.66136f, -1.19963f, -1.59847f, -0.32218f, -0.71028f, -1.04720f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { -0.07819f, 1.98968f, 2.08628f, 1.74267f, 2.12019f, 2.12019f, 2.12019f, 2.12019f, 2.09440f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.92965f, -1.02974f, -1.13819f, -1.18645f, -1.18645f, -0.58901f, -1.18645f, -1.18645f, -1.04720f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
 +
            keys.Add(new[] { 0.18850f, 0.00004f, 0.00618f, -0.00456f, -0.09813f, -0.01376f, -0.09507f, 0.03532f, 0.00000f });
 +
 +
 +
            ChangePosition(names, times, keys);
 +
        }
 +
 +
        private void ChangePositionFromSideToBelly()
 +
        {
 +
            List<string> names = new List<string>();
 +
            List<float[]> times = new List<float[]>();
 +
            List<float[]> keys = new List<float[]>();
 +
 +
            names.Add("HeadYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.72094f, 0.72094f });
 +
 +
            names.Add("HeadPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.14876f, 0.14876f });
 +
 +
            names.Add("LShoulderPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 1.89752f, 1.89752f });
 +
 +
            names.Add("LShoulderRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.80224f, 0.80224f });
 +
 +
            names.Add("LElbowYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.99254f, -0.99254f });
 +
 +
            names.Add("LElbowRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.46783f, -0.46783f });
 +
 +
            names.Add("LWristYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.67807f, -0.67807f });
 +
 +
            names.Add("LHand");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.00350f, 0.00350f });
 +
 +
            names.Add("RShoulderPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 1.97891f, 1.97891f });
 +
 +
            names.Add("RShoulderRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.77931f, -0.77931f });
 +
 +
            names.Add("RElbowYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.92343f, 0.92343f });
 +
 +
            names.Add("RElbowRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.34059f, 0.34059f });
 +
 +
            names.Add("RWristYaw");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.64577f, 0.64577f });
 +
 +
            names.Add("RHand");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.00256f, 0.00256f });
 +
 +
            names.Add("LHipYawPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -1.14432f, -1.14432f });
 +
 +
            names.Add("LHipRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.25162f, 0.25162f });
 +
 +
            names.Add("LHipPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.03532f, 0.03532f });
 +
 +
            names.Add("LKneePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.09208f, -0.09208f });
 +
 +
            names.Add("LAnklePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.35585f, 0.35585f });
 +
 +
            names.Add("LAnkleRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.34366f, 0.34366f });
 +
 +
            names.Add("RHipRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.03064f, -0.03064f });
 +
 +
            names.Add("RHipPitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.41567f, 0.41567f });
 +
 +
            names.Add("RKneePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.09200f, -0.09200f });
 +
 +
            names.Add("RAnklePitch");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { 0.52314f, 0.52314f });
 +
 +
            names.Add("RAnkleRoll");
 +
            times.Add(new[] { 0.50000f, 1.00000f });
 +
            keys.Add(new[] { -0.06745f, -0.06745f });
 +
 +
 +
            ChangePosition(names, times, keys);
 +
 +
        }
 +
 +
        private NAOPositions GetPosition()
 +
        {
 +
            var curPositionAndTime = (System.Collections.ArrayList)robotPoseProxy.getActualPoseAndTime();
 +
            string curPositionStr = curPositionAndTime[0].ToString();
 +
            NAOPositions curPosition;
 +
            Enum.TryParse<NAOPositions>(curPositionStr, true, out curPosition);
 +
            return curPosition;
 +
        }
 +
 +
        private void ChangePosition(List<string> names, List<float[]> times, List<float[]> keys)
 +
        {
 +
            motionProxy.angleInterpolation(names, keys, times, true);
 +
        }     
 +
    }
 +
 +
 +
}"

Version du 10 février 2015 à 15:17

Cahier des charges

Contexte

De nos jours, la robotique envahi nos foyers de plus en plus. On peut les remarquer dans nos cuisines mais aussi dans nos salons. La société Aldebaran Robotics est allé plus loin en créant un robot humanoïde du nom de NAO. Véritable compagnon éléctronique, ce robot est entièrement programmable et autonome et mesure 60 cm et présente des intérêts dans tous les domaines qu'il soit industriel, pédagogique mais aussi sociologique. GAPAS est une associaciation de plusieurs services au personnes handicapées. Parmi ceux-ci, on retrouve l'aide aux enfants autistes avec des difficultés visuelles. L'éducation de ces enfants diffère de celle donnée dans les écoles régulières. Il est important de s'adapter à leur(s) déficience(s) et donc de s'adapter à chacun d'eux. L'autisme se caractérise par des difficultés cognitves, socio-affectives et motrices. Cela se reflète par des troubles de la communication et de mauvaise compréhensions des émotions de leurs interlocuteurs et entraîne un repli de l'enfant. Le robot Nao par sa voix neutre et son manque d'expression faciale peut aider un enfant atteint de ces difficultés à apprendre et interagir avec le monde

«  Les enfants autistes ont un besoin d’attention considérable et NAO permet de combler ce vide en proposant des jeux simples, sans jamais se lasser. Le robot et l’enfant autiste partagent un trait commun : une lassitude inexistante. NAO vient les aider en éduquant et en faisant jouer les enfants sans se lasser. » (site internet : robotique TPE)

Objectif

Imaginer une collaboration avec la création de séquences de jeux et d’exercices éducatifs spécifiques aux problématiques rencontrées par les jeunes en utilisant l’aspect ludique et novateur de Nao. Il s’agirait de développer, en lien avec les professionnels un programme dédié à l’éducation des enfants souffrants de l’autisme : utiliser un robot Nao dans le cadre de leur accompagnement, pour stimuler les apprentissages. Par l’intermédiaire du Robot, on vise à stimuler les capacités de concentration, la capacité d’écoute, les interactions donc le travail sur la reconnaissance et l’expression des émotions.

Il s’agirait de développer des applications éducatives ayant toutes un objectif thérapeutique bien précis.

Rapport de soutenance Mi-parcours [[Fichier:Fichier:Projetpfenaomi.pdf]]

Caractéristiques du NAO

Hauteur 58 cm

Poids 4,8 kg

Autonomie 90 min

Degrés de liberté de 14 à 25 selon la version

Processeur Intel ATOM 1,6 GHz (V4) ou AMD Geode 550Mhz (V3.3 ...) selon la version

Système d'exploitation intégré Linux

Systèmes d'exploitation compatibles Windows, Mac OS, Linux

Langages de programmation C++, Python, Java, MATLAB, Urbi, C, .Net

Connectivité Ethernet, Wi-Fi ( b,g,n )

Vision 2 caméras 920p, 30ips (localisation, reconnaissance de visages ou d'bojets)

Audio 4 Microphones (reconnaissance vocale, localisation de source)

Capteurs:

-1 Capteur tactile au niveau de la tête et 1 au niveau de chaque main

-2 bumpers (capteur de contact) au niveau des avant-pieds

-2 sonars utilisant des capteurs ultrason (émetteurs et récepteurs)

-1 accéléromètre 3 axes

-2 gyromètres

-2 LED infrarouges

-2 Haut-parleurs (synthèse vocale)

Nao caract.png


Exemple de collaboration avec NAO [ https://www.youtube.com/watch?v=8BiayzQxTQA ]

Déroulement du projet

Semaine 1 : 15/09 - 21/09

Prise de contact avec les différents acteurs du projet.

Semaine 2 : 22/09 - 28/09

Recherche sur les besoins des enfants autiste. Constitution d'un dossier avec des méthodes de développement


Méthode ABBA [1]


Méthode de développement Fichier:Autisme.pdf


Méthode TEACCH Fichier:TEACCH.pdf

Semaine 3 : 29/09 - 05/10

Mise en place d'un cahier des charges et prise de rendez vous avec l'IME la pépinière

Semaine 4 : 06/10 - 12/10

Entretien avec notre professeur encadrant M. Grisoni. Différents points ont été abordés, notamment:

-La bonne prise en main du projet et l'élaboration du cahier des charges.

-La faisabilité du projet au regard des capacités techniques du NAO (Technologie embarquée, temps de charge, logiciels,... )

-Le déroulement des séquences de jeux et la prise en compte de chaque détail dans la réalisations de celles ci.

Semaine 5 : 13/10 - 19/10

Rencontre avec les représentants de GAPAS à l'IME la pépinière à Loos, en présence notamment des accompagnateurs des enfants et adolescents atteints d'autisme et de déficience visuelle, ainsi qu'une orthophoniste qui suit régulièrement le progrès de ces derniers.

-Présentation du robot NAO et discussion autour de ces différentes possibilités techniques.

-Débat autour de la faisabilité du projet et de l'influence d'un robot tel que NAO dans le processus d'épanouissement et d'éveil d'enfants atteints d'autisme.

- Discussion autour des différentes possibilités de séquences de jeux entre le robot et les enfants qui favoriseraient le développement dudit processus. Prise en compte des différences entre enfants et adultes (salle Esperenza et salle Argos)

-Dialogue autour de la technologie à utiliser pour que le NAO puisse reconnaitre chaque enfant en sa présence. Utilisation de la technologie RFID par exemple, notamment sous forme de patch à coller sur des vêtements.

-Choix d'une première séquence de jeu avec le robot qui permettrait une approche initiale avec les enfants.

-Visite des locaux et des salle de jeux


Semaine 6 : 20/10 - 26/10

Suite à la rencontre au sein de l'IME la pépinière, nous avons commencé un brainstorming autour des technologies à utiliser pour la réalisation de séquences de jeux avec le NAO et les enfants. Etant donnée le manque de fiabilité de la reconnaissance faciale ou vocale du NAO, il nous a fallu trouver une solution pour pallier à ce problème afin que le robot puisse reconnaitre et interagir avec les enfants présents lors des séances. Des lors, plusieurs solutions nous ont parues réalisables:

-Utilisation de puces BLE (Bluetooth Low Energy)

-Utilisation de la technologie RFID (Radio Frequency IDentification)

-


Nous avons également demandé l'accès au NAO afin que l'on puisse débuter la programmation de la première séquence.


Semaine 7 : 27/10 - 02/11

Vacances de la Toussaint.


Semaine 8 : 03/11 - 09/11

Discussion via emails avec GAPAS pour définir la technologie à utiliser lors des séquences de jeux. Ici, plusieurs paramètres ont été pris en compte, notamment le prix des composants, leur fiabilité et l'implémentation de la technologie en question.

Robot NAO toujours indisponible pour le début de la programmation.

Semaine 9 : 10/11 - 16/11

Récupération du robot NAO et prise en main du logiciel de programmation associé "choregraphe". Test des différents modes de connexion du robot afin de choisir le plus adapté aux futurs exercices.


Semaine 10: 17/11 - 23/11

Développement de la première séquence sur "choregraphe" qui permettra une première approche entre les enfants et le NAO. Naoprojet.png


Semaine 11: 24/11 - 30/11

Finalisation de la séquence, vérification et test.

Cette séquence sera réalisée de la façon suivante:

-NAO se lève puis, fait quelques pas et se présente en disant "Bonjour je suis NAO et je suis un robot, et toi, comment t'appelles tu ?"

-Un enfant arrive et lui répond. Lorsque c'est chose faite, un éducateur appuie sur la tête du NAO.

-NAO répond alors "Appuie sur ma main" en tendant la main droite.

-NAO attend un contact avec sa main droite puis répond "c'est bien".

-A ce stade, l'éducateur à deux possibilités: appuyer sur la tête du NAO pour que celui ci demande à un second enfant de se présenter puis d'appuyer sur sa main droite, ou appuyer sur sa main gauche afin que le robot exécute une danse et dise au revoir aux enfants.

Séquence réalisée afin de favoriser un premier contact visuel, tactile et auditif avec les enfants.

Prise de rendez vous avec les représentants de GAPAS afin d'effectuer un premier test avec les enfants.

Semaine 12: 01/12 - 07/12

Amélioration du programme précédent, notamment en ajoutant un bruitage encourageant lorsque l'enfant réussi a appuyer sur la main du robot, et en ajoutant une musique d'introduction lorsque le robot se lève et fait quelques pas.

Ajout de quelques mouvements afin d'accompagner les paroles de NAO.

Confirmation du rendez vous à l'IME pour la semaine suivante.

Semaine 12: 08/12 - 14/12

expérience avec les enfants

Mois de Janvier

Après avoir fait plusieurs exercices avec les enfants en touchant divers stimulis, nous avons donc pris la décision d'axer des exercices que sur l'audition et de mettre au point un remote controlable par un éducateur

Nous avons donc créé des fonctions de base de mouvements en langage c# afin de mettre une application intuitive sous Windows. Fonction StandUp "Robot se lève peu importe sa position de base"

"using System; using System.Collections.Generic; using System.Linq; using System.Text; using Aldebaran.Proxies;

namespace StandUp {

   class StandUpExecuter
   {
       MotionProxy motionProxy;
       RobotPoseProxy robotPoseProxy;
       public StandUpExecuter(MotionProxy motionProxy, RobotPoseProxy robotPoseProxy)
       {
           this.motionProxy = motionProxy;
           this.robotPoseProxy = robotPoseProxy;
       }
       public bool Start()
       {
           NAOPositions curPosition = GetPosition();
           if (curPosition == null || curPosition == NAOPositions.Unknown || curPosition == NAOPositions.HeadBack)
           {
               return false;
           }
           motionProxy.stiffnessInterpolation("Body", 1, 1);
           while (curPosition != NAOPositions.Stand)
           {
               switch (curPosition)
               {
                   case NAOPositions.Sit:
                       ChangePositionFromSitToCrouch();
                       ChangePositionFromCrouchToStand();
                       break;
                   case NAOPositions.Crouch:
                       ChangePositionFromCrouchToStand();
                       break;
                   case NAOPositions.Belly:
                   case NAOPositions.Frog:
                   case NAOPositions.Knee:
                       ChangePositionFromBellyToStand();
                       break;
                   case NAOPositions.Back:
                       ChangePositionFromBackToStand();
                       break;
                   case NAOPositions.Right:
                   case NAOPositions.Left:
                       ChangePositionFromSideToBelly();
                       break;
                   case NAOPositions.HeadBack:
                       break;
                   case NAOPositions.Unknown:
                       return false;
                   default:
                       return false;
               }
               curPosition = GetPosition();
           }
           return true;
       }
       private void ChangePositionFromSideToBelly()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();


           names.Add("HeadYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.72094f, 0.72094f });
           names.Add("HeadPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.14876f, 0.14876f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 1.89752f, 1.89752f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.80224f, 0.80224f });
           names.Add("LElbowYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.99254f, -0.99254f });
           names.Add("LElbowRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.46783f, -0.46783f });
           names.Add("LWristYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.67807f, -0.67807f });
           names.Add("LHand");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.00350f, 0.00350f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 1.97891f, 1.97891f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.77931f, -0.77931f });
           names.Add("RElbowYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.92343f, 0.92343f });
           names.Add("RElbowRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.34059f, 0.34059f });
           names.Add("RWristYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.64577f, 0.64577f });
           names.Add("RHand");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.00256f, 0.00256f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -1.14432f, -1.14432f });
           names.Add("LHipRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.25162f, 0.25162f });
           names.Add("LHipPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.03532f, 0.03532f });
           names.Add("LKneePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.09208f, -0.09208f });
           names.Add("LAnklePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.35585f, 0.35585f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.34366f, 0.34366f });
           names.Add("RHipRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.03064f, -0.03064f });
           names.Add("RHipPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.41567f, 0.41567f });
           names.Add("RKneePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.09200f, -0.09200f });
           names.Add("RAnklePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.52314f, 0.52314f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.06745f, -0.06745f });


           ChangePosition(names, times, keys);
       }
       private void ChangePositionFromBackToStand()
       {
           var bodyPart = NAOBodyParts.Arms.ToString();
           motionProxy.setCollisionProtectionEnabled(bodyPart, false);
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -0.02459f, -0.00000f, 0.04138f, 0.00000f, 0.00000f, 0.00000f, -0.51393f, -0.30224f, 0.26354f, 0.12043f, -0.00940f, 0.06592f });
           names.Add("HeadPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.06285f, 0.00000f, 0.51490f, -0.67200f, 0.00000f, 0.34907f, 0.17177f, 0.34511f, 0.01047f, 0.02967f, 0.37886f, -0.03993f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 2.08560f, 2.08567f, 2.03865f, 2.08560f, 2.08560f, 2.08560f, 2.03251f, 1.28698f, 0.95717f, 0.73321f, 0.73321f, 1.59225f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.00000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.98018f, 0.36652f, 0.34907f, 0.34907f, 0.36652f, 0.00870f, 0.29909f, 0.44331f, 0.07052f, 0.26687f, 0.49851f, 0.49851f, 0.21932f });
           names.Add("LElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -0.76091f, 0.15708f, 0.12268f, 0.08727f, 0.08727f, 0.08727f, -1.96049f, -0.85448f, -0.25008f, 0.08308f, 0.08308f, -1.03089f });
           names.Add("LElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.20000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -0.62430f, -0.03491f, -1.54462f, -1.56210f, -0.69813f, -0.00870f, -0.19199f, -1.04615f, -0.87280f, -0.82985f, -0.80535f, -0.80535f, -0.68105f });
           names.Add("LWristYaw");
           times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
           keys.Add(new[] { -0.70568f, -1.78868f, -0.72562f, -0.71949f, -0.72256f, -0.71182f });
           names.Add("LHand");
           times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
           keys.Add(new[] { 0.00088f, 0.00133f, 0.00219f, 0.00269f, 0.00290f, 0.00451f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 2.08560f, 2.08567f, 1.95283f, 2.08560f, 2.08560f, 2.08560f, 2.07708f, 2.03720f, 1.44047f, 0.89131f, 0.89131f, 1.46961f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -1.07384f, -0.36652f, -0.34907f, -0.34907f, -0.36652f, -0.00870f, -0.45717f, -0.56455f, -0.70568f, -0.87266f, -0.68068f, -0.15037f });
           names.Add("RElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.76542f, -0.15708f, -0.25775f, -0.08727f, -0.08727f, -0.08727f, -0.08134f, -0.06907f, -0.02612f, 0.00062f, 0.00062f, 0.91269f });
           names.Add("RElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.71028f, 0.03491f, 1.54462f, 1.56210f, 0.69813f, 0.00870f, 0.09668f, 0.02152f, 0.19026f, 0.45379f, 0.55995f, 0.38047f });
           names.Add("RWristYaw");
           times.Add(new[] { 1.00000f, 2.60000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
           keys.Add(new[] { 1.15199f, 1.72571f, 1.07989f, 1.07836f, 1.07989f, 1.08450f });
           names.Add("RHand");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 4.30000f, 5.60000f, 6.50000f, 7.40000f, 10.50000f });
           keys.Add(new[] { 0.00000f, 0.00000f, 0.00032f, 0.00000f, 0.00287f, 0.00300f, 0.00308f, 0.00376f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -0.47090f, 0.00000f, 0.00771f, -0.00000f, -0.65498f, -0.49851f, -0.85900f, -0.69639f, -0.40225f, -0.40225f, -0.23006f });
           names.Add("LHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.04453f, 0.00000f, 0.05833f, 0.00000f, -0.06478f, 0.54105f, 0.15498f, -0.17483f, 0.00925f, 0.19199f, 0.17453f, 0.15191f });
           names.Add("LHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.25008f, -0.17453f, 0.37434f, -0.17453f, -1.17808f, -1.57080f, -0.85746f, -0.05672f, -0.45556f, -0.85521f, -0.83599f, 0.21327f });
           names.Add("LKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.09200f, 1.67552f, 2.02024f, 1.67552f, 1.07777f, 1.67552f, 2.11075f, 2.11227f, 2.11253f, 2.11253f, 2.11253f, -0.08901f });
           names.Add("LAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.82525f, 0.24435f, -0.45411f, 0.24435f, 0.67748f, 0.66323f, -0.45564f, -1.10145f, -1.18952f, -1.18952f, -1.18952f, 0.08126f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -0.03371f, 0.00000f, -0.03371f, 0.00000f, 0.17109f, -0.10472f, -0.39573f, -0.09967f, 0.04299f, 0.11810f, 0.08727f, -0.13648f });
           names.Add("RHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { -0.14876f, 0.00000f, -0.02297f, 0.00000f, 0.06190f, -0.54105f, -0.55833f, -0.61049f, -0.62657f, -0.29671f, -0.01745f, -0.06285f });
           names.Add("RHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.18557f, -0.17453f, 0.28221f, -0.17453f, -1.16456f, -1.57080f, -1.52484f, -1.56012f, -1.02974f, -0.90583f, -0.90583f, 0.20858f });
           names.Add("RKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.19179f, 1.67552f, 2.02645f, 1.67552f, 1.09386f, 1.67552f, 1.22111f, 1.09992f, 1.08385f, 0.87616f, 1.76278f, -0.07666f });
           names.Add("RAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.46638f, 0.24435f, -0.37425f, 0.24435f, 0.71737f, 0.66323f, 0.78545f, 0.78392f, 0.44157f, 0.40317f, -0.57945f, 0.08134f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.20000f, 3.60000f, 4.30000f, 4.80000f, 5.60000f, 6.50000f, 7.40000f, 8.00000f, 8.60000f, 10.50000f });
           keys.Add(new[] { 0.16725f, 0.00000f, -0.03984f, 0.00000f, -0.16084f, 0.10472f, 0.00925f, -0.09813f, 0.44331f, 0.67960f, 0.27751f, 0.06294f });


           ChangePosition(names, times, keys);
           motionProxy.setCollisionProtectionEnabled(bodyPart, true);
       }
       private void ChangePositionFromBellyToStand()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00000f, -0.22689f, 0.28623f, 0.29671f, -0.49567f, -0.29671f, 0.05236f, -0.39095f, 0.06745f });
           names.Add("HeadPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.67195f, -0.54768f, 0.10734f, 0.51487f, 0.38048f, 0.37119f, -0.10472f, -0.53387f, 0.12741f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.08433f, -1.51146f, -1.25025f, 0.07206f, 1.27409f, 0.75573f, 0.75049f, 1.29154f, 1.59378f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 1.55390f, 0.01683f, 0.07666f, 0.07052f, 0.15643f, 0.93899f, 0.67719f, 0.84648f, 0.21779f });
           names.Add("LElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -2.07694f, -1.58006f, -1.60461f, -1.78715f, -1.32695f, -1.24791f, -0.97260f, -0.95993f, -1.01862f });
           names.Add("LElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.00873f, -0.35278f, -0.63810f, -0.85133f, -1.55083f, -0.73304f, -0.73653f, -1.15506f, -0.68105f });
           names.Add("LWristYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -1.55092f, -1.55092f, -1.51717f, -1.44814f, -1.40519f, -1.40825f, -1.40825f, -0.54454f, -0.71028f });
           names.Add("LHand");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00102f, 0.00034f, 0.00071f, 0.00095f, 0.00100f, 0.00100f, 0.00096f, 0.00100f, 0.00455f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.02757f, -1.51146f, -1.22256f, -0.23619f, 0.21787f, 0.44950f, 0.91431f, 0.96033f, 1.47882f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -1.53558f, -0.19199f, -0.08288f, -0.08288f, -0.22707f, -0.18259f, -0.00870f, -0.13197f, -0.14884f });
           names.Add("RElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 2.07694f, 1.56157f, 1.61373f, 1.68582f, 1.96041f, 1.95121f, 0.66571f, 0.39573f, 0.90962f });
           names.Add("RElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.10472f, 0.38201f, 0.63512f, 1.55705f, 0.00870f, 0.00870f, 0.42343f, 0.64926f, 0.39275f });
           names.Add("RWristYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 1.50941f, 1.50941f, 1.49714f, 1.42811f, 1.43578f, 1.44038f, 1.44345f, 1.44038f, 1.08603f });
           names.Add("RHand");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00032f, 0.00022f, 0.00046f, 0.00082f, 0.00086f, 0.00084f, 0.00083f, 0.00084f, 0.00380f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.02269f, 0.03491f, -0.43561f, -0.77923f, -1.04154f, -1.14530f, -1.14530f, -0.56754f, -0.23619f });
           names.Add("LHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00000f, 0.00004f, 0.00158f, -0.37732f, -0.29755f, -0.29755f, 0.19486f, 0.12736f, 0.14884f });
           names.Add("LHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.80460f, 0.00004f, -1.56924f, -1.28085f, -1.03694f, -1.15966f, -1.18267f, -1.27011f, 0.21327f });
           names.Add("LKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.09233f, 1.98968f, 2.11253f, 0.28221f, 0.40493f, 0.35738f, 0.71940f, 2.01409f, -0.09055f });
           names.Add("LAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.64403f, -1.02974f, -1.15054f, 0.21625f, 0.71020f, 0.92275f, 0.82525f, -0.50166f, 0.07666f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00000f, 0.00004f, -0.00149f, -0.45249f, -0.30062f, -0.11808f, -0.04138f, -0.12114f, -0.13495f });
           names.Add("RHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00000f, 0.00004f, 0.00158f, 0.31144f, 0.25469f, 0.32065f, 0.22707f, -0.07512f, -0.05978f });
           names.Add("RHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.80460f, -0.00004f, -1.57699f, -1.66136f, -1.19963f, -1.59847f, -0.32218f, -0.71028f, 0.21012f });
           names.Add("RKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.09233f, 1.98968f, 2.08628f, 1.74267f, 2.12019f, 2.12019f, 2.12019f, 2.12019f, -0.07666f });
           names.Add("RAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.64403f, -1.02974f, -1.13819f, -1.18645f, -1.18645f, -0.58901f, -1.18645f, -1.18645f, 0.08595f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.00000f, 0.00004f, 0.00618f, -0.00456f, -0.09813f, -0.01376f, -0.09507f, 0.03532f, 0.06140f });


           ChangePosition(names, times, keys);
       }
       private void ChangePositionFromCrouchToStand()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();


           names.Add("HeadYaw");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.06745f });
           names.Add("HeadPitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.04146f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 1.58611f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.22392f });
           names.Add("LElbowYaw");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -1.02169f });
           names.Add("LElbowRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.65958f });
           names.Add("LWristYaw");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.75170f });
           names.Add("LHand");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.00456f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 1.46808f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.14731f });
           names.Add("RElbowYaw");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.90962f });
           names.Add("RElbowRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.37127f });
           names.Add("RWristYaw");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 1.05535f });
           names.Add("RHand");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.00381f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.23926f });
           names.Add("LHipRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.14884f });
           names.Add("LHipPitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.21173f });
           names.Add("LKneePitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.09055f });
           names.Add("LAnklePitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.08126f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.13342f });
           names.Add("RHipRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.05825f });
           names.Add("RHipPitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.21165f });
           names.Add("RKneePitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { -0.07666f });
           names.Add("RAnklePitch");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.07981f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 1.50000f });
           keys.Add(new[] { 0.05987f });
           ChangePosition(names, times, keys);
       }
       private NAOPositions GetPosition()
       {
           var curPositionAndTime = (System.Collections.ArrayList)robotPoseProxy.getActualPoseAndTime();
           string curPositionStr = curPositionAndTime[0].ToString();
           NAOPositions curPosition;
           Enum.TryParse<NAOPositions>(curPositionStr, true, out curPosition);
           return curPosition;
       }
       private void ChangePositionFromSitToCrouch()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 0.00149f, -0.27925f, -0.34826f, -0.47124f, -0.52360f, -0.28690f, -0.00940f, -0.00940f });
           names.Add("HeadPitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 0.37119f, 0.36965f, 0.22689f, 0.27576f, 0.15708f, 0.34971f, 0.37886f, 0.37886f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 0.49567f, 0.57421f, 0.62657f, 1.01393f, 0.65101f, 0.75162f, 1.12050f, 1.17286f, 1.40848f, 1.06814f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 0.60000f, 0.84000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 0.24386f, 0.49393f, 0.41567f, 0.50964f, 0.75398f, 0.61087f, 0.51078f, 0.07666f, 0.15489f, 0.68872f, 0.59208f });
           names.Add("LElbowYaw");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { -0.54461f, -0.71182f, -0.73636f, -0.81766f, -0.29764f, -0.16265f, -0.86982f, -1.01402f, -0.92044f, -0.36513f });
           names.Add("LElbowRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 2.56000f, 3.00000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { -1.06302f, -0.93416f, -0.79304f, -0.94183f, -1.06762f, -0.62430f, -0.89121f, -0.85133f, -1.23636f, -1.07836f });
           names.Add("LWristYaw");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
           keys.Add(new[] { -0.74250f, -0.77164f, -0.73636f });
           names.Add("LHand");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
           keys.Add(new[] { 0.00032f, 0.00116f, 0.00126f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 0.60000f, 0.84000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 0.56609f, 0.73304f, 1.32695f, 1.96664f, 2.08560f, 2.06787f, 2.08560f, 2.02799f, 1.61381f, 1.42359f, 1.21650f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 1.92000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { -0.35133f, -0.67500f, -0.63819f, -0.62832f, -0.00873f, -0.50933f, -0.55382f, -0.56455f, -0.66580f, -0.57683f, -0.26236f });
           names.Add("RElbowYaw");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 0.48010f, 1.82235f, 2.05399f, -0.08727f, -0.08134f, -0.07674f, -0.06600f, 0.31596f, 1.67969f, 0.96638f });
           names.Add("RElbowRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 1.92000f, 2.20000f, 3.00000f, 3.40000f, 3.76000f, 4.36000f, 5.04000f, 7.20000f });
           keys.Add(new[] { 1.21650f, 1.02936f, 0.68114f, 0.85521f, 0.00873f, 0.07981f, 0.05987f, 0.01692f, 0.33292f, 0.49706f, 0.97260f });
           names.Add("RWristYaw");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
           keys.Add(new[] { 1.07529f, 1.09523f, 1.08756f });
           names.Add("RHand");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f });
           keys.Add(new[] { 0.00032f, 0.00221f, 0.00244f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { -0.67645f, -0.65173f, -0.75144f, -0.49909f, -0.64559f, -0.85897f, -0.43408f, -0.28221f, -0.17330f });
           names.Add("LHipRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { 0.13964f, 0.14501f, 0.14808f, 0.54105f, 0.15498f, -0.02526f, -0.17453f, 0.18259f, 0.44030f, 0.13043f });
           names.Add("LHipPitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 4.36000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { -1.57077f, -1.57080f, -1.57080f, -1.57080f, -0.85706f, -0.59586f, -0.05236f, -0.40143f, -0.69026f, -0.75469f, -0.73014f });
           names.Add("LKneePitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { 0.72401f, 0.95923f, 1.18473f, 1.67552f, 2.11255f, 2.11255f, 2.11255f, 2.00182f, 2.11253f, 1.61833f });
           names.Add("LAnklePitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { 0.77616f, 0.75542f, 0.75542f, 0.66323f, -0.45379f, -0.68068f, -1.09956f, -1.18944f, -1.18944f, -0.73636f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { -0.05058f, -0.00550f, 0.02671f, -0.17453f, -0.39573f, -0.08681f, -0.10379f, 0.01078f, -0.01683f, -0.08893f });
           names.Add("RHipRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { -0.14415f, -0.13336f, -0.21006f, -0.54105f, -0.55842f, -0.73468f, -0.61087f, -0.25460f, -0.08126f, -0.07512f });
           names.Add("RHipPitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { -1.55245f, -1.57080f, -1.57080f, -1.57080f, -1.52484f, -1.57080f, -1.55965f, -0.84988f, -0.67807f, -0.64892f });
           names.Add("RKneePitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { 0.84374f, 0.95426f, 1.21504f, 1.67552f, 1.22173f, 1.27333f, 1.09956f, 0.59523f, 1.44047f, 1.58006f });
           names.Add("RAnklePitch");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.76000f, 4.36000f, 4.72000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { 0.78545f, 0.76027f, 0.76027f, 0.66323f, 0.78540f, 0.66976f, 0.78540f, 0.78540f, 0.54105f, 0.48172f, -0.36965f, -0.78843f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.28000f, 3.52000f, 3.76000f, 4.36000f, 4.72000f, 5.04000f, 5.80000f, 7.20000f });
           keys.Add(new[] { 0.04913f, 0.00073f, -0.00081f, 0.17453f, 0.00929f, 0.07436f, -0.15708f, -0.10472f, 0.24435f, 0.34907f, 0.44030f, 0.42343f, 0.09055f });
           ChangePosition(names, times, keys);
       }
       private void ChangePosition(List<string> names, List<float[]> times, List<float[]> keys)
       {
           motionProxy.angleInterpolation(names, keys, times, true);
       }
   }

}"

Fonction SitDown " Le robot s'asseoit peu importe sa position de base" "using System; using System.Collections.Generic; using System.Linq; using System.Text; using Aldebaran.Proxies;

namespace StandUp {

   class SitDownExecuter
   {
       MotionProxy motionProxy;
       RobotPoseProxy robotPoseProxy;
       public SitDownExecuter(MotionProxy motionProxy, RobotPoseProxy robotPoseProxy)
       {
           this.motionProxy = motionProxy;
           this.robotPoseProxy = robotPoseProxy;
       }
    public bool Start()
       {
           NAOPositions curPosition = GetPosition();
           if (curPosition == NAOPositions.Unknown || curPosition == NAOPositions.HeadBack)
           {
               return false;
           }
           motionProxy.stiffnessInterpolation(NAOBodyParts.Body.ToString(), 1, 1);
           while (curPosition != NAOPositions.Sit)
           {
               switch (curPosition)
               {
                   case NAOPositions.Belly:
                   case NAOPositions.Frog:
                   case NAOPositions.Knee:
                       ChangePositionFromBellyToCrouch();
                       break;
                   case NAOPositions.Back:
                       ChangePositionFromBackToSitDown();
                       break;
                   case NAOPositions.Right:
                   case NAOPositions.Left:
                       ChangePositionFromSideToBelly();
                       break;
                   case NAOPositions.Stand:
                   case NAOPositions.Crouch:
                       ChangePositionFromCrouchToSitDown();
                       break;
                   case NAOPositions.HeadBack:
                       return false;
                       break;
                   case NAOPositions.Unknown:
                       return false;
                   default:
                       return false;
               }
               curPosition = GetPosition();
           }
           //motionProxy.stiffnessInterpolation(NAOBodyParts.Body.ToString(), 0, 1);//Release stiffness
           return true;
       }
       private void ChangePositionFromCrouchToSitDown()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.31605f, -0.44030f, -0.63358f, -0.62745f, -0.40962f, -0.18412f, -0.00004f, -0.02152f });
           names.Add("HeadPitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.37119f, 0.37579f, 0.36812f, 0.35738f, 0.36812f, 0.36658f, 0.37886f, -0.03379f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 1.30000f, 2.40000f, 2.90000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 1.04371f, 0.72955f, 0.72955f, 1.01086f, 1.50481f, 2.03404f, 1.53764f, 0.67719f, 0.91269f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.33590f, 0.53532f, 0.48171f, 0.83776f, 0.44331f, 1.55683f, 0.56907f, 0.21932f });
           names.Add("LElbowYaw");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.56302f, -0.08552f, 0.10580f, -1.55334f, -0.32985f, -1.20777f, -0.76091f, -0.43723f });
           names.Add("LElbowRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 4.70000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.95871f, -1.20253f, -0.69813f, -0.90234f, -0.48695f, -0.68591f, -0.95993f, -1.32227f, -1.28085f });
           names.Add("LWristYaw");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.08740f, -0.71642f, -0.71642f, -0.71642f, -0.72869f, -0.73023f, -0.72256f, 0.05978f });
           names.Add("LHand");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.00404f, 0.00117f, 0.00116f, 0.00116f, 0.00128f, 0.00121f, 0.00106f, 0.00524f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.80809f, 0.59341f, 1.74420f, 2.07694f, 2.08560f, 1.88839f, 0.78191f, 0.93118f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 2.90000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.56762f, -0.93026f, -0.76271f, -0.55327f, -0.52534f, -0.55327f, -1.55683f, -0.54921f, -0.26389f });
           names.Add("RElbowYaw");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.13963f, 0.00456f, -0.06907f, -0.14731f, 0.11694f, 1.75179f, 1.12745f, 0.51845f });
           names.Add("RElbowRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.83776f, 0.47865f, 0.07828f, 0.10129f, 0.59167f, 1.03856f, 1.29320f, 1.28247f });
           names.Add("RWristYaw");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.18864f, 1.05535f, 1.05228f, 1.04308f, 1.04308f, 1.04768f, 1.06149f, -0.05987f });
           names.Add("RHand");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 3.90000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.00709f, 0.00149f, 0.00154f, 0.00180f, 0.00206f, 0.00201f, 0.00211f, 0.00523f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.39726f, -0.42334f, -0.90809f, -0.58595f, -0.53993f, -0.79304f, -0.77770f, -0.61356f });
           names.Add("LHipRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.18259f, 0.31416f, -0.26227f, 0.29304f, 0.52927f, 0.23321f, 0.35746f, 0.27923f });
           names.Add("LHipPitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.59055f, -0.86974f, -0.00916f, -0.62123f, -1.55390f, -1.59378f, -1.46493f, -1.57998f });
           names.Add("LKneePitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 2.11253f, 2.11253f, 1.81774f, 1.98189f, 1.07687f, 1.15813f, 1.08756f, 1.39743f });
           names.Add("LAnklePitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -1.18952f, -1.18952f, -1.18952f, -0.52314f, 0.73935f, 0.92275f, 0.92189f, 0.83599f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.04299f, 0.07981f, 0.04146f, -0.45402f, -0.18097f, -0.04751f, -0.05058f, -0.00916f });
           names.Add("RHipRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.13802f, -0.25614f, -0.38039f, -0.57521f, -0.53532f, -0.21472f, -0.32210f, -0.26841f });
           names.Add("RHipPitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.76551f, -0.85908f, -1.36530f, -1.65063f, -1.54171f, -1.60921f, -1.41746f, -1.58620f });
           names.Add("RKneePitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 1.89300f, 0.77471f, 1.14287f, 1.14594f, 1.03847f, 1.19810f, 0.98027f, 1.40672f });
           names.Add("RAnklePitch");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { -0.73014f, 0.33445f, 0.45717f, 0.77164f, 0.71335f, 0.93206f, 0.93206f, 0.84988f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 1.30000f, 2.40000f, 3.40000f, 4.30000f, 5.20000f, 5.90000f, 6.60000f, 7.20000f });
           keys.Add(new[] { 0.21020f, 0.50626f, 0.05833f, -0.05518f, 0.18872f, 0.03532f, 0.04760f, 0.01845f });
           ChangePosition(names, times, keys);
       }
       private void ChangePositionFromBackToSitDown()
       {
           var bodyPart = NAOBodyParts.Arms.ToString();
           motionProxy.setCollisionProtectionEnabled(bodyPart, false);
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { -0.02459f, -0.00000f, 0.04138f, 0.00000f, -0.00618f, 0.01376f, 0.02450f, -0.02305f });
           names.Add("HeadPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 2.80000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { 0.06285f, 0.00000f, 0.51490f, -0.67200f, -0.02152f, 0.19478f, 0.07359f, -0.03532f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 5.90000f, 6.20000f });
           keys.Add(new[] { 2.08560f, 2.09440f, 2.03865f, 2.08560f, 2.08567f, 1.30846f, 0.90348f, 0.70337f, 0.90195f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { 0.98018f, 0.36652f, 0.34907f, 0.34907f, 0.40143f, 1.17193f, 0.39726f, 0.21625f });
           names.Add("LElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { -0.76091f, 0.15708f, 0.12268f, 0.08727f, 0.05978f, -0.95419f, -0.96339f, -0.43877f });
           names.Add("LElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 4.60000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { -0.62430f, 0.00000f, -1.54462f, -1.56210f, -0.87441f, -1.03498f, -0.67952f, -1.12285f, -1.23483f });
           names.Add("LWristYaw");
           times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { -0.70568f, -1.78868f, -0.71949f, -0.04760f, 0.05978f });
           names.Add("LHand");
           times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.00088f, 0.00133f, 0.00158f, 0.00022f, 0.00524f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 5.90000f, 6.20000f });
           keys.Add(new[] { 2.08560f, 2.09440f, 1.95283f, 2.08560f, 2.08567f, 1.91908f, 1.03703f, 0.72955f, 0.92504f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { -1.07384f, -0.36652f, -0.34907f, -0.34907f, -0.41015f, -0.89897f, -0.74096f, -0.24241f });
           names.Add("RElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { 0.76542f, -0.15708f, -0.25775f, -0.08727f, -0.06600f, 1.57691f, 1.06762f, 0.52459f });
           names.Add("RElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 3.60000f, 4.40000f, 4.60000f, 5.00000f, 5.60000f, 6.20000f });
           keys.Add(new[] { 0.71028f, 0.00000f, 1.54462f, 1.56210f, 0.71384f, 1.05418f, 0.95726f, 1.14594f, 1.27019f });
           names.Add("RWristYaw");
           times.Add(new[] { 1.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 1.15199f, 1.72571f, 1.11057f, 0.24693f, -0.05680f });
           names.Add("RHand");
           times.Add(new[] { 1.00000f, 2.00000f, 2.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.00000f, 0.00000f, 0.00032f, 0.00005f, 0.00221f, 0.00523f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { -0.47090f, 0.00000f, 0.00771f, -0.00000f, -0.65498f, -0.70407f, -0.61816f });
           names.Add("LHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.04453f, 0.00000f, 0.05833f, 0.00000f, -0.06439f, 0.14083f, 0.27923f });
           names.Add("LHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.25008f, -0.17453f, 0.37434f, -0.17453f, -1.17807f, -1.57080f, -1.57998f });
           names.Add("LKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.09200f, 1.67552f, 2.02024f, 1.67552f, 1.07683f, 1.40161f, 1.39590f });
           names.Add("LAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.82525f, 0.24435f, -0.45411f, 0.24435f, 0.67799f, 0.77609f, 0.83599f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { -0.03371f, 0.00000f, -0.03371f, 0.00000f, 0.17032f, 0.09083f, -0.00916f });
           names.Add("RHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { -0.14876f, 0.00000f, -0.02297f, 0.00000f, 0.06140f, -0.12200f, -0.26841f });
           names.Add("RHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.18557f, -0.17453f, 0.28221f, -0.17453f, -1.16435f, -1.57080f, -1.59233f });
           names.Add("RKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.19179f, 1.67552f, 2.02645f, 1.67552f, 1.09532f, 1.37682f, 1.40825f });
           names.Add("RAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.46638f, 0.24435f, -0.37425f, 0.24435f, 0.71795f, 0.76985f, 0.84988f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.10000f, 3.60000f, 4.40000f, 5.00000f, 6.20000f });
           keys.Add(new[] { 0.16725f, 0.00000f, -0.03984f, 0.00000f, -0.14415f, -0.11992f, 0.01845f });


           ChangePosition(names, times, keys);
           motionProxy.setCollisionProtectionEnabled(bodyPart, true);
       }
       private void ChangePositionFromBellyToCrouch()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.17453f, -0.22689f, 0.28623f, 0.29671f, -0.49567f, -0.29671f, 0.05236f, -0.39095f, -0.01745f });
           names.Add("HeadPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.57683f, -0.54768f, 0.10734f, 0.51487f, 0.38048f, 0.37119f, -0.10472f, -0.53387f, 0.00698f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.08433f, -1.51146f, -1.25025f, 0.07206f, 1.27409f, 0.75573f, 0.75049f, 1.29154f, 1.51708f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 1.55390f, 0.01683f, 0.07666f, 0.07052f, 0.15643f, 0.93899f, 0.67719f, 0.84648f, 0.08433f });
           names.Add("LElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -2.07694f, -1.58006f, -1.60461f, -1.78715f, -1.32695f, -1.24791f, -0.97260f, -0.95993f, -0.90203f });
           names.Add("LElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.00873f, -0.35278f, -0.63810f, -0.85133f, -1.55083f, -0.73304f, -0.73653f, -1.15506f, -1.09984f });
           names.Add("LWristYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
           keys.Add(new[] { -1.55092f, -1.55092f, -1.51717f, -1.44814f, -1.40519f, -1.40825f, -1.40825f, -0.54454f });
           names.Add("LHand");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
           keys.Add(new[] { 0.00102f, 0.00034f, 0.00071f, 0.00095f, 0.00100f, 0.00100f, 0.00096f, 0.00100f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.02757f, -1.51146f, -1.22256f, -0.23619f, 0.21787f, 0.44950f, 0.91431f, 0.96033f, 1.35303f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -1.53558f, -0.19199f, -0.08288f, -0.08288f, -0.22707f, -0.18259f, -0.00870f, -0.13197f, -0.01385f });
           names.Add("RElbowYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 2.07694f, 1.56157f, 1.61373f, 1.68582f, 1.96041f, 1.95121f, 0.66571f, 0.39573f, 0.86667f });
           names.Add("RElbowRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.10472f, 0.38201f, 0.63512f, 1.55705f, 0.00870f, 0.00870f, 0.42343f, 0.64926f, 0.83454f });
           names.Add("RWristYaw");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
           keys.Add(new[] { 1.50941f, 1.50941f, 1.49714f, 1.42811f, 1.43578f, 1.44038f, 1.44345f, 1.44038f });
           names.Add("RHand");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f });
           keys.Add(new[] { 0.00032f, 0.00022f, 0.00046f, 0.00082f, 0.00086f, 0.00084f, 0.00083f, 0.00084f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.03371f, 0.03491f, -0.43561f, -0.77923f, -1.04154f, -1.14530f, -1.14530f, -0.56754f, 0.00000f });
           names.Add("LHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.06294f, 0.00004f, 0.00158f, -0.37732f, -0.29755f, -0.29755f, 0.19486f, 0.12736f, -0.01047f });
           names.Add("LHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.06140f, 0.00004f, -1.56924f, -1.28085f, -1.03694f, -1.15966f, -1.18267f, -1.27011f, -1.04720f });
           names.Add("LKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.12043f, 1.98968f, 2.11253f, 0.28221f, 0.40493f, 0.35738f, 0.71940f, 2.01409f, 2.09440f });
           names.Add("LAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.92189f, -1.02974f, -1.15054f, 0.21625f, 0.71020f, 0.92275f, 0.82525f, -0.50166f, -1.04720f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.00149f, 0.00004f, -0.00149f, -0.45249f, -0.30062f, -0.11808f, -0.04138f, -0.12114f, 0.00000f });
           names.Add("RHipRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.03142f, 0.00004f, 0.00158f, 0.31144f, 0.25469f, 0.32065f, 0.22707f, -0.07512f, 0.01047f });
           names.Add("RHipPitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.07666f, -0.00004f, -1.57699f, -1.66136f, -1.19963f, -1.59847f, -0.32218f, -0.71028f, -1.04720f });
           names.Add("RKneePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { -0.07819f, 1.98968f, 2.08628f, 1.74267f, 2.12019f, 2.12019f, 2.12019f, 2.12019f, 2.09440f });
           names.Add("RAnklePitch");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.92965f, -1.02974f, -1.13819f, -1.18645f, -1.18645f, -0.58901f, -1.18645f, -1.18645f, -1.04720f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 1.00000f, 2.00000f, 3.40000f, 4.50000f, 5.80000f, 6.50000f, 7.60000f, 8.90000f, 10.00000f });
           keys.Add(new[] { 0.18850f, 0.00004f, 0.00618f, -0.00456f, -0.09813f, -0.01376f, -0.09507f, 0.03532f, 0.00000f });


           ChangePosition(names, times, keys);
       }
       private void ChangePositionFromSideToBelly()
       {
           List<string> names = new List<string>();
           List<float[]> times = new List<float[]>();
           List<float[]> keys = new List<float[]>();
           names.Add("HeadYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.72094f, 0.72094f });
           names.Add("HeadPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.14876f, 0.14876f });
           names.Add("LShoulderPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 1.89752f, 1.89752f });
           names.Add("LShoulderRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.80224f, 0.80224f });
           names.Add("LElbowYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.99254f, -0.99254f });
           names.Add("LElbowRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.46783f, -0.46783f });
           names.Add("LWristYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.67807f, -0.67807f });
           names.Add("LHand");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.00350f, 0.00350f });
           names.Add("RShoulderPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 1.97891f, 1.97891f });
           names.Add("RShoulderRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.77931f, -0.77931f });
           names.Add("RElbowYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.92343f, 0.92343f });
           names.Add("RElbowRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.34059f, 0.34059f });
           names.Add("RWristYaw");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.64577f, 0.64577f });
           names.Add("RHand");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.00256f, 0.00256f });
           names.Add("LHipYawPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -1.14432f, -1.14432f });
           names.Add("LHipRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.25162f, 0.25162f });
           names.Add("LHipPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.03532f, 0.03532f });
           names.Add("LKneePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.09208f, -0.09208f });
           names.Add("LAnklePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.35585f, 0.35585f });
           names.Add("LAnkleRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.34366f, 0.34366f });
           names.Add("RHipRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.03064f, -0.03064f });
           names.Add("RHipPitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.41567f, 0.41567f });
           names.Add("RKneePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.09200f, -0.09200f });
           names.Add("RAnklePitch");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { 0.52314f, 0.52314f });
           names.Add("RAnkleRoll");
           times.Add(new[] { 0.50000f, 1.00000f });
           keys.Add(new[] { -0.06745f, -0.06745f });


           ChangePosition(names, times, keys);
       }
       private NAOPositions GetPosition()
       {
           var curPositionAndTime = (System.Collections.ArrayList)robotPoseProxy.getActualPoseAndTime();
           string curPositionStr = curPositionAndTime[0].ToString();
           NAOPositions curPosition;
           Enum.TryParse<NAOPositions>(curPositionStr, true, out curPosition);
           return curPosition;
       }
       private void ChangePosition(List<string> names, List<float[]> times, List<float[]> keys)
       {
           motionProxy.angleInterpolation(names, keys, times, true);
       }      
   }


}"