P7 Utilisation d'un Robot Nao pour les enfants autistes : Différence entre versions
| 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
Sommaire
- 1 Cahier des charges
- 2 Déroulement du projet
- 2.1 Semaine 1 : 15/09 - 21/09
- 2.2 Semaine 2 : 22/09 - 28/09
- 2.3 Semaine 3 : 29/09 - 05/10
- 2.4 Semaine 4 : 06/10 - 12/10
- 2.5 Semaine 5 : 13/10 - 19/10
- 2.6 Semaine 6 : 20/10 - 26/10
- 2.7 Semaine 7 : 27/10 - 02/11
- 2.8 Semaine 8 : 03/11 - 09/11
- 2.9 Semaine 9 : 10/11 - 16/11
- 2.10 Semaine 10: 17/11 - 23/11
- 2.11 Semaine 11: 24/11 - 30/11
- 2.12 Semaine 12: 01/12 - 07/12
- 2.13 Semaine 12: 08/12 - 14/12
- 2.14 Mois de Janvier
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)
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.
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);
}
}
}"