In [2]:
import time
from poppy.creatures import PoppyHumanoid
from cherry import Cherry
from pypot.primitive.move import MoveRecorder, Move, MovePlayer
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 [3]:
for m in cherry.robot.arms :
m.compliant = False
for m in cherry.robot.arms :
m.goal_position = 0
In [4]:
import numpy as np
def computeJacobPoppy(Pxi,Pyi,Pzi,q1,q2,q3,q4):
t2 = np.sin(q2);
t3 = np.cos(q2);
t4 = np.sin(q4);
t5 = np.cos(q4);
t6 = np.sin(q3);
t7 = np.cos(q3);
t8 = np.cos(q1);
t9 = np.sin(q1);
t10 = t7*t9;
t12 = t2*t6*t8;
t11 = t10-t12;
t13 = t6*t8;
t15 = t2*t7*t9;
t14 = t13-t15;
t16 = t7*t8;
t17 = t2*t6*t9;
t18 = t16+t17;
t19 = t5*t18;
t20 = t19-t3*t4*t9;
t21 = t4*t18;
t22 = t3*t5*t9;
t23 = t21+t22;
t24 = t3*2.0;
t25 = t3*t7*9.0;
t26 = t3*t6*9.6e1;
t27 = Pzi*t2*t5*1.0e2;
t28 = Pyi*t2*t4*1.0e2;
t29 = Pyi*t3*t5*t6*1.0e2;
t30 = t2*-1.84e3+t24+t25+t26+t27+t28+t29-t2*t4*2.6e3-Pxi*t3*t7*1.0e2-t3*t5*t6*2.6e3-Pzi*t3*t4*t6*1.0e2;
t31 = t6*t9;
t32 = t2*t7*t8;
t33 = t31+t32;
t34 = t4*t11;
t35 = t34-t3*t5*t8;
t36 = t5*t11;
t37 = t3*t4*t8;
t38 = t36+t37;
out1 = np.reshape((0.0,t8*(-7.0/1.0e2)+t9*(1.0/2.5e1)-Pxi*t33-Pyi*t38+Pzi*t35+t2*t8*(1.0/5.0e1)+t3*t8*(9.2e1/5.0)+t6*t9*(9.0/1.0e2)+t5*t11*2.6e1-t7*t9*(2.4e1/2.5e1)+t3*t4*t8*2.6e1+t2*t6*t8*(2.4e1/2.5e1)+t2*t7*t8*(9.0/1.0e2),t8*(-1.0/2.5e1)-t9*(7.0/1.0e2)+Pxi*t14+Pyi*t20-Pzi*t23+t2*t9*(1.0/5.0e1)+t3*t9*(9.2e1/5.0)-t6*t8*(9.0/1.0e2)+t7*t8*(2.4e1/2.5e1)-t5*t18*2.6e1+t3*t4*t9*2.6e1+t2*t6*t9*(2.4e1/2.5e1)+t2*t7*t9*(9.0/1.0e2),t2*(1.0/5.0e1)+t3*(9.2e1/5.0)-Pyi*(t3*t4-t2*t5*t6)-Pzi*(t3*t5+t2*t4*t6)+t3*t4*2.6e1+t2*t6*(2.4e1/2.5e1)+t2*t7*(9.0/1.0e2)-Pxi*t2*t7-t2*t5*t6*2.6e1,t9*t30*(1.0/1.0e2),t8*t30*(-1.0/1.0e2),t3*(t6*9.0-t7*9.6e1-Pxi*t6*1.0e2+t5*t7*2.6e3-Pyi*t5*t7*1.0e2+Pzi*t4*t7*1.0e2)*(1.0/1.0e2),Pxi*t18-t6*t8*(2.4e1/2.5e1)-t7*t8*(9.0/1.0e2)+t5*t14*2.6e1-Pyi*t5*t14+Pzi*t4*t14-t2*t6*t9*(9.0/1.0e2)+t2*t7*t9*(2.4e1/2.5e1),Pxi*t11-t6*t9*(2.4e1/2.5e1)-t7*t9*(9.0/1.0e2)+t5*t33*2.6e1-Pyi*t5*t33+Pzi*t4*t33+t2*t6*t8*(9.0/1.0e2)-t2*t7*t8*(2.4e1/2.5e1),-Pyi*(t2*t5-t3*t4*t6)+Pzi*(t2*t4+t3*t5*t6)+t2*t5*2.6e1-t3*t4*t6*2.6e1,-Pyi*t23-Pzi*t20+t4*t18*2.6e1+t3*t5*t9*2.6e1,-Pyi*t35-Pzi*t38+t4*t11*2.6e1-t3*t5*t8*2.6e1),(4, 3));
return out1
In [5]:
import numpy as np
def computeGeometricPoppy(q1,q2,q3,q4):
t2 = np.cos(q2);
t3 = np.cos(q4);
t4 = np.sin(q2);
t5 = np.sin(q3);
t6 = np.sin(q4);
t7 = np.cos(q3);
t8 = np.cos(q1);
t9 = np.sin(q1);
t10 = t7*t8;
t11 = t4*t5*t9;
t12 = t10+t11;
t13 = t7*t9;
t15 = t4*t5*t8;
t14 = t13-t15;
out1 = np.reshape((t2*t7,t5*t8-t4*t7*t9,t5*t9+t4*t7*t8,0.0,-t4*t6-t2*t3*t5,t3*t12-t2*t6*t9,t3*t14+t2*t6*t8,0.0,-t3*t4+t2*t5*t6,-t6*t12-t2*t3*t9,-t6*t14+t2*t3*t8,0.0,t2*(-1.0/5.0e1)+t4*(9.2e1/5.0)-t2*t5*(2.4e1/2.5e1)-t2*t7*(9.0/1.0e2)+t4*t6*2.6e1+t2*t3*t5*2.6e1+1.033e1,t8*(-1.0/2.5e1)-t9*(7.0/1.0e2)+t2*t9*(9.2e1/5.0)+t4*t9*(1.0/5.0e1)-t5*t8*(9.0/1.0e2)-t3*t12*2.6e1+t7*t8*(2.4e1/2.5e1)+t2*t6*t9*2.6e1+t4*t5*t9*(2.4e1/2.5e1)+t4*t7*t9*(9.0/1.0e2)-4.3e1/1.0e2,t8*(7.0/1.0e2)-t9*(1.0/2.5e1)-t2*t8*(9.2e1/5.0)-t4*t8*(1.0/5.0e1)-t5*t9*(9.0/1.0e2)+t7*t9*(2.4e1/2.5e1)-t3*t14*2.6e1-t2*t6*t8*2.6e1-t4*t5*t8*(2.4e1/2.5e1)-t4*t7*t8*(9.0/1.0e2)+3.23e2/2.5e1,1.0),(4, 4));
return out1
In [6]:
import numpy as np
def findMotorAngle(Cx,Cy,Cz,CQ1,CQ2,CQ3,CQ4,Gx,Gy,Gz) :
fQi = np.matrix([[Cx],[Cy],[Cz]])
Q= np.matrix([[CQ1],[CQ2],[CQ3],[CQ4]])
fQi_plus_1 = np.matrix([[Gx],[Gy],[Gz]])
itera = 0
while itera < 200 and np.linalg.norm(fQi_plus_1 - fQi) > 0.001 :
Jac = computeJacobPoppy(0,0,0,Q.item(0),Q.item(1),Q.item(2),Q.item(3))
JacTrans = Jac.T
Jinv = np.linalg.pinv(JacTrans)
diff_fQi = fQi_plus_1 - fQi
delta_qi = Jinv *0.6* diff_fQi
qi_plus_1 = Q + delta_qi
fqinew = computeGeometricPoppy(qi_plus_1.item(0),qi_plus_1.item(1),qi_plus_1.item(2),qi_plus_1.item(3))
fqinewTrans = fqinew.T
fQi = np.matrix([[fqinewTrans.item(3)], [fqinewTrans.item(7)],[fqinewTrans.item(11)]])
Q = qi_plus_1
itera = itera +1
angleWrong = 12;
while angleWrong > 0 :
angleWrong = 0;
for i in range(0,4) :
if Q[i] > np.pi :
Q[i] = Q[i] - 2*np.pi;
angleWrong = angleWrong + 1;
elif Q[i] < - np.pi :
Q[i] = Q[i] + 2*np.pi;
angleWrong = angleWrong + 1;
#print itera
return [Q.item(0),Q.item(1),Q.item(2),Q.item(3),fQi.item(0),fQi.item(1),fQi.item(2)]
In [ ]:
computeGeometricPoppy(0,0,0,0).T
In [ ]:
computeJacobPoppy(0,0,0,0,0,0,0).T
In [ ]:
findMotorAngle(10,26,-5,0,0,0,0,10,25.5,-5)
In [ ]:
computeGeometricPoppy(-4.09367151e-01,1.99157627e-17,7.93135759e-18,3.61680794e-01).T
In [7]:
#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
self._x = 10.22
self._y = -25.51
self._z = -5.41
self._CQ1 = 0
self._CQ2 = 0
self._CQ3 = 0
self._CQ4 = 0
self._Gx = 10.22
self._Gy = -25.51
self._Gz = -5.41
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 += temp
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
if(tab[0]<-0.1 or tab[0]>0.1):
self._Gx = self._x+tab[0]*-1
if(tab[1]<-0.1 or tab[1]>0.1):
self._Gz = self._z+tab[1]*-1
if tab[9] == 1 :
self._Gy = self._y+1
if tab[10] == 1 :
self._Gy = self._y-1
[self._CQ1,self._CQ2,self._CQ3,self._CQ4,self._x,self._y,self._z] = findMotorAngle(self._x,self._y,self._z,self._CQ1,self._CQ2,self._CQ3,self._CQ4,self._Gx,self._Gy,self._Gz)
#print [self._CQ1*360/2*np.pi,self._CQ2*360/2*np.pi,self._CQ3*360/2*np.pi,self._CQ4*360/2*np.pi, self._x,self._y,self._z,self._Gx,self._Gy,self._Gz]
cherry.robot.l_shoulder_y.goal_position = self._CQ1*360/(2*np.pi)
cherry.robot.l_shoulder_x.goal_position = self._CQ2*360/(2*np.pi)
cherry.robot.l_arm_z.goal_position = self._CQ3*360/(2*np.pi)
cherry.robot.l_elbow_y.goal_position = self._CQ4*360/(2*np.pi)
time.sleep(0.02)
def stop(self):
self.running = False
In [8]:
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 = 60
thread = GamePadRead("Test")
thread.start()
In [ ]:
thread.stop()