diff --git a/src/quadarm_task_priority_ctrl.cpp b/src/quadarm_task_priority_ctrl.cpp
index 6d842bf7d60b2c970a3833d14824dbc4deb31d42..b97ccaa46fe416dc8d27233ca1e9974f92d3f5e1 100644
--- a/src/quadarm_task_priority_ctrl.cpp
+++ b/src/quadarm_task_priority_ctrl.cpp
@@ -13,13 +13,7 @@ CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
 {
 }
 
-/*void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
-                                                             const ht& HT,
-                                                             const Chain& kdl_chain,
-                                                             const vector<arm_joint> joint_info,
-                                                             ctrl_params& ctrl_params,
-                                                             MatrixXd& joint_pos,
-                                                             MatrixXd& robot_vels)*/
+// Main public function
 void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goal,
                                                              const ht& HT,
                                                              const arm& arm,
@@ -57,6 +51,7 @@ void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const goal_obj& goa
   robot_vel = this->vel_.quadarm;
 }
 
+// Kinematics
 void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
 
   // Get arm Homogenous transform, arm tip in base_link
@@ -71,6 +66,7 @@ void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
 
   // Quadrotor Jacobian body to inertial frames
   MatrixXd Jb2i = quadrotor_jacobian();
+
   // Inertial to body frames
   MatrixXd Ji2b = calcPinv(Jb2i);
 
@@ -103,63 +99,6 @@ void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
   this->Jqa_.block(0,0,6,6) = Rot*Jb2c*Ji2b;
   this->Jqa_.block(0,6,6,this->arm_.nj)=Rot*Ja;
 }
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
-{
-  double psi,theta,phi;
-
-  phi = this->arm_.jnt_pos(3,0);
-  theta = this->arm_.jnt_pos(4,0);
-  // psi = this->arm_.jnt_pos(5,0);
-  psi = 0;
-
-  MatrixXd Jq(6,6);
-  Jq(0,0) = cos(psi)*cos(theta);
-  Jq(0,1) = sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
-  Jq(0,2) = cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
-  Jq(1,0) = sin(psi)*cos(theta);
-  Jq(1,1) = sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);
-
-  Jq(1,2) = cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
-  Jq(2,0) = -sin(theta);
-  Jq(2,1) = sin(phi)*cos(theta);
-  Jq(2,2) = cos(phi)*cos(theta);
-  Jq.block(0,3,3,3) = MatrixXd::Zero(3,3);
-  Jq.block(3,0,3,3) = MatrixXd::Zero(3,3);
-  Jq(3,3) = 1;
-  Jq(3,4) = sin(phi)*tan(theta);
-  Jq(3,5) = cos(phi)*tan(theta);
-  Jq(4,3) = 0;
-  Jq(4,4) = cos(phi);
-  Jq(4,5) = -sin(phi);
-  Jq(5,3) = 0;
-  Jq(5,4) = sin(phi)*cos(theta);
-  Jq(5,5) = cos(phi)*cos(theta);
-
-  return Jq;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
-{
-  // constructs the kdl solver in non-realtime
-  this->arm_.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(this->arm_.chain));
-
-  // resize the joint state vector in non-realtime
-  this->arm_.jacobian.resize(this->arm_.nj);
-
-  // compute Jacobian in realtime
-  this->arm_.jnt_to_jac_solver->JntToJac(this->arm_.jnt_pos_kdl, this->arm_.jacobian);
-
-  MatrixXd Ja = MatrixXd::Zero(6,this->arm_.nj);
-
-  for (int ii=0; ii<6; ++ii){
-    for (unsigned int jj=0; jj<this->arm_.nj; ++jj){
-      Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
-    }
-  }
-  return Ja;
-}
-
 Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
 {
 
@@ -198,7 +137,62 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
 
   return Tarm;
 }
+MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
+{
+  // constructs the kdl solver in non-realtime
+  this->arm_.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(this->arm_.chain));
+
+  // resize the joint state vector in non-realtime
+  this->arm_.jacobian.resize(this->arm_.nj);
 
+  // compute Jacobian in realtime
+  this->arm_.jnt_to_jac_solver->JntToJac(this->arm_.jnt_pos_kdl, this->arm_.jacobian);
+
+  MatrixXd Ja = MatrixXd::Zero(6,this->arm_.nj);
+
+  for (int ii=0; ii<6; ++ii){
+    for (unsigned int jj=0; jj<this->arm_.nj; ++jj){
+      Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
+    }
+  }
+  return Ja;
+}
+MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
+{
+  double psi,theta,phi;
+
+  phi = this->arm_.jnt_pos(3,0);
+  theta = this->arm_.jnt_pos(4,0);
+  // psi = this->arm_.jnt_pos(5,0);
+  psi = 0;
+
+  MatrixXd Jq(6,6);
+  Jq(0,0) = cos(psi)*cos(theta);
+  Jq(0,1) = sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
+  Jq(0,2) = cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
+  Jq(1,0) = sin(psi)*cos(theta);
+  Jq(1,1) = sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);
+
+  Jq(1,2) = cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
+  Jq(2,0) = -sin(theta);
+  Jq(2,1) = sin(phi)*cos(theta);
+  Jq(2,2) = cos(phi)*cos(theta);
+  Jq.block(0,3,3,3) = MatrixXd::Zero(3,3);
+  Jq.block(3,0,3,3) = MatrixXd::Zero(3,3);
+  Jq(3,3) = 1;
+  Jq(3,4) = sin(phi)*tan(theta);
+  Jq(3,5) = cos(phi)*tan(theta);
+  Jq(4,3) = 0;
+  Jq(4,4) = cos(phi);
+  Jq(4,5) = -sin(phi);
+  Jq(5,3) = 0;
+  Jq(5,4) = sin(phi)*cos(theta);
+  Jq(5,5) = cos(phi)*cos(theta);
+
+  return Jq;
+}
+
+// Control
 void CQuadarm_Task_Priority_Ctrl::qa_control(){
   
   // Underactuated Quadrotor 
@@ -263,7 +257,7 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   this->ctrl_params_.jntlim_vel = Nqa1_G * JL_pseudo * lambdaL * sigmaL;
 
   //******************************************************************
-
+  // Weighted sum of subtasks
   // this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
   // this->ctrl_params_.jntlim_vel = MatrixXd::Zero(9,1);
 
@@ -288,36 +282,6 @@ void CQuadarm_Task_Priority_Ctrl::qa_control(){
   this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
   this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
 }
-
-void CQuadarm_Task_Priority_Ctrl::weight_inverse(const MatrixXd& J, MatrixXd& J_inverse)
-{
-
-  double w_min = this->ctrl_params_.vs_delta_gain(0,0);
-  double w_max = this->ctrl_params_.vs_delta_gain(1,0);
-  double alpha_min = this->ctrl_params_.vs_alpha_min;
-
-  double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159;
-
-  double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);
-  // double alpha = (0.5*alpha_min) + (0.5*alpha_min*tanh(tmp));
-  
-  MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-
-  for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii)
-  {
-    // Quadrotor values
-    if(ii<4)
-      W(ii,ii) = 1-alpha;
-    // Arm values
-    else
-      W(ii,ii) = alpha;
-  }
-  
-  MatrixXd temp = J*W.inverse()*J.transpose();
-
-  J_inverse = W.inverse()*J.transpose()*temp.inverse();
-}
-
 void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
 {
   // Initializations
@@ -374,7 +338,6 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
 
   for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
   {
-
     // Get mass of each link
     mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
     // Joint frames  
@@ -407,7 +370,15 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
 
   // X and Y values of CoG in quadrotor inertial frame
   MatrixXd cog_arm_in_qi = Rib * this->ctrl_params_.cog_arm.block(0,0,3,1);
-  this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
+
+  // This change is done considering arm base frame orientation
+  // TODO: Work with quadrotor orientation
+  //std::cout << "******" << std::endl << cog_arm_in_qi << std::endl;
+  //this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
+  this->ctrl_params_.cog_PGxy = MatrixXd::Zero(2,1);
+  this->ctrl_params_.cog_PGxy(0,0) = cog_arm_in_qi(2,0);
+  this->ctrl_params_.cog_PGxy(1,0) = 	cog_arm_in_qi(1,0);
+
   MatrixXd X_partial;
 
   // Partial jacobian of each link (augmented links)
@@ -451,7 +422,6 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
 
   sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
 }
-
 void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL)
 {
   // Get diagonal gain matrix
@@ -479,288 +449,35 @@ void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL)
   JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose();
 }
 
-void CQuadarm_Task_Priority_Ctrl::old_sec_task_vel_unina(){
-
-  double ndof = 4+this->arm_.nj;
-  this->vel_.old_quadarm= MatrixXd::Zero(ndof,1);
-
-  // Kinematic Stabilization
-  MatrixXd w_stabil = old_sec_task_stabil();
-  this->vel_.old_quadarm = this->vel_.old_quadarm + this->ctrl_params_.stabil_gain*w_stabil;
-
-  // Center of Gravity alignement
-  MatrixXd w_cg = old_sec_task_cg();
-  this->vel_.old_quadarm.block(4,0,5,1) = this->vel_.old_quadarm.block(4,0,5,1) + this->ctrl_params_.cog_gain*w_cg;
-
-  // Joint limits
-  MatrixXd w_jointlimits = old_sec_task_jointlimits();
-  this->vel_.old_quadarm.block(4,0,5,1) = this->vel_.old_quadarm.block(4,0,5,1) + this->ctrl_params_.jntlim_gain*w_jointlimits;
-
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_stabil(){
-
-  // // Stabilize quadrotor ///////////////////////////////////////////////
-  MatrixXd w_stabil = MatrixXd::Zero(4+this->arm_.nj,1);
-
-  float dq = 0.001;
-
-  for (unsigned int i = 0; i < this->arm_.nj; ++i)
-  {
-    MatrixXd q_old(this->arm_.nj,1),q_new(this->arm_.nj,1);
-    MatrixXd angles_old(3,1),angles_new(3,1);
-
-    q_old = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
-    q_old(i,0) = q_old(i,0)-dq;
-    q_new = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
-    q_new(i,0) = q_new(i,0)+dq;
-
-    angles_old = old_get_stabil_angles(q_old);
-    angles_new = old_get_stabil_angles(q_new);
-
-    w_stabil(i,0)=(((angles_new(0)*angles_new(0))+(angles_new(1)*angles_new(1)))-((angles_old(0)*angles_old(0))+(angles_old(1)*angles_old(1))))/dq;
-
-  }
-
-  return w_stabil;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_get_stabil_angles(const MatrixXd &qa){
-
-  for (unsigned int jj = 0; jj < this->arm_.nj; ++jj){
-    this->arm_.jnt_pos_kdl.data(jj) = qa(jj,0);
-  }
-  
-  // compute Cartesian pose in realtime
-  Frame kdlTarm;
-  this->arm_.jnt_to_pose_solver->JntToCart(this->arm_.jnt_pos_kdl, kdlTarm);
-  double ro,pit,ya;
-  kdlTarm.M.GetEulerZYX(ya,pit,ro);
-
-  Matrix4d HT,inv_HT;  
-  Matrix3d Rarm;
-  // euler convention zyx    
-  Rarm = Eigen::AngleAxisd(ya, Vector3d::UnitZ()) * Eigen::AngleAxisd(pit, Vector3d::UnitY()) * Eigen::AngleAxisd(ro, Vector3d::UnitX());
-  HT.block(0,0,3,3) = Rarm;
-  HT(0,3) = (double)kdlTarm.p.data[0];
-  HT(1,3) = (double)kdlTarm.p.data[1];
-  HT(2,3) = (double)kdlTarm.p.data[2];
-  HT.row(3) << 0, 0, 0, 1;
-
-  // inv_HT = this->T0c_new_*HT.inverse();
-  inv_HT = HT.inverse();
-
-  MatrixXd angles(3,1);
-  Matrix3d Rot;
-  Rot = inv_HT.block(0,0,3,3);
-  angles = Rot.eulerAngles(2,1,0);
-
-  return angles;
-}
-
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_cg(){
-  
-  double ndof = 4+this->arm_.nj;
-  MatrixXd w_cg = MatrixXd::Zero(this->arm_.nj,1);
-  this->vel_.old_quadarm= MatrixXd::Zero(ndof,1);
-  
-  // Get arm segments
-  // num of segments
-  unsigned int js = this->arm_.chain.getNrOfSegments();
-  vector<Segment> arm_segment(js);
-  arm_segment.reserve(js);
-
-  for (unsigned int ii = 1; ii < js; ++ii)
-  {
-    arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
-  }
-
-  double dq=0.01;
-
-  MatrixXd qa_old(this->arm_.nj,1),qa_new(this->arm_.nj,1);
-
-  for (unsigned int ii = 0; ii < this->arm_.nj; ++ii){
-
-    qa_old = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
-    qa_new = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
-    qa_old(ii,0) = qa_old(ii,0)-dq;
-    qa_new(ii,0) = qa_new(ii,0)+dq;
-
-    MatrixXd rcm_old = old_get_cg_dist(qa_old,arm_segment);
-    MatrixXd rcm_new = old_get_cg_dist(qa_new,arm_segment);    
-
-    w_cg(ii,0)=-(((rcm_new(0,0)*rcm_new(0,0)+rcm_new(1,0)*rcm_new(1,0))-(rcm_old(0,0)*rcm_old(0,0)+rcm_old(1,0)*rcm_old(1,0)))/(2*dq));
-  }
-
-  return w_cg;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_get_cg_dist(const MatrixXd &qa,const vector<Segment> &arm_segment){
-
-
-    MatrixXd rcm = MatrixXd::Zero(3,1);
-
-    vector<Frame> joint_pose(this->arm_.nj);
-    vector<Matrix4d> Transf(this->arm_.nj);
-    vector<MatrixXd> T_base_to_joint(this->arm_.nj+1);
-    vector<MatrixXd> dist(this->arm_.nj);
-    vector<MatrixXd> link_cg(this->arm_.nj);
-    // mass of each link
-    MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1);
-
-    joint_pose.reserve(this->arm_.nj);
-    Transf.reserve(this->arm_.nj);
-    T_base_to_joint.reserve(this->arm_.nj+1);
-    dist.reserve(this->arm_.nj);
-    link_cg.reserve(this->arm_.nj);
-
-    double arm_mass = 0;
-    MatrixXd arm_cg = MatrixXd::Zero(4,1);
-
-    // Get Homogenous transforms of the joints
-    for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
-    {
-      joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0));
-      Transf.at(jj) = GetTransform(joint_pose.at(jj));
-      mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
-    }
-
-
-    MatrixXd pini(4,1);
-    pini.col(0) << 0,0,0,1;
-  
-    //Fixed transform between base_link and arm_base
-    Matrix4d Tbase = Matrix4d::Zero();
-    Tbase(0,2) = 1;
-    Tbase(1,1) = 1;
-    Tbase(2,0) = -1;
-    Tbase(3,3) = 1;
-    
-    T_base_to_joint.at(0) = Tbase*pini;
-    T_base_to_joint.at(1) = Tbase*Transf.at(0)*pini;
-    T_base_to_joint.at(2) = Tbase*Transf.at(0)*Transf.at(1)*pini;
-    T_base_to_joint.at(3) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*pini;
-    T_base_to_joint.at(4) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*Transf.at(3)*pini;
-    T_base_to_joint.at(5) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*Transf.at(3)*Transf.at(4)*pini;
-
-    for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
-    {
-      // Distance to the middle of the link jj
-      dist.at(jj) = (T_base_to_joint.at(jj+1)-T_base_to_joint.at(jj))/2;
-
-      // gravity for the link jj 
-      link_cg.at(jj) = mass(jj,0)*(T_base_to_joint.at(jj)+dist.at(jj));
-
-      // arm CoG
-      arm_cg = arm_cg + link_cg.at(jj);
-
-      // arm mass  
-      arm_mass = arm_mass + mass(jj,0);
-    }
-
-    // vector from arm base to CoG
-    rcm = arm_cg/arm_mass;
-
-   // *************************************************************
-
-    // // CoG r.t. quadrotor body frame
-    // MatrixXd rcm_qb(4,1);
-    // rcm_qb = Tbase.inverse() * rcm;
-
-    // // Rotation between quadrotor body and inertial frames  
-    // Matrix3d Rot;
-    // Rot = AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX());
-  
-    // MatrixXd rcm_2(3,1),rcm_3(3,1), mat1(3,1),mat2(3,1),mat3(3,1), Q1(3,1), Q2(3,1);
-
-    // // CoG in quadrotor inertial frame
-    // rcm_2 = Rot.transpose()*rcm_qb.block(0,0,3,1);
-
-    // // distance of the CoG to the vertical centered line of the quadrotor frame.
-    // Vector3d Qs(3,1), vec1(3,1), vec2(3,1);
-
-    // Q1 = MatrixXd::Zero(3,1);
-    // Q2 = MatrixXd::Zero(3,1);
-    // Q1(2,0) = -0.1;
-    // Q2(2,0) = -0.5;
-
-    // vec1 = rcm_2 - Q1;
-    // vec2 = rcm_2 - Q2;
-    // Qs = Q2 - Q1;
-
-    // mat1 = vec1.cross(vec2);
-    // mat2 = mat1.array().abs();
-    // mat3 = Qs.array().abs();
-
-    // cout << "11" << endl;
-    // cout << rcm_2 << endl;
-    // cout << "22" << endl;
-    // cout << mat3 << endl;
-
-
-    // for (unsigned int ii = 0; ii < 3; ++ii)
-    // {
-    //   if (mat3(ii,0)==0) {
-    //     rcm_3(ii,0)=0;
-    //   } 
-    //   else {
-    //     cout << "yeyeyeye" << endl;
-    //     rcm_3(ii,0) = mat2(ii,0)/mat3(ii,0);
-    //   }
-    // }
-
-    // rcm.block(0,0,3,1) = rcm_3;
-
-    // cout << "33" << endl;
-    // cout << rcm << endl;
-
-   // *************************************************************
-
-    return rcm;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_jointlimits(){
-  // Joint Limits ////////////////////////////////////////////////////77
-
-  MatrixXd w_jointlimits = MatrixXd::Zero(this->arm_.nj,1);
-
-  MatrixXd joint_optim(this->arm_.nj,1), joint_max(this->arm_.nj,1), joint_min(this->arm_.nj,1);
+// MISC functions
+void CQuadarm_Task_Priority_Ctrl::weight_inverse(const MatrixXd& J, MatrixXd& J_inverse)
+{
 
-  joint_max(0,0) = 0.548;
-  joint_min(0,0) = -0.548;
-  
-  joint_max(1,0) = 1.57;
-  joint_min(1,0) = -1.57;
+  double w_min = this->ctrl_params_.vs_delta_gain(0,0);
+  double w_max = this->ctrl_params_.vs_delta_gain(1,0);
+  double alpha_min = this->ctrl_params_.vs_alpha_min;
 
-  joint_max(2,0) = 5.2;
-  joint_min(2,0) = 0;
+  double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159;
 
-  joint_max(3,0) = 5.2;
-  joint_min(3,0) = 0;
+  double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);
+  // double alpha = (0.5*alpha_min) + (0.5*alpha_min*tanh(tmp));
 
-  joint_max(4,0) = 3.1416;
-  joint_min(4,0) = -3.1416;
+  MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
 
-  for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
+  for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii)
   {
-    joint_optim(jj,0) = (joint_max(jj,0)+joint_min(jj,0))/2;
-
-    double sq_pos = this->arm_.nj * (joint_max(jj,0)-joint_min(jj,0));
-
-    w_jointlimits(jj,0)=-((this->arm_.jnt_pos(6+jj,0)-joint_optim(jj,0))/sq_pos);  
+    // Quadrotor values
+    if(ii<4)
+      W(ii,ii) = 1-alpha;
+    // Arm values
+    else
+      W(ii,ii) = alpha;
   }
 
-  return w_jointlimits;
-}
+  MatrixXd temp = J*W.inverse()*J.transpose();
 
-void CQuadarm_Task_Priority_Ctrl::old_sec_task_vel_catec()
-{
-  //DOF's
-  double ndof = 4+this->arm_.nj;
-  this->vel_.old_quadarm = MatrixXd::Zero(ndof,1);
+  J_inverse = W.inverse()*J.transpose()*temp.inverse();
 }
-
 MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
 
         // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
@@ -804,7 +521,6 @@ MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
         
         return m_pinv;
 }
-
 Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
 {
 
@@ -826,7 +542,6 @@ Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
 
   return T;
 }
-
 void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts)
 {
   // Get home directory
diff --git a/src/quadarm_task_priority_ctrl.h b/src/quadarm_task_priority_ctrl.h
index a26e7ba75edc46e114d56e22a92d612d1debf51a..a9d04217ec93817aca7579d5574e8d7a7dc178d8 100644
--- a/src/quadarm_task_priority_ctrl.h
+++ b/src/quadarm_task_priority_ctrl.h
@@ -46,7 +46,7 @@ typedef struct{
     Matrix4d tag_in_cam; // Tag in camera frames.
     Matrix4d tag_in_baselink; // Tag in base_link frames.
     Matrix4d cam_in_tip; // Camera in arm tip frames.
-    Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to las arm link frame).
+    Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to arm link frame).
     Matrix4d cam_in_baselink; // Camera in base_link frames.
 }ht;
 
@@ -87,9 +87,9 @@ typedef struct{
     MatrixXd vs_vel;  // Primary task velocity.      
     double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
     MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse.
-    double cog_gain; // Gain of CoG alignement secondary task.
+    double cog_gain; // Gain of CoG alignment secondary task.
     MatrixXd cog_vel; // Secondary task velocity.
-    MatrixXd cog_PGxy; // X and Y fo the CoG r.t. Quadrotor body inertial frame.
+    MatrixXd cog_PGxy; // X and Y of the CoG r.t. Quadrotor body inertial frame.
     MatrixXd cog_arm; // Arm Center of Gravity r.t. quadrotor body frame.
     double jntlim_gain; // Gain of joint limits secondary task.
     MatrixXd jntlim_vel; // Joint limit task velocity.
@@ -105,7 +105,7 @@ class CQuadarm_Task_Priority_Ctrl
 
   private:
 
-  	ht T_; // Homogenous Transforms
+  	ht T_; // Homogeneous Transforms
 
     obj_vel vel_; // Robot velocities
 
@@ -120,7 +120,7 @@ class CQuadarm_Task_Priority_Ctrl
     /**
     * \brief Compute Quadrotor and Arm Kinematics
     *
-    * Compute the quadrotor and arm jacobian and the position of the quadrotor using forward kinematics
+    * Compute the quadrotor and arm Jacobian and the position of the quadrotor using forward kinematics
     *
     */      
     void qa_kinematics();
@@ -160,13 +160,13 @@ class CQuadarm_Task_Priority_Ctrl
     /**
     * \brief Get weighted generalized Jacobian inverse 
     *
-    * Compute the jacobian inverse weighted depending on target distance
+    * Compute the Jacobian inverse weighted depending on target distance
     *
     */ 
     void weight_inverse(const MatrixXd& J, MatrixXd& J_inverse);
 
     /**
-    * \brief Secondary task gravity alignement
+    * \brief Secondary task gravity alignment
     *
     * Compute vector of the secondary task to vertically align the arm center of gravity 
     *
@@ -184,74 +184,16 @@ class CQuadarm_Task_Priority_Ctrl
     void sec_task_jl(MatrixXd& JG,MatrixXd& sigmaG);
     
     /**
-    * \brief Compute Desired pose velocities at the jacobian null space (over-determined system) 
-    *
-    * Compute the vector of motion of the jacobian null space
-    *
-    */      
-    void old_sec_task_vel_unina();
-   
-    /**
-    * \brief Compute Desired pose velocities at the jacobian null space (over-determined system) 
-    *
-    * Compute the vector of motion of the jacobian null space
-    *
-    */      
-    void old_sec_task_vel_catec();
-
-    /**
-    * \brief Secondary task kinematic stabilization
-    *
-    * Compute those solutions that accomplish the primary task and also that reaches horizontal arm base. 
-    *
-    */  
-    MatrixXd old_sec_task_stabil();
-
-    /**
-    * \brief Get Arm base angles due to arm kinematics
-    *
-    * Compute the arm kinematics to know the produced angles in the arm base. 
-    *
-    */ 
-    MatrixXd old_get_stabil_angles(const MatrixXd &qa);
-
-    /**
-    * \brief OLD Secondary task gravity alignement
-    *
-    * Compute vector of the secondary task to vertically align the arm center of gravity 
-    *
-    * with the quadrotor gravitational vector
-    *
-    */  
-    MatrixXd old_sec_task_cg();
-
-    /**
-    * \brief OLD Secondary task Arm joint limits avoidance
-    *
-    * Compute vector of the secondary task to avoid arm joint limits
-    *
-    */ 
-    MatrixXd old_sec_task_jointlimits();
-
-    /**
-    * \brief Get arm's center of gravity distance
-    *
-    * Get arm's center of gravity distance 
-    *
-    */  
-    MatrixXd old_get_cg_dist(const MatrixXd &qa,const vector<Segment> &arm_segment);
-
-    /**
-    * \bried Matrix pseudo-inverse
+    * \brief Matrix pseudo-inverse
     *
     * Compute the matrix pseudo-inverse using SVD
     */
     MatrixXd calcPinv(const MatrixXd &a);
 
     /**
-    * \bried KDL Frame to Homogenous transform
+    * \brief KDL Frame to Homogeneous transform
     *
-    * Compute the convertion from KDL Frame to Homogenous transform (Eigen Matrix 4f)
+    * Compute the conversion from KDL Frame to Homogeneous transform (Eigen Matrix 4f)
     */
     Matrix4d GetTransform(const Frame &f);
 
@@ -285,7 +227,6 @@ class CQuadarm_Task_Priority_Ctrl
                                     ctrl_params& ctrl_params,
 																		MatrixXd& joint_pos,
                                     MatrixXd& robot_vel);
-
     /**
     * \brief Write to file
     *