void ConstraintDynamics::computeConstraintWithoutContact() {
updateMassMat();
updateConstraintTerms();
VectorXd lambda = mGInv * mTauHat;
for (int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState())
continue;
mTotalConstrForces[i] = mJ[i].transpose() * lambda;
}
}
void ContactDynamics::fillMatrices() {
updateMassMat();
updateTauStar();
updateNBMatrices();
// updateNormalMatrix();
// updateBasisMatrix();
MatrixXd E = getContactMatrix();
MatrixXd mu = getMuMatrix();
// Construct the intermediary blocks.
MatrixXd Ntranspose = mN.transpose();
MatrixXd Btranspose = mB.transpose();
MatrixXd nTmInv = Ntranspose * mMInv;
MatrixXd bTmInv = Btranspose * mMInv;
// Construct
int c = getNumContacts();
int cd = c * mNumDir;
int dimA = c * (2 + mNumDir); // dimension of A is c + cd + c
mA.resize(dimA, dimA);
mA.topLeftCorner(c, c) = nTmInv * mN;
mA.block(0, c, c, cd) = nTmInv * mB;
mA.block(c, 0, cd, c) = bTmInv * mN;
mA.block(c, c, cd, cd) = bTmInv * mB;
// mA.block(c, c + cd, cd, c) = E * (mDt * mDt);
mA.block(c, c + cd, cd, c) = E;
// mA.block(c + cd, 0, c, c) = mu * (mDt * mDt);
mA.bottomLeftCorner(c, c) = mu; // Note: mu is a diagonal matrix, but we also set the surrounding zeros
// mA.block(c + cd, c, c, cd) = -E.transpose() * (mDt * mDt);
mA.block(c + cd, c, c, cd) = -E.transpose();
mA.topRightCorner(c, c).setZero();
mA.bottomRightCorner(c, c).setZero();
int cfmSize = getNumContacts() * (1 + mNumDir);
for (int i = 0; i < cfmSize; ++i) //add small values to diagnal to keep it away from singular, similar to cfm varaible in ODE
mA(i, i) += 0.001 * mA(i, i);
// Construct Q
mQBar = VectorXd::Zero(dimA);
/*
VectorXd MinvTauStar(mN.rows());
int rowStart = 0;
for (int i = 0; i < mSkels.size(); i++) {
int nDof = mSkels[i]->getNumDofs();
if (mSkels[i]->getImmobileState()) {
continue;
} else {
MinvTauStar.segment(rowStart, nDof) = mMInv.block(rowStart, rowStart, nDof, nDof) * mTauStar.segment(rowStart, nDof);
}
rowStart += nDof;
}
*/
//mQBar.block(0, 0, c, 1) = Ntranspose * MinvTauStar;
//mQBar.block(c, 0, cd, 1) = Btranspose * MinvTauStar;
mQBar.head(c) = nTmInv * mTauStar;
mQBar.segment(c,cd) = bTmInv * mTauStar;
mQBar /= mDt;
}
void ConstraintDynamics::fillMatrices() {
int nContacts = getNumContacts();
int nJointLimits = mLimitingDofIndex.size();
int nConstrs = mConstraints.size();
int cd = nContacts * mNumDir;
int dimA = nContacts * (2 + mNumDir) + nJointLimits;
mA = MatrixXd::Zero(dimA, dimA);
mQBar = VectorXd::Zero(dimA);
updateMassMat();
updateTauStar();
MatrixXd augMInv = mMInv;
VectorXd tauVec = VectorXd::Zero(getTotalNumDofs());
if (nConstrs > 0) {
updateConstraintTerms();
augMInv -= mZ;
VectorXd tempVec = mDt * mGInv * mTauHat;
for (int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState())
continue;
tauVec.segment(mIndices[i], mSkels[i]->getNumDofs()) = mJ[i].transpose() * tempVec;
}
}
tauVec = mMInv * (tauVec + mTauStar);
MatrixXd Ntranspose(nContacts, getTotalNumDofs());
MatrixXd Btranspose(cd, getTotalNumDofs());
MatrixXd NTerm(getTotalNumDofs(), nContacts);
MatrixXd BTerm(getTotalNumDofs(), cd);
if (nContacts > 0) {
updateNBMatrices();
MatrixXd E = getContactMatrix();
MatrixXd mu = getMuMatrix();
// Construct the intermediary blocks.
Ntranspose = mN.transpose();
Btranspose = mB.transpose();
// Compute NTerm and BTerm
NTerm = augMInv * mN;
BTerm = augMInv * mB;
mA.block(0, 0, nContacts, nContacts) = Ntranspose * NTerm;
mA.block(0, nContacts, nContacts, cd) = Ntranspose * BTerm;
mA.block(nContacts, 0, cd, nContacts) = Btranspose * NTerm;
mA.block(nContacts, nContacts, cd, cd) = Btranspose * BTerm;
mA.block(nContacts, nContacts + cd, cd, nContacts) = E;
mA.block(nContacts + cd, 0, nContacts, nContacts) = mu;
mA.block(nContacts + cd, nContacts, nContacts, cd) = -E.transpose();
mQBar.segment(0, nContacts) = Ntranspose * tauVec;
mQBar.segment(nContacts, cd) = Btranspose * tauVec;
}
if (nJointLimits > 0) {
int jointStart = 2 * nContacts + cd;
for (int i = 0; i < nJointLimits; i++)
for (int j = 0; j < nJointLimits; j++) {
if (mLimitingDofIndex[i] * mLimitingDofIndex[j] < 0)
mA(jointStart + i, jointStart + j) = -augMInv(abs(mLimitingDofIndex[i]) - 1, abs(mLimitingDofIndex[j]) - 1);
else
mA(jointStart + i, jointStart + j) = augMInv(abs(mLimitingDofIndex[i]) - 1, abs(mLimitingDofIndex[j]) - 1);
}
for (int i = 0; i < nJointLimits; i++) {
if (mLimitingDofIndex[i] > 0) // hitting upper bound
mQBar[jointStart + i] = -tauVec[abs(mLimitingDofIndex[i]) - 1];
else // hitting lower bound
mQBar[jointStart + i] = tauVec[abs(mLimitingDofIndex[i]) - 1];
}
if (nContacts > 0) {
MatrixXd STerm(mMInv.rows(), nJointLimits);
for (int i = 0; i < nJointLimits; i++) {
if (mLimitingDofIndex[i] > 0) // hitting upper bound
STerm.col(i) = -augMInv.col(mLimitingDofIndex[i] - 1);
else
STerm.col(i) = augMInv.col(abs(mLimitingDofIndex[i]) - 1);
}
mA.block(0, jointStart, nContacts, nJointLimits) = Ntranspose * STerm;
mA.block(nContacts, jointStart, cd, nJointLimits) = Btranspose * STerm;
for (int i = 0; i < nJointLimits; i++) {
if (mLimitingDofIndex[i] > 0) { //hitting uppder bound
mA.block(jointStart + i, 0, 1, nContacts) = -NTerm.row(mLimitingDofIndex[i] - 1);
mA.block(jointStart + i, nContacts, 1, cd) = -BTerm.row(mLimitingDofIndex[i] - 1);
} else {
mA.block(jointStart + i, 0, 1, nContacts) = NTerm.row(abs(mLimitingDofIndex[i]) - 1);
mA.block(jointStart + i, nContacts, 1, cd) = BTerm.row(abs(mLimitingDofIndex[i]) - 1);
}
}
}
}
mQBar /= mDt;
int cfmSize = getNumContacts() * (1 + mNumDir);
for (int i = 0; i < cfmSize; ++i) //add small values to diagnal to keep it away from singular, similar to cfm varaible in ODE
mA(i, i) += 0.001 * mA(i, i);
}