C++ DiagonalMatrix::Nrows方法代码示例

/*!
  @fn Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
                 const Real eps_, const Real lambda_max_, const Real dt);
  @brief Constructor.
*/
Clik::Clik(const mRobot & mrobot_, const DiagonalMatrix & Kp_, const DiagonalMatrix & Ko_,
           const Real eps_, const Real lambda_max_, const Real dt_):
      dt(dt_),
      eps(eps_),
      lambda_max(lambda_max_),
      mrobot(mrobot_)
{
   robot_type = CLICK_mDH;
   // Initialize with same joint position (and rates) has the robot.
   q = mrobot.get_q();
   qp = mrobot.get_qp();
   qp_prev = qp;
   Kpep = ColumnVector(3); Kpep = 0;
   Koe0Quat = ColumnVector(3); Koe0Quat = 0;
   v = ColumnVector(6); v = 0;

   if(Kp_.Nrows()==3)
      Kp = Kp_;
   else
   {
      Kp = DiagonalMatrix(3); Kp = 0.0;
      cerr << "Clik::Clik-->mRobot, Kp if not 3x3, set gain to 0." << endl;
   }
   if(Ko_.Nrows()==3)
      Ko = Ko_;
   else
   {
      Ko = DiagonalMatrix(3); Ko = 0.0;
      cerr << "Clik::Cli, Ko if not 3x3, set gain to 0." << endl;
   }
}

void Print(const DiagonalMatrix& X)
{
   ++PCN;
   cout << "\nMatrix type: " << X.Type().Value() << " (";
   cout << X.Nrows() << ", ";
   cout << X.Ncols() << ")\n\n";
   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
   int nr=X.Nrows(); int nc=X.Ncols();
   for (int i=1; i<=nr; i++)
   {
      for (int j=1; j<i; j++) cout << "\t";
      if (i<=nc) cout << X(i,i) << "\t";
      cout << "\n";
   }
   cout << flush; ++PCZ;
}

static void tql1(DiagonalMatrix& D, DiagonalMatrix& E)
{
   Tracer et("Evalue(tql1)");
   Real eps = FloatingPointPrecision::Epsilon();
   int n = D.Nrows(); int l;
   for (l=1; l<n; l++) E.element(l-1) = E.element(l);
   Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
   for (l=0; l<n; l++)
   {
      int i,j;
      Real& dl = D.element(l); Real& el = E.element(l);
      Real h = eps * ( fabs(dl) + fabs(el) );
      if (b < h) b = h;
      int m;
      for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
      bool test = false;
      for (j=0; j<30; j++)
      {
         if (m==l) { test = true; break; }
         Real& dl1 = D.element(l+1);
	 Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
	 dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
         Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;

	 p = D.element(m); Real c = 1.0; Real s = 0.0;
	 for (i=m-1; i>=l; i--)
	 {
            Real ei = E.element(i); Real di = D.element(i);
            Real& ei1 = E.element(i+1);
	    g = c * ei; h = c * p;
	    if ( fabs(p) >= fabs(ei))
	    {
	       c = ei / p; r = sqrt(c*c + 1.0); 
               ei1 = s*p*r; s = c/r; c = 1.0/r;
	    }
	    else
	    {
	       c = p / ei; r = sqrt(c*c + 1.0);
	       ei1 = s * ei * r; s = 1.0/r; c /= r;
	    }
	    p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
	 }
	 el = s*p; dl = c*p;
	 if (fabs(el) <= b) { test = true; break; }
      }
      if (!test) Throw ( ConvergenceException(D) );
      Real p = dl + f;
      test = false;
      for (i=l; i>0; i--)
      {
         if (p < D.element(i-1)) D.element(i) = D.element(i-1);
         else { test = true; break; }
      }
      if (!test) i=0;
      D.element(i) = p;
   }
}

void getGeneralizedInverse(Matrix& G, Matrix& Gi) {
#ifdef DEBUG
  cout << "\n\ngetGeneralizedInverse - Singular Value\n";
#endif  

  // Singular value decomposition method
  
  // do SVD
  Matrix U, V;
  DiagonalMatrix D;
  SVD(G,D,U,V);            // X = U * D * V.t()
  
#ifdef DEBUG
  cout << "D:\n";
  cout << setw(9) << setprecision(6) << (D);
  cout << "\n\n";
#endif
  
  DiagonalMatrix Di;
  Di << D.i();
  
#ifdef DEBUG
  cout << "Di:\n";
  cout << setw(9) << setprecision(6) << (Di);
  cout << "\n\n";
#endif
  

  int i=Di.Nrows();
  for (; i>=1; i--) {
    if (Di(i) > 1000.0) {
      Di(i) = 0.0;
    }
  }
  
#ifdef DEBUG
  cout << "Di with biggies zeroed out:\n";
  cout << setw(9) << setprecision(6) << (Di);
  cout << "\n\n";
#endif
  
  //Matrix Gi;
  Gi << (U * (Di * V.t()));
  
  return;
}

void Clean(DiagonalMatrix& A, Real c)
{
   int nr = A.Nrows();
   for (int i=1; i<=nr; i++)
   { Real a = A(i,i); if ((a < c) && (a > -c)) A(i,i) = 0.0; }
}