Mettre en place un asservissement PID lié au couple mesuré sur un moteur. En cherchant à atteindre le couple minimum, on cherche en fait à garder une position verticale.
Utiliser les primitives de Pypot.
Instanciation du robot et paramétrage des notebooks pour utiliser les graphiques
In [1]:
from poppy.creatures import Poppy4dofArmMini
In [2]:
mini_dof = Poppy4dofArmMini(simulator='vrep')
In [3]:
import time
%pylab inline
Utilisation des primitives :
In [4]:
from pypot.primitive import Primitive
Réalisation d'une primitive permettant d'enregistrer les données des moteurs au cours du temps :
In [5]:
class Graph_primitive(Primitive):
def __init__(self,robot,motors_name):
self.robot = robot
Primitive.__init__(self, robot)
self.fake_motors={}
for name in motors_name:
self.fake_motors[name] = getattr(self.robot, name)
self.position={}
self.load={}
self.speed={}
def setup(self):
for m in self.fake_motors.keys():
self.position[m] = []
self.speed[m] = []
self.load[m] = []
self.python_time=[]
self.pypot_time=[]
def run(self):
t0 = time.time()
while not self.should_stop():
for m in self.fake_motors.keys():
self.position[m].append(self.fake_motors[m].present_position)
self.load[m].append(self.fake_motors[m].present_load)
self.speed[m].append(self.fake_motors[m].present_speed)
self.python_time.append(time.time()-t0)
self.pypot_time.append(self.elapsed_time)
time.sleep(0.02)
Test de cette primitive pour monitorer le present_load :
In [6]:
graph = Graph_primitive(mini_dof,['m3',])
In [7]:
graph.start()
mini_dof.m2.goto_position(90,2,wait=True)
mini_dof.m2.goto_position(-90,4,wait=True)
mini_dof.m2.goto_position(90,4,wait=True)
mini_dof.m2.goto_position(0,2,wait=True)
graph.stop()
In [8]:
figure(1)
plot(graph.pypot_time,graph.load['m3'])
xlabel('elapsed time seconds')
ylabel('load')
title ('Record load function of elapsed time')
Out[8]:
Le present_load dans le simulateur est trés variable. Nous réaliserons donc un objet permettant d'en faire une moyenne glissante :
In [6]:
class Filter_PID:
def __init__(self,nb_record=10,goal=0):
self.nb_record = nb_record
self.goal = goal
self.record_pos=[0 for i in range(nb_record)]
self.filter_load=[[0,0] for i in range(nb_record*10)]
def add(self,l,t):
self.record_pos.append(l-self.goal)
del self.record_pos[0]
self.filter_load.append([t,sum(self.record_pos)/len(self.record_pos)])
del self.filter_load[0]
def integrate(self,nb_values=10):
x=[i[0] for i in self.filter_load]
y=[i[1] for i in self.filter_load]
return np.trapz(y[-nb_values-1:-1],x[-nb_values-1:-1])
def derivative(self):
if self.filter_load[-1][0] != 0:
return ((self.filter_load[-1][1]+self.filter_load[-2][1])/2-(self.filter_load[-10][1]+self.filter_load[-9][1])/2)/((self.filter_load[-1][0]+self.filter_load[-2][0])/2-(self.filter_load[-10][0]+self.filter_load[-9][0])/2)
else :
return 0
def last(self):
return self.filter_load[-1][1]
L'objet Load_primitive permet l'enregistrement du present_load lissé :
In [7]:
class Load_primitive(Primitive):
def __init__(self,robot,motors_name):
self.robot = robot
Primitive.__init__(self, robot)
self.fake_motors = getattr(self.robot, motors_name)
def setup(self):
self.load=Filter_PID(30)
self.filter_load=[]
self.python_time=[]
self.pypot_time=[]
def run(self):
t0 = time.time()
while not self.should_stop():
self.load.add(self.fake_motors.present_load,self.elapsed_time)
self.filter_load.append(self.load.last())
self.python_time.append(time.time()-t0)
self.pypot_time.append(self.elapsed_time)
time.sleep(0.02)
On peut tester notre present_load corrigé :
In [10]:
load = Load_primitive(mini_dof,'m3')
In [9]:
load.start()
mini_dof.m2.goto_position(90,2,wait=True)
mini_dof.m2.goto_position(-90,4,wait=True)
mini_dof.m2.goto_position(90,4,wait=True)
mini_dof.m2.goto_position(0,2,wait=True)
load.stop()
In [10]:
figure(1)
plot(load.pypot_time,load.filter_load)
xlabel('elapsed time seconds')
ylabel('Filter load')
title ('Record filter load function of elapsed time')
Out[10]:
Le present_load filtré peut donc être utilisé pour corriger des positions car il est suffisament stable dans le temps :
In [11]:
load.start()
mini_dof.m2.goto_position(90,2,wait=True)
time.sleep(2)
mini_dof.m3.goto_position(-90,2,wait=True)
time.sleep(2)
load.stop()
mini_dof.m2.goto_position(0,2)
mini_dof.m3.goto_position(0,2,wait=True)
In [15]:
figure(1)
plot(load.pypot_time,load.filter_load)
xlabel('elapsed time seconds')
ylabel('Filter load')
title ('Record filter load function of elapsed time')
Out[15]:
L'objet Load_primitive_correction permet de corriger la position du moteur en fonction du present_load filtré :
In [8]:
class Load_primitive_correction(Primitive):
def __init__(self,robot,motors_name):
self.robot = robot
Primitive.__init__(self, robot)
self.fake_motors = getattr(self.robot, motors_name)
self.P=4
self.I=1
self.D=1
def setup(self):
self.load=Filter_PID(40)
self.filter_load=[]
self.python_time=[]
self.pypot_time=[]
self.P_record=[]
self.I_record=[]
self.D_record=[]
self.correction=[]
self.angle=self.fake_motors.present_position
self.angle_record=[]
def run(self):
t0 = time.time()
while not self.should_stop():
# ajout du oresent load à l'objet Filter PID
self.load.add(self.fake_motors.present_load,self.elapsed_time)
#ajout de la moyenne des load calculer par Filter PID
self.filter_load.append(self.load.last())
# ajout du temps
self.python_time.append(time.time()-t0)
self.pypot_time.append(self.elapsed_time)
# calcul de la correction en fonction de l'écart avec l'objectif
P = self.P*self.load.last()
I = self.I*self.load.integrate(30)
D = self.D*self.load.derivative()
self.P_record.append(P)
self.I_record.append(I)
self.D_record.append(D)
correction_value = P + D + I
self.correction.append(correction_value)
self.angle_record.append(self.angle)
self.angle = self.fake_motors.present_position - correction_value
self.fake_motors.goal_position = self.angle
# pause déterminant la fréquance de la boucle
t1 = self.elapsed_time
while self.elapsed_time-t1<0.02:
time.sleep(0.001)
In [10]:
load = Load_primitive_correction(mini_dof,'m3')
In [12]:
load.start()
mini_dof.m2.goto_position(90,4,wait=True)
mini_dof.m2.goto_position(0,4,wait=True)
time.sleep(1)
load.stop()
mini_dof.m3.goto_position(0,2)
In [43]:
figure(1)
plot(load.pypot_time,load.P_record,'b-')
plot(load.pypot_time,load.I_record,'r-')
plot(load.pypot_time,load.D_record,'g-')
plot(load.pypot_time,load.correction,'c-')
twinx()
plot(load.pypot_time,load.angle_record,'b-')
xlabel('elapsed time seconds')
ylabel('Correction')
title ('Record filter load function of elapsed time')
figure(2)
plot(load.pypot_time,load.correction,'c-')
plot(load.pypot_time,load.filter_load,'g-')
xlabel('elapsed time seconds')
ylabel('Filter load')
title ('Record filter load function of elapsed time')
Out[43]:
La correction permet de maintenir la position verticale. il n'est pas possible d'avoir un temps de réponse rapide car le lissage des present_load brut prend environ une demi seconde et décale d'autant la réaction en cas de changement de position.
In [16]:
mini_dof.reset_simulation()
In [7]:
mini_dof = Poppy4dofArmMini()
In [8]:
for m in mini_dof.motors:
m.compliant = False
m.goto_position(0,1)
In [9]:
graph = Graph_primitive(mini_dof,['m3',])
In [ ]:
graph.start()
time.sleep(1)
mini_dof.m2.goto_position(90,1,wait=True)
time.sleep(1)
mini_dof.m2.goto_position(0,1,wait=True)
time.sleep(1)
graph.stop()
In [13]:
figure(1)
plot(graph.pypot_time,graph.load['m3'])
xlabel('elapsed time seconds')
ylabel('load')
title ('Record load function of elapsed time')
Out[13]:
Le present_load est évidement bien différent de celui du simulateur. Le problème réside maintenant dans la stabilité de la mesure. Même en l'absence de changement,le present load effectue des sauts relativment important. En outre la sensibilité n'est pas bonne car il faut un changment de couple important pour faire varier la mesure. De la même façon qu'avec le simulateur, il est possible de faire une moyenne qui filtrera les variations mais fera perdre du temps de réaction.
In [34]:
class Load_primitive_correction(Primitive):
def __init__(self,robot,motors_name):
self.robot = robot
Primitive.__init__(self, robot)
self.fake_motors = getattr(self.robot, motors_name)
self.P=0.5
self.I=0.3
self.D=0
def setup(self):
self.load=Filter_PID(10)
self.filter_load=[]
self.python_time=[]
self.pypot_time=[]
self.P_record=[]
self.I_record=[]
self.D_record=[]
self.correction=[]
self.angle=self.fake_motors.present_position
self.angle_record=[]
def run(self):
t0 = time.time()
while not self.should_stop():
# ajout du present load à l'objet Filter PID
self.load.add(self.fake_motors.present_load,self.elapsed_time)
#ajout de la moyenne des load calculer par Filter PID
self.filter_load.append(self.load.last())
# ajout du temps
self.python_time.append(time.time()-t0)
self.pypot_time.append(self.elapsed_time)
# calcul de la correction en fonction de l'écart avec l'objectif
P = self.P*self.load.last()
I = self.I*self.load.integrate(30)
D = self.D*self.load.derivative()
self.P_record.append(P)
self.I_record.append(I)
self.D_record.append(D)
correction_value = P + D + I
self.correction.append(correction_value)
self.angle_record.append(self.angle)
self.angle = self.fake_motors.present_position - correction_value
self.fake_motors.goal_position = self.angle
# pause déterminant la fréquance de la boucle
t1 = self.elapsed_time
while self.elapsed_time-t1<0.02:
time.sleep(0.001)
In [35]:
load = Load_primitive_correction(mini_dof,'m3')
In [24]:
load.start()
In [25]:
load.stop()
In [36]:
time.sleep(3)
load.start()
mini_dof.m2.goto_position(90,3,wait=True)
time.sleep(4)
mini_dof.m2.goto_position(0,3,wait=True)
time.sleep(4)
load.stop()
mini_dof.m3.goto_position(0,2)
In [100]:
figure(1)
plot(load.pypot_time,load.P_record,'b-')
plot(load.pypot_time,load.I_record,'r-')
plot(load.pypot_time,load.D_record,'g-')
plot(load.pypot_time,load.correction,'c-')
twinx()
plot(load.pypot_time,load.angle_record,'b-')
xlabel('elapsed time seconds')
ylabel('Correction')
title ('Record filter load function of elapsed time')
figure(2)
plot(load.pypot_time,load.correction,'c-')
plot(load.pypot_time,load.filter_load,'g-')
xlabel('elapsed time seconds')
ylabel('Filter load')
title ('Record filter load function of elapsed time')
Out[100]:
Le robot réagit bien en corrigeant sa position pour atteindre la verticale, il faut cependant garder des valeurs PID très faibles ce qui entraine un temps de réaction encore ralongé. En effet, sur le robot réel, le mouvement du moteur entraine des variations de couple importante dans le même sens que la correction. Si la correction est trop forte le robot entre en oscillations croissantes ( si on corrige trop fort, on augmente le couple dans le sens opposé et on trouvera une correction de plus en forte jusqu'à que le robot se bloque dans un angle limite).
In [26]:
for m in mini_dof.motors :
m.goto_position(0,1)
In [37]:
mini_dof.close()
Pour utiliser le couple de façon efficace lors de mouvements d'équilibre, il faudrait donc des capteurs de couple plus précis et plus rapide que ceux présent sur les XL-320.
In [ ]: