Merge pull request #104 from poppy-project/synchronous-dxl-register

Add Light Synchronous Loop and better support for XL-320.

changelog.md

@@ -1,6 +1,12 @@
 # Changelog
 
 ## V 2.8
+### Sync Loop
+* possibility to define synchronous loop
+* define a "light" synchronization loop
+* can now choose sync. loop inside the config
+
+### Better Sensor Integration
 * can specify sensor inside the config file
 
 ## V 2.1

pypot/_version.py

@@ -1 +1 @@
-__version__ = '2.7.8'
+__version__ = '2.8.0'

pypot/dynamixel/__init__.py

@@ -3,7 +3,7 @@
 
 from .io import DxlIO, Dxl320IO, DxlError
 from .error import BaseErrorHandler
-from .controller import BaseDxlController
+from .syncloop import BaseDxlController
 from .motor import DxlMXMotor, DxlAXRXMotor, DxlXL320Motor
 
 from ..robot import Robot

pypot/dynamixel/controller.py

@@ -4,99 +4,62 @@
 
 
 class DxlController(MotorsController):
-    """ Synchronizes the reading/writing of :class:`~pypot.dynamixel.motor.DxlMotor` with the real motors.
-
-        This class handles synchronization loops that automatically read/write values from the "software" :class:`~pypot.dynamixel.motor.DxlMotor` with their "hardware" equivalent. Those loops shared a same :class:`~pypot.dynamixel.io.DxlIO` connection to avoid collision in the bus. Each loop run within its own thread as its own frequency.
-
-        .. warning:: As all the loop attached to a controller shared the same bus, you should make sure that they can run without slowing down the other ones.
-
-        """
-    def __init__(self, io, motors, controllers):
-        MotorsController.__init__(self, io, motors, 1.)
-        self.controllers = controllers
-
-    def setup(self):
-        """ Starts all the synchronization loops. """
-        [c.start() for c in self.controllers]
-        [c.wait_to_start() for c in self.controllers]
-
-    def update(self):
-        pass
-
-    def teardown(self):
-        """ Stops the synchronization loops. """
-        [c.stop() for c in self.controllers]
-
-
-class BaseDxlController(DxlController):
-    """ Implements a basic controller that synchronized the most frequently used values.
-
-    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 gains (or compliance margin and slope) at 10Hz
-        * reads the present voltage and temperature at 1Hz
-
-    """
-    def __init__(self, io, motors):
-        factory = _DxlRegisterController
-
-        controllers = [_PosSpeedLoadDxlController(io, motors, 50),
-                       AngleLimitRegisterController(io, motors,
-                                                    1, 'get', 'angle_limit'),
-                       factory(io, motors, 1, 'get', 'present_voltage'),
-                       factory(io, motors, 1, 'get', 'present_temperature')]
-
-        models = set(m.model for m in motors)
-
-        if [m for m in models if m.startswith('MX') or m.startswith('XL-320')]:
-            controllers.append(factory(io, motors, 10, 'set', 'pid_gain', 'pid'))
-        elif [m for m in models if m.startswith('AX') or m.startswith('RX')]:
-            controllers.append(factory(io, motors, 10, 'set', 'compliance_margin'))
-            controllers.append(factory(io, motors, 10, 'set', 'compliance_slope'))
-
-        DxlController.__init__(self, io, motors, controllers)
-
-
-class _DxlController(MotorsController):
-    def __init__(self, io, motors, sync_freq=50.):
+    def __init__(self, io, motors, sync_freq, synchronous,
+                 mode, regname, varname=None):
         MotorsController.__init__(self, io, motors, sync_freq)
 
         self.ids = [m.id for m in self.working_motors]
+        self.synchronous = synchronous
+
+        self.mode = mode
+        self.regname = regname
+        self.varname = regname if varname is None else varname
+
+        for m in motors:
+            if mode == 'get':
+                m._read_synchronous[self.varname] = self.synchronous
+            else:
+                m._write_synchronous[self.varname] = self.synchronous
 
     @property
     def working_motors(self):
         return [m for m in self.motors if not m._broken]
 
+    @property
+    def synced_motors(self):
+        motors = [m for m in self.working_motors if self.varname in m.registers]
 
-class _DxlRegisterController(_DxlController):
-    def __init__(self, io, motors, sync_freq,
-                 mode, regname, varname=None):
-        _DxlController.__init__(self, io, motors, sync_freq)
+        if self.synchronous:
+            motors = ([m for m in motors if m._read_synced[self.varname].needed]
+                      if self.mode == 'get' else
+                      [m for m in motors if m._write_synced[self.varname].needed])
 
-        self.mode = mode
-        self.regname = regname
-        self.varname = regname if varname is None else varname
+        return motors
 
     def setup(self):
         if self.mode == 'set':
             MAX_TRIALS = 25
             for _ in range(MAX_TRIALS):
-                if self.get_register():
+                if self.get_register(self.working_motors):
                     break
                 time.sleep(0.1)
             else:
                 raise IOError('Cannot initialize syncloop for "{}"'.format(
                               self.regname))
 
     def update(self):
-        self.get_register() if self.mode == 'get' else self.set_register()
+        if not self.synced_motors:
+            return
+
+        return (self.get_register(self.synced_motors)
+                if self.mode == 'get' else
+                self.set_register(self.synced_motors))
 
-    def get_register(self):
+    def get_register(self, motors):
         """ Gets the value from the specified register and sets it to the :class:`~pypot.dynamixel.motor.DxlMotor`. """
-        motors = [m for m in self.working_motors if hasattr(m, self.varname)]
         if not motors:
             return False
+
         ids = [m.id for m in motors]
 
         values = getattr(self.io, 'get_{}'.format(self.regname))(ids)
@@ -106,23 +69,51 @@ def get_register(self):
         for m, val in zip(motors, values):
             m.__dict__[self.varname] = val
 
+        for m in motors:
+            m._read_synced[self.varname].done()
+
         return True
 
-    def set_register(self):
+    def set_register(self, motors):
         """ Gets the value from :class:`~pypot.dynamixel.motor.DxlMotor` and sets it to the specified register. """
-        motors = [m for m in self.working_motors if hasattr(m, self.varname)]
-
         if not motors:
             return
         ids = [m.id for m in motors]
 
         values = (m.__dict__[self.varname] for m in motors)
         getattr(self.io, 'set_{}'.format(self.regname))(dict(zip(ids, values)))
 
+        for m in motors:
+            m._write_synced[self.varname].done()
+
 
-class AngleLimitRegisterController(_DxlRegisterController):
-    def get_register(self):
-        motors = [m for m in self.working_motors if hasattr(m, self.varname)]
+class AngleLimitRegisterController(DxlController):
+    def __init__(self, io, motors, sync_freq, synchronous):
+        DxlController.__init__(self, io, motors, sync_freq,
+                               synchronous, 'get', 'angle_limit')
+
+        self.varnames = ['lower_limit', 'upper_limit']
+        for m in motors:
+            for var in self.varnames:
+                m._read_synchronous[var] = self.synchronous
+
+    @property
+    def synced_motors(self):
+        motors = self.working_motors
+
+        if self.synchronous:
+            sync_motors = []
+
+            for m in motors:
+                for var in self.varnames:
+                    if m._read_synced[var].needed:
+                        sync_motors.append(m)
+
+            motors = sync_motors
+
+        return motors
+
+    def get_register(self, motors):
         if not motors:
             return
 
@@ -132,8 +123,16 @@ def get_register(self):
         for m, val in zip(motors, values):
             m.__dict__['lower_limit'], m.__dict__['upper_limit'] = val
 
+        for m in motors:
+            for var in ['lower_limit', 'upper_limit']:
+                m._read_synced[var].done()
+
+
+class PosSpeedLoadDxlController(DxlController):
+    def __init__(self, io, motors, sync_freq):
+        DxlController.__init__(self, io, motors, sync_freq,
+                               False, 'get', 'present_position')
 
-class _PosSpeedLoadDxlController(_DxlController):
     def setup(self):
         torques = self.io.is_torque_enabled(self.ids)
         for m, c in zip(self.working_motors, torques):
@@ -148,33 +147,34 @@ def setup(self):
             m.__dict__['torque_limit'] = l
 
     def update(self):
-        self.get_present_position_speed_load()
-        self.set_goal_position_speed_load()
+        self.get_present_position_speed_load(self.working_motors)
+        self.set_goal_position_speed_load(self.working_motors)
 
-    def get_present_position_speed_load(self):
-        values = self.io.get_present_position_speed_load(self.ids)
+    def get_present_position_speed_load(self, motors):
+        ids = [m.id for m in motors]
+        values = self.io.get_present_position_speed_load(ids)
 
         if not values:
             return
 
         positions, speeds, loads = zip(*values)
 
-        for m, p, s, l in zip(self.working_motors, positions, speeds, loads):
+        for m, p, s, l in zip(motors, positions, speeds, loads):
             m.__dict__['present_position'] = p
             m.__dict__['present_speed'] = s
             m.__dict__['present_load'] = l
 
-    def set_goal_position_speed_load(self):
+    def set_goal_position_speed_load(self, motors):
         change_torque = {}
-        torques = [not m.compliant for m in self.working_motors]
-        for m, t, old_t in zip(self.working_motors, torques, self._old_torques):
+        torques = [not m.compliant for m in motors]
+        for m, t, old_t in zip(motors, torques, self._old_torques):
             if t != old_t:
                 change_torque[m.id] = t
         self._old_torques = torques
         if change_torque:
             self.io._set_torque_enable(change_torque)
 
-        rigid_motors = [m for m in self.working_motors if not m.compliant]
+        rigid_motors = [m for m in motors if not m.compliant]
         ids = tuple(m.id for m in rigid_motors)
 
         if not ids:

pypot/dynamixel/motor.py

@@ -1,9 +1,13 @@
 import numpy
 import logging
 
+from collections import defaultdict
+
 import pypot.utils.pypot_time as time
 
 from ..robot.motor import Motor
+
+from ..utils import SyncEvent
 from ..utils.trajectory import GotoMinJerk
 from ..utils.stoppablethread import StoppableLoopThread
 
@@ -16,7 +20,15 @@ def __init__(self, rw=False):
         self.rw = rw
 
     def __get__(self, instance, owner):
-        return instance.__dict__.get(self.label, 0)
+        if instance._read_synchronous[self.label]:
+            sync = instance._read_synced[self.label]
+
+            if not sync.is_recent:
+                sync.request()
+
+        value = instance.__dict__.get(self.label, 0)
+
+        return value
 
     def __set__(self, instance, value):
         if not self.rw:
@@ -26,6 +38,10 @@ def __set__(self, instance, value):
                      instance.name, self.label, value)
         instance.__dict__[self.label] = value
 
+        if instance._write_synchronous[self.label]:
+            sync = instance._write_synced[self.label]
+            sync.request()
+
 
 class DxlOrientedRegister(DxlRegister):
     def __get__(self, instance, owner):
@@ -119,6 +135,12 @@ def __init__(self, id, name=None, model='',
 
         self._broken = broken
 
+        self._read_synchronous = defaultdict(lambda: False)
+        self._read_synced = defaultdict(SyncEvent)
+
+        self._write_synchronous = defaultdict(lambda: False)
+        self._write_synced = defaultdict(SyncEvent)
+
     def __repr__(self):
         return ('<DxlMotor name={self.name} '
                 'id={self.id} '