void JointDynamicsProvider::assembleTimeDependentMatrices()
{
  const float cycleTimeMs = cycleTime * 1000.0f;
  /* Row major form! Yeeha! */
  smallA(0, 0) = 1.0f; smallA(0, 1) = cycleTimeMs; smallA(0, 2) = sqrBH(cycleTimeMs) / 2.0f;
  smallA(1, 0) = 0.0f; smallA(1, 1) = 1.0f;        smallA(1, 2) = cycleTimeMs;
  smallA(2, 0) = 0.0f; smallA(2, 1) = 0.0f;        smallA(2, 2) = 1.0f;

  smallAtranspose = smallA.transpose();

  A = Eigen::Matrix18f::Zero();
  for(int i = 0; i < numOfLegJoints; ++i)
  {
    const int index = i * 3;
    A.block<3, 3>(index, index) = smallA;
  }
  Atranspose = A.transpose();

  C = Eigen::Matrix6x18f::Zero();
  for(int i = 0; i < numOfLegJoints; ++i)
    C(i, i * 3) = 1.0f;
  Ctranspose = C.transpose();
}
Example #2
0
int main(int argc, char *argv[])
{
  int ierr = 0, i, j, k;
  bool debug = false;

#ifdef EPETRA_MPI
  MPI_Init(&argc,&argv);
  Epetra_MpiComm Comm( MPI_COMM_WORLD );
#else
  Epetra_SerialComm Comm;
#endif

  bool verbose = false;

  // Check if we should print results to standard out
  if (argc>1) if (argv[1][0]=='-' && argv[1][1]=='v') verbose = true;

  if (verbose && Comm.MyPID()==0)
    cout << Epetra_Version() << endl << endl;

  int rank = Comm.MyPID();
  //  char tmp;
  //  if (rank==0) cout << "Press any key to continue..."<< endl;
  //  if (rank==0) cin >> tmp;
  //  Comm.Barrier();

  Comm.SetTracebackMode(0); // This should shut down any error traceback reporting
  if (verbose) cout << Comm <<endl;

  //  bool verbose1 = verbose;

  // Redefine verbose to only print on PE 0
  if (verbose && rank!=0) verbose = false;
	
  int N = 20;
  int NRHS = 4;
  double * A = new double[N*N];
  double * A1 = new double[N*N];
  double * X = new double[(N+1)*NRHS];
  double * X1 = new double[(N+1)*NRHS];
  int LDX = N+1;
  int LDX1 = N+1;
  double * B = new double[N*NRHS];
  double * B1 = new double[N*NRHS];
  int LDB = N;
  int LDB1 = N;

  int LDA = N;
  int LDA1 = LDA;
  double OneNorm1;
  bool Transpose = false;

  Epetra_SerialDenseSolver solver;
  Epetra_SerialDenseMatrix * Matrix;
  for (int kk=0; kk<2; kk++) {
    for (i=1; i<=N; i++) {
      GenerateHilbert(A, LDA, i);
      OneNorm1 = 0.0;
      for (j=1; j<=i; j++) OneNorm1 += 1.0/((double) j); // 1-Norm = 1 + 1/2 + ...+1/n

      if (kk==0) {
	Matrix = new Epetra_SerialDenseMatrix(View, A, LDA, i, i);
	LDA1 = LDA;
      }
      else {
	Matrix = new Epetra_SerialDenseMatrix(Copy, A, LDA, i, i);
	LDA1 = i;
      }

      GenerateHilbert(A1, LDA1, i);
	
      if (kk==1) {
	solver.FactorWithEquilibration(true);
	solver.SolveWithTranspose(true);
	Transpose = true;
	solver.SolveToRefinedSolution(true);
      }

      for (k=0; k<NRHS; k++)
	for (j=0; j<i; j++) {
	  B[j+k*LDB] = 1.0/((double) (k+3)*(j+3));
	  B1[j+k*LDB1] = B[j+k*LDB1];
	}
      Epetra_SerialDenseMatrix Epetra_B(View, B, LDB, i, NRHS);
      Epetra_SerialDenseMatrix Epetra_X(View, X, LDX, i, NRHS);

      solver.SetMatrix(*Matrix);
      solver.SetVectors(Epetra_X, Epetra_B);

      ierr = check(solver, A1, LDA1,  i, NRHS, OneNorm1, B1, LDB1,  X1, LDX1, Transpose, verbose);
      assert (ierr>-1);
      delete Matrix;
      if (ierr!=0) {
	if (verbose) cout << "Factorization failed due to bad conditioning.  This is normal if RCOND is small."
			  << endl;
	break;
      }
    }
  }

  delete [] A;
  delete [] A1;
  delete [] X;
  delete [] X1;
  delete [] B;
  delete [] B1;

  /////////////////////////////////////////////////////////////////////
  // Now test norms and scaling functions
  /////////////////////////////////////////////////////////////////////

  Epetra_SerialDenseMatrix D;
  double ScalarA = 2.0;

  int DM = 10;
  int DN = 8;
  D.Shape(DM, DN);
  for (j=0; j<DN; j++)
    for (i=0; i<DM; i++) D[j][i] = (double) (1+i+j*DM) ;

  //cout << D << endl;

  double NormInfD_ref = (double)(DM*(DN*(DN+1))/2);
  double NormOneD_ref = (double)((DM*DN*(DM*DN+1))/2 - (DM*(DN-1)*(DM*(DN-1)+1))/2 );

  double NormInfD = D.NormInf();
  double NormOneD = D.NormOne();

  if (verbose) {
    cout << " *** Before scaling *** " << endl
	 << " Computed one-norm of test matrix = " << NormOneD << endl
	 << " Expected one-norm                = " << NormOneD_ref << endl
	 << " Computed inf-norm of test matrix = " << NormInfD << endl
	 << " Expected inf-norm                = " << NormInfD_ref << endl;
  }
  D.Scale(ScalarA); // Scale entire D matrix by this value
  NormInfD = D.NormInf();
  NormOneD = D.NormOne();
  if (verbose) {
    cout << " *** After scaling *** " << endl
	 << " Computed one-norm of test matrix = " << NormOneD << endl
	 << " Expected one-norm                = " << NormOneD_ref*ScalarA << endl
	 << " Computed inf-norm of test matrix = " << NormInfD << endl
	 << " Expected inf-norm                = " << NormInfD_ref*ScalarA << endl;
  }


  /////////////////////////////////////////////////////////////////////
  // Now test that A.Multiply(false, x, y) produces the same result
  // as y.Multiply('N','N', 1.0, A, x, 0.0).
  /////////////////////////////////////////////////////////////////////

  N = 10;
  int M = 10;
  LDA = N;
  Epetra_SerialDenseMatrix smallA(N, M, false);
  Epetra_SerialDenseMatrix x(N, 1, false);
  Epetra_SerialDenseMatrix y1(N, 1, false);
  Epetra_SerialDenseMatrix y2(N, 1, false);

  for(i=0; i<N; ++i) {
    for(j=0; j<M; ++j) {
      smallA(i,j) = 1.0*i+2.0*j+1.0;
    }
    x(i,0) = 1.0;
    y1(i,0) = 0.0;
    y2(i,0) = 0.0;
  }

  //quick check of operator==
  if (x == y1) {
    if (verbose) cout << "err in Epetra_SerialDenseMatrix::operator==, "
        << "erroneously returned true." << std::endl;
    return(-1);
  }

  //quick check of operator!=
  if (x != x) {
    if (verbose) cout << "err in Epetra_SerialDenseMatrix::operator==, "
        << "erroneously returned true." << std::endl;
    return(-1);
  }

  int err1 = smallA.Multiply(false, x, y1);
  int err2 = y2.Multiply('N','N', 1.0, smallA, x, 0.0);
  if (err1 != 0 || err2 != 0) {
    if (verbose) cout << "err in Epetra_SerialDenseMatrix::Multiply"<<endl;
    return(err1+err2);
  }

  for(i=0; i<N; ++i) {
    if (y1(i,0) != y2(i,0)) {
      if (verbose) cout << "different versions of Multiply don't match."<<endl;
      return(-99);
    }
  }

  /////////////////////////////////////////////////////////////////////
  // Now test for larger system, both correctness and performance.
  /////////////////////////////////////////////////////////////////////


  N = 2000;
  NRHS = 5;
  LDA = N;
  LDB = N;
  LDX = N;

  if (verbose) cout << "\n\nComputing factor of an " << N << " x " << N << " general matrix...Please wait.\n\n" << endl;

  // Define A and X

  A = new double[LDA*N];
  X = new double[LDB*NRHS];

  for (j=0; j<N; j++) {
    for (k=0; k<NRHS; k++) X[j+k*LDX] = 1.0/((double) (j+5+k));
    for (i=0; i<N; i++) {
      if (i==((j+2)%N)) A[i+j*LDA] = 100.0 + i;
      else A[i+j*LDA] = -11.0/((double) (i+5)*(j+2));
    }
  }

  // Define Epetra_SerialDenseMatrix object

  Epetra_SerialDenseMatrix BigMatrix(Copy, A, LDA, N, N);
  Epetra_SerialDenseMatrix OrigBigMatrix(View, A, LDA, N, N);

  Epetra_SerialDenseSolver BigSolver;
  BigSolver.FactorWithEquilibration(true);
  BigSolver.SetMatrix(BigMatrix);

  // Time factorization

  Epetra_Flops counter;
  BigSolver.SetFlopCounter(counter);
  Epetra_Time Timer(Comm);
  double tstart = Timer.ElapsedTime();
  ierr = BigSolver.Factor();
  if (ierr!=0 && verbose) cout << "Error in factorization = "<<ierr<< endl;
  assert(ierr==0);
  double time = Timer.ElapsedTime() - tstart;

  double FLOPS = counter.Flops();
  double MFLOPS = FLOPS/time/1000000.0;
  if (verbose) cout << "MFLOPS for Factorization = " << MFLOPS << endl;

  // Define Left hand side and right hand side
  Epetra_SerialDenseMatrix LHS(View, X, LDX, N, NRHS);
  Epetra_SerialDenseMatrix RHS;
  RHS.Shape(N,NRHS); // Allocate RHS

  // Compute RHS from A and X

  Epetra_Flops RHS_counter;
  RHS.SetFlopCounter(RHS_counter);
  tstart = Timer.ElapsedTime();
  RHS.Multiply('N', 'N', 1.0, OrigBigMatrix, LHS, 0.0);
  time = Timer.ElapsedTime() - tstart;

  Epetra_SerialDenseMatrix OrigRHS = RHS;

  FLOPS = RHS_counter.Flops();
  MFLOPS = FLOPS/time/1000000.0;
  if (verbose) cout << "MFLOPS to build RHS (NRHS = " << NRHS <<") = " << MFLOPS << endl;

  // Set LHS and RHS and solve
  BigSolver.SetVectors(LHS, RHS);

  tstart = Timer.ElapsedTime();
  ierr = BigSolver.Solve();
  if (ierr==1 && verbose) cout << "LAPACK guidelines suggest this matrix might benefit from equilibration." << endl;
  else if (ierr!=0 && verbose) cout << "Error in solve = "<<ierr<< endl;
  assert(ierr>=0);
  time = Timer.ElapsedTime() - tstart;

  FLOPS = BigSolver.Flops();
  MFLOPS = FLOPS/time/1000000.0;
  if (verbose) cout << "MFLOPS for Solve (NRHS = " << NRHS <<") = " << MFLOPS << endl;

  double * resid = new double[NRHS];
  bool OK = Residual(N, NRHS, A, LDA, BigSolver.Transpose(), BigSolver.X(), BigSolver.LDX(),
		     OrigRHS.A(), OrigRHS.LDA(), resid);

  if (verbose) {
    if (!OK) cout << "************* Residual do not meet tolerance *************" << endl;
    for (i=0; i<NRHS; i++)
      cout << "Residual[" << i <<"] = "<< resid[i] << endl;
    cout  << endl;
  }

  // Solve again using the Epetra_SerialDenseVector class for LHS and RHS

  Epetra_SerialDenseVector X2;
  Epetra_SerialDenseVector B2;
  X2.Size(BigMatrix.N());
  B2.Size(BigMatrix.M());
  int length = BigMatrix.N();
  {for (int kk=0; kk<length; kk++) X2[kk] = ((double ) kk)/ ((double) length);} // Define entries of X2

  RHS_counter.ResetFlops();
  B2.SetFlopCounter(RHS_counter);
  tstart = Timer.ElapsedTime();
  B2.Multiply('N', 'N', 1.0, OrigBigMatrix, X2, 0.0); // Define B2 = A*X2
  time = Timer.ElapsedTime() - tstart;

  Epetra_SerialDenseVector OrigB2 = B2;

  FLOPS = RHS_counter.Flops();
  MFLOPS = FLOPS/time/1000000.0;
  if (verbose) cout << "MFLOPS to build single RHS = " << MFLOPS << endl;

  // Set LHS and RHS and solve
  BigSolver.SetVectors(X2, B2);

  tstart = Timer.ElapsedTime();
  ierr = BigSolver.Solve();
  time = Timer.ElapsedTime() - tstart;
  if (ierr==1 && verbose) cout << "LAPACK guidelines suggest this matrix might benefit from equilibration." << endl;
  else if (ierr!=0 && verbose) cout << "Error in solve = "<<ierr<< endl;
  assert(ierr>=0);

  FLOPS = counter.Flops();
  MFLOPS = FLOPS/time/1000000.0;
  if (verbose) cout << "MFLOPS to solve single RHS = " << MFLOPS << endl;

  OK = Residual(N, 1, A, LDA, BigSolver.Transpose(), BigSolver.X(), BigSolver.LDX(), OrigB2.A(),
		OrigB2.LDA(), resid);

  if (verbose) {
    if (!OK) cout << "************* Residual do not meet tolerance *************" << endl;
      cout << "Residual = "<< resid[0] << endl;
  }
  delete [] resid;
  delete [] A;
  delete [] X;

  ///////////////////////////////////////////////////
  // Now test default constructor and index operators
  ///////////////////////////////////////////////////

  N = 5;
  Epetra_SerialDenseMatrix C; // Implicit call to default constructor, should not need to call destructor
  C.Shape(5,5); // Make it 5 by 5
  double * C1 = new double[N*N];
  GenerateHilbert(C1, N, N); // Generate Hilber matrix

  C1[1+2*N] = 1000.0;  // Make matrix nonsymmetric

  // Fill values of C with Hilbert values
  for (i=0; i<N; i++)
    for (j=0; j<N; j++)
      C(i,j) = C1[i+j*N];

  // Test if values are correctly written and read
  for (i=0; i<N; i++)
    for (j=0; j<N; j++) {
      assert(C(i,j) == C1[i+j*N]);
      assert(C(i,j) == C[j][i]);
    }

  if (verbose)
    cout << "Default constructor and index operator check OK.  Values of Hilbert matrix = "
	 << endl << C << endl
	 << "Values should be 1/(i+j+1), except value (1,2) should be 1000" << endl;

  delete [] C1;

  // now test sized/shaped constructor
  Epetra_SerialDenseMatrix shapedMatrix(10, 12);
  assert(shapedMatrix.M() == 10);
  assert(shapedMatrix.N() == 12);
  for(i = 0; i < 10; i++)
    for(j = 0; j < 12; j++)
      assert(shapedMatrix(i, j) == 0.0);
  Epetra_SerialDenseVector sizedVector(20);
  assert(sizedVector.Length() == 20);
  for(i = 0; i < 20; i++)
    assert(sizedVector(i) == 0.0);
  if (verbose)
    cout << "Shaped/sized constructors check OK." << endl;

  // test Copy/View mode in op= and cpy ctr
  int temperr = 0;
  temperr = matrixAssignment(verbose, debug);
  if(verbose && temperr == 0)
    cout << "Operator = checked OK." << endl;
  EPETRA_TEST_ERR(temperr, ierr);
  temperr = matrixCpyCtr(verbose, debug);
  if(verbose && temperr == 0)
    cout << "Copy ctr checked OK." << endl;
  EPETRA_TEST_ERR(temperr, ierr);

  // Test some vector methods

  Epetra_SerialDenseVector v1(3);
  v1[0] = 1.0;
  v1[1] = 3.0;
  v1[2] = 2.0;

  Epetra_SerialDenseVector v2(3);
  v2[0] = 2.0;
  v2[1] = 1.0;
  v2[2] = -2.0;

  temperr = 0;
  if (v1.Norm1()!=6.0) temperr++;
  if (fabs(sqrt(14.0)-v1.Norm2())>1.0e-6) temperr++;
  if (v1.NormInf()!=3.0) temperr++;
  if(verbose && temperr == 0)
    cout << "Vector Norms checked OK." << endl;
  temperr = 0;
  if (v1.Dot(v2)!=1.0) temperr++;
  if(verbose && temperr == 0)
    cout << "Vector Dot product checked OK." << endl;

#ifdef EPETRA_MPI
  MPI_Finalize() ;
#endif

/* end main
*/
return ierr ;
}