Examples with FrictionInfo artisynth.core.mechmodels.MechSystem.FrictionInfo used on opensource projects

Example 1 with FrictionInfo

use of artisynth.core.mechmodels.MechSystem.FrictionInfo in project artisynth_core by artisynth.

the class MechSystemSolver method projectFrictionConstraints.

protected void projectFrictionConstraints(VectorNd vel, double t0) {
    // BEGIN project friction constraints
    updateFrictionConstraints();
    // assumes that updateMassMatrix() has been called
    myRBSolver.updateStructure(myMass, myGT, myGTVersion);
    myRBSolver.projectFriction(myMass, myGT, myNT, myDT, myRg, myBg, myRn, myBn, myBd, myFrictionInfo, vel, myLam, myThe, myPhi);
    // do a Gauss-Siedel project on remaining friction constraints:
    int[] RBDTmap = myRBSolver.getDTMap();
    if (RBDTmap != null) {
        int[] DTmap = new int[myDT.numBlockCols() - RBDTmap.length];
        int i = 0;
        int k = 0;
        for (int bj = 0; bj < myDT.numBlockCols(); bj++) {
            if (k < RBDTmap.length && RBDTmap[k] == bj) {
                k++;
            } else {
                DTmap[i++] = bj;
            }
        }
        if (i != DTmap.length) {
            throw new InternalErrorException("inconsistent DTmap");
        }
        updateInverseMassMatrix(t0);
        for (i = 0; i < DTmap.length; i++) {
            FrictionInfo info = myFrictionInfo[DTmap[i]];
            double phiMax;
            if ((info.flags & FrictionInfo.BILATERAL) != 0) {
                phiMax = info.getMaxFriction(myLam);
            } else {
                phiMax = info.getMaxFriction(myThe);
            }
            int bj = DTmap[i];
            int j = myDT.getBlockColOffset(bj);
            double doff = myBd.get(j);
            double phi = projectSingleFrictionConstraint(vel, myDT, bj, phiMax, doff, /*ignore rigid bodies=*/
            true);
            myPhi.set(j, phi);
        }
    }
    if (myUpdateForcesAtStepEnd) {
        int velSize = myActiveVelSize;
        if (myDsize > 0) {
            myDT.mulAdd(myFcon, myPhi, velSize, myDsize);
        }
    }
}

Example 2 with FrictionInfo

use of artisynth.core.mechmodels.MechSystem.FrictionInfo in project artisynth_core by artisynth.

the class RigidBodySolver method projectFriction.

public boolean projectFriction(SparseBlockMatrix M, SparseBlockMatrix GT, SparseBlockMatrix NT, SparseBlockMatrix DT, VectorNd Rg, VectorNd bg, VectorNd Rn, VectorNd bn, VectorNd bd, FrictionInfo[] finfo, VectorNd vel, VectorNd lam, VectorNd the, VectorNd phi) {
    if (mySizeM == 0) {
        myDTMap = new int[0];
        return false;
    }
    updateMass(M);
    updateGT(GT);
    updateNT(NT);
    updateDT(DT);
    if (mySizeD == 0) {
        return false;
    }
    lam.getSubVector(myLamIdxs, myLam);
    Rg.getSubVector(myLamIdxs, myRg);
    bg.getSubVector(myLamIdxs, myBg);
    the.getSubVector(myTheIdxs, myThe);
    Rn.getSubVector(myTheIdxs, myRn);
    bn.getSubVector(myTheIdxs, myBn);
    bd.getSubVector(myPhiIdxs, myBd);
    // remove initial constraints from righthand side. This means
    // we will 'solve' for theta again, but greatly reduces chatter.
    myGT.mul(myBf, myLam);
    myNT.mulAdd(myBf, myThe);
    myBf.negate();
    // create bf from vel
    vel.getSubVector(myVelIdxs, myVel);
    myMass.mulAdd(myBf, myVel);
    // XXX also need to set bd from info.offset - or compute this elsewhere
    // set the friction limits
    int k = 0;
    for (int bk = 0; bk < myDT.numBlockCols(); bk++) {
        FrictionInfo info = finfo[myDTMap[bk]];
        double phiMax;
        if ((info.flags & FrictionInfo.BILATERAL) != 0) {
            phiMax = info.getMaxFriction(lam);
        } else {
            phiMax = info.getMaxFriction(the);
        }
        // System.out.println ("fm"+bk+" "+phiMax);
        for (int i = 0; i < myDT.getBlockColSize(bk); i++) {
            myFlim.set(k++, phiMax);
        }
    }
    mySolver.factor(myMass, mySizeM, myGT, myRg, myNT, myRn, myDT);
    mySolver.solve(myVel, myLam, myThe, myPhi, myBf, myBg, myBn, myBd, myFlim);
    // ArraySupport.print ("zstate", mySolver.getZState());
    // System.out.println ("the: " + myThe.toString ("%8.3f"));
    // System.out.println ("phi: " + myPhi.toString ("%8.3f"));
    vel.setSubVector(myVelIdxs, myVel);
    phi.getSubVector(myPhiIdxs, myPhi);
    // add lam to the total force result?
    return true;
}