In [1]:
import time
from poppy.creatures import PoppyHumanoid
from cherry import Cherry
from pypot.primitive.move import MoveRecorder, Move, MovePlayer
In [2]:
cherry = Cherry()
cherry.setup()
for m in cherry.robot.motors :
m.goal_position = 0.0
for m in cherry.robot.head :
m.compliant = False
for m in cherry.robot.arms :
m.compliant = True
cherry.robot.head_z.goal_position = 0.0
cherry.robot.head_y.goal_position = 0.0
In [ ]:
fileName = "../utils/moveRecord/test.move"
In [ ]:
for m in cherry.robot.motors :
m.compliant = False
for m in cherry.robot.motors :
m.goal_position = 0.0
In [ ]:
for m in cherry.robot.motors:
m.moving_speed = 100
for m in cherry.robot.arms :
m.compliant = False
In [ ]:
cherry.robot.rest_tilted_behave.start()
In [ ]:
cherry.robot.rest_position.start()#1
In [ ]:
cherry.robot.me_behave.start()#2
In [ ]:
cherry.robot.rest_open_behave.start()
In [ ]:
cherry.robot.question_behave.start()#4
In [ ]:
cherry.robot.hug_behave.start()#6
In [ ]:
cherry.robot.left_arm_up_behave.start()
In [ ]:
cherry.robot.you_and_me_behave.start()
In [ ]:
cherry.robot.little_laugh_behave.start()
In [ ]:
cherry.robot.left_arms_behave.start()#stickgauche
In [ ]:
cherry.robot.right_arms_behave.start()#stickdroit
In [ ]:
cherry.robot.right_hand_up_behave.start()#stickhaut
In [ ]:
cherry.robot.show_front_hunkers_behave.start()#stickbas
In [ ]:
cherry.robot.show_right_rest_behave.start()#9
In [ ]:
cherry.robot.sigh_behave.start()
In [ ]:
cherry.robot.little_hug_behave.start()
In [ ]:
cherry.robot.open_arms_behave.start()
In [ ]:
cherry.robot.point_arm_left_behave.start()
In [ ]:
cherry.robot.open_move_behave.start()
In [ ]:
cherry.robot.double_me_behave.start()
In [ ]:
cherry.robot.swap_behave.start()
In [ ]:
cherry.robot.disappointment_behave.start()
In [ ]:
cherry.robot.little_arms_up_behave.start()
In [ ]:
for m in cherry.robot.motors :
m.moving_speed = 0
In [ ]:
cherry.robot.active_primitives
cherry.robot.head_y.present_position
#abs(-135 - cherry.robot.r_shoulder_y.present_position)
In [ ]:
move_recorder = MoveRecorder(cherry.robot, 50, cherry.robot.motors)
for m in cherry.robot.arms:
m.compliant = True
cherry.robot.abs_z.compliant = True
move_recorder.start()
In [ ]:
move_recorder.stop()
with open(fileName, 'w') as f:
move_recorder.move.save(f)
In [ ]:
import time
with open("../utils/moveRecord/test.move") as f:
m = Move.load(f)
cherry.robot.compliant = False
move_player = MovePlayer(cherry.robot, m)
move_player.start()
In [ ]:
for m in cherry.robot.arms :
m.compliant = True
In [ ]:
cherry.robot.copy_arm_behave.start()
In [ ]:
cherry.robot.copy_arm_behave.stop()
In [ ]:
for m in cherry.robot.motors:
m.moving_speed = 0
for m in cherry.robot.arms :
m.compliant = False
cherry.robot.r_shoulder_y.goto_position(-80, 1.5, wait=False)
cherry.robot.r_shoulder_x.goto_position(-70, 1.5, wait=False)
cherry.robot.r_arm_z.goto_position(10, 1.5, wait=False)
cherry.robot.r_elbow_y.goto_position(-20, 1.5, wait=False)
cherry.robot.l_shoulder_y.goto_position(-80, 1.5, wait=False)
cherry.robot.l_shoulder_x.goto_position(70, 1.5, wait=False)
cherry.robot.l_arm_z.goto_position(-10, 1.5, wait=False)
cherry.robot.l_elbow_y.goto_position(-20, 1.5, wait=True)
In [ ]:
import time
for m in cherry.robot.motors:
m.moving_speed = 50
with open("../utils/moveRecord/step1.move") as f:
m = Move.load(f)
cherry.robot.compliant = False
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(4)
cherry.robot.say_sentence_local.start("Rodri gue aise!")
with open("../utils/moveRecord/step2.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
#time.sleep(1)
with open("../utils/moveRecord/step3.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(2.5)
cherry.robot.say_sentence_local.start("Père!")
with open("../utils/moveRecord/step4.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(2.5)
cherry.robot.say_sentence_local.start("Et pisse!")
with open("../utils/moveRecord/step5.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
with open("../utils/moveRecord/step6.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(1)
cherry.robot.say_sentence_local.start("Nous sommes techniciens du cinéma")
with open("../utils/moveRecord/step7.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(2)
cherry.robot.say_sentence_local.start("Depuis plus. de 150 ans !")
with open("../utils/moveRecord/step8.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
cherry.robot.say_sentence_local.start("Vous désirez réalisé un film de type anglossaxon.")
time.sleep(2)
with open("../utils/moveRecord/step9.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(2)
cherry.robot.say_sentence_local.start("Soit. De type cowboy.")
with open("../utils/moveRecord/step10.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(1)
cherry.robot.say_sentence_local.start("Soit. De type mission impossible.")
time.sleep(2)
with open("../utils/moveRecord/step11.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
cherry.robot.say_sentence_local.start("Soit. De type Nintendo.")
time.sleep(2)
with open("../utils/moveRecord/step12.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(2)
cherry.robot.say_sentence_local.start("ite ize possibeule. Bicause.")
time.sleep(2)
with open("../utils/moveRecord/step13.move") as f:
m = Move.load(f)
move_player = MovePlayer(cherry.robot, m)
move_player.start()
time.sleep(3)
cherry.robot.say_sentence_local.start("Rodri gue aise!")
In [ ]:
#CHERRY EXTRAVERTI
import pygame
import time
import random
import sys
from threading import Thread, RLock
def get():
out = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
it = 0 #iterator
pygame.event.pump()
#Read input from the two joysticks
for i in range(0, j.get_numaxes()):
out[it] = j.get_axis(i)
it+=1
for i in range(0,j.get_numhats()):
out[it] = j.get_hat(i)
it+=1
#Read input from buttons
for i in range(0, j.get_numbuttons()):
out[it] = j.get_button(i)
it+=1
return out
verrou = RLock()
class GamePadRead(Thread):
"""Thread chargé simplement d'afficher un mot dans la console."""
def __init__(self, mot):
Thread.__init__(self)
self.mot = mot
self.running = False
self.zHead = cherry.robot.head_z.present_position
self.yHead = cherry.robot.head_y.present_position
def run(self):
"""Code à exécuter pendant l'exécution du thread."""
self.running = True
while self.running:
with verrou:
tab = get()
#Mouvement de tête avec le Hat
temp = tab[4][0]*2
if(-90<self.zHead+temp <90):
self.zHead += tab[4][0]*2
temp = tab[4][1]*(-2)
if(self.yHead+temp < 20):
self.yHead += temp
cherry.robot.head_z.goal_position = self.zHead
cherry.robot.head_y.goal_position = self.yHead
#Mouvement
if tab[5] == 1 :
cherry.robot.rest_position.start()
if tab[6] == 1 :
cherry.robot.me_behave.start()
if tab[7] == 1 :
cherry.robot.me_arm_behave.start()
if tab[8] == 1 :
cherry.robot.question_behave.start()
if tab[9] == 1 :
cherry.robot.extra_arms_up.start()
if tab[10] == 1 :
cherry.robot.hug_behave.start()
if tab[11] == 1 :
cherry.robot.hunkers_behave.start()
if tab[12] == 1 :
cherry.robot.laugh_behave.start()
if tab[13] == 1 :
cherry.robot.show_right_rest_behave.start()
if tab[0] < -0.9 :
cherry.robot.left_arms_behave.start()
if tab[0] > 0.9 :
cherry.robot.right_arms_behave.start()
if tab[1] < -0.9 :
cherry.robot.right_hand_up_behave.start()
if tab[1] > 0.9 :
cherry.robot.show_front_hunkers_behave.start()
time.sleep(0.02)
def stop(self):
self.running = False
In [ ]:
#CHERRY AGREABLE
import pygame
import time
import random
import sys
from threading import Thread, RLock
def get():
out = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
it = 0 #iterator
pygame.event.pump()
#Read input from the two joysticks
for i in range(0, j.get_numaxes()):
out[it] = j.get_axis(i)
it+=1
for i in range(0,j.get_numhats()):
out[it] = j.get_hat(i)
it+=1
#Read input from buttons
for i in range(0, j.get_numbuttons()):
out[it] = j.get_button(i)
it+=1
return out
verrou = RLock()
class GamePadRead(Thread):
"""Thread chargé simplement d'afficher un mot dans la console."""
def __init__(self, mot):
Thread.__init__(self)
self.mot = mot
self.running = False
self.zHead = cherry.robot.head_z.present_position
self.yHead = cherry.robot.head_y.present_position
def run(self):
"""Code à exécuter pendant l'exécution du thread."""
self.running = True
runPrimitive = False
while self.running:
with verrou:
tab = get()
#Mouvement de tête avec le Hat
temp = tab[4][0]*2
if(-90<self.zHead+temp <90):
self.zHead += tab[4][0]*2
temp = tab[4][1]*(-2)
if(self.yHead+temp < 20):
self.yHead += temp
cherry.robot.head_z.goal_position = self.zHead
cherry.robot.head_y.goal_position = self.yHead
#Mouvement
if tab[5] == 1 :
cherry.robot.rest_position.start()
runPrimitive = True
if tab[6] == 1 :
cherry.robot.rest_tilted_behave.start()
runPrimitive = True
if tab[7] == 1 :
cherry.robot.question_behave.start()
runPrimitive = True
if tab[8] == 1 :
cherry.robot.left_arm_up_behave.start()
runPrimitive = True
if tab[9] == 1 :
cherry.robot.me_behave.start()
runPrimitive = True
if tab[10] == 1 :
cherry.robot.you_and_me_behave.start()
runPrimitive = True
if tab[11] == 1 :
cherry.robot.little_laugh_behave.start()
runPrimitive = True
if tab[12] == 1 :
cherry.robot.sigh_behave.start()
runPrimitive = True
if tab[13] == 1 :
cherry.robot.show_right_rest_behave.start()
runPrimitive = True
if tab[0] < -0.9 :
cherry.robot.left_arms_behave.start()
runPrimitive = True
if tab[0] > 0.9 :
cherry.robot.right_arms_behave.start()
runPrimitive = True
if tab[1] < -0.9 :
cherry.robot.little_hug_behave.start()
runPrimitive = True
if tab[1] > 0.9 :
cherry.robot.show_front_hunkers_behave.start()
runPrimitive = True
if runPrimitive :
time.sleep(0.2)
runPrimitive = False
time.sleep(0.02)
def stop(self):
self.running = False
In [ ]:
#CHERRY OUVERT
import pygame
import time
import random
import sys
from threading import Thread, RLock
def get():
out = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
it = 0 #iterator
pygame.event.pump()
#Read input from the two joysticks
for i in range(0, j.get_numaxes()):
out[it] = j.get_axis(i)
it+=1
for i in range(0,j.get_numhats()):
out[it] = j.get_hat(i)
it+=1
#Read input from buttons
for i in range(0, j.get_numbuttons()):
out[it] = j.get_button(i)
it+=1
return out
verrou = RLock()
class GamePadRead(Thread):
"""Thread chargé simplement d'afficher un mot dans la console."""
def __init__(self, mot):
Thread.__init__(self)
self.mot = mot
self.running = False
self.zHead = cherry.robot.head_z.present_position
self.yHead = cherry.robot.head_y.present_position
def run(self):
"""Code à exécuter pendant l'exécution du thread."""
self.running = True
runPrimitive = False
while self.running:
with verrou:
tab = get()
#Mouvement de tête avec le Hat
temp = tab[4][0]*2
if(-90<self.zHead+temp <90):
self.zHead += tab[4][0]*2
temp = tab[4][1]*(-2)
if(self.yHead+temp < 20):
self.yHead += temp
cherry.robot.head_z.goal_position = self.zHead
cherry.robot.head_y.goal_position = self.yHead
#Mouvement
if tab[5] == 1 :
cherry.robot.rest_open_behave.start()
runPrimitive = True
if tab[6] == 1 :
cherry.robot.question_behave.start()
runPrimitive = True
if tab[7] == 1 :
cherry.robot.swap_behave.start()
runPrimitive = True
if tab[8] == 1 :
cherry.robot.double_me_behave.start()
runPrimitive = True
if tab[9] == 1 :
cherry.robot.point_arm_left_behave.start()
runPrimitive = True
if tab[10] == 1 :
cherry.robot.open_move_behave.start()
runPrimitive = True
if tab[11] == 1 :
cherry.robot.little_arms_up_behave.start()
runPrimitive = True
if tab[12] == 1 :
cherry.robot.disappointment_behave.start()
runPrimitive = True
#if tab[13] == 1 :
#cherry.robot.show_right_rest_behave.start()
#runPrimitive = True
if tab[0] < -0.9 :
cherry.robot.left_arms_behave.start()
runPrimitive = True
if tab[0] > 0.9 :
cherry.robot.right_arms_behave.start()
runPrimitive = True
if tab[1] < -0.9 :
cherry.robot.open_arms_behave.start()
runPrimitive = True
if tab[1] > 0.9 :
cherry.robot.show_right_rest_behave.start()
runPrimitive = True
if runPrimitive :
time.sleep(0.2)
runPrimitive = False
time.sleep(0.02)
def stop(self):
self.running = False
In [1]:
pygame.init()
j = pygame.joystick.Joystick(0)
j.init()
print 'Initialized Joystick : %s' % j.get_name()
for m in cherry.robot.arms :
m.compliant = False
for m in cherry.robot.motors:
m.moving_speed = 0
thread = GamePadRead("Test")
thread.start()
In [ ]:
thread.stop()
In [ ]:
import time
import sys
from threading import Thread, RLock
import mp3play
class MP3Read(Thread):
"""Thread chargé simplement d'afficher un mot dans la console."""
def __init__(self, fname):
Thread.__init__(self)
self.filename = fname
def run(self):
mp3 = mp3play.load(self.filename)
mp3.play()
time.sleep(min(30, mp3.seconds()))
mp3.stop()
In [ ]:
cherry.robot.extra_arms_up.start()
time.sleep(0.5)
threadMp3 = MP3Read('Phrase1.mp3')
threadMp3.start()
time.sleep(1)
cherry.robot.me_behave.start()
time.sleep(1.2)
cherry.robot.question_behave.start()
time.sleep(1.3)
cherry.robot.rest_position.start()
In [ ]:
cherry.robot.rest_position.start()#1
In [13]:
import numpy as np
def robot3_inv(x,y,z) :
l1 = 16; #taille de bras
l2 = 20; #taille de avant-bras
Cq2 = (x^2 + y^2 + z^2 - l1^2 - l2^2)/(2*l1*l2);
q2 = -np.arccos(Cq2);
q1 = np.arctan(z/np.sqrt(x^2 + y^2)) - np.arctan(-l2*np.sqrt(1-Cq2^2)/(l1+l2*Cq2) );
q0 = -np.arctan(x/y);
q2 = round((np.real(q2*180/np.pi))); #% l_elbox_y
q1 = round(np.real(q1*180/np.pi)); #% l_shoulder_x
q0 = round(np.real(q0*180/np.pi)); #% l_shoulder_y
return [q0,q1,q2]
In [ ]:
In [16]:
robot3_inv(16,20,0)
Out[16]: