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