diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ab9fc5a14849792a2766b21ed6bcc8b232becbb5..b8cea50c4bb605e34d636e511097aa98bebd6f60 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,10 +1,10 @@
 # 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)
+SET(sources_tasks tasks/ir.cpp tasks/vs.cpp tasks/cog.cpp tasks/jl.cpp tasks/eepos.cpp tasks/heading.cpp tasks/basepos.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)
+SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h tasks/heading.h tasks/basepos.h)
 
 # locate the necessary dependencies
 
diff --git a/src/tasks/basepos.cpp b/src/tasks/basepos.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b950433a2cd7aa0def7a489944930dcca374271
--- /dev/null
+++ b/src/tasks/basepos.cpp
@@ -0,0 +1,78 @@
+// Own
+#include "basepos.h"
+#include "common_fc.h"
+#include "kinematics.h"
+
+// MISC
+#include <iostream>      /* input, output operations */
+ 
+namespace UAM {
+
+CTaskBASEPOS::CTaskBASEPOS(){}
+
+CTaskBASEPOS::~CTaskBASEPOS(){}
+
+Eigen::Vector3d CTaskBASEPOS::RToEulerMin(const Eigen::Matrix3d& Rot)
+{
+  Eigen::Vector3d ang;
+
+  if (Rot(2,0)!=-1.0 && Rot(2,0)!=1.0)
+  {
+    double phi1, theta1, psi1;
+    double phi2, theta2, psi2;
+
+    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)); 
+
+    // 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;
+    else
+      ang << psi2, theta2, phi2; // ZYX Euler convention.
+  }
+  else
+  {
+    double phi, theta, psi;
+    phi = 0.0;
+    if (Rot(2,0)==-1.0)
+    {
+      theta = 3.141592653589793/2;
+      psi = phi + atan2(Rot(0,1),Rot(0,2)); 
+    }
+    else
+    {
+      theta = -3.141592653589793/2;
+      psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); 
+    }
+    ang << phi, theta, psi;
+  }
+
+  return ang;
+}
+
+void CTaskBASEPOS::TaskErrorJac(const UAM::CHT& HT, const UAM::CArm& arm, const Eigen::MatrixXd& pos_des, Eigen::MatrixXd& sigmaBASEPOS, Eigen::MatrixXd& JBASEPOS, Eigen::MatrixXd& JBASEPOS_pseudo)
+{
+  // Initialize sizes
+  sigmaBASEPOS = Eigen::MatrixXd::Zero(4,1);
+  
+  // Task Error ________________________________________________
+
+  // End-Effector Position Error 
+  sigmaBASEPOS.block(0,0,3,1) = pos_des.block(0,0,3,1) - HT.baselink_in_w.block(0,3,3,1);
+
+  // End-Effector Orientation Error
+  Eigen::Vector3d angles = RToEulerMin(HT.baselink_in_w.block(0,0,3,3));
+  sigmaBASEPOS(3,0) = pos_des(3,0) - angles(2,0);
+
+  // Task Jacobian _____________________________________________
+  JBASEPOS = Eigen::MatrixXd::Zero(pos_des.rows(),4+arm.nj);
+  JBASEPOS.block(0,0,4,4) = Eigen::Matrix4d::Identity();
+  JBASEPOS_pseudo = UAM::CCommon_Fc::CalcPinv(JBASEPOS);
+}
+
+} // End of UAM namespace
diff --git a/src/tasks/basepos.h b/src/tasks/basepos.h
new file mode 100644
index 0000000000000000000000000000000000000000..6219903a3e93c80d231079a6863138e3c6825490
--- /dev/null
+++ b/src/tasks/basepos.h
@@ -0,0 +1,58 @@
+#ifndef _TASK_BASEPOS_H
+#define _TASK_BASEPOS_H
+
+// Own
+#include <common_obj.h>
+
+// Eigen
+#include <eigen3/Eigen/Dense>
+
+// Other
+#include <math.h>
+#include <cmath>
+
+namespace UAM {
+
+class CTaskBASEPOS
+{
+  private:
+
+  public:
+
+    /**
+    * \brief Constructor
+    *
+    * Class constructor.
+    *
+    */
+    CTaskBASEPOS();
+
+    /**
+    * \brief Destructor
+    *
+    * Class destructor.
+    *
+    */
+    ~CTaskBASEPOS();
+
+    /**
+    * \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.
+    *
+    */ 
+    static void TaskErrorJac(const UAM::CHT& HT, const UAM::CArm& arm, const Eigen::MatrixXd& pos_des, Eigen::MatrixXd& sigmaBASEPOS, Eigen::MatrixXd& JBASEPOS, Eigen::MatrixXd& JBASEPOS_pseudo);
+    
+    /**
+    * \brief Rotation matrix to euler
+    *
+    * Returns the euler angles corresponding to the rotation matrix. Specifically returns the angles with the "shortest" path.
+    * Based on http://www.staff.city.ac.uk/%7Esbbh653/publications/euler.pdf
+    *
+    */ 
+    static Eigen::Vector3d RToEulerMin(const Eigen::Matrix3d& Rot);
+};
+
+} // End of UAM namespace
+
+#endif