Ejemplo n.º 1
0
void FactorQTZ::factor( const Matrix_<ELT>& m ){
    delete rep;

    // if user does not supply rcond set it to max(nRow,nCol)*(eps)^7/8 (similar to matlab)
    int mnmax = (m.nrow() > m.ncol()) ? m.nrow() : m.ncol();
    rep = new FactorQTZRep<typename CNT<ELT>::StdNumber>(m, mnmax*NTraits<typename CNT<ELT>::Precision>::getSignificant());
}
Ejemplo n.º 2
0
// Same thing but for comparing matrices where one has Vec3 elements.
static void compareElementwise(const Matrix_<Vec3>& JS,
                               const Matrix&        JSf) 
{
    const int m = JS.nrow(), n = JS.ncol();
    SimTK_TEST(JSf.nrow()==3*m && JSf.ncol()==n);

    for (int b=0; b<m; ++b) {
        const int r = 3*b; // row start for JSf
        for (int i=0; i<3; ++i)
            for (int j=0; j<n; ++j) {
                SimTK_TEST_EQ(JS (b,  j)[i], 
                              JSf(r+i,j));
            }
    }
}
Ejemplo n.º 3
0
// Compare two representations of the same matrix: one as an mXn matrix
// of SpatialVecs, the other as a 6mXn matrix of scalars. Note that this
// will also work if the first actual parameter is a Vector_<SpatialVec> 
// or RowVector_<SpatialVec> since those have implicit conversions to mX1 
// or 1Xn Matrix_<SpatialVec>, resp.
static void compareElementwise(const Matrix_<SpatialVec>& J,
                               const Matrix&              Jf) 
{
    const int m = J.nrow(), n = J.ncol();
    SimTK_TEST(Jf.nrow()==6*m && Jf.ncol()==n);

    for (int b=0; b<m; ++b) {
        const int r = 6*b; // row start for Jf
        for (int i=0; i<6; ++i)
            for (int j=0; j<n; ++j) {
                SimTK_TEST_EQ(J (b,  j)[i/3][i%3], 
                              Jf(r+i,j));
            }
    }
}
Ejemplo n.º 4
0
void FactorQTZRep<T>::inverse(  Matrix_<T>& inverse ) const {
    Matrix_<T> iden(mn,mn);
    inverse.resize(mn,mn);
    iden = 1.0;
    doSolve( iden, inverse );

    return;
}
Ejemplo n.º 5
0
void FactorQTZRep<T>::solve(  const Matrix_<T>& b, Matrix_<T>& x ) const {
    SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve",
       "No matrix was passed to FactorQTZ. \n"  );

    SimTK_APIARGCHECK2_ALWAYS(b.nrow()==nRow,"FactorQTZ","solve",
       "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
        b.nrow(), nRow );

    x.resize(nCol, b.ncol() );
    Matrix_<T> tb;
    tb.resize(maxmn, b.ncol() );
    for(int j=0;j<b.ncol();j++) for(int i=0;i<b.nrow();i++) tb(i,j) = b(i,j);
    doSolve(tb, x);
}
Ejemplo n.º 6
0
int main () {
    double errnorm;
    try { 
           // Default precision (Real, normally double) test.

        Matrix a(4,4, A);
        Vector_<std::complex<double> > expectedValues(4);
        for(int i=0;i<4;i++) expectedValues[i] = expEigen[i];
        Matrix_<std::complex<double> > expectedVectors(4,4);
        for(int i=0;i<4;i++) for(int j=0;j<4;j++) expectedVectors(i,j) = expVectors[j*4+i];
        Vector_<std::complex<double> > values; // should get sized automatically to 4 by getAllEigenValuesAndVectors()
        Vector_<std::complex<double> > expectVec(4);
        Vector_<std::complex<double> > computeVec(4);
        Matrix_<std::complex<double> > vectors; // should get sized automatically to 4x4 by getAllEigenValuesAndVectors()

        Eigen  es(a);   // setup the eigen system 

        es.getAllEigenValuesAndVectors( values, vectors );  // solve for the eigenvalues and eigenvectors of the system 

        printf("\n *****  NON-SYMMETRIC:  ***** \n\n"  );
        cout << " Real SOLUTION: " << values << "  errnorm=" << absNormComplex(values,expectedValues) << endl;
        ASSERT(absNormComplex(values,expectedValues) < 0.001);

        cout << "Vectors = "  << endl;
        for(int i=0;i<4;i++) {
            computeVec = vectors(i); 
            expectVec = expectedVectors(i);

            errnorm =  absNormComplex( computeVec, expectVec );
            cout << computeVec << "  errnorm=" << errnorm << endl;
            ASSERT( errnorm < 0.00001 );

        }
  
        cout << endl << endl;

        Vector_<std::complex<float> > expectedValuesf(4);
        Matrix_<std::complex<float> > expectedVectorsf(4,4);
        Vector_<std::complex<float> > expectVecf(4);
        Vector_<std::complex<float> > computeVecf(4);
        Matrix_<std::complex<float> > vectorsf; // should get sized automatically to 4x4 by getAllEigenValuesAndVectors()
        Vector_<std::complex<float> > valuesf; // should get sized automatically to 4 by getAllEigenValuesAndVectors()
        Matrix_<float> af(4,4); for (int i=0; i<4; ++i) for (int j=0; j<4; ++j) af(i,j)=(float)a(i,j); 

        for(int i=0;i<4;i++) expectedValuesf[i] = (std::complex<float>)expEigen[i];
        for(int i=0;i<4;i++) for(int j=0;j<4;j++) expectedVectorsf(i,j) = (std::complex<float>)expVectors[j*4+i];

        Eigen  esf(af);   // setup the eigen system

        esf.getAllEigenValuesAndVectors( valuesf, vectorsf);   // solve for the eigenvalues and vectors of the system

        cout << " float SOLUTION: " << valuesf << "  errnorm=" << absNormComplex(valuesf,expectedValuesf) << endl;
        ASSERT(absNormComplex(valuesf,expectedValuesf) < 0.001);

        cout << "Vectors = " << endl;
        for(int i=0;i<4;i++) {
            computeVecf = vectorsf(i); 
            expectVecf = expectedVectorsf(i);

            errnorm = absNormComplex( computeVecf, expectVecf );
            cout << computeVecf << "  errnorm=" << errnorm << endl;
            ASSERT( errnorm < 0.0001 );
        }
        Real Z[4] = { 0.0,   0.0,
                      0.0,   0.0  };

        Matrix z(2,2, Z);
        Eigen zeigen(z);
        zeigen.getAllEigenValuesAndVectors( values, vectors); 
        cout << "Matrix of all zeros" << endl << "eigenvalues: " << values << endl;
        cout << "Eigen vectors: " << endl;
        for(int i=0;i<vectors.nrow();i++ ) {
               cout << vectors(i) << endl;
        }

        Matrix_<double> z0;
        Eigen z0Eigen(z0);
        zeigen.getAllEigenValuesAndVectors( values, vectors); 
        cout << "Matrix of dimension 0,0" << endl << "eigenvalues: " << values << endl;
        cout << "Eigen vectors: " << endl;
        for(int i=0;i<vectors.nrow();i++ ) {
               cout << vectors(i) << endl;
        }

/*
//
//         SYMMETRIC TESTS:
//
        Real S[16] = {     1.0,  2.0,  3.0,  4.0,
                           0.0,  2.0,  3.0,  4.0,
                           0.0,  0.0,  3.0,  4.0,
                           0.0,  0.0,  0.0,  4.0 };

        Real expectSymVals[4] = { -2.0531, -0.5146, -0.2943, 12.8621 };
        Real expectSymVecs[16] = {  -0.7003, -0.5144,  0.2767, -0.4103,
                                    -0.3592,  0.4851, -0.6634, -0.4422,
                                     0.1569,  0.5420,  0.6504, -0.5085,
                                     0.5965, -0.4543, -0.2457, -0.6144 };



        Matrix as(4,4, S);
        as.setMatrixStructure( MatrixStructures::Symmetric ) ;

        Eigen  esym(as);   // setup the eigen system
        Vector symValues;
        Vector expectedSymValues(4);
        for(int i=0;i<4;i++) expectedSymValues[i] = expectSymVals[i];
        Matrix symVectors;
        Vector symVector;
        Vector expectedSymVector; 
        Matrix expectedSymVectors(4,4,expectSymVecs );
        esym.getAllEigenValuesAndVectors( symValues, symVectors );  // solve for the eigenvalues and eigenvectors of the system 
        printf("\n *****  SYMMETRIC:  ***** \n\n"  );

        cout << " Real SOLUTION:  All Values and Vectors" << symValues << "  errnorm=" << absNorm(symValues,expectedSymValues) << endl;

        cout << " Eigen Vectors = " << endl;
        for(int i=0;i<4;i++) {
            symVector = symVectors(i);
            expectedSymVector = expectedSymVectors(i);
            errnorm = absNorm(symVector,expectedSymVector);
            cout << symVectors(i) << "  errnorm=" << errnorm << endl;
//            cout << expectedSymVectors(i) <<  endl;
        }

        Eigen  efewi(as);   // setup the eigen system
        efewi.getFewEigenValuesAndVectors( symValues, symVectors, 2, 3 );  // solve for few eigenvalues and eigenvectors of the system 
        cout << " Real SOLUTION:  Values/Vectors between indices 2 and 3" << symValues << endl;
        for(int j=0;j<symVectors.ncol();j++)  cout << symVectors(j) <<  endl;

        Eigen  efewr(as);   // setup the eigen system

        // solve for few eigenvalues/vectors between -1, 1 
        efewr.getFewEigenValuesAndVectors( symValues, symVectors, -1.0, 1.0 );  
        cout << " Real SOLUTION:  Values/Vectors  between -1, 1 " << symValues << endl;
        for(int j=0;j<symVectors.ncol();j++)  cout << symVectors(j) <<  endl;
*/       

        return 0;
    } 
    catch (std::exception& e) {
        std::printf("FAILED: %s\n", e.what());
        return 1;
    }
}
Ejemplo n.º 7
0
void FactorQTZRep<T>::factor(const Matrix_<ELT>&mat )  {
    SimTK_APIARGCHECK2_ALWAYS(mat.nelt() > 0,"FactorQTZ","factor",
       "Can't factor a matrix that has a zero dimension -- got %d X %d.",
       (int)mat.nrow(), (int)mat.ncol());


    // allocate and initialize the matrix we pass to LAPACK
    // converts (negated,conjugated etc.) to LAPACK format
    LapackConvert::convertMatrixToLapack( qtz.data, mat );

    int info;
    const int smallestSingularValue = 2;
    const int largestSingularValue = 1;
    typedef typename CNT<T>::TReal  RealType;
    RealType smlnum, bignum, smin, smax;

    if (mat.nelt() == 0) return;


    // Compute optimal size for work space for dtzrzf and dgepq3. The
    // arguments here should match the calls below, although we'll use maxRank
    // rather than rank since we don't know the rank yet.
    T workSz;
    const int maxRank = std::min(nRow, nCol);
    LapackInterface::tzrzf<T>(maxRank, nCol, 0, nRow, 0, &workSz, -1, info);
    const int lwork1 = (int)NTraits<T>::real(workSz);

    LapackInterface::geqp3<T>(nRow, nCol, 0, nRow, 0, 0, &workSz, -1, info);
    const int lwork2 = (int)NTraits<T>::real(workSz);

    TypedWorkSpace<T> work(std::max(lwork1, lwork2));

    LapackInterface::getMachinePrecision<RealType>( smlnum, bignum);

    // scale the input system of equations
    anrm = (RealType)LapackInterface::lange<T>('M', nRow, nCol, qtz.data, nRow);

    if (anrm > 0 && anrm < smlnum) {
        scaleLinSys = true;
        linSysScaleF = smlnum;
    } else if( anrm > bignum )  {
        scaleLinSys = true;
        linSysScaleF = bignum;
    }
    if (anrm == 0) { // matrix all zeros
        rank = 0;
    } else {
        if (scaleLinSys) {
           LapackInterface::lascl<T>('G', 0, 0, anrm, linSysScaleF, nRow, nCol,
                                     qtz.data, nRow, info );
        }

        // compute QR factorization with column pivoting: A = Q * R
        // Q * R is returned in qtz.data
        LapackInterface::geqp3<T>(nRow, nCol, qtz.data, nRow, pivots.data,
                                  tauGEQP3.data, work.data, work.size, info );

        // compute Rank

        smax = CNT<T>::abs( qtz.data[0] );
        smin = smax;
        if( CNT<T>::abs(qtz.data[0]) == 0 ) {
            rank = 0;
            actualRCond = 0;
        } else {
            T s1,s2,c1,c2;
            RealType smaxpr,sminpr;

            // Determine rank using incremental condition estimate
            Array_<T> xSmall(mn), xLarge(mn); // temporaries
            xSmall[0] = xLarge[0] = 1;
            for (rank=1,smaxpr=0.0,sminpr=1.0;
                 rank<mn && smaxpr*rcond < sminpr; )
            {
                LapackInterface::laic1<T>(smallestSingularValue, rank,
                    xSmall.begin(), smin, &qtz.data[rank*nRow],
                    qtz.data[(rank*nRow)+rank], sminpr, s1, c1);

                LapackInterface::laic1<T>(largestSingularValue, rank,
                    xLarge.begin(), smax, &qtz.data[rank*nRow],
                    qtz.data[(rank*nRow)+rank], smaxpr, s2, c2);

                if (smaxpr*rcond < sminpr) {
                    for(int i=0; i<rank; i++) {
                         xSmall[i] *= s1;
                         xLarge[i] *= s2;
                    }
                    xSmall[rank] = c1;
                    xLarge[rank] = c2;
                    smin = sminpr;
                    smax = smaxpr;
                    actualRCond = (double)(smin/smax);
                    rank++;
                }
            }

            // R => T * Z
            // Matrix is rank deficient so complete the orthogonalization by
            // applying orthogonal transformations from the right to
            // factor R from an upper trapezoidal to an upper triangular matrix
            // T is returned in qtz.data and Z is returned in tauORMQR.data
            if (rank < nCol) {
                LapackInterface::tzrzf<T>(rank, nCol, qtz.data, nRow,
                                          tauORMQR.data, work.data,
                                          work.size, info);
            }
        }
    }
}
Ejemplo n.º 8
0
void FactorQTZRep<T>::doSolve(Matrix_<T>& b, Matrix_<T>& x) const {
    int info;
    typedef typename CNT<T>::TReal RealType;
    RealType bnrm, smlnum, bignum;
    int nrhs = b.ncol();
    int n = nCol;
    int m = nRow;
    typename CNT<T>::TReal rhsScaleF; // scale factor applied to right hand side
    bool scaleRHS = false; // true if right hand side should be scaled

    if (rank == 0) return;

    // Ask the experts for their optimal workspace sizes. The size parameters
    // here must match the calls below.
    T workSz;
    LapackInterface::ormqr<T>('L', 'T', nRow, b.ncol(), mn, 0, nRow,
                               0, 0, b.nrow(), &workSz, -1, info );
    const int lwork1 = (int)NTraits<T>::real(workSz);

    LapackInterface::ormrz<T>('L', 'T', nCol, b.ncol(), rank, nCol-rank,
                              0, nRow, 0, 0,
                              b.nrow(), &workSz, -1, info );
    const int lwork2 = (int)NTraits<T>::real(workSz);

    TypedWorkSpace<T> work(std::max(lwork1, lwork2));

    // compute norm of RHS
    bnrm = (RealType)LapackInterface::lange<T>('M', m, nrhs, &b(0,0), b.nrow());

    LapackInterface::getMachinePrecision<RealType>(smlnum, bignum);

    // compute scale for RHS
    if (bnrm > Zero && bnrm < smlnum) {
        scaleRHS = true;
        rhsScaleF = smlnum;
    } else if (bnrm > bignum) {
        scaleRHS = true;
        rhsScaleF = bignum;
    }


    if (scaleRHS) {   // apply scale factor to RHS
        LapackInterface::lascl<T>('G', 0, 0, bnrm, rhsScaleF, b.nrow(), nrhs,
                                  &b(0,0), b.nrow(), info );
    }
    // b1 = Q'*b0
    LapackInterface::ormqr<T>('L', 'T', nRow, b.ncol(), mn, qtz.data,
                              nRow, tauGEQP3.data, &b(0,0), b.nrow(),
                              work.data, work.size, info );
    // b2 = T^-1*b1 = T^-1 * Q' * b0
    LapackInterface::trsm<T>('L', 'U', 'N', 'N', rank, b.ncol(), 1.0,
                             qtz.data, nRow, &b(0,0), b.nrow() );

    //  zero out elements of RHS for rank deficient systems
    for(int j = 0; j<nrhs; ++j) {
        for(int i = rank; i<n; ++i)
            b(i,j) = 0;
    }

    if (rank < nCol) {
        // b3 = Z'*b2 = Z'*T^-1*Q'*b0
        LapackInterface::ormrz<T>('L', 'T', nCol, b.ncol(), rank, nCol-rank,
                                  qtz.data, nRow, tauORMQR.data, &b(0,0),
                                  b.nrow(), work.data, work.size, info );
    }

    // adjust for pivoting
    Array_<T> b_pivot(n);
    for(int j = 0; j<nrhs; ++j) {
        for(int i = 0; i<n; ++i)
            b_pivot[pivots.data[i]-1] = b(i,j);

        LapackInterface::copy<T>(n, b_pivot.begin(), 1, &x(0,j), 1 );
    }

    // compensate for scaling of linear system
    if (scaleLinSys) {
        LapackInterface::lascl<T>('g', 0, 0, anrm, linSysScaleF, nCol, x.ncol(),
                                  &x(0,0), nCol, info );
    }

    // compensate for scaling of RHS
    if (scaleRHS) {
        LapackInterface::lascl<T>('g', 0, 0, bnrm, rhsScaleF, nCol, x.ncol(),
                                  &x(0,0), nCol, info);
    }
}
Ejemplo n.º 9
0
FactorQTZRep<T>::FactorQTZRep( const Matrix_<ELT>& mat, typename CNT<T>::TReal rc)
:   mn( (mat.nrow() < mat.ncol()) ? mat.nrow() : mat.ncol() ),
    maxmn( (mat.nrow() > mat.ncol()) ? mat.nrow() : mat.ncol() ),
    nRow( mat.nrow() ),
    nCol( mat.ncol() ),
    scaleLinSys(false),
    linSysScaleF(NTraits<typename CNT<T>::Precision>::getNaN()),
    anrm(NTraits<typename CNT<T>::Precision>::getNaN()),
    rcond(rc),
    pivots(mat.ncol()),
    qtz( mat.nrow()*mat.ncol() ),
    tauGEQP3(mn),
    tauORMQR(mn)
{
    for(int i=0; i<mat.ncol(); ++i)
        pivots.data[i] = 0;
    FactorQTZRep<T>::factor( mat );
    isFactored = true;
}
Ejemplo n.º 10
0
void testTaskJacobians() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    system.realize(state, Stage::Position);

    Matrix_<SpatialVec> J;
    Matrix Jmat, Jmat2;
    matter.calcSystemJacobian(state, J);
    SimTK_TEST_EQ(J.nrow(), nb); SimTK_TEST_EQ(J.ncol(), nu);
    matter.calcSystemJacobian(state, Jmat);
    SimTK_TEST_EQ(Jmat.nrow(), 6*nb); SimTK_TEST_EQ(Jmat.ncol(), nu);

    // Unpack J into Jmat2 and compare with Jmat.
    Jmat2.resize(6*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 6*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<3; ++k) {
                Jmat2(nxtr+k, col) = J(row,col)[0][k];
                Jmat2(nxtr+3+k, col) = J(row,col)[1][k];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(Jmat2, Jmat, SignificantReal);

    Vector randU = 100.*Test::randVector(nu), resultU1, resultU2;
    Vector_<SpatialVec> randF(nb), resultF1, resultF2;
    for (int i=0; i<nb; ++i) randF[i] = 100.*Test::randSpatialVec();

    matter.multiplyBySystemJacobian(state, randU, resultF1);
    resultF2 = J*randU;
    SimTK_TEST_EQ_TOL(resultF1, resultF2, Slop);

    matter.multiplyBySystemJacobianTranspose(state, randF, resultU1);
    resultU2 = ~J*randF;
    SimTK_TEST_EQ_TOL(resultU1, resultU2, Slop);

    // See if Station Jacobian can be used to duplicate the translation
    // rows of the System Jacobian, and if Frame Jacobian can be used to
    // duplicate the whole thing.
    Array_<MobilizedBodyIndex> allBodies(nb);
    for (int i=0; i<nb; ++i) allBodies[i]=MobilizedBodyIndex(i);
    Array_<Vec3> allOrigins(nb, Vec3(0));

    Matrix_<Vec3> JS, JS2, JSbyrow;
    Matrix_<SpatialVec> JF, JF2, JFbyrow;

    matter.calcStationJacobian(state, allBodies, allOrigins, JS);
    matter.calcFrameJacobian(state, allBodies, allOrigins, JF);
    for (int i=0; i<nb; ++i) {
        for (int j=0; j<nu; ++j) {
            SimTK_TEST_EQ(JS(i,j), J(i,j)[1]);
            SimTK_TEST_EQ(JF(i,j), J(i,j));
        }
    }

    // Now use random stations to calculate JS & JF.
    Array_<Vec3> randS(nb);
    for (int i=0; i<nb; ++i) randS[i] = 10.*Test::randVec3();
    matter.calcStationJacobian(state, allBodies, randS, JS);
    matter.calcFrameJacobian(state, allBodies, randS, JF);

    // Recalculate one row at a time to test non-contiguous memory handling.
    // Do it backwards just to show off.
    JSbyrow.resize(nb, nu); JFbyrow.resize(nb, nu);
    for (int i=nb-1; i >= 0; --i) {
        matter.calcStationJacobian(state, allBodies[i], randS[i], JSbyrow[i]);
        matter.calcFrameJacobian(state, allBodies[i], randS[i], JFbyrow[i]);
    }
    SimTK_TEST_EQ(JS, JSbyrow);
    SimTK_TEST_EQ(JF, JFbyrow);

    // Calculate JS2=JS and JF2=JF again using multiplication by mobility-space 
    // unit vectors.
    JS2.resize(nb, nu); JF2.resize(nb, nu);
    Vector zeroU(nu, 0.);
    for (int i=0; i < nu; ++i) {
        zeroU[i] = 1;
        matter.multiplyByStationJacobian(state, allBodies, randS, zeroU, JS2(i));
        matter.multiplyByFrameJacobian(state, allBodies, randS, zeroU, JF2(i));
        zeroU[i] = 0;
    }
    SimTK_TEST_EQ_TOL(JS2, JS, Slop);
    SimTK_TEST_EQ_TOL(JF2, JF, Slop);

    // Calculate JS2t=~JS using multiplication by force-space unit vectors.
    Matrix_<Row3> JS2t(nu,nb);
    Vector_<Vec3> zeroF(nb, Vec3(0));
    // While we're at it, let's test non-contiguous vectors by filling in
    // this scalar version and using its non-contig rows as column temps.
    Matrix JS3mat(3*nb,nu);
    for (int b=0; b < nb; ++b) {
        for (int k=0; k<3; ++k) {
            zeroF[b][k] = 1;
            RowVectorView JS3matr = JS3mat[3*b+k];
            matter.multiplyByStationJacobianTranspose(state, allBodies, randS, 
                zeroF, ~JS3matr);
            zeroF[b][k] = 0;
            for (int u=0; u < nu; ++u)
                JS2t(u,b)[k] = JS3matr[u];
        }
    }
    SimTK_TEST_EQ_TOL(JS2, ~JS2t, Slop); // we'll check JS3mat below

    // Calculate JF2t=~JF using multiplication by force-space unit vectors.
    Matrix_<SpatialRow> JF2t(nu,nb);
    Vector_<SpatialVec> zeroSF(nb, SpatialVec(Vec3(0)));
    // While we're at it, let's test non-contiguous vectors by filling in
    // this scalar version and using its non-contig rows as column temps.
    Matrix JF3mat(6*nb,nu);
    for (int b=0; b < nb; ++b) {
        for (int k=0; k<6; ++k) {
            zeroSF[b][k/3][k%3] = 1;
            RowVectorView JF3matr = JF3mat[6*b+k];
            matter.multiplyByFrameJacobianTranspose(state, allBodies, randS, 
                zeroSF, ~JF3matr);
            zeroSF[b][k/3][k%3] = 0;
            for (int u=0; u < nu; ++u)
                JF2t(u,b)[k/3][k%3] = JF3matr[u];
        }
    }
    SimTK_TEST_EQ_TOL(JF2, ~JF2t, Slop); // we'll check JS3mat below


    // All three methods match. Now let's see if they are right by shifting
    // the System Jacobian to the new stations.

    for (int i=0; i<nb; ++i) {
        const MobilizedBody& mobod = matter.getMobilizedBody(allBodies[i]);
        const Rotation& R_GB = mobod.getBodyRotation(state);
        const Vec3 S_G = R_GB*randS[i];
        for (int j=0; j<nu; ++j) {
            const Vec3 w = J(i,j)[0];
            const Vec3 v = J(i,j)[1];
            const Vec3 vJ = v + w % S_G; // Shift
            const Vec3 vS = JS2(i,j);
            const SpatialVec vF = JF2(i,j);
            SimTK_TEST_EQ(vS, vJ);
            SimTK_TEST_EQ(vF, SpatialVec(w, vJ));
        }
    }


    // Now create a scalar version of JS and make sure it matches the Vec3 one.
    Matrix JSmat, JSmat2, JFmat, JFmat2;

    matter.calcStationJacobian(state, allBodies, randS, JSmat);
    matter.calcFrameJacobian(state, allBodies, randS, JFmat);
    SimTK_TEST_EQ(JSmat.nrow(), 3*nb); SimTK_TEST_EQ(JSmat.ncol(), nu);
    SimTK_TEST_EQ(JFmat.nrow(), 6*nb); SimTK_TEST_EQ(JFmat.ncol(), nu);

    SimTK_TEST_EQ_TOL(JSmat, JS3mat, Slop); // same as above?
    SimTK_TEST_EQ_TOL(JFmat, JF3mat, Slop); // same as above?

    // Unpack JS into JSmat2 and compare with JSmat.
    JSmat2.resize(3*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 3*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<3; ++k) {
                JSmat2(nxtr+k, col) = JS(row,col)[k];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(JSmat2, JSmat, SignificantReal);

    // Unpack JF into JFmat2 and compare with JFmat.
    JFmat2.resize(6*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 6*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<6; ++k) {
                JFmat2(nxtr+k, col) = JF(row,col)[k/3][k%3];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(JFmat2, JFmat, SignificantReal);
}