From ace83da0311ce3dc1ebf7cfb8fa9aaf3d4311a31 Mon Sep 17 00:00:00 2001
From: Francesco Riccio <riccio.fran@gmail.com>
Date: Tue, 28 Jan 2014 17:49:56 +0100
Subject: [PATCH] R2

---
 naoqi/modules/R2module/CMakeLists.txt         |   2 +
 naoqi/modules/R2module/include/configParams.h |  28 ++-
 naoqi/modules/R2module/include/task.h         |   9 +
 .../R2module/soth-master/src/Stage.cpp        |   2 +-
 naoqi/modules/R2module/src/r2Module.cpp       | 201 ++++++++++++++++--
 naoqi/modules/R2module/src/task.cpp           |   3 -
 naoqi/modules/R2module/src/taskManager.cpp    | 132 ++++++++++--
 7 files changed, 335 insertions(+), 42 deletions(-)

diff --git a/naoqi/modules/R2module/CMakeLists.txt b/naoqi/modules/R2module/CMakeLists.txt
index b6c4f47..5165470 100644
--- a/naoqi/modules/R2module/CMakeLists.txt
+++ b/naoqi/modules/R2module/CMakeLists.txt
@@ -19,12 +19,14 @@ set(_srcs
   ${include_path}/libmath/Rutils.h
   ${include_path}/task.h
   ${include_path}/r2Module.h
+  ${include_path}/configParams.h
 
   ${src_path}/configReader.cpp
   ${src_path}/libmath/transform.cpp
   ${src_path}/libmath/kinChain.cpp
   ${src_path}/libmath/Rutils.cpp
   ${src_path}/task.cpp
+  ${src_path}/taskManager.cpp
   ${src_path}/r2ModuleMain.cpp
 
   ${src_path}/r2Module.cpp
diff --git a/naoqi/modules/R2module/include/configParams.h b/naoqi/modules/R2module/include/configParams.h
index 390420a..25cbfd1 100644
--- a/naoqi/modules/R2module/include/configParams.h
+++ b/naoqi/modules/R2module/include/configParams.h
@@ -6,6 +6,7 @@
 //#define DEBUG_MODE
 //#define TASK_DEBUG
 //#define TASK_MANAGER_DEBUG
+#define LOG
 
 
 // Discrete integration time step
@@ -94,7 +95,7 @@ static Eigen::Matrix4d base_ankle;
 
 /** ---------------------------- Task parameters ---------------------------- */
 
-static const float ACTIVATION_STEP = 1.00;
+static const float ACTIVATION_STEP = 0.1;
 
 // Task names
 static const std::string HEAD_TASK = "Head task";
@@ -121,13 +122,13 @@ static const int LLEG_TASK_PRIORITY = 1;
 static const double K_HEAD = 1.0;
 static const double K_RARM = 1.0;
 static const double K_LARM = 1.0;
-static const double K_RLEG = 0.8;
-static const double K_LLEG = 0.8;
+static const double K_RLEG = 0.6;
+static const double K_LLEG = 0.6;
 
 // Control points number
 static const int HEAD_TASK_NSTEPS = 1;
 static const int RARM_TASK_NSTEPS = 15;
-static const int LARM_TASK_NSTEPS = 8;
+static const int LARM_TASK_NSTEPS = 30;
 static const int RLEG_TASK_NSTEPS = 30;
 static const int LLEG_TASK_NSTEPS = 30;
 
@@ -138,7 +139,7 @@ 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
@@ -147,7 +148,7 @@ static const TaskType ARMS_TASK = MIRROR_TASK;
 static const double MINIMUM_HANDS_DISTANCE = 100.0;
 
 // Circle trajectory task parameters
-//#define LARM_CIRCLE_TASK
+#define LARM_CIRCLE_TASK
 //#define RARM_CIRCLE_TASK
 static const double CIRCLE_Z_DEPTH = -70.0;         // wrt ee-frame
 static const double CIRCLE_RADIUS = 80.0;
@@ -158,9 +159,9 @@ static const int CIRCLE_LAPS = 10;
 static int UP_DOWN = 80.0;
 
 
-
 /** ------------------------------- Desired poses ------------------------------- */
 
+
 static Eigen::VectorXd desiredHeadPose(6);
 static Eigen::VectorXd desiredRHandPose(6);
 static Eigen::VectorXd desiredLHandPose(6);
@@ -230,6 +231,19 @@ static void initialization()
             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;
+
+
+    /*TOFIX*/
+    assert(HEAD_TASK_DIM <=6);
+    desiredHeadPose = desiredHeadPose.head(HEAD_TASK_DIM);
+    assert(RARM_TASK_DIM <=6);
+    desiredRHandPose = desiredRHandPose.head(RARM_TASK_DIM);
+    assert(LARM_TASK_DIM <= 6);
+    desiredLHandPose = desiredLHandPose.head(LARM_TASK_DIM);
+    assert(RLEG_TASK_DIM <= 6);
+//    desiredRLegPose = desiredRLegPose.head(RLEG_TASK_DIM);
+    assert(LLEG_TASK_DIM <= 6);
+//    desiredLLegPose = desiredLLegPose.head(LLEG_TASK_DIM);
 }
 
 #endif
diff --git a/naoqi/modules/R2module/include/task.h b/naoqi/modules/R2module/include/task.h
index cbf1cac..7e5d7b6 100644
--- a/naoqi/modules/R2module/include/task.h
+++ b/naoqi/modules/R2module/include/task.h
@@ -180,6 +180,8 @@ class Task : public TaskBase
     void activate(float activationStep = ACTIVATION_STEP);
     void stop( float decayStep = ACTIVATION_STEP);
 
+    inline float activationValue(){ return parameters.activationValue; }
+
     void set_qd(Eigen::VectorXd new_qd)
     {
         assert(new_qd.size() == parameters.qd_n.size());
@@ -220,6 +222,13 @@ class Task : public TaskBase
             return (parameters.path_currentStep == parameters.path.size()-1);
     }
 
+    // Set a desired velocity in the task space
+    inline void setTargetVelocity(const Eigen::VectorXd& r)
+    {
+        assert(r.size() == parameters.targetVelocity.size());
+        parameters.targetVelocity = r;
+        parameters.positioningActive = false;
+    }
     // Set a desired pose in the task space
     void setDesiredPose( const Eigen::VectorXd& dp, int n_controlPoints = 1 );
     void setDesiredPose( const Eigen::VectorXd& idp, const Eigen::VectorXd& dp, int n_controlPoints = 1 );
diff --git a/naoqi/modules/R2module/soth-master/src/Stage.cpp b/naoqi/modules/R2module/soth-master/src/Stage.cpp
index 84504cf..38c2464 100644
--- a/naoqi/modules/R2module/soth-master/src/Stage.cpp
+++ b/naoqi/modules/R2module/soth-master/src/Stage.cpp
@@ -548,7 +548,7 @@ namespace soth
 		G1.transpose() >> M;
 		G1.transpose() >> Ln;
 		W << G1;
-		assert( std::abs(ML_(Irn(j),sizeM))<EPSILON*EPSILON );
+//		assert( std::abs(ML_(Irn(j),sizeM))<EPSILON*EPSILON );
 	      }
 
 	    /* Commute the lines in L. */
diff --git a/naoqi/modules/R2module/src/r2Module.cpp b/naoqi/modules/R2module/src/r2Module.cpp
index 70a6537..cc4653e 100644
--- a/naoqi/modules/R2module/src/r2Module.cpp
+++ b/naoqi/modules/R2module/src/r2Module.cpp
@@ -16,7 +16,8 @@
     - Simulations
 */
 
-#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl;
+//#define INFO(x) std::cerr << "\033[22;34;1m" << "[r2module] " << x << "\033[0m" << std::endl;
+#define INFO(x) std::cout << x << std::endl;
 
 namespace AL
 {
@@ -208,6 +209,39 @@ void R2Module::init()
   taskManager.createTask(RIGHT_LEG, Rsupporting);
   taskManager.createTask(LEFT_LEG, Lsupporting);
 
+//  Task* rleg1 = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, 1, *theKinChainRightLeg, 0, soth::Bound::BOUND_INF);
+//  Task* lleg1 = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, 1, *theKinChainLeftLeg, 0, soth::Bound::BOUND_INF);
+//  Task* rleg2 = new Task(RLEG_TASK_DIM, RLEG_CHAIN_SIZE, 1, *theKinChainRightLeg, 0, soth::Bound::BOUND_SUP);
+//  Task* lleg2 = new Task(LLEG_TASK_DIM, LLEG_CHAIN_SIZE, 1, *theKinChainLeftLeg, 0, soth::Bound::BOUND_SUP);
+//  taskManager.createTask("rleg1",rleg1);
+//  taskManager.createTask("lleg1", lleg1);
+//  taskManager.createTask("rleg2",rleg2);
+//  taskManager.createTask("lleg2", lleg2);
+
+    Task* Rbound = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE+LLEG_CHAIN_SIZE,
+                             2, *theKinChain_LLBase + *CoM_RightArm + *theKinChainRightArm,
+                             LLEG_CHAIN_SIZE, soth::Bound::BOUND_INF);
+    taskManager.createTask("Rbound", Rbound);
+
+//  Task* Rpointing2 = new Task(RARM_TASK_DIM, RARM_CHAIN_SIZE+LLEG_CHAIN_SIZE,
+//                             4, *theKinChain_LLBase + *CoM_RightArm + *theKinChainRightArm,
+//                             LLEG_CHAIN_SIZE);
+//  taskManager.createTask("jesus",Rpointing2);
+
+//  Task* Lpointing1 = new Task(3, LARM_CHAIN_SIZE, 2, *theKinChainLeftArm);
+//  Task* Lpointing2 = new Task(3, LARM_CHAIN_SIZE, 3, *theKinChainLeftArm);
+//  Task* Lpointing3 = new Task(3, LARM_CHAIN_SIZE, 3, *theKinChainLeftArm);
+//  taskManager.createTask("LJesus",Lpointing1);
+//  taskManager.createTask("LMary",Lpointing2);
+//  taskManager.createTask("LHolySpirit",Lpointing3);
+
+//  Task* Rpointing1 = new Task(3, RARM_CHAIN_SIZE, 2, *theKinChainRightArm);
+//  Task* Rpointing2 = new Task(3, RARM_CHAIN_SIZE, 3, *theKinChainRightArm);
+//  Task* Rpointing3 = new Task(3, RARM_CHAIN_SIZE, 3, *theKinChainRightArm);
+//  taskManager.createTask("RJesus",Rpointing1);
+//  taskManager.createTask("RMary",Rpointing2);
+//  taskManager.createTask("RHolySpirit",Rpointing3);
+
 #ifdef DEBUG_MODE
   INFO("Initializing tasks: ");
   INFO("- Head task, with priority " << looking->getPriority());
@@ -217,6 +251,7 @@ void R2Module::init()
   INFO("- Left leg task, with priority " << Lsupporting->getPriority());
 #endif
   }
+
 }
 
 /*
@@ -296,7 +331,7 @@ void R2Module::getCurrentFrame()
   if ((char) cv::waitKey(33) != 27)
       cv::imshow("Frame", fcurrImageHeader);
 
-  // Release the proxystd::cout<<"jesus: \n"<<desiredLHandPose<<std::endl;
+  // Release the proxy
   fCamProxy->releaseImage(fVideoClientName);
 }
 
@@ -343,6 +378,25 @@ void R2Module::motion()
     taskManager.task(RIGHT_LEG).activate(RLEG_TRANSITION_STEP);
     taskManager.task(LEFT_LEG).activate(LLEG_TRANSITION_STEP);
 
+    /****/
+//    taskManager.task("rleg1").activate(1.0);
+//    taskManager.task("lleg1").activate(1.0);
+//    taskManager.task("rleg2").activate(1.0);
+//    taskManager.task("lleg2").activate(1.0);
+
+//    taskManager.task("jesus").stop(1.0);
+
+    taskManager.task("Rbound").stop(1.0);
+
+//    taskManager.task("LJesus").activate();
+//    taskManager.task("LMary").activate();
+//    taskManager.task("LHolySpirit").activate();
+//    taskManager.task("RJesus").activate();
+//    taskManager.task("RMary").activate();
+//    taskManager.task("RHolySpirit").activate();
+
+    /****/
+
     Eigen::Matrix4d h_CoMRL, h_CoMLL;
     CoM_RightLeg->forward(&h_CoMRL);
     CoM_LeftLeg->forward(&h_CoMLL);
@@ -354,29 +408,85 @@ void R2Module::motion()
     taskManager.task(LEFT_ARM).set_baseTransform(base_ankle);
     taskManager.task(RIGHT_ARM).set_baseTransform(base_ankle);
 
+    /****/
+//    taskManager.task("rleg1").set_baseTransform(h_CoMRL);
+//    taskManager.task("lleg1").set_baseTransform(h_CoMLL);
+//    taskManager.task("rleg2").set_baseTransform(h_CoMRL);
+//    taskManager.task("lleg2").set_baseTransform(h_CoMLL);
+
+    taskManager.task("Rbound").set_baseTransform(base_ankle);
+
+//    taskManager.task("jesus").set_baseTransform(base_ankle);
+
+//    taskManager.task("LJesus").set_baseTransform(base_ankle);
+//    taskManager.task("LMary").set_baseTransform(base_ankle);
+//    taskManager.task("LHolySpirit").set_baseTransform(base_ankle);
+//    taskManager.task("RJesus").set_baseTransform(base_ankle);
+//    taskManager.task("RMary").set_baseTransform(base_ankle);
+//    taskManager.task("RHolySpirit").set_baseTransform(base_ankle);
+
+//    taskManager.task("LJesus").setTargetVelocity(Eigen::Vector3d::UnitX()*50);
+//    taskManager.task("LMary").setTargetVelocity(Eigen::Vector3d::UnitZ()*50);
+//    taskManager.task("LHolySpirit").setTargetVelocity(Eigen::Vector3d::UnitY()*-50);
+//    taskManager.task("RJesus").setTargetVelocity(Eigen::Vector3d::UnitX()*50);
+//    taskManager.task("RMary").setTargetVelocity(Eigen::Vector3d::UnitZ()*-50);
+//    taskManager.task("RHolySpirit").setTargetVelocity(Eigen::Vector3d::UnitY()*-50);
+
+    // Tasks rleg1-2, lleg1-2
+//    Eigen::VectorXd desiredRLegPose2 = desiredRLegPose;
+//    desiredRLegPose2(2) += 100.0;
+//    Eigen::VectorXd desiredLLegPose2 = desiredLLegPose;
+//    desiredLLegPose2(2) += 100.0;
+//    taskManager.task("rleg1").setDesiredPose(desiredRLegPose, 1);
+//    taskManager.task("lleg1").setDesiredPose(desiredLLegPose, 1);
+//    taskManager.task("rleg2").setDesiredPose(desiredRLegPose2, 1);
+//    taskManager.task("lleg2").setDesiredPose(desiredLLegPose2, 1);
+
+    // Task jesus
+//    Eigen::VectorXd desiredRA2pose(3);
+//    desiredRA2pose << 0.0, desiredRHandPose(1), 618.0;
+//    taskManager.task("jesus").setDesiredPose(desiredRA2pose, RARM_TASK_NSTEPS);
+
+    // Task Rbound
+    Eigen::VectorXd desiredRHandPose2 = desiredRHandPose;
+    desiredRHandPose2(0) = -Eigen::Infinity;
+    desiredRHandPose2(1) = RARM_DESIRED_Y;
+    desiredRHandPose2(2) = -Eigen::Infinity;
+    taskManager.task("Rbound").setDesiredPose(desiredRHandPose2);
+
+    /****/
+
+#ifndef UP_DOWN_TASK
+    if(taskManager.task(RIGHT_LEG).taskStatus() != inactive)
+        taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose, RLEG_TASK_NSTEPS );
+
+    if(taskManager.task(LEFT_LEG).taskStatus() != inactive)
+        taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose, LLEG_TASK_NSTEPS );
+#else
     if(taskManager.task(RIGHT_LEG).taskStatus() != inactive)
-        taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose.head(RLEG_TASK_DIM), RLEG_TASK_NSTEPS );
+        taskManager.task(RIGHT_LEG).setDesiredPose( desiredRLegPose, 5 );
 
     if(taskManager.task(LEFT_LEG).taskStatus() != inactive)
-        taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose.head(LLEG_TASK_DIM), LLEG_TASK_NSTEPS );
+        taskManager.task(LEFT_LEG).setDesiredPose( desiredLLegPose, 5 );
+#endif
 
     if(taskManager.task(HEAD_TASK).taskStatus() != inactive)
-        taskManager.task(HEAD_TASK).setDesiredPose( desiredHeadPose.head(HEAD_TASK_DIM), HEAD_TASK_NSTEPS );
+        taskManager.task(HEAD_TASK).setDesiredPose( desiredHeadPose, HEAD_TASK_NSTEPS );
 
     if(taskManager.task(LEFT_ARM).taskStatus() != inactive)
 #ifndef LARM_CIRCLE_TASK
-        taskManager.task(LEFT_ARM).setDesiredPose( desiredLHandPose.head(LARM_TASK_DIM), LARM_TASK_NSTEPS );
+        taskManager.task(LEFT_ARM).setDesiredPose( desiredLHandPose, LARM_TASK_NSTEPS );
 #else
-        taskManager.task(LEFT_ARM).circularPathGenerator(desiredLHandPose.head(LARM_TASK_DIM), CIRCLE_Z_DEPTH,
+        taskManager.task(LEFT_ARM).circularPathGenerator(desiredLHandPose, CIRCLE_Z_DEPTH,
                                                         LARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS );
 #endif
 
 #ifndef RARM_LARM_JOINT_TASK
     if(taskManager.task(RIGHT_ARM).taskStatus() != inactive)
 #ifndef RARM_CIRCLE_TASK
-        taskManager.task(RIGHT_ARM).setDesiredPose(desiredRHandPose.head(RARM_TASK_DIM), RARM_TASK_NSTEPS );
+        taskManager.task(RIGHT_ARM).setDesiredPose(desiredRHandPose, RARM_TASK_NSTEPS );
 #else
-        taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose.head(RARM_TASK_DIM), CIRCLE_Z_DEPTH,
+        taskManager.task(RIGHT_ARM).circularPathGenerator(desiredRHandPose, CIRCLE_Z_DEPTH,
                                                          RARM_TASK_NSTEPS, CIRCLE_RADIUS, CIRCLE_LAPS );
 #endif
 #endif
@@ -390,28 +500,85 @@ void R2Module::motion()
 #endif
     INFO("Desired right foot position in space: [\n" << desiredRLegPose << "]");
     INFO("Desired Hip position in space: [\n" << desiredLLegPose << "]");
+
+#endif
+#ifdef UP_DOWN_TASK
+    int updown_counter = 0;
+    int mary_holyspirit_flag = 1;
 #endif
 
+    int counter = 0;
     // Main loop
     while(true)
     {
+#ifdef LOG
+        ++counter;
+        INFO("----------------------- iteration: "<<counter<<" ----------------------------");
+#endif
+        if(counter == 35)
+            taskManager.task("Rbound").activate(0.1);
+
+//            taskManager.changePriority("Rbound", 2);
+
+#ifdef DEBUG_MODE
+        if (1/*counter == 40*/)
+        {
+//            Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose();
+//            Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose();
+//            desiredRLegPose(2) += 100.0;
+//            desiredLLegPose(2) += 100.0;
+
+//            taskManager.task(LEFT_LEG).setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS);
+//            taskManager.task(RIGHT_LEG).setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS);
+//            taskManager.changePriority("Rbound", 2);
+//            INFO("Changing Rbound task!");
+        }
+#endif
 
 #ifdef UP_DOWN_TASK
         if ( (taskManager.task(RIGHT_LEG).done()) && (taskManager.task(LEFT_LEG).done()) )
         {
-            UP_DOWN *= -1;
-
-            Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose();
-            Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose();
-            desiredRLegPose(2) += UP_DOWN;
-            desiredLLegPose(2) += UP_DOWN;
-            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);
+            if (updown_counter == 1)
+            {
+                UP_DOWN *= -1;
+
+                Eigen::VectorXd desiredRLegPose = taskManager.task(RIGHT_LEG).getTargetPose();
+                Eigen::VectorXd desiredLLegPose = taskManager.task(LEFT_LEG).getTargetPose();
+                desiredRLegPose(2) += UP_DOWN;
+                desiredLLegPose(2) += UP_DOWN;
+
+                taskManager.task(RIGHT_LEG).setDesiredPose(desiredRLegPose, RLEG_TASK_NSTEPS);
+                taskManager.task(LEFT_LEG).setDesiredPose(desiredLLegPose, LLEG_TASK_NSTEPS);
+
+                if (mary_holyspirit_flag == 1)
+                {
+                    taskManager.changePriority("LJesus",3);
+                    taskManager.changePriority("LHolySpirit",2);
+                    taskManager.changePriority("RJesus",3);
+                    taskManager.changePriority("RHolySpirit",2);
+//                    taskManager.task("LMary").stop(1.0);
+                    mary_holyspirit_flag = 2;
+                }
+                else if (mary_holyspirit_flag == 2)
+                {
+                    taskManager.changePriority("LJesus",2);
+                    taskManager.changePriority("LHolySpirit",3);
+                    taskManager.changePriority("RJesus",2);
+                    taskManager.changePriority("RHolySpirit",3);
+//                    taskManager.task("LMary").stop(1.0);
+                    mary_holyspirit_flag = 1;
+                }
+
+                updown_counter = 0;
+            }
+            else
+                ++updown_counter;
         }
 #endif
         // Retrieve current robot configuration
         Eigen::VectorXd q(JOINTS_NUM);
         q = getConfiguration();
+        INFO("current configuration: \n" << q);
 
     #ifdef DEBUG_MODE
         INFO("\nCurrent joint configuration:");
diff --git a/naoqi/modules/R2module/src/task.cpp b/naoqi/modules/R2module/src/task.cpp
index 6889f44..5ac6e00 100644
--- a/naoqi/modules/R2module/src/task.cpp
+++ b/naoqi/modules/R2module/src/task.cpp
@@ -563,8 +563,6 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel,
         // 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 +
@@ -623,7 +621,6 @@ void Task::update( const Eigen::VectorXd& _q, const Eigen::VectorXd& desiredVel,
         // 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)
             {
diff --git a/naoqi/modules/R2module/src/taskManager.cpp b/naoqi/modules/R2module/src/taskManager.cpp
index ce77ae8..30ac05b 100644
--- a/naoqi/modules/R2module/src/taskManager.cpp
+++ b/naoqi/modules/R2module/src/taskManager.cpp
@@ -11,8 +11,8 @@
 
 #include "r2Module.h"
 
-#define INFO(x) std::cerr << "\033[22;34;1m" << "[TaskManager] " << x << "\033[0m" << std::endl;
-
+//#define INFO(x) std::cerr << "\033[22;34;1m" << "[TaskManager] " << x << "\033[0m" << std::endl;
+#define INFO(x) std::cout << x << std::endl;
 using namespace AL;
 
 void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen::VectorXd& q)
@@ -31,26 +31,47 @@ void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen:
 #endif
     }
 
-    else if( (taskName == LEFT_ARM) )
+    else if(taskName == LEFT_ARM)
     {
         // Left arm task update
-        Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE);
+        Eigen::VectorXd q_LA (LLEG_CHAIN_SIZE+RARM_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) )
+//    else if( (taskName == "LHolySpirit") || (taskName == "LJesus") || (taskName == "LMary") )
+//    {
+//        // Left arm task update
+//        Eigen::VectorXd q_LA (LARM_CHAIN_SIZE);
+//        q_LA << 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 == "RHolySpirit") || (taskName == "RJesus") || (taskName == "RMary") )
+//    {
+//        // Left arm task update
+//        Eigen::VectorXd q_RA (RARM_CHAIN_SIZE);
+//        q_RA << q.segment<RARM_CHAIN_SIZE>(RARM_CHAIN_BEGIN);
+//        ( taskMap[taskName] )->update( q_RA, Eigen::VectorXd::Zero(RARM_TASK_DIM), K_RARM );
+
+//#ifdef DEBUG_MODE
+//        INFO("Right arm task constraint equation: ");
+//        INFO(std::endl << *( taskMap[taskName] ) );
+//#endif
+//    }
+
+    else if( (taskName == RIGHT_ARM) || (taskName == "Rbound") )
     {
         // 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 );
@@ -83,6 +104,7 @@ void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen:
                 for (int i=3; i < LARM_TASK_DIM; ++i)
                     desiredRHandVel(i) = -desiredRHandVel(i);
 
+            ( taskMap[taskName] )->setTargetVelocity(desiredRHandVel);
             ( taskMap[taskName] )->update( q_RA, desiredRHandVel, K_RARM );
         }
 #else
@@ -94,23 +116,32 @@ void R2Module::TaskManager::taskUpdate(const std::string& taskName, const Eigen:
         INFO(std::endl << *( taskMap[taskName] ) );
 #endif
     }
-
+//    else if ( (taskName == "rleg1") || (taskName == "rleg2") )
+//        ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG );
     else if( (taskName == RIGHT_LEG) )
     {
         // Right leg task update
+#ifndef UP_DOWN_TASK
         ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), K_RLEG );
+#else
+        ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(RLEG_TASK_DIM), 0.4 );
+#endif
 
 #ifdef DEBUG_MODE
         INFO("Right leg task constraint equation: ");
         INFO(std::endl << *( taskMap[taskName] ) );
 #endif
     }
-
+//    else if ( (taskName == "lleg1") || (taskName == "lleg2") )
+//        ( taskMap[taskName] )->update( q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG );
     else if( (taskName == LEFT_LEG) )
     {
+#ifndef UP_DOWN_TASK
         // Left leg task update
         ( taskMap[taskName] )->update( q.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), K_LLEG );
-
+#else
+        ( taskMap[taskName] )->update( q.segment<RLEG_CHAIN_SIZE>(RLEG_CHAIN_BEGIN), Eigen::VectorXd::Zero(LLEG_TASK_DIM), 0.4 );
+#endif
 #ifdef DEBUG_MODE
         INFO("Left leg task constraint equation: ");
         INFO(std::endl << *( taskMap[taskName] ) );
@@ -329,7 +360,7 @@ void R2Module::TaskManager::exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot
     std::map<std::string,Task*>::const_iterator watcher = taskMap.begin();
     while(watcher != taskMap.end())
     {
-        if ((watcher->first == "LJesus") || (watcher->first == "LMary"))
+//        if ((watcher->first == "Rbound") || (watcher->first == RIGHT_ARM) )
         {
             INFO(watcher->first << ": ");
             INFO("Status = " << (watcher->second)->taskStatus());
@@ -388,4 +419,77 @@ void R2Module::TaskManager::exec(const Eigen::VectorXd& q, Eigen::VectorXd* qdot
 
     // Compute the final solution
     computeCompleteSolution(q, partialSolution, qdot);
+
+#ifdef LOG
+    Eigen::VectorXd qdot_LOG(JOINTS_NUM);
+    qdot_LOG = *qdot;
+    std::map<std::string,Task*>::const_iterator logger = taskMap.begin();
+    while(logger != taskMap.end())
+    {
+        if((logger->first == LEFT_ARM))
+        {
+            INFO("-------- task name: "<< logger->first <<" --------");
+
+            INFO("task status: " << (logger->second)->taskStatus() );
+            INFO("priority: " << (logger->second)->getPriority());
+            INFO("activation function: " << (logger->second)->activationValue());
+
+            Eigen::Vector3d ee_position_LOG;
+            Eigen::Matrix4d h_LOG;
+            ((logger->second)->kinChain())->forward(&h_LOG);
+            ee_position_LOG = h_LOG.topRightCorner(3,1);
+
+            Eigen::Vector3d velocity;
+            Eigen::Vector3d velocityError_LOG;
+            Eigen::Vector3d velocityTarget_LOG;
+            velocityTarget_LOG = (logger->second)->getTargetVelocity();
+            Eigen::MatrixXd j_LOG;
+            ((logger->second)->kinChain())->differential(&j_LOG);
+
+            Eigen::VectorXd qdot_LA (LLEG_CHAIN_SIZE+LARM_CHAIN_SIZE);
+            qdot_LA << qdot_LOG.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
+                    qdot_LOG.segment<LARM_CHAIN_SIZE>(LARM_CHAIN_BEGIN);
+            velocity = (j_LOG*qdot_LA).head(3);
+            velocityError_LOG = velocityTarget_LOG - velocity;
+
+            INFO("end effector position [" << ee_position_LOG(0) <<", "<<ee_position_LOG(1)<<", "<<ee_position_LOG(2)<<"]");
+            INFO("velocity [" << velocity(0) <<", "<<velocity(1)<<", "<<velocity(2)<<"]");
+            INFO("desired velocity [" << velocityTarget_LOG(0) <<", "<<velocityTarget_LOG(1)<<", "<<velocityTarget_LOG(2)<<"]");
+            INFO("velocity error [" << velocityError_LOG(0) <<", "<<velocityError_LOG(1) <<", "<<velocityError_LOG(2)<<"]");
+            INFO("velocity error norm: "<< velocityError_LOG.norm());
+        }
+        if( (logger->first == RIGHT_ARM) || (logger->first == "Rbound") )
+        {
+            INFO("-------- task name: "<< logger->first <<" --------");
+
+            INFO("task status: " << (logger->second)->taskStatus() );
+            INFO("priority: " << (logger->second)->getPriority());
+            INFO("activation function: " << (logger->second)->activationValue());
+
+            Eigen::Vector3d ee_position_LOG;
+            Eigen::Matrix4d h_LOG;
+            ((logger->second)->kinChain())->forward(&h_LOG);
+            ee_position_LOG = h_LOG.topRightCorner(3,1);
+            Eigen::Vector3d velocity;
+            Eigen::Vector3d velocityError_LOG;
+            Eigen::Vector3d velocityTarget_LOG;
+            velocityTarget_LOG = (logger->second)->getTargetVelocity();
+            Eigen::MatrixXd j_LOG;
+            ((logger->second)->kinChain())->differential(&j_LOG);
+            Eigen::VectorXd qdot_RA (LLEG_CHAIN_SIZE+RARM_CHAIN_SIZE);
+            qdot_RA << qdot_LOG.segment<LLEG_CHAIN_SIZE>(LLEG_CHAIN_BEGIN),
+                    qdot_LOG.segment<RARM_CHAIN_SIZE>(RARM_CHAIN_BEGIN);
+            velocity = (j_LOG*qdot_RA).head(3);
+            velocityError_LOG = velocityTarget_LOG - velocity;
+
+            INFO("end effector position [" << ee_position_LOG(0) <<", "<<ee_position_LOG(1)<<", "<<ee_position_LOG(2)<<"]");
+            INFO("velocity [" << velocity(0) <<", "<<velocity(1)<<", "<<velocity(2)<<"]");
+            INFO("desired velocity [" << velocityTarget_LOG(0) <<", "<<velocityTarget_LOG(1)<<", "<<velocityTarget_LOG(2)<<"]");
+            INFO("velocity error [" << velocityError_LOG(0) <<", "<<velocityError_LOG(1) <<", "<<velocityError_LOG(2)<<"]");
+            INFO("velocity error norm: "<< velocityError_LOG.norm());
+        }
+
+        ++logger;
+    }
+#endif
 }