from poppy.creatures import PoppyTorso

"""
Explanation: Activité - Faire danser PoppyTorso
Première partie : en utilisant, le simulateur V-REP :

Compétences visées par cette activité :
Savoir utiliser des modules en y récupérant des classes. Instancier un objet à partir d'une classe. Utiliser une méthode et un attribut liée à un objet.
Faire le lien entre rotation des moteurs et position du robot dans l'espace.
Faire preuve de créativité en developpant une chorégraphie.

Lien avec les programmes scolaires, voir :
Pour ICN en classe de seconde : http://www.poppy-prof.fr/?page_id=4&id=67<br>
Pour les mathématiques en classe de seconde : http://www.poppy-prof.fr/?page_id=4&id=37

Pour faire fonctionner notre robot, il faut utiliser Python mais pas seulement. Nous allons aussi avoir besoin de ce que l'on appelle une librairie. La librairie qui permet d'utiliser notre robot s'appelle Pypot et elle est entièrement écrite avec le language Python.
Cette librairie a été construite par des chercheurs très compétants et nous allons simplement apprendre à l'utiliser. 
La première chose à faire est d'aller chercher dans la librairie Pypot, les bons "livres", ceux dont nous allons avoir besoin. Ces "livres" se nomment des modules en vocabulaire Python.
Toutes les instructions seront passées au robot via l'interface sur laquelle vous êtes en train de lire ces lignes. Cette interface se nomme Jupyter ou Notebook.
Pour éxécuter les instructions écrites dans une case de Jupyter, il faut :<br>
     _Sélectionner la case en cliquant dessus.<br>
     _Cliquez sur la case lecture située dans la barre de menu : <img src="images/play.jpg" alt="play" /><br>
     _Ou appuyez simultanément sur shitf+entrée.<br>
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
poppy = PoppyTorso(simulator='vrep')

"""
Explanation: Ensuite, vous allez créer un objet s'appellant poppy et étant un robot de type PoppyTorso. Vous pouvez donner le nom que vous souhaitez à votre robot. Il vous suffit d'écrire : 
&gt; nom_du_robot = PoppyTorso(simulator='vrep')
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
poppy.motors






"""
Explanation: Comme toute chose en language Python, notre robot poppy est un objet qui contient d'autres objets qui sont ses moteurs.
Ca y est, si vous arrivez à accéder aux moteurs de Poppy, vous pourrez le faire bouger...
Vous devez donc accéder aux moteurs de poppy (qui se nomment "motors") et qui se trouve à l'intérieur de Poppy pour cela tapez :
&gt; nom_du_robot.motors
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
poppy.head_z.goto_position(90,1)

"""
Explanation: Tous les mouvements sont basés sur des rotations des moteurs situés aux articulations. Il suffit de fixer l'angle que l'on désire pour un moteur. Pour cela, nous pouvons utiliser la méthode :
    > goto_position(angle_en_degrées,temps)
Cette méthode doit s'appliquer aux objets moteurs de l'objet robot :
    > nom_du_robot.nom_du_moteur.goto_position(angle_en_degrées,temps)
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
# pour remettre la simulation à zéro :
poppy.reset_simulation()

"""
Explanation: A présent choisissez un moteur au hasard dans la liste de moteurs obtenues précédemment et faîtes le bouger pour le localiser sur le robot.
Vous devez remplir le tableau suivant avec les noms des 10 moteurs :
<img src="./images/moteur_torso2.jpg" alt="poppy-torso" style="height: 500px;"/>
Le tableau suivant doit être rempli par les élèves.
La correction est donnée à titre indicatif :
Nom du moteur 1 :  ...........               <br>
Nom du moteur 2 :  ...........               <br>
Nom du moteur 3 :  ...........               <br>
Nom du moteur 4 :  ...........               <br>
Nom du moteur 5 :  ...........               <br>
Nom du moteur 6 :  ...........               <br>
Nom du moteur 7 :  ...........               <br>
Nom du moteur 8 :  ...........               <br>
Nom du moteur 9 :  ...........               <br>
Nom du moteur 10 : ...........              <br>
Nom du moteur 11 : ...........              <br>
Nom du moteur 12 : ...........               <br>
Nom du moteur 13 : ...........               <br>
Si lors de vos essais, vous faîtes tomber votre robot, il est important de connaitre l'instruction qui permet de remettre la simulation à zéro :
    > nom_du_robot.reset_simulation()
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
# pour mettre les bras à l'horizontale

poppy.r_shoulder_x.goto_position(-100,1)
poppy.l_shoulder_x.goto_position(100,1)

poppy.r_elbow_y.goto_position(100,1)
poppy.l_elbow_y.goto_position(100,1)

"""
Explanation: Si votre robot ne répond plus et que vous ne comprenez pas pourquoi, le programme de contrôle du robot ou l'interface Jupiter est peut être hors service, dans ce cas vous pouvez recharger les programmes en choissisant Kernel puis Restart dans le menu de Jupyter. Il faut ensuite tout recommencer au début de ce guide. 
Maintenant, à vous de mettre les bras de votre robot à l'horizontale.
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
# pour mettre les bras à l'horizontale

poppy.r_shoulder_x.goto_position(-100,1)
poppy.l_shoulder_x.goto_position(100,1,wait='True')

poppy.r_elbow_y.goto_position(100,1)
poppy.l_elbow_y.goto_position(100,1)

"""
Explanation: Vous avez sans doute remarqué que les mouvements de tous les moteurs s'éxécutent en même temps, en simultané.
Il peut être utile de décomposer les mouvements. Par exemple, pour mettre les bras à l'horizontale : bouger d'abord les épaules puis ensuite les coudes. Pour faire cela, il faut rajouter à la méthode goto_position() un argument wait='True' :
    > nom_du_robot.nom_du_moteur.goto_position(angle_en_degrées,temps,wait='True')
A présent, mettez les brais à l'horizotale en bougeant d'abord les épaules, puis ensuite les coudes :
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
# pour remettre les bras dans leur position de départ :


poppy.r_elbow_y.goto_position(0,1)
poppy.l_elbow_y.goto_position(0,1,wait='True')

poppy.r_shoulder_x.goto_position(0,1)
poppy.l_shoulder_x.goto_position(0,1,wait='True')



"""
Explanation: Les bras sont à l'horizontale, remettez les dans leur position de départ, c'est à dire avec les angles des moteurs à 0 degrés.
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
poppy.head_z.goto_position(40,1,wait='True')
poppy.head_z.goto_position(-40,1,wait='True')
poppy.head_z.goto_position(40,1,wait='True')
poppy.head_z.goto_position(-40,1,wait='True')
poppy.head_z.goto_position(0,1,wait='True')
poppy.r_shoulder_x.goto_position(-90,2)
poppy.l_shoulder_x.goto_position(90,2)
poppy.l_arm_z.goto_position(90,2)
poppy.r_arm_z.goto_position(50,2,wait='True')
poppy.r_shoulder_x.goto_position(0,2)
poppy.l_shoulder_x.goto_position(0,2)
poppy.l_arm_z.goto_position(0,2)
poppy.r_arm_z.goto_position(0,2,wait='True')
poppy.r_shoulder_x.goto_position(-90,2)
poppy.l_shoulder_x.goto_position(90,2)
poppy.l_arm_z.goto_position(-50,2)
poppy.r_arm_z.goto_position(-90,2,wait='True')
poppy.r_shoulder_x.goto_position(0,2)
poppy.l_shoulder_x.goto_position(0,2)
poppy.l_arm_z.goto_position(0,2)
poppy.r_arm_z.goto_position(0,2,wait='True')

poppy.l_arm_z.goto_position(90,3)
poppy.r_arm_z.goto_position(-90,3,wait='True')
poppy.r_arm_z.goto_position(0,3)
poppy.l_arm_z.goto_position(0,3,wait='True')
poppy.l_arm_z.goto_position(90,3)
poppy.r_arm_z.goto_position(-90,3,wait='True')
poppy.r_arm_z.goto_position(0,3)
poppy.l_arm_z.goto_position(0,3,wait='True')
poppy.r_shoulder_x.goto_position(-90,3)
poppy.l_shoulder_x.goto_position(90,3,wait='True')
poppy.r_shoulder_y.goto_position(30,3)
poppy.l_shoulder_y.goto_position(-30,3,wait='True')
poppy.r_shoulder_y.goto_position(-30,3)
poppy.l_shoulder_y.goto_position(30,3,wait='True')
for m in poppy.motors : 
    m.goto_position(0,1)






"""
Explanation: A présent que vous savez, faire bouger votre robot, soyez créatif et inventez une danse pour lui !
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
poppy.close()

"""
Explanation: Pour terminer la simulation, il faut arréter le robot :
    > nom_du_robot.close()
End of explanation
"""


# Ecrivez votre code ci-dessous et éxecutez le. 

# Une correction est donnée à titre indicatif :
poppy = PoppyTorso()

"""
Explanation: Deuxième partie : en utilisant un véritable robot :
Tout le code développé à l'aide du simulateur doit normalement être valide sur un véritable robot. 
Il suffit d'instancier la class robot sans l'argument du simulateur :
&gt; nom_du_robot = PoppyTorso()

Attention dans le cas du controle d'un véritable PoppyTorso, le code doit être éxécuté dans une interface Jupyter qui pointe sur le nom réseau du robot et non pas sur localhost comme pour le simulateur.
End of explanation
"""


