VectorXd DampedNumericalFilteredController::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{
    thetas_ = thetas;
    ///--Controller Step

    //Calculate jacobian
    task_jacobian_  = robot_.analyticalJacobian(thetas_);

    // Recalculation of measured data.
    // End effectors pose
    end_effector_pose_ = robot_.fkm(thetas_);

    //Error
    last_error_      = error_;
    error_           = vec8(reference - end_effector_pose_);
    integral_error_ += error_;
    //error_ = vec8(dq_one_ - conj(end_effector_pose_)*reference);

    svd_.compute(task_jacobian_, ComputeFullU);
    singular_values_ = svd_.singularValues();

    //Damping Calculation
    double sigma_min = singular_values_(5);
    VectorXd u_min = svd_.matrixU().col(5);
    double lambda = lambda_max_;
    if (sigma_min < epsilon_)
    {
        lambda = (1-(sigma_min/epsilon_)*(sigma_min/epsilon_))*lambda_max_*lambda_max_;
    }

    //We want to solve the equation J+ = J^T(JJ^T+aI)^-1, in which the matrix
    //being inverted is obviously positive definite if a > 0.
    //The solver gives us the solution to X = A^-1.B
    //Therefore I chose to find X^T = B^T(A^T)^-1 then take the transpose of X.
    task_jacobian_pseudoinverse_ = ((task_jacobian_*task_jacobian_.transpose() + (beta_*beta_)*identity_ + (lambda*lambda)*u_min*u_min.transpose()).transpose()).ldlt().solve(task_jacobian_);
    task_jacobian_pseudoinverse_.transposeInPlace();

    if( at_least_one_error_ )
        delta_thetas_ = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ + kd_*(error_ - last_error_) );
    else
    {
        at_least_one_error_ = true;
        delta_thetas_ = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ );
    }

    return delta_thetas_;

}
/** Computes the dynamics (task model)
 * Assumes that the data_->model_.gc_model_ has been updated. */
bool CTaskOpPosition::computeModel(const SRobotSensors* arg_sensors)
{
#ifdef DEBUG
  assert(has_been_init_);
  assert(data_->has_been_init_);
  assert(S_NULL!=data_->rbd_);
  assert(S_NULL!=dynamics_);
#endif
  if(data_->has_been_init_)
  {
    bool flag = true;
    const SGcModel* gcm = data_->gc_model_;

    flag = flag && dynamics_->computeJacobian(data_->J_6_,*(data_->rbd_),
        arg_sensors->q_,data_->pos_in_parent_);

    //Use the position jacobian only. This is an op-point task.
    data_->J_ = data_->J_6_.block(0,0,3,data_->robot_->dof_);

    //Operational space mass/KE matrix:
//    // NOTE TODO : Decide a good scheme for disabling operational space inertia
//    if(data_->flag_compute_op_inertia_)
//    {   }
//    else
//    { data_->M_task_inv_ = Eigen::Matrix3d::Identity();  }

    //Lambda = (J * Ainv * J')^-1
    data_->M_task_inv_ = data_->J_ * gcm->M_gc_inv_ * data_->J_.transpose();

#ifdef SCL_PRINT_INFO_MESSAGES
    std::cout<<"\n\tJx6:\n"<<data_->J_6_
            <<"\n\tFgrav_gc:\n"<<data_->gc_model_->force_gc_grav_.transpose();

    std::cout<<"\n\tMx_inv:\n"<<data_->M_task_inv_
        <<"\n\tJx:\n"<<data_->J_
        <<"\n\tJx6:\n"<<data_->J_6_
        <<"\n\tMgcinv:\n"<<gcm->M_gc_inv_;

    std::cout<<"\n\tTo_lnk: \n"<<data_->rbd_->T_o_lnk_.matrix()
              <<"\n\tPosInPar: "<<data_->pos_in_parent_.transpose()
              <<"\n\t       X: "<<data_->x_.transpose()
              <<"\n\t   Xgoal: "<<data_->x_goal_.transpose()
              <<"\n\t   Ftask: "<<data_->force_task_.transpose()
              <<"\n\t Ftaskgc: "<<data_->force_gc_.transpose()
              <<"\n\t  Fxgrav: "<<data_->force_task_grav_.transpose();
#endif

    if(!use_svd_for_lambda_inv_)
    {
      //The general inverse function works very well for op-point controllers.
      //3x3 matrix inversion behaves quite well. Even near singularities where
      //singular values go down to ~0.001. If the model is coarse, use a n-k rank
      //approximation with the SVD for a k rank loss in a singularity.
      qr_.compute(data_->M_task_inv_);
      if(qr_.isInvertible())
      { data_->M_task_ = qr_.inverse();  }
      else
      { use_svd_for_lambda_inv_ = true; }
    }

    if(use_svd_for_lambda_inv_)
    {
      //Use a Jacobi svd. No preconditioner is required coz lambda inv is square.
      //NOTE : This is slower and generally performs worse than the simple inversion
      //for small (3x3) matrices that are usually used in op-space controllers.
      svd_.compute(data_->M_task_inv_,
          Eigen::ComputeFullU | Eigen::ComputeFullV | Eigen::ColPivHouseholderQRPreconditioner);

#ifdef DEBUG
      std::cout<<"\n Singular values : "<<svd_.singularValues().transpose();
#endif
      int rank_loss=0;

      //NOTE : A threshold of .005 works quite well for most robots.
      //Experimentally determined: Take the robot to a singularity
      //and observe the response as you allow the min singular values
      //to decrease. Stop when the robot starts to go unstable.
      //NOTE : This also strongly depends on how good your model is
      //and how fast you update it. A bad model will require higher
      //thresholds and will result in coarse motions. A better model
      //will allow much lower thresholds and will result in smooth
      //motions.
      if(svd_.singularValues()(0) > 0.005)
      { singular_values_(0,0) = 1.0/svd_.singularValues()(0);  }
      else { singular_values_(0,0) = 0.0; rank_loss++; }
      if(svd_.singularValues()(1) > 0.005)
      { singular_values_(1,1) = 1.0/svd_.singularValues()(1);  }
      else { singular_values_(1,1) = 0.0; rank_loss++; }
      if(svd_.singularValues()(2) > 0.005)
      { singular_values_(2,2) = 1.0/svd_.singularValues()(2);  }
      else { singular_values_(2,2) = 0.0; rank_loss++; }

      if(0 < rank_loss)
      { std::cout<<"\nCTaskOpPosition::computeModel() : Warning. Lambda_inv is ill conditioned. SVD rank loss (@.005) = "<<rank_loss; }

      data_->M_task_ = svd_.matrixV() * singular_values_ * svd_.matrixU().transpose();

      //Turn off the svd after 50 iterations
      //Don't worry, the qr will pop back to svd if it is still singular
      static sInt svd_ctr = 0; svd_ctr++;
      if(50>=svd_ctr)
      { svd_ctr = 0; use_svd_for_lambda_inv_ = false;  }
    }

    //Compute the Jacobian dynamically consistent generalized inverse :
    //J_dyn_inv = Ainv * J' (J * Ainv * J')^-1
    data_->J_dyn_inv_ = gcm->M_gc_inv_ * data_->J_.transpose() * data_->M_task_;

    //J' * J_dyn_inv'
    sUInt dof = data_->robot_->dof_;
    data_->null_space_ = Eigen::MatrixXd::Identity(dof, dof) -
        data_->J_.transpose() * data_->J_dyn_inv_.transpose();

    // We do not use the centrifugal/coriolis forces. They can cause instabilities.
    // NOTE TODO : Fix this...
//    if(data_->flag_compute_op_gravity_)
//    { /** I need some code */ }
//    else
//    { data_->force_task_cc_.setZero(data_->dof_task_,1);  }
    data_->force_task_cc_.setZero(data_->dof_task_,1);

    // J' * J_dyn_inv' * g(q)
    if(data_->flag_compute_op_gravity_)
    { data_->force_task_grav_ =  data_->J_dyn_inv_.transpose() * gcm->force_gc_grav_;  }

    return flag;
  }
  else
  { return false; }
}
VectorXd Pid_SRIvar_TrackingController::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{

    thetas_ = thetas; //This is necessary for the getNewJointPositions to work

    DQ robot_base = robot_.base();
    DQ robot_effector = robot_.effector();

    VectorXd pseudo_dummy_joint_marker = VectorXd::Zero(robot_dofs_);
    VectorXd pseudo_thetas = thetas_;
    VectorXd pseudo_delta_thetas = VectorXd::Zero(robot_dofs_);
    VectorXd possible_new_thetas = VectorXd::Zero(robot_dofs_);

    MatrixXd original_dh_matrix = robot_.getDHMatrix();
    MatrixXd step_dh_matrix = original_dh_matrix;
    DQ_kinematics pseudo_robot(original_dh_matrix);
    pseudo_robot.set_base(robot_base);
    pseudo_robot.set_effector(robot_effector);

    bool should_break_loop = false;
    while(not should_break_loop) {

        //Calculate jacobian
        task_jacobian_  = pseudo_robot.analyticalJacobian(pseudo_thetas);

        // Recalculation of measured data.
        end_effector_pose_ = pseudo_robot.fkm(pseudo_thetas);

        //Error
        last_error_      = error_;
        error_           = vec8(reference - end_effector_pose_);
        integral_error_ = error_ + ki_memory_*integral_error_;

        //error_ = vec8(dq_one_ - conj(end_effector_pose_)*reference);


        ///Inverse calculation
        svd_.compute(task_jacobian_, ComputeFullU);
        singular_values_ = svd_.singularValues();

        //Damping Calculation
        int current_step_relevant_dof = ( pseudo_robot.links() - pseudo_robot.n_dummy() );
        double sigma_min = singular_values_( current_step_relevant_dof - 1);
        VectorXd u_min = svd_.matrixU().col( current_step_relevant_dof - 1);
        double lambda = lambda_max_;
        if (sigma_min < epsilon_)
        {
            lambda = (1-(sigma_min/epsilon_)*(sigma_min/epsilon_))*lambda_max_*lambda_max_;
        }

        task_jacobian_pseudoinverse_ =    (task_jacobian_.transpose())*((task_jacobian_*task_jacobian_.transpose()
                                          + (beta_*beta_)*identity_ + (lambda*lambda)*u_min*u_min.transpose()).inverse());


        // pseudo_delta_thetas = task_jacobian_pseudoinverse_*kp_*error_;
        if( at_least_one_error_ )
            pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ + kd_*(error_ - last_error_) );
        else
        {
            at_least_one_error_ = true;
            pseudo_delta_thetas = task_jacobian_pseudoinverse_*( kp_*error_ + ki_*integral_error_ );
        }


        //Update delta_thetas for possiblenewthetas calculation
        for(int i=0,j=0; i < robot_dofs_; i++)
        {
            if(pseudo_dummy_joint_marker(i) == 0)
            {
                delta_thetas_(i) = pseudo_delta_thetas(j);
                ++j;
            }
            //Do NOTHING if it should be ignored.
        }


        //Possible new thetas
        possible_new_thetas = thetas_ + delta_thetas_;

        //Verify if loop should end
        should_break_loop = true;
        int j=0;
        //For all joints
        for(int i = 0; i < robot_dofs_; i++) {

            //If joint is not yet marked for not being considered in the minimization
            if(pseudo_dummy_joint_marker(i) == 0) {

                //If the controller is trying to put a joint further than any of its limits
                if(    possible_new_thetas(i) > upper_joint_limits_(i)
                        || possible_new_thetas(i) < lower_joint_limits_(i) )
                {

                    //If the joint was already saturated sometime ago
                    double ep = 1.e-05;
                    if (    thetas_(i) > upper_joint_limits_(i) - ep
                            || thetas_(i) < lower_joint_limits_(i) + ep) {

                        pseudo_dummy_joint_marker(i) = 1; //Mark it to be ignored in the minization
                        //std::cout << std::endl << "Joint " << i << " will be ignored in the next controller step.";
                        step_dh_matrix(4,i) = 1;          //Set matrix as dummy.
                        step_dh_matrix(0,i) = thetas_(i); //Set matrix theta as a fixed value.
                        should_break_loop = false;
                    }
                    //If the joint was not yet saturated and the controller wants to saturate it
                    else {

                        // Saturate the joint in this step.
                        if   ( possible_new_thetas(i) > upper_joint_limits_(i) ) {
                            delta_thetas_(i) = upper_joint_limits_(i) - thetas_(i);
                            //std::cout << std::endl << "Joint = " << i << " was saturated in its upper_limit";
                        }
                        else if ( possible_new_thetas(i) < lower_joint_limits_(i) ) {
                            delta_thetas_(i) = lower_joint_limits_(i) - thetas_(i);
                            //std::cout << std::endl << "Joint = " << i << " was saturated in its lower_limit";
                        }
                        else {
                            std::cout << std::endl << "Something is really wrong";
                        }
                        //The joint should still be considered in the minimizations.
                        pseudo_thetas(j) = thetas_(i);
                        ++j;
                    }


                }
                //If the controller is not trying to put this joint further than any of its limits, we consider the velocity given normally
                else {
                    delta_thetas_(i) = pseudo_delta_thetas(j);
                    pseudo_thetas(j) = thetas_(i);
                    ++j;
                }
            }
            //If joint was marked to be ignored, it shall be ignored.
            else {
                delta_thetas_(i) = 0;
            }

        }
        if( j == 0 ) {
            std::cout << std::endl << "Robot will be unable to get out of this configuration using this controller.";
            delta_thetas_ = VectorXd::Zero(robot_dofs_);
            break;
        }

        if(not should_break_loop) {
            pseudo_robot = DQ_kinematics(step_dh_matrix);  //Change DH
            pseudo_robot.set_base(robot_base);
            pseudo_robot.set_effector(robot_effector);
            pseudo_thetas.conservativeResize(j);           //Resize pseudothetas
        }



    }//While not should_break_loop

    return delta_thetas_;

}