<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 Corriger une position en fonction du couple mesuré sur un moteur
Compétences visées par cette activité 
Step1:  Utilisation des primitives 
Step2:  Réalisation d'une primitive permettant d'enregistrer les données des moteurs au cours du temps 
Step3:  Test de cette primitive pour monitorer le present_load 
Step4:  Le present_load dans le simulateur est trés variable. Nous réaliserons donc un objet permettant d'en faire une moyenne glissante 
Step5:  L'objet Load_primitive permet l'enregistrement du present_load lissé 
Step6:  On peut tester notre present_load corrigé 
Step7:  Le present_load filtré peut donc être utilisé pour corriger des positions car il est suffisament stable dans le temps 
Step8:  L'objet Load_primitive_correction permet de corriger la position du moteur en fonction du present_load filtré 
Step9:  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.
Step10:  Sur le robot réel
Step11:  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.
Step12:  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).
<ASSISTANT_TASK:>
Python Code:
from poppy.creatures import Poppy4dofArmMini
mini_dof = Poppy4dofArmMini(simulator='vrep')
import time
%pylab inline
Explanation: Corriger une position en fonction du couple mesuré sur un moteur
Compétences visées par cette activité :
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.
Plus d'information sur l'activité (vidéo, résultats, commentaires...) :
http://www.poppy-prof.fr/?page_id=4&id=76
Sur le robot dans le simulateur V-REP :
Instanciation du robot et paramétrage des notebooks pour utiliser les graphiques
End of explanation
from pypot.primitive import Primitive
Explanation: Utilisation des primitives :
End of explanation
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)
            
            
Explanation: Réalisation d'une primitive permettant d'enregistrer les données des moteurs au cours du temps :
End of explanation
graph = Graph_primitive(mini_dof,['m3',])
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()
figure(1)
plot(graph.pypot_time,graph.load['m3'])
xlabel('elapsed time seconds')
ylabel('load')
title ('Record load function of elapsed time')
Explanation: Test de cette primitive pour monitorer le present_load :
End of explanation
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]
Explanation: Le present_load dans le simulateur est trés variable. Nous réaliserons donc un objet permettant d'en faire une moyenne glissante :
End of explanation
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)
Explanation: L'objet Load_primitive permet l'enregistrement du present_load lissé :
End of explanation
load = Load_primitive(mini_dof,'m3')
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()
figure(1)
plot(load.pypot_time,load.filter_load)
xlabel('elapsed time seconds')
ylabel('Filter load')
title ('Record filter load function of elapsed time')
Explanation: On peut tester notre present_load corrigé :
End of explanation
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)
figure(1)
plot(load.pypot_time,load.filter_load)
xlabel('elapsed time seconds')
ylabel('Filter load')
title ('Record filter load function of elapsed time')
Explanation: Le present_load filtré peut donc être utilisé pour corriger des positions car il est suffisament stable dans le temps :
End of explanation
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)
load = Load_primitive_correction(mini_dof,'m3')
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)
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')
Explanation: L'objet Load_primitive_correction permet de corriger la position du moteur en fonction du present_load filtré :
End of explanation
mini_dof.reset_simulation()
Explanation: 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.
End of explanation
mini_dof = Poppy4dofArmMini()
for m in mini_dof.motors:
    m.compliant = False
    m.goto_position(0,1)
graph = Graph_primitive(mini_dof,['m3',])
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()
figure(1)
plot(graph.pypot_time,graph.load['m3'])
xlabel('elapsed time seconds')
ylabel('load')
title ('Record load function of elapsed time')
Explanation: Sur le robot réel
End of explanation
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)
load = Load_primitive_correction(mini_dof,'m3')
load.start()
load.stop()
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)
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')
Explanation: 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.
End of explanation
for m in mini_dof.motors :
    m.goto_position(0,1)
mini_dof.close()
Explanation: 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).
End of explanation
<END_TASK>