Using pypot's primitive

In this notebook we will show how you can write simple motions thanks to primitive and how you can easily combine them into more complex behaviors.

We will create a robot for the rest of this tutorial. Here again you can use a real or simulated version.



In [1]:

    
import os
import poppytools

from pypot.robot import from_json

config_path = os.path.join(os.path.dirname(poppytools.__file__), 'configuration', 'poppy_config.json')

robot = from_json(config_path)
robot.start_sync()

What do we call primitive?

In pypot, a primitive is basically a thread that have acess (both read and write) to the different motors and sensors of a robot.

You write them in the exact same way as you would write if you want to directly control your robot. For instance, using the sinus written in this notebook:



In [2]:

    
%pylab inline









    



Populating the interactive namespace from numpy and matplotlib



In [3]:

    
amp = 30 # in degrees
freq = 0.3 # in Hz 
duration = 10 # in s

# Our sinus will be send at 50Hz to match the refresh frequency.
# This is not at all mandatory, yet sending at a higher frequency will have no impact.
step = 1 / 50.
N = duration / step

t = linspace(0, duration, N)
pos = amp * sin(2 * pi * freq * t)

import time

for p in pos:
    robot.head_z.goal_position = p
    time.sleep(step)

You can easily make it a primitive:



In [4]:

    
from pypot.primitive import Primitive

class MySinusPrimitive(Primitive):
    def run(self):
        amp = 30 # in degrees
        freq = 0.3 # in Hz 
        duration = 10 # in s

        # Our sinus will be send at 50Hz to match the refresh frequency.
        # This is not at all mandatory, yet sending at a higher frequency will have no impact.
        step = 1 / 50.
        N = duration / step

        t = linspace(0, duration, N)
        pos = amp * sin(2 * pi * freq * t)

        for p in pos:
            self.robot.head_z.goal_position = p
            time.sleep(step)

Basically all you had to do was to wrapped the code in the run method of your primitive and change all the robot by self.robot.

You should be able to run your primitive juste by doing:



In [5]:

    
prim = MySinusPrimitive(robot)
prim.start()

And wait for it to end.



In [6]:

    
prim.wait_to_stop()

You can restart it as many times as you want. This will run the primitives 3 times:



In [7]:

    
for _ in range(3):
    prim.start()
    prim.wait_to_stop()

So what's the point? Well, primitive can be easily combined.

Combining primitives

Let's re-write this primitve so you can choose the amplitude, duration, and frequency of your sinus at instantiation.



In [8]:

    
class MySinusPrimitive(Primitive):
    def __init__(self, robot, freq, amp, duration):
        Primitive.__init__(self, robot)
        
        self.freq = freq
        self.amp = amp
        self.duration = duration
        
    def run(self):
        # Our sinus will be send at 50Hz to match the refresh frequency.
        # This is not at all mandatory, yet sending at a higher frequency will have no impact.
        step = 1 / 50.
        N = self.duration / step

        t = linspace(0, self.duration, N)
        pos = self.amp * sin(2 * pi * self.freq * t)

        for p in pos:
            self.robot.head_z.goal_position = p
            time.sleep(step)

Now we can create two sinuses with different settings:



In [9]:

    
s1 = MySinusPrimitive(robot, 0.25, 20., 10.)
s2 = MySinusPrimitive(robot, 1., 5., 20.)

You can run one at a time:



In [10]:

    
s1.start()
s1.wait_to_stop()



In [11]:

    
s2.start()
s2.wait_to_stop()

Or both in parallel:



In [12]:

    
s1.start()
s2.start()

s1.wait_to_stop() 
s2.wait_to_stop()

You should observe that the motion actually made by the motor was the mean of the two sinuses. Ssomething like this:

Primitives are automatically combined through a PrimitiveManager. You can defined the way they are combined through a filter (which is numpy.mean by default). In future versions, we hope to provide users with more control on how primitives could be combined: e.g. using primitives hierarchy, having special filter per motors... (See this issue on our github for details).

Primitive and LoopPrimitive

Primitives come in two flavors:

regular Primitive
and LoopPrimitive

LoopPrimitive are just Primitive with a predifined run that calls an update method at a predefined frequency.

For instance, we could re-write our sinus as a LoopPrimitive:



In [13]:

    
from pypot.primitive import LoopPrimitive

class MySinusLoopPrimitive(LoopPrimitive):
    def __init__(self, robot, update_freq, 
                 sin_freq, amp, duration):
        LoopPrimitive.__init__(self, robot, update_freq)
        
        self.freq = sin_freq
        self.amp = amp
        self.duration = duration
        
    def update(self):
        t = self.elapsed_time
        p = self.amp * sin(2 * pi * self.freq * t)
        self.robot.head_z.goal_position = p

Note that LoopPrimitive will run until stopped.



In [14]:

    
ls1 = MySinusLoopPrimitive(robot, 50.0, 0.25, 20., 10.)
ls1.start()

And stop it whenever you want.



In [15]:

    
ls1.stop()

You can also pause and resume primitives.



In [16]:

    
ls1.start()

time.sleep(5)

ls1.pause()

time.sleep(5)

ls1.resume()

time.sleep(5)

ls1.stop()

The code above will result in a motion looking like this:

Other uses of Primitive

Primitive are also useful for other things that motion. For instance, it's a very simple way to write a recorder. All the figures above were actually created using this recorder:



In [17]:

    
class MyRecorder(LoopPrimitive):
    def setup(self):
        self.pos = []
        
    def update(self):
        self.pos.append(self.robot.head_z.present_position)
        
    def plot(self, ax):
        t = linspace(0, self.elapsed_time, len(self.pos))
        ax.plot(t, self.pos)

And using it like this:



In [18]:

    
r = MyRecorder(robot, 10.)

r.start()

s1.start()
s2.start()
s1.wait_to_stop()
s2.wait_to_stop()

r.stop()

ax = axes()
r.plot(ax)



In [ ]: