Tutorial¶

Opening/Closing a communication port¶

In order to open a connection with the device, you will need to know what port it is connected to. PyPot has a function named get_available_ports() which will try to auto-discover any compatible devices connected to the communication ports.

To create a connection, open up a python terminal and type the following code:

import pypot.dynamixel

ports = pypot.dynamixel.get_available_ports()

if not ports:
    raise IOError('no port found!')

print 'ports found', ports

print 'connecting on the first available port:', ports[0]
dxl_io = pypot.dynamixel.DxlIO(ports[0])

This should open a connection through a virtual communication port to your device.

To set up a connection with another baud rate you can write:

dxl_io = pypot.dynamixel.DxlIO(port, baudrate=57140)

The communication can be closed using the close() method.

with pypot.dynamixel.DxlIO('/dev/ttyUSB0') as dxl_io:
    ...

Finding motors¶

Pypot has been designed to work specifically with the Robotis range of motors. These motors use two different protocols to communicate: TTL (3 wire bus) and RS485 (4 wire Bus). The motors can be daisy chained together with other types of motors on the same bus as long as the bus communicates using the same protocol. This means that MX-28 and AX-12 can communicate on the same bus, but cannot be connected to a RX-28.

All motors work sufficiently well with a 12V supply. Some motors can use more than 12V but you must be careful not to connect an 18V supply on a bus that contains motors that can only use 12V! Connect this 12V SMPS supply (switch mode power supply) to a Robotis SMPS2Dynamixel device which regulates the voltage coming from the SMPS. Connect your controller device and a single motor to this SMPS2Dynamixel.

Open your python terminal and create your DxlIO as described in the above section Opening/Closing a communication port.

To detect the motors and find their id you can scan the bus. To avoid spending a long time searching all possible values, you can add a list of values to test:

dxl_io.scan()
>>> [4, 23, 24, 25]

dxl_io.scan([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])
>>> [4]

Or, you can use the shorthand:

dxl_io.scan(range(10))
>>> [4]

This should produce a list of the ids of the motors that are connected to the bus. Each motor on the bus must have a unique id. This means that unless your motors have been configured in advance, it is better to connect them one by one to ensure they all have unique ids first.

Low-level control¶

Now we have the id of the motors connected, we can begin to access their functions by using their id. Try to find out the present position (in degrees) of the motor by typing the following:

dxl_io.get_present_position((4, ))
>>> (67.8, )

You can also write a goal position (in degrees) to the motor using the following:

dxl_io.set_goal_position({4: 0})

The motors are handled in degrees where 0 is considered the central point of the motor turn. For the MX motors, the end points are -180° and 180°. For the AX and RX motors, these end points are -150° to 150°.

As an example of what you can do with the low-level API, we are going to apply a sinusoid on two motors (make sure that the motion will not damage your robot before running the example!). Here is a complete listing of the code needed:

import itertools
import numpy
import time

import pypot.dynamixel

AMP = 30
FREQ = 0.5

if __name__ == '__main__':
    ports = pypot.dynamixel.get_available_ports()
    print 'available ports:', ports

    if not ports:
        raise IOError('No port available.')

    port = ports[0]
    print 'Using the first on the list', port

    dxl_io = pypot.dynamixel.DxlIO(port)
    print 'Connected!'

    found_ids = dxl_io.scan()
    print 'Found ids:', found_ids

    if len(found_ids) < 2:
        raise IOError('You should connect at least two motors on the bus for this test.')

    ids = found_ids[:2]

    dxl_io.enable_torque(ids)

    speed = dict(zip(ids, itertools.repeat(200)))
    dxl_io.set_moving_speed(speed)

    pos = dict(zip(ids, itertools.repeat(0)))
    dxl_io.set_goal_position(pos)


    t0 = time.time()
    while True:
        t = time.time()
        if (t - t0) > 5:
            break

        pos = AMP * numpy.sin(2 * numpy.pi * FREQ * t)
        dxl_io.set_goal_position(dict(zip(ids, itertools.repeat(pos))))

        time.sleep(0.02)

Thanks to PyPot, you can access all registers of your motors using the same syntax (e.g. get_present_speed(), set_max_torque(), get_pid_gain()). Some shortcuts have been provided to make the code more readable (e.g. enable_torque() instead of set_torque_enabled). All the getter functions takes a list of ids as argument and the setter takes a dictionary of (id: value) pairs. You can refer to the documentation of DxlIO for a complete list of all the available methods.

Using the robot abstraction¶

While the Low-level API provides access to all functionalities of the dynamixel motors, it forces you to have synchronous calls which can take a non-negligible amount of time. In particular, most programs will need to have a really fast read/write synchronization loop, where we typically read all motor position, speed, load and set new values, while in parallel we would like to have higher level code that computes those new values. This is pretty much what the robot abstraction is doing for you. More precisely, through the use of the class Robot you can:

• automatically initialize all connections (make transparent the use of multiple USB2serial connections),
• define offset and direct attributes for motors,
• automatically define accessor for motors and their most frequently used registers (such as goal_position, present_speed, present_load, pid, compliant),
• define read/write synchronization loop that will run in background.

We will first see how to define your robot thanks to the writing of a configuration, then we will describe how to set up synchronization loops. Finally, we will show how to easily control this robot through asynchronous commands.

Writing the configuration¶

The configuration, described as a Python dictionary, contains several important features that help build both your robot and the software to manage you robot. The important fields are listed below:

• controllers - This key holds the information pertaining to a controller and all the items connected to its bus.
• motors - This is a description of all the custom setup values for each motor. Meta information, such as the motor access name or orientation, is also included here. It is also there that you will set the angle limits of the motor.
• motorgroups - This is used to define alias of a group of motors (e.g. left_leg).

Now let’s detail each section. To better understand how the configuration is structure it is probably easier to start from one of the example provided with PyPot and modify it (e.g. pypot.robot.config.ergo_robot_config):

1. controllers: You can have a single or multiple DxlController. For each of them, you should indicate whether or not to use the SYNC_READ instruction (only the USB2AX device currently supported it). When you describe your controller, you must also include the port that the device is connected to (see Opening/Closing a communication port). You also have to specify which motors are attached to this bus. You can either give individual motors or groups (see the sections below):

   my_config['controllers'] = {}
   my_config['controllers']['upper_body_controler'] = {
       'port': '/dev/ttyUSB0',
       'sync_read': False,
       'attached_motors': ['torso', 'head', 'arms']
   }
   

2. motorgroups: Here, you can define the different motors group corresponding to the structure of your robot. It will automatically create an alias for the group. Groups can be nested, i.e. a group can be included inside another group, as in the example below:

   my_config['motorgroups'] = {
       'torso': ['arms', 'head_x', 'head_y'],
       'arms': ['left_arm', 'right_arm'],
       'left_arm': ['l_shoulder_x', 'l_shoulder_y', 'l_elbow'],
       'right_arm': ['r_shoulder_x', 'r_shoulder_y', 'r_elbow']
   }
   

3. motors: Then, you add all the motors. The attributes are not optional and describe how the motors can be used in the software. You have to specify the type of motor, it will change which attributes are available (e.g. compliance margin versus pid gains). The name and id are used to access the motor specifically. Orientation describes whether the motor will act in an anti-clockwise fashion (direct) or clockwise (indirect). You should also provide the angle limits of your motor. They will be checked automatically at every start up and changed if needed:

   my_config['motors'] = {}
   my_config['motors']['l_hip_y'] = {
       'id': 11,
       'type': 'RX-64',
       'orientation': 'direct',
       'offset': 22.5,
       'angle_limit': (-67.5, 112.5),
   }
   

4. This is all you need to create and interact with your robot. All that remains is to connect your robot to your computer. To create your robot use the from_config() function which takes your configuration as an argument. Here is an example of how to create your first robot and start using it:

   import pypot.robot
   
   robot = pypot.robot.from_config(my_config)
   robot.start_sync()
   
   for m in robot.left_arm:
       print m.present_position
   

5. (optional) If you prefer working with file, you can read/write your config to any format that can be transformed into a dictionary. For instance, you can easily use the JSON format:

   import json
   
   import pypot.robot
   
   from pypot.robot.config import ergo_robot_config
   
   with open('ergo.json', 'w') as f:
       json.dump(ergo_robot_config, f, indent=2)
   
   ergo = pypot.robot.from_json('ergo.json')
   

To give you a complete overview of what your config should look like, here is the listing of the Ergo-Robot config dictionary:

ergo_robot_config = {
    'controllers': {
        'my_dxl_controller': {
            'port': '/dev/ttyUSB0', # Depends on your OS
            'sync_read': False,
            'attached_motors': ['base', 'head'], # You can mix motorgroups or individual motors
        },
    },

    'motorgroups': {
        'base': ['base_pan', 'base_tilt_lower', 'base_tilt_upper'],
        'head': ['head_pan', 'head_tilt_lower', 'head_tilt_upper'],
    },

    'motors': {
        'base_pan': {
            'id': 11,
            'type': 'RX-64',
            'orientation': 'direct',
            'offset': 22.5,
            'angle_limit': (-67.5, 112.5),
        },
        'base_tilt_lower': {
            'id': 12,
            'type': 'RX-64',
            'orientation': 'direct',
            'offset': 0.0,
            'angle_limit': (-90.0, 90.0),
        },
        'base_tilt_upper': {
            'id': 13,
            'type': 'RX-64',
            'orientation': 'direct',
            'offset': 0.0,
            'angle_limit': (-90.0, 90.0),
        },
        'head_pan': {
            'id': 14,
            'type': 'RX-28',
            'orientation': 'direct',
            'offset': 22.5,
            'angle_limit': (-67.5, 112.5),
        },
        'head_tilt_lower': {
            'id': 15,
            'type': 'RX-28',
            'orientation': 'indirect',
            'offset': 0.0,
            'angle_limit': (-90.0, 90.0),
        },
        'head_tilt_upper': {
            'id': 16,
            'type': 'RX-28',
            'orientation': 'indirect',
            'offset': 0.0,
            'angle_limit': (-90.0, 90.0),
        },
    },
}

Dynamixel controller and Synchronization Loop¶

As indicated above, the Robot held instances of DxlMotor. Each of this instance represents a real motor of your physical robot. The attributes of those “software” motors are automatically synchronized with the real “hardware” motors. In order to do that, the Robot class uses a DxlController which defines synchronization loops that will read/write the registers of dynamixel motors at a predefined frequency.

If you looked closely at the example above, you could have noticed that even without defining any controller nor synchronization loop, you can already read the present position of the motors. Indeed, by default the class Robot uses a particular controller BaseDxlController which already defines synchronization loops. More precisely, this controller:

• reads the present position, speed, load at 50Hz,
• writes the goal position, moving speed and torque limit at 50Hz,
• writes the pid or compliance margin/slope (depending on the type of motor) at 10Hz,
• reads the present temperature and voltage at 1Hz.

So, in most case you should not have to worry about synchronization loop and it should directly work. Off course, if you want to synchronize other values than the ones listed above you will have to modify this default behavior.

To start all the synchronization loops, you only need to call the start_sync() method. You can also stop the synchronization if needed (see the stop_sync() method):

import pypot.robot

robot = pypot.robot.from_config(my_config)
robot.start_sync()

Now you have a robot that is reading and writing values to each motor in an infinite loop. Whenever you access these values, you are accessing only their most recent versions that have been read at the frequency of the loop. This automatically make the synchronization loop run in background. You do not need to wait the answer of a read command to access data (this can take some time) so that algorithms with heavy computation do not encounter a bottleneck when values from motors must be known.

Now you are ready to create some behaviors for your robot.

Controlling your robot¶

import pypot.robot

from pypot.robot.config import ergo_robot_config

robot = pypot.robot.from_config(ergo_robot_config)
ergo_start_sync()

# Note that all these calls will return immediately,
# and the orders will not be directly sent
# (they will be sent during the next write loop iteration).
for m in ergo_robot.base:
    m.compliant = False
    m.goal_position = 0

# This will return the last synchronized value
print ergo_robot.base_pan.present_position

import time
import numpy

import pypot.robot

from pypot.robot.config import ergo_robot_config

amp = 30
freq = 0.5

robot = pypot.robot.from_config(ergo_robot_config)    ergo_robot.start_sync()

# Put the robot in its initial position
for m in ergo_robot.motors: # Note that we always provide an alias for all motors.
    m.compliant = False
    m.goal_position = 0

# Wait for the robot to actually reach the base position.
time.sleep(2)

# Do the sinusoidal motions for 10 seconds
t0 = time.time()

while True:
    t = time.time() - t0

    if t > 10:
        break

    pos = amp * numpy.sin(2 * numpy.pi * freq * t)

    ergo_robot.base_pan.goal_position = pos

    # In order to make the other sinus more visible,
    # we apply it with an opposite phase and we increase the amplitude.
    ergo_robot.head_pan.goal_position = -1.5 * ergo_robot.base_pan.present_position

    # We want to run this loop at 50Hz.
    time.sleep(0.02)

t = numpy.arange(0, 10, 0.01)
speeds = amp * numpy.cos(2 * numpy.pi * freq * t)

positions = []

for s in speeds:
    ergo_robot.head_pan.goal_speed = s
    positions.append(ergo_robot.head_pan.present_position)
    time.sleep(0.05)

# By applying a cosinus on the speed
# You observe a sinusoid on the position
plot(positions)

What do we call “Primitive”?¶

We call Primitive any simple or complex behavior applied to a Robot. A primitive can access all sensors and effectors in the robot. A primitive is supposed to be independent of other primitives. In particular, a primitive is not aware of the other primitives running on the robot at the same time. We imagine those primitives as elementary blocks that can be combined to create more complex blocks in a hierarchical manner.

To ensure this independence, the primitive is running in a sort of sandbox. More precisely, this means that the primitive has not direct access to the robot. It can only request commands (e.g. set a new goal position of a motor) to a PrimitiveManager which transmits them to the “real” robot. As multiple primitives can run on the robot at the same time, their request orders are combined by the manager.

The manager uses a filter function to combine all orders sent by primitives. By default, this filter function is a simple mean but you can choose your own specific filter (e.g. add function).

Writing your own primitive¶

To write you own primitive, you have to subclass the Primitive class. It provides you with basic mechanisms (e.g. connection to the manager, setup of the thread) to allow you to directly “plug” your primitive to your robot and run it.

As an example, let’s write a simple primitive that recreate the dance behavior written in the Controlling your Ergo-Robot section:

import time

import pypot.primitive

class DancePrimitive(pypot.primitive.Primitive):
    def run(self, amp=30, freq=0.5):
        # self.elapsed_time gives you the time (in s) since the primitive has been running
        while self.elapsed_time < 30:
            x = amp * numpy.sin(2 * numpy.pi * freq * self.elapsed_time)

            self.robot.base_pan.goal_position = x
            self.robot.head_pan.goal_position = -x

            time.sleep(0.02)

To run this primitive on your robot, you simply have to do:

ergo_robot = pypot.robot.from_config(...)
ergo_robot.start_sync()

dance = DancePrimitive(ergo_robot)
dance.start()

If you want to make the dance primitive infinite you can use the LoopPrimitive class:

class LoopDancePrimitive(pypot.primitive.LoopPrimitive):
    # The update function is automatically called at the frequency given on the constructor
    def update(self, amp=30, freq=0.5):
        pypot.primitive.LoopPrimitive.update(self)

        x = amp * numpy.sin(2 * numpy.pi * freq * self.elapsed_time)

        self.robot.base_pan.goal_position = x
        self.robot.head_pan.goal_position = -x

And then runs it with:

ergo_robot = pypot.robot.from_config(...)
ergo_robot.start_sync()

dance = LoopDancePrimitive(ergo_robot, 50)
# The robot will dance until you call dance.stop()
dance.start()

class MyDummyPrimitive(pypot.primitive.Primitive):
    def run(self, motors_name):
        motors = [getattr(self.robot, name) for name in motors_name]

        while True:
            for m in fake_motors:
                ...

class MyDummyPrimitive(pypot.primitive.Primitive):
    def run(self, motors):
        fake_motors = [self.get_mockup_motor(m) for m in motors]

        while True:
            for m in fake_motors:
                ...

Starting/pausing primitives¶

The primitive can be start(), stop(), pause() and resume(). Unlike regular python thread, primitive can be restart by calling again the start() method.

When overriding the Primitive, you are responsible for correctly handling those events. For instance, the stop method will only trigger the should stop event that you should watch in your run loop and break it when the event is set. In particular, you should check the should_stop() and should_pause() in your run loop. You can also use the wait_to_stop() and wait_to_resume() to wait until the commands have really been executed.

Attaching a primitive to the robot¶

In the previous section, we explain that the primitives run in a sandbox in the sense that they are not aware of the other primitives running at the same time. In fact, this is not exactly true. More precisely, a primitive can access everything attached to the robot: e.g. motors, sensors. But you can also attach a primitive to the robot.

Let’s go back on our DancePrimitive example. You can write:

ergo_robot = pypot.robot.from_config(...)
ergo_robot.start_sync()

ergo_robot.attach_primitive(DancePrimitive(ergo_robot), 'dance')
ergo_robot.dance.start()

By attaching a primitive to the robot, you make it accessible from within other primitive.

For instance you could then write:

class SelectorPrimitive(pypot.primitive.Primitive):
    def run(self):
        if song == 'my_favorite_song_to_dance' and not self.robot.dance.is_alive():
            self.robot.dance.start()

Protocol¶

Our protocol allows you define three types of requests:

• Get Request (to retrieve a motor register value, access a primitive variable)
• Set Request (to set a motor register value, modify any variable within the robot instance)
• Call Request (to call a method of any object defined within the robot instance, e.g. a primitive)

An entire request is defined as follows:

req = {
    'get': get_request,
    'set': set_request,
    'call': call_request
}

get_request = {
    obj_name_1: (var_path_1, var_path_2, ...),
    obj_name_2: (var_path_1, var_path_2, ...),
    ...
}

get_request = {
    'base_tilt_lower': ('present_position', 'present_load'),
    'base_tilt_upper': ('present_temperature', ),
    'dance', ('current_song.filename', ) # Where dance is an attached primitive
}

answer = {
    'get': {
        'base_tilt_lower': {'present_position': 10.0, 'present_load': 23.0},
        'base_tilt_upper': {'present_temperature': 40},
        'dance': {'current_song.filename': 'never_gonna_give_you_up.mp3'}
    }
}

set_request = {
    obj_name_1: {var_path_1: value1, var_path_2: value2, ...},
    obj_name_2: {var_path_1: value1, var_path_2: value2, ...},
    ...
}

answer = {
    'set': None,
}

call_request = {
    obj_name_1: {meth_name_1: args, meth_name_2: args, ...},
    obj_name_2: {meth_name_1: args, meth_name_2: args, ...},
    ...
}

call_request = {
    'dance', {'start': ()} # The start method does not take any argument so we pass the empty list.
}

answer = {
    'call': {
        'dance': {'start': None}, # The start methods does not return anything.
    }
}

Zmq Server¶

The Zmq Server used a Zmq socket to send (resp. receive) JSON request (JSON answer). It is based on the REQ/REP pattern. So you should always alternate sending and receiving. It will probably be switched to PUB/SUB soon.

Zmq has been chosen as it has been binded to most language (http://zeromq.org/bindings:_start) and can thus be used to connect code from other language to PyPot. For instance, we used it to connect RLPark (a Java reinforcement learning library) to PyPot.

Here is an example of how you can create a zmq server and send request:

import zmq

robot = pypot.robot.from_config(...)
robot.start_sync()

server = pypot.server.ZMQServer(robot, host, port)
server.start()

c = zmq.Context()
s = c.socket(zmq.REQ)
s.connect('tcp://{}:{}'.format(host, port))

req = {
    'get': {motor_name: ('present_load', )},
    'set': {motor_name: {'goal_position': 20.0}}
}

s.send_json(req)
answer = s.recv_json()

Http Server¶

The HTTPServer is based on the bottle python framework (http://bottlepy.org/). We have developed a sort of REST API based on the protocol described above:

• GET /motor/list.json
• GET /primitive/list.json
• GET /motor/<name>/register.json (or GET /<name>/register.json)
• GET /motor/<name>/<register> (or GET /<name>/<register>)
• POST /motor/<name>/<register> (or POST /<name>/<register>)
• POST /primitive/<prim_name>/call/<meth_name> (or GET /<prim_name>/call/<meth_name>)
• POST /request.json

An example of how you can use the HTTP server:

import urllib2
import json
import time

import pypot.robot
import pypot.server

robot = pypot.robot.from_config(...)
robot.start_sync()

server = pypot.server.HTTPServer(robot, host, port)
server.start()

time.sleep(1) # Make sure the server is really started

url = 'http://{}:{}/motor/list.json'.format(host, port)
print urllib2.urlopen(url).read()

url = 'http://{}:{}/motor/base_tilt_lower/goal_position'.format(host, port)
data = 20.0
r = urllib2.Request(url, data=json.dumps(data), headers={'Content-Type': 'application/json'})
print urllib2.urlopen(r).read()