P7 Utilisation d'un Robot Nao pour les enfants autistes
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
- 3 Fonctions Remote du NAO
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. Utilisation du langage C#
Fonctions Remote du NAO
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(); } 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); } }
}"