diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h
index 50afb37..390420a 100644
--- a/naoqi/modules/R2module/include/configParams.h
+++ b/naoqi/modules/R2module/include/configParams.h
@@ -3,7 +3,9 @@
 
 #include <Eigen/Core>
 
-#define DEBUG_MODE
+//#define DEBUG_MODE
+//#define TASK_DEBUG
+//#define TASK_MANAGER_DEBUG
 
 
 // Discrete integration time step
@@ -30,8 +32,8 @@ static const double TIME_STEP = 1.0;
 
 
 // Config (.cfg) files directory and paths
-//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/";
 static const std::string CONFIG_PATH = "/home/francesco/naoqi/modules/R2module/config/";
+//static const std::string CONFIG_PATH = "/home/claudio/Naoqi/NaoModules/r2module/config/";
 // Joint parameters
 static const std::string JOINT_BOUNDS_CFG = "joints_params.cfg";
 // Base kinematic chains
@@ -92,7 +94,14 @@ static Eigen::Matrix4d base_ankle;
 
 /** ---------------------------- Task parameters ---------------------------- */
 
+static const float ACTIVATION_STEP = 1.00;
 
+// Task names
+static const std::string HEAD_TASK = "Head task";
+static const std::string RIGHT_ARM = "Right arm task";
+static const std::string LEFT_ARM = "Left arm task";
+static const std::string RIGHT_LEG = "Right leg task";
+static const std::string LEFT_LEG = "Left leg task";
 
 // Task dimensions
 static const int HEAD_TASK_DIM = 6;
@@ -104,7 +113,7 @@ static const int LLEG_TASK_DIM = 6;
 // Task priorities
 static const int HEAD_TASK_PRIORITY = 5;
 static const int RARM_TASK_PRIORITY = 3;
-static const int LARM_TASK_PRIORITY = 2;
+static const int LARM_TASK_PRIORITY = 3;
 static const int RLEG_TASK_PRIORITY = 1;
 static const int LLEG_TASK_PRIORITY = 1;
 
@@ -117,25 +126,31 @@ static const double K_LLEG = 0.8;
 
 // Control points number
 static const int HEAD_TASK_NSTEPS = 1;
-static const int RARM_TASK_NSTEPS = 30;
-static const int LARM_TASK_NSTEPS = 30;
+static const int RARM_TASK_NSTEPS = 15;
+static const int LARM_TASK_NSTEPS = 8;
 static const int RLEG_TASK_NSTEPS = 30;
 static const int LLEG_TASK_NSTEPS = 30;
 
+// Smooth transition step
+static const double HEAD_TRANSITION_STEP = 1.0;
+static const double RARM_TRANSITION_STEP = 1.0;
+static const double LARM_TRANSITION_STEP = 1.0;
+static const double RLEG_TRANSITION_STEP = 1.0;
+static const double LLEG_TRANSITION_STEP = 1.0;
 
-#define RARM_LARM_JOINT_TASK
+//#define RARM_LARM_JOINT_TASK
 enum TaskType {
     MIRROR_TASK,
     MIMIC_TASK
 };
 static const TaskType ARMS_TASK = MIRROR_TASK;
-static const double MINIMUM_HANDS_DISTANCE = 350.0;
+static const double MINIMUM_HANDS_DISTANCE = 100.0;
 
 // Circle trajectory task parameters
 //#define LARM_CIRCLE_TASK
 //#define RARM_CIRCLE_TASK
 static const double CIRCLE_Z_DEPTH = -70.0;         // wrt ee-frame
-static const double CIRCLE_RADIUS = 70.0;
+static const double CIRCLE_RADIUS = 80.0;
 static const int CIRCLE_LAPS = 10;
 
 // Up-down with legs task
@@ -146,7 +161,11 @@ static int UP_DOWN = 80.0;
 
 /** ------------------------------- Desired poses ------------------------------- */
 
-
+static Eigen::VectorXd desiredHeadPose(6);
+static Eigen::VectorXd desiredRHandPose(6);
+static Eigen::VectorXd desiredLHandPose(6);
+static Eigen::VectorXd desiredRLegPose(6);
+static Eigen::VectorXd desiredLLegPose(6);
 
 //Head
 static const double HEAD_DESIRED_X = 0.0;   // [mm]
@@ -168,7 +187,7 @@ static const double LARM_DESIRED_Y = -HIP_PELVIS_SHIFT+TORSO_SHOULDER_Y+ELBOW_SH
 static const double LARM_DESIRED_Z = FOOT_HEIGHT+HIP_HEIGHT+HIP_TORSO+TORSO_SHOULDER_Z+WRIST_SHIFT_Z-9.9;
 static const double LARM_DESIRED_ROLL = 0.0;
 static const double LARM_DESIRED_PITCH = 0.0;
-static const double LARM_DESIRED_YAW = M_PI_2;
+static const double LARM_DESIRED_YAW = 0.0;
 // Right leg
 static const double RLEG_DESIRED_X = 20.0;
 static const double RLEG_DESIRED_Y = -HIP_PELVIS_SHIFT;
@@ -193,4 +212,24 @@ static const double LLEG_DESIRED_PITCH = 0.0;
 static const double LLEG_DESIRED_YAW = 0.0;
 
 
+static void initialization()
+{
+    // Initialize the fixed transformation base-ankle
+    base_ankle << 0.0,      0.0,     1.0,    BASE_ANKLE_X,
+                  0.0,     -1.0,     0.0,    BASE_ANKLE_Y,
+                  1.0,      0.0,     0.0,    BASE_ANKLE_Z,
+                  0.0,      0.0,     0.0,             1.0;
+
+    desiredHeadPose << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z,
+            HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW;
+    desiredRHandPose << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z,
+            RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW;
+    desiredLHandPose << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z,
+            LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW;
+    desiredRLegPose << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z,
+            RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW;
+    desiredLLegPose << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z,
+            LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW;
+}
+
 #endif
diff --git a/naoqi/modules/R2module/include/r2Module.h b/naoqi/modules/R2module/include/r2Module.h
index 1c89ae1..1b9d481 100644
--- a/naoqi/modules/R2module/include/r2Module.h
+++ b/naoqi/modules/R2module/include/r2Module.h
@@ -24,9 +24,6 @@
 #include <alproxies/alvideodeviceproxy.h>
 #include <alproxies/alredballtrackerproxy.h>
 
-// Soth includes
-#include <soth/HCOD.hpp>
-
 // Libmath includes
 #include "libmath/kinChain.h"
 #include "libmath/Rutils.h"
@@ -71,13 +68,76 @@ class R2Module : public AL::ALModule
     std::vector<std::string> jointID;
     // Container for the jointBounds
     Eigen::MatrixXd jointBounds;
-    // Joint limits primary task
-    TaskBase* jointLimits;
 
-    // All initialized tasks
-    std::map <std::string, Task*> taskMap;
-    // Stack of tasks currently active
-    std::set<Task*, taskCmp> taskSet;
+    // Task handler and scheduler
+    class TaskManager
+    {
+
+    private:
+
+        // Pointer to the robot module
+        R2Module* module_ptr;
+
+        // Joint limits primary task
+        TaskBase* jointLimits;
+        // All initialized tasks
+        std::map <std::string, Task*> taskMap;
+        // Stack of tasks currently active
+        std::set<Task*, taskCmp> taskSet;
+        // Set of tasks changing their priority value
+        std::map<std::string, int> changingTaskMap;
+
+        // Update a single task
+        void taskUpdate( const std::string& taskName, const Eigen::VectorXd& q );
+        // Update all tasks fully enabled
+        void updateActiveTasks( const Eigen::VectorXd& q );
+        // Update all tasks in a transition phase
+        void updateIntermediateTasks( const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot );
+
+        // Compute a partial HQP solution involving only fully enabled tasks
+        void computePartialSolution(const Eigen::VectorXd& q, Eigen::VectorXd* partial_qdot);
+        // Compute the final HQP involving all enabled tasks
+        void computeCompleteSolution(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot, Eigen::VectorXd* qdot);
+
+    public:
+
+        // Constructor
+        TaskManager( R2Module* m_ptr ): module_ptr(m_ptr){}
+        // Destructor
+        ~TaskManager();
+
+        TaskBase* Rbound;
+
+        // Set joint limits
+        void setJointLimits(const Eigen::MatrixXd& velBounds);
+        // Declare a new task
+        inline void createTask(const std::string& taskName, Task* taskPtr)
+        {
+            taskMap.insert( std::pair<std::string, Task*>(taskName,taskPtr) );
+        }
+        // Retrieve a reference to specific task
+        inline Task& task(const std::string& taskName)
+        {
+            assert(taskMap.find(taskName) != taskMap.end());
+            return *(taskMap[taskName]);
+        }
+        // Change task priorities
+        inline void changePriority(const std::string& taskName, int new_priority)
+        {
+            // Legal priority value check
+            assert(new_priority > 0);
+            assert(taskMap.find(taskName) != taskMap.end());
+
+            if ( (taskMap[taskName]->getPriority() != new_priority) &&
+                 (changingTaskMap.find(taskName) == changingTaskMap.end()) )
+                changingTaskMap.insert( std::pair<std::string,int>(taskName, new_priority) );
+        }
+
+        // Solve the HQP problem with the current task set
+        void exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot);
+    };
+    TaskManager taskManager;
+
 
     // Common base kinematic chain (left foot to body center)
     Rmath::KinChain* theKinChain_LLBase;
@@ -125,8 +185,6 @@ class R2Module : public AL::ALModule
 
     // Retrieve the current joint configuration
     Eigen::VectorXd getConfiguration();
-    // Update every active task with the current joint configuration
-    void updateConstraints(const Eigen::VectorXd& q);
     // Call Naoqi motion proxy to actually execute the motion
     bool updateConfiguration(const Eigen::VectorXd &delta_q);
 
diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h
index 9c92c5d..cbf1cac 100644
--- a/naoqi/modules/R2module/include/task.h
+++ b/naoqi/modules/R2module/include/task.h
@@ -2,7 +2,7 @@
 * @class: Task
 * This class (and related hierarchy) defines a generic task in the HQP framework.
 *
-* @file transform.h
+* @file task.h
 * @author Claudio Delli Bovi, Chiara Picardi, Francesco Riccio
 */
 
@@ -14,6 +14,7 @@
 
 #include "libmath/kinChain.h"
 #include "configReader.h"
+#include "configParams.h"
 
 /** ------------------- TaskBase class declaration ------------------- */
 /**
@@ -26,16 +27,16 @@
 
 class TaskBase
 {
-
 protected:
 
     // Task description
     Eigen::MatrixXd constraint_matrix;
     soth::VectorBound bounds;
+    // Bound type
+    soth::Bound::bound_t boundType;
+
     // Priority value
     int priority;
-    // Task status
-    bool active;
 
 public:
 
@@ -53,11 +54,9 @@ class TaskBase
     // Task getters
     inline const soth::VectorBound& vectorBounds() const { return bounds; }
     inline const Eigen::MatrixXd& constraintMatrix() const { return constraint_matrix; }
-    inline bool isActive() const { return active; }
-    // Status togglers
-    inline void activate() { active = true; }
-    inline void stop() { active = false; }
+    inline const soth::Bound::bound_t getType() const { return boundType; }
     // Task setters
+    void setVectorBounds(const Eigen::VectorXd& b, soth::Bound::bound_t type);
     void setVectorBounds( const Eigen::MatrixXd& b );
     void setVectorBounds( const soth::VectorBound& b );
     void setConstraintMatrix( const Eigen::MatrixXd& A );
@@ -99,44 +98,111 @@ struct taskCmp
  * the desired pose and/or desired velocity.
  */
 
+enum status{
+    active = 1,
+    inactive,
+    inactive2active,
+    active2inactive
+};
 
 class Task : public TaskBase
 {
-
 private:
 
     // The task kinematic chain
     Rmath::KinChain* theKinChain;
     int base_end;
 
-    // Toggle positioning/velocity task
-    bool positioningActive;
-    // Toggle joint space/cartesian space control
-    bool jointControlActive;
-
-    // Discrete path to desired pose
-    std::vector<Eigen::VectorXd> path;
-    // Current progress in the path
-    int path_currentStep;
-    // Current target velocity
-    Eigen::VectorXd targetVelocity;
-
+    class Parameters
+    {
+    public:
+
+        status taskStatus;
+        float activationValue;
+        float activationStep;
+
+        // Partial solution of the HQP problem without the task
+        Eigen::VectorXd qd_n;
+
+        // Toggle positioning/velocity task
+        bool positioningActive;
+        // Toggle joint space/cartesian space control
+        bool jointControlActive;
+
+        // Discrete path to desired pose
+        std::vector<Eigen::VectorXd> path;
+        // Current progress in the path
+        int path_currentStep;
+        // Current target velocity
+        Eigen::VectorXd targetVelocity;
+        // Fixed base transform for the current task
+        Eigen::MatrixXd baseT;
+
+        Parameters( status _s, float h, float ts, Eigen::VectorXd tv, Eigen::VectorXd _qd, Eigen::Matrix4d _baseT ):
+            taskStatus(_s), activationValue(h), activationStep(ts), qd_n(_qd),
+            positioningActive(false), jointControlActive(false),
+            path_currentStep(0), targetVelocity(tv), baseT(_baseT){}
+
+        inline void increaseActivationValue()
+        {
+            activationValue += activationStep;
+            if(activationValue >= 1.0)
+            {
+                activationValue = 1.0;
+                taskStatus = active;
+            }
+        }
+
+        inline void decreaseActivationValue()
+        {
+            activationValue -= activationStep;
+            if(activationValue <= 0.0)
+            {
+                activationValue = 0.0;
+                taskStatus = inactive;
+            }
+        }
+
+    };
+
+    Parameters parameters;
 
 public:
     // Constructor
-    Task(int m, int n,  int _priority, ConfigReader theConfigReader);
-    Task(int m, int n,  int _priority, const Rmath::KinChain& _kc, int _base = 0);
+    Task(int m, int n,  int _priority, ConfigReader theConfigReader, soth::Bound::bound_t boundType = soth::Bound::BOUND_TWIN );
+    Task(int m, int n,  int _priority, const Rmath::KinChain& _kc, int _base = 0, soth::Bound::bound_t boundType = soth::Bound::BOUND_TWIN );
     // Destructor
     ~Task();
 
-//    inline Rmath::KinChain* kinChain(){ return theKinChain; }
+    inline Rmath::KinChain* kinChain(){ return theKinChain; }
+
+    inline status taskStatus(){ return parameters.taskStatus; }
+    void activate(float activationStep = ACTIVATION_STEP);
+    void stop( float decayStep = ACTIVATION_STEP);
+
+    void set_qd(Eigen::VectorXd new_qd)
+    {
+        assert(new_qd.size() == parameters.qd_n.size());
+        parameters.qd_n = new_qd;
+    }
+    void set_baseTransform(const Eigen::Matrix4d& _baseT)
+    {
+        parameters.baseT = _baseT;
+    }
+
+#ifdef TASK_MANAGER_DEBUG
+    inline float activationValue() const
+    {
+        return parameters.activationValue;
+    }
+#endif
 
     // Retrieve the current end-effector pose
-    Eigen::VectorXd getCurrentPose(const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity()) const;
+    Eigen::VectorXd getCurrentPose() const;
     // Retrieve the current target velocity
     inline const Eigen::VectorXd& getTargetVelocity() const
     {
-        return targetVelocity;
+        return parameters.targetVelocity;
     }
     // Retrieve the current target pose
     const Eigen::VectorXd getTargetPose() const;
@@ -146,31 +212,31 @@ class Task : public TaskBase
         theKinChain->getJointsIDs(jointsIDs);
     }
     // Check if the task is done
-    inline bool done() const {
-        return (path_currentStep == path.size()-1);
+    inline bool done() const
+    {
+        if ((parameters.taskStatus == inactive))
+            return false;
+        else
+            return (parameters.path_currentStep == parameters.path.size()-1);
     }
 
     // Set a desired pose in the task space
-    void setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints = 1,
-                        const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity());
-    void setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1,
-                        const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity());
+    void setDesiredPose( const Eigen::VectorXd& dp, int n_controlPoints = 1 );
+    void setDesiredPose( const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1 );
     // Set a desired configuration in the joint space
     void setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints);
     // Reset the current target pose (either in joint or task space)
     inline void resetDesiredPose()
     {
-        positioningActive = false;
-        path.clear();
+        parameters.positioningActive = false;
+        parameters.path.clear();
     }
 
     // design a circular path
-    void circularPathGenerator( const Eigen::VectorXd& dp, float z_shift = 0.0, int n_controlPoints = 1, float radius = 0.0, int n = 1,
-                                const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity() );
+    void circularPathGenerator( const Eigen::VectorXd& dp, float z_shift = 0.0, int n_controlPoints = 1, float radius = 0.0, int n = 1 );
 
     // Update function
-    void update(const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel,
-                double K = 1.0, const Eigen::Matrix4d& baseTransform = Eigen::Matrix4d::Identity());
+    void update(const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K = 1.0 );
 };
 
 #endif
diff --git a/naoqi/modules/R2module/include/taskManager.h b/naoqi/modules/R2module/include/taskManager.h
new file mode 100644
index 0000000..81129cf
--- /dev/null
+++ b/naoqi/modules/R2module/include/taskManager.h
@@ -0,0 +1,75 @@
+/**
+* @class: TaskManager
+* This class defines a scheduler for tasks in the HQP framework.
+*
+* @file taskManager.h
+* @author Claudio Delli Bovi, Chiara Picardi, Francesco Riccio
+*/
+
+#ifndef TASK_MANAGER
+#define TASK_MANAGER
+
+#include <string>
+#include <map>
+#include <set>
+#include "Eigen/Core"
+
+#include "task.h"
+#include "configParams.h"
+
+
+class R2Module;
+
+class TaskManager
+{
+
+private:
+
+    // Pointer to the robot module
+    R2Module* module_ptr;
+
+    // Joint limits primary task
+    TaskBase* jointLimits;
+    // All initialized tasks
+    std::map <std::string, Task*> taskMap;
+    // Stack of tasks currently active
+    std::set<Task*, taskCmp> taskSet;
+
+    // Update a single task
+    void taskUpdate( const std::string& taskName, const Eigen::VectorXd& q );
+    // Update all tasks fully enabled
+    void updateActiveTasks( const Eigen::VectorXd& q );
+    // Update all tasks in a transition phase
+    void updateIntermediateTasks( const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot );
+
+    // Compute a partial HQP solution involving only fully enabled tasks
+    void computePartialSolution(const Eigen::VectorXd& q, Eigen::VectorXd* partial_qdot);
+    // Compute the final HQP involving all enabled tasks
+    void computeCompleteSolution(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot, Eigen::VectorXd* qdot);
+
+public:
+
+    // Constructor
+    TaskManager( R2Module* m_ptr ): module_ptr(m_ptr){}
+    // Destructor
+    ~TaskManager();
+
+    // Set joint limits
+    void setJointLimits(const Eigen::MatrixXd& velBounds);
+    // Declare a new task
+    inline void createTask(const std::string& taskName, Task* taskPtr)
+    {
+        taskMap.insert( std::pair<std::string, Task*>(taskName,taskPtr) );
+    }
+    // Retrieve a reference to specific task
+    inline Task& task(const std::string& taskName)
+    {
+        assert(taskMap.find(taskName) != taskMap.end());
+        return *(taskMap[taskName]);
+    }
+
+    // Solve the HQP problem with the current task set
+    void exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot);
+};
+
+#endif
diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp
index 739d8da..70a6537 100644
--- a/naoqi/modules/R2module/src/r2Module.cpp
+++ b/naoqi/modules/R2module/src/r2Module.cpp
@@ -12,8 +12,8 @@
 
 /**
   TODO:
-    - force soth convergence (stop criteria)
-    - task formalization
+    - rearraging priorities
+    - Simulations
 */
 
 #define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl;
@@ -25,7 +25,7 @@ namespace AL
  * Module constructor using Naoqi API methods.
  */
 R2Module::R2Module( boost::shared_ptr<ALBroker> pBroker, const std::string& pName ):
-  ALModule(pBroker , pName), fRegisteredToVideoDevice(false)
+    ALModule(pBroker , pName), fRegisteredToVideoDevice(false), taskManager(this)
 {
   // Module description
   setModuleDescription("Robotics2 Project");
@@ -67,18 +67,6 @@ R2Module::~R2Module()
     // Destroy thread
     pthread_cancel( motionThreadId );
     pthread_cancel( cameraThreadId );
-
-    // Deallocate memory
-    if (jointLimits) delete jointLimits;
-    // Cleaning up containers
-    std::map<std::string, Task*>::iterator destroyer = taskMap.begin();
-    while (destroyer != taskMap.end())
-    {
-        if(destroyer->second) delete destroyer->second;
-        ++destroyer;
-    }
-    taskMap.clear();
-    taskSet.clear();
 }
 
 /*
@@ -87,6 +75,9 @@ R2Module::~R2Module()
  */
 void R2Module::init()
 {
+    // Static initialization of parameters
+    initialization();
+
     ConfigReader theConfigReader(CONFIG_PATH + JOINT_BOUNDS_CFG);
     theConfigReader.storeJointsID(&jointID);
 
@@ -98,12 +89,6 @@ void R2Module::init()
     theConfigReader.setKinChain(CONFIG_PATH + RIGHT_LEG_BASE_CFG);
     theKinChain_RLBase = new Rmath::KinChain( theConfigReader );
 
-    // Initialize the fixed transformation base-ankle
-    base_ankle << 0.0,      0.0,     1.0,    BASE_ANKLE_X,
-                  0.0,     -1.0,     0.0,    BASE_ANKLE_Y,
-                  1.0,      0.0,     0.0,    BASE_ANKLE_Z,
-                  0.0,      0.0,     0.0,             1.0;
-
     // initialize kinChain transofrmation w.r.t. CoM
     theConfigReader.setKinChain(CONFIG_PATH + COM_HEAD_CFG);
     CoM_Head = new Rmath::KinChain( theConfigReader );
@@ -190,7 +175,7 @@ void R2Module::init()
   posBound2velBound(jointBounds, getConfiguration(), &velBounds);
 
   // Actual task initialization
-  jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds);
+  taskManager.setJointLimits(velBounds);
 
 #ifdef DEBUG_MODE
   INFO("Initializing: retrieving joint limits... ");
@@ -217,16 +202,14 @@ void R2Module::init()
                              LLEG_CHAIN_SIZE);
 
   //  // Push every task into the task map
-  taskMap.insert( std::pair<std::string, Task*> ("Head task", looking) );
-  taskMap.insert( std::pair<std::string, Task*> ("Right arm task", Rpointing) );
-  taskMap.insert( std::pair<std::string, Task*> ("Left arm task", Lpointing) );
-  taskMap.insert( std::pair<std::string, Task*> ("Right leg task", Rsupporting) );
-  taskMap.insert( std::pair<std::string, Task*> ("Left leg task", Lsupporting) );
-
+  taskManager.createTask(HEAD_TASK, looking);
+  taskManager.createTask(RIGHT_ARM, Rpointing);
+  taskManager.createTask(LEFT_ARM, Lpointing);
+  taskManager.createTask(RIGHT_LEG, Rsupporting);
+  taskManager.createTask(LEFT_LEG, Lsupporting);
 
 #ifdef DEBUG_MODE
   INFO("Initializing tasks: ");
-  INFO("- Joint limits, with priority " << jointLimits->getPriority());
   INFO("- Head task, with priority " << looking->getPriority());
   INFO("- Right arm task, with priority " << Rpointing->getPriority());
   INFO("- Left arm task, with priority " << Lpointing->getPriority());
@@ -234,7 +217,6 @@ void R2Module::init()
   INFO("- Left leg task, with priority " << Lsupporting->getPriority());
 #endif
   }
-
 }
 
 /*
@@ -314,7 +296,7 @@ void R2Module::getCurrentFrame()
   if ((char) cv::waitKey(33) != 27)
       cv::imshow("Frame", fcurrImageHeader);
 
-  // Release the proxy
+  // Release the proxystd::cout<<"jesus: \n"<<desiredLHandPose<<std::endl;
   fCamProxy->releaseImage(fVideoClientName);
 }
 
@@ -354,108 +336,48 @@ void R2Module::motion()
     INFO("Executing main loop...");
 #endif
 
-//    // Turn tasks to active
-    jointLimits->activate();
-    taskMap["Head task"]->activate();
-    taskMap["Right arm task"]->activate();
-    taskMap["Left arm task"]->activate();
-    taskMap["Right leg task"]->activate();
-    taskMap["Left leg task"]->activate();
-
-    // Defining the desired pose vectors in the task space
-    Eigen::VectorXd desiredHeadPose(HEAD_TASK_DIM),
-                    desiredLHandPose(LARM_TASK_DIM),
-                    desiredRHandPose(RARM_TASK_DIM),
-                    desiredRLegPose(RLEG_TASK_DIM),
-                    desiredLLegPose(LLEG_TASK_DIM);
-    Eigen::VectorXd desiredHeadPosition(3),
-                    desiredRHandPosition(3),
-                    desiredLHandPosition(3),
-                    desiredRLegPosition(3),
-                    desiredLLegPosition(3);
-    Eigen::VectorXd desiredHeadOrientation(3),
-                    desiredRHandOrientation(3),
-                    desiredLHandOrientation(3),
-                    desiredRLegOrientation(3),
-                    desiredLLegOrientation(3);
-
-    // head position in 3D space
-    desiredHeadPosition << HEAD_DESIRED_X, HEAD_DESIRED_Y, HEAD_DESIRED_Z;
-    // head orientation in 3D space
-    desiredHeadOrientation << HEAD_DESIRED_ROLL, HEAD_DESIRED_PITCH, HEAD_DESIRED_YAW;
-    // Right arm position in 3D space
-    desiredRHandPosition << RARM_DESIRED_X, RARM_DESIRED_Y, RARM_DESIRED_Z;
-    // Right arm orientation in 3D space
-    desiredRHandOrientation << RARM_DESIRED_ROLL, RARM_DESIRED_PITCH, RARM_DESIRED_YAW;
-    // left arm position in 3D space
-    desiredLHandPosition << LARM_DESIRED_X, LARM_DESIRED_Y, LARM_DESIRED_Z;
-    // left arm orientation in 3D space
-    desiredLHandOrientation << LARM_DESIRED_ROLL, LARM_DESIRED_PITCH, LARM_DESIRED_YAW;
-    // Right leg position in 3D space
-    desiredRLegPosition << RLEG_DESIRED_X, RLEG_DESIRED_Y, RLEG_DESIRED_Z;
-    // Right leg orientation in 3D space
-    desiredRLegOrientation << RLEG_DESIRED_ROLL, RLEG_DESIRED_PITCH, RLEG_DESIRED_YAW;
-    // Left leg position in 3D space
-    desiredLLegPosition << LLEG_DESIRED_X, LLEG_DESIRED_Y, LLEG_DESIRED_Z;
-    // Left leg orientation in 3D space
-    desiredLLegOrientation << LLEG_DESIRED_ROLL, LLEG_DESIRED_PITCH, LLEG_DESIRED_YAW;
-
-    if( HEAD_TASK_DIM > 3 )
-        desiredHeadPose << desiredHeadPosition, desiredHeadOrientation.head(HEAD_TASK_DIM-3);
-    else
-        desiredHeadPose << desiredHeadPosition.head(HEAD_TASK_DIM);
-
-    if( LARM_TASK_DIM > 3 )
-        desiredLHandPose << desiredLHandPosition, desiredLHandOrientation.head(LARM_TASK_DIM-3);
-    else
-        desiredLHandPose << desiredLHandPosition.head(LARM_TASK_DIM);
-
-    if( RARM_TASK_DIM > 3 )
-        desiredRHandPose << desiredRHandPosition, desiredRHandOrientation.head(RARM_TASK_DIM-3);
-    else
-        desiredRHandPose << desiredRHandPosition.head(RARM_TASK_DIM);
-
-    if( RLEG_TASK_DIM > 3 )
-        desiredRLegPose << desiredRLegPosition, desiredRLegOrientation.head(RLEG_TASK_DIM-3);
-    else
-        desiredRLegPose << desiredRLegPosition.head(RLEG_TASK_DIM);
+    // Turn tasks to active
+    taskManager.task(HEAD_TASK).activate(HEAD_TRANSITION_STEP);
+    taskManager.task(RIGHT_ARM).activate(RARM_TRANSITION_STEP);
+    taskManager.task(LEFT_ARM).activate(LARM_TRANSITION_STEP);
+    taskManager.task(RIGHT_LEG).activate(RLEG_TRANSITION_STEP);
+    taskManager.task(LEFT_LEG).activate(LLEG_TRANSITION_STEP);
 
-    if( LLEG_TASK_DIM > 3 )
-        desiredLLegPose << desiredLLegPosition, desiredLLegOrientation.head(LLEG_TASK_DIM-3);
-    else
-        desiredLLegPose << desiredLLegPosition.head(LLEG_TASK_DIM);
+    Eigen::Matrix4d h_CoMRL, h_CoMLL;
+    CoM_RightLeg->forward(&h_CoMRL);
+    CoM_LeftLeg->forward(&h_CoMLL);
 
-    Eigen::Vector3d iDesiredPose;
-    iDesiredPose << 0.0, 48, 590;
+    // Set fixed base transform for the tasks
+    taskManager.task(RIGHT_LEG).set_baseTransform(h_CoMRL);
+    taskManager.task(LEFT_LEG).set_baseTransform(h_CoMLL);
+    taskManager.task(HEAD_TASK).set_baseTransform(base_ankle);
+    taskManager.task(LEFT_ARM).set_baseTransform(base_ankle);
+    taskManager.task(RIGHT_ARM).set_baseTransform(base_ankle);
 
-    Eigen::Matrix4d h_CoMRL;
-    CoM_RightLeg->forward(&h_CoMRL);
-    if(taskMap["Right leg task"]->isActive())
-        taskMap["Right leg task"]->setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS, h_CoMRL );
+    if(taskManager.task(RIGHT_LEG).taskStatus() != inactive)
+        taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS );
 
-    Eigen::Matrix4d h_CoMLL;
-    CoM_LeftLeg->forward(&h_CoMLL);
-    if(taskMap["Left leg task"]->isActive())
-        taskMap["Left leg task"]->setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS, h_CoMLL );
+    if(taskManager.task(LEFT_LEG).taskStatus() != inactive)
+        taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS );
 
-    if(taskMap["Head task"]->isActive())
-        taskMap["Head task"]->setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS, base_ankle );
+    if(taskManager.task(HEAD_TASK).taskStatus() != inactive)
+        taskManager.task(HEAD_TASK).setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS );
 
-    if(taskMap["Left arm task"]->isActive())
+    if(taskManager.task(LEFT_ARM).taskStatus() != inactive)
 #ifndef LARM_CIRCLE_TASK
-        taskMap["Left arm task"]->setDesiredPose(iDesiredPose, desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS, base_ankle );
+        taskManager.task(LEFT_ARM).setDesiredPose( desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS );
 #else
-        taskMap["Left arm task"]->circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH,
-                                                        LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle );
+        taskManager.task(LEFT_ARM).circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH,
+                                                        LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS );
 #endif
 
 #ifndef RARM_LARM_JOINT_TASK
-    if(taskMap["Right arm task"]->isActive())
+    if(taskManager.task(RIGHT_ARM).taskStatus() != inactive)
 #ifndef RARM_CIRCLE_TASK
-        taskMap["Right arm task"]->setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS, base_ankle );
+        taskManager.task(RIGHT_ARM).setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS );
 #else
-        taskMap["Right arm task"]->circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH,
-                                                         RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS, base_ankle );
+        taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH,
+                                                         RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS );
 #endif
 #endif
 
@@ -470,116 +392,37 @@ void R2Module::motion()
     INFO("Desired Hip position in space: [\n" << desiredLLegPose << "]");
 #endif
 
-    updateConstraints(getConfiguration());
-
     // Main loop
     while(true)
     {
+
 #ifdef UP_DOWN_TASK
-        if ( (taskMap["Right leg task"]->done()) && (taskMap["Left leg task"]->done()) )
+        if ( (taskManager.task(RIGHT_LEG).done()) && (taskManager.task(LEFT_LEG).done()) )
         {
             UP_DOWN *= -1;
 
-            Eigen::Matrix4d h_CoMRL, h_CoMLL;
-            CoM_RightLeg->forward(&h_CoMRL);
-            CoM_LeftLeg->forward(&h_CoMLL);
-
-            Eigen::VectorXd desiredRLegPose = taskMap["Right leg task"]->getTargetPose();
-            Eigen::VectorXd desiredLLegPose = taskMap["Left leg task"]->getTargetPose();
+            Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose();
+            Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose();
             desiredRLegPose(2) += UP_DOWN;
             desiredLLegPose(2) += UP_DOWN;
-            taskMap["Right leg task"]->setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS, h_CoMRL );
-            taskMap["Left leg task"]->setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS, h_CoMLL );
+            taskManager.task(RIGHT_LEG).setDesiredPose(desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS);
+            taskManager.task(LEFT_LEG).setDesiredPose(desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS);
         }
 #endif
-
         // Retrieve current robot configuration
-        Eigen::VectorXd q(jointID.size());
+        Eigen::VectorXd q(JOINTS_NUM);
         q = getConfiguration();
 
-#ifdef DEBUG_MODE
+    #ifdef DEBUG_MODE
         INFO("\nCurrent joint configuration:");
-        for(unsigned int i=0; i<q.size(); ++i)
+        for(unsigned int i = 0; i < q.size(); ++i)
             INFO(jointID.at(i) << "\t" << q(i));
-#endif
-        // Update all tasks
-        updateConstraints(q);
-
-        // Push active tasks into the ordered set
-        taskSet.clear();
-        std::map<std::string, Task*>::const_iterator adder;
-        for ( adder = taskMap.begin(); adder != taskMap.end(); ++adder )
-            if( (adder->second)->isActive())
-                taskSet.insert(adder->second);
+    #endif
 
-        // Initialize the HQP solver
-        soth::HCOD hsolver( q.size(), taskSet.size()+1 );
-        Eigen::VectorXd qdot(q.size());
+        // Solve the HQP problem with the current task set
+        Eigen::VectorXd qdot(JOINTS_NUM);
+        taskManager.exec(q, &qdot);
 
-#ifdef DEBUG_MODE
-        INFO("Initializing HQP solver...");
-        INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) );
-        INFO("Generating stack...");
-
-        int task_i = 2;
-#endif
-        // Set up the hierarchy
-        std::vector<Eigen::MatrixXd> A;
-        std::vector<soth::VectorBound> b;
-        // Joint limits first (top priority task)
-        A.push_back(jointLimits->constraintMatrix());
-        b.push_back(jointLimits->vectorBounds());
-        // All the remaining tasks (in descending order of priority)
-        std::set<Task*>::iterator updater = taskSet.begin();
-        while( updater != taskSet.end() )
-        {
-            // Retrieve current kinchain joints information
-            std::map<std::string, int> jointsIndicesMap;
-            (*updater)->getJointsIDs(&jointsIndicesMap);
-            // Current task constraint matrix (restricted to the joint involved)
-            Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix();
-
-            // Rewrite the task constraint matrix with the complete joint configuration
-            Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM );
-            // The task constraint matrix is the only non-zero block in A
-            for (unsigned int i = 0; i < jointID.size(); ++i)
-            {
-                if (jointsIndicesMap.find(jointID.at(i)) != jointsIndicesMap.end())
-                    currentA.col(i) = currentA_task.col( jointsIndicesMap[jointID.at(i)] );
-                else
-                    currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows());
-            }
-
-            A.push_back(currentA);
-            b.push_back((*updater)->vectorBounds());
-            ++updater;
-
-#ifdef DEBUG_MODE
-            INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA);
-            ++task_i;
-#endif
-        }
-        hsolver.pushBackStages(A, b);
-
-        // Configure the solver
-        hsolver.setDamping(0.0);
-        hsolver.setInitialActiveSet();
-
-#ifdef DEBUG_MODE
-        INFO("Configuring HQP solver...");
-        INFO("Start active search... ");
-#endif
-        // Run the solver
-        hsolver.activeSearch(qdot);
-        // Trim ssolution
-        Rmath::trim(&qdot);
-
-#ifdef DEBUG_MODE
-        INFO("Done.");
-        INFO("Active set: ");
-        hsolver.showActiveSet(std::cerr);
-        INFO("Solution: [\n" << qdot << "]");
-#endif
         // Update the robot configuration with the obtained solution
         updateConfiguration(q + qdot*TIME_STEP);
     }
@@ -607,158 +450,16 @@ Eigen::VectorXd R2Module::getConfiguration()
     return currentConfiguration;
 }
 
-/*
- * Update every active task with the current joint configuration.
- */
-void R2Module::updateConstraints(const Eigen::VectorXd& q)
-{
-    /* ------------------------- Joint bounds update -------------------------*/
-
-    Eigen::MatrixXd velBounds(q.size(), 2);
-    posBound2velBound(jointBounds, q, &velBounds);
-    jointLimits->setVectorBounds(velBounds);
-
-#ifdef DEBUG_MODE
-    INFO("Updating joint bounds: ");
-    INFO(std::endl << *jointLimits);
-#endif
-
-    /* -------------------- Head and arms tasks update --------------------*/
-
-
-    // Head task update
-    Eigen::VectorXd q_H (LLEG_CHAIN_SIZE+HEAD_CHAIN_SIZE);
-    q_H << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
-            q.segment<HEAD_CHAIN_SIZE>(HEAD_CHAIN_BEGIN);
-    if(taskMap["Head task"]->isActive())
-        ( taskMap["Head task"] )->update( q_H, Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD, base_ankle );
-
-#ifdef DEBUG_MODE
-    INFO("Head task constraint equation: ");
-    INFO(std::endl << *( taskMap["Head task"] ) );
-#endif
-
-    // Left arm task update
-    Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE);
-    q_LA << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
-            q.segment<LARM_CHAIN_SIZE>(LARM_CHAIN_BEGIN);
-    if( taskMap["Left arm task"]->isActive() )
-        ( taskMap["Left arm task"] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM, base_ankle );
-
-#ifdef DEBUG_MODE
-    INFO("Left arm task constraint equation: ");
-    INFO(std::endl << *( taskMap["Left arm task"] ) );
-#endif
-
-    // Right arm task update
-    Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE);
-    q_RA << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
-            q.segment<RARM_CHAIN_SIZE>(RARM_CHAIN_BEGIN);
-#ifdef RARM_LARM_JOINT_TASK
-    if ( ARMS_TASK == MIMIC_TASK )
-    {
-        Eigen::VectorXd desiredRHandPose = ( taskMap["Left arm task"] )->getTargetPose();
-        assert(RARM_TASK_DIM == LARM_TASK_DIM);
-        assert(LARM_TASK_DIM >= 2);
-        desiredRHandPose(1) = desiredRHandPose(1)-MINIMUM_HANDS_DISTANCE;
-
-        if (LARM_TASK_DIM > 3)
-            for (int i=3; i < LARM_TASK_DIM; ++i)
-                desiredRHandPose(i) = -desiredRHandPose(i);
-
-        ( taskMap["Right arm task"] )->setDesiredPose(desiredRHandPose, 1, base_ankle );
-        ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle );
-    }
-    else if ( ARMS_TASK == MIRROR_TASK )
-    {
-        Eigen::VectorXd desiredRHandVel = ( taskMap["Left arm task"] )->getTargetVelocity();
-
-        assert(RARM_TASK_DIM == LARM_TASK_DIM);
-        assert(LARM_TASK_DIM >= 2);
-        desiredRHandVel(1) = -desiredRHandVel(1);
-
-        if (LARM_TASK_DIM > 3)
-            for (int i=3; i < LARM_TASK_DIM; ++i)
-                desiredRHandVel(i) = -desiredRHandVel(i);
-
-        ( taskMap["Right arm task"] )->update( q_RA, desiredRHandVel, K_RARM, base_ankle );
-    }
-#else
-    if( taskMap["Right arm task"]->isActive() )
-        ( taskMap["Right arm task"] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM, base_ankle );
-#endif
-
-#ifdef DEBUG_MODE
-    INFO("Right arm task constraint equation: ");
-    INFO(std::endl << *( taskMap["Right arm task"] ) );
-#endif
-
-    // Right leg task update
-    Eigen::Matrix4d h_CoMRL;
-    CoM_RightLeg->forward(&h_CoMRL);
-    if( taskMap["Right leg task"]->isActive() )
-        ( taskMap["Right leg task"] )->update( q.segment<RLEG_CHAIN_SIZE>(RLEG_CHAIN_BEGIN),
-                                               Eigen::VectorXd::Zero(RLEG_TASK_DIM),
-                                               K_RLEG, h_CoMRL);
-
-#ifdef DEBUG_MODE
-    INFO("Right leg task constraint equation: ");
-    INFO(std::endl << *( taskMap["Right leg task"] ) );
-#endif
-
-
-    // Left leg task update
-    Eigen::Matrix4d h_CoMLL;
-    CoM_LeftLeg->forward(&h_CoMLL);
-    if( taskMap["Left leg task"]->isActive() )
-        ( taskMap["Left leg task"] )->update( q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
-                                              Eigen::VectorXd::Zero(LLEG_TASK_DIM),
-                                              K_LLEG, h_CoMLL );
-
-#ifdef DEBUG_MODE
-    INFO("Left leg task constraint equation: ");
-    INFO(std::endl << *( taskMap["Left leg task"] ) );
-#endif
-
-}
-
 /*
  * Call Naoqi motion proxy to actually execute the motion.
  */
 bool R2Module::updateConfiguration(const Eigen::VectorXd& delta_q)
 {
-
-#ifdef TEST_KINCHAIN
-    Eigen::VectorXd delta_q_ = delta_q;
-#endif
-
     // Initialize variables
     AL::ALValue jointId, angles, setStiff, zeroArray;
     // Push into the vector all joint value increments to be requested
     for(int i=0; i< delta_q.size(); ++i)
     {
-
-#ifdef TEST_KINCHAIN
-        if (delta_q_(i) > 0)
-        {
-            if ( delta_q_(i) > jointBounds(i,1) )
-                delta_q_(i) = jointBounds(i,1);
-        }
-        else
-        {
-            if ( delta_q_(i) < jointBounds(i,0) )
-                delta_q_(i) = jointBounds(i,0);
-        }
-
-        if(delta_q_(i) != 0)
-        {
-            jointId.arrayPush( jointID.at(i) );
-            angles.arrayPush( delta_q_(i) );
-            setStiff.arrayPush( 1.0 );
-            zeroArray.arrayPush( 0.0 );
-        }
-        continue;
-#endif
         if(delta_q(i) != 0)
         {
             jointId.arrayPush( jointID.at(i) );
diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp
index 3faa0cd..6889f44 100644
--- a/naoqi/modules/R2module/src/task.cpp
+++ b/naoqi/modules/R2module/src/task.cpp
@@ -9,7 +9,7 @@
 #include "task.h"
 
 #define INFO(x) std::cerr << "\033[22;34;1m" << "[Task] " << x << "\033[0m" << std::endl;
-//#define DEBUG_MODE
+//#define TASK_DEBUG
 
 #define POSITION_TASK_DIM 3
 
@@ -24,7 +24,7 @@
  * the class private members using their copy constructors.
  */
 TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const soth::VectorBound& b):
-    priority(_priority), active(false)
+    boundType(b[0].getType()), priority(_priority)
 {
     // Matrices dimensions have to be consistent
     assert( A.rows() == b.rows() );
@@ -33,7 +33,7 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const soth::VectorBo
     constraint_matrix = A;
     bounds = b;
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("Task base class initalized.");
     INFO("Constraint matrix:");
     INFO(constraint_matrix);
@@ -45,7 +45,8 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const soth::VectorBo
 /*
  * Overloaded version of the constructor. Takes as input the bound vector as a general Eigen::MatrixXd.
  */
-TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixXd& b): priority(_priority)
+TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixXd& b):
+    boundType(soth::Bound::BOUND_DOUBLE), priority(_priority)
 {
     // Matrices dimensions have to be consistent
     assert( A.rows() == b.rows() );
@@ -57,7 +58,7 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixX
     for(int i=0; i<b.rows(); ++i)
         bounds[i] = soth::Bound(b(i,0), b(i,1));
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("Task base class initalized.");
     INFO("Constraint matrix:");
     INFO(constraint_matrix);
@@ -69,7 +70,8 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::MatrixX
 /*
  * Overloaded version of the constructor. Takes as input the bound vector as a Eigen::VectorXd and the bound type.
  */
-TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::VectorXd& b, soth::Bound::bound_t bt): priority(_priority)
+TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::VectorXd& b, soth::Bound::bound_t bt):
+    boundType(bt), priority(_priority)
 {
     // Matrices dimensions have to be consistent
     assert( A.rows() == b.rows() );
@@ -81,7 +83,7 @@ TaskBase::TaskBase(int _priority, const Eigen::MatrixXd& A, const Eigen::VectorX
     for(int i=0; i<b.rows(); ++i)
         bounds[i] = soth::Bound(b(i), bt);
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("Task base class initalized.");
     INFO("Constraint matrix:");
     INFO(constraint_matrix);
@@ -97,9 +99,10 @@ TaskBase::TaskBase(const TaskBase& tb)
 {
     constraint_matrix = tb.constraint_matrix;
     bounds = tb.bounds;
+    boundType = tb.boundType;
     priority = tb.priority;
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("Task base class initalized.");
     INFO("Constraint matrix:");
     INFO(constraint_matrix);
@@ -108,8 +111,29 @@ TaskBase::TaskBase(const TaskBase& tb)
 #endif
 }
 
+/*
+ * Setter that replaces the bound vector with another bound vector (given as a vector + bound type).
+ */
+void TaskBase::setVectorBounds(const Eigen::VectorXd& b, soth::Bound::bound_t type)
+{
+    // Dimensions must be consistent
+    assert( bounds.rows() == b.size() );
+
+    // Replacing the old bound vector with the new one
+    bounds.setZero(b.size(), 1);
+    boundType = type;
+    for(int i = 0; i < b.size(); ++i)
+        bounds[i] = soth::Bound(b(i), type);
+
+#ifdef TASK_DEBUG
+    INFO("New bound vector:");
+    INFO(bounds);
+#endif
+}
+
 /*
  * Setter that replaces the bound vector with another bound vector (given as a general Eigen::MatrixXd).
+ * NOTE: in this case, the new bound is assumed to be of type soth::Bound::BOUND_DOUBLE.
  */
 void TaskBase::setVectorBounds( const Eigen::MatrixXd& b )
 {
@@ -117,12 +141,13 @@ void TaskBase::setVectorBounds( const Eigen::MatrixXd& b )
     assert( bounds.rows() == b.rows() );
     assert( b.cols() == 2 );
 
-    // Replacing the old bound vector with the new one
+    // Replacing the old bounds with the new ones
     bounds.setZero(b.rows(),1);
-    for(int i=0; i<b.rows(); ++i)
+    boundType = soth::Bound::BOUND_DOUBLE;
+    for(int i = 0; i < b.rows(); ++i)
         bounds[i] = soth::Bound(b(i,0), b(i,1));
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("New bound vector:");
     INFO(bounds);
 #endif
@@ -137,8 +162,9 @@ void TaskBase::setVectorBounds( const soth::VectorBound& b )
     assert( bounds.rows() == b.rows() );
 
     bounds = b;
+    boundType = b[0].getType();
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("New bound vector:");
     INFO(bounds);
 #endif
@@ -154,7 +180,7 @@ void TaskBase::setConstraintMatrix( const Eigen::MatrixXd& A )
 
     constraint_matrix = A;
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("New constraint matrix:");
     INFO(constraint_matrix);
 #endif
@@ -232,33 +258,29 @@ TaskBase TaskBase::operator--(int)
  * - A ConfigReader class object to initialize the task kinematic chain;
  * - Two indices _base, _ee identifying the task kinematic chain in the ConfigReader object file.
  */
-Task::Task(int m, int n, int _priority, ConfigReader theConfigReader):
+Task::Task(int m, int n, int _priority, ConfigReader theConfigReader, soth::Bound::bound_t boundType):
     // Base class initalization
-    TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ),
+    TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), boundType),
     // Kinematic chain initialization
     theKinChain( new Rmath::KinChain(theConfigReader) ), base_end(0),
-    // Flag initialization
-    positioningActive(false), jointControlActive(false),
-    // Target velocit initialization
-    targetVelocity(Eigen::VectorXd::Zero(m))
+    parameters( inactive, 0, ACTIVATION_STEP, Eigen::VectorXd::Zero(m),
+        Eigen::VectorXd::Zero(n), Eigen::Matrix4d::Identity())
 {
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("Task kinematic chain:");
     INFO(theKinChain);
 #endif
 }
 
-Task::Task(int m, int n,  int _priority, const Rmath::KinChain& _kc, int _base):
+Task::Task(int m, int n,  int _priority, const Rmath::KinChain& _kc, int _base, soth::Bound::bound_t boundType):
     // Base class initalization
-    TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), soth::Bound::BOUND_TWIN ),
+    TaskBase(_priority, Eigen::MatrixXd::Identity(m, n), Eigen::VectorXd::Zero(m), boundType),
     // Kinematic chain initialization
     theKinChain( new Rmath::KinChain(_kc) ), base_end(_base),
-    // Flag initialization
-    positioningActive(false), jointControlActive(false),
-    // Target velocit initialization
-    targetVelocity(Eigen::VectorXd::Zero(m))
+    parameters( inactive, 0, ACTIVATION_STEP, Eigen::VectorXd::Zero(m),
+        Eigen::VectorXd::Zero(n), Eigen::Matrix4d::Identity())
 {
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
     INFO("Task kinematic chain:");
     INFO(theKinChain);
 #endif
@@ -272,11 +294,44 @@ Task::~Task()
     if(theKinChain) delete theKinChain;
 }
 
-Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const
+void Task::activate(float activationStep)
+{
+    if (parameters.taskStatus == inactive)
+    {
+        // Recover information about the target
+        if (parameters.positioningActive && !parameters.path.empty())
+            // Rebuild the path from scratch
+            setDesiredPose( parameters.path.at(parameters.path.size()-1), parameters.path.size());
+    }
+
+    if(parameters.taskStatus != active && parameters.taskStatus != inactive2active )
+    {
+        parameters.activationStep = activationStep;
+        if(parameters.activationValue == 1.0)
+            parameters.taskStatus = active;
+        else
+            parameters.taskStatus = inactive2active;
+    }
+}
+
+void Task::stop(float decayStep)
+{
+    if(parameters.taskStatus != inactive && parameters.taskStatus != active2inactive )
+    {
+        parameters.activationStep = decayStep;
+        if(parameters.activationValue == 0.0)
+            parameters.taskStatus = inactive;
+        else
+            parameters.taskStatus = active2inactive;
+    }
+}
+
+
+Eigen::VectorXd Task::getCurrentPose() const
 {
     Eigen::Matrix4d H_chain;
     theKinChain->forward((&H_chain));
-    H_chain = baseTransform * H_chain;
+    H_chain = parameters.baseT * H_chain;
     Eigen::VectorXd currentPose (constraint_matrix.rows());
     if(constraint_matrix.rows() > POSITION_TASK_DIM)
         // Retrieving the position from the translation vector of the forward kinematics
@@ -293,18 +348,19 @@ Eigen::VectorXd Task::getCurrentPose(const Eigen::Matrix4d& baseTransform) const
 /* TOCOMMENT */
 const Eigen::VectorXd Task::getTargetPose() const
 {
-    if(positioningActive)
-        return path.at(path_currentStep);
+    if(parameters.positioningActive)
+        return parameters.path.at(parameters.path_currentStep);
     else
         return getCurrentPose();
 }
 
-void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform)
+
+void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints)
 {
     // Re-computing direct kinematics
     Eigen::Matrix4d H_chain;
     theKinChain->forward((&H_chain));
-    H_chain = baseTransform * H_chain;
+    H_chain = parameters.baseT * H_chain;
 
     Eigen::VectorXd initialPose (constraint_matrix.rows());
     if(constraint_matrix.rows() > POSITION_TASK_DIM)
@@ -318,21 +374,21 @@ void Task::setDesiredPose(const Eigen::VectorXd& dp, int n_controlPoints, const
 
     assert(dp.size() == initialPose.size());
 
-    path.clear();
+    parameters.path.clear();
     for (float i = 1.0; i <= n_controlPoints; ++i)
-        path.push_back(initialPose + (i/static_cast<float>(n_controlPoints)) * (dp - initialPose));
+        parameters.path.push_back(initialPose + (i/static_cast<float>(n_controlPoints)) * (dp - initialPose));
 
-    path_currentStep = 0;
-    positioningActive = true;
-    if (jointControlActive) jointControlActive = false;
+    parameters.path_currentStep = 0;
+    parameters.positioningActive = true;
+    if (parameters.jointControlActive) parameters.jointControlActive = false;
 }
 
-void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints, const Eigen::Matrix4d& baseTransform)
+void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints)
 {
     // Re-computing direct kinematics
     Eigen::Matrix4d H_chain;
     theKinChain->forward((&H_chain));
-    H_chain = baseTransform * H_chain;
+    H_chain = parameters.baseT * H_chain;
 
     Eigen::VectorXd initialPose (constraint_matrix.rows());
     if(constraint_matrix.rows() > POSITION_TASK_DIM)
@@ -347,16 +403,16 @@ void Task::setDesiredPose(const Eigen::VectorXd& idp, const Eigen::VectorXd& dp,
     assert(idp.size() == initialPose.size());
     assert(dp.size() == initialPose.size());
 
-    path.clear();
+    parameters.path.clear();
     for (float i = 1.0; i <= n_controlPoints; ++i)
-        path.push_back(initialPose + (i/static_cast<float>(n_controlPoints)) * (idp - initialPose));
+        parameters.path.push_back(initialPose + (i/static_cast<float>(n_controlPoints)) * (idp - initialPose));
 
     for (float i = 1.0; i <= n_controlPoints; ++i)
-        path.push_back(idp + (i/static_cast<float>(n_controlPoints)) * (dp - idp));
+       parameters.path.push_back(idp + (i/static_cast<float>(n_controlPoints)) * (dp - idp));
 
-    path_currentStep = 0;
-    positioningActive = true;
-    if (jointControlActive) jointControlActive = false;
+    parameters.path_currentStep = 0;
+    parameters.positioningActive = true;
+    if (parameters.jointControlActive) parameters.jointControlActive = false;
 }
 
 void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_controlPoints)
@@ -368,25 +424,24 @@ void Task::setDesiredConfiguration(const Eigen::VectorXd& desiredConf, int n_con
     constraint_matrix = Eigen::MatrixXd::Identity(initialConf.size(), initialConf.size());
     bounds.setZero(initialConf.size(),1);
 
-    path.clear();
+    parameters.path.clear();
     for (float i = 1.0; i <= n_controlPoints; ++i)
-        path.push_back(initialConf + (i/static_cast<float>(n_controlPoints)) * (desiredConf - initialConf));
+        parameters.path.push_back(initialConf + (i/static_cast<float>(n_controlPoints)) * (desiredConf - initialConf));
 
-    path_currentStep = 0;
-    positioningActive = true;
-    if (!jointControlActive) jointControlActive = true;
+    parameters.path_currentStep = 0;
+    parameters.positioningActive = true;
+    if (!parameters.jointControlActive) parameters.jointControlActive = true;
 }
 
-void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n,
-                            const Eigen::Matrix4d& baseTransform )
+void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int n_controlPoints, float radius, int n )
 {
 
     // Re-computing direct kinematics
     Eigen::Matrix4d H_chain;
     theKinChain->forward((&H_chain));
-    H_chain = baseTransform * H_chain;
+    H_chain = parameters.baseT * H_chain;
 
-    path.clear();
+    parameters.path.clear();
     for (float i = 0.0; i < n*2*M_PI; i+= 2*M_PI/n_controlPoints)
     {
         Eigen::VectorXd ee_desiredPose_handframe(4);
@@ -401,19 +456,19 @@ void Task::circularPathGenerator( const Eigen::VectorXd& dp, float z_shift, int
 
         Rmath::trim(&ee_desiredPose_CoMframe);
 
-        path.push_back( ee_desiredPose_CoMframe );
+        parameters.path.push_back( ee_desiredPose_CoMframe );
     }
 
-    path_currentStep = 0;
-    positioningActive = true;
-    if (jointControlActive) jointControlActive = false;
+    parameters.path_currentStep = 0;
+    parameters.positioningActive = true;
+    if (parameters.jointControlActive) parameters.jointControlActive = false;
 }
 
 /*
  * Task update function: update the constraint matrix A=A(q) with a new value of q and replace the target vector
  * with a proportional/derivative control law of the type K*POSE_ERROR + DESIRED_VELOCITY.
  */
-void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K, const Eigen::Matrix4d& baseTransform )
+void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel, double K )
 {
     // Dimensions must be consistent
     assert(constraint_matrix.rows() == desiredVel.size());
@@ -422,50 +477,57 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel,
     Eigen::VectorXd q(_q.size());
     q << -_q.head(base_end).reverse(), _q.tail(_q.size()-base_end);
 
-    Eigen::Matrix4d H_chain;
-    theKinChain->forward((&H_chain));
+    // Updating the task kinematic chain with the new joint values
+    theKinChain->update(q);
 
-    // Cartesian space task
-    if (!jointControlActive)
-    {
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
         INFO("Updating task...");
+        INFO("Current joint configuration: \n" << q);
+        INFO("Kinematic chain: \n" << (*theKinChain));
 #endif
 
-        // Updating the task kinematic chain with the new joint values
-        theKinChain->update(q);
-
-        // Re-computing differential kinematics
-        Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols());
-        theKinChain->differential(&J_chain,constraint_matrix.rows());
+    // Cartesian space task
+    if (!parameters.jointControlActive)
+    {
 
         // Replacing the task constraint matrix with the updated version
         if (constraint_matrix.rows() > POSITION_TASK_DIM)
         {
+            // Re-computing differential kinematics
+            Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols());
+            theKinChain->differential(&J_chain);
+
             // Computing base transform
             Eigen::MatrixXd baseT = Eigen::MatrixXd::Zero(6,6);
-            baseT.topLeftCorner(3,3) = baseTransform.topLeftCorner(3,3);
-            baseT.bottomRightCorner(3,3) = baseTransform.topLeftCorner(3,3);
+            baseT.topLeftCorner(3,3) = parameters.baseT.topLeftCorner(3,3);
+            baseT.bottomRightCorner(3,3) = parameters.baseT.topLeftCorner(3,3);
             // Pre-multipling base transform
             constraint_matrix << (baseT * J_chain).topRows(constraint_matrix.rows());
         }
         else
         {
+            // Re-computing differential kinematics
+            Eigen::MatrixXd J_chain(constraint_matrix.rows(), constraint_matrix.cols());
+            theKinChain->differential(&J_chain, POSITION_TASK_DIM);
+
             // Pre-multipling base transform
-            constraint_matrix << (baseTransform.topLeftCorner(3,3) * J_chain).topRows(constraint_matrix.rows());
+            constraint_matrix << (parameters.baseT.topLeftCorner(3,3) * J_chain).topRows(constraint_matrix.rows());
         }
 
-#ifdef DEBUG_MODE
+#ifdef TASK_DEBUG
         INFO("New constraint matrix:");
-        INFO(constraint_matrix);
+        INFO(std::endl << constraint_matrix);
 #endif
+        Eigen::VectorXd transitionVelocity(constraint_matrix.rows());
+        transitionVelocity = constraint_matrix * parameters.qd_n;
 
-        if (positioningActive)
+        // Equality task with position control in the Cartesian space
+        if ( (parameters.positioningActive) && (bounds[0].getType() == soth::Bound::BOUND_TWIN) )
         {
             // Re-computing direct kinematics
             Eigen::Matrix4d H_chain;
             theKinChain->forward((&H_chain));
-            H_chain = baseTransform * H_chain;
+            H_chain = parameters.baseT * H_chain;
 
             // Computing the current pose in the task space
             Eigen::VectorXd currentPose(constraint_matrix.rows());
@@ -480,76 +542,112 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel,
                 currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows());
 
             Eigen::VectorXd pose_error;
-            if(path_currentStep < path.size())
-                 pose_error = path.at(path_currentStep) - currentPose;
+            if(parameters.path_currentStep < parameters.path.size())
+                 pose_error = parameters.path.at(parameters.path_currentStep) - currentPose;
             else
-                pose_error = path.at(path.size()-1) - currentPose;
+                pose_error = parameters.path.at(parameters.path.size()-1) - currentPose;
+
+            parameters.targetVelocity = K * pose_error + desiredVel;
 
             // Updating the bound vector with the task error + a feedforward term
             for(int i=0; i<bounds.rows(); ++i)
             {
-                bounds[i] = K * pose_error(i) + desiredVel(i);
-                targetVelocity(i) = K * pose_error(i) + desiredVel(i);
+                bounds[i] = parameters.targetVelocity(i) * parameters.activationValue +
+                        (1- parameters.activationValue)* transitionVelocity(i);
             }
 
-            if ( (pose_error.norm() < POSE_ERROR_TOLERANCE) && (path_currentStep < path.size()-1) )
-                ++path_currentStep;
+            if ( (pose_error.norm() < POSE_ERROR_TOLERANCE) && (parameters.path_currentStep < parameters.path.size()-1) )
+                ++parameters.path_currentStep;
 
-//            if ((pose_error.norm() < POSE_ERROR_TOLERANCE) && (path_currentStep == path.size()-1) )
-//                stop();
+        }
+        // Equality task with velocity control in the Cartesian space
+        else if ( bounds[0].getType() == soth::Bound::BOUND_TWIN )
+        {
+            parameters.targetVelocity = K * desiredVel;
 
+            // Updating the bound vector with the task error + a feedforward term
+            for(int i=0; i<bounds.rows(); ++i)
+                bounds[i] = parameters.targetVelocity(i) * parameters.activationValue +
+                        (1- parameters.activationValue)* transitionVelocity(i);
         }
+        // Inequality task in the Cartesian space
         else
         {
+            // Re-computing direct kinematics
+            Eigen::Matrix4d H_chain;
+            theKinChain->forward((&H_chain));
+            H_chain = parameters.baseT * H_chain;
+
+            // Computing the current pose in the task space
+            Eigen::VectorXd currentPose(constraint_matrix.rows());
+            // If the task target is a pose (position+orientation) vector, a minimal description of the orientation has to be computed
+            if(constraint_matrix.rows() > POSITION_TASK_DIM)
+                // Retrieving the position from the translation vector of the forward kinematics
+                // Retrieving the orientation from the Euler fixed frame x-y-z angles
+                currentPose << H_chain.topRightCorner(POSITION_TASK_DIM,1),
+                               Rmath::xyzEulerAngles( H_chain.topLeftCorner(3,3) ).head(constraint_matrix.rows()-POSITION_TASK_DIM);
+            // If the task target is a position vector (A row size <= 3) just the translation vector of the forward kinematics is needed
+            else
+                currentPose << H_chain.col(POSITION_TASK_DIM).head(constraint_matrix.rows());
+
+            Eigen::VectorXd new_b (bounds.rows());
+            new_b = ( parameters.path.at(parameters.path.size()-1) - currentPose ) / TIME_STEP;
             for(int i=0; i<bounds.rows(); ++i)
-            {
-                bounds[i] = K * desiredVel(i);
-                targetVelocity(i) = K * desiredVel(i);
-            }
+                bounds[i] = soth::Bound(new_b(i) * parameters.activationValue + (1- parameters.activationValue)* transitionVelocity(i),
+                                        boundType);
         }
     }
     // Joint space task
     else
     {
-#ifdef DEBUG_MODE
-        INFO("Updating task...");
-#endif
-
-        // Updating the task kinematic chain with the new joint values
-        theKinChain->update(q);
-
-        if (positioningActive)
+        // Equality task with position control in the joint space
+        if ( (parameters.positioningActive) && (bounds[0].getType() == soth::Bound::BOUND_TWIN) )
         {
             Eigen::VectorXd joint_error;
-            if(path_currentStep < path.size())
-                 joint_error = path.at(path_currentStep) - theKinChain->jointConfiguration();
+            if(parameters.path_currentStep < parameters.path.size())
+                 joint_error = parameters.path.at(parameters.path_currentStep) - theKinChain->jointConfiguration();
             else
-                joint_error = path.at(path.size()-1) - theKinChain->jointConfiguration();
+                joint_error = parameters.path.at(parameters.path.size()-1) - theKinChain->jointConfiguration();
 
+            parameters.targetVelocity = K * joint_error + desiredVel;
             // Updating the bound vector with the task error + a feedforward term
             for(int i=0; i<bounds.rows(); ++i)
             {
-                bounds[i] = K * joint_error(i) + desiredVel(i);
-                targetVelocity(i) = K * joint_error(i) + desiredVel(i);
+                bounds[i] = parameters.targetVelocity(i) * parameters.activationValue +
+                        (1- parameters.activationValue)* parameters.qd_n(i);
             }
 
-            if ( (joint_error.norm() < POSE_ERROR_TOLERANCE) && (path_currentStep < path.size()-1) )
-                ++path_currentStep;
-
-//            if ((joint_error.norm() < POSE_ERROR_TOLERANCE) && (path_currentStep == path.size()-1) )
-//                stop();
+            if ( (joint_error.norm() < POSE_ERROR_TOLERANCE) && (parameters.path_currentStep < parameters.path.size()-1) )
+                ++parameters.path_currentStep;
         }
-        else
+        // Equality task with velocity control in the joint space
+        else if ( bounds[0].getType() == soth::Bound::BOUND_TWIN )
         {
+            parameters.targetVelocity = K * desiredVel;
+            // Updating the bound vector with the task error + a feedforward term
             for(int i=0; i<bounds.rows(); ++i)
             {
-                bounds[i] = K * desiredVel(i);
-                targetVelocity(i) = K * desiredVel(i);
+                bounds[i] = parameters.targetVelocity(i) * parameters.activationValue +
+                        (1- parameters.activationValue)* parameters.qd_n(i);
             }
         }
+        // Inequality task in the joint space
+        else
+        {
+            Eigen::VectorXd new_b (bounds.rows());
+            new_b = (parameters.path.at(parameters.path.size()-1) - q) / TIME_STEP;
+            for(int i=0; i<bounds.rows(); ++i)
+                bounds[i] = soth::Bound (new_b(i) * parameters.activationValue + (1- parameters.activationValue)* parameters.qd_n(i),
+                                         boundType);
+        }
     }
 
-#ifdef DEBUG_MODE
+    if(parameters.taskStatus == inactive2active)
+        parameters.increaseActivationValue();
+    else if(parameters.taskStatus == active2inactive)
+        parameters.decreaseActivationValue();
+
+#ifdef TASK_DEBUG
     INFO("New bound vector:");
     INFO(bounds);
 #endif
diff --git a/naoqi/modules/R2module/src/taskManager.cpp b/naoqi/modules/R2module/src/taskManager.cpp
new file mode 100644
index 0000000..ce77ae8
--- /dev/null
+++ b/naoqi/modules/R2module/src/taskManager.cpp
@@ -0,0 +1,391 @@
+/**
+* @class: TaskManager
+* This class implements a scheduler for tasks in the HQP framework.
+*
+* @file taskManager.h
+* @author Claudio Delli Bovi, Chiara Picardi, Francesco Riccio
+*/
+
+#include <iostream>
+#include <soth/HCOD.hpp>
+
+#include "r2Module.h"
+
+#define INFO(x) std::cerr << "\033[22;34;1m" << "[TaskManager] " << x << "\033[0m" << std::endl;
+
+using namespace AL;
+
+void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen::VectorXd& q)
+{
+    if( taskName == HEAD_TASK)
+    {
+        // Head task update
+        Eigen::VectorXd q_H (LLEG_CHAIN_SIZE+HEAD_CHAIN_SIZE);
+        q_H << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
+               q.segment<HEAD_CHAIN_SIZE>(HEAD_CHAIN_BEGIN);
+        ( taskMap[taskName] )->update( q_H, Eigen::VectorXd::Zero(HEAD_TASK_DIM), K_HEAD );
+
+#ifdef DEBUG_MODE
+        INFO("Head task constraint equation: ");
+        INFO(std::endl << *( taskMap[taskName] ) );
+#endif
+    }
+
+    else if( (taskName == LEFT_ARM) )
+    {
+        // Left arm task update
+        Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE);
+        q_LA << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
+                q.segment<LARM_CHAIN_SIZE>(LARM_CHAIN_BEGIN);
+        ( taskMap[taskName] )->update( q_LA, Eigen::VectorXd::Zero(LARM_TASK_DIM), K_LARM );
+
+#ifdef DEBUG_MODE
+        INFO("Left arm task constraint equation: ");
+        INFO(std::endl << *( taskMap[taskName] ) );
+#endif
+    }
+
+    else if( (taskName == RIGHT_ARM) )
+    {
+        // Right arm task update
+        Eigen::VectorXd q_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE);
+        q_RA << q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
+                q.segment<RARM_CHAIN_SIZE>(RARM_CHAIN_BEGIN);
+#ifdef RARM_LARM_JOINT_TASK
+        if (taskName == "Rbound")
+            ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM );
+        // Right arm mimic task
+        else if ( ARMS_TASK == MIMIC_TASK )
+        {
+            Eigen::VectorXd desiredRHandPose = ( taskMap[LEFT_ARM] )->getTargetPose();
+            assert(RARM_TASK_DIM == LARM_TASK_DIM);
+            assert(LARM_TASK_DIM >= 2);
+            desiredRHandPose(1) -= MINIMUM_HANDS_DISTANCE;
+
+            if (LARM_TASK_DIM > 3)
+                for (int i=3; i < LARM_TASK_DIM; ++i)
+                    desiredRHandPose(i) = -desiredRHandPose(i);
+
+            ( taskMap[taskName] )->setDesiredPose(desiredRHandPose, 1 );
+            ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM );
+
+        }
+        // Right arm mirror task
+        else if ( ARMS_TASK == MIRROR_TASK )
+        {
+            Eigen::VectorXd desiredRHandVel = ( taskMap[LEFT_ARM] )->getTargetVelocity();
+
+            assert(RARM_TASK_DIM == LARM_TASK_DIM);
+            assert(LARM_TASK_DIM >= 2);
+            desiredRHandVel(1) = -desiredRHandVel(1);
+
+            if (LARM_TASK_DIM > 3)
+                for (int i=3; i < LARM_TASK_DIM; ++i)
+                    desiredRHandVel(i) = -desiredRHandVel(i);
+
+            ( taskMap[taskName] )->update( q_RA, desiredRHandVel, K_RARM );
+        }
+#else
+        ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM );
+#endif
+
+#ifdef DEBUG_MODE
+        INFO("Right arm task constraint equation: ");
+        INFO(std::endl << *( taskMap[taskName] ) );
+#endif
+    }
+
+    else if( (taskName == RIGHT_LEG) )
+    {
+        // Right leg task update
+        ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG );
+
+#ifdef DEBUG_MODE
+        INFO("Right leg task constraint equation: ");
+        INFO(std::endl << *( taskMap[taskName] ) );
+#endif
+    }
+
+    else if( (taskName == LEFT_LEG) )
+    {
+        // Left leg task update
+        ( taskMap[taskName] )->update( q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG );
+
+#ifdef DEBUG_MODE
+        INFO("Left leg task constraint equation: ");
+        INFO(std::endl << *( taskMap[taskName] ) );
+#endif
+    }
+}
+
+void R2Module::TaskManager::updateActiveTasks(const Eigen::VectorXd& q)
+{
+    std::map<std::string, Task*>::iterator updater=taskMap.begin();
+    while(updater != taskMap.end())
+    {
+        if( (updater->second)->taskStatus() == active )
+        {
+            taskUpdate(updater->first, q );
+            taskSet.insert(updater->second);
+        }
+
+        ++updater;
+    }
+}
+
+void R2Module::TaskManager::updateIntermediateTasks(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot )
+{
+    std::map<std::string, Task*>::iterator updater=taskMap.begin();
+    while(updater != taskMap.end())
+    {
+        if( (updater->second)->taskStatus() == inactive2active || (updater->second)->taskStatus() == active2inactive )
+        {
+            // Retrieve current kinchain joints information
+            std::map<std::string, int> jointsIndicesMap;
+            (updater->second)->getJointsIDs(&jointsIndicesMap);
+            Eigen::VectorXd current_qd((updater->second)->constraintMatrix().cols());
+
+            for (unsigned int i = 0; i < module_ptr->jointID.size(); ++i)
+                if (jointsIndicesMap.find(module_ptr->jointID.at(i)) != jointsIndicesMap.end())
+                    current_qd(jointsIndicesMap[module_ptr->jointID.at(i)]) = partial_qdot(i);
+
+            // qd_n <- cropped qdot
+            (updater->second)->set_qd(current_qd);
+
+            taskUpdate(updater->first, q );
+            taskSet.insert(updater->second);
+        }
+
+        ++updater;
+    }
+}
+
+void R2Module::TaskManager::computePartialSolution(const Eigen::VectorXd& q, Eigen::VectorXd* partial_qdot)
+{
+    assert(partial_qdot->size() == q.size());
+
+    updateActiveTasks(q);
+
+    // Initialize the HQP solver
+    soth::HCOD hsolver( q.size(), taskSet.size()+1 );
+
+    // Set up the hierarchy
+    std::vector<Eigen::MatrixXd> A;
+    std::vector<soth::VectorBound> b;
+    // Joint limits first (top priority task)
+    A.push_back(jointLimits->constraintMatrix());
+    b.push_back(jointLimits->vectorBounds());
+    // All the remaining tasks (in descending order of priority)
+    std::set<Task*>::iterator updater = taskSet.begin();
+    while( updater != taskSet.end() )
+    {
+        // Retrieve current kinchain joints information
+        std::map<std::string, int> jointsIndicesMap;
+        (*updater)->getJointsIDs(&jointsIndicesMap);
+        // Current task constraint matrix (restricted to the joint involved)
+        Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix();
+
+        // Rewrite the task constraint matrix with the complete joint configuration
+        Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM );
+        // The task constraint matrix is the only non-zero block in A
+        for (unsigned int i = 0; i <module_ptr->jointID.size(); ++i)
+        {
+            if (jointsIndicesMap.find(module_ptr->jointID.at(i)) != jointsIndicesMap.end())
+                currentA.col(i) = currentA_task.col( jointsIndicesMap[module_ptr->jointID.at(i)] );
+            else
+                currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows());
+        }
+
+        A.push_back(currentA);
+        b.push_back((*updater)->vectorBounds());
+        ++updater;
+    }
+
+    hsolver.pushBackStages(A, b);
+
+    // Configure the solver
+    hsolver.setDamping(0.0);
+    hsolver.setInitialActiveSet();
+
+    // Run the solver
+    hsolver.activeSearch(*partial_qdot);
+    // Trim solution
+    Rmath::trim(partial_qdot);
+}
+
+void R2Module::TaskManager::computeCompleteSolution(const Eigen::VectorXd& q, const Eigen::VectorXd& partial_qdot, Eigen::VectorXd* qdot)
+{
+    assert(partial_qdot.size() == q.size());
+    assert(qdot->size() == q.size());
+
+    updateIntermediateTasks(q, partial_qdot);
+
+    // Initialize the HQP solver
+    soth::HCOD hsolver( q.size(), taskSet.size()+1 );
+#ifdef DEBUG_MODE
+        INFO("Initializing HQP solver...");
+        INFO( "Active tasks: " << (taskSet.size()+1) << " out of " << (taskMap.size()+1) );
+        INFO("Generating stack...");
+
+        int task_i = 2;
+#endif
+    // Set up the hierarchy
+    std::vector<Eigen::MatrixXd> A;
+    std::vector<soth::VectorBound> b;
+    // Joint limits first (top priority task)
+    A.push_back(jointLimits->constraintMatrix());
+    b.push_back(jointLimits->vectorBounds());
+    // All the remaining tasks (in descending order of priority)
+    std::set<Task*>::iterator updater = taskSet.begin();
+    while( updater != taskSet.end() )
+    {
+        // Retrieve current kinchain joints information
+        std::map<std::string, int> jointsIndicesMap;
+        (*updater)->getJointsIDs(&jointsIndicesMap);
+        // Current task constraint matrix (restricted to the joint involved)
+        Eigen::MatrixXd currentA_task = (*updater)->constraintMatrix();
+
+        // Rewrite the task constraint matrix with the complete joint configuration
+        Eigen::MatrixXd currentA = Eigen::MatrixXd::Zero( currentA_task.rows(), JOINTS_NUM );
+        // The task constraint matrix is the only non-zero block in A
+        for (unsigned int i = 0; i <module_ptr->jointID.size(); ++i)
+        {
+            if (jointsIndicesMap.find(module_ptr->jointID.at(i)) != jointsIndicesMap.end())
+                currentA.col(i) = currentA_task.col( jointsIndicesMap[module_ptr->jointID.at(i)] );
+            else
+                currentA.col(i) = Eigen::VectorXd::Zero(currentA_task.rows());
+        }
+
+        A.push_back(currentA);
+        b.push_back((*updater)->vectorBounds());
+        ++updater;
+
+#ifdef DEBUG_MODE
+            INFO("Task n. " << task_i << ", constraint matrix (enlarged):\n" << currentA);
+            ++task_i;
+#endif
+    }
+
+    hsolver.pushBackStages(A, b);
+
+#ifdef DEBUG_MODE
+        INFO("Configuring HQP solver...");
+        INFO("Start active search... ");
+#endif
+
+    // Configure the solver
+    hsolver.setDamping(0.0);
+    hsolver.setInitialActiveSet();
+
+    // Run the solver
+    hsolver.activeSearch(*qdot);
+    // Trim solution
+    Rmath::trim(qdot);
+
+#ifdef DEBUG_MODE
+        INFO("Done.");
+        INFO("Active set: ");
+        hsolver.showActiveSet(std::cerr);
+        INFO("Solution: [\n" << *qdot << "]");
+#endif
+
+}
+
+
+R2Module::TaskManager::~TaskManager()
+{
+    // Deallocate memory
+    if (jointLimits) delete jointLimits;
+
+    // Cleaning up containers
+    std::map<std::string, Task*>::iterator destroyer = taskMap.begin();
+    while (destroyer != taskMap.end())
+    {
+        if(destroyer->second) delete destroyer->second;
+        ++destroyer;
+    }
+    taskMap.clear();
+    taskSet.clear();
+}
+
+void R2Module::TaskManager::setJointLimits(const Eigen::MatrixXd& velBounds)
+{
+    // Dimensions check
+    assert(velBounds.rows() == JOINTS_NUM);
+    assert(velBounds.cols() == 2);
+
+    // Replace current TaskBase*
+    if (jointLimits) delete jointLimits;
+    jointLimits = new TaskBase(0, Eigen::MatrixXd::Identity( JOINTS_NUM, JOINTS_NUM ), velBounds);
+}
+
+void R2Module::TaskManager::exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot)
+{
+    // Dimension check
+    assert(qdot->size() == JOINTS_NUM);
+
+#ifdef TASK_MANAGER_DEBUG
+    INFO("-----------------------");
+    std::map<std::string,Task*>::const_iterator watcher = taskMap.begin();
+    while(watcher != taskMap.end())
+    {
+        if ((watcher->first == "LJesus") || (watcher->first == "LMary"))
+        {
+            INFO(watcher->first << ": ");
+            INFO("Status = " << (watcher->second)->taskStatus());
+            INFO("Priority = " << (watcher->second)->getPriority());
+            INFO("Activation value = " << (watcher->second)->activationValue());
+            if ((watcher->second)->done())
+            {
+                INFO("Task done.");
+            }
+            else
+            {
+                INFO("Task in progress...");
+            }
+        }
+        ++watcher;
+    }
+#endif
+
+    // Handle tasks waiting for a priority change
+    std::map<std::string,int>::iterator priorityHandler = changingTaskMap.begin();
+    while(priorityHandler != changingTaskMap.end())
+    {
+        Task* currentTask = taskMap[priorityHandler->first];
+
+        // The task finally stopped and has to be re-activated with the new priority value
+        if ( (currentTask->getPriority() != priorityHandler->second) && (currentTask->taskStatus() == inactive) )
+        {
+            // Set the task priority to the new value
+            currentTask->setPriority( priorityHandler->second );
+            // Insert the task back
+            currentTask->activate();
+            // Remove the task from the queue
+            changingTaskMap.erase(priorityHandler);
+        }
+        // The task is still active with the old priority
+        else if ( (currentTask->getPriority() != priorityHandler->second) && (currentTask->taskStatus() != active2inactive) )
+            currentTask->stop();
+
+        ++priorityHandler;
+    }
+
+    // Update joint limits task
+    Eigen::MatrixXd velBounds(q.size(), 2);
+    module_ptr->posBound2velBound(module_ptr->jointBounds, q, &velBounds);
+    jointLimits->setVectorBounds(velBounds);
+
+#ifdef DEBUG_MODE
+    INFO("Updating joint bounds: ");
+    INFO(std::endl << *jointLimits);
+#endif
+
+    taskSet.clear();
+    // Compute the partial solution first
+    Eigen::VectorXd partialSolution(q.size());
+    computePartialSolution(q, &partialSolution);
+
+    // Compute the final solution
+    computeCompleteSolution(q, partialSolution, qdot);
+}