diff --git a/CMakeLists.txt b/CMakeLists.txt
index affbaafcb3d38c27891747c6d542b1f53e7793af..739e6371c8ee997882c66e624cf16b6a54cbe2d0 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,9 +2,9 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
 
 if(COMMAND cmake_policy)
-  cmake_policy(SET CMP0005 NEW) 
+  cmake_policy(SET CMP0005 NEW)
   cmake_policy(SET CMP0003 NEW)
-   
+
 endif(COMMAND cmake_policy)
 
 # The project name and the type of project
@@ -15,12 +15,26 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
 
 IF (NOT CMAKE_BUILD_TYPE)
- SET(CMAKE_BUILD_TYPE "RELEASE") 
+ SET(CMAKE_BUILD_TYPE "RELEASE")
 ENDIF (NOT CMAKE_BUILD_TYPE)
 
 SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
 SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
 
+#Set compiler according C++11 support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
 ADD_SUBDIRECTORY(src)
 
 FIND_PACKAGE(Doxygen)
@@ -85,4 +99,3 @@ ELSE(UNIX)
     TARGET  uninstall
   )
 ENDIF(UNIX)
-
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b8cea50c4bb605e34d636e511097aa98bebd6f60..12d6f077b4b357c851a55fa0af4e7292189476e3 100755
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,10 +1,12 @@
 # driver source files
 SET(sources common_fc.cpp kinematics.cpp uam_task_ctrl.cpp)
-SET(sources_tasks tasks/ir.cpp tasks/vs.cpp tasks/cog.cpp tasks/jl.cpp tasks/eepos.cpp tasks/heading.cpp tasks/basepos.cpp)
+SET(sources_tasks tasks/ir.cpp tasks/vs.cpp tasks/cog.cpp tasks/jl.cpp tasks/eepos.cpp
+tasks/heading.cpp tasks/basepos.cpp tasks/jl_ineq.cpp)
 
 # application header files
 SET(headers common_obj.h common_fc.h kinematics.h uam_task_ctrl.h)
-SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h tasks/heading.h tasks/basepos.h)
+SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h
+tasks/heading.h tasks/basepos.h tasks/jl_ineq.h)
 
 # locate the necessary dependencies
 
@@ -12,23 +14,21 @@ SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h tas
 INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
 
 # Boost #######
-set(Boost_USE_STATIC_LIBS ON) 
-set(Boost_USE_MULTITHREADED OFF)  
-set(Boost_USE_STATIC_RUNTIME OFF) 
+set(Boost_USE_STATIC_LIBS ON)
+set(Boost_USE_MULTITHREADED OFF)
+set(Boost_USE_STATIC_RUNTIME OFF)
 FIND_PACKAGE(Boost REQUIRED)
 
 # Eigen #######
 FIND_PACKAGE(Eigen3 REQUIRED)
 
-SET(CMAKE_BUILD_TYPE release)
-
 # add the necessary include directories
 INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS})
 
 # create the shared library
 ADD_LIBRARY(uam_task_ctrl SHARED ${sources} ${sources_tasks})
 
-TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${orocos_kdl_LIBRARIES}) 
+TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
 # link necessary libraries
 INSTALL(TARGETS uam_task_ctrl
diff --git a/src/tasks/eepos.cpp b/src/tasks/eepos.cpp
index 7c0c4a11e976c1ecda3b2e44f54d4f5b953c88f0..37aab95d89c3e2fb2bfd7ab727ad8094de817dd6 100755
--- a/src/tasks/eepos.cpp
+++ b/src/tasks/eepos.cpp
@@ -23,12 +23,12 @@ Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot)
 
     theta1 = -asin(Rot(2,0));
     theta2 = 3.141592653589793 - theta1;
-    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); 
-    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); 
-    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); 
-    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); 
+    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1));
+    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2));
+    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1));
+    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2));
 
-    // TODO: This statement can cause big discontinuities in the obtained angles when 
+    // TODO: This statement can cause big discontinuities in the obtained angles when
     // the two norms are similar.
     if (sqrt(std::pow(theta1,2)+std::pow(psi1,2)+std::pow(phi1,2)) < sqrt(std::pow(theta2,2)+std::pow(psi2,2)+std::pow(phi2,2)))
       ang << psi1, theta1, phi1;
@@ -42,12 +42,12 @@ Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot)
     if (Rot(2,0)==-1.0)
     {
       theta = 3.141592653589793/2;
-      psi = phi + atan2(Rot(0,1),Rot(0,2)); 
+      psi = phi + atan2(Rot(0,1),Rot(0,2));
     }
     else
     {
       theta = -3.141592653589793/2;
-      psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); 
+      psi = -phi + atan2(-Rot(0,1),-Rot(0,2));
     }
     ang << phi, theta, psi;
   }
@@ -69,10 +69,10 @@ Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const
 
     theta1 = -asin(Rot(2,0));
     theta2 = 3.141592653589793 - theta1;
-    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); 
-    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); 
-    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); 
-    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); 
+    psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1));
+    psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2));
+    phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1));
+    phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2));
 
     ang1 << psi1, theta1, phi1;
     ang2 << psi2, theta2, phi2;
@@ -89,19 +89,22 @@ Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const
     if (Rot(2,0)==-1.0)
     {
       theta = 3.141592653589793/2;
-      psi = phi + atan2(Rot(0,1),Rot(0,2)); 
+      psi = phi + atan2(Rot(0,1),Rot(0,2));
     }
     else
     {
       theta = -3.141592653589793/2;
-      psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); 
+      psi = -phi + atan2(-Rot(0,1),-Rot(0,2));
     }
     ang << phi, theta, psi;
   }
   return ang;
 }
 
-void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::MatrixXd& vs_delta_gain, const double& vs_alpha_min, const Eigen::MatrixXd& eepos_des, Eigen::MatrixXd& sigmaEEPOS, Eigen::MatrixXd& JEEPOS_reduced, Eigen::MatrixXd& JEEPOS_pseudo_reduced)
+void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::MatrixXd& vs_delta_gain,
+                              const double& vs_alpha_min, const Eigen::MatrixXd& eepos_des,
+                              Eigen::MatrixXd& sigmaEEPOS, Eigen::MatrixXd& JEEPOS_reduced,
+                              Eigen::MatrixXd& JEEPOS_pseudo_reduced)
 {
   // Initialize sizes
   sigmaEEPOS = Eigen::MatrixXd::Zero(6,1);
@@ -110,11 +113,41 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
 
   // Task Error ________________________________________________
 
-  // End-Effector Position Error 
+  // End-Effector Position Error
   sigmaEEPOS.block(0,0,3,1) = eepos_des.block(0,0,3,1) - HT.cam_in_w.block(0,3,3,1);
 
   // End-Effector Orientation Error
-  sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) -  RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1));
+  // sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) -  RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1));
+
+  // // Position and orientation error
+  // Eigen::Translation<double,3> p_des(eepos_des(0,0), eepos_des(1,0), eepos_des(2,0));
+  // Eigen::Quaterniond q_des = Eigen::AngleAxisd(eepos_des(3,0), Eigen::Vector3d::UnitX())
+  //     * Eigen::AngleAxisd(eepos_des(4,0), Eigen::Vector3d::UnitY())
+  //     * Eigen::AngleAxisd(eepos_des(5,0), Eigen::Vector3d::UnitZ());
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_des_in_w = p_des*q_des;
+  //
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w(HT.cam_in_w);
+  //
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_w_in_des = HT_des_in_w.inverse();
+  //
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_des = HT_robot_in_w*HT_w_in_des;
+  //
+  //
+
+
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_aux = HT_des_in_w.inverse()*HT_robot_in_w;
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_error = HT_aux.inverse();
+
+  // Eigen::Translation<double,3> p_curr_in_w(0.0,0.0,0.0);
+  // Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w_in_w;
+  // HT_robot_in_w_in_w = HT_robot_in_w;
+  // HT_robot_in_w_in_w.translation() = p_curr_in_w;
+  //
+  // HT_error = HT_robot_in_w_in_w*HT_error;
+
+  // sigmaEEPOS.block(0,0,3,1) = HT_des_in_w.rotation() * HT_robot_in_des.translation();
+  // std::cout << "Error" << '\n' << sigmaEEPOS.block(0,0,3,1) << '\n';
+  // sigmaEEPOS.block(3,0,3,1) = HT_error.rotate(HT_robot_in_w.inverse().rotation()).rotation().eulerAngles(0, 1, 2);
 
   // Task Jacobian _____________________________________________
 
@@ -126,7 +159,7 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
 
   Eigen::Vector3d euRb_in_w = RToEulerMin(Rb_in_w);
   Eigen::Vector3d euRe_in_w = RToEulerMin(Re_in_w);
-  
+
   Eigen::Matrix3d wTb = UAM::CCommon_Fc::GetWronskian(euRb_in_w(0),euRb_in_w(1));
   Eigen::Matrix3d wTb_inv = wTb.inverse();
   Eigen::Matrix3d wTe = UAM::CCommon_Fc::GetWronskian(euRe_in_w(0),euRe_in_w(1));
@@ -138,9 +171,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   // Aux_Frame_Matrix1(1,1) = -1.0;
   // Aux_Frame_Matrix1(2,0) = 1.0;
   //JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(Aux_Frame_Matrix1*pe_in_b))*wTb_inv;
-  
+
   Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj);
-  JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); 
+  JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
   JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3);
   JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv;
   JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv;
@@ -163,9 +196,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
   Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);
   omega2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
   omega2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose();
-  
-  JEEPOS.block(0,0,6,6) = omega2euldiff_in_w*JEEPOS_omw.block(0,0,6,6); 
-  JEEPOS.block(0,6,6,arm.nj) = omega2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj); 
+
+  JEEPOS.block(0,0,6,6) = omega2euldiff_in_w*JEEPOS_omw.block(0,0,6,6);
+  JEEPOS.block(0,6,6,arm.nj) = omega2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj);
 
   // task jacobian pseudo inverse
   JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm());
diff --git a/src/tasks/jl.cpp b/src/tasks/jl.cpp
index 9a99a9a176e6ae4c3c59290e8c9a961e2fabe42b..2e87948486a2a8e41dc42d920c5808eecd2c165d 100755
--- a/src/tasks/jl.cpp
+++ b/src/tasks/jl.cpp
@@ -25,7 +25,8 @@ Eigen::MatrixXd CTaskJL::TaskErrorCommon(const UAM::CArm& arm, const double& pow
   return AAL;
 }
 
-void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
+void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des,
+Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
 {
   // Get common elements
   double pow_n = 4;
@@ -55,18 +56,18 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos
   TaskErrorCommon(arm,pow_n,jntlim_pos_des,jntlim_pos_error,AAL);
 
   sigmaL = Eigen::MatrixXd::Zero(arm.nj,1);
-  JL = Eigen::MatrixXd::Zero(arm.nj,4+arm.nj);  
-    
+  JL = Eigen::MatrixXd::Zero(arm.nj,4+arm.nj);
+
   for (unsigned int ii = 0; ii < arm.nj; ++ii)
   {
     // Task sigma
-    sigmaL(ii,0) = -AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n); 
+    sigmaL(ii,0) = -AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n);
     // Task Jacobian
     JL(ii,4+ii) = -pow_n*AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n-1);
-  }   
+  }
 
   // Task Jacobian Pseudo-inverse
   JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
 }
 
-} // End of UAM namespace
\ No newline at end of file
+} // End of UAM namespace
diff --git a/src/tasks/jl_ineq.cpp b/src/tasks/jl_ineq.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6105f96bc666d81357cabb822e25b622d038c8c8
--- /dev/null
+++ b/src/tasks/jl_ineq.cpp
@@ -0,0 +1,41 @@
+// Ownsr/include/eigen3/Eigen/src/Core/Product.h:133: Eigen::Product<Lhs, Rhs, Option>::Product(const Lhs&, const Rhs&) [with _Lhs = Eigen::Block<Eigen::Matrix<double, -1, -1>, 1, -1, false>; _Rh
+#include "jl_ineq.h"
+#include <common_fc.h>
+
+namespace UAM {
+
+CTaskJLIn::CTaskJLIn(){}
+
+CTaskJLIn::~CTaskJLIn(){}
+
+void CTaskJLIn::TaskErrorJacFull(UAM::CArm& arm,
+                                 const Eigen::MatrixXd& jntlim_ub, const Eigen::MatrixXd& jntlim_lb,
+                                 Eigen::MatrixXd& sigmaL,
+                                 Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
+{
+
+  sigmaL = Eigen::MatrixXd::Zero(2*arm.nj,1);
+  JL = Eigen::MatrixXd::Zero(arm.nj*2,4+arm.nj);
+	JL.block(0, 4, arm.nj, arm.nj) = Eigen::MatrixXd::Identity(arm.nj, arm.nj);
+	JL.block(arm.nj, 4, arm.nj, arm.nj) = -1*Eigen::MatrixXd::Identity(arm.nj, arm.nj);
+  // JL.block(0, 4, arm.nj, arm.nj) = -1*Eigen::MatrixXd::Identity(arm.nj, arm.nj);
+  // JL.block(arm.nj, 4, arm.nj, arm.nj) = Eigen::MatrixXd::Identity(arm.nj, arm.nj);
+
+  for (unsigned int ii = 0; ii < arm.nj; ++ii)
+  {
+    // Task sigma
+    // sigmaL(ii,0)          = arm.jnt_pos(ii,0) - jntlim_ub(ii,0); // q-ub<=0
+    // sigmaL(ii + arm.nj,0) = jntlim_lb(ii,0) - arm.jnt_pos(ii,0); // lb-q<=0
+    // Note: Although the ideal formulation should consider the above equations,
+    // we formulate the errors as e >= 0 to keep compatibility with the other tasks
+    // which are also defined as e >= 0.
+    // Inside the solver code there is a fix to solve for e<=0.
+    sigmaL(ii,0)          = jntlim_ub(ii,0) - arm.jnt_pos(ii,0); // ub-q>=0
+    sigmaL(ii + arm.nj,0) = arm.jnt_pos(ii,0) - jntlim_lb(ii,0); // q-lb>=0
+  }
+
+  // Task Jacobian Pseudo-inverse
+  JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
+}
+
+} // End of UAM namespace
diff --git a/src/tasks/jl_ineq.h b/src/tasks/jl_ineq.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2694042d9fe905d0008a799156366c756d75e97
--- /dev/null
+++ b/src/tasks/jl_ineq.h
@@ -0,0 +1,49 @@
+#ifndef _TASK_JLIN_H
+#define _TASK_JLIN_H
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// Own
+#include <common_obj.h>
+
+namespace UAM {
+
+class CTaskJLIn
+{
+  private:
+
+  public:
+
+  	/**
+    * \brief Constructor
+    *
+    * Class constructor.
+    *
+    */
+    CTaskJLIn();
+
+  	/**
+    * \brief Destructor
+    *
+    * Class destructor.
+    *
+    */
+    ~CTaskJLIn();
+
+    /**
+    * \brief Task Error and Jacobians
+    *
+    * Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
+    * The returned Jacobian has full size (Arm DoF rows) and similarly the pseudo inverse of the jacobian.
+    *
+    */
+    static void TaskErrorJacFull(UAM::CArm& arm,
+                                     const Eigen::MatrixXd& jntlim_ub, const Eigen::MatrixXd& jntlim_lb,
+                                     Eigen::MatrixXd& sigmaL,
+                                     Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo);
+};
+
+} // End of UAM namespace
+
+#endif