NaN when calculating the SVD of a matrix with Eigne

Dear Samir, dear Gerald, dear anyone who might know,

in most cases my program works fine, but sometimes it crashes right after starting. I tracked the problem as far as I could. At first I thought it might be the SVD itself, but it appears that the problem starts earlier. I exit the program immediately if any element of data_->Lambda_omega_body_ is a NaN (see code attached).

The previous cout of data shows, that all the values used to calculate data_->Lambda_omega_inv_body_ which is used for the SVD are already NaN (see console output of the program). The first matrix to become NaN is data_->Trans_matrix_.matrix(). It's calculated by TAO.

Any ideas on how to fix the problem?

Thanks a lot, Fabian

CODE:

  bool CKUKAOrientationTask::computeModel()
  {
#ifdef DEBUG
    assert(has_been_init_);
    assert(S_NULL!=dynamics_);
#endif
    if(data_->has_been_init_)
    {
      bool flag = true;
      const scl::SGcModel* gcm = data_->gc_model_;

      // Calculate Body Transformation and Jacobian
      dynamics_->calculateTransformationMatrix(data_->link_id_body_,data_->Trans_matrix_);
      Eigen::Vector3d pos = data_->Trans_matrix_ * data_->pos_in_parent_;
      flag = flag && dynamics_->calculateJacobian(data_->link_id_body_,pos,data_->Jacobian_body_);



      // Split into J_v and J_omega
      data_->J_omega_body_ = data_->Jacobian_body_.block(3,0,3,data_->robot_->dof_); //angluar velocity Jacobian of Body


      // Find #DOF
      scl::sUInt dof = data_->robot_->dof_;

      std::cout << "\nBEGINNING\n";
      std::cout << "\ndata_->Trans_matrix_:\n";
      std::cout << data_->Trans_matrix_.matrix() << std::endl;
      std::cout << "\ndata_->pos_in_parent_:\n";
      std::cout << data_->pos_in_parent_ << std::endl;

      std::cout << "\ndata_->link_id_body_:\n";
      std::cout << data_->link_id_body_ << std::endl;
      std::cout << "\npos:\n";
      std::cout << pos << std::endl;

      std::cout << "\ndata_->J_omega_body_:\n";
      std::cout << data_->J_omega_body_ << std::endl;
      std::cout << "\ngcm->Ainv_:\n";
      std::cout << gcm->Ainv_ << std::endl;
      std::cout << "\ndata_->J_omega_body_.transpose():\n";
      std::cout << data_->J_omega_body_.transpose() << std::endl;

      //Lambda = (J * Ainv * J')^-1
      data_->Lambda_omega_inv_body_ = data_->J_omega_body_ * gcm->Ainv_ * data_->J_omega_body_.transpose(); // Lambda_omega for Body



      if(!lambda_inv_singular_)
      {
        //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_->Lambda_omega_inv_body_);
        qr_.setThreshold(1e-25);

        if(qr_.isInvertible())
        { data_->Lambda_omega_body_ = qr_.inverse(); }
        else
        {
          std::cout<<"\nCKUKAOrientationTask::computeModel() : Warning. Lambda_omega_inv_body_ is rank deficient. Using svd. Rank = "<<qr_.rank() << std::endl;
          std::cout << std::endl << std::endl << data_->Lambda_omega_inv_body_ << std::endl;
//          std::cout << "Threshold= " << qr_.threshold() << std::endl;

          lambda_inv_singular_ = true;
        }
      }

      if(lambda_inv_singular_)
      {
        //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_->Lambda_omega_inv_body_,
            Eigen::ComputeFullU | Eigen::ComputeFullV | Eigen::ColPivHouseholderQRPreconditioner);
//        svd_.compute(data_->Lambda_omega_inv_body_,
//                    Eigen::ComputeFullU | Eigen::ComputeFullV | Eigen::FullPivHouseholderQRPreconditioner);



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

        //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; }
        if(svd_.singularValues()(1) > 0.005)
        { singular_values_(1,1) = 1.0/svd_.singularValues()(1);  }
        else { singular_values_(1,1) = 0.0; }
        if(svd_.singularValues()(2) > 0.005)
        { singular_values_(2,2) = 1.0/svd_.singularValues()(2);  }
        else { singular_values_(2,2) = 0.0; }

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

        std::cout<<"\nComputing the SVD:" << std::endl;
        std::cout << std::endl << std::endl << data_->Lambda_omega_body_ << std::endl << std::endl;


        for (int i=0; i<3; i++) {
            for (int j=0; j<3; j++) {
                if ( isnan(data_->Lambda_omega_body_(i,j)) ) {
                    data_->Lambda_omega_body_(i,j) = 0.0;

                    std::cout << "\nNOT A NUMBER!!!\n";
//                  exit(0);
                }
            }
        }

        std::cout<<"\nComputing the SVD:" << std::endl;
        std::cout << std::endl << std::endl << data_->Lambda_omega_body_ << std::endl << std::endl;

        for (int i=0; i<3; i++) {
            for (int j=0; j<3; j++) {
                if ( isnan(data_->Lambda_omega_body_(i,j)) ) {
                    data_->Lambda_omega_body_(i,j) = 0.0;
                    exit(0);
                }
            }
        }

        //Turn off the svd after 20 iterations
        //Don't worry, the qr will pop back to svd if it is still singular
        static scl::sInt svd_ctr = 0; svd_ctr++;
        std::cout << "Counter=" << svd_ctr << "\n";
        if(20>=svd_ctr)
        { svd_ctr = 0; lambda_inv_singular_ = false;  }
      }



      //Compute the Jacobian dynamically consistent generalized inverse :
      //J_dyn_inv = Ainv * J' * Lambda
      data_->jacobian_dyn_inv_ = gcm->Ainv_ * data_->J_omega_body_.transpose() * data_->Lambda_omega_body_;


      // Set up the null space
      // N = I-J'*J_dyn_inv'
      data_->null_space_ = Eigen::MatrixXd::Identity(dof, dof) - data_->J_omega_body_.transpose() * data_->jacobian_dyn_inv_.transpose();


      // We do not use the centrifugal/coriolis forces. They can cause instabilities.
      data_->mu_.setZero(data_->dof_task_,1);

      // J' * J_dyn_inv' * g(q)
      //data_->p_ =  data_->jacobian_dyn_inv_.transpose() * gcm->g_;
      data_->p_.setZero(data_->dof_task_,1);



      return flag;

    }
    else
    { return false; }
  }

CONSOLE OUTPUT:

CTaskOpPos::computeModel() : Warning. Lambda_inv is rank deficient. Using svd. Rank = 0
BEGINNING

data_->Trans_matrix_:
nan nan nan nan
nan nan nan nan
nan nan nan nan
  0   0   0   1

data_->pos_in_parent_:
0
1
0

data_->link_id_body_:
0xa45a228

pos:
nan
nan
nan

data_->J_omega_body_:
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan

gcm->Ainv_:
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan
nan nan nan nan nan nan nan

data_->J_omega_body_.transpose():
nan nan nan
nan nan nan
nan nan nan
nan nan nan
nan nan nan
nan nan nan
nan nan nan

CKUKAOrientationTask::computeModel() : Warning. Lambda_omega_inv_body_ is rank deficient. Using svd. Rank = 0


nan nan nan
nan nan nan
nan nan nan

Computing the SVD:


nan nan nan
nan nan nan
nan nan nan


NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

NOT A NUMBER!!!

Computing the SVD:


0 0 0
0 0 0
0 0 0

Counter=1

#!

Comments (13)