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();
}
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 ;
}