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


In [1]:
from poppy.creatures import Poppy4dofArmMini

In [2]:
mini_dof = Poppy4dofArmMini(simulator='vrep')

In [3]:
import time
%pylab inline


Populating the interactive namespace from numpy and matplotlib

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]:
<matplotlib.text.Text at 0x82680f0>

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]:
<matplotlib.text.Text at 0x8252870>

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]:
<matplotlib.text.Text at 0x855a3d0>

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]:
<matplotlib.text.Text at 0x9c4fdb0>

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()

Sur le robot réel


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]:
<matplotlib.text.Text at 0x7578ab0>

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]:
<matplotlib.text.Text at 0x963b3d0>

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 [ ]: