refactor storages/losses/algos/priorities

pyrobolearn/algos/__init__.py

@@ -1,6 +1,6 @@
 
 # import RL algo
-# from rl_algo import *
+from .rl_algo import *
 
 # import CEM
 from .cem import CEM
@@ -21,7 +21,22 @@ from .fd import FD
 from .power import PoWER
 
 # import REINFORCE
-# from reinforce import REINFORCE
+from .reinforce import REINFORCE
+
+# import DQN
+from .dqn import DQN
+
+# import TRPO
+# from .trpo import TRPO
 
 # import PPO
-# from ppo import PPO
+from .ppo import PPO
+
+# import DDPG
+from .ddpg import DDPG
+
+# import TD3
+from .td3 import TD3
+
+# import SAC
+from .sac import SAC

pyrobolearn/algos/dqn.py

@@ -16,6 +16,7 @@ from pyrobolearn.values import ParametrizedQValueOutput
 from pyrobolearn.exploration import EpsilonGreedyActionExploration
 
 from pyrobolearn.storages import ExperienceReplay
+from pyrobolearn.samplers import BatchRandomSampler
 from pyrobolearn.returns import TDQLearningReturn
 from pyrobolearn.losses import MSBELoss, HuberLoss
 from pyrobolearn.optimizers import Adam
@@ -139,14 +140,17 @@ class DQN(GradientRLAlgo):
         # create action exploration strategy
         exploration = EpsilonGreedyActionExploration(policy=policy, action=policy.actions)
 
-        # create experience replay
+        # create experience replay and sampler
         storage = ExperienceReplay(capacity=capacity)
+        sampler = BatchRandomSampler(storage)
 
         # create target return estimator
-        estimator = TDQLearningReturn(q_value=q_value, target_qvalue=q_target, gamma=gamma)
+        # target = QLearningTarget(q_values=q_target, gamma=gamma)
+        td_return = TDQLearningReturn(q_value=q_value, target_qvalue=q_target, gamma=gamma)
 
         # create loss
-        loss = HuberLoss(MSBELoss(td_return=estimator), delta=1.)
+        # loss = HuberLoss(L2Loss(target=target, predictor=q_value))
+        loss = HuberLoss(MSBELoss(td_return=td_return), delta=1.)
 
         # create optimizer
         optimizer = Adam(learning_rate=lr)
@@ -156,8 +160,9 @@ class DQN(GradientRLAlgo):
 
         # define the 3 main steps in RL: explore, evaluate, and update
         explorer = Explorer(task, exploration, storage, num_workers=num_workers)
-        evaluator = Evaluator(estimator)
-        updater = Updater(policy, storage, loss, optimizer, updaters={target_updater: q_target})
+        evaluator = Evaluator(None)  # off-policy
+        updater = Updater(policy, sampler, loss, optimizer, evaluators=[td_return],
+                          updaters={target_updater: q_target})
 
         # initialize RL algorithm
         super(DQN, self).__init__(explorer, evaluator, updater)

pyrobolearn/algos/evaluator.py

@@ -27,6 +27,8 @@ class Evaluator(object):
     3. Update: Update the policy (and/or value function) parameters based on the loss
 
     This class focuses on the second step of RL algorithms.
+
+    Note that step is used in the on-policy case, where we evaluate complete trajectories based on estimators
     """
 
     def __init__(self, estimator):

pyrobolearn/algos/explorer.py

@@ -45,7 +45,7 @@ class Explorer(object):
         Args:
             task (Task, Env, tuple of Env and Policy): RL task or environment.
             explorer (Exploration): policies.
-            storage (RolloutStorage): Rollout storage unit (=replay memory). It will save the rollouts in the storage
+            storage (DictStorage): Rollout storage unit (=replay memory). It will save the rollouts in the storage
                 while exploring.
             num_workers (int): number of processes / workers to run in parallel.
         """
@@ -122,7 +122,7 @@ class Explorer(object):
     # Methods #
     ###########
 
-    def explore(self, num_steps, deterministic=False):
+    def explore(self, num_steps, rollout_idx=0, deterministic=False):
         """
         Explore the environment.
 
@@ -134,11 +134,11 @@ class Explorer(object):
             Rollout: memory storage
         """
         # reset environment
-        obs = self.env.reset()
-        print("\nExplorer - initial state: {}".format(obs))
+        observation = self.env.reset()
+        print("\nExplorer - initial state: {}".format(observation))
 
         # reset storage
-        self.storage.reset(init_observations=obs)
+        self.storage.reset(init_states=observation, rollout_idx=rollout_idx)
 
         # reset explorer
         self.explorer.reset()
@@ -146,28 +146,35 @@ class Explorer(object):
         # run RL task for T steps
         for step in range(num_steps):
             # get action and corresponding distribution from policy
-            act, dist = self.explorer.act(obs, deterministic=deterministic)
+            action, distribution = self.explorer.act(observation, deterministic=deterministic)
 
             # perform one step in the environment
-            next_obs, reward, done, info = self.env.step(act)
+            next_observation, reward, done, info = self.env.step(action)
 
             # insert in storage
             print("\nExplorer:")
-            print("1. Observation data: {}".format(obs))  # .merged_torch_data))
-            print("2. Action data: {}".format(act))
-            print("3. Next observation data: {}".format(next_obs))  # merged_torch_data))
+            print("1. Observation data: {}".format(observation))
+            print("2. Action data: {}".format(action))
+            print("3. Next observation data: {}".format(next_observation))
             print("4. Reward: {}".format(reward))
-            print("5. \\pi(.|s): {}".format(dist))
-            print("6. log \\pi(a|s): {}".format([d.log_prob(act) for d in dist]))
+            print("5. \\pi(.|s): {}".format(distribution))
+            print("6. log \\pi(a|s): {}".format([d.log_prob(action) for d in distribution]))
 
-            self.storage.insert(next_obs, act, reward, mask=done, distributions=dist)
+            self.storage.insert(observation, action, next_observation, reward, mask=(1-done),
+                                distributions=distribution, rollout_idx=rollout_idx)
 
-            raw_input('enter')
-
-            obs = next_obs
+            observation = next_observation
             if done:
+                self.storage.end(rollout_idx)  # fill remaining mask values
                 break
 
+        # print("states: {}".format(self.storage['states']))
+        print("actions: {}".format(self.storage['actions']))
+        print("rewards: {}".format(self.storage['rewards']))
+        # print("masks: {}".format(self.storage['masks']))
+        # print("distributions: {}".format(self.storage['distributions']))
+        raw_input('enter')
+
         # # clear explorer
         # self.explorer.clear()
 
@@ -185,6 +192,6 @@ class Explorer(object):
         """Return a string describing the class."""
         return self.__class__.__name__
 
-    def __call__(self, num_steps):
+    def __call__(self, num_steps, rollout_idx=0):
         """Explore in the environment with the specified number of time steps."""
-        self.explore(num_steps)
+        self.explore(num_steps, rollout_idx=rollout_idx)

pyrobolearn/algos/ppo.py

@@ -17,7 +17,7 @@ from pyrobolearn.exploration import ActionExploration
 from pyrobolearn.storages import RolloutStorage
 from pyrobolearn.samplers import BatchRandomSampler
 from pyrobolearn.returns import GAE
-from pyrobolearn.losses import CLIPLoss, ValueLoss, EntropyLoss
+from pyrobolearn.losses import CLIPLoss, L2Loss, EntropyLoss
 from pyrobolearn.optimizers import Adam
 
 from pyrobolearn import logger
@@ -180,19 +180,21 @@ class PPO(GradientRLAlgo):
         logger.debug('creating the action exploration strategies for each action')
         exploration = ActionExploration(policy)
 
-        # create storage and estimator
+        # create storage and sampler
         states, actions = policy.states, policy.actions
         logger.debug('create rollout storage')
         storage = RolloutStorage(num_steps=1000, state_shapes=states.merged_shape,
                                  action_shapes=actions.merged_shape, num_trajectories=num_workers)
-        logger.debug('create return estimator (GAE)')
-        estimator = GAE(storage, gamma=gamma, tau=tau)
         logger.debug('create storage sampler')
         sampler = BatchRandomSampler(storage)
 
+        # create estimator
+        logger.debug('create return estimator (GAE)')
+        estimator = GAE(storage, gamma=gamma, tau=tau)
+
         # create loss
         logger.debug('create loss')
-        loss = CLIPLoss(clip=clip) + l2_coeff * ValueLoss() + entropy_coeff * EntropyLoss()
+        loss = CLIPLoss(estimator, clip=clip) + l2_coeff * L2Loss(estimator, value) + entropy_coeff * EntropyLoss()
 
         # create optimizer
         logger.debug('create Adam optimizer')

pyrobolearn/algos/reinforce.py

@@ -196,7 +196,7 @@ class REINFORCE(GradientRLAlgo):
         estimator = ActionRewardEstimator(storage, gamma=gamma)
 
         # create loss for policy
-        loss = PGLoss()
+        loss = PGLoss(estimator)
 
         # create optimizer for policy (and possibly value function)
         optimizer = Adam(learning_rate=lr)

pyrobolearn/algos/rl_algo.py

@@ -205,7 +205,7 @@ class RLAlgo(object):  # Algo):
     @evaluator.setter
     def evaluator(self, evaluator):
         """Set the evaluator for the 2nd phase of RL algorithms."""
-        if not isinstance(evaluator, Evaluator):
+        if evaluator is not None and not isinstance(evaluator, Evaluator):
             raise TypeError("Expecting the evaluator to be an instance of `Evaluator`, instead got: "
                             "{}".format(type(evaluator)))
         self._evaluator = evaluator
@@ -266,11 +266,14 @@ class RLAlgo(object):  # Algo):
     # Methods #
     ###########
 
-    def init(self, explorer, evaluator, updater):
-        """Initialize the RL algo."""
-        self.explorer = explorer
-        self.evaluator = evaluator
-        self.updater = updater
+    def init(self, *args, **kwargs):
+        pass
+
+    # def init(self, explorer, evaluator, updater):
+    #     """Initialize the RL algo."""
+    #     self.explorer = explorer
+    #     self.evaluator = evaluator
+    #     self.updater = updater
 
     def rollout(self, deterministic=True):
         """
@@ -307,8 +310,8 @@ class RLAlgo(object):  # Algo):
         Train the policy in the provided environment.
 
         Args:
-            num_steps (int): number of step per rollout
-            num_rollouts (int): number of rollouts per episode (default: 1)
+            num_steps (int): number of step per rollout/trajectory
+            num_rollouts (int): number of rollouts/trajectories per episode (default: 1)
             num_episodes (int): number of episodes (default: 1)
             verbose(bool): if True, print details about the optimization process
             seed (int): random seed
@@ -329,8 +332,9 @@ class RLAlgo(object):  # Algo):
                 # TODO: consider to learn the dynamic model if provided
 
                 # Explore, evaluate, and update
-                self.explorer(num_steps)
-                self.evaluator()
+                self.explorer(num_steps, rollout)
+                if self.evaluator is not None:
+                    self.evaluator()
                 loss = self.updater()
 
                 # add the loss in the history
@@ -384,12 +388,8 @@ class GradientRLAlgo(RLAlgo):
         TD residual,...)
     """
 
-    # def __init__(self, task, exploration_strategy, memory, hyperparameters):
-    #    super(GradientRLAlgo, self).__init__(task, exploration_strategy, memory, hyperparameters)
-    def __init__(self, explorer, evaluator, updater, hyperparameters=None, dynamic_model=None):  # , num_workers=1):
+    def __init__(self, explorer, evaluator, updater, hyperparameters=None, dynamic_model=None):
         super(GradientRLAlgo, self).__init__(explorer, evaluator, updater, hyperparameters, dynamic_model)
-        # , num_workers)
-        # self.optimizer = hyperparameters.get('optimizer') #TODO
 
 
 class EMRLAlgo(RLAlgo):

pyrobolearn/algos/updater.py

@@ -41,7 +41,7 @@ class Updater(object):
     This class focuses on the third step of RL algorithms.
     """
 
-    def __init__(self, approximators, sampler, losses, optimizers, updaters=None, subevaluators=None, delays=None):
+    def __init__(self, approximators, sampler, losses, optimizers, evaluators=None, updaters=None, delays=None):
         """
         Initialize the update phase.
 
@@ -51,8 +51,8 @@ class Updater(object):
             losses (Loss, list/dict of losses): losses. If dict: key=approximator, value=loss.
             optimizers (Optimizer, or list/dict of optimizers): optimizer to use. If dict: key=approximator,
                 value=optimizer.
+            evaluators (list of Estimator/Return): list of sub-evaluators that are evaluated on batches.
             updaters (None, dictionary, list of tuple): list of parameter updaters to run at the end.
-            subevaluators (list of Estimator/Return): list of sub-evaluators that are evaluated on batches.
             delays (None, dictionary): dictionary containing as the key the number of time steps to wait before
                 updating the specified values (can be the updaters or losses).
         """
@@ -62,6 +62,9 @@ class Updater(object):
         self.optimizers = optimizers
         self._evaluator = ApproximatorEvaluator(approximators)
 
+        self.evaluators = evaluators
+        self.updaters = updaters
+
     ##############
     # Properties #
     ##############

pyrobolearn/controllers/controller.py

@@ -21,8 +21,34 @@ class Controller(object):
     programming.
     """
 
-    def __init__(self):
-        pass
+    def __init__(self, rate=1):
+        """
+        Initialize the controller.
+
+        Args:
+            rate (int, float): rate (float) at which the controller operates if we are operating in real-time. If we
+                are stepping deterministically in the simulator, it represents the number of ticks (int) to sleep
+                before executing the model.
+        """
+        self.rate = rate
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def rate(self):
+        """Return the rate."""
+        return self._rate
+
+    @rate.setter
+    def rate(self, rate):
+        """Set the rate."""
+        if not isinstance(rate, int):
+            raise TypeError("Expecting the given 'rate' to be an int, instead got: {}".format(type(rate)))
+        if rate <= 0:
+            raise ValueError("Expecting the rate to be positive, instead got: {}".format(rate))
+        self._rate = rate
 
     ###########
     # Methods #
@@ -35,5 +61,11 @@ class Controller(object):
     # Operators #
     #############
 
+    def __repr__(self):
+        return self.__class__.__name__
+
+    def __str__(self):
+        return self.__class__.__name__
+
     def __call__(self, *args, **kwargs):
         return self.act(*args, **kwargs)

pyrobolearn/controllers/task_controller.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python
+"""Provide the task controller.
+"""
+
+from pyrobolearn.controllers.controller import Controller
+from pyrobolearn.priorities import TaskSolver
+
+__author__ = ["Brian Delhaisse"]
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class TaskController(Controller):
+    r"""Task Controller.
+
+    The task controller accepts a priority task, and execute it.
+    """
+
+    def __init__(self, solver, rate=1):
+        """
+        Initialize the task controller.
+
+        Args:
+            solver (TaskSolver): the task solver.
+            rate (int, float): rate (float) at which the controller operates if we are operating in real-time. If we
+                are stepping deterministically in the simulator, it represents the number of ticks (int) to sleep
+                before executing the model.
+        """
+        super(TaskController, self).__init__()
+        self.solver = solver
+        self.cnt = 0
+        self.rate = rate
+        self.x = None
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def solver(self):
+        """Return the task solver."""
+        return self._solver
+
+    @solver.setter
+    def solver(self, solver):
+        """Set the task solver."""
+        if not isinstance(solver, TaskSolver):
+            raise TypeError("Expecting the given 'solver' to be an instance of `TaskSolver`, instead got: "
+                            "{}".format(type(solver)))
+        self._solver = solver
+
+    @property
+    def task(self):
+        """Return the priority task."""
+        return self.solver.task
+
+    @property
+    def robot(self):
+        """Return the robot."""
+        return self.solver.task.model
+
+    ###########
+    # Methods #
+    ###########
+
+    def compute(self, *args, **kwargs):
+        # update the task
+        self.solver.update()
+        # solve the task
+        x = self.solver.solve()
+        # return the optimal vector
+        return x
+
+    def act(self, *args, **kwargs):
+        if (self.cnt % self.rate) == 0:
+            # compute optimal variables
+            self.x = self.compute(*args, **kwargs)
+
+        # set variables using the model
+        # TODO
+
+        # update counter
+        self.cnt += 1
+
+        return self.x

pyrobolearn/losses/losses.py

@@ -43,14 +43,19 @@ class L2Loss(Loss):
         self._predictor = predictor
 
     def compute(self, batch):
+        # get target data
         if self._target in batch:
             target = batch[self._target]
         else:
             target = self._target(batch)
+
+        # get predicted data
         if self._predictor in batch:
             output = batch[self._predictor]
         else:
             output = self._predictor(batch)
+
+        # compute L2 loss
         return 0.5 * (target - output).pow(2).mean()
 
 

pyrobolearn/losses/value_losses.py

@@ -6,8 +6,8 @@ import torch
 
 from pyrobolearn.losses.loss import Loss
 from pyrobolearn.policies import Policy
-from pyrobolearn.values import QValue
-from pyrobolearn.returns import TDReturn
+from pyrobolearn.values import QValue, Value
+from pyrobolearn.returns import TDReturn, Estimator, Return
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -22,12 +22,32 @@ __status__ = "Development"
 class ValueLoss(Loss):
     r"""L2 loss for values
     """
-    def __init__(self):
+
+    def __init__(self, returns, value):
+        """
+        Initialize the L2 loss between the returns and values.
+
+        Args:
+            returns ():
+            value (Value): value function approximator.
+        """
         super(ValueLoss, self).__init__()
 
+        # check the given returns or estimators
+        if not isinstance(returns, (Estimator, Return)):
+            raise TypeError("Expecting the given 'returns' to be an instance of `Estimator` or `Return`, instead got: "
+                            "{}".format(type(returns)))
+        self._returns = returns
+
+        # check the given value approximator
+        if not isinstance(value, Value):
+            raise TypeError("Expecting the given 'value' to be an instance of `Value`, instead got: "
+                            "{}".format(type(value)))
+        self._value = value
+
     def compute(self, batch):
-        returns = batch['returns']
-        values = batch.current['values']
+        returns = batch[self._returns]
+        values = batch.current[self._value]
         return 0.5 * (returns - values).pow(2).mean()
 
 

pyrobolearn/optimizers/qpsolvers_optimizer.py

@@ -127,4 +127,21 @@ class QP(object):
         return np.allclose(X, X.T, atol=tol)
 
     def optimize(self, P, q, x0=None, G=None, h=None, A=None, b=None):
+        r"""
+        Optimize the given quadratic problem.
+
+        .. math::
+
+            \min_{x \in R^n} \frac{1}{2} x^T P x + q^T x
+
+        subject to
+
+        .. math::
+
+            Gx \leq h
+            Ax = b
+
+        Returns:
+            np.array: QP solution
+        """
         return qpsolvers.solve_qp(P, q, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj)

pyrobolearn/priorities/__init__.py

@@ -0,0 +1,12 @@
+
+# import model interface
+from .model import ModelInterface
+
+# import constraints
+from .constraints import *
+
+# import tasks
+from .tasks import *
+
+# import solvers
+from .solver import *

pyrobolearn/priorities/constraints/__init__.py

@@ -0,0 +1,9 @@
+
+# import constraint
+from .constraint import *
+
+# import kinematic constraints
+from .kinematic_constraints import *
+
+# import dynamic constraints
+from .dynamic_constraints import *

pyrobolearn/priorities/model.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python
+r"""Model interface used in priority tasks.
+
+This is based on the implementation in `https://github.com/ADVRHumanoids/ModelInterfaceRBDL`.
+
+References:
+    [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
+    [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+    [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
+"""
+
+import numpy as np
+import rbdl
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ModelInterface(object):
+    r"""Model interface.
+
+    """
+
+    def __init__(self, urdf):
+        self.model = rbdl.loadModel(filename=urdf)
+        self.model = rbdl.Model()
+        self.q = np.zeros(self.model.q_size)
+        self.dq = np.zeros(self.model.qdot_size)
+        self.ddq = np.zeros(self.model.qdot_size)
+        self.mass = 0
+        self.com = np.zeros(3)
+        self.com_vel = np.zeros(3)
+        self.com_acc = np.zeros(3)
+        self.angular_momentum_com = np.zeros(3)
+        self.change_angular_momentum_com = np.zeros(3)
+
+    @property
+    def num_dof(self):
+        return self.model.dof_count
+
+    def get_com(self):
+        return rbdl.CalcCenterOfMass(self.model, self.q, self.dq, self.ddq, self.com, self.com_vel, self.com_acc,
+                                     self.angular_momentum_com, self.change_angular_momentum_com,
+                                     update_kinematics=True)
+
+    def get_com_jacobian(self):
+        pass
+
+    def get_com_velocity(self):
+        pass
+
+    def get_com_acceleration(self):
+        pass
+
+    def get_gravity(self):
+        pass
+
+    def get_jacobian(self):
+        pass
+
+    def get_pose(self):
+        pass
+
+    def get_acceleration_twist(self):
+        pass
+
+    def get_velocity_twist(self):
+        pass
+
+    def set_floating_base_pose(self):
+        pass
+
+    def set_floating_base_twist(self):
+        pass
+
+    def set_gravity(self):
+        pass
+
+    def compute_gravity_compensation(self):
+        pass
+
+    def get_centroidal_momentum(self):
+        pass
+
+    def compute_inverse_dynamics(self):
+        pass
+
+    def compute_non_linear_term(self):
+        pass
+
+    def get_inertia_matrix(self):
+        pass
+
+    def get_link_id(self, link_name):
+        pass
+
+    def update(self, q=None, dq=None, ddq=None):
+        if q is None:
+            q = self.q
+        if dq is None:
+            dq = self.dq
+        if ddq is None:
+            ddq = self.ddq
+        rbdl.UpdateKinematics(self.model, q, dq, ddq)

pyrobolearn/priorities/solver.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+r"""Provide the various task solvers which uses QP.
+
+
+References:
+    [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
+    [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+    [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
+"""
+
+import numpy as np
+
+from pyrobolearn.priorities.tasks.task import Task
+from pyrobolearn.optimizers.qpsolvers_optimizer import QP
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class TaskSolver(object):
+    r"""Task solver.
+    """
+
+    def __init__(self, task):
+        """
+        Initialize the task solver.
+
+        Args:
+            task (Task): Priority tasks.
+        """
+        self.task = task
+        self.solver = QP(method='qpoases')
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def task(self):
+        """Return the priority task."""
+        return self._task
+
+    @task.setter
+    def task(self, task):
+        """Set the priority task."""
+        if not isinstance(task, Task):
+            raise TypeError("Expecting the given 'task' to be an instance of `Task`, instead got: "
+                            "{}".format(type(task)))
+        self._task = task
+
+    ###########
+    # Methods #
+    ###########
+
+    def update(self):
+        """Update the priority task; compute the matrices and vectors to be used later in the `solve` method."""
+        self.task.update()
+
+    def solve(self):
+        """Solve the priority task."""
+        if self.task.tasks:
+            for soft_task in self.task.tasks:
+                As = np.vstack([np.dot(np.sqrt(task.weight), task.A) for task in soft_task])
+                bs = np.vstack([np.dot(np.sqrt(task.weight), task.b) for task in soft_task])
+                # x = self.solver.optimize(P=As.T.dot(As), q=-bs.T.dot(), G=, h=, A=, b=)
+        else:
+            pass
+
+    #############
+    # Operators #
+    #############
+
+    def __call__(self):
+        return self.solve()

pyrobolearn/priorities/tasks/__init__.py

@@ -0,0 +1,9 @@
+
+# import task
+from .task import *
+
+# import kinematic tasks
+from .kinematic_tasks import *
+
+# import dynamic tasks
+from .dynamic_tasks import *

pyrobolearn/priorities/tasks/task.py

@@ -167,7 +167,8 @@ class Task(object):
     @weight.setter
     def weight(self, weight):
         if not isinstance(weight, (int, float)):
-            raise TypeError("Expecting the relative weight to be an int or float, instead got: {}".format(type(weight)))
+            raise TypeError("Expecting the relative weight to be an int or float, instead got: "
+                            "{}".format(type(weight)))
         if weight < 0:
             raise ValueError("Expecting the relative weight to be positive.")
         self._weight = weight
@@ -194,6 +195,8 @@ class Task(object):
     def _update(self):
         """Update the task.
 
+        Compute the A matrix and b vector that will be used by the task solver.
+
         Returns:
             np.array: A matrix used in QP.
             np.array: b vector used in QP.

pyrobolearn/storages/er.py

@@ -290,7 +290,7 @@ class ExperienceReplay(DictStorage):  # ExperienceReplayStorage(DictStorage):
         # space for log probabilities on policy, distributions, scalar values from value functions,
         # recurrent hidden states, and others have to be allocated outside the class
 
-    def reset(self):
+    def reset(self, *args, **kwargs):
         """Reset the experience replay storage."""
         pass
 
@@ -300,16 +300,16 @@ class ExperienceReplay(DictStorage):  # ExperienceReplayStorage(DictStorage):
         # super(ExperienceReplayStorage, self).clear()
         super(ExperienceReplay, self).clear()
 
-    def insert(self, states, actions, reward, mask, next_states, **kwargs):
+    def insert(self, states, actions, next_states, reward, mask, **kwargs):
         """
         Insert the given parameters into the storage.
 
         Args:
             states (torch.Tensor, list of torch.Tensor): (list of) state(s) / observation(s).
             actions (torch.Tensor, list of torch.Tensor): (list of) action(s).
+            next_states (torch.Tensor, list of torch.Tensor): (list of) next state(s) / observation(s).
             reward (float, int, torch.Tensor): reward value.
             mask (float, int, torch.Tensor): masks. They are set to zeros after an episode has terminated.
-            next_states (torch.Tensor, list of torch.Tensor): (list of) next state(s) / observation(s).
             **kwargs (dict): kwargs
         """
         print("ER - insert state: {}".format(states))

pyrobolearn/storages/storage.py

@@ -582,6 +582,10 @@ class DictStorage(dict, PyTorchStorage):
             default = self._to(default, device=self.device, dtype=self.dtype)
         super(DictStorage, self).setdefault(key, default)
 
+    def end(self, *args, **kwargs):
+        """End; fill the remaining value. This has to be inherited in the child classes."""
+        pass
+
     #############
     # Operators #
     #############
@@ -650,7 +654,7 @@ class RolloutStorage(DictStorage):
         print("\nStorage: state shape: {}".format(state_shapes))
         print("Storage: action shape: {}".format(action_shapes))
         super(RolloutStorage, self).__init__()
-        self._step = 0
+        self._step = np.zeros(int(num_trajectories), dtype=np.int)
         self._num_steps = int(num_steps)
         self._num_trajectories = int(num_trajectories)
         self._shifts = {}  # dictionary that maps the key to the time shift; this is add to the current time step
@@ -684,10 +688,10 @@ class RolloutStorage(DictStorage):
     # Methods #
     ###########
 
-    def step(self):
+    def step(self, rollout_idx=0):
         """Perform one step; increment by one the current step. If it reaches the end, start from 0 again."""
         # if end of storage, go at the beginning
-        self._step = (self._step + 1) % self.num_steps
+        self._step[rollout_idx] = (self._step[rollout_idx] + 1) % self.num_steps
 
     def create_new_entry(self, key, shapes, num_steps=None, dtype=torch.dtype):
         """Create a new entry (=tensor) in the rollout storage dictionary. The tensor will have the dimension
@@ -763,7 +767,7 @@ class RolloutStorage(DictStorage):
         """
         # clear itself: remove all items from the DictStorage, and reset all variables
         self.clear()
-        self._step = 0
+        self._step = np.zeros(int(num_trajectories), dtype=np.int)
         self._num_steps = int(num_steps)
         self._num_trajectories = int(num_trajectories)
 
@@ -794,27 +798,30 @@ class RolloutStorage(DictStorage):
         # space for log probabilities on policy, distributions, scalar values from value functions,
         # recurrent hidden states, and others have to be allocated outside the class
 
-    def reset(self, init_states=None):
-        """Reset the storage by copying the last value and setting it to the first value."""
-        # for key, value in self.iteritems():
-        #     if isinstance(value, list):
-        #         for idx, item in enumerate(value):
-        #             if isinstance(item, torch.Tensor) and len(item) == self.num_steps + 1:
-        #                 item[0].copy_(item[-1])
-        #     elif isinstance(value, torch.Tensor) and len(value) == self.num_steps + 1:
-        #             self[key][0].copy_(self[key][-1])
+    def reset(self, init_states=None, rollout_idx=0, *args, **kwargs):
+        """Reset the storage by copying the last value and setting it to the first value.
+
+        Args:
+            init_states (torch.Tensor, list of torch.Tensor): (list of) initial state(s) / observation(s).
+            rollout_idx (int, torch.tensor, np.array, list): trajectory/rollout index(ices). This index must be below
+                `self.num_trajectories`.
+        """
+        # reset the step
+        self._step[rollout_idx] = 0
+
+        # insert initial states
         if init_states is None:
             for state in self.states:
-                state[0].copy_(state[-1])
+                state[0][rollout_idx].copy_(state[-1][rollout_idx])
         else:
             if not isinstance(init_states, list):
                 init_states = [init_states]
             for observation, value in zip(self.states, init_states):
-                observation[0].copy_(self._convert_to_tensor(value))
-        self.masks[0].copy_(self.masks[-1])
+                observation[0][rollout_idx].copy_(self._convert_to_tensor(value))
+        self.masks[0][rollout_idx].copy_(self.masks[-1][rollout_idx])
         # self.recurrent_hidden_states[0].copy_(self.recurrent_hidden_states[-1])
 
-    def update_tensor(self, key, values, step=None, copy=True):
+    def update_tensor(self, key, values, step=None, rollout_idx=None, copy=True):
         """
         Update one particular (or several) tensor(s) in the dictionary at the specified time step. It will convert the
         given value tensors to the correct data type if not already done.
@@ -823,12 +830,14 @@ class RolloutStorage(DictStorage):
             key (object): dictionary key
             values ((list of) torch.Tensor, np.array, int, float, np.generic): items
             step (None, int): the time step.
+            rollout_idx (int, torch.tensor, np.array, list): trajectory/rollout index(ices). This index must be below
+                `self.num_trajectories`.
             copy (bool): if the item(s) should be copied. If False, it will not copy the item(s). Note that if you
                 modify these item(s) outside the storage, it will be reflected in the storage as well.
         """
         # check the given time step
         if step is None:
-            step = self._step
+            step = self._step[rollout_idx]
 
         # check if the key is inside the storage
         if key in self:
@@ -838,16 +847,16 @@ class RolloutStorage(DictStorage):
                 # if torch tensor
                 if isinstance(tensor, torch.Tensor):
                     if copy:
-                        tensor[step].copy_(self._convert_to_tensor(value))
+                        tensor[step][rollout_idx].copy_(self._convert_to_tensor(value))
                     else:
-                        tensor[step] = self._convert_to_tensor(value)
+                        tensor[step][rollout_idx] = self._convert_to_tensor(value)
 
                 # if numpy array
                 elif isinstance(tensor, np.ndarray):
                     if copy:
-                        tensor[step] = np.copy(value)
+                        tensor[step][rollout_idx] = np.copy(value)
                     else:
-                        tensor[step] = value
+                        tensor[step][rollout_idx] = value
 
             # if we have a list of tensors at the specified key
             if isinstance(self[key], list):
@@ -866,7 +875,8 @@ class RolloutStorage(DictStorage):
             else:
                 set_tensor(self[key], step, values, copy=copy)
 
-    def insert(self, states, actions, reward, mask, distributions=None, update_step=True, **kwargs):
+    def insert(self, states, actions, next_states, reward, mask, distributions=None, update_step=True, rollout_idx=0,
+               **kwargs):
         # distributions, values=None):
         # recurrent_hidden_state, action_log_prob):
         """
@@ -874,46 +884,50 @@ class RolloutStorage(DictStorage):
 
         Args:
             states (torch.Tensor, list of torch.Tensor): (list of) state(s) / observation(s).
-            actions (torch.Tensor, list of torch.Tensor): (list of) action(s)
+            actions (torch.Tensor, list of torch.Tensor): (list of) action(s).
+            next_states (torch.Tensor, list of torch.Tensor): (list of) next state(s) / observation(s).
             reward (float, int, torch.Tensor): reward value
             mask (float, int, torch.Tensor): masks. They are set to zeros after an episode has terminated.
             distributions (torch.distributions.Distribution, None): action distribution.
             update_step (bool): if True, it will update the current time step. If False, the user needs to call
                 `step()` in order to update it.
+            rollout_idx (int, torch.tensor, np.array, list): trajectory/rollout index(ices). This index must be below
+                `self.num_trajectories`.
             **kwargs (dict): dictionary containing other parameters to update in the storage. The other parameters
                 had to be added using the `create_new_entry()` method.
         """
         print("Storage - insert state: {}".format(states))
         print("Storage - insert action: {}".format(actions))
+        t = self._step[rollout_idx]
 
         # check given observations/states and actions
-        if not isinstance(states, list):
-            states = [states]
+        if not isinstance(next_states, list):
+            next_states = [next_states]
         if not isinstance(actions, list):
             actions = [actions]
         if not isinstance(distributions, list):
             distributions = [distributions]
 
         # insert each observation / action
-        for observation, storage in zip(states, self.states):
-            storage[self._step + 1].copy_(self._convert_to_tensor(observation))
-        for action, storage in zip(actions, self.actions):
-            storage[self._step].copy_(self._convert_to_tensor(action))
+        for observation, storage in zip(next_states, self['states']):
+            storage[t + 1][rollout_idx].copy_(self._convert_to_tensor(observation))
+        for action, storage in zip(actions, self['actions']):
+            storage[t][rollout_idx].copy_(self._convert_to_tensor(action))
 
         # insert rewards and masks
-        self.rewards[self._step].copy_(self._convert_to_tensor(reward))
+        self['rewards'][t][rollout_idx].copy_(self._convert_to_tensor(reward))
         if mask is None:
             mask = torch.tensor(1.)
-        self.masks[self._step + 1].copy_(self._convert_to_tensor(mask))
+        self['masks'][t + 1][rollout_idx].copy_(self._convert_to_tensor(mask))
 
         # insert distributions
-        for distribution, storage in zip(distributions, self.distributions):
-            storage[self._step] = distribution
+        for distribution, storage in zip(distributions, self['distributions']):
+            storage[t][rollout_idx] = distribution
 
         # add other elements
         for key, value in kwargs:
             if key in self and key in self._shifts:
-                self.update_tensor(key, value, step=self._step+self._shifts[key], copy=True)
+                self.update_tensor(key, value, step=self._step+self._shifts[key], rollout_idx=rollout_idx, copy=True)
             else:
                 raise ValueError("The given keys in kwargs do not exist in this storage or in its 'shift' dictionary.")
 
@@ -955,6 +969,16 @@ class RolloutStorage(DictStorage):
         # return batch (which is given to the updater (and loss))
         return Batch(batch, device=self.device, dtype=self.dtype)
 
+    def end(self, rollout_idx=0, *args, **kwargs):
+        """Once arrived at the end of an episode, it will fill the remaining mask values.
+
+        Args:
+            rollout_idx (int, torch.tensor, np.array, list): trajectory/rollout index(ices). This index must be below
+                `self.num_trajectories`.
+        """
+        t = self._step[rollout_idx]
+        self['masks'][t+1:, rollout_idx] = torch.zeros(self.num_steps - t, 1)
+
     #############
     # Operators #
     #############

pyrobolearn/values/value.py

@@ -81,6 +81,10 @@ class ValueApproximator(object):
     # Methods #
     ###########
 
+    def reset(self):
+        """Reset the value approximator."""
+        pass
+
     def evaluate(self, *args, **kwargs):
         """Predict the value."""
         pass