Ejemplo n.º 1
0
        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;
        }
Ejemplo n.º 2
0
void ContactDynamics::fillMatrices() {
    updateTauStar();

    updateNBMatrices();
    //        updateNormalMatrix();
    //        updateBasisMatrix();

    MatrixXd E = getContactMatrix();

    int c = getNumContacts();
    int cd = c * mNumDir;

    // Construct the intermediary blocks.
    // nTmInv = mN.transpose() * MInv
    // bTmInv = mB.transpose() * MInv
    // Where MInv is the imaginary diagonal block matrix that combines the inverted mass matrices of all skeletons.
    // Muliplying each block independently is more efficient that multiplyting the whole MInv matrix.
    MatrixXd nTmInv(c, getNumTotalDofs());
    MatrixXd bTmInv(cd, getNumTotalDofs());
    for (int i = 0; i < getNumSkels(); i++) {
        if (mSkels[i]->getImmobileState()) {
            assert(mIndices[i] == mIndices[i+1]); // If the user sets a skeleton to be immobile without reinitializing ContactDynamics, this assertion will fail.
            continue;
        }
        const MatrixXd skelMInv = mSkels[i]->getInvMassMatrix();
        const int skelNumDofs = mSkels[i]->getNumDofs();
        nTmInv.middleCols(mIndices[i], skelNumDofs).noalias() = mN.transpose().middleCols(mIndices[i], skelNumDofs) * skelMInv;
        bTmInv.middleCols(mIndices[i], skelNumDofs).noalias() = mB.transpose().middleCols(mIndices[i], skelNumDofs) * skelMInv;
    }

    // Construct
    int dimA = c * (2 + mNumDir); // dimension of A is c + cd + c
    mA.resize(dimA, dimA);
    mA.topLeftCorner(c, c).triangularView<Upper>() = nTmInv * mN;
    mA.topLeftCorner(c, c).triangularView<StrictlyLower>() = mA.topLeftCorner(c, c).transpose();
    mA.block(0, c, c, cd).noalias() = nTmInv * mB;
    mA.block(c, 0, cd, c) = mA.block(0, c, c, cd).transpose(); // since B^T * Minv * N = (N^T * Minv * B)^T
    mA.block(c, c, cd, cd).triangularView<Upper>() = bTmInv * mB;
    mA.block(c, c, cd, cd).triangularView<StrictlyLower>() = mA.block(c, c, cd, cd).transpose();
    //        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) = getMuMatrix(); // 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) = mN.transpose() * MinvTauStar;
    //mQBar.block(c, 0, cd, 1) = mB.transpose() * MinvTauStar;

    mQBar.head(c).noalias() = nTmInv * mTauStar;
    mQBar.segment(c,cd).noalias() = bTmInv * mTauStar;
    mQBar /= mDt;
}
Ejemplo n.º 3
0
        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);
        }