From 57d2c79b8a6912d019179ed165344b7320779ef9 Mon Sep 17 00:00:00 2001
From: sonelu <alex.sonea@gmail.com>
Date: Mon, 18 May 2020 10:42:49 +0100
Subject: [PATCH 1/3] preliminary commit

---
 roboglia/base/robot.py | 157 ++++++++++++++++++++++++++++++++++++-----
 tests.py               |   4 +-
 tests/dummy_robot.yml  |   6 +-
 3 files changed, 147 insertions(+), 20 deletions(-)

diff --git a/roboglia/base/robot.py b/roboglia/base/robot.py
index 01305ec..1dae093 100644
--- a/roboglia/base/robot.py
+++ b/roboglia/base/robot.py
@@ -15,8 +15,12 @@
 
 import yaml
 import logging
+# import statistics
 
 from ..utils import get_registered_class, check_key, check_type
+# , check_options
+from .thread import BaseLoop
+from .joint import Joint
 
 logger = logging.getLogger(__name__)
 
@@ -93,7 +97,7 @@ class of the bus
         sync.
     """
     def __init__(self, name='ROBOT', buses={}, inits={}, devices={},
-                 joints={}, sensors={}, groups={}, syncs={}):
+                 joints={}, sensors={}, groups={}, syncs={}, manager={}):
         logger.info('***** Initializing robot *************')
         self.__name = name
         if not buses:
@@ -112,6 +116,7 @@ def __init__(self, name='ROBOT', buses={}, inits={}, devices={},
         self.__init_sensors(sensors)
         self.__init_groups(groups)
         self.__init_syncs(syncs)
+        self.__init_manager(manager)
         logger.info('***** Initialization complete ********')
 
     @classmethod
@@ -132,7 +137,7 @@ def from_yaml(cls, file_name):
             in case the file is not available
 
         """
-        logger.info(f'Instantiating robot from YAML file {file_name}')
+        logger.info(f'Creating robot from YAML file {file_name}')
         with open(file_name, 'r') as f:
             init_dict = yaml.load(f, Loader=yaml.FullLoader)
             if len(init_dict) > 1:
@@ -144,7 +149,7 @@ def from_yaml(cls, file_name):
     def __init_buses(self, buses):
         """Called by ``__init__`` to parse and instantiate buses."""
         self.__buses = {}
-        logger.info('Initializing buses...')
+        logger.info('Settting up buses...')
         for bus_name, bus_info in buses.items():
             # add the name in the dict
             bus_info['name'] = bus_name
@@ -160,7 +165,7 @@ def __init_devices(self, devices):
         """Called by ``__init__`` to parse and instantiate devices."""
         self.__devices = {}
         self.__dev_by_id = {}
-        logger.info('Initializing devices...')
+        logger.info('Setting up devices...')
         for dev_name, dev_info in devices.items():
             # add the name in the dev_info
             dev_info['name'] = dev_name
@@ -188,7 +193,7 @@ def __init_devices(self, devices):
     def __init_joints(self, joints):
         """Called by ``__init__`` to parse and instantiate joints."""
         self.__joints = {}
-        logger.info('Initializing joints...')
+        logger.info('Setting up joints...')
         for joint_name, joint_info in joints.items():
             # add the name in the joint_info
             joint_info['name'] = joint_name
@@ -210,7 +215,7 @@ def __init_joints(self, joints):
     def __init_sensors(self, sensors):
         """Called by ``__init__`` to parse and instantiate sensors."""
         self.__sensors = {}
-        logger.info('Initializing sensors...')
+        logger.info('Setting up sensors...')
         for sensor_name, sensor_info in sensors.items():
             # add the name in the joint_info
             sensor_info['name'] = sensor_name
@@ -232,7 +237,7 @@ def __init_sensors(self, sensors):
     def __init_groups(self, groups):
         """Called by ``__init__`` to parse and instantiate groups."""
         self.__groups = {}
-        logger.info('Initializing groups...')
+        logger.info('Setting up groups...')
         for grp_name, grp_info in groups.items():
             new_grp = set()
             # groups of devices
@@ -256,7 +261,7 @@ def __init_groups(self, groups):
     def __init_syncs(self, syncs):
         """Called by ``__init__`` to parse and instantiate syncs."""
         self.__syncs = {}
-        logger.info('Initializing syncs...')
+        logger.info('Setting up syncs...')
         for sync_name, sync_info in syncs.items():
             sync_info['name'] = sync_name
             check_key('group', sync_info, 'sync', sync_name, logger)
@@ -272,6 +277,31 @@ def __init_syncs(self, syncs):
             self.__syncs[sync_name] = new_sync
             logger.debug(f'sync {sync_name} added')
 
+    def __init_manager(self, manager):
+        """Called by ``__init__`` to parse and instantiate the robot
+        manager."""
+        # process joints and replace names with objects
+        logger.info('Setting up manager...')
+        joints = manager.get('joints', [])
+        for index, joint_name in enumerate(joints):
+            check_key(joint_name, self.joints, 'manager', self.name, logger)
+            joints[index] = self.joints[joint_name]
+        group_name = manager.get('group', '')
+        if group_name:
+            check_key(group_name, self.groups, 'manager', self.name, logger)
+            group = self.groups[group_name]
+            for joint in group:
+                check_type(joint, Joint, 'manager', self.name, logger)
+        else:
+            group = set()
+        if 'joints' in manager:
+            del manager['joints']
+        if 'group' in manager:
+            del manager['group']
+        self.__manager = JointManager(name=self.name, joints=joints,
+                                      group=group, **manager)
+        logger.debug(f'manager {self.name} added')
+
     @property
     def name(self):
         """(read-only) The name of the robot."""
@@ -328,6 +358,11 @@ def syncs(self):
         """(read-only) The syncs of the robot as a dict."""
         return self.__syncs
 
+    @property
+    def manager(self):
+        """The RobotManager of the robot."""
+        return self.__manager
+
     def start(self):
         """Starts the robot operation. It will:
 
@@ -351,13 +386,8 @@ def start(self):
         for device in self.devices.values():
             logger.info(f'--> Opening device: {device.name}')
             device.open()
-        logger.info('Activating joints...')
-        for joint in self.joints.values():
-            if joint.auto_activate:
-                logger.info(f'--> Activating joint: {joint.name}')
-                joint.active = True
-            else:
-                logger.info(f'--> Activating joint: {joint.name} - skipped')
+        logger.info('Starting joint manager...')
+        self.manager.start()
         logger.info('Starting syncs...')
         for sync in self.syncs.values():
             if sync.auto_start:
@@ -380,8 +410,8 @@ def stop(self):
         for sync in self.syncs.values():
             logger.debug(f'--> Stopping sync: {sync.name}')
             sync.stop()
-        for joint in self.joints.values():
-            logger.debug(f'--> Deactivating joint: {joint.name}')
+        logger.info('Stopping joint manager...')
+        self.manager.stop()
         logger.info('Closing devices...')
         for device in self.devices.values():
             logger.debug(f'--> Closing device: {device.name}')
@@ -391,3 +421,96 @@ def stop(self):
             logger.debug(f'--> Closing bus: {bus.name}')
             bus.close()
         logger.info('***** Robot stopped ******************')
+
+
+class JointManager(BaseLoop):
+    """Implements the management of the joints by alowing multiple movement
+    streams to submit position commands to the robot.
+
+    The ``JointManager`` inherits the constructor paramters from
+    :py:class:`BaseLoop`. Please refer to that class for mote details.
+
+    In addition the class introduces the following additional paramters:
+
+    Parameters
+    ----------
+    joints: list of :py:class:roboglia.Base.`Joint` or subclass
+        The list of joints that the manager is having under control.
+        Alternatively you can use the parameter ``group`` (see below)
+
+    group: set of :py:class:roboglia.Base.`Joint` or subclass
+        A group of joints that was defined earlier with a ``group``
+        statement in the robot definition file.
+
+    function: str
+        The function used to produce the blended command for the joints.
+        Allowed values are 'mean', 'median', 'weighted'.
+    """
+    def __init__(self, name='JointManager', frequency=100.0, joints=[],
+                 group=None, function='mean', **kwargs):
+        super().__init__(name=name, frequency=frequency, **kwargs)
+        temp_joints = []
+        if joints:
+            temp_joints.extend(joints)
+        if group:
+            temp_joints.extend(group)
+        # eliminate duplicates
+        self.__joints = list(set(temp_joints))
+        if len(self.__joints) == 0:
+            logger.warning('joint manager does not have any joints '
+                           'attached to it')
+        # check_options(function, ['mean', 'median', ])
+        self.__streams = []
+        self.__commands = [[]] * len(self.__joints)
+
+    @property
+    def joints(self):
+        return self.__joints
+
+    def register_stream(self, stream):
+        """Used by a stream of commands to notify the Joint Manager that they
+        would like to submit commad data.
+
+        Paramters
+        ---------
+        stream: :py:class:`Mover` or subclass
+            The class that will provide joint commands in the future.
+
+        Returns
+        -------
+        list of `Joint`
+            The method returns an ID for the stream to use in submission
+            and the list of joints that the stream must supply in order.
+            It is the responsibility of the stream to process this
+            information appropriately and make sure that the commads they
+            supply are suitable for the joints advertised by the manager.
+        """
+
+    def start(self):
+        """Starts the JointManager. Before calling the
+        :py:meth:`BaseThread.start` it activates the joints if they
+        indicate they have the ``auto`` flag set.
+        """
+        for joint in self.joints:
+            if joint.auto_activate and not joint.active:
+                logger.info(f'--> Activating joint: {joint.name}')
+                joint.active = True
+            else:
+                logger.info(f'--> Activating joint: {joint.name} - skipped')
+        super().start()
+
+    def stop(self):
+        """Stops the JointManager. After calling the
+        :py:meth:`BaseThread.stop` it deactivates the joints if they
+        indicate they have the ``auto`` flag set.
+        """
+        super().stop()
+        for joint in self.joints:
+            if joint.auto_activate and joint.active:
+                logger.info(f'--> Deactivating joint: {joint.name}')
+                joint.active = False
+            else:
+                logger.info(f'--> Deactivating joint: {joint.name} - skipped')
+
+    def atomic(self):
+        pass
diff --git a/tests.py b/tests.py
index 04773f5..82431b5 100644
--- a/tests.py
+++ b/tests.py
@@ -641,12 +641,12 @@ def test_dynamixel_bus_acquire(self, mock_robot_init, caplog):
         # read
         caplog.clear()
         bus.read(dev.return_delay_time)
-        assert len(caplog.records) == 1
+        assert len(caplog.records) >= 1
         assert 'failed to acquire bus ttys1' in caplog.text
         # write
         caplog.clear()
         bus.write(dev.return_delay_time, 10)
-        assert len(caplog.records) == 1
+        assert len(caplog.records) >= 1
         assert 'failed to acquire bus ttys1' in caplog.text
         # release bus
         bus.stop_using()
diff --git a/tests/dummy_robot.yml b/tests/dummy_robot.yml
index 25b2fa1..5ceba8d 100644
--- a/tests/dummy_robot.yml
+++ b/tests/dummy_robot.yml
@@ -114,4 +114,8 @@ dummy:
       auto: False
       # we specifically want the loops not to start automatically
       # to control the test and make sure that they are no side
-      # effects on more simple checks
\ No newline at end of file
+      # effects on more simple checks
+
+  manager:
+    group: joints
+    frequency: 50.0

From 96e4df2c690f681a27edca1db8c06d17d26511ca Mon Sep 17 00:00:00 2001
From: sonelu <alex.sonea@gmail.com>
Date: Mon, 18 May 2020 19:57:07 +0100
Subject: [PATCH 2/3] JointManager, not testing

---
 roboglia/base/joint.py |  73 ++++++++++++++++++
 roboglia/base/robot.py | 171 ++++++++++++++++++++++++++++++++++++-----
 2 files changed, 224 insertions(+), 20 deletions(-)

diff --git a/roboglia/base/joint.py b/roboglia/base/joint.py
index 83f581c..8f2d7fd 100644
--- a/roboglia/base/joint.py
+++ b/roboglia/base/joint.py
@@ -220,6 +220,28 @@ def desired_position(self):
         value += self.__offset
         return value
 
+    @property
+    def value(self):
+        """Generic accessor / setter that uses tuples to interact with the
+        joint. For position only joints it is a tuple with one element.
+        """
+        return (self.position,)
+
+    @value.setter
+    def value(self, values):
+        """``values`` should be a tuple in all circumstances. For position
+        only joints is a tuple with one element.
+        """
+        pos = values[0]
+        if pos is not None:
+            self.position = pos
+
+    def desired(self):
+        """Generic accessor for desired joint values. Always a tuple. For
+        position only joints is a tuple with one element.
+        """
+        return (self.desired_position, )
+
     def __repr__(self):
         return f'{self.name}: p={self.position:.3f}'
 
@@ -285,6 +307,32 @@ def desired_velocity(self):
         # should be absolute only
         return self.__vel_w.value
 
+    @property
+    def value(self):
+        """For a PV joint the value is a tuple with 2 values: (position,
+        velocity)."""
+        return (self.position, self.velocity)
+
+    @value.setter
+    def value(self, values):
+        """For a PV joint the value is a tuple with 2 values.
+
+        Parameters
+        ----------
+        values: tuple (position, velocity)
+        """
+        pos = values[0]
+        vel = values[1]
+        if pos is not None:
+            self.position = pos
+        if vel is not None:
+            self.velocity = vel
+
+    def desired(self):
+        """For PV joint the desired is a tuple of 2 values.
+        """
+        return (self.desired_position, self.desired_velocity)
+
     def __repr__(self):
         return f'{Joint.__repr__(self)}, v={self.velocity:.3f}'
 
@@ -350,5 +398,30 @@ def desired_load(self):
         # should be absolute value!
         return self.__load_w.value
 
+    @property
+    def value(self):
+        """For a PVL joint the value is a tuple of 3 values (position,
+        velocity, load)
+        """
+        return (self.position, self.velocity, self.load)
+
+    @value.setter
+    def value(self, values):
+        """For a PVL joint the value is a tuple of 3 values.
+
+        Parameters
+        ----------
+        values: tuple (position, velocity, load)
+        """
+        pos = values[0]
+        vel = values[1]
+        load = values[2]
+        if pos is not None:
+            self.position = pos
+        if vel is not None:
+            self.velocity = vel
+        if vel is not None:
+            self.load = load
+
     def __repr__(self):
         return f'{JointPV.__repr__(self)}, l={self.load:.3f}'
diff --git a/roboglia/base/robot.py b/roboglia/base/robot.py
index 1dae093..13bb869 100644
--- a/roboglia/base/robot.py
+++ b/roboglia/base/robot.py
@@ -15,10 +15,10 @@
 
 import yaml
 import logging
-# import statistics
+import threading
+import statistics
 
-from ..utils import get_registered_class, check_key, check_type
-# , check_options
+from ..utils import get_registered_class, check_key, check_type, check_options
 from .thread import BaseLoop
 from .joint import Joint
 
@@ -444,10 +444,15 @@ class JointManager(BaseLoop):
 
     function: str
         The function used to produce the blended command for the joints.
-        Allowed values are 'mean', 'median', 'weighted'.
+        Allowed values are 'mean' and 'median'.
+
+    timeout: float
+        Is a time in seconds an accessor will wait before issuing a timeout
+        when trying to submit data to the manager or the manager preparing
+        the data for the joints.
     """
     def __init__(self, name='JointManager', frequency=100.0, joints=[],
-                 group=None, function='mean', **kwargs):
+                 group=None, function='mean', timeout=0.5, **kwargs):
         super().__init__(name=name, frequency=frequency, **kwargs)
         temp_joints = []
         if joints:
@@ -459,32 +464,111 @@ def __init__(self, name='JointManager', frequency=100.0, joints=[],
         if len(self.__joints) == 0:
             logger.warning('joint manager does not have any joints '
                            'attached to it')
-        # check_options(function, ['mean', 'median', ])
-        self.__streams = []
-        self.__commands = [[]] * len(self.__joints)
+        check_options(function, ['mean', 'median'], 'JointManager', name,
+                      logger)
+        if function == 'mean':
+            self.__func = statistics.mean
+        elif function == 'median':
+            self.__func = statistics.median
+        else:
+            raise NotImplementedError
+        self.__submissions = {}
+        self.__adjustments = {}
+        self.__lock = threading.Lock()
 
     @property
     def joints(self):
         return self.__joints
 
-    def register_stream(self, stream):
-        """Used by a stream of commands to notify the Joint Manager that they
-        would like to submit commad data.
+    def submit(self, name, commands, adjustments=False):
+        """Used by a stream of commands to notify the Joint Manager they
+        joint commands they want.
 
         Paramters
         ---------
-        stream: :py:class:`Mover` or subclass
-            The class that will provide joint commands in the future.
+        name: str
+            The name of the stream providing the data. It is used to keep the
+            request separate and be able to merge later.
+
+        commands: dict
+            A dictionary with the commands requests in the format::
+
+                {joint_name: (values)}
+
+            Where ``values`` is a tuple with the command for that joint. It
+            is acceptable to send partial commands to a joint, for instance
+            you can send only (100,) (position 100) to a JointPVL. Submitting
+            more information to a joint will have no effect, for instance
+            (100, 20, 40) (position, velocity, load) to a Joint will only
+            use the position part of the request.
+
+        adjustments: bool
+            Indicates that the values are to be treated as adjustments to
+            the other requests instead of absolute requests. This is
+            convenient for streams that request postion correction like
+            an accelerometer based balance control. Internally the
+            JointManger keeps the commands separate between the absolute
+            and the adjustments ones and calculates separate averages then
+            adjusts the absolute results with the ones from the adjustments
+            to produce the final numbers.
 
         Returns
         -------
-        list of `Joint`
-            The method returns an ID for the stream to use in submission
-            and the list of joints that the stream must supply in order.
-            It is the responsibility of the stream to process this
-            information appropriately and make sure that the commads they
-            supply are suitable for the joints advertised by the manager.
+        bool:
+            ``True`` if the operation was successful. False if there was an
+            error (most likely the lock was not acquired). Caller needs to
+            review this and decide if they should retry to send data.
         """
+        if not self.__lock.acquire():
+            logger.error(f'failed to acquire manager for stream {name}')
+            return False
+        else:
+            if adjustments:
+                self.__adjustments[name] = commands
+            else:
+                self.__submissions[name] = commands
+            self.__lock.release()
+            return True
+
+    def stop_submit(self, name, adjustments=False):
+        """Notifies the ``JointManager`` that the stream has finished
+        sending data and as a result the data in the ``JointManager`` cache
+        should be removed.
+
+        .. warning:: If the stream does not call this method when it
+            finished with a routine the last submission will remain in
+            the cache and will continue to be averaged with the other
+            requests, creating problems. Don't forget to call this method
+            when your move finishes!
+
+        Parameters
+        ----------
+        name: str
+            The name of the move sending the data
+
+        adjustments: bool
+            Indicates the move submitted to the adjustment stream.
+
+        Returns
+        -------
+        bool:
+            ``True`` if the operation was successful. False if there was an
+            error (most likely the lock was not acquired). Caller needs to
+            review this and decide if they should retry to send data. In the
+            case of this method it is advisable to try resending the request,
+            otherwise stale data will stay in the cache.
+        """
+        if not self.__lock.acquire():
+            logger.error(f'failed to acquire manager for stream {name}')
+            return False
+        else:
+            if adjustments:
+                if name in self.__adjustments:      # pragma: no branch
+                    del self.__adjustments[name]
+            else:
+                if name in self.__submissions:      # pragma: no branch
+                    del self.__submissions[name]
+            return True
 
     def start(self):
         """Starts the JointManager. Before calling the
@@ -513,4 +597,51 @@ def stop(self):
                 logger.info(f'--> Deactivating joint: {joint.name} - skipped')
 
     def atomic(self):
-        pass
+        if not self.__lock.acquire():
+            logger.error('failed to acquire lock for atomic processing')
+        else:
+            for joint in self.joints:
+                comm = self.__process_request(joint, self.__submissions)
+                adj = self.__process_request(joint, self.__adjustments)
+                joint.value = self.__add_with_none(comm, adj)
+        self.__lock.release()
+
+    def __process_request(self, joint, requests):
+        """Processes a list of requests and calculates averages."""
+        pos_req = []
+        vel_req = []
+        ld_req = []
+        for request in requests:
+            values = request.get(joint.name, None)
+            if not values:
+                continue
+            else:
+                pos_req.append(values[0])
+                if len(values) > 1:             # pragma: no branch
+                    vel_req.append(values[1])
+                    if len(values) > 2:         # pragma: no branch
+                        ld_req.append(values[2])
+        if len(pos_req) == 0:
+            return (None, None, None)
+        else:
+            pos = self.__func(pos_req)
+            vel = self.__func(vel_req) if len(vel_req) > 0 else None
+            ld = self.__func(ld_req) if len(ld_req) > 0 else None
+            return (pos, vel, ld)
+
+    def __add_with_none(self, val1, val2):
+        """Adds two numbers that could be ``None``."""
+        if val1 is None:
+            return val2
+        else:
+            if val2 is None:
+                return val1
+            else:
+                return val1 + val2
+
+    def __add_command_tuples(self, comm1, comm2):
+        c1_p, c1_v, c1_l = comm1
+        c2_p, c2_v, c2_l = comm2
+        return (self.__add_with_none(c1_p, c2_p),
+                self.__add_with_none(c1_v, c2_v),
+                self.__add_with_none(c1_l, c2_l))

From 0c55b8d16d690d1b30a1373ec465079cccea6c7a Mon Sep 17 00:00:00 2001
From: sonelu <alex.sonea@gmail.com>
Date: Tue, 19 May 2020 18:10:56 +0100
Subject: [PATCH 3/3] finished manager and move with tests

---
 roboglia/base/joint.py    |   2 +-
 roboglia/base/robot.py    |  41 +++--
 roboglia/base/thread.py   |  12 +-
 roboglia/move/__init__.py |   1 +
 roboglia/move/moves.py    | 367 ++++++++++++++++++++++++++++++++++++++
 roboglia/move/thread.py   |  74 +++++---
 tests.py                  |  53 ++++++
 tests/move_robot.yml      |  80 +++++++++
 tests/moves/sequence.yml  |  47 +++++
 9 files changed, 632 insertions(+), 45 deletions(-)
 create mode 100644 roboglia/move/moves.py
 create mode 100644 tests/move_robot.yml
 create mode 100644 tests/moves/sequence.yml

diff --git a/roboglia/base/joint.py b/roboglia/base/joint.py
index 8f2d7fd..967e134 100644
--- a/roboglia/base/joint.py
+++ b/roboglia/base/joint.py
@@ -420,7 +420,7 @@ def value(self, values):
             self.position = pos
         if vel is not None:
             self.velocity = vel
-        if vel is not None:
+        if load is not None:
             self.load = load
 
     def __repr__(self):
diff --git a/roboglia/base/robot.py b/roboglia/base/robot.py
index 13bb869..0665cb4 100644
--- a/roboglia/base/robot.py
+++ b/roboglia/base/robot.py
@@ -497,10 +497,10 @@ def submit(self, name, commands, adjustments=False):
 
             Where ``values`` is a tuple with the command for that joint. It
             is acceptable to send partial commands to a joint, for instance
-            you can send only (100,) (position 100) to a JointPVL. Submitting
-            more information to a joint will have no effect, for instance
-            (100, 20, 40) (position, velocity, load) to a Joint will only
-            use the position part of the request.
+            you can send only (100,) meaning position 100 to a JointPVL.
+            Submitting more information to a joint will have no effect, for
+            instance (100, 20, 40) (position, velocity, load) to a Joint will
+            only use the position part of the request.
 
         adjustments: bool
             Indicates that the values are to be treated as adjustments to
@@ -588,6 +588,8 @@ def stop(self):
         :py:meth:`BaseThread.stop` it deactivates the joints if they
         indicate they have the ``auto`` flag set.
         """
+        if self.__lock.locked():
+            self.__lock.release()
         super().stop()
         for joint in self.joints:
             if joint.auto_activate and joint.active:
@@ -603,24 +605,43 @@ def atomic(self):
             for joint in self.joints:
                 comm = self.__process_request(joint, self.__submissions)
                 adj = self.__process_request(joint, self.__adjustments)
-                joint.value = self.__add_with_none(comm, adj)
+                value = self.__add_command_tuples(comm, adj)
+                logger.debug(f'Setting joint {joint.name}: value={value}')
+                joint.value = value
         self.__lock.release()
 
     def __process_request(self, joint, requests):
-        """Processes a list of requests and calculates averages."""
+        """Processes a list of requests and calculates averages.
+
+        Paramters
+        ---------
+        joint: Joint or subclass
+            The joint being processed
+
+        requests: dict
+            A dictionary that contains all the requests submitted by streams.
+            They are normally either the :py:class:`JointManager`'s
+            ``submissions`` or ``adjustments``, the two buffers with requests
+            for joint positions. The dictionary has as key the submitter's
+            name and the data is another dict of {joint : (pos, vel, load)}
+            records.
+        """
         pos_req = []
         vel_req = []
         ld_req = []
-        for request in requests:
+        for request in requests.values():
             values = request.get(joint.name, None)
             if not values:
                 continue
             else:
-                pos_req.append(values[0])
+                if values[0] is not None:
+                    pos_req.append(values[0])
                 if len(values) > 1:             # pragma: no branch
-                    vel_req.append(values[1])
+                    if values[1] is not None:
+                        vel_req.append(values[1])
                     if len(values) > 2:         # pragma: no branch
-                        ld_req.append(values[2])
+                        if values[2] is not None:
+                            ld_req.append(values[2])
         if len(pos_req) == 0:
             return (None, None, None)
         else:
diff --git a/roboglia/base/thread.py b/roboglia/base/thread.py
index df1a96e..a0e69de 100644
--- a/roboglia/base/thread.py
+++ b/roboglia/base/thread.py
@@ -262,7 +262,12 @@ def run(self):
         last_count_reset = time.time()
         factor = 1.0            # fine adjust the rate
         while not self.stopped:
-            if not self.paused:
+            if self.paused:
+                # paused; reset the statistics
+                exec_counts = 0
+                last_count_reset = time.time()
+                time.sleep(self.period)
+            else:
                 start_time = time.time()
                 self.atomic()
                 end_time = time.time()
@@ -290,11 +295,6 @@ def run(self):
                     # reset counters
                     exec_counts = 0
                     last_count_reset = time.time()
-            else:
-                # paused; reset the statistics
-                exec_counts = 0
-                last_count_reset = time.time()
-                time.sleep(self.period)
 
     def atomic(self):
         """This method implements the periodic task that needs to be
diff --git a/roboglia/move/__init__.py b/roboglia/move/__init__.py
index 30b393e..07c6ae6 100644
--- a/roboglia/move/__init__.py
+++ b/roboglia/move/__init__.py
@@ -1 +1,2 @@
 from .thread import StepLoop                    # noqa: 401
+from .moves import Script                       # noqa: 401
diff --git a/roboglia/move/moves.py b/roboglia/move/moves.py
new file mode 100644
index 0000000..9e65365
--- /dev/null
+++ b/roboglia/move/moves.py
@@ -0,0 +1,367 @@
+import yaml
+import logging
+
+# from ..utils import check_key
+from .thread import StepLoop
+
+logger = logging.getLogger(__name__)
+
+
+class Script(StepLoop):
+
+    def __init__(self, name='SCRIPT', robot=None, times=1, joints=[],
+                 frames={}, sequences={}, scenes={}, script=[], **kwargs):
+        super().__init__(name=name, times=times, **kwargs)
+        self.__robot = robot
+        self.__init_joints(joints)
+        self.__init_frames(frames)
+        self.__init_sequences(sequences)
+        self.__init_scenes(scenes)
+        self.__init_script(script)
+
+    @classmethod
+    def from_yaml(cls, robot, file_name):
+        with open(file_name, 'r') as f:
+            init_dict = yaml.load(f, Loader=yaml.FullLoader)
+        if len(init_dict) > 1:              # pragma: no branch
+            logger.warning('only the first script will be loaded')
+        name = list(init_dict)[0]
+        components = init_dict[name]
+        return cls(name=name, robot=robot, **components)
+
+    def __init_joints(self, joints):
+        """Used by __init__ to setup the joints. Incorrect joints will be
+        marked with ``None`` and will be filtered out when commands are
+        issued.
+        """
+        for index, joint_name in enumerate(joints):
+            if joint_name not in self.robot.joints:
+                logger.warning(f'joint {joint_name} used in script {self.name}'
+                               f' does not exist in robot {self.robot.name} '
+                               'and will be skipped')
+                joints[index] = None
+            else:
+                joints[index] = self.robot.joints[joint_name]
+        self.__joints = joints
+
+    def __init_frames(self, frames):
+        """Used by __init__ to setup the frames. Handles full frames (dict
+        of position, velocity and loads) or simplified frames (list of
+        positions only)."""
+        self.__frames = {}
+        for frame_name, frame_info in frames.items():
+            if isinstance(frame_info, list):
+                new_frame = Frame(name=frame_name, positions=frame_info)
+            elif isinstance(frame_info, dict):
+                new_frame = Frame(name=frame_name, **frame_info)
+            else:
+                raise NotImplementedError
+            self.__frames[frame_name] = new_frame
+
+    def __init_sequences(self, sequences):
+        """Used by __init__ to setup the sequences. Frames incorrectly
+        referenced will the skipped."""
+        self.__sequences = {}
+        for seq_name, seq_info in sequences.items():
+            frames = seq_info.get('frames', [])
+            if not frames:
+                logger.warning(f'sequence {seq_name} has no frames defined')
+                self.__sequences[seq_name] = None
+            else:
+                # check the frame names and replace with objects
+                for index, frame_name in enumerate(frames):
+                    if frame_name not in self.frames:
+                        logger.warning(f'frame {frame_name} used by sequence '
+                                       f'{seq_name} does not exits '
+                                       'and will be skipped')
+                        frames[index] = None
+                    else:
+                        frames[index] = self.frames[frame_name]
+
+                self.__sequences[seq_name] = \
+                    Sequence(name=seq_name, **seq_info)
+
+    def __init_scenes(self, scenes):
+        """Used by __init__ to setup scenes."""
+        self.__scenes = {}
+        for scene_name, scene_info in scenes.items():
+            sequences = scene_info.get('sequences', None)
+            if not sequences:
+                logger.warning(f'Scene {scene_name} does not have any '
+                               f'sequences defined, it will be skipped')
+                self.__scenes[scene_name] = None
+            else:
+                # replace sequence names with object references
+                for index, seq_name in enumerate(sequences):
+                    # for a scene the sequence representation will be a dict
+                    # that includes the sequence reference and the direction
+                    # of play (inverse)
+                    seq = {}
+                    # check for inverse request
+                    if '.reverse' in seq_name:
+                        seq['reverse'] = True
+                        seq_name = seq_name.replace('.reverse', '')
+                    else:
+                        seq['reverse'] = False
+                    # validate the sequence exists
+                    # if not log error and use None
+                    if seq_name not in self.sequences:
+                        logger.warning(f'sequence {seq_name} used by scene '
+                                       f'{scene_name} does not exist; '
+                                       'it will be skipped')
+                        seq['sequence'] = None
+                    else:
+                        seq['sequence'] = self.sequences[seq_name]
+                    # now replace the sequence name with the enhanced reference
+                    sequences[index] = seq
+                # update the scenes dictionary
+                self.__scenes[scene_name] = \
+                    Scene(name=scene_name, **scene_info)
+
+    def __init_script(self, script):
+        """Called by __init__ to setup the script steps."""
+        for index, scene_name in enumerate(script):
+            if scene_name not in self.scenes:
+                logger.warning(f'scene {scene_name} used by script '
+                               f'{self.name} does not exist - will be skipped')
+                script[index] = None
+            else:
+                script[index] = self.scenes[scene_name]
+        self.__script = script
+
+    @property
+    def robot(self):
+        return self.__robot
+
+    @property
+    def joints(self):
+        return self.__joints
+
+    @property
+    def frames(self):
+        return self.__frames
+
+    @property
+    def sequences(self):
+        return self.__sequences
+
+    @property
+    def scenes(self):
+        return self.__scenes
+
+    @property
+    def script(self):
+        """Returns the script (the list of scenes to be executed)."""
+        return self.__script
+
+    def play(self):
+        """Inherited from :py:class:`StepLoop`. Iterates over the scenes
+        and produces the commands."""
+        logger.debug(f'Script {self.name} playing')
+        for scene in self.script:
+            if scene:           # pragma: no branch
+                logger.debug(f'Script {self.name} playing scene {scene.name}')
+                for frame, duration in scene.play():
+                    yield frame, duration
+
+    def atomic(self, data):
+        """Inherited from :py:class:`StepLoop`. Submits the data to the
+        robot manager only for valid joints."""
+        # data is a list of tuples with the commands for each joint
+        assert len(data) == len(self.joints)
+        # because self.joints could contain None values we cannot use zip
+        commands = {}
+        for index, joint in enumerate(self.joints):
+            if joint:
+                commands[joint.name] = data[index]
+        logger.debug(f'Submitting: {commands}')
+        self.robot.manager.submit(self.name, commands)
+
+    def teardown(self):
+        """Informs the robot manager we are finished."""
+        self.robot.manager.stop_submit(self.name)
+
+
+class Scene():
+
+    def __init__(self, name='SCENE', sequences=[], times=1):
+        self.__name = name
+        self.__sequences = sequences
+        self.__times = times
+
+    @property
+    def name(self):
+        return self.__name
+
+    @property
+    def sequences(self):
+        return self.__sequences
+
+    @property
+    def times(self):
+        return self.__times
+
+    def play(self):
+        for step in range(self.times):
+            logger.debug(f'Scene {self.name} playing iteration {step+1}')
+            for seq_ext in self.sequences:
+                sequence = seq_ext['sequence']
+                reverse = seq_ext['reverse']
+                rev_text = ' in reverse' if reverse else ''
+                if not sequence:
+                    logger.debug('Skipping None sequence')
+                else:
+                    logger.debug(f'Scene {self.name} playing sequence '
+                                 f'{sequence.name}{rev_text}')
+                    for frame, duration in sequence.play(reverse=reverse):
+                        yield frame, duration
+
+
+class Sequence():
+    """A Sequence is an ordered list of of frames that have associated
+    durations in seconds and can be played in a loop a number of times.
+
+    Parameters
+    ----------
+    name: str
+        The name of the sequence
+
+    frames: list of :py:class:`Frame`
+        The frames contained in the sequence. The order in which the frames
+        are listed is the order in which they will be played
+
+    durations: list of float
+        The durations in seconds for each frame. If the length of the list
+        is different than the length of the frames there will be a
+        critical error logged and the sequence will not be loaded.
+
+    times: int
+        The number of times the sequence should be played. Default is 1.
+    """
+    def __init__(self, name='SEQUENCE', frames=[], durations=[], times=1):
+        self.__name = name
+        if len(frames) != len(durations):
+            logger.critical(f'durations supplied for sequence {name} do not '
+                            'match the number of frames')
+            return None
+        else:
+            self.__frames = frames
+            self.__durations = durations
+            self.__times = times
+
+    @property
+    def name(self):
+        """The name of the sequence."""
+        return self.__name
+
+    @property
+    def frames(self):
+        """The list of ``Frame``s in the sequence."""
+        return self.__frames
+
+    @property
+    def durations(self):
+        """The durations associated with each frame."""
+        return self.__durations
+
+    @property
+    def times(self):
+        """The number of times the sequence will be played in a loop."""
+        return self.__times
+
+    def play(self, reverse=False):
+        """Plays the sequence. Produces an iterator over all the frames,
+        repeating as many ``times`` as requested.
+
+        Parameters
+        ----------
+        reverse: bool
+            Indicates if the frames should be played in reverse order.
+
+        Returns
+        -------
+        iterator of tuple (commands, duration)
+            ``commands`` is the list of (pos, vel, load) for each joint
+            from the frame, and ``duration`` is the specified duration for
+            the frame.
+        """
+        for step in range(self.times):
+            logger.debug(f'Sequence {self.name} playing iteration {step+1}')
+            if reverse:
+                zipped = zip(reversed(self.frames), reversed(self.durations))
+            else:
+                zipped = zip(self.frames, self.durations)
+            for frame, duration in zipped:
+                if frame:
+                    logger.debug(f'Sequence {self.name} playing frame '
+                                 f'{frame.name}, duration {duration}')
+                    yield frame.commands, duration
+                else:
+                    logger.debug('None frame - skipping')
+
+
+class Frame():
+    """A ``Frame`` is a single representation of the robots' joints at one
+    point in time. It is described by a list of positions, the velocities
+    wanted to get to those positions and the loads. The last two of them
+    are optional and will be padded with ``None`` in case they do not cover
+    all positions listed in the first parameter.
+
+    Parameters
+    ----------
+    name: str
+        The name of the frame
+
+    positions: list of floats
+        The desired positions for the joints. They are provided in the same
+        order as the number of joints that are described at the begining
+        of the :py:class:`Script` where the frame is used. The unit of
+        measure is the one used for the joints which in turn is dependent
+        on the settings of the registers used by joints.
+
+    velocities: list of floats
+        The velocities used to move to the desired positions. If they are
+        empty or not all covered, the constructor will padded with ``None``s
+        to make it the same size as the positions. You can also use ``None``
+        in the list to indicate that a particular joint does not need to
+        change the velocity (will continue to use the one set previously).
+
+    loads: list of floats
+        The loads used to move to the desired positions. If they are
+        empty or not all covered, the constructor will padded with ``None``s
+        to make it the same size as the positions. You can also use ``None``
+        in the list to indicate that a particular joint does not need to
+        change the load (will continue to use the one set previously).
+    """
+    def __init__(self, name='FRAME', positions=[], velocities=[], loads=[]):
+        self.__name = name
+        p_len = len(positions)
+        self.__pos = positions
+        self.__vel = velocities + [None] * max(0, p_len - len(velocities))
+        self.__loads = loads + [None] * max(0, p_len - len(loads))
+
+    @property
+    def name(self):
+        return self.__name
+
+    @property
+    def positions(self):
+        """Returns the positions of a frame."""
+        return self.__pos
+
+    @property
+    def velocities(self):
+        """Returns the (padded) velocities of a frame."""
+        return self.__vel
+
+    @property
+    def loads(self):
+        """Returns the (padded) loads of a frame."""
+        return self.__loads
+
+    @property
+    def commands(self):
+        """Returns a list of tuples (pos, vel, load) for each joint in the
+        frame.
+        """
+        return list(zip(self.positions, self.velocities, self.loads))
diff --git a/roboglia/move/thread.py b/roboglia/move/thread.py
index 4bddc55..71b44ec 100644
--- a/roboglia/move/thread.py
+++ b/roboglia/move/thread.py
@@ -8,45 +8,63 @@
 
 class StepLoop(BaseThread):
     """A thread that runs in the background and runs a sequence of steps.
+
+    Parameters
+    ----------
+    name: str
+        The name of the step loop
+
+    times: int
+        How many times the loop should be played. If a negative number is
+        given (ex. -1) the loop will play to infinite
     """
-    def __init__(self, init_dict):
-        super().__init__(init_dict)
-        self.steps = init_dict['steps']
-        self.loop = init_dict.get('loop', False)
-        self.index = 0
+    def __init__(self, name='STEPLOOP', times=1, **kwargs):
+        super().__init__(name=name, **kwargs)
+        self.__times = times
+
+    @property
+    def times(self):
+        return self.__times
+
+    def play(self):
+        """Provides the step data. Should be overridden by subclasses and
+        implement a ``yield`` logic. :py:meth:`run` invokes ``next`` on this
+        method to get the data and the duration needed to perform one step.
+        """
+        yield None, 0                   # pragma: no cover
 
     def setup(self):
         """Resets the loop from the begining."""
-        self.index = 0
+        pass
 
     def run(self):
         """Wraps the execution between the duration provided and
-        increments index.
+        decrements iteration run.
         """
-        while not self.stopped:
-            if not self.paused:
+        iteration = self.times
+        while iteration != 0:
+            for data, duration in self.play():
+                logger.debug(f'data={data}, duration={duration}')
+                # handle stop requests
+                if self.stopped:
+                    logger.debug('Thread stopped')
+                    return None
+                # handle pause requests
+                while self.paused:
+                    time.sleep(0.001)          # 1ms
+                # process
                 start_time = time.time()
-                self.atomic()
+                self.atomic(data)
                 end_time = time.time()
-                step_duration = self.steps[self.index]['duration']
-                wait_time = step_duration - (end_time - start_time)
-                if wait_time > 0:
+                wait_time = duration - (end_time - start_time)
+                if wait_time > 0:               # pragma: no branch
                     time.sleep(wait_time)
-                self.index += 1
-                if self.index == len(self.steps):
-                    if self.loop:
-                        self.index = 0
-                    else:
-                        break
-            else:
-                time.sleep(0.001)          # 1ms
-
-    def atomic(self):
+            iteration -= 1
+
+    def atomic(self, data):
         """Executes the step.
 
-        Retrieves the execution method and the parameters from the steps
-        dictionary.
+        Must be overridden in subclass to perform the specific operation on
+        data.
         """
-        method = getattr(self, self.steps[self.index]['execute'])
-        params = self.steps[self.index]['parameters']
-        method(params)
+        raise NotImplementedError
diff --git a/tests.py b/tests.py
index 82431b5..9ec6d53 100644
--- a/tests.py
+++ b/tests.py
@@ -17,6 +17,8 @@
 
 from roboglia.i2c import SharedI2CBus
 
+from roboglia.move import Script
+
 # format = '%(asctime)s %(levelname)-7s %(threadName)-18s %(name)-32s %(message)s'
 # logging.basicConfig(format=format, 
 #                     # file = 'test.log', 
@@ -927,3 +929,54 @@ def test_i2c_closed_bus(self, mock_robot_init, caplog):
         bus.write_block_data(1, 1, 6, [1,2,3,4,5,6])
         assert len(caplog.records) >= 1
         assert 'attempted to write to a closed bus' in caplog.text
+
+
+class TestMove:
+
+    @pytest.fixture
+    def mock_robot(self):   
+        robot = BaseRobot.from_yaml('tests/move_robot.yml')
+        robot.start()
+        yield robot
+        robot.stop()
+
+    def test_move_load_robot(self, mock_robot):
+        manager = mock_robot.manager
+        assert len(manager.joints) == 3
+        p = (100, None, None)
+        pv = (100, 10, None)
+        pvl = (100, 10, 50)
+        all_comm = [p, pv, pvl]
+        for joint in manager.joints:
+            for comm in all_comm:
+                joint.value = comm
+
+    def test_move_load_script(self, mock_robot, caplog):
+        caplog.set_level(logging.DEBUG, logger='roboglia.move.moves')
+        caplog.clear()
+        script = Script.from_yaml(robot=mock_robot, file_name='tests/moves/sequence.yml')
+        assert len(script.joints) == 4
+        assert len(script.frames) == 6
+        assert len(script.sequences) == 4
+        c = list(script.scenes['greet'].play())
+        assert len(c) == 28
+        assert len(caplog.records) >= 49
+
+    def test_move_execute_script(self, mock_robot, caplog):
+        script = Script.from_yaml(robot=mock_robot, file_name='tests/moves/sequence.yml')
+        script.start()
+        time.sleep(1)
+        script.pause()
+        time.sleep(0.5)
+        script.resume()
+        while script.running:
+            time.sleep(0.5)
+        caplog.set_level(logging.DEBUG)
+
+    def test_move_execute_script_with_stop(self, mock_robot, caplog):
+        script = Script.from_yaml(robot=mock_robot, file_name='tests/moves/sequence.yml')
+        script.start()
+        time.sleep(1)
+        script.stop()
+        while script.running:
+            time.sleep(0.5)
diff --git a/tests/move_robot.yml b/tests/move_robot.yml
new file mode 100644
index 0000000..3fe4fd6
--- /dev/null
+++ b/tests/move_robot.yml
@@ -0,0 +1,80 @@
+dummy:
+  buses:
+    busA:
+      class: SharedFileBus
+      port: /tmp/busA.log
+
+  inits:
+    dummy_init:
+      model: null
+      revision: null
+      delay: 0
+      desired_load: 50
+
+    speed_init:
+      desired_speed: 30
+
+  devices:
+    d01:
+      class: BaseDevice
+      bus: busA
+      dev_id: 1
+      model: DUMMY
+      inits: [dummy_init, speed_init]
+
+    d02:
+      class: BaseDevice
+      bus: busA
+      dev_id: 2
+      model: DUMMY
+      inits: [dummy_init]
+
+    d03:
+      class: BaseDevice
+      bus: busA
+      dev_id: 3
+      model: DUMMY
+      inits: [dummy_init, speed_init]
+
+  joints:
+    j01:
+      class: Joint
+      device: d01
+      pos_read: current_pos
+      pos_write: desired_pos
+      activate: enable_device
+
+    j02:
+      class: JointPV
+      device: d02
+      pos_read: current_pos
+      pos_write: desired_pos
+      vel_read: current_speed
+      vel_write: desired_speed
+      activate: enable_device
+
+    j03:
+      class: JointPVL
+      device: d03
+      pos_read: current_pos
+      pos_write: desired_pos
+      vel_read: current_speed
+      vel_write: desired_speed
+      load_read: current_load
+      load_write: desired_load
+      activate: enable_device
+
+  groups:
+    devices:
+      devices: [d01, d02, d03]
+
+    joints:
+      joints: [j01, j02, j03]
+
+    all:
+      groups: [devices, joints]
+
+  manager:
+    group: joints
+    joints: [j01, j02]
+    frequency: 50.0
diff --git a/tests/moves/sequence.yml b/tests/moves/sequence.yml
new file mode 100644
index 0000000..7d10372
--- /dev/null
+++ b/tests/moves/sequence.yml
@@ -0,0 +1,47 @@
+script_1:
+  joints: [j01, j02, j03, extra]
+  defaults:
+    duration: 0.2
+
+  frames:
+    start: 
+      positions: [0, 0, 0, 0]
+      velocities: [10, 10, 10, 0]
+      loads: [100, 100, 100, 0]
+
+    frame_01: [100, 100, 100, 0]
+    frame_02: [200, 200, 200, 0]
+    frame_03: [400, 400, 400, 0]
+    frame_04: [null, null, 300, 0]
+    frame_05: [null, null, 100, 0]
+
+  sequences:
+    move_1:
+      frames: [start, frame_01, frame_02, frame_03]
+      durations: [0.2, 0.1, 0.2, 0.1]
+      times: 1
+
+    move_2:
+      frames: [frame_04, frame_05, wrong]
+      durations: [0.2, 0.15, 0.2]
+      times: 3
+
+    empty:
+      times: 1
+
+    unequal:
+      frames: [frame_01, frame_02]
+      durations: [0.1, 0.2, 0.3]
+      times: 1
+
+  scenes:
+    greet:
+      sequences: [move_1, move_2, move_1.reverse, missing]
+      times: 2
+
+    empty:
+      times: 1
+
+  script: [greet, missing_scene]
+
+script_2: For test